master/fsm_slave.c
changeset 2589 2b9c78543663
parent 2414 f35c7c8e6591
child 2597 0e145bb05859
--- a/master/fsm_slave.c	Thu Sep 06 14:21:02 2012 +0200
+++ b/master/fsm_slave.c	Mon Nov 03 15:20:05 2014 +0100
@@ -2,7 +2,7 @@
  *
  *  $Id$
  *
- *  Copyright (C) 2006-2008  Florian Pose, Ingenieurgemeinschaft IgH
+ *  Copyright (C) 2006-2012  Florian Pose, Ingenieurgemeinschaft IgH
  *
  *  This file is part of the IgH EtherCAT Master.
  *
@@ -36,19 +36,22 @@
 #include "globals.h"
 #include "master.h"
 #include "mailbox.h"
+#include "slave_config.h"
 
 #include "fsm_slave.h"
 
 /*****************************************************************************/
 
-void ec_fsm_slave_state_idle(ec_fsm_slave_t *);
-void ec_fsm_slave_state_ready(ec_fsm_slave_t *);
-int ec_fsm_slave_action_process_sdo(ec_fsm_slave_t *);
-void ec_fsm_slave_state_sdo_request(ec_fsm_slave_t *);
-int ec_fsm_slave_action_process_foe(ec_fsm_slave_t *);
-void ec_fsm_slave_state_foe_request(ec_fsm_slave_t *);
-int ec_fsm_slave_action_process_soe(ec_fsm_slave_t *);
-void ec_fsm_slave_state_soe_request(ec_fsm_slave_t *);
+void ec_fsm_slave_state_idle(ec_fsm_slave_t *, ec_datagram_t *);
+void ec_fsm_slave_state_ready(ec_fsm_slave_t *, ec_datagram_t *);
+int ec_fsm_slave_action_process_sdo(ec_fsm_slave_t *, ec_datagram_t *);
+void ec_fsm_slave_state_sdo_request(ec_fsm_slave_t *, ec_datagram_t *);
+int ec_fsm_slave_action_process_reg(ec_fsm_slave_t *, ec_datagram_t *);
+void ec_fsm_slave_state_reg_request(ec_fsm_slave_t *, ec_datagram_t *);
+int ec_fsm_slave_action_process_foe(ec_fsm_slave_t *, ec_datagram_t *);
+void ec_fsm_slave_state_foe_request(ec_fsm_slave_t *, ec_datagram_t *);
+int ec_fsm_slave_action_process_soe(ec_fsm_slave_t *, ec_datagram_t *);
+void ec_fsm_slave_state_soe_request(ec_fsm_slave_t *, ec_datagram_t *);
 
 /*****************************************************************************/
 
@@ -56,22 +59,23 @@
  */
 void ec_fsm_slave_init(
         ec_fsm_slave_t *fsm, /**< Slave state machine. */
-        ec_slave_t *slave, /**< EtherCAT slave. */
-        ec_mailbox_t *mbox/**< Datagram object to use. */
+        ec_slave_t *slave /**< EtherCAT slave. */
         )
 {
     fsm->slave = slave;
-    fsm->mbox = mbox;
-    slave->datagram.data_size = 0;
-
-    EC_SLAVE_DBG(slave, 1, "Init FSM.\n");
+    INIT_LIST_HEAD(&fsm->list); // mark as unlisted
 
     fsm->state = ec_fsm_slave_state_idle;
-
-    // init sub-state-machines
-    ec_fsm_coe_init(&fsm->fsm_coe, fsm->mbox);
-    ec_fsm_foe_init(&fsm->fsm_foe, fsm->mbox);
-    ec_fsm_soe_init(&fsm->fsm_soe, fsm->mbox);
+    fsm->datagram = NULL;
+    fsm->sdo_request = NULL;
+    fsm->reg_request = NULL;
+    fsm->foe_request = NULL;
+    fsm->soe_request = NULL;
+
+    // Init sub-state-machines
+    ec_fsm_coe_init(&fsm->fsm_coe);
+    ec_fsm_foe_init(&fsm->fsm_foe);
+    ec_fsm_soe_init(&fsm->fsm_soe);
 }
 
 /*****************************************************************************/
@@ -82,6 +86,28 @@
         ec_fsm_slave_t *fsm /**< Master state machine. */
         )
 {
+    // signal requests that are currently in operation
+
+    if (fsm->sdo_request) {
+        fsm->sdo_request->state = EC_INT_REQUEST_FAILURE;
+        wake_up_all(&fsm->slave->master->request_queue);
+    }
+
+    if (fsm->reg_request) {
+        fsm->reg_request->state = EC_INT_REQUEST_FAILURE;
+        wake_up_all(&fsm->slave->master->request_queue);
+    }
+
+    if (fsm->foe_request) {
+        fsm->foe_request->state = EC_INT_REQUEST_FAILURE;
+        wake_up_all(&fsm->slave->master->request_queue);
+    }
+
+    if (fsm->soe_request) {
+        fsm->soe_request->state = EC_INT_REQUEST_FAILURE;
+        wake_up_all(&fsm->slave->master->request_queue);
+    }
+
     // clear sub-state machines
     ec_fsm_coe_clear(&fsm->fsm_coe);
     ec_fsm_foe_clear(&fsm->fsm_foe);
@@ -92,30 +118,34 @@
 
 /** Executes the current state of the state machine.
  *
- * If the state machine's datagram is not sent or received yet, the execution
- * of the state machine is delayed to the next cycle.
- *
- * \return true, if the state machine was executed
+ * \return 1 if \a datagram was used, else 0.
  */
 int ec_fsm_slave_exec(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
-        )
-{
-    if (ec_mbox_is_datagram_state(fsm->mbox, EC_DATAGRAM_QUEUED)
-        || ec_mbox_is_datagram_state(fsm->mbox, EC_DATAGRAM_SENT)) {
-        // datagram was not sent or received yet.
-        return 0;
-    }
-
-    fsm->state(fsm);
-    return 1;
+        ec_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< New datagram to use. */
+        )
+{
+    int datagram_used;
+
+    fsm->state(fsm, datagram);
+
+    datagram_used = fsm->state != ec_fsm_slave_state_idle &&
+        fsm->state != ec_fsm_slave_state_ready;
+
+    if (datagram_used) {
+        fsm->datagram = datagram;
+    } else {
+        fsm->datagram = NULL;
+    }
+
+    return datagram_used;
 }
 
 /*****************************************************************************/
 
 /** Sets the current state of the state machine to READY
  */
-void ec_fsm_slave_ready(
+void ec_fsm_slave_set_ready(
         ec_fsm_slave_t *fsm /**< Slave state machine. */
         )
 {
@@ -125,6 +155,19 @@
     }
 }
 
+/*****************************************************************************/
+
+/** Returns, if the FSM is currently not busy and ready to execute.
+ *
+ * \return Non-zero if ready.
+ */
+int ec_fsm_slave_is_ready(
+        const ec_fsm_slave_t *fsm /**< Slave state machine. */
+        )
+{
+    return fsm->state == ec_fsm_slave_state_ready;
+}
+
 /******************************************************************************
  * Slave state machine
  *****************************************************************************/
@@ -132,7 +175,8 @@
 /** Slave state: IDLE.
  */
 void ec_fsm_slave_state_idle(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
+        ec_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     // do nothing
@@ -143,20 +187,29 @@
 /** Slave state: READY.
  */
 void ec_fsm_slave_state_ready(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
+        ec_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     // Check for pending external SDO requests
-    if (ec_fsm_slave_action_process_sdo(fsm))
-        return;
+    if (ec_fsm_slave_action_process_sdo(fsm, datagram)) {
+        return;
+    }
+
+    // Check for pending external register requests
+    if (ec_fsm_slave_action_process_reg(fsm, datagram)) {
+        return;
+    }
 
     // Check for pending FoE requests
-    if (ec_fsm_slave_action_process_foe(fsm))
-        return;
+    if (ec_fsm_slave_action_process_foe(fsm, datagram)) {
+        return;
+    }
 
     // Check for pending SoE requests
-    if (ec_fsm_slave_action_process_soe(fsm))
-        return;
+    if (ec_fsm_slave_action_process_soe(fsm, datagram)) {
+        return;
+    }
 }
 
 /*****************************************************************************/
@@ -166,52 +219,49 @@
  * \return non-zero, if an SDO request is processed.
  */
 int ec_fsm_slave_action_process_sdo(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
-        )
-{
-    ec_slave_t *slave = fsm->slave;
-    ec_master_sdo_request_t *request, *next;
-
-    // search the first external request to be processed
-    list_for_each_entry_safe(request, next, &slave->slave_sdo_requests, list) {
-
-        list_del_init(&request->list); // dequeue
-        if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
-            EC_SLAVE_WARN(slave, "Aborting SDO request %p,"
-                    " slave has error flag set.\n",request);
-            request->req.state = EC_INT_REQUEST_FAILURE;
-            kref_put(&request->refcount,ec_master_sdo_request_release);
-            wake_up(&slave->sdo_queue);
-            fsm->sdo_request = NULL;
-            fsm->state = ec_fsm_slave_state_idle;
-            return 0;
-        }
-
-        if (slave->current_state == EC_SLAVE_STATE_INIT) {
-            EC_SLAVE_WARN(slave, "Aborting SDO request %p,"
-                    " slave is in INIT.\n", request);
-            request->req.state = EC_INT_REQUEST_FAILURE;
-            kref_put(&request->refcount,ec_master_sdo_request_release);
-            wake_up(&slave->sdo_queue);
-            fsm->sdo_request = NULL;
-            fsm->state = ec_fsm_slave_state_idle;
-            return 0;
-        }
-
-        request->req.state = EC_INT_REQUEST_BUSY;
-
-        // Found pending SDO request. Execute it!
-        EC_SLAVE_DBG(slave, 1, "Processing SDO request %p...\n", request);
-
-        // Start SDO transfer
-        fsm->sdo_request = request;
-        fsm->state = ec_fsm_slave_state_sdo_request;
-        ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req);
-        ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
-        ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
+        ec_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+    ec_sdo_request_t *request;
+
+    if (list_empty(&slave->sdo_requests)) {
+        return 0;
+    }
+
+    // take the first request to be processed
+    request = list_entry(slave->sdo_requests.next, ec_sdo_request_t, list);
+    list_del_init(&request->list); // dequeue
+
+    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
+        EC_SLAVE_WARN(slave, "Aborting SDO request,"
+                " slave has error flag set.\n");
+        request->state = EC_INT_REQUEST_FAILURE;
+        wake_up_all(&slave->master->request_queue);
+        fsm->state = ec_fsm_slave_state_idle;
         return 1;
     }
-    return 0;
+
+    if (slave->current_state == EC_SLAVE_STATE_INIT) {
+        EC_SLAVE_WARN(slave, "Aborting SDO request, slave is in INIT.\n");
+        request->state = EC_INT_REQUEST_FAILURE;
+        wake_up_all(&slave->master->request_queue);
+        fsm->state = ec_fsm_slave_state_idle;
+        return 1;
+    }
+
+    fsm->sdo_request = request;
+    request->state = EC_INT_REQUEST_BUSY;
+
+    // Found pending SDO request. Execute it!
+    EC_SLAVE_DBG(slave, 1, "Processing SDO request...\n");
+
+    // Start SDO transfer
+    fsm->state = ec_fsm_slave_state_sdo_request;
+    ec_fsm_coe_transfer(&fsm->fsm_coe, slave, request);
+    ec_fsm_coe_exec(&fsm->fsm_coe, datagram); // execute immediately
+    return 1;
 }
 
 /*****************************************************************************/
@@ -219,75 +269,193 @@
 /** Slave state: SDO_REQUEST.
  */
 void ec_fsm_slave_state_sdo_request(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
-        )
-{
-    ec_slave_t *slave = fsm->slave;
-    ec_master_sdo_request_t *request = fsm->sdo_request;
-
-    if (ec_fsm_coe_exec(&fsm->fsm_coe)) {
-        ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
-        return;
-    }
+        ec_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+    ec_sdo_request_t *request = fsm->sdo_request;
+
+    if (ec_fsm_coe_exec(&fsm->fsm_coe, datagram)) {
+        return;
+    }
+
     if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
-        EC_SLAVE_ERR(slave, "Failed to process SDO request %p.\n", request);
-        request->req.state = EC_INT_REQUEST_FAILURE;
-        kref_put(&request->refcount, ec_master_sdo_request_release);
-        wake_up(&slave->sdo_queue);
+        EC_SLAVE_ERR(slave, "Failed to process SDO request.\n");
+        request->state = EC_INT_REQUEST_FAILURE;
+        wake_up_all(&slave->master->request_queue);
         fsm->sdo_request = NULL;
-        fsm->state = ec_fsm_slave_state_idle;
-        return;
-    }
-
-    EC_SLAVE_DBG(slave, 1, "Finished SDO request %p.\n", request);
+        fsm->state = ec_fsm_slave_state_ready;
+        return;
+    }
+
+    EC_SLAVE_DBG(slave, 1, "Finished SDO request.\n");
 
     // SDO request finished
-    request->req.state = EC_INT_REQUEST_SUCCESS;
-    kref_put(&request->refcount, ec_master_sdo_request_release);
-    wake_up(&slave->sdo_queue);
-
+    request->state = EC_INT_REQUEST_SUCCESS;
+    wake_up_all(&slave->master->request_queue);
     fsm->sdo_request = NULL;
     fsm->state = ec_fsm_slave_state_ready;
 }
 
 /*****************************************************************************/
 
-/** Check for pending FOE requests and process one.
- *
- * \return non-zero, if an FOE request is processed.
+/** Check for pending register requests and process one.
+ *
+ * \return non-zero, if a register request is processed.
+ */
+int ec_fsm_slave_action_process_reg(
+        ec_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+    ec_reg_request_t *reg;
+
+    fsm->reg_request = NULL;
+
+    if (slave->config) {
+        // search the first internal register request to be processed
+        list_for_each_entry(reg, &slave->config->reg_requests, list) {
+            if (reg->state == EC_INT_REQUEST_QUEUED) {
+                fsm->reg_request = reg;
+                break;
+            }
+        }
+    }
+
+    if (!fsm->reg_request && !list_empty(&slave->reg_requests)) {
+        // take the first external request to be processed
+        fsm->reg_request =
+            list_entry(slave->reg_requests.next, ec_reg_request_t, list);
+        list_del_init(&fsm->reg_request->list); // dequeue
+    }
+
+    if (!fsm->reg_request) { // no register request to process
+        return 0;
+    }
+
+    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
+        EC_SLAVE_WARN(slave, "Aborting register request,"
+                " slave has error flag set.\n");
+        fsm->reg_request->state = EC_INT_REQUEST_FAILURE;
+        wake_up_all(&slave->master->request_queue);
+        fsm->reg_request = NULL;
+        fsm->state = ec_fsm_slave_state_idle;
+        return 1;
+    }
+
+    // Found pending register request. Execute it!
+    EC_SLAVE_DBG(slave, 1, "Processing register request...\n");
+
+    fsm->reg_request->state = EC_INT_REQUEST_BUSY;
+
+    // Start register access
+    if (fsm->reg_request->dir == EC_DIR_INPUT) {
+        ec_datagram_fprd(datagram, slave->station_address,
+                fsm->reg_request->address, fsm->reg_request->transfer_size);
+        ec_datagram_zero(datagram);
+    } else {
+        ec_datagram_fpwr(datagram, slave->station_address,
+                fsm->reg_request->address, fsm->reg_request->transfer_size);
+        memcpy(datagram->data, fsm->reg_request->data,
+                fsm->reg_request->transfer_size);
+    }
+    datagram->device_index = slave->device_index;
+    fsm->state = ec_fsm_slave_state_reg_request;
+    return 1;
+}
+
+/*****************************************************************************/
+
+/** Slave state: Register request.
+ */
+void ec_fsm_slave_state_reg_request(
+        ec_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+    ec_reg_request_t *reg = fsm->reg_request;
+
+    if (!reg) {
+        // configuration was cleared in the meantime
+        fsm->state = ec_fsm_slave_state_ready;
+        fsm->reg_request = NULL;
+        return;
+    }
+
+    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+        EC_SLAVE_ERR(slave, "Failed to receive register"
+                " request datagram: ");
+        ec_datagram_print_state(fsm->datagram);
+        reg->state = EC_INT_REQUEST_FAILURE;
+        wake_up_all(&slave->master->request_queue);
+        fsm->reg_request = NULL;
+        fsm->state = ec_fsm_slave_state_ready;
+        return;
+    }
+
+    if (fsm->datagram->working_counter == 1) {
+        if (reg->dir == EC_DIR_INPUT) { // read request
+            memcpy(reg->data, fsm->datagram->data, reg->transfer_size);
+        }
+
+        reg->state = EC_INT_REQUEST_SUCCESS;
+        EC_SLAVE_DBG(slave, 1, "Register request successful.\n");
+    } else {
+        reg->state = EC_INT_REQUEST_FAILURE;
+        ec_datagram_print_state(fsm->datagram);
+        EC_SLAVE_ERR(slave, "Register request failed"
+                " (working counter is %u).\n",
+                fsm->datagram->working_counter);
+    }
+
+    wake_up_all(&slave->master->request_queue);
+    fsm->reg_request = NULL;
+    fsm->state = ec_fsm_slave_state_ready;
+}
+
+/*****************************************************************************/
+
+/** Check for pending FoE requests and process one.
+ *
+ * \return non-zero, if an FoE request is processed.
  */
 int ec_fsm_slave_action_process_foe(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
-        )
-{
-    ec_slave_t *slave = fsm->slave;
-    ec_master_foe_request_t *request, *next;
-
-    // search the first request to be processed
-    list_for_each_entry_safe(request, next, &slave->foe_requests, list) {
-        if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
-            EC_SLAVE_WARN(slave, "Aborting FOE request %p,"
-                    " slave has error flag set.\n", request);
-            request->req.state = EC_INT_REQUEST_FAILURE;
-            kref_put(&request->refcount, ec_master_foe_request_release);
-            wake_up(&slave->foe_queue);
-            fsm->sdo_request = NULL;
-            fsm->state = ec_fsm_slave_state_idle;
-            return 0;
-        }
-        list_del_init(&request->list); // dequeue
-        request->req.state = EC_INT_REQUEST_BUSY;
-
-        EC_SLAVE_DBG(slave, 1, "Processing FoE request %p.\n", request);
-
-        fsm->foe_request = request;
-        fsm->state = ec_fsm_slave_state_foe_request;
-        ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req);
-        ec_fsm_foe_exec(&fsm->fsm_foe);
-        ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
+        ec_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+    ec_foe_request_t *request;
+
+    if (list_empty(&slave->foe_requests)) {
+        return 0;
+    }
+
+    // take the first request to be processed
+    request = list_entry(slave->foe_requests.next, ec_foe_request_t, list);
+    list_del_init(&request->list); // dequeue
+
+    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
+        EC_SLAVE_WARN(slave, "Aborting FoE request,"
+                " slave has error flag set.\n");
+        request->state = EC_INT_REQUEST_FAILURE;
+        wake_up_all(&slave->master->request_queue);
+        fsm->state = ec_fsm_slave_state_idle;
         return 1;
     }
-    return 0;
+
+    request->state = EC_INT_REQUEST_BUSY;
+    fsm->foe_request = request;
+
+    EC_SLAVE_DBG(slave, 1, "Processing FoE request.\n");
+
+    fsm->state = ec_fsm_slave_state_foe_request;
+    ec_fsm_foe_transfer(&fsm->fsm_foe, slave, request);
+    ec_fsm_foe_exec(&fsm->fsm_foe, datagram);
+    return 1;
 }
 
 /*****************************************************************************/
@@ -295,35 +463,32 @@
 /** Slave state: FOE REQUEST.
  */
 void ec_fsm_slave_state_foe_request(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
-        )
-{
-    ec_slave_t *slave = fsm->slave;
-    ec_master_foe_request_t *request = fsm->foe_request;
-
-    if (ec_fsm_foe_exec(&fsm->fsm_foe)) {
-        ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
+        ec_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+    ec_foe_request_t *request = fsm->foe_request;
+
+    if (ec_fsm_foe_exec(&fsm->fsm_foe, datagram)) {
         return;
     }
 
     if (!ec_fsm_foe_success(&fsm->fsm_foe)) {
-        EC_SLAVE_ERR(slave, "Failed to handle FoE request %p.\n", request);
-        request->req.state = EC_INT_REQUEST_FAILURE;
-        kref_put(&request->refcount, ec_master_foe_request_release);
-        wake_up(&slave->foe_queue);
+        EC_SLAVE_ERR(slave, "Failed to handle FoE request.\n");
+        request->state = EC_INT_REQUEST_FAILURE;
+        wake_up_all(&slave->master->request_queue);
         fsm->foe_request = NULL;
-        fsm->state = ec_fsm_slave_state_idle;
+        fsm->state = ec_fsm_slave_state_ready;
         return;
     }
 
     // finished transferring FoE
-    EC_SLAVE_DBG(slave, 1, "FoE request %p successfully"
-            " transferred %zu bytes.\n", request, request->req.data_size);
-
-    request->req.state = EC_INT_REQUEST_SUCCESS;
-    kref_put(&request->refcount, ec_master_foe_request_release);
-    wake_up(&slave->foe_queue);
-
+    EC_SLAVE_DBG(slave, 1, "Successfully transferred %zu bytes of FoE"
+            " data.\n", request->data_size);
+
+    request->state = EC_INT_REQUEST_SUCCESS;
+    wake_up_all(&slave->master->request_queue);
     fsm->foe_request = NULL;
     fsm->state = ec_fsm_slave_state_ready;
 }
@@ -335,49 +500,49 @@
  * \return non-zero, if a request is processed.
  */
 int ec_fsm_slave_action_process_soe(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
-        )
-{
-    ec_slave_t *slave = fsm->slave;
-    ec_master_soe_request_t *request, *next;
-
-    // search the first request to be processed
-    list_for_each_entry_safe(request, next, &slave->soe_requests, list) {
-
-        list_del_init(&request->list); // dequeue
-        if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
-            EC_SLAVE_WARN(slave, "Aborting SoE request,"
-                    " slave has error flag set.\n");
-            request->req.state = EC_INT_REQUEST_FAILURE;
-            kref_put(&request->refcount, ec_master_soe_request_release);
-            wake_up(&slave->soe_queue);
-            fsm->state = ec_fsm_slave_state_idle;
-            return 0;
-        }
-
-        if (slave->current_state == EC_SLAVE_STATE_INIT) {
-            EC_SLAVE_WARN(slave, "Aborting SoE request, slave is in INIT.\n");
-            request->req.state = EC_INT_REQUEST_FAILURE;
-            kref_put(&request->refcount, ec_master_soe_request_release);
-            wake_up(&slave->soe_queue);
-            fsm->state = ec_fsm_slave_state_idle;
-            return 0;
-        }
-
-        request->req.state = EC_INT_REQUEST_BUSY;
-
-        // Found pending request. Execute it!
-        EC_SLAVE_DBG(slave, 1, "Processing SoE request...\n");
-
-        // Start SoE transfer
-        fsm->soe_request = request;
-        fsm->state = ec_fsm_slave_state_soe_request;
-        ec_fsm_soe_transfer(&fsm->fsm_soe, slave, &request->req);
-        ec_fsm_soe_exec(&fsm->fsm_soe); // execute immediately
-        ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
+        ec_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+    ec_soe_request_t *req;
+
+    if (list_empty(&slave->soe_requests)) {
+        return 0;
+    }
+
+    // take the first request to be processed
+    req = list_entry(slave->soe_requests.next, ec_soe_request_t, list);
+    list_del_init(&req->list); // dequeue
+
+    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
+        EC_SLAVE_WARN(slave, "Aborting SoE request,"
+                " slave has error flag set.\n");
+        req->state = EC_INT_REQUEST_FAILURE;
+        wake_up_all(&slave->master->request_queue);
+        fsm->state = ec_fsm_slave_state_idle;
         return 1;
     }
-    return 0;
+
+    if (slave->current_state == EC_SLAVE_STATE_INIT) {
+        EC_SLAVE_WARN(slave, "Aborting SoE request, slave is in INIT.\n");
+        req->state = EC_INT_REQUEST_FAILURE;
+        wake_up_all(&slave->master->request_queue);
+        fsm->state = ec_fsm_slave_state_idle;
+        return 0;
+    }
+
+    fsm->soe_request = req;
+    req->state = EC_INT_REQUEST_BUSY;
+
+    // Found pending request. Execute it!
+    EC_SLAVE_DBG(slave, 1, "Processing SoE request...\n");
+
+    // Start SoE transfer
+    fsm->state = ec_fsm_slave_state_soe_request;
+    ec_fsm_soe_transfer(&fsm->fsm_soe, slave, req);
+    ec_fsm_soe_exec(&fsm->fsm_soe, datagram); // execute immediately
+    return 1;
 }
 
 /*****************************************************************************/
@@ -385,34 +550,31 @@
 /** Slave state: SOE_REQUEST.
  */
 void ec_fsm_slave_state_soe_request(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
-        )
-{
-    ec_slave_t *slave = fsm->slave;
-    ec_master_soe_request_t *request = fsm->soe_request;
-
-    if (ec_fsm_soe_exec(&fsm->fsm_soe)) {
-        ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
+        ec_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+    ec_soe_request_t *request = fsm->soe_request;
+
+    if (ec_fsm_soe_exec(&fsm->fsm_soe, datagram)) {
         return;
     }
 
     if (!ec_fsm_soe_success(&fsm->fsm_soe)) {
         EC_SLAVE_ERR(slave, "Failed to process SoE request.\n");
-        request->req.state = EC_INT_REQUEST_FAILURE;
-        kref_put(&request->refcount, ec_master_soe_request_release);
-        wake_up(&slave->soe_queue);
+        request->state = EC_INT_REQUEST_FAILURE;
+        wake_up_all(&slave->master->request_queue);
         fsm->soe_request = NULL;
-        fsm->state = ec_fsm_slave_state_idle;
+        fsm->state = ec_fsm_slave_state_ready;
         return;
     }
 
     EC_SLAVE_DBG(slave, 1, "Finished SoE request.\n");
 
     // SoE request finished
-    request->req.state = EC_INT_REQUEST_SUCCESS;
-    kref_put(&request->refcount, ec_master_soe_request_release);
-    wake_up(&slave->soe_queue);
-
+    request->state = EC_INT_REQUEST_SUCCESS;
+    wake_up_all(&slave->master->request_queue);
     fsm->soe_request = NULL;
     fsm->state = ec_fsm_slave_state_ready;
 }