--- a/master/fsm_coe.c Thu Nov 19 13:53:33 2009 +0100
+++ b/master/fsm_coe.c Fri Nov 20 19:01:38 2009 +0100
@@ -62,6 +62,8 @@
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 @@
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 @@
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 @@
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 @@
}
}
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 @@
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 @@
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
+ }
}
/*****************************************************************************/