diff -r a0b686241178 -r 74ede087bc85 master/fsm_slave.c
--- a/master/fsm_slave.c	Tue Dec 04 14:54:12 2012 +0100
+++ b/master/fsm_slave.c	Tue Dec 04 16:05:23 2012 +0100
@@ -180,45 +180,45 @@
         )
 {
     ec_slave_t *slave = fsm->slave;
-    ec_sdo_request_t *request, *next;
-
-    // search the first external request to be processed
-    list_for_each_entry_safe(request, next, &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,"
-                    " slave has error flag set.\n");
-            request->state = EC_INT_REQUEST_FAILURE;
-            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, slave is in INIT.\n");
-            request->state = EC_INT_REQUEST_FAILURE;
-            wake_up(&slave->sdo_queue);
-            fsm->sdo_request = NULL;
-            fsm->state = ec_fsm_slave_state_idle;
-            return 0;
-        }
-
-        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->sdo_request = request;
-        fsm->state = ec_fsm_slave_state_sdo_request;
-        ec_fsm_coe_transfer(&fsm->fsm_coe, slave, request);
-        ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
-        ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
+    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(&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(&slave->master->request_queue);
+        fsm->state = ec_fsm_slave_state_idle;
+        return 1;
+    }
+
+    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->sdo_request = request;
+    fsm->state = ec_fsm_slave_state_sdo_request;
+    ec_fsm_coe_transfer(&fsm->fsm_coe, slave, request);
+    ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
+    ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
+    return 1;
 }
 
 /*****************************************************************************/
@@ -240,7 +240,7 @@
     if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
         EC_SLAVE_ERR(slave, "Failed to process SDO request.\n");
         request->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->sdo_queue);
+        wake_up(&slave->master->request_queue);
         fsm->sdo_request = NULL;
         fsm->state = ec_fsm_slave_state_ready;
         return;
@@ -250,8 +250,7 @@
 
     // SDO request finished
     request->state = EC_INT_REQUEST_SUCCESS;
-    wake_up(&slave->sdo_queue);
-
+    wake_up(&slave->master->request_queue);
     fsm->sdo_request = NULL;
     fsm->state = ec_fsm_slave_state_ready;
 }
@@ -296,7 +295,8 @@
         EC_SLAVE_WARN(slave, "Aborting register request,"
                 " slave has error flag set.\n");
         fsm->reg_request->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->reg_queue);
+        wake_up(&slave->master->request_queue);
+        fsm->reg_request = NULL;
         fsm->state = ec_fsm_slave_state_idle;
         return 1;
     }
@@ -345,7 +345,8 @@
                 " request datagram: ");
         ec_datagram_print_state(fsm->datagram);
         reg->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->reg_queue);
+        wake_up(&slave->master->request_queue);
+        fsm->reg_request = NULL;
         fsm->state = ec_fsm_slave_state_ready;
         return;
     }
@@ -365,7 +366,8 @@
                 fsm->datagram->working_counter);
     }
 
-    wake_up(&slave->reg_queue);
+    wake_up(&slave->master->request_queue);
+    fsm->reg_request = NULL;
     fsm->state = ec_fsm_slave_state_ready;
 }
 
@@ -394,8 +396,9 @@
         EC_SLAVE_WARN(slave, "Aborting FoE request,"
                 " slave has error flag set.\n");
         request->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->sdo_queue);
-        return 0;
+        wake_up(&slave->master->request_queue);
+        fsm->state = ec_fsm_slave_state_idle;
+        return 1;
     }
 
     request->state = EC_INT_REQUEST_BUSY;
@@ -429,7 +432,7 @@
     if (!ec_fsm_foe_success(&fsm->fsm_foe)) {
         EC_SLAVE_ERR(slave, "Failed to handle FoE request.\n");
         request->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->foe_queue);
+        wake_up(&slave->master->request_queue);
         fsm->foe_request = NULL;
         fsm->state = ec_fsm_slave_state_ready;
         return;
@@ -440,8 +443,7 @@
             " data.\n", request->data_size);
 
     request->state = EC_INT_REQUEST_SUCCESS;
-    wake_up(&slave->foe_queue);
-
+    wake_up(&slave->master->request_queue);
     fsm->foe_request = NULL;
     fsm->state = ec_fsm_slave_state_ready;
 }
@@ -471,14 +473,16 @@
         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;
+        wake_up(&slave->master->request_queue);
+        fsm->state = ec_fsm_slave_state_idle;
+        return 1;
     }
 
     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);
+        wake_up(&slave->master->request_queue);
+        fsm->state = ec_fsm_slave_state_idle;
         return 0;
     }
 
@@ -515,7 +519,7 @@
     if (!ec_fsm_soe_success(&fsm->fsm_soe)) {
         EC_SLAVE_ERR(slave, "Failed to process SoE request.\n");
         request->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->soe_queue);
+        wake_up(&slave->master->request_queue);
         fsm->soe_request = NULL;
         fsm->state = ec_fsm_slave_state_ready;
         return;
@@ -525,8 +529,7 @@
 
     // SoE request finished
     request->state = EC_INT_REQUEST_SUCCESS;
-    wake_up(&slave->soe_queue);
-
+    wake_up(&slave->master->request_queue);
     fsm->soe_request = NULL;
     fsm->state = ec_fsm_slave_state_ready;
 }