diff --git a/master/fsm_coe_map.c b/master/fsm_coe_map.c
index 80ddc7e5819035f486d0f07f218f050d4a55b6c7..75caaf7c0ed116591f6e7c33477e216185fb2c2e 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->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);
+ EC_DBG(" PDO 0x%04X.\n", fsm->pdo->index);
- if (!(fsm->pdo_sdo = ec_slave_get_sdo(fsm->slave, 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->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))) {
+ 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->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++;
ec_fsm_coe_map_action_next_pdo_entry(fsm);
diff --git a/master/fsm_coe_map.h b/master/fsm_coe_map.h
index cf0aa58987ba7878402f015e15c51648b99920a6..bbf306db6943729c8c4c05dfc40a933a99aa902e 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;