--- a/master/fsm_coe.c Mon Jun 15 09:24:58 2009 +0000
+++ b/master/fsm_coe.c Mon Jun 15 12:33:48 2009 +0000
@@ -1504,7 +1504,8 @@
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
- uint8_t *data, mbox_prot;
+ uint16_t rec_index;
+ uint8_t *data, mbox_prot, rec_subindex;
size_t rec_size, data_size;
ec_sdo_request_t *request = fsm->request;
unsigned int expedited, size_specified;
@@ -1553,7 +1554,7 @@
return;
}
- if (rec_size < 3) {
+ if (rec_size < 6) {
fsm->state = ec_fsm_coe_error;
EC_ERR("Received currupted SDO upload response (%u bytes)!\n", rec_size);
ec_print_data(data, rec_size);
@@ -1561,7 +1562,7 @@
}
if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
- EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
+ EC_READ_U8(data + 2) >> 5 == 0x4) { // abort SDO transfer request
EC_ERR("SDO upload 0x%04X:%02X aborted on slave %u.\n",
request->index, request->subindex, slave->ring_position);
if (rec_size >= 10) {
@@ -1574,34 +1575,37 @@
return;
}
+ if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response
+ EC_READ_U8(data + 2) >> 5 != 0x2) { // upload response
+ EC_ERR("Received unknown response while uploading SDO 0x%04X:%02X"
+ " from slave %u.\n", request->index, request->subindex,
+ slave->ring_position);
+ ec_print_data(data, rec_size);
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ rec_index = EC_READ_U16(data + 3);
+ rec_subindex = EC_READ_U8(data + 5);
+
+ if (rec_index != request->index || rec_subindex != request->subindex) {
+ EC_ERR("Received upload response for wrong SDO (0x%04X:%02X,"
+ " requested: 0x%04X:%02X) from slave %u.\n",
+ rec_index, rec_subindex, request->index, request->subindex,
+ slave->ring_position);
+ ec_print_data(data, rec_size);
+
+ // check for CoE response again
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_up_check;
+ return;
+ }
+
// normal or expedited?
expedited = EC_READ_U8(data + 2) & 0x02;
if (expedited) {
- if (rec_size < 7) {
- fsm->state = ec_fsm_coe_error;
- EC_ERR("Received currupted SDO expedited upload"
- " response (only %u bytes)!\n", rec_size);
- ec_print_data(data, rec_size);
- return;
- }
-
- if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response
- EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response
- EC_READ_U16(data + 3) != request->index || // index
- EC_READ_U8 (data + 5) != request->subindex) { // subindex
- if (fsm->slave->master->debug_level) {
- EC_DBG("Invalid SDO upload expedited response at slave %u!\n",
- slave->ring_position);
- ec_print_data(data, rec_size);
- }
- // check for CoE response again
- ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
- fsm->retries = EC_FSM_RETRIES;
- fsm->state = ec_fsm_coe_up_check;
- return;
- }
-
size_specified = EC_READ_U8(data + 2) & 0x01;
if (size_specified) {
fsm->complete_size = 4 - ((EC_READ_U8(data + 2) & 0x0C) >> 2);
@@ -1630,22 +1634,6 @@
return;
}
- if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response
- EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response
- EC_READ_U16(data + 3) != request->index || // index
- EC_READ_U8 (data + 5) != request->subindex) { // subindex
- if (fsm->slave->master->debug_level) {
- EC_DBG("Invalid SDO normal upload response at slave %u!\n",
- slave->ring_position);
- ec_print_data(data, rec_size);
- }
- // check for CoE response again
- ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
- fsm->retries = EC_FSM_RETRIES;
- fsm->state = ec_fsm_coe_up_check;
- return;
- }
-
data_size = rec_size - 10;
fsm->complete_size = EC_READ_U32(data + 6);
@@ -1862,7 +1850,7 @@
}
if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
- EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
+ EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
EC_ERR("SDO upload 0x%04X:%02X aborted on slave %u.\n",
request->index, request->subindex, slave->ring_position);
request->abort_code = EC_READ_U32(data + 6);