Fixed mailbox bug, added mailbox error codes.

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;

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);

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;
 }
 

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 *);
 
 /*****************************************************************************/