--- a/master/fsm_slave.c	Tue Dec 04 14:26:30 2012 +0100
+++ b/master/fsm_slave.c	Tue Dec 04 14:49:23 2012 +0100
@@ -457,43 +457,43 @@
         )
 {
     ec_slave_t *slave = fsm->slave;
-    ec_master_soe_request_t *req, *next;
-
-    // search the first request to be processed
-    list_for_each_entry_safe(req, next, &slave->soe_requests, 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->req.state = EC_INT_REQUEST_FAILURE;
-            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");
-            req->req.state = EC_INT_REQUEST_FAILURE;
-            wake_up(&slave->soe_queue);
-            fsm->state = ec_fsm_slave_state_idle;
-            return 0;
-        }
-
-        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->soe_request = &req->req;
-        fsm->state = ec_fsm_slave_state_soe_request;
-        ec_fsm_soe_transfer(&fsm->fsm_soe, slave, &req->req);
-        ec_fsm_soe_exec(&fsm->fsm_soe); // execute immediately
-        ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
-        return 1;
-    }
-    return 0;
+    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(&slave->soe_queue);
+        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(&slave->soe_queue);
+        return 0;
+    }
+
+    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 = req;
+    fsm->state = ec_fsm_slave_state_soe_request;
+    ec_fsm_soe_transfer(&fsm->fsm_soe, slave, req);
+    ec_fsm_soe_exec(&fsm->fsm_soe); // execute immediately
+    ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
+    return 1;
 }
 
 /*****************************************************************************/