# HG changeset patch # User Florian Pose # Date 1190293115 0 # Node ID c8f1395f818a46ffdf5958e3bf2d91859637e7fb # Parent 6c8801bd547f5f60fd929ff5bc10c430968693ee Only exchange sync manager PDOs, if they could completely be read in. diff -r 6c8801bd547f -r c8f1395f818a master/fsm_coe_map.c --- a/master/fsm_coe_map.c Thu Sep 20 12:57:28 2007 +0000 +++ b/master/fsm_coe_map.c Thu Sep 20 12:58:35 2007 +0000 @@ -59,6 +59,8 @@ void ec_fsm_coe_map_action_next_pdo(ec_fsm_coe_map_t *); void ec_fsm_coe_map_action_next_pdo_entry(ec_fsm_coe_map_t *); +void ec_fsm_coe_map_clear_pdos(ec_fsm_coe_map_t *); + /*****************************************************************************/ /** @@ -72,6 +74,7 @@ { fsm->state = NULL; fsm->fsm_coe = fsm_coe; + INIT_LIST_HEAD(&fsm->pdos); } /*****************************************************************************/ @@ -82,6 +85,26 @@ void ec_fsm_coe_map_clear(ec_fsm_coe_map_t *fsm /**< finite state machine */) { + ec_fsm_coe_map_clear_pdos(fsm); +} + +/*****************************************************************************/ + +/** + */ + +void ec_fsm_coe_map_clear_pdos( + ec_fsm_coe_map_t *fsm /**< finite state machine */ + ) +{ + ec_pdo_t *pdo, *next; + + // free all PDOs + list_for_each_entry_safe(pdo, next, &fsm->pdos, list) { + list_del(&pdo->list); + ec_pdo_clear(pdo); + kfree(pdo); + } } /*****************************************************************************/ @@ -152,7 +175,7 @@ ec_slave_t *slave = fsm->slave; ec_sdo_entry_t *entry; - for (; fsm->sync_index < 4; fsm->sync_index++) { + for (; fsm->sync_index < slave->sii_sync_count; fsm->sync_index++) { if (!(fsm->sync_sdo = ec_slave_get_sdo(slave, 0x1C10 + fsm->sync_index))) continue; @@ -160,6 +183,8 @@ EC_DBG("Reading PDO mapping of sync manager %u of slave %u.\n", fsm->sync_index, slave->ring_position); + ec_fsm_coe_map_clear_pdos(fsm); + if (!(entry = ec_sdo_get_entry(fsm->sync_sdo, 0))) { EC_ERR("SDO 0x%04X has no subindex 0 on slave %u.\n", fsm->sync_sdo->index, @@ -235,9 +260,21 @@ return; } - // next sync manager - fsm->sync_index++; - ec_fsm_coe_map_action_next_sync(fsm); + { + ec_sync_t *sync = fsm->slave->sii_syncs + fsm->sync_index; + ec_pdo_t *pdo; + + // exchange sync manager PDO mapping + ec_sync_clear_pdos(sync); + list_for_each_entry(pdo, &fsm->pdos, list) { + ec_sync_add_pdo(sync, pdo); + } + ec_fsm_coe_map_clear_pdos(fsm); + + // next sync manager + fsm->sync_index++; + ec_fsm_coe_map_action_next_sync(fsm); + } } /*****************************************************************************/ @@ -259,26 +296,58 @@ } { - uint16_t pdo_index = EC_READ_U16(fsm->request.data); ec_sdo_entry_t *entry; + if (!(fsm->pdo = (ec_pdo_t *) + kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) { + EC_ERR("Failed to allocate PDO.\n"); + fsm->state = ec_fsm_coe_map_state_error; + return; + } + + ec_pdo_init(fsm->pdo); + fsm->pdo->index = EC_READ_U16(fsm->request.data); + fsm->pdo->type = + ec_sync_get_pdo_type(fsm->slave->sii_syncs + fsm->sync_index); + if (fsm->slave->master->debug_level) - EC_DBG(" PDO 0x%04X.\n", pdo_index); - - if (!(fsm->pdo_sdo = ec_slave_get_sdo(fsm->slave, pdo_index))) { + EC_DBG(" PDO 0x%04X.\n", fsm->pdo->index); + + if (!(fsm->pdo_sdo = ec_slave_get_sdo(fsm->slave, fsm->pdo->index))) { EC_ERR("Slave %u has no SDO 0x%04X.\n", - fsm->slave->ring_position, pdo_index); - fsm->state = ec_fsm_coe_map_state_error; - return; + fsm->slave->ring_position, fsm->pdo->index); + ec_pdo_clear(fsm->pdo); + kfree(fsm->pdo); + fsm->state = ec_fsm_coe_map_state_error; + return; + } + + if (fsm->pdo_sdo->name && strlen(fsm->pdo_sdo->name)) { + if (!(fsm->pdo->name = (char *) + kmalloc(strlen(fsm->pdo_sdo->name) + 1, GFP_KERNEL))) { + EC_ERR("Failed to allocate PDO name.\n"); + ec_pdo_clear(fsm->pdo); + kfree(fsm->pdo); + fsm->state = ec_fsm_coe_map_state_error; + return; + } + memcpy(fsm->pdo->name, fsm->pdo_sdo->name, + strlen(fsm->pdo_sdo->name) + 1); + } else { + fsm->pdo->name = NULL; } if (!(entry = ec_sdo_get_entry(fsm->pdo_sdo, 0))) { EC_ERR("SDO 0x%04X has no subindex 0 on slave %u.\n", fsm->pdo_sdo->index, fsm->slave->ring_position); - fsm->state = ec_fsm_coe_map_state_error; - return; - } + ec_pdo_clear(fsm->pdo); + kfree(fsm->pdo); + fsm->state = ec_fsm_coe_map_state_error; + return; + } + + list_add_tail(&fsm->pdo->list, &fsm->pdos); ec_sdo_request_init_read(&fsm->request, entry); fsm->state = ec_fsm_coe_map_state_pdo_entry_count; @@ -369,24 +438,60 @@ { uint32_t pdo_entry_info; - uint16_t pdo_entry_index; ec_sdo_t *sdo; + ec_sdo_entry_t *entry; + ec_pdo_entry_t *pdo_entry; pdo_entry_info = EC_READ_U32(fsm->request.data); - pdo_entry_index = pdo_entry_info >> 16; - - if (!(sdo = ec_slave_get_sdo(fsm->slave, pdo_entry_index))) { + + if (!(pdo_entry = (ec_pdo_entry_t *) + kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) { + EC_ERR("Failed to allocate PDO entry.\n"); + fsm->state = ec_fsm_coe_map_state_error; + return; + } + + pdo_entry->index = pdo_entry_info >> 16; + pdo_entry->subindex = (pdo_entry_info >> 8) & 0xFF; + pdo_entry->bit_length = pdo_entry_info & 0xFF; + + if (!(sdo = ec_slave_get_sdo(fsm->slave, pdo_entry->index))) { EC_ERR("Slave %u has no SDO 0x%04X.\n", - fsm->slave->ring_position, pdo_entry_index); - fsm->state = ec_fsm_coe_map_state_error; - return; + fsm->slave->ring_position, pdo_entry->index); + kfree(pdo_entry); + fsm->state = ec_fsm_coe_map_state_error; + return; + } + + if (!(entry = ec_sdo_get_entry(sdo, pdo_entry->subindex))) { + EC_ERR("Slave %u has no SDO entry 0x%04X:%u.\n", + fsm->slave->ring_position, pdo_entry->index, + pdo_entry->subindex); + kfree(pdo_entry); + fsm->state = ec_fsm_coe_map_state_error; + return; + } + + if (entry->description && strlen(entry->description)) { + if (!(pdo_entry->name = (char *) + kmalloc(strlen(entry->description) + 1, GFP_KERNEL))) { + EC_ERR("Failed to allocate PDO entry name.\n"); + kfree(pdo_entry); + fsm->state = ec_fsm_coe_map_state_error; + return; + } + memcpy(pdo_entry->name, entry->description, strlen(entry->description) + 1); + } else { + pdo_entry->name = NULL; } if (fsm->slave->master->debug_level) { - size_t bitsize = pdo_entry_info & 0xFFFF; EC_DBG(" PDO entry 0x%04X \"%s\" (%u bit).\n", - pdo_entry_index, sdo->name, bitsize); - } + pdo_entry->index, pdo_entry->name ? pdo_entry->name : "???", + pdo_entry->bit_length); + } + + list_add_tail(&pdo_entry->list, &fsm->pdo->entries); // next PDO entry fsm->pdo_subindex++; diff -r 6c8801bd547f -r c8f1395f818a master/fsm_coe_map.h --- a/master/fsm_coe_map.h Thu Sep 20 12:57:28 2007 +0000 +++ b/master/fsm_coe_map.h Thu Sep 20 12:58:35 2007 +0000 @@ -66,6 +66,8 @@ uint8_t sync_subindices; uint16_t sync_subindex; + struct list_head pdos; + ec_pdo_t *pdo; ec_sdo_t *pdo_sdo; uint8_t pdo_subindices; uint16_t pdo_subindex;