--- a/master/fsm_coe.c Fri Aug 10 15:08:44 2007 +0000
+++ b/master/fsm_coe.c Fri Aug 10 15:27:08 2007 +0000
@@ -260,7 +260,6 @@
EC_WRITE_U16(data + 4, 0x0000);
EC_WRITE_U16(data + 6, 0x0001); // deliver all SDOs!
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_request;
}
@@ -277,16 +276,14 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
- // FIXME: request again?
- ec_master_queue_datagram(fsm->slave->master, datagram);
- return;
- }
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return; // FIXME: request again?
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Failed to receive CoE dictionary request datagram for"
- " slave %i.\n", slave->ring_position);
+ " slave %i (datagram state %i).\n",
+ slave->ring_position, datagram->state);
return;
}
@@ -300,7 +297,6 @@
fsm->cycles_start = datagram->cycles_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_check;
}
@@ -316,15 +312,14 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
- ec_master_queue_datagram(fsm->slave->master, datagram);
- return;
- }
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Failed to receive CoE mailbox check datagram for slave %i.\n",
- slave->ring_position);
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_coe_error;
+ EC_ERR("Failed to receive CoE mailbox check datagram for slave %i"
+ " (datagram state %i).\n",
+ slave->ring_position, datagram->state);
return;
}
@@ -346,14 +341,12 @@
}
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
return;
}
// Fetch response
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_response;
}
@@ -375,16 +368,14 @@
uint16_t sdo_index;
ec_sdo_t *sdo;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
- // FIXME: request again?
- ec_master_queue_datagram(fsm->slave->master, datagram);
- return;
- }
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return; // FIXME: request again?
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Failed to receive CoE dictionary response datagram for"
- " slave %i.\n", slave->ring_position);
+ " slave %i (datagram state %i).\n",
+ slave->ring_position, datagram->state);
return;
}
@@ -461,7 +452,6 @@
if (EC_READ_U8(data + 2) & 0x80) { // more messages waiting. check again.
fsm->cycles_start = datagram->cycles_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_check;
return;
@@ -487,7 +477,6 @@
EC_WRITE_U16(data + 4, 0x0000);
EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_desc_request;
}
@@ -504,16 +493,14 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
- // FIXME: check for response first?
- ec_master_queue_datagram(fsm->slave->master, datagram);
- return;
- }
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return; // FIXME: check for response first?
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Failed to receive CoE SDO description request datagram for"
- " slave %i.\n", slave->ring_position);
+ " slave %i (datagram state %i).\n",
+ slave->ring_position, datagram->state);
return;
}
@@ -527,7 +514,6 @@
fsm->cycles_start = datagram->cycles_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_desc_check;
}
@@ -543,15 +529,14 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
- ec_master_queue_datagram(fsm->slave->master, datagram);
- return;
- }
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Failed to receive CoE mailbox check datagram from slave %i.\n",
- slave->ring_position);
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_coe_error;
+ EC_ERR("Failed to receive CoE mailbox check datagram from slave %i"
+ " (datagram state %i).\n",
+ slave->ring_position, datagram->state);
return;
}
@@ -572,14 +557,12 @@
}
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
return;
}
// Fetch response
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_desc_response;
}
@@ -600,16 +583,14 @@
uint8_t *data, mbox_prot;
size_t rec_size, name_size;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
- // FIXME: request again?
- ec_master_queue_datagram(fsm->slave->master, datagram);
- return;
- }
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return; // FIXME: request again?
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Failed to receive CoE SDO description response datagram from"
- " slave %i.\n", slave->ring_position);
+ " slave %i (datagram state %i).\n",
+ slave->ring_position, datagram->state);
return;
}
@@ -698,7 +679,6 @@
EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex
EC_WRITE_U8 (data + 9, 0x00); // value info (no values)
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_entry_request;
}
@@ -716,16 +696,14 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
- // FIXME: check for response first?
- ec_master_queue_datagram(fsm->slave->master, datagram);
- return;
- }
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return; // FIXME: check for response first?
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Failed to receive CoE SDO entry request datagram for"
- " slave %i.\n", slave->ring_position);
+ " slave %i (datagram state %i).\n",
+ slave->ring_position, datagram->state);
return;
}
@@ -739,7 +717,6 @@
fsm->cycles_start = datagram->cycles_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_entry_check;
}
@@ -756,15 +733,14 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
- ec_master_queue_datagram(fsm->slave->master, datagram);
- return;
- }
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Failed to receive CoE mailbox check datagram from slave %i.\n",
- slave->ring_position);
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_coe_error;
+ EC_ERR("Failed to receive CoE mailbox check datagram from slave %i"
+ " (datagram state %i).\n",
+ slave->ring_position, datagram->state);
return;
}
@@ -785,14 +761,12 @@
}
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
return;
}
// Fetch response
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_entry_response;
}
@@ -814,16 +788,14 @@
size_t rec_size, data_size;
ec_sdo_entry_t *entry;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
- // FIXME: request again?
- ec_master_queue_datagram(fsm->slave->master, datagram);
- return;
- }
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return; // FIXME: request again?
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Failed to receive CoE SDO description response datagram from"
- " slave %i.\n", slave->ring_position);
+ " slave %i (datagram state %i).\n",
+ slave->ring_position, datagram->state);
return;
}
@@ -923,7 +895,6 @@
EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex
EC_WRITE_U8 (data + 9, 0x00); // value info (no values)
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_entry_request;
return;
@@ -944,7 +915,6 @@
EC_WRITE_U16(data + 4, 0x0000);
EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_desc_request;
return;
@@ -992,7 +962,6 @@
EC_WRITE_U32(data + 6, sdodata->size);
memcpy(data + 10, sdodata->data, sdodata->size);
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_down_request;
}
@@ -1009,16 +978,14 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
- // FIXME: check for response first?
- ec_master_queue_datagram(fsm->slave->master, datagram);
- return;
- }
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return; // FIXME: check for response first?
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Failed to receive CoE download request datagram for"
- " slave %i.\n", slave->ring_position);
+ " slave %i (datagram state %i).\n",
+ slave->ring_position, datagram->state);
return;
}
@@ -1032,7 +999,6 @@
fsm->cycles_start = datagram->cycles_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_down_check;
}
@@ -1048,15 +1014,14 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
- ec_master_queue_datagram(fsm->slave->master, datagram);
- return;
- }
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Failed to receive CoE mailbox check datagram for slave %i.\n",
- slave->ring_position);
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_coe_error;
+ EC_ERR("Failed to receive CoE mailbox check datagram for slave %i"
+ " (datagram state %i).\n",
+ slave->ring_position, datagram->state);
return;
}
@@ -1077,14 +1042,12 @@
}
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
return;
}
// Fetch response
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_down_response;
}
@@ -1104,16 +1067,14 @@
size_t rec_size;
ec_sdo_data_t *sdodata = fsm->sdodata;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
- // FIXME: request again?
- ec_master_queue_datagram(fsm->slave->master, datagram);
- return;
- }
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return; // FIXME: request again?
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Failed to receive CoE download response datagram from"
- " slave %i.\n", slave->ring_position);
+ " slave %i (datagram state %i).\n",
+ slave->ring_position, datagram->state);
return;
}
@@ -1210,7 +1171,6 @@
ec_print_data(data, 10);
}
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_request;
}
@@ -1227,16 +1187,14 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
- // FIXME: check for response first?
- ec_master_queue_datagram(fsm->slave->master, datagram);
- return;
- }
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Failed to receive CoE upload request for slave %i.\n",
- slave->ring_position);
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return; // FIXME: check for response first?
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_coe_error;
+ EC_ERR("Failed to receive CoE upload request for slave %i"
+ " (datagram state %i).\n",
+ slave->ring_position, datagram->state);
return;
}
@@ -1250,7 +1208,6 @@
fsm->cycles_start = datagram->cycles_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_check;
}
@@ -1266,15 +1223,14 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
- ec_master_queue_datagram(fsm->slave->master, datagram);
- return;
- }
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Failed to receive CoE mailbox check datagram from slave %i.\n",
- slave->ring_position);
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_coe_error;
+ EC_ERR("Failed to receive CoE mailbox check datagram from slave %i"
+ " (datagram state %i).\n",
+ slave->ring_position, datagram->state);
return;
}
@@ -1295,14 +1251,12 @@
}
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
return;
}
// Fetch response
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_response;
}
@@ -1327,16 +1281,14 @@
uint32_t complete_size;
unsigned int expedited, size_specified;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
- // FIXME: request again?
- ec_master_queue_datagram(fsm->slave->master, datagram);
- return;
- }
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return; // FIXME: request again?
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Failed to receive CoE upload response datagram for"
- " slave %i.\n", slave->ring_position);
+ " slave %i (datagram state %i).\n",
+ slave->ring_position, datagram->state);
return;
}
@@ -1453,7 +1405,6 @@
ec_print_data(data, 3);
}
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_seg_request;
return;
@@ -1475,16 +1426,14 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
- // FIXME: check for response first?
- ec_master_queue_datagram(fsm->slave->master, datagram);
- return;
- }
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return; // FIXME: check for response first?
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Failed to receive CoE upload segment request datagram for"
- " slave %i.\n", slave->ring_position);
+ " slave %i (datagram state %i).\n",
+ slave->ring_position, datagram->state);
return;
}
@@ -1498,7 +1447,6 @@
fsm->cycles_start = datagram->cycles_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_seg_check;
}
@@ -1514,15 +1462,14 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
- ec_master_queue_datagram(fsm->slave->master, datagram);
- return;
- }
-
- if (datagram->state != EC_DATAGRAM_RECEIVED) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Failed to receive CoE mailbox check datagram for slave %i.\n",
- slave->ring_position);
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_coe_error;
+ EC_ERR("Failed to receive CoE mailbox check datagram for slave %i"
+ " (datagram state %i).\n",
+ slave->ring_position, datagram->state);
return;
}
@@ -1543,14 +1490,12 @@
}
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
return;
}
// Fetch response
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_seg_response;
}
@@ -1575,16 +1520,14 @@
uint32_t seg_size;
unsigned int last_segment;
- if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
- // FIXME: request again?
- ec_master_queue_datagram(fsm->slave->master, datagram);
- return;
- }
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return; // FIXME: request again?
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Failed to receive CoE upload segment response datagram for"
- " slave %i.\n", slave->ring_position);
+ " slave %i (datagram state %i).\n",
+ slave->ring_position, datagram->state);
return;
}
@@ -1668,7 +1611,6 @@
ec_print_data(data, 3);
}
- ec_master_queue_datagram(fsm->slave->master, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_seg_request;
return;