--- a/master/fsm.c Wed Oct 11 12:35:10 2006 +0000
+++ b/master/fsm.c Thu Oct 12 13:49:01 2006 +0000
@@ -2064,7 +2064,7 @@
EC_WRITE_U16(data + 3, sdodata->index);
EC_WRITE_U8 (data + 5, sdodata->subindex);
EC_WRITE_U32(data + 6, sdodata->size);
- memcpy(data + 6, sdodata->data, sdodata->size);
+ memcpy(data + 10, sdodata->data, sdodata->size);
ec_master_queue_datagram(fsm->master, datagram);
fsm->coe_state = ec_fsm_coe_down_request;
@@ -2142,7 +2142,7 @@
{
ec_datagram_t *datagram = &fsm->datagram;
ec_slave_t *slave = fsm->slave;
- uint8_t *data;
+ uint8_t *data, mbox_type;
size_t rec_size;
ec_sdo_data_t *sdodata = fsm->sdodata;
@@ -2153,11 +2153,22 @@
return;
}
- if (!(data = ec_slave_mbox_fetch(slave, datagram, 0x03, &rec_size))) {
+ if (!(data = ec_slave_mbox_fetch(slave, datagram,
+ &mbox_type, &rec_size))) {
fsm->coe_state = ec_fsm_error;
return;
}
+ if (mbox_type != 0x03) {
+ EC_WARN("Received mailbox protocol 0x%02X as a response."
+ " Trying again.\n", mbox_type);
+ fsm->coe_start = get_cycles();
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ ec_master_queue_datagram(fsm->master, datagram);
+ fsm->coe_state = ec_fsm_coe_down_check;
+ return;
+ }
+
if (rec_size < 6) {
fsm->coe_state = ec_fsm_error;
EC_ERR("Received data is too small (%i bytes):\n", rec_size);