diff --git a/master/ethernet.c b/master/ethernet.c
index 44cc42a94cddf0de249e3ff6841d7b949a461531..316dd1309e7eef5a1c6e838fe9472de684537099 100644
--- a/master/ethernet.c
+++ b/master/ethernet.c
@@ -362,7 +362,7 @@ void ec_eoe_state_rx_check(ec_eoe_t *eoe /**< EoE handler */)
void ec_eoe_state_rx_fetch(ec_eoe_t *eoe /**< EoE handler */)
{
size_t rec_size, data_size;
- uint8_t *data, frame_type, last_fragment, time_appended;
+ uint8_t *data, frame_type, last_fragment, time_appended, mbox_type;
uint8_t frame_number, fragment_offset, fragment_number;
off_t offset;
#if EOE_DEBUG_LEVEL > 1
@@ -376,7 +376,13 @@ void ec_eoe_state_rx_fetch(ec_eoe_t *eoe /**< EoE handler */)
}
if (!(data = ec_slave_mbox_fetch(eoe->slave, &eoe->datagram,
- 0x02, &rec_size))) {
+ &mbox_type, &rec_size))) {
+ eoe->stats.rx_errors++;
+ eoe->state = ec_eoe_state_tx_start;
+ return;
+ }
+
+ if (mbox_type != 0x02) {
eoe->stats.rx_errors++;
eoe->state = ec_eoe_state_tx_start;
return;
diff --git a/master/fsm.c b/master/fsm.c
index f310c02f82e06ac1e53d0e5a30a61cdd70c94556..6a35929a5b73c524d210cceb13904d0c115c5f7d 100644
--- a/master/fsm.c
+++ b/master/fsm.c
@@ -2064,7 +2064,7 @@ void ec_fsm_coe_down_start(ec_fsm_t *fsm /**< finite state machine */)
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 @@ void ec_fsm_coe_down_response(ec_fsm_t *fsm /**< finite state machine */)
{
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 @@ void ec_fsm_coe_down_response(ec_fsm_t *fsm /**< finite state machine */)
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);
diff --git a/master/mailbox.c b/master/mailbox.c
index 45176770524db64bc7baefb497b1c5e8ed865408..ac317d0f1bdc3d59a0c5308e771623c99c736fbf 100644
--- a/master/mailbox.c
+++ b/master/mailbox.c
@@ -134,6 +134,24 @@ int ec_slave_mbox_prepare_fetch(const ec_slave_t *slave, /**< slave */
/*****************************************************************************/
+/**
+ Mailbox error codes.
+*/
+
+const ec_code_msg_t mbox_error_messages[] = {
+ {0x00000001, "MBXERR_SYNTAX"},
+ {0x00000002, "MBXERR_UNSUPPORTEDPROTOCOL"},
+ {0x00000003, "MBXERR_INVAILDCHANNEL"},
+ {0x00000004, "MBXERR_SERVICENOTSUPPORTED"},
+ {0x00000005, "MBXERR_INVALIDHEADER"},
+ {0x00000006, "MBXERR_SIZETOOSHORT"},
+ {0x00000007, "MBXERR_NOMOREMEMORY"},
+ {0x00000008, "MBXERR_INVALIDSIZE"},
+ {}
+};
+
+/*****************************************************************************/
+
/**
Processes received mailbox data.
\return pointer to the received data
@@ -141,26 +159,39 @@ int ec_slave_mbox_prepare_fetch(const ec_slave_t *slave, /**< slave */
uint8_t *ec_slave_mbox_fetch(const ec_slave_t *slave, /**< slave */
ec_datagram_t *datagram, /**< datagram */
- uint8_t type, /**< expected mailbox protocol */
+ uint8_t *type, /**< expected mailbox protocol */
size_t *size /**< size of the received data */
)
{
size_t data_size;
- if ((EC_READ_U8(datagram->data + 5) & 0x0F) != type) {
- EC_ERR("Unexpected mailbox protocol 0x%02X (exp.: 0x%02X) at"
- " slave %i!\n", EC_READ_U8(datagram->data + 5), type,
- slave->ring_position);
- return NULL;
- }
-
if ((data_size = EC_READ_U16(datagram->data)) >
slave->sii_tx_mailbox_size - 6) {
- EC_ERR("Currupt mailbox response detected!\n");
+ EC_ERR("Corrupt mailbox response detected!\n");
return NULL;
}
+ *type = EC_READ_U8(datagram->data + 5) & 0x0F;
*size = data_size;
+
+ if (*type == 0x00) {
+ const ec_code_msg_t *mbox_msg;
+ uint16_t code = EC_READ_U16(datagram->data + 8);
+
+ EC_ERR("Mailbox error response received.\n");
+ for (mbox_msg = mbox_error_messages; mbox_msg->code; mbox_msg++) {
+ if (mbox_msg->code != code) continue;
+ EC_ERR("Error reply code: 0x%04X: \"%s\".\n",
+ mbox_msg->code, mbox_msg->message);
+ break;
+ }
+
+ if (!mbox_msg->code)
+ EC_ERR("Unknown error reply code 0x%04X.\n", code);
+
+ return NULL;
+ }
+
return datagram->data + 6;
}
diff --git a/master/mailbox.h b/master/mailbox.h
index 263ab8aeb9bf7bb039db69931f4f5ca18a62f280..cc989385dee0242aace362c7224a96e0f02ab54c 100644
--- a/master/mailbox.h
+++ b/master/mailbox.h
@@ -51,7 +51,7 @@ int ec_slave_mbox_prepare_check(const ec_slave_t *, ec_datagram_t *);
int ec_slave_mbox_check(const ec_datagram_t *);
int ec_slave_mbox_prepare_fetch(const ec_slave_t *, ec_datagram_t *);
uint8_t *ec_slave_mbox_fetch(const ec_slave_t *, ec_datagram_t *,
- uint8_t, size_t *);
+ uint8_t *, size_t *);
/*****************************************************************************/