--- a/master/fsm_soe.c Thu Sep 06 14:21:02 2012 +0200
+++ b/master/fsm_soe.c Mon Nov 03 15:20:05 2014 +0100
@@ -64,18 +64,18 @@
/*****************************************************************************/
-void ec_fsm_soe_read_start(ec_fsm_soe_t *);
-void ec_fsm_soe_read_request(ec_fsm_soe_t *);
-void ec_fsm_soe_read_check(ec_fsm_soe_t *);
-void ec_fsm_soe_read_response(ec_fsm_soe_t *);
-
-void ec_fsm_soe_write_start(ec_fsm_soe_t *);
-void ec_fsm_soe_write_request(ec_fsm_soe_t *);
-void ec_fsm_soe_write_check(ec_fsm_soe_t *);
-void ec_fsm_soe_write_response(ec_fsm_soe_t *);
-
-void ec_fsm_soe_end(ec_fsm_soe_t *);
-void ec_fsm_soe_error(ec_fsm_soe_t *);
+void ec_fsm_soe_read_start(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_read_request(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_read_check(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_read_response(ec_fsm_soe_t *, ec_datagram_t *);
+
+void ec_fsm_soe_write_start(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_write_request(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_write_check(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_write_response(ec_fsm_soe_t *, ec_datagram_t *);
+
+void ec_fsm_soe_end(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_error(ec_fsm_soe_t *, ec_datagram_t *);
/*****************************************************************************/
@@ -105,12 +105,12 @@
/** Constructor.
*/
void ec_fsm_soe_init(
- ec_fsm_soe_t *fsm, /**< finite state machine */
- ec_mailbox_t *mbox /**< mailbox */
+ ec_fsm_soe_t *fsm /**< finite state machine */
)
{
fsm->state = NULL;
- fsm->mbox = mbox;
+ fsm->datagram = NULL;
+ fsm->fragment_size = 0;
}
/*****************************************************************************/
@@ -135,6 +135,7 @@
{
fsm->slave = slave;
fsm->request = request;
+
if (request->dir == EC_DIR_OUTPUT) {
fsm->state = ec_fsm_soe_write_start;
} else {
@@ -144,26 +145,46 @@
/*****************************************************************************/
-/**
- Executes the current state of the state machine.
- \return false, if state machine has terminated
-*/
-
-int ec_fsm_soe_exec(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
- fsm->state(fsm);
-
- return fsm->state != ec_fsm_soe_end && fsm->state != ec_fsm_soe_error;
-}
-
-/*****************************************************************************/
-
-/**
- Returns, if the state machine terminated with success.
- \return non-zero if successful.
-*/
-
-int ec_fsm_soe_success(ec_fsm_soe_t *fsm /**< Finite state machine */)
+/** Executes the current state of the state machine.
+ *
+ * \return 1 if the datagram was used, else 0.
+ */
+int ec_fsm_soe_exec(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ int datagram_used = 0;
+
+ if (fsm->datagram &&
+ (fsm->datagram->state == EC_DATAGRAM_INIT ||
+ fsm->datagram->state == EC_DATAGRAM_QUEUED ||
+ fsm->datagram->state == EC_DATAGRAM_SENT)) {
+ // datagram not received yet
+ return datagram_used;
+ }
+
+ fsm->state(fsm, datagram);
+
+ datagram_used =
+ fsm->state != ec_fsm_soe_end && fsm->state != ec_fsm_soe_error;
+
+ if (datagram_used) {
+ fsm->datagram = datagram;
+ } else {
+ fsm->datagram = NULL;
+ }
+
+ return datagram_used;
+}
+
+/*****************************************************************************/
+
+/** Returns, if the state machine terminated with success.
+ *
+ * \return non-zero if successful.
+ */
+int ec_fsm_soe_success(const ec_fsm_soe_t *fsm /**< Finite state machine */)
{
return fsm->state == ec_fsm_soe_end;
}
@@ -191,32 +212,24 @@
* SoE read state machine
*****************************************************************************/
-/** SoE state: READ START.
- */
-void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
- ec_mailbox_t *mbox = fsm->mbox;
+/** Prepare a read operation.
+ *
+ * \return 0 on success, otherwise a negative error code.
+ */
+int ec_fsm_soe_prepare_read(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ uint8_t *data;
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
ec_soe_request_t *request = fsm->request;
- uint8_t *data;
-
- EC_SLAVE_DBG(slave, 1, "Reading IDN 0x%04X of drive %u.\n", request->idn,
- request->drive_no);
-
- if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
- EC_SLAVE_ERR(slave, "Slave does not support SoE!\n");
- fsm->state = ec_fsm_soe_error;
- ec_fsm_soe_print_error(fsm);
- return;
- }
-
- data = ec_slave_mbox_prepare_send(slave, mbox, EC_MBOX_TYPE_SOE,
+
+ data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,
EC_SOE_SIZE);
if (IS_ERR(data)) {
- fsm->state = ec_fsm_soe_error;
- ec_fsm_soe_print_error(fsm);
- return;
+ return PTR_ERR(data);
}
EC_WRITE_U8(data, OPCODE_READ_REQUEST | (request->drive_no & 0x07) << 5);
@@ -228,52 +241,94 @@
ec_print_data(data, EC_SOE_SIZE);
}
- fsm->request->data_size = 0;
fsm->request->jiffies_sent = jiffies;
+ fsm->state = ec_fsm_soe_read_request;
+
+ return 0;
+}
+
+/*****************************************************************************/
+
+/** SoE state: READ START.
+ */
+void ec_fsm_soe_read_start(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_soe_request_t *request = fsm->request;
+
+ EC_SLAVE_DBG(slave, 1, "Reading IDN 0x%04X of drive %u.\n", request->idn,
+ request->drive_no);
+
+ if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
+ EC_SLAVE_ERR(slave, "Slave does not support SoE!\n");
+ fsm->state = ec_fsm_soe_error;
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ request->data_size = 0;
fsm->retries = EC_FSM_RETRIES;
- fsm->state = ec_fsm_soe_read_request;
+
+ if (ec_fsm_soe_prepare_read(fsm, datagram)) {
+ fsm->state = ec_fsm_soe_error;
+ ec_fsm_soe_print_error(fsm);
+ }
}
/*****************************************************************************/
/** SoE state: READ REQUEST.
*/
-void ec_fsm_soe_read_request(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
- ec_mailbox_t *mbox = fsm->mbox;
+void ec_fsm_soe_read_request(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
unsigned long diff_ms;
- if (ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_TIMED_OUT) && fsm->retries--)
- return; // FIXME: check for response first?
-
- if (!ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_RECEIVED)) {
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ if (ec_fsm_soe_prepare_read(fsm, datagram)) {
+ fsm->state = ec_fsm_soe_error;
+ ec_fsm_soe_print_error(fsm);
+ }
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Failed to receive SoE read request: ");
- ec_datagram_print_state(mbox->datagram);
+ ec_datagram_print_state(fsm->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;
- if (!ec_mbox_is_datagram_wc(mbox, 1)) {
- if (ec_mbox_is_datagram_wc(mbox, 0)) {
+ if (fsm->datagram->working_counter != 1) {
+ if (!fsm->datagram->working_counter) {
if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {
// no response; send request datagram again
+ if (ec_fsm_soe_prepare_read(fsm, datagram)) {
+ fsm->state = ec_fsm_soe_error;
+ ec_fsm_soe_print_error(fsm);
+ }
return;
}
}
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Reception of SoE read request"
" failed after %lu ms: ", diff_ms);
- ec_datagram_print_wc_error(mbox->datagram);
- ec_fsm_soe_print_error(fsm);
- return;
- }
-
- fsm->jiffies_start = mbox->datagram->jiffies_sent;
- ec_slave_mbox_prepare_check(slave, mbox); // can not fail.
+ ec_datagram_print_wc_error(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_soe_read_check;
}
@@ -282,34 +337,39 @@
/** CoE state: READ CHECK.
*/
-void ec_fsm_soe_read_check(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
- ec_mailbox_t *mbox = fsm->mbox;
- ec_slave_t *slave = fsm->slave;
-
- if (ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_TIMED_OUT) && fsm->retries--)
- return;
-
- if (!ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_RECEIVED)) {
+void ec_fsm_soe_read_check(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Failed to receive SoE mailbox check datagram: ");
- ec_datagram_print_state(mbox->datagram);
- ec_fsm_soe_print_error(fsm);
- return;
- }
-
- if (!ec_mbox_is_datagram_wc(mbox, 1)) {
+ ec_datagram_print_state(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Reception of SoE mailbox check"
" datagram failed: ");
- ec_datagram_print_wc_error(mbox->datagram);
- ec_fsm_soe_print_error(fsm);
- return;
- }
-
- if (!ec_slave_mbox_check(mbox)) {
+ ec_datagram_print_wc_error(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(fsm->datagram)) {
unsigned long diff_ms =
- (mbox->datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+ (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+ 1000 / HZ;
if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for"
@@ -318,13 +378,13 @@
return;
}
- ec_slave_mbox_prepare_check(slave, mbox); // can not fail.
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
return;
}
// Fetch response
- ec_slave_mbox_prepare_fetch(slave, mbox); // can not fail.
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_soe_read_response;
}
@@ -333,9 +393,11 @@
/** SoE state: READ RESPONSE.
*/
-void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
- ec_mailbox_t *mbox = fsm->mbox;
+void ec_fsm_soe_read_response(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
uint8_t *data, mbox_prot, header, opcode, incomplete, error_flag,
@@ -343,26 +405,28 @@
size_t rec_size, data_size;
ec_soe_request_t *req = fsm->request;
- if (ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_TIMED_OUT) && fsm->retries--)
- return; // FIXME: request again?
-
- if (!ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_RECEIVED)) {
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Failed to receive SoE read response datagram: ");
- ec_datagram_print_state(mbox->datagram);
- ec_fsm_soe_print_error(fsm);
- return;
- }
-
- if (!ec_mbox_is_datagram_wc(mbox, 1)) {
+ ec_datagram_print_state(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Reception of SoE read response failed: ");
- ec_datagram_print_wc_error(mbox->datagram);
- ec_fsm_soe_print_error(fsm);
- return;
- }
-
- data = ec_slave_mbox_fetch(slave, mbox, &mbox_prot, &rec_size);
+ ec_datagram_print_wc_error(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_soe_error;
ec_fsm_soe_print_error(fsm);
@@ -435,8 +499,8 @@
if (incomplete) {
EC_SLAVE_DBG(slave, 1, "SoE data incomplete. Waiting for fragment"
" at offset %zu.\n", req->data_size);
- fsm->jiffies_start = mbox->datagram->jiffies_sent;
- ec_slave_mbox_prepare_check(slave, mbox); // can not fail.
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_soe_read_check;
} else {
@@ -456,15 +520,15 @@
/** Write next fragment.
*/
void ec_fsm_soe_write_next_fragment(
- ec_fsm_soe_t *fsm /**< finite state machine */
- )
-{
- ec_mailbox_t *mbox = fsm->mbox;
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
ec_soe_request_t *req = fsm->request;
uint8_t incomplete, *data;
- size_t header_size, max_fragment_size, remaining_size, fragment_size;
+ size_t header_size, max_fragment_size, remaining_size;
uint16_t fragments_left;
header_size = EC_MBOX_HEADER_SIZE + EC_SOE_SIZE;
@@ -479,14 +543,14 @@
remaining_size = req->data_size - fsm->offset;
max_fragment_size = slave->configured_rx_mailbox_size - header_size;
incomplete = remaining_size > max_fragment_size;
- fragment_size = incomplete ? max_fragment_size : remaining_size;
- fragments_left = remaining_size / fragment_size - 1;
- if (remaining_size % fragment_size) {
+ fsm->fragment_size = incomplete ? max_fragment_size : remaining_size;
+ fragments_left = remaining_size / fsm->fragment_size - 1;
+ if (remaining_size % fsm->fragment_size) {
fragments_left++;
}
- data = ec_slave_mbox_prepare_send(slave, mbox, EC_MBOX_TYPE_SOE,
- EC_SOE_SIZE + fragment_size);
+ data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,
+ EC_SOE_SIZE + fsm->fragment_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_soe_error;
ec_fsm_soe_print_error(fsm);
@@ -497,16 +561,14 @@
(req->drive_no & 0x07) << 5);
EC_WRITE_U8(data + 1, 1 << 6); // only value included
EC_WRITE_U16(data + 2, incomplete ? fragments_left : req->idn);
- memcpy(data + 4, req->data + fsm->offset, fragment_size);
- fsm->offset += fragment_size;
+ memcpy(data + 4, req->data + fsm->offset, fsm->fragment_size);
if (master->debug_level) {
EC_SLAVE_DBG(slave, 0, "SCC write request:\n");
- ec_print_data(data, EC_SOE_SIZE + fragment_size);
+ ec_print_data(data, EC_SOE_SIZE + fsm->fragment_size);
}
req->jiffies_sent = jiffies;
- fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_soe_write_request;
}
@@ -514,7 +576,10 @@
/** SoE state: WRITE START.
*/
-void ec_fsm_soe_write_start(ec_fsm_soe_t *fsm /**< finite state machine */)
+void ec_fsm_soe_write_start(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
{
ec_slave_t *slave = fsm->slave;
ec_soe_request_t *req = fsm->request;
@@ -530,50 +595,56 @@
}
fsm->offset = 0;
- ec_fsm_soe_write_next_fragment(fsm);
+ fsm->retries = EC_FSM_RETRIES;
+ ec_fsm_soe_write_next_fragment(fsm, datagram);
}
/*****************************************************************************/
/** SoE state: WRITE REQUEST.
*/
-void ec_fsm_soe_write_request(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
- ec_mailbox_t *mbox = fsm->mbox;
+void ec_fsm_soe_write_request(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
unsigned long diff_ms;
- if (ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_TIMED_OUT) && fsm->retries--)
- return; // FIXME: check for response first?
-
- if (!ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_RECEIVED)) {
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_fsm_soe_write_next_fragment(fsm, datagram);
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Failed to receive SoE write request: ");
- ec_datagram_print_state(mbox->datagram);
+ ec_datagram_print_state(fsm->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;
- if (!ec_mbox_is_datagram_wc(mbox, 1)) {
- if (ec_mbox_is_datagram_wc(mbox, 0)) {
+ if (fsm->datagram->working_counter != 1) {
+ if (!fsm->datagram->working_counter) {
if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {
// no response; send request datagram again
+ ec_fsm_soe_write_next_fragment(fsm, datagram);
return;
}
}
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Reception of SoE write request"
" failed after %lu ms: ", diff_ms);
- ec_datagram_print_wc_error(mbox->datagram);
- ec_fsm_soe_print_error(fsm);
- return;
- }
-
- fsm->jiffies_start = mbox->datagram->jiffies_sent;
-
- ec_slave_mbox_prepare_check(slave, mbox); // can not fail.
+ ec_datagram_print_wc_error(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
+
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_soe_write_check;
}
@@ -582,64 +653,65 @@
/** CoE state: WRITE CHECK.
*/
-void ec_fsm_soe_write_check(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
- ec_mailbox_t *mbox = fsm->mbox;
- ec_slave_t *slave = fsm->slave;
- ec_soe_request_t *req = fsm->request;
-
- if (ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_TIMED_OUT) && fsm->retries--)
- return;
-
- if (!ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_RECEIVED)) {
+void ec_fsm_soe_write_check(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Failed to receive SoE write request datagram: ");
- ec_datagram_print_state(mbox->datagram);
- ec_fsm_soe_print_error(fsm);
- return;
- }
-
- if (!ec_mbox_is_datagram_wc(mbox, 1)) {
+ ec_datagram_print_state(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Reception of SoE write request datagram: ");
- ec_datagram_print_wc_error(mbox->datagram);
- ec_fsm_soe_print_error(fsm);
- return;
- }
-
- if (fsm->offset < req->data_size) {
- ec_fsm_soe_write_next_fragment(fsm);
- } else {
- if (!ec_slave_mbox_check(mbox)) {
- unsigned long diff_ms =
- (mbox->datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
- if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
- fsm->state = ec_fsm_soe_error;
- EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting"
- " for write response.\n", diff_ms);
- ec_fsm_soe_print_error(fsm);
- return;
- }
-
- ec_slave_mbox_prepare_check(slave, mbox); // can not fail.
- fsm->retries = EC_FSM_RETRIES;
+ ec_datagram_print_wc_error(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(fsm->datagram)) {
+ unsigned long diff_ms =
+ (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+ if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
+ fsm->state = ec_fsm_soe_error;
+ EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting"
+ " for write response.\n", diff_ms);
+ ec_fsm_soe_print_error(fsm);
return;
}
- // Fetch response
- ec_slave_mbox_prepare_fetch(slave, mbox); // can not fail.
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
- fsm->state = ec_fsm_soe_write_response;
- }
+ return;
+ }
+
+ // Fetch response
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_soe_write_response;
}
/*****************************************************************************/
/** SoE state: WRITE RESPONSE.
*/
-void ec_fsm_soe_write_response(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
- ec_mailbox_t *mbox = fsm->mbox;
+void ec_fsm_soe_write_response(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
ec_soe_request_t *req = fsm->request;
@@ -647,27 +719,29 @@
uint16_t idn;
size_t rec_size;
- if (ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_TIMED_OUT) && fsm->retries--)
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
return; // FIXME: request again?
-
- if (!ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_RECEIVED)) {
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Failed to receive SoE write"
" response datagram: ");
- ec_datagram_print_state(mbox->datagram);
- ec_fsm_soe_print_error(fsm);
- return;
- }
-
- if (!ec_mbox_is_datagram_wc(mbox, 1)) {
+ ec_datagram_print_state(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Reception of SoE write response failed: ");
- ec_datagram_print_wc_error(mbox->datagram);
- ec_fsm_soe_print_error(fsm);
- return;
- }
-
- data = ec_slave_mbox_fetch(slave, mbox, &mbox_prot, &rec_size);
+ ec_datagram_print_wc_error(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_soe_error;
ec_fsm_soe_print_error(fsm);
@@ -689,7 +763,7 @@
if (rec_size < EC_SOE_SIZE) {
fsm->state = ec_fsm_soe_error;
- EC_SLAVE_ERR(slave, "Received currupted SoE write response"
+ EC_SLAVE_ERR(slave, "Received corrupted SoE write response"
" (%zu bytes)!\n", rec_size);
ec_print_data(data, rec_size);
ec_fsm_soe_print_error(fsm);
@@ -735,14 +809,24 @@
req->error_code = 0x0000;
}
- fsm->state = ec_fsm_soe_end; // success
+ fsm->offset += fsm->fragment_size;
+
+ if (fsm->offset < req->data_size) {
+ fsm->retries = EC_FSM_RETRIES;
+ ec_fsm_soe_write_next_fragment(fsm, datagram);
+ } else {
+ fsm->state = ec_fsm_soe_end; // success
+ }
}
/*****************************************************************************/
/** State: ERROR.
*/
-void ec_fsm_soe_error(ec_fsm_soe_t *fsm /**< finite state machine */)
+void ec_fsm_soe_error(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
{
}
@@ -750,8 +834,11 @@
/** State: END.
*/
-void ec_fsm_soe_end(ec_fsm_soe_t *fsm /**< finite state machine */)
-{
-}
-
-/*****************************************************************************/
+void ec_fsm_soe_end(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+}
+
+/*****************************************************************************/