From c5f6c6073f55d92efcf959ba0eff95b9895dc827 Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 20 Sep 2007 12:58:35 +0000 Subject: [PATCH] Only exchange sync manager PDOs, if they could completely be read in. --- master/fsm_coe_map.c | 145 +++++++++++++++++++++++++++++++++++++------ master/fsm_coe_map.h | 2 + 2 files changed, 127 insertions(+), 20 deletions(-) diff --git a/master/fsm_coe_map.c b/master/fsm_coe_map.c index 80ddc7e5..75caaf7c 100644 --- a/master/fsm_coe_map.c +++ b/master/fsm_coe_map.c @@ -59,6 +59,8 @@ void ec_fsm_coe_map_action_next_sync(ec_fsm_coe_map_t *); 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 @@ void ec_fsm_coe_map_init( { fsm->state = NULL; fsm->fsm_coe = fsm_coe; + INIT_LIST_HEAD(&fsm->pdos); } /*****************************************************************************/ @@ -82,6 +85,26 @@ void ec_fsm_coe_map_init( 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 @@ void ec_fsm_coe_map_action_next_sync( 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 @@ void ec_fsm_coe_map_action_next_sync( 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 @@ void ec_fsm_coe_map_action_next_pdo( 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,27 +296,59 @@ void ec_fsm_coe_map_state_pdo( } { - uint16_t pdo_index = EC_READ_U16(fsm->request.data); ec_sdo_entry_t *entry; - 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_ERR("Slave %u has no SDO 0x%04X.\n", - fsm->slave->ring_position, pdo_index); + 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", 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, 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); + 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; ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request); @@ -369,25 +438,61 @@ void ec_fsm_coe_map_state_pdo_entry( { 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))) { - EC_ERR("Slave %u has no SDO 0x%04X.\n", - fsm->slave->ring_position, 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; } - 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_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); + 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) { + EC_DBG(" PDO entry 0x%04X \"%s\" (%u bit).\n", + 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++; ec_fsm_coe_map_action_next_pdo_entry(fsm); diff --git a/master/fsm_coe_map.h b/master/fsm_coe_map.h index cf0aa589..bbf306db 100644 --- a/master/fsm_coe_map.h +++ b/master/fsm_coe_map.h @@ -66,6 +66,8 @@ struct ec_fsm_coe_map 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;