# HG changeset patch # User Florian Pose # Date 1173289041 0 # Node ID d304ef4af5426532f0a76ed722c3f4596bc910df # Parent ae5fb70b359fbb9176c1a0a09a48dbd5e69e53a4 Implemented alternative PDO mapping configuration interface. diff -r ae5fb70b359f -r d304ef4af542 NEWS --- a/NEWS Wed Mar 07 17:27:25 2007 +0000 +++ b/NEWS Wed Mar 07 17:37:21 2007 +0000 @@ -17,6 +17,9 @@ of the init script to handle device ID lists. * Realtime interface changes: - Added ecrt_master_get_status() to get information about the bus. + - Added functions to set up an alternative PDO mapping, i. e. + ec_slave_pdo_mapping_clear(), ec_slave_pdo_mapping_add() and + ec_slave_pdo_mapping(). * Device interface changes: - Replaced ecdev_register() and ecdev_unregister() with ecdev_offer() and ecdev_withdraw(), respectively. The device modules now offer all their diff -r ae5fb70b359f -r d304ef4af542 TODO --- a/TODO Wed Mar 07 17:27:25 2007 +0000 +++ b/TODO Wed Mar 07 17:37:21 2007 +0000 @@ -17,13 +17,12 @@ * Future features: - SDO dictionary and -access in operation mode. - SDO write access in sysfs. - - Speed up IDLE-FSM through fast mode with schedule(). + - Speed up IDLE state machine through fast mode with schedule(). - Evaluate EEPROM contents after writing. - Interface/buffers for asynchronous domain IO. - Distributed clocks. - Read dynamic PDO mapping from SDO dictionary (see can-cia.org: cia301ds4). - Redundancy with 2 network adapters. - - Interface for alternative PDO mapping. - Read out CRC counters. - Optimize alignment of process data. - Calculate expected working counter for domains. diff -r ae5fb70b359f -r d304ef4af542 examples/mini/mini.c --- a/examples/mini/mini.c Wed Mar 07 17:27:25 2007 +0000 +++ b/examples/mini/mini.c Wed Mar 07 17:37:21 2007 +0000 @@ -62,10 +62,16 @@ #endif static void *r_dig_out; +static void *r_ana_out; +static void *r_count; +static void *r_freq; #if 1 ec_pdo_reg_t domain1_pdos[] = { - {"4", Beckhoff_EL2004_Outputs, &r_dig_out}, + {"2", Beckhoff_EL2004_Outputs, &r_dig_out}, + {"3", Beckhoff_EL4132_Output1, &r_ana_out}, + {"4", Beckhoff_EL5101_Value, &r_count}, + {"4", Beckhoff_EL5101_Frequency, &r_freq}, {} }; #endif @@ -154,7 +160,7 @@ int __init init_mini_module(void) { -#if 0 +#if 1 ec_slave_t *slave; #endif @@ -173,6 +179,15 @@ goto out_release_master; } +#if 1 + printk(KERN_INFO PFX "Configuring alternative PDO mapping...\n"); + if (!(slave = ecrt_master_get_slave(master, "4"))) + goto out_release_master; + + if (ecrt_slave_pdo_mapping(slave, EC_DIR_INPUT, 2, 0x1A00, 0x1A02)) + goto out_release_master; +#endif + printk(KERN_INFO PFX "Registering PDOs...\n"); #if 1 if (ecrt_domain_register_pdo_list(domain1, domain1_pdos)) { @@ -195,13 +210,16 @@ #endif #if 0 - if (!(slave = ecrt_master_get_slave(master, "2"))) + if (!(slave = ecrt_master_get_slave(master, "4"))) goto out_release_master; if (ecrt_slave_conf_sdo8(slave, 0x4061, 1, 0)) goto out_release_master; #endif +#if 1 +#endif + printk(KERN_INFO PFX "Activating master...\n"); if (ecrt_master_activate(master)) { printk(KERN_ERR PFX "Failed to activate master!\n"); diff -r ae5fb70b359f -r d304ef4af542 include/ecrt.h --- a/include/ecrt.h Wed Mar 07 17:27:25 2007 +0000 +++ b/include/ecrt.h Wed Mar 07 17:37:21 2007 +0000 @@ -111,8 +111,8 @@ ecrt_master_get_slave()) */ uint32_t vendor_id; /**< vendor ID */ uint32_t product_code; /**< product code */ - uint16_t pdo_index; /**< PDO index */ - uint8_t pdo_subindex; /**< PDO subindex */ + uint16_t pdo_entry_index; /**< PDO entry index */ + uint8_t pdo_entry_subindex; /**< PDO entry subindex */ void **data_ptr; /**< address of the process data pointer */ } ec_pdo_reg_t; @@ -121,7 +121,11 @@ Direction type for ecrt_domain_register_pdo_range() */ -typedef enum {EC_DIR_INPUT, EC_DIR_OUTPUT} ec_direction_t; +typedef enum { + EC_DIR_OUTPUT, + EC_DIR_INPUT +} +ec_direction_t; /****************************************************************************** * Master request functions @@ -191,6 +195,10 @@ int ecrt_slave_conf_sdo32(ec_slave_t *slave, uint16_t sdo_index, uint8_t sdo_subindex, uint32_t value); +void ecrt_slave_pdo_mapping_clear(ec_slave_t *, ec_direction_t); +int ecrt_slave_pdo_mapping_add(ec_slave_t *, ec_direction_t, uint16_t); +int ecrt_slave_pdo_mapping(ec_slave_t *, ec_direction_t, unsigned int, ...); + /****************************************************************************** * Bitwise read/write macros *****************************************************************************/ diff -r ae5fb70b359f -r d304ef4af542 master/Kbuild --- a/master/Kbuild Wed Mar 07 17:27:25 2007 +0000 +++ b/master/Kbuild Wed Mar 07 17:37:21 2007 +0000 @@ -37,7 +37,7 @@ ec_master-objs := module.o master.o device.o device_id.o pdo.o sync.o fmmu.o \ slave.o datagram.o domain.o mailbox.o canopen.o ethernet.o fsm_sii.o \ - fsm_change.o fsm_coe.o fsm_slave.o fsm_master.o xmldev.o + fsm_change.o fsm_coe.o fsm_mapping.o fsm_slave.o fsm_master.o xmldev.o ifeq ($(EC_DBG_IF),1) ec_master-objs += debug.o diff -r ae5fb70b359f -r d304ef4af542 master/Makefile.am --- a/master/Makefile.am Wed Mar 07 17:27:25 2007 +0000 +++ b/master/Makefile.am Wed Mar 07 17:37:21 2007 +0000 @@ -47,6 +47,7 @@ fsm_sii.c fsm_sii.h \ fsm_change.c fsm_change.h \ fsm_coe.c fsm_coe.h \ + fsm_mapping.c fsm_mapping.h \ fsm_slave.c fsm_slave.h \ fsm_master.c fsm_master.h \ globals.h \ diff -r ae5fb70b359f -r d304ef4af542 master/domain.c --- a/master/domain.c Wed Mar 07 17:27:25 2007 +0000 +++ b/master/domain.c Wed Mar 07 17:37:21 2007 +0000 @@ -182,39 +182,26 @@ /*****************************************************************************/ /** - Registeres a PDO entry. - \return 0 in case of success, else < 0 -*/ - -int ec_domain_reg_pdo_entry(ec_domain_t *domain, /**< EtherCAT domain */ - ec_slave_t *slave, /**< slave */ - const ec_pdo_t *pdo, /**< PDO */ - const ec_pdo_entry_t *entry, - /**< PDO registration entry */ - void **data_ptr /**< pointer to the process data - pointer */ - ) + * Registers a PDO entry. + * \return 0 in case of success, else < 0 + */ + +int ec_domain_reg_pdo_entry( + ec_domain_t *domain, /**< EtherCAT domain */ + ec_sync_t *sync, /**< sync manager */ + const ec_pdo_entry_t *entry, /**< PDO entry to register */ + void **data_ptr /**< pointer to the process data pointer */ + ) { ec_data_reg_t *data_reg; - const ec_sync_t *sync; const ec_pdo_t *other_pdo; const ec_pdo_entry_t *other_entry; unsigned int bit_offset, byte_offset; - // Find sync manager for PDO - if (pdo->sync_index >= slave->sii_sync_count) { - EC_ERR("No sync manager for PDO 0x%04X:%i.", - pdo->index, entry->subindex); - return -1; - } - sync = &slave->sii_syncs[pdo->sync_index]; - // Calculate offset (in sync manager) for process data pointer bit_offset = 0; byte_offset = 0; - list_for_each_entry(other_pdo, &slave->sii_pdos, list) { - if (other_pdo->sync_index != sync->index) continue; - + list_for_each_entry(other_pdo, &sync->pdos, list) { list_for_each_entry(other_entry, &other_pdo->entries, list) { if (other_entry == entry) { byte_offset = bit_offset / 8; @@ -231,17 +218,16 @@ return -1; } - if (ec_slave_prepare_fmmu(slave, domain, sync)) { + if (ec_slave_prepare_fmmu(sync->slave, domain, sync)) { EC_ERR("FMMU configuration failed.\n"); kfree(data_reg); return -1; } - data_reg->slave = slave; + data_reg->slave = sync->slave; data_reg->sync = sync; data_reg->sync_offset = byte_offset; data_reg->data_ptr = data_ptr; - list_add_tail(&data_reg->list, &domain->data_regs); return 0; @@ -386,7 +372,7 @@ fmmu = &slave->fmmus[j]; if (fmmu->domain == domain) { fmmu->logical_start_address = base_address + domain->data_size; - sync_size = ec_slave_calc_sync_size(slave, fmmu->sync); + sync_size = ec_sync_size(fmmu->sync); domain->data_size += sync_size; if (datagram_data_size + sync_size > EC_MAX_DATA_SIZE) { if (ec_domain_add_datagram(domain, datagram_offset, @@ -473,33 +459,28 @@ *****************************************************************************/ /** - Registers a PDO in a domain. - \return pointer to the slave on success, else NULL - \ingroup RealtimeInterface -*/ - -ec_slave_t *ecrt_domain_register_pdo(ec_domain_t *domain, - /**< EtherCAT domain */ - const char *address, - /**< ASCII address of the slave, - see ecrt_master_get_slave() */ - uint32_t vendor_id, - /**< vendor ID */ - uint32_t product_code, - /**< product code */ - uint16_t pdo_index, - /**< PDO index */ - uint8_t pdo_subindex, - /**< PDO subindex */ - void **data_ptr - /**< address of the process data - pointer */ - ) + * Registers a PDO for a domain. + * \return pointer to the slave on success, else NULL + * \ingroup RealtimeInterface + */ + +ec_slave_t *ecrt_domain_register_pdo( + ec_domain_t *domain, /**< EtherCAT domain */ + const char *address, /**< ASCII address of the slave, + see ecrt_master_get_slave() */ + uint32_t vendor_id, /**< vendor ID */ + uint32_t product_code, /**< product code */ + uint16_t entry_index, /**< PDO entry index */ + uint8_t entry_subindex, /**< PDO entry subindex */ + void **data_ptr /**< address of the process data pointer */ + ) { ec_slave_t *slave; ec_master_t *master; + ec_sync_t *sync; const ec_pdo_t *pdo; const ec_pdo_entry_t *entry; + unsigned int i; master = domain->master; @@ -507,48 +488,48 @@ if (!(slave = ecrt_master_get_slave(master, address))) return NULL; if (ec_slave_validate(slave, vendor_id, product_code)) return NULL; - list_for_each_entry(pdo, &slave->sii_pdos, list) { - list_for_each_entry(entry, &pdo->entries, list) { - if (entry->index != pdo_index - || entry->subindex != pdo_subindex) continue; - - if (ec_domain_reg_pdo_entry(domain, slave, pdo, entry, data_ptr)) { - return NULL; + for (i = 0; i < slave->sii_sync_count; i++) { + sync = &slave->sii_syncs[i]; + list_for_each_entry(pdo, &sync->pdos, list) { + list_for_each_entry(entry, &pdo->entries, list) { + if (entry->index != entry_index || + entry->subindex != entry_subindex) continue; + + if (ec_domain_reg_pdo_entry(domain, sync, entry, data_ptr)) { + return NULL; + } + + return slave; } - - return slave; - } - } - - EC_ERR("Slave %i does not provide PDO 0x%04X:%i.\n", - slave->ring_position, pdo_index, pdo_subindex); + } + } + + EC_ERR("PDO entry 0x%04X:%u is not mapped in slave %u.\n", + entry_index, entry_subindex, slave->ring_position); return NULL; } /*****************************************************************************/ /** - Registeres a bunch of data fields. - \attention The list has to be terminated with a NULL structure ({})! - \return 0 in case of success, else < 0 - \ingroup RealtimeInterface -*/ - -int ecrt_domain_register_pdo_list(ec_domain_t *domain, - /**< EtherCAT domain */ - const ec_pdo_reg_t *pdos - /**< array of PDO registrations */ - ) + * Registers a bunch of data fields. + * \attention The list has to be terminated with a NULL structure ({})! + * \return 0 in case of success, else < 0 + * \ingroup RealtimeInterface + */ + +int ecrt_domain_register_pdo_list( + ec_domain_t *domain, /**< EtherCAT domain */ + const ec_pdo_reg_t *pdos /**< array of PDO registrations */ + ) { const ec_pdo_reg_t *pdo; for (pdo = pdos; pdo->slave_address; pdo++) if (!ecrt_domain_register_pdo(domain, pdo->slave_address, - pdo->vendor_id, - pdo->product_code, - pdo->pdo_index, - pdo->pdo_subindex, - pdo->data_ptr)) + pdo->vendor_id, pdo->product_code, + pdo->pdo_entry_index, pdo->pdo_entry_subindex, + pdo->data_ptr)) return -1; return 0; diff -r ae5fb70b359f -r d304ef4af542 master/fmmu.c --- a/master/fmmu.c Wed Mar 07 17:27:25 2007 +0000 +++ b/master/fmmu.c Wed Mar 07 17:37:21 2007 +0000 @@ -71,9 +71,7 @@ uint8_t *data /**> configuration memory */ ) { - size_t sync_size; - - sync_size = ec_slave_calc_sync_size(fmmu->slave, fmmu->sync); + size_t sync_size = ec_sync_size(fmmu->sync); if (fmmu->slave->master->debug_level) { EC_DBG("FMMU%u: LogAddr 0x%08X, Size %3i, PhysAddr 0x%04X, Dir %s\n", diff -r ae5fb70b359f -r d304ef4af542 master/fsm_mapping.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/master/fsm_mapping.c Wed Mar 07 17:37:21 2007 +0000 @@ -0,0 +1,367 @@ +/****************************************************************************** + * + * $Id$ + * + * Copyright (C) 2006 Florian Pose, Ingenieurgemeinschaft IgH + * + * This file is part of the IgH EtherCAT Master. + * + * The IgH EtherCAT Master is free software; you can redistribute it + * and/or modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 of the + * License, or (at your option) any later version. + * + * The IgH EtherCAT Master is distributed in the hope that it will be + * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with the IgH EtherCAT Master; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + * The right to use EtherCAT Technology is granted and comes free of + * charge under condition of compatibility of product made by + * Licensee. People intending to distribute/sell products based on the + * code, have to sign an agreement to guarantee that products using + * software based on IgH EtherCAT master stay compatible with the actual + * EtherCAT specification (which are released themselves as an open + * standard) as the (only) precondition to have the right to use EtherCAT + * Technology, IP and trade marks. + * + *****************************************************************************/ + +/** + \file + EtherCAT PDO mapping state machine. +*/ + +/*****************************************************************************/ + +#include "globals.h" +#include "master.h" +#include "mailbox.h" +#include "fsm_mapping.h" + +/*****************************************************************************/ + +void ec_fsm_mapping_state_start(ec_fsm_mapping_t *); +void ec_fsm_mapping_state_zero_count(ec_fsm_mapping_t *); +void ec_fsm_mapping_state_add_pdo(ec_fsm_mapping_t *); +void ec_fsm_mapping_state_pdo_count(ec_fsm_mapping_t *); +void ec_fsm_mapping_state_end(ec_fsm_mapping_t *); +void ec_fsm_mapping_state_error(ec_fsm_mapping_t *); + +void ec_fsm_mapping_next_sync(ec_fsm_mapping_t *); + +/*****************************************************************************/ + +/** + * Constructor. + */ + +void ec_fsm_mapping_init(ec_fsm_mapping_t *fsm, /**< mapping state machine */ + ec_fsm_coe_t *fsm_coe /**< CoE state machine to use */ + ) +{ + fsm->fsm_coe = fsm_coe; + fsm->sdodata.data = (uint8_t *) &fsm->sdo_value; +} + +/*****************************************************************************/ + +/** + * Destructor. + */ + +void ec_fsm_mapping_clear(ec_fsm_mapping_t *fsm /**< mapping state machine */) +{ +} + +/*****************************************************************************/ + +/** + * Start PDO mapping configuration state machine. + */ + +void ec_fsm_mapping_start( + ec_fsm_mapping_t *fsm, /**< mapping state machine */ + ec_slave_t *slave /**< slave to configure */ + ) +{ + fsm->slave = slave; + fsm->state = ec_fsm_mapping_state_start; +} + +/*****************************************************************************/ + +/** + * \return false, if state machine has terminated + */ + +int ec_fsm_mapping_running( + const ec_fsm_mapping_t *fsm /**< mapping state machine */ + ) +{ + return fsm->state != ec_fsm_mapping_state_end + && fsm->state != ec_fsm_mapping_state_error; +} + +/*****************************************************************************/ + +/** + * Executes the current state of the state machine. + * If the state machine's datagram is not sent or received yet, the execution + * of the state machine is delayed to the next cycle. + * \return false, if state machine has terminated + */ + +int ec_fsm_mapping_exec( + ec_fsm_mapping_t *fsm /**< mapping state machine */ + ) +{ + fsm->state(fsm); + return ec_fsm_mapping_running(fsm); +} + +/*****************************************************************************/ + +/** + * \return true, if the state machine terminated gracefully + */ + +int ec_fsm_mapping_success( + const ec_fsm_mapping_t *fsm /**< mapping state machine */ + ) +{ + return fsm->state == ec_fsm_mapping_state_end; +} + +/****************************************************************************** + * PDO mapping state machine + *****************************************************************************/ + +/** + */ + +void ec_fsm_mapping_state_start( + ec_fsm_mapping_t *fsm /**< mapping state machine */ + ) +{ + fsm->dir = EC_DIR_OUTPUT; + ec_fsm_mapping_next_sync(fsm); +} + +/*****************************************************************************/ + +/** + */ + +void ec_fsm_mapping_next_sync( + ec_fsm_mapping_t *fsm /**< mapping state machine */ + ) +{ + do { + if (fsm->dir > EC_DIR_INPUT) { + // no more sync managers to configure mappings for + fsm->state = ec_fsm_mapping_state_end; + return; + } + + if (!(fsm->sync = ec_slave_get_pdo_sync(fsm->slave, fsm->dir))) { + fsm->state = ec_fsm_mapping_state_error; + return; + } + fsm->dir++; + } + while (!fsm->sync->alt_mapping); + + if (fsm->slave->master->debug_level) { + EC_DBG("Configuring PDO mapping for SM%u of slave %i.\n", + fsm->sync->index, fsm->slave->ring_position); + } + + // set mapped PDO count to zero + fsm->sdodata.index = 0x1C10 + fsm->sync->index; + fsm->sdodata.subindex = 0; // mapped PDO count + EC_WRITE_U8(&fsm->sdo_value, 0); // zero PDOs mapped + fsm->sdodata.size = 1; + if (fsm->slave->master->debug_level) + EC_DBG("Setting PDO count to zero for SM%u.\n", fsm->sync->index); + + fsm->state = ec_fsm_mapping_state_zero_count; + ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); + ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately +} + +/*****************************************************************************/ + +/** + */ + +ec_pdo_t *ec_fsm_mapping_next_pdo( + ec_fsm_mapping_t *fsm, /**< mapping state machine */ + struct list_head *list /**< current PDO list item */ + ) +{ + ec_pdo_t *pdo; + + do { + list = list->next; + if (list == &fsm->sync->pdos) + return NULL; // no next PDO + pdo = list_entry(list, ec_pdo_t, list); + } + while (pdo->sync_index != fsm->sync->index); + + return pdo; +} + +/*****************************************************************************/ + +/** + */ + +void ec_fsm_mapping_state_zero_count( + ec_fsm_mapping_t *fsm /**< mapping state machine */ + ) +{ + if (ec_fsm_coe_exec(fsm->fsm_coe)) return; + + if (!ec_fsm_coe_success(fsm->fsm_coe)) { + EC_ERR("Failed to clear PDO mapping for slave %u.\n", + fsm->slave->ring_position); + fsm->state = ec_fsm_mapping_state_error; + return; + } + + // map all PDOs belonging to the current sync manager + + // find first PDO + if (!(fsm->pdo = ec_fsm_mapping_next_pdo( + fsm, &fsm->sync->pdos))) { + if (fsm->slave->master->debug_level) + EC_DBG("No PDOs to map for SM%u of slave %u.\n", + fsm->sync->index, fsm->slave->ring_position); + ec_fsm_mapping_next_sync(fsm); + return; + } + + // add first PDO to mapping + fsm->pdo_count = 1; + fsm->sdodata.subindex = fsm->pdo_count; + EC_WRITE_U16(&fsm->sdo_value, fsm->pdo->index); + fsm->sdodata.size = 2; + + if (fsm->slave->master->debug_level) + EC_DBG("Mapping PDO 0x%04X at position %u.\n", + fsm->pdo->index, fsm->sdodata.subindex); + + fsm->state = ec_fsm_mapping_state_add_pdo; + ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); + ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately +} + +/*****************************************************************************/ + +/** + */ + +void ec_fsm_mapping_state_add_pdo( + ec_fsm_mapping_t *fsm /**< mapping state machine */ + ) +{ + if (ec_fsm_coe_exec(fsm->fsm_coe)) return; + + if (!ec_fsm_coe_success(fsm->fsm_coe)) { + EC_ERR("Failed to map PDO 0x%04X for SM%u of slave %u.\n", + fsm->pdo->index, fsm->sync->index, fsm->slave->ring_position); + fsm->state = ec_fsm_mapping_state_error; + return; + } + + // find next PDO + if (!(fsm->pdo = ec_fsm_mapping_next_pdo( + fsm, &fsm->pdo->list))) { + // no more PDOs to map. write PDO count + fsm->sdodata.subindex = 0; + EC_WRITE_U8(&fsm->sdo_value, fsm->pdo_count); + fsm->sdodata.size = 1; + + if (fsm->slave->master->debug_level) + EC_DBG("Setting number of mapped PDOs to %u.\n", + fsm->pdo_count); + + fsm->state = ec_fsm_mapping_state_pdo_count; + ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); + ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately + return; + } + + // add next PDO to mapping + fsm->pdo_count++; + fsm->sdodata.subindex = fsm->pdo_count; + EC_WRITE_U16(&fsm->sdo_value, fsm->pdo->index); + fsm->sdodata.size = 2; + + if (fsm->slave->master->debug_level) + EC_DBG("Mapping PDO 0x%04X at position %u.\n", + fsm->pdo->index, fsm->sdodata.subindex); + + ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); + ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately +} + +/*****************************************************************************/ + +/** + */ + +void ec_fsm_mapping_state_pdo_count( + ec_fsm_mapping_t *fsm /**< mapping state machine */ + ) +{ + if (ec_fsm_coe_exec(fsm->fsm_coe)) return; + + if (!ec_fsm_coe_success(fsm->fsm_coe)) { + EC_ERR("Failed to set number of mapped PDOs for slave %u.\n", + fsm->slave->ring_position); + fsm->state = ec_fsm_mapping_state_error; + return; + } + + if (fsm->slave->master->debug_level) + EC_DBG("Successfully set PDO mapping for SM%u of slave %u.\n", + fsm->sync->index, fsm->slave->ring_position); + + // mapping configuration for this sync manager complete. + ec_fsm_mapping_next_sync(fsm); +} + +/****************************************************************************** + * Common state functions + *****************************************************************************/ + +/** + State: ERROR. +*/ + +void ec_fsm_mapping_state_error( + ec_fsm_mapping_t *fsm /**< mapping state machine */ + ) +{ +} + +/*****************************************************************************/ + +/** + State: END. +*/ + +void ec_fsm_mapping_state_end( + ec_fsm_mapping_t *fsm /**< mapping state machine */ + ) +{ +} + +/*****************************************************************************/ diff -r ae5fb70b359f -r d304ef4af542 master/fsm_mapping.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/master/fsm_mapping.h Wed Mar 07 17:37:21 2007 +0000 @@ -0,0 +1,83 @@ +/****************************************************************************** + * + * $Id$ + * + * Copyright (C) 2006 Florian Pose, Ingenieurgemeinschaft IgH + * + * This file is part of the IgH EtherCAT Master. + * + * The IgH EtherCAT Master is free software; you can redistribute it + * and/or modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 of the + * License, or (at your option) any later version. + * + * The IgH EtherCAT Master is distributed in the hope that it will be + * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with the IgH EtherCAT Master; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + * The right to use EtherCAT Technology is granted and comes free of + * charge under condition of compatibility of product made by + * Licensee. People intending to distribute/sell products based on the + * code, have to sign an agreement to guarantee that products using + * software based on IgH EtherCAT master stay compatible with the actual + * EtherCAT specification (which are released themselves as an open + * standard) as the (only) precondition to have the right to use EtherCAT + * Technology, IP and trade marks. + * + *****************************************************************************/ + +/** + \file + EtherCAT PDO mapping state machine structures. +*/ + +/*****************************************************************************/ + +#ifndef __EC_FSM_MAPPING__ +#define __EC_FSM_MAPPING__ + +#include "globals.h" +#include "../include/ecrt.h" +#include "datagram.h" +#include "fsm_coe.h" + +/*****************************************************************************/ + +typedef struct ec_fsm_mapping ec_fsm_mapping_t; /**< \see ec_fsm_slave */ + +/** + * Finite state machine of an EtherCAT slave. + */ + +struct ec_fsm_mapping +{ + void (*state)(ec_fsm_mapping_t *); /**< state function */ + ec_fsm_coe_t *fsm_coe; /**< CoE state machine to use */ + + ec_slave_t *slave; /**< slave the FSM runs on */ + + ec_direction_t dir; /**< current PDO direction */ + ec_sync_t *sync; /**< current sync manager */ + ec_pdo_t *pdo; /**< current PDO */ + ec_sdo_data_t sdodata; /**< SDO configuration data */ + uint16_t sdo_value; /**< SDO value */ + unsigned int pdo_count; +}; + +/*****************************************************************************/ + +void ec_fsm_mapping_init(ec_fsm_mapping_t *, ec_fsm_coe_t *); +void ec_fsm_mapping_clear(ec_fsm_mapping_t *); + +void ec_fsm_mapping_start(ec_fsm_mapping_t *, ec_slave_t *); +int ec_fsm_mapping_exec(ec_fsm_mapping_t *); +int ec_fsm_mapping_success(const ec_fsm_mapping_t *); + +/*****************************************************************************/ + +#endif diff -r ae5fb70b359f -r d304ef4af542 master/fsm_slave.c --- a/master/fsm_slave.c Wed Mar 07 17:27:25 2007 +0000 +++ b/master/fsm_slave.c Wed Mar 07 17:37:21 2007 +0000 @@ -42,6 +42,7 @@ #include "master.h" #include "mailbox.h" #include "fsm_slave.h" +#include "fsm_mapping.h" /*****************************************************************************/ @@ -61,6 +62,7 @@ void ec_fsm_slave_conf_state_pdo_sync(ec_fsm_slave_t *); void ec_fsm_slave_conf_state_fmmu(ec_fsm_slave_t *); void ec_fsm_slave_conf_state_sdoconf(ec_fsm_slave_t *); +void ec_fsm_slave_conf_state_mapconf(ec_fsm_slave_t *); void ec_fsm_slave_conf_state_saveop(ec_fsm_slave_t *); void ec_fsm_slave_conf_state_op(ec_fsm_slave_t *); @@ -69,6 +71,7 @@ void ec_fsm_slave_conf_enter_pdo_sync(ec_fsm_slave_t *); void ec_fsm_slave_conf_enter_fmmu(ec_fsm_slave_t *); void ec_fsm_slave_conf_enter_sdoconf(ec_fsm_slave_t *); +void ec_fsm_slave_conf_enter_mapconf(ec_fsm_slave_t *); void ec_fsm_slave_conf_enter_saveop(ec_fsm_slave_t *); void ec_fsm_slave_state_end(ec_fsm_slave_t *); @@ -90,6 +93,7 @@ ec_fsm_sii_init(&fsm->fsm_sii, fsm->datagram); ec_fsm_change_init(&fsm->fsm_change, fsm->datagram); ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram); + ec_fsm_mapping_init(&fsm->fsm_map, &fsm->fsm_coe); } /*****************************************************************************/ @@ -104,6 +108,7 @@ ec_fsm_sii_clear(&fsm->fsm_sii); ec_fsm_change_clear(&fsm->fsm_change); ec_fsm_coe_clear(&fsm->fsm_coe); + ec_fsm_mapping_clear(&fsm->fsm_map); } /*****************************************************************************/ @@ -786,6 +791,107 @@ return; } + ec_fsm_slave_conf_enter_sdoconf(fsm); +} + +/*****************************************************************************/ + +/** + */ + +void ec_fsm_slave_conf_enter_sdoconf(ec_fsm_slave_t *fsm /**< slave state machine */) +{ + ec_slave_t *slave = fsm->slave; + + // No CoE configuration to be applied? + if (list_empty(&slave->sdo_confs)) { // skip SDO configuration + ec_fsm_slave_conf_enter_mapconf(fsm); + return; + } + + // start SDO configuration + fsm->state = ec_fsm_slave_conf_state_sdoconf; + fsm->sdodata = list_entry(fsm->slave->sdo_confs.next, ec_sdo_data_t, list); + ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata); + ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately +} + +/*****************************************************************************/ + +/** + Slave configuration state: SDOCONF. +*/ + +void ec_fsm_slave_conf_state_sdoconf( + ec_fsm_slave_t *fsm /**< slave state machine */ + ) +{ + if (ec_fsm_coe_exec(&fsm->fsm_coe)) return; + + if (!ec_fsm_coe_success(&fsm->fsm_coe)) { + EC_ERR("SDO configuration failed for slave %u.\n", + fsm->slave->ring_position); + fsm->slave->error_flag = 1; + fsm->state = ec_fsm_slave_state_error; + return; + } + + // Another SDO to configure? + if (fsm->sdodata->list.next != &fsm->slave->sdo_confs) { + fsm->sdodata = list_entry(fsm->sdodata->list.next, + ec_sdo_data_t, list); + ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata); + ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately + return; + } + + // All SDOs are now configured. + ec_fsm_slave_conf_enter_mapconf(fsm); +} + +/*****************************************************************************/ + +/** + */ + +void ec_fsm_slave_conf_enter_mapconf( + ec_fsm_slave_t *fsm /**< slave state machine */ + ) +{ + ec_slave_t *slave = fsm->slave; + + if (!(slave->sii_mailbox_protocols & EC_MBOX_COE)) { + // Slave does not support CoE: no configuration of PDO mapping. + ec_fsm_slave_conf_enter_pdo_sync(fsm); + return; + } + + // start configuring PDO mapping + fsm->state = ec_fsm_slave_conf_state_mapconf; + ec_fsm_mapping_start(&fsm->fsm_map, fsm->slave); + ec_fsm_mapping_exec(&fsm->fsm_map); // execute immediately +} + +/*****************************************************************************/ + +/** + Slave configuration state: MAPCONF. +*/ + +void ec_fsm_slave_conf_state_mapconf( + ec_fsm_slave_t *fsm /**< slave state machine */ + ) +{ + if (ec_fsm_mapping_exec(&fsm->fsm_map)) return; + + if (!ec_fsm_mapping_success(&fsm->fsm_map)) { + EC_ERR("PDO mapping configuration failed for slave %u.\n", + fsm->slave->ring_position); + fsm->slave->error_flag = 1; + fsm->state = ec_fsm_slave_state_error; + return; + } + ec_fsm_slave_conf_enter_pdo_sync(fsm); } @@ -825,8 +931,7 @@ /*****************************************************************************/ /** - Slave configuration state: SYNC2. -*/ + */ void ec_fsm_slave_conf_state_pdo_sync(ec_fsm_slave_t *fsm /**< slave state machine */) { @@ -870,7 +975,7 @@ unsigned int j; if (!slave->base_fmmu_count) { // skip FMMU configuration - ec_fsm_slave_conf_enter_sdoconf(fsm); + ec_fsm_slave_conf_enter_saveop(fsm); return; } @@ -919,59 +1024,6 @@ return; } - ec_fsm_slave_conf_enter_sdoconf(fsm); -} - -/*****************************************************************************/ - -/** - */ - -void ec_fsm_slave_conf_enter_sdoconf(ec_fsm_slave_t *fsm /**< slave state machine */) -{ - ec_slave_t *slave = fsm->slave; - - // No CoE configuration to be applied? Jump to SAVEOP state. - if (list_empty(&slave->sdo_confs)) { // skip SDO configuration - ec_fsm_slave_conf_enter_saveop(fsm); - return; - } - - // start SDO configuration - fsm->state = ec_fsm_slave_conf_state_sdoconf; - fsm->sdodata = list_entry(fsm->slave->sdo_confs.next, ec_sdo_data_t, list); - ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata); - ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately -} - -/*****************************************************************************/ - -/** - Slave configuration state: SDOCONF. -*/ - -void ec_fsm_slave_conf_state_sdoconf(ec_fsm_slave_t *fsm /**< slave state machine */) -{ - if (ec_fsm_coe_exec(&fsm->fsm_coe)) return; - - if (!ec_fsm_coe_success(&fsm->fsm_coe)) { - fsm->slave->error_flag = 1; - fsm->state = ec_fsm_slave_state_error; - return; - } - - // Another SDO to configure? - if (fsm->sdodata->list.next != &fsm->slave->sdo_confs) { - fsm->sdodata = list_entry(fsm->sdodata->list.next, - ec_sdo_data_t, list); - ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata); - ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately - return; - } - - // All SDOs are now configured. - - // set state to SAVEOP ec_fsm_slave_conf_enter_saveop(fsm); } diff -r ae5fb70b359f -r d304ef4af542 master/fsm_slave.h --- a/master/fsm_slave.h Wed Mar 07 17:27:25 2007 +0000 +++ b/master/fsm_slave.h Wed Mar 07 17:37:21 2007 +0000 @@ -48,6 +48,7 @@ #include "fsm_sii.h" #include "fsm_change.h" #include "fsm_coe.h" +#include "fsm_mapping.h" /*****************************************************************************/ @@ -70,6 +71,7 @@ ec_fsm_sii_t fsm_sii; /**< SII state machine */ ec_fsm_change_t fsm_change; /**< State change state machine */ ec_fsm_coe_t fsm_coe; /**< CoE state machine */ + ec_fsm_mapping_t fsm_map; /**< PDO mapping state machine */ }; /*****************************************************************************/ diff -r ae5fb70b359f -r d304ef4af542 master/slave.c --- a/master/slave.c Wed Mar 07 17:27:25 2007 +0000 +++ b/master/slave.c Wed Mar 07 17:37:21 2007 +0000 @@ -262,7 +262,7 @@ kfree(slave->sii_syncs); } - // free all PDOs + // free all SII PDOs list_for_each_entry_safe(pdo, next_pdo, &slave->sii_pdos, list) { list_del(&pdo->list); ec_pdo_clear(pdo); @@ -301,7 +301,6 @@ ec_sdo_data_t *sdodata, *next_sdodata; unsigned int i; - // remove FMMU configurations slave->fmmu_count = 0; slave->pdos_registered = 0; @@ -379,7 +378,7 @@ /** */ -void ec_slave_request_state(ec_slave_t *slave, /**< ETherCAT slave */ +void ec_slave_request_state(ec_slave_t *slave, /**< EtherCAT slave */ ec_slave_state_t state /**< new state */ ) { @@ -556,6 +555,31 @@ word_count -= 4; data += 8; } + + // if sync manager index is positive, the PDO is mapped by default + if (pdo->sync_index >= 0) { + ec_pdo_t *mapped_pdo; + + if (pdo->sync_index >= slave->sii_sync_count) { + EC_ERR("Invalid SM index %i for PDO 0x%04X in slave %u.", + pdo->sync_index, pdo->index, slave->ring_position); + return -1; + } + + if (!(mapped_pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) { + EC_ERR("Failed to allocate PDO memory.\n"); + return -1; + } + + if (ec_pdo_copy(mapped_pdo, pdo)) { + EC_ERR("Failed to copy PDO.\n"); + kfree(mapped_pdo); + return -1; + } + + list_add_tail(&mapped_pdo->list, + &slave->sii_syncs[pdo->sync_index].pdos); + } } return 0; @@ -762,31 +786,51 @@ off += sprintf(buffer + off, " Order number: %s\n", slave->sii_order); if (slave->sii_sync_count) - off += sprintf(buffer + off, "\nSync-Managers:\n"); + off += sprintf(buffer + off, "\nSync managers / PDO mapping:\n"); for (i = 0; i < slave->sii_sync_count; i++) { sync = &slave->sii_syncs[i]; - off += sprintf(buffer + off, " %u) 0x%04X, length %i," - " control 0x%02X, %s\n", + off += sprintf(buffer + off, + " SM%u: addr 0x%04X, size %i, control 0x%02X, %s\n", sync->index, sync->physical_start_address, - sync->length, sync->control_register, + ec_sync_size(sync), sync->control_register, sync->enable ? "enable" : "disable"); + + if (list_empty(&sync->pdos)) + off += sprintf(buffer + off, " No PDOs mapped.\n"); + + list_for_each_entry(pdo, &sync->pdos, list) { + off += sprintf(buffer + off, " %s 0x%04X \"%s\"\n", + pdo->type == EC_RX_PDO ? "RXPDO" : "TXPDO", + pdo->index, pdo->name ? pdo->name : "???"); + + list_for_each_entry(pdo_entry, &pdo->entries, list) { + off += sprintf(buffer + off, + " 0x%04X:%X \"%s\", %i bit\n", + pdo_entry->index, pdo_entry->subindex, + pdo_entry->name ? pdo_entry->name : "???", + pdo_entry->bit_length); + } + } } if (!list_empty(&slave->sii_pdos)) - off += sprintf(buffer + off, "\nPDOs:\n"); + off += sprintf(buffer + off, "\nAvailable PDOs:\n"); list_for_each_entry(pdo, &slave->sii_pdos, list) { - off += sprintf(buffer + off, - " %s \"%s\" (0x%04X), Sync-Manager %i\n", + off += sprintf(buffer + off, " %s 0x%04X \"%s\"", pdo->type == EC_RX_PDO ? "RXPDO" : "TXPDO", - pdo->name ? pdo->name : "???", - pdo->index, pdo->sync_index); + pdo->index, pdo->name ? pdo->name : "???"); + if (pdo->sync_index >= 0) + off += sprintf(buffer + off, ", default mapping: SM%u.\n", + pdo->sync_index); + else + off += sprintf(buffer + off, ", no default mapping.\n"); list_for_each_entry(pdo_entry, &pdo->entries, list) { - off += sprintf(buffer + off, " \"%s\" 0x%04X:%X, %i bit\n", + off += sprintf(buffer + off, " 0x%04X:%X \"%s\", %i bit\n", + pdo_entry->index, pdo_entry->subindex, pdo_entry->name ? pdo_entry->name : "???", - pdo_entry->index, pdo_entry->subindex, pdo_entry->bit_length); } } @@ -1056,42 +1100,6 @@ /*****************************************************************************/ /** - * Calculates the size of a sync manager by evaluating PDO sizes. - * \return sync manager size - */ - -uint16_t ec_slave_calc_sync_size( - const ec_slave_t *slave, /**< EtherCAT slave */ - const ec_sync_t *sync /**< sync manager */ - ) -{ - const ec_pdo_t *pdo; - const ec_pdo_entry_t *pdo_entry; - unsigned int bit_size, byte_size; - - if (sync->length) return sync->length; - if (sync->est_length) return sync->est_length; - - bit_size = 0; - list_for_each_entry(pdo, &slave->sii_pdos, list) { - if (pdo->sync_index != sync->index) continue; - - list_for_each_entry(pdo_entry, &pdo->entries, list) { - bit_size += pdo_entry->bit_length; - } - } - - if (bit_size % 8) // round up to full bytes - byte_size = bit_size / 8 + 1; - else - byte_size = bit_size / 8; - - return byte_size; -} - -/*****************************************************************************/ - -/** */ ec_sync_t *ec_slave_get_pdo_sync( @@ -1101,14 +1109,12 @@ { unsigned int sync_index; - switch (dir) { - case EC_DIR_OUTPUT: sync_index = 0; break; - case EC_DIR_INPUT: sync_index = 1; break; - default: - EC_ERR("Invalid direction!\n"); - return NULL; - } - + if (dir != EC_DIR_INPUT && dir != EC_DIR_OUTPUT) { + EC_ERR("Invalid direction!\n"); + return NULL; + } + + sync_index = (unsigned int) dir; if (slave->sii_mailbox_protocols) sync_index += 2; if (sync_index >= slave->sii_sync_count) { @@ -1279,11 +1285,104 @@ /*****************************************************************************/ +void ecrt_slave_pdo_mapping_clear( + ec_slave_t *slave, /**< EtherCAT slave */ + ec_direction_t dir /**< output/input */ + ) +{ + ec_sync_t *sync; + + if (!(slave->sii_mailbox_protocols & EC_MBOX_COE)) { + EC_ERR("Slave %i does not support CoE!\n", slave->ring_position); + return; + } + + if (!(sync = ec_slave_get_pdo_sync(slave, dir))) + return; + + ec_sync_clear_pdos(sync); +} + +/*****************************************************************************/ + +int ecrt_slave_pdo_mapping_add( + ec_slave_t *slave, /**< EtherCAT slave */ + ec_direction_t dir, /**< input/output */ + uint16_t pdo_index /**< Index of PDO mapping list */) +{ + ec_pdo_t *pdo; + ec_sync_t *sync; + unsigned int not_found = 1; + + if (!(slave->sii_mailbox_protocols & EC_MBOX_COE)) { + EC_ERR("Slave %i does not support CoE!\n", slave->ring_position); + return -1; + } + + // does the slave provide the PDO list? + list_for_each_entry(pdo, &slave->sii_pdos, list) { + if (pdo->index == pdo_index) { + not_found = 0; + break; + } + } + + if (not_found) { + EC_ERR("Slave %i does not provide PDO %04X!\n", + slave->ring_position, pdo_index); + return -1; + } + + // check direction + if ((pdo->type == EC_TX_PDO && dir == EC_DIR_OUTPUT) || + (pdo->type == EC_RX_PDO && dir == EC_DIR_INPUT)) { + EC_ERR("Invalid direction for PDO 0x%04X.\n", pdo_index); + return -1; + } + + + if (!(sync = ec_slave_get_pdo_sync(slave, dir))) + return -1; + + return ec_sync_add_pdo(sync, pdo); +} + +/*****************************************************************************/ + +int ecrt_slave_pdo_mapping(ec_slave_t *slave, /**< EtherCAT slave */ + ec_direction_t dir, /**< input/output */ + unsigned int num_args, /**< Number of following arguments */ + ... /**< PDO indices to map */ + ) +{ + va_list ap; + + ecrt_slave_pdo_mapping_clear(slave, dir); + + va_start(ap, num_args); + + for (; num_args; num_args--) { + if (ecrt_slave_pdo_mapping_add( + slave, dir, (uint16_t) va_arg(ap, int))) { + return -1; + } + } + + va_end(ap); + return 0; +} + + +/*****************************************************************************/ + /** \cond */ EXPORT_SYMBOL(ecrt_slave_conf_sdo8); EXPORT_SYMBOL(ecrt_slave_conf_sdo16); EXPORT_SYMBOL(ecrt_slave_conf_sdo32); +EXPORT_SYMBOL(ecrt_slave_pdo_mapping_clear); +EXPORT_SYMBOL(ecrt_slave_pdo_mapping_add); +EXPORT_SYMBOL(ecrt_slave_pdo_mapping); /** \endcond */ diff -r ae5fb70b359f -r d304ef4af542 master/slave.h --- a/master/slave.h Wed Mar 07 17:27:25 2007 +0000 +++ b/master/slave.h Wed Mar 07 17:37:21 2007 +0000 @@ -60,8 +60,8 @@ /*****************************************************************************/ /** - State of an EtherCAT slave. -*/ + * State of an EtherCAT slave. + */ typedef enum { @@ -94,8 +94,8 @@ /*****************************************************************************/ /** - Supported mailbox protocols. -*/ + * Supported mailbox protocols. + */ enum { @@ -110,8 +110,8 @@ /*****************************************************************************/ /** - EtherCAT slave. -*/ + * EtherCAT slave. + */ struct ec_slave { @@ -204,14 +204,10 @@ // misc. ec_sync_t *ec_slave_get_pdo_sync(ec_slave_t *, ec_direction_t); -uint16_t ec_slave_calc_sync_size(const ec_slave_t *, const ec_sync_t *); - int ec_slave_is_coupler(const ec_slave_t *); - int ec_slave_validate(const ec_slave_t *, uint32_t, uint32_t); - void ec_slave_sdo_dict_info(const ec_slave_t *, - unsigned int *, unsigned int *); + unsigned int *, unsigned int *); /*****************************************************************************/ diff -r ae5fb70b359f -r d304ef4af542 master/sync.c --- a/master/sync.c Wed Mar 07 17:27:25 2007 +0000 +++ b/master/sync.c Wed Mar 07 17:37:21 2007 +0000 @@ -41,6 +41,7 @@ #include "globals.h" #include "slave.h" #include "master.h" +#include "pdo.h" #include "sync.h" /*****************************************************************************/ @@ -51,7 +52,7 @@ void ec_sync_init( ec_sync_t *sync, /**< EtherCAT sync manager */ - const ec_slave_t *slave, /**< EtherCAT slave */ + ec_slave_t *slave, /**< EtherCAT slave */ unsigned int index /**< sync manager index */ ) { @@ -59,6 +60,8 @@ sync->index = index; sync->est_length = 0; + INIT_LIST_HEAD(&sync->pdos); + sync->alt_mapping = 0; } /*****************************************************************************/ @@ -71,6 +74,40 @@ ec_sync_t *sync /**< EtherCAT sync manager */ ) { + ec_sync_clear_pdos(sync); +} + +/*****************************************************************************/ + +/** + * Calculates the size of a sync manager by evaluating PDO sizes. + * \return sync manager size + */ + +uint16_t ec_sync_size( + const ec_sync_t *sync /**< sync manager */ + ) +{ + const ec_pdo_t *pdo; + const ec_pdo_entry_t *pdo_entry; + unsigned int bit_size, byte_size; + + if (sync->length) return sync->length; + if (sync->est_length) return sync->est_length; + + bit_size = 0; + list_for_each_entry(pdo, &sync->pdos, list) { + list_for_each_entry(pdo_entry, &pdo->entries, list) { + bit_size += pdo_entry->bit_length; + } + } + + if (bit_size % 8) // round up to full bytes + byte_size = bit_size / 8 + 1; + else + byte_size = bit_size / 8; + + return byte_size; } /*****************************************************************************/ @@ -85,9 +122,7 @@ uint8_t *data /**> configuration memory */ ) { - size_t sync_size; - - sync_size = ec_slave_calc_sync_size(sync->slave, sync); + size_t sync_size = ec_sync_size(sync); if (sync->slave->master->debug_level) { EC_DBG("SM%i: Addr 0x%04X, Size %3i, Ctrl 0x%02X, En %i\n", @@ -103,3 +138,63 @@ } /*****************************************************************************/ + +/** + */ + +int ec_sync_add_pdo( + ec_sync_t *sync, /**< EtherCAT sync manager */ + const ec_pdo_t *pdo /**< PDO to map */ + ) +{ + ec_pdo_t *mapped_pdo; + + // PDO already mapped? + list_for_each_entry(mapped_pdo, &sync->pdos, list) { + if (mapped_pdo->index != pdo->index) continue; + EC_ERR("PDO 0x%04X is already mapped!\n", pdo->index); + return -1; + } + + if (!(mapped_pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) { + EC_ERR("Failed to allocate memory for PDO mapping.\n"); + return -1; + } + + ec_pdo_init(mapped_pdo); + if (ec_pdo_copy(mapped_pdo, pdo)) { + ec_pdo_clear(mapped_pdo); + kfree(mapped_pdo); + return -1; + } + + // set appropriate sync manager index + mapped_pdo->sync_index = sync->index; + + list_add_tail(&mapped_pdo->list, &sync->pdos); + sync->alt_mapping = 1; + return 0; +} + +/*****************************************************************************/ + +/** + */ + +void ec_sync_clear_pdos( + ec_sync_t *sync /**< EtherCAT sync manager */ + ) +{ + ec_pdo_t *pdo, *next; + + // free all mapped PDOs + list_for_each_entry_safe(pdo, next, &sync->pdos, list) { + list_del(&pdo->list); + ec_pdo_clear(pdo); + kfree(pdo); + } + + sync->alt_mapping = 1; +} + +/*****************************************************************************/ diff -r ae5fb70b359f -r d304ef4af542 master/sync.h --- a/master/sync.h Wed Mar 07 17:27:25 2007 +0000 +++ b/master/sync.h Wed Mar 07 17:37:21 2007 +0000 @@ -59,26 +59,30 @@ typedef struct { - const ec_slave_t *slave; /**< slave, the sync manager belongs to */ + ec_slave_t *slave; /**< slave, the sync manager belongs to */ unsigned int index; /**< sync manager index */ uint16_t physical_start_address; /**< physical start address */ uint16_t length; /**< data length in bytes */ uint8_t control_register; /**< control register value */ uint8_t enable; /**< enable bit */ - uint16_t est_length; /**< Estimated length. This is no field of the SII, - but it is used to calculate the length via - PDO ranges */ + uint16_t est_length; /**< used to calculate the length via PDO ranges */ + struct list_head pdos; /**< list of mapped PDOs */ + unsigned int alt_mapping; /**< alternative mapping configured */ } ec_sync_t; /*****************************************************************************/ -void ec_sync_init(ec_sync_t *, const ec_slave_t *, unsigned int); +void ec_sync_init(ec_sync_t *, ec_slave_t *, unsigned int); void ec_sync_clear(ec_sync_t *); +uint16_t ec_sync_size(const ec_sync_t *); void ec_sync_config(const ec_sync_t *, uint8_t *); +int ec_sync_add_pdo(ec_sync_t *, const ec_pdo_t *); +void ec_sync_clear_pdos(ec_sync_t *); + /*****************************************************************************/ #endif