SDO segmented download.

master/fsm_coe.c

@@ -62,6 +62,8 @@ void ec_fsm_coe_down_start(ec_fsm_coe_t *);
 void ec_fsm_coe_down_request(ec_fsm_coe_t *);
 void ec_fsm_coe_down_check(ec_fsm_coe_t *);
 void ec_fsm_coe_down_response(ec_fsm_coe_t *);
+void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *);
+void ec_fsm_coe_down_seg_response(ec_fsm_coe_t *);
 
 void ec_fsm_coe_up_start(ec_fsm_coe_t *);
 void ec_fsm_coe_up_request(ec_fsm_coe_t *);
@@ -1071,7 +1073,7 @@ void ec_fsm_coe_down_start(ec_fsm_coe_t *fsm /**< finite state machine */)
     ec_slave_t *slave = fsm->slave;
     ec_sdo_request_t *request = fsm->request;
     uint8_t *data;
-    uint8_t size;
+    uint8_t data_set_size;
 
     if (fsm->slave->master->debug_level) {
         char subidxstr[10];
@@ -1091,6 +1093,12 @@ void ec_fsm_coe_down_start(ec_fsm_coe_t *fsm /**< finite state machine */)
         return;
     }
 
+    if (slave->configured_rx_mailbox_size < 16) {
+        EC_ERR("Mailbox too small!\n");
+        fsm->state = ec_fsm_coe_error;
+        return;
+    }
+
     if (request->data_size <= 4) { // use expedited transfer type
         data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10);
         if (IS_ERR(data)) {
@@ -1098,11 +1106,13 @@ void ec_fsm_coe_down_start(ec_fsm_coe_t *fsm /**< finite state machine */)
             return;
         }
 
-        size = 4 - request->data_size;
+        fsm->remaining = 0;
+
+        data_set_size = 4 - request->data_size;
 
         EC_WRITE_U16(data, 0x2 << 12); // SDO request
         EC_WRITE_U8 (data + 2, (0x3 // size specified, expedited
-                    | size << 2
+                    | data_set_size << 2
                     | ((request->complete_access ? 1 : 0) << 4) 
                     | 0x1 << 5)); // Download request
         EC_WRITE_U16(data + 3, request->index);
@@ -1117,19 +1127,26 @@ void ec_fsm_coe_down_start(ec_fsm_coe_t *fsm /**< finite state machine */)
         }
     }
     else { // request->data_size > 4, use normal transfer type
-        if (slave->configured_rx_mailbox_size < 6 + 10 + request->data_size) {
-            EC_ERR("SDO fragmenting not supported yet!\n"); // FIXME
-            fsm->state = ec_fsm_coe_error;
-            return;
+        size_t data_size;
+
+        if (slave->configured_rx_mailbox_size <
+                6 + 10 + request->data_size) {
+            // segmenting needed
+            data_size = slave->configured_rx_mailbox_size - 6;
+        } else {
+            data_size = 10 + request->data_size;
         }
 
         data = ec_slave_mbox_prepare_send(slave, datagram, 0x03,
-                request->data_size + 10);
+                data_size);
         if (IS_ERR(data)) {
             fsm->state = ec_fsm_coe_error;
             return;
         }
 
+        fsm->offset = 0;
+        fsm->remaining = request->data_size;
+
         EC_WRITE_U16(data, 0x2 << 12); // SDO request
         EC_WRITE_U8 (data + 2, (0x1 // size indicator, normal
                     | ((request->complete_access ? 1 : 0) << 4) 
@@ -1138,11 +1155,17 @@ void ec_fsm_coe_down_start(ec_fsm_coe_t *fsm /**< finite state machine */)
         EC_WRITE_U8 (data + 5,
                 request->complete_access ? 0x00 : request->subindex);
         EC_WRITE_U32(data + 6, request->data_size);
-        memcpy(data + 10, request->data, request->data_size);
+
+        if (data_size > 10) {
+            size_t segment_size = data_size - 10;
+            fsm->remaining -= segment_size;
+            memcpy(data + 10, request->data, segment_size);
+            fsm->offset += segment_size;
+        }
 
         if (slave->master->debug_level) {
             EC_DBG("Normal download request:\n");
-            ec_print_data(data, 10 + request->data_size);
+            ec_print_data(data, data_size);
         }
     }
 
@@ -1357,7 +1380,274 @@ void ec_fsm_coe_down_response(ec_fsm_coe_t *fsm /**< finite state machine */)
         return;
     }
 
-    fsm->state = ec_fsm_coe_end; // success
+    if (fsm->remaining) { // more segments to download
+        size_t max_segment_size = slave->configured_rx_mailbox_size - 9;
+        size_t segment_size, data_size;
+        uint8_t last_segment, seg_data_size;
+
+        if (fsm->remaining > max_segment_size) {
+            segment_size = max_segment_size;
+            last_segment = 0;
+        } else {
+            segment_size = fsm->remaining;
+            last_segment = 1;
+        }
+
+        if (segment_size > 7) {
+            seg_data_size = 0x00;
+            data_size = 3 + segment_size;
+        } else {
+            seg_data_size = 7 - segment_size;
+            data_size = 10;
+        }
+
+        data = ec_slave_mbox_prepare_send(slave, datagram, 0x03,
+                data_size);
+        if (IS_ERR(data)) {
+            fsm->state = ec_fsm_coe_error;
+            return;
+        }
+
+        fsm->toggle = 0;
+
+        EC_WRITE_U16(data, 0x2 << 12); // SDO request
+        EC_WRITE_U8(data + 2, (last_segment ? 1 : 0)
+                | (seg_data_size << 1) 
+                | (fsm->toggle << 4)
+                | (0x00 << 5)); // Download segment request
+        memcpy(data + 3, request->data + fsm->offset, segment_size);
+        if (segment_size < 7) {
+            memset(data + 3 + segment_size, 0x00, 7 - segment_size);
+        }
+
+        fsm->offset += segment_size;
+        fsm->remaining -= segment_size;
+
+        if (slave->master->debug_level) {
+            EC_DBG("Download segment request:\n");
+            ec_print_data(data, data_size);
+        }
+
+        fsm->state = ec_fsm_coe_down_seg_check; // success
+    } else {
+        fsm->state = ec_fsm_coe_end; // success
+    }
+}
+
+/*****************************************************************************/
+
+/**
+   CoE state: DOWN SEG CHECK.
+*/
+
+void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */)
+{
+    ec_datagram_t *datagram = fsm->datagram;
+    ec_slave_t *slave = fsm->slave;
+
+    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 %u"
+                " (datagram state %u).\n",
+               slave->ring_position, datagram->state);
+        return;
+    }
+
+    if (datagram->working_counter != 1) {
+        fsm->state = ec_fsm_coe_error;
+        EC_ERR("Reception of CoE mailbox segment check"
+                " datagram failed on slave %u: ", slave->ring_position);
+        ec_datagram_print_wc_error(datagram);
+        return;
+    }
+
+    if (!ec_slave_mbox_check(datagram)) {
+        unsigned long diff_ms =
+            (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+        if (diff_ms >= fsm->request->response_timeout) {
+            fsm->state = ec_fsm_coe_error;
+            EC_ERR("Timeout while waiting for SDO download segment response "
+                    "on slave %u.\n", slave->ring_position);
+            return;
+        }
+
+        ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+        fsm->retries = EC_FSM_RETRIES;
+        return;
+    }
+
+    // Fetch response
+    ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+    fsm->retries = EC_FSM_RETRIES;
+    fsm->state = ec_fsm_coe_down_seg_response;
+}
+
+/*****************************************************************************/
+
+/**
+   CoE state: DOWN SEG RESPONSE.
+   \todo Timeout behavior
+*/
+
+void ec_fsm_coe_down_seg_response(
+        ec_fsm_coe_t *fsm /**< finite state machine */
+        )
+{
+    ec_datagram_t *datagram = fsm->datagram;
+    ec_slave_t *slave = fsm->slave;
+    uint8_t *data, mbox_prot;
+    size_t rec_size;
+    ec_sdo_request_t *request = fsm->request;
+
+    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 %u (datagram state %u).\n",
+               slave->ring_position, datagram->state);
+        return;
+    }
+
+    if (datagram->working_counter != 1) {
+        fsm->state = ec_fsm_coe_error;
+        EC_ERR("Reception of CoE download response failed on slave %u: ",
+                slave->ring_position);
+        ec_datagram_print_wc_error(datagram);
+        return;
+    }
+
+    data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
+    if (IS_ERR(data)) {
+        fsm->state = ec_fsm_coe_error;
+        return;
+    }
+
+    if (mbox_prot != 0x03) { // CoE
+        fsm->state = ec_fsm_coe_error;
+        EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
+        return;
+    }
+
+    if (ec_fsm_coe_check_emergency(fsm, 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_down_check;
+        return;
+    }
+
+    if (slave->master->debug_level) {
+        EC_DBG("Download response:\n");
+        ec_print_data(data, rec_size);
+    }
+
+    if (rec_size < 6) {
+        fsm->state = ec_fsm_coe_error;
+        EC_ERR("Received data are too small (%zu bytes):\n", rec_size);
+        ec_print_data(data, rec_size);
+        return;
+    }
+
+    if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
+        EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
+        char subidxstr[10];
+        fsm->state = ec_fsm_coe_error;
+        if (request->complete_access) {
+            subidxstr[0] = 0x00;
+        } else {
+            sprintf(subidxstr, ":%02X", request->subindex);
+        }
+        EC_ERR("SDO download 0x%04X%s (%zu bytes) aborted on slave %u.\n",
+                request->index, subidxstr, request->data_size,
+                slave->ring_position);
+        if (rec_size < 10) {
+            EC_ERR("Incomplete abort command:\n");
+            ec_print_data(data, rec_size);
+        } else {
+            fsm->request->abort_code = EC_READ_U32(data + 6);
+            ec_canopen_abort_msg(fsm->request->abort_code);
+        }
+        return;
+    }
+
+    if (EC_READ_U16(data) >> 12 != 0x3 ||
+            ((EC_READ_U8(data + 2) >> 5) != 0x01)) { // segment response
+        if (slave->master->debug_level) {
+            EC_DBG("Invalid SDO download response at slave %u! Retrying...\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_down_seg_check;
+        return;
+    }
+
+    if (((EC_READ_U8(data + 2) >> 4) & 0x01) != fsm->toggle) {
+        EC_ERR("Invalid toggle received during segmented download:\n");
+        ec_print_data(data, rec_size);
+        fsm->state = ec_fsm_coe_error;
+        return;
+    }
+
+    if (fsm->remaining) { // more segments to download
+        size_t max_segment_size = slave->configured_rx_mailbox_size - 9;
+        size_t segment_size, data_size;
+        uint8_t last_segment, seg_data_size;
+
+        if (fsm->remaining > max_segment_size) {
+            segment_size = max_segment_size;
+            last_segment = 0;
+        } else {
+            segment_size = fsm->remaining;
+            last_segment = 1;
+        }
+
+        if (segment_size > 7) {
+            seg_data_size = 0x00;
+            data_size = 3 + segment_size;
+        } else {
+            seg_data_size = 7 - segment_size;
+            data_size = 10;
+        }
+
+        data = ec_slave_mbox_prepare_send(slave, datagram, 0x03,
+                data_size);
+        if (IS_ERR(data)) {
+            fsm->state = ec_fsm_coe_error;
+            return;
+        }
+
+        fsm->toggle = !fsm->toggle;
+
+        EC_WRITE_U16(data, 0x2 << 12); // SDO request
+        EC_WRITE_U8(data + 2, (last_segment ? 1 : 0)
+                | (seg_data_size << 1) 
+                | (fsm->toggle << 4)
+                | (0x00 << 5)); // Download segment request
+        memcpy(data + 3, request->data + fsm->offset, segment_size);
+        if (segment_size < 7) {
+            memset(data + 3 + segment_size, 0x00, 7 - segment_size);
+        }
+
+        fsm->offset += segment_size;
+        fsm->remaining -= segment_size;
+
+        if (slave->master->debug_level) {
+            EC_DBG("Download segment request:\n");
+            ec_print_data(data, data_size);
+        }
+
+        fsm->state = ec_fsm_coe_down_seg_check; // success
+    } else {
+        fsm->state = ec_fsm_coe_end; // success
+    }
 }
 
 /*****************************************************************************/

master/fsm_coe.h

@@ -61,6 +61,8 @@ struct ec_fsm_coe {
     ec_sdo_request_t *request; /**< SDO request */
     uint32_t complete_size; /**< Used when segmenting. */
     uint8_t toggle; /**< toggle bit for segment commands */
+    uint32_t offset; /**< Data offset during segmented download. */
+    uint32_t remaining; /**< Remaining bytes during segmented download. */
 };
 
 /*****************************************************************************/