diff -r 505cf41488a4 -r 9cdd7669dc0b master/fsm_slave.c
--- a/master/fsm_slave.c	Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_slave.c	Thu Jan 10 17:36:41 2013 +0100
@@ -42,16 +42,16 @@
 
 /*****************************************************************************/
 
-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_reg(ec_fsm_slave_t *);
-void ec_fsm_slave_state_reg_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 *);
 
 /*****************************************************************************/
 
@@ -59,22 +59,23 @@
  */
 void ec_fsm_slave_init(
         ec_fsm_slave_t *fsm, /**< Slave state machine. */
-        ec_slave_t *slave, /**< EtherCAT slave. */
-        ec_datagram_t *datagram /**< Datagram object to use. */
+        ec_slave_t *slave /**< EtherCAT slave. */
         )
 {
     fsm->slave = slave;
-    fsm->datagram = datagram;
-    fsm->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->datagram);
-    ec_fsm_foe_init(&fsm->fsm_foe, fsm->datagram);
-    ec_fsm_soe_init(&fsm->fsm_soe, fsm->datagram);
+    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);
 }
 
 /*****************************************************************************/
@@ -85,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);
@@ -95,29 +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.
- */
-void ec_fsm_slave_exec(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
-        )
-{
-    if (fsm->datagram->state == EC_DATAGRAM_SENT
-        || fsm->datagram->state == EC_DATAGRAM_QUEUED) {
-        // datagram was not sent or received yet.
-        return;
-    }
-
-    fsm->state(fsm);
-
-    ec_datagram_output_stats(fsm->datagram);
+ * \return 1 if \a datagram was used, else 0.
+ */
+int ec_fsm_slave_exec(
+        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. */
         )
 {
@@ -127,6 +155,17 @@
     }
 }
 
+/*****************************************************************************/
+
+/** Returns, if the FSM is currently not busy and ready to execute.
+ */
+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
  *****************************************************************************/
@@ -134,7 +173,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
@@ -145,26 +185,27 @@
 /** 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)) {
+    if (ec_fsm_slave_action_process_sdo(fsm, datagram)) {
         return;
     }
 
     // Check for pending external register requests
-    if (ec_fsm_slave_action_process_reg(fsm)) {
+    if (ec_fsm_slave_action_process_reg(fsm, datagram)) {
         return;
     }
 
     // Check for pending FoE requests
-    if (ec_fsm_slave_action_process_foe(fsm)) {
+    if (ec_fsm_slave_action_process_foe(fsm, datagram)) {
         return;
     }
 
     // Check for pending SoE requests
-    if (ec_fsm_slave_action_process_soe(fsm)) {
+    if (ec_fsm_slave_action_process_soe(fsm, datagram)) {
         return;
     }
 }
@@ -176,7 +217,8 @@
  * \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_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     ec_slave_t *slave = fsm->slave;
@@ -194,7 +236,7 @@
         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);
+        wake_up_all(&slave->master->request_queue);
         fsm->state = ec_fsm_slave_state_idle;
         return 1;
     }
@@ -202,22 +244,21 @@
     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);
+        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->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_fsm_coe_exec(&fsm->fsm_coe, datagram); // execute immediately
     return 1;
 }
 
@@ -226,21 +267,21 @@
 /** Slave state: SDO_REQUEST.
  */
 void ec_fsm_slave_state_sdo_request(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
+        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)) {
-        ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
+    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.\n");
         request->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->master->request_queue);
+        wake_up_all(&slave->master->request_queue);
         fsm->sdo_request = NULL;
         fsm->state = ec_fsm_slave_state_ready;
         return;
@@ -250,7 +291,7 @@
 
     // SDO request finished
     request->state = EC_INT_REQUEST_SUCCESS;
-    wake_up(&slave->master->request_queue);
+    wake_up_all(&slave->master->request_queue);
     fsm->sdo_request = NULL;
     fsm->state = ec_fsm_slave_state_ready;
 }
@@ -262,7 +303,8 @@
  * \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_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     ec_slave_t *slave = fsm->slave;
@@ -295,7 +337,7 @@
         EC_SLAVE_WARN(slave, "Aborting register request,"
                 " slave has error flag set.\n");
         fsm->reg_request->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->master->request_queue);
+        wake_up_all(&slave->master->request_queue);
         fsm->reg_request = NULL;
         fsm->state = ec_fsm_slave_state_idle;
         return 1;
@@ -308,17 +350,16 @@
 
     // Start register access
     if (fsm->reg_request->dir == EC_DIR_INPUT) {
-        ec_datagram_fprd(fsm->datagram, slave->station_address,
+        ec_datagram_fprd(datagram, slave->station_address,
                 fsm->reg_request->address, fsm->reg_request->transfer_size);
-        ec_datagram_zero(fsm->datagram);
+        ec_datagram_zero(datagram);
     } else {
-        ec_datagram_fpwr(fsm->datagram, slave->station_address,
+        ec_datagram_fpwr(datagram, slave->station_address,
                 fsm->reg_request->address, fsm->reg_request->transfer_size);
-        memcpy(fsm->datagram->data, fsm->reg_request->data,
+        memcpy(datagram->data, fsm->reg_request->data,
                 fsm->reg_request->transfer_size);
     }
-    fsm->datagram->device_index = slave->device_index;
-    ec_master_queue_external_datagram(slave->master, fsm->datagram);
+    datagram->device_index = slave->device_index;
     fsm->state = ec_fsm_slave_state_reg_request;
     return 1;
 }
@@ -328,7 +369,8 @@
 /** Slave state: Register request.
  */
 void ec_fsm_slave_state_reg_request(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
+        ec_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     ec_slave_t *slave = fsm->slave;
@@ -337,6 +379,7 @@
     if (!reg) {
         // configuration was cleared in the meantime
         fsm->state = ec_fsm_slave_state_ready;
+        fsm->reg_request = NULL;
         return;
     }
 
@@ -345,7 +388,7 @@
                 " request datagram: ");
         ec_datagram_print_state(fsm->datagram);
         reg->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->master->request_queue);
+        wake_up_all(&slave->master->request_queue);
         fsm->reg_request = NULL;
         fsm->state = ec_fsm_slave_state_ready;
         return;
@@ -366,7 +409,7 @@
                 fsm->datagram->working_counter);
     }
 
-    wake_up(&slave->master->request_queue);
+    wake_up_all(&slave->master->request_queue);
     fsm->reg_request = NULL;
     fsm->state = ec_fsm_slave_state_ready;
 }
@@ -378,7 +421,8 @@
  * \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_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     ec_slave_t *slave = fsm->slave;
@@ -396,20 +440,19 @@
         EC_SLAVE_WARN(slave, "Aborting FoE request,"
                 " slave has error flag set.\n");
         request->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->master->request_queue);
+        wake_up_all(&slave->master->request_queue);
         fsm->state = ec_fsm_slave_state_idle;
         return 1;
     }
 
     request->state = EC_INT_REQUEST_BUSY;
+    fsm->foe_request = request;
 
     EC_SLAVE_DBG(slave, 1, "Processing FoE request.\n");
 
-    fsm->foe_request = request;
     fsm->state = ec_fsm_slave_state_foe_request;
     ec_fsm_foe_transfer(&fsm->fsm_foe, slave, request);
-    ec_fsm_foe_exec(&fsm->fsm_foe);
-    ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
+    ec_fsm_foe_exec(&fsm->fsm_foe, datagram);
     return 1;
 }
 
@@ -418,21 +461,21 @@
 /** Slave state: FOE REQUEST.
  */
 void ec_fsm_slave_state_foe_request(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
+        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)) {
-        ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
+    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.\n");
         request->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->master->request_queue);
+        wake_up_all(&slave->master->request_queue);
         fsm->foe_request = NULL;
         fsm->state = ec_fsm_slave_state_ready;
         return;
@@ -443,7 +486,7 @@
             " data.\n", request->data_size);
 
     request->state = EC_INT_REQUEST_SUCCESS;
-    wake_up(&slave->master->request_queue);
+    wake_up_all(&slave->master->request_queue);
     fsm->foe_request = NULL;
     fsm->state = ec_fsm_slave_state_ready;
 }
@@ -455,7 +498,8 @@
  * \return non-zero, if a request is processed.
  */
 int ec_fsm_slave_action_process_soe(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
+        ec_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     ec_slave_t *slave = fsm->slave;
@@ -473,7 +517,7 @@
         EC_SLAVE_WARN(slave, "Aborting SoE request,"
                 " slave has error flag set.\n");
         req->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->master->request_queue);
+        wake_up_all(&slave->master->request_queue);
         fsm->state = ec_fsm_slave_state_idle;
         return 1;
     }
@@ -481,22 +525,21 @@
     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->master->request_queue);
+        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->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);
+    ec_fsm_soe_exec(&fsm->fsm_soe, datagram); // execute immediately
     return 1;
 }
 
@@ -505,21 +548,21 @@
 /** Slave state: SOE_REQUEST.
  */
 void ec_fsm_slave_state_soe_request(
-        ec_fsm_slave_t *fsm /**< Slave state machine. */
+        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)) {
-        ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
+    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->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->master->request_queue);
+        wake_up_all(&slave->master->request_queue);
         fsm->soe_request = NULL;
         fsm->state = ec_fsm_slave_state_ready;
         return;
@@ -529,7 +572,7 @@
 
     // SoE request finished
     request->state = EC_INT_REQUEST_SUCCESS;
-    wake_up(&slave->master->request_queue);
+    wake_up_all(&slave->master->request_queue);
     fsm->soe_request = NULL;
     fsm->state = ec_fsm_slave_state_ready;
 }