diff -r 505cf41488a4 -r 9cdd7669dc0b master/fsm_pdo_entry.c
--- a/master/fsm_pdo_entry.c	Thu Jan 10 12:34:58 2013 +0100
+++ b/master/fsm_pdo_entry.c	Thu Jan 10 17:36:41 2013 +0100
@@ -42,21 +42,24 @@
 
 /*****************************************************************************/
 
-void ec_fsm_pdo_entry_read_state_start(ec_fsm_pdo_entry_t *);
-void ec_fsm_pdo_entry_read_state_count(ec_fsm_pdo_entry_t *);
-void ec_fsm_pdo_entry_read_state_entry(ec_fsm_pdo_entry_t *);
-
-void ec_fsm_pdo_entry_read_action_next(ec_fsm_pdo_entry_t *);
-
-void ec_fsm_pdo_entry_conf_state_start(ec_fsm_pdo_entry_t *);
-void ec_fsm_pdo_entry_conf_state_zero_entry_count(ec_fsm_pdo_entry_t *);
-void ec_fsm_pdo_entry_conf_state_map_entry(ec_fsm_pdo_entry_t *);
-void ec_fsm_pdo_entry_conf_state_set_entry_count(ec_fsm_pdo_entry_t *);
-
-void ec_fsm_pdo_entry_conf_action_map(ec_fsm_pdo_entry_t *);
-
-void ec_fsm_pdo_entry_state_end(ec_fsm_pdo_entry_t *);
-void ec_fsm_pdo_entry_state_error(ec_fsm_pdo_entry_t *);
+void ec_fsm_pdo_entry_read_state_start(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+void ec_fsm_pdo_entry_read_state_count(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+void ec_fsm_pdo_entry_read_state_entry(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+
+void ec_fsm_pdo_entry_read_action_next(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+
+void ec_fsm_pdo_entry_conf_state_start(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+void ec_fsm_pdo_entry_conf_state_zero_entry_count(ec_fsm_pdo_entry_t *,
+        ec_datagram_t *);
+void ec_fsm_pdo_entry_conf_state_map_entry(ec_fsm_pdo_entry_t *,
+        ec_datagram_t *);
+void ec_fsm_pdo_entry_conf_state_set_entry_count(ec_fsm_pdo_entry_t *,
+        ec_datagram_t *);
+
+void ec_fsm_pdo_entry_conf_action_map(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+
+void ec_fsm_pdo_entry_state_end(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+void ec_fsm_pdo_entry_state_error(ec_fsm_pdo_entry_t *, ec_datagram_t *);
 
 /*****************************************************************************/
 
@@ -87,7 +90,7 @@
 /** Print the current and desired PDO mapping.
  */
 void ec_fsm_pdo_entry_print(
-        ec_fsm_pdo_entry_t *fsm /**< PDO configuration state machine. */
+        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
         )
 {
     printk("Currently mapped PDO entries: ");
@@ -103,7 +106,7 @@
  */
 void ec_fsm_pdo_entry_start_reading(
         ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
-        ec_slave_t *slave, /**< slave to configure */
+        ec_slave_t *slave, /**< Slave to configure. */
         ec_pdo_t *pdo /**< PDO to read entries for. */
         )
 {
@@ -121,7 +124,7 @@
  */
 void ec_fsm_pdo_entry_start_configuration(
         ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
-        ec_slave_t *slave, /**< slave to configure */
+        ec_slave_t *slave, /**< Slave to configure. */
         const ec_pdo_t *pdo, /**< PDO with the desired entries. */
         const ec_pdo_t *cur_pdo /**< Current PDO mapping. */
         )
@@ -160,10 +163,12 @@
  * \return false, if state machine has terminated
  */
 int ec_fsm_pdo_entry_exec(
-        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
-        )
-{
-    fsm->state(fsm);
+        ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    fsm->state(fsm, datagram);
+
     return ec_fsm_pdo_entry_running(fsm);
 }
 
@@ -187,7 +192,8 @@
 /** Request reading the number of mapped PDO entries.
  */
 void ec_fsm_pdo_entry_read_state_start(
-        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
+        ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     ecrt_sdo_request_index(&fsm->request, fsm->target_pdo->index, 0);
@@ -195,7 +201,7 @@
 
     fsm->state = ec_fsm_pdo_entry_read_state_count;
     ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
-    ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+    ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
 }
 
 /*****************************************************************************/
@@ -203,11 +209,13 @@
 /** Read number of mapped PDO entries.
  */
 void ec_fsm_pdo_entry_read_state_count(
-        ec_fsm_pdo_entry_t *fsm /**< finite state machine */
-        )
-{
-    if (ec_fsm_coe_exec(fsm->fsm_coe))
-        return;
+        ec_fsm_pdo_entry_t *fsm, /**< finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+        return;
+    }
 
     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
         EC_SLAVE_ERR(fsm->slave,
@@ -231,7 +239,7 @@
 
     // read first PDO entry
     fsm->entry_pos = 1;
-    ec_fsm_pdo_entry_read_action_next(fsm);
+    ec_fsm_pdo_entry_read_action_next(fsm, datagram);
 }
 
 /*****************************************************************************/
@@ -239,7 +247,8 @@
 /** Read next PDO entry.
  */
 void ec_fsm_pdo_entry_read_action_next(
-        ec_fsm_pdo_entry_t *fsm /**< finite state machine */
+        ec_fsm_pdo_entry_t *fsm, /**< finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     if (fsm->entry_pos <= fsm->entry_count) {
@@ -248,7 +257,7 @@
         ecrt_sdo_request_read(&fsm->request);
         fsm->state = ec_fsm_pdo_entry_read_state_entry;
         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
-        ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+        ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
         return;
     }
 
@@ -261,10 +270,13 @@
 /** Read PDO entry information.
  */
 void ec_fsm_pdo_entry_read_state_entry(
-        ec_fsm_pdo_entry_t *fsm /**< finite state machine */
-        )
-{
-    if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
+        ec_fsm_pdo_entry_t *fsm, /**< finite state machine */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+        return;
+    }
 
     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
         EC_SLAVE_ERR(fsm->slave, "Failed to read mapped PDO entry.\n");
@@ -315,7 +327,7 @@
 
         // next PDO entry
         fsm->entry_pos++;
-        ec_fsm_pdo_entry_read_action_next(fsm);
+        ec_fsm_pdo_entry_read_action_next(fsm, datagram);
     }
 }
 
@@ -326,7 +338,8 @@
 /** Start PDO mapping.
  */
 void ec_fsm_pdo_entry_conf_state_start(
-        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
+        ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     if (ec_sdo_request_alloc(&fsm->request, 4)) {
@@ -344,7 +357,7 @@
 
     fsm->state = ec_fsm_pdo_entry_conf_state_zero_entry_count;
     ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
-    ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+    ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
 }
 
 /*****************************************************************************/
@@ -367,11 +380,13 @@
 /** Set the number of mapped entries to zero.
  */
 void ec_fsm_pdo_entry_conf_state_zero_entry_count(
-        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
-        )
-{
-    if (ec_fsm_coe_exec(fsm->fsm_coe))
-        return;
+        ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+        return;
+    }
 
     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
         EC_SLAVE_WARN(fsm->slave, "Failed to clear PDO mapping.\n");
@@ -392,7 +407,7 @@
 
     // add first entry
     fsm->entry_pos = 1;
-    ec_fsm_pdo_entry_conf_action_map(fsm);
+    ec_fsm_pdo_entry_conf_action_map(fsm, datagram);
 }
 
 /*****************************************************************************/
@@ -400,7 +415,8 @@
 /** Starts to add a PDO entry.
  */
 void ec_fsm_pdo_entry_conf_action_map(
-        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
+        ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
     uint32_t value;
@@ -420,7 +436,7 @@
 
     fsm->state = ec_fsm_pdo_entry_conf_state_map_entry;
     ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
-    ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+    ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
 }
 
 /*****************************************************************************/
@@ -428,10 +444,13 @@
 /** Add a PDO entry.
  */
 void ec_fsm_pdo_entry_conf_state_map_entry(
-        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
-        )
-{
-    if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
+        ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+        return;
+    }
 
     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
         EC_SLAVE_WARN(fsm->slave, "Failed to map PDO entry"
@@ -458,13 +477,13 @@
 
         fsm->state = ec_fsm_pdo_entry_conf_state_set_entry_count;
         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
-        ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+        ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
         return;
     }
 
     // add next entry
     fsm->entry_pos++;
-    ec_fsm_pdo_entry_conf_action_map(fsm);
+    ec_fsm_pdo_entry_conf_action_map(fsm, datagram);
 }
 
 /*****************************************************************************/
@@ -472,10 +491,13 @@
 /** Set the number of entries.
  */
 void ec_fsm_pdo_entry_conf_state_set_entry_count(
-        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
-        )
-{
-    if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
+        ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+    if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+        return;
+    }
 
     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
         EC_SLAVE_WARN(fsm->slave, "Failed to set number of entries.\n");
@@ -497,7 +519,8 @@
 /** State: ERROR.
  */
 void ec_fsm_pdo_entry_state_error(
-        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
+        ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
         )
 {
 }
@@ -507,9 +530,10 @@
 /** State: END.
  */
 void ec_fsm_pdo_entry_state_end(
-        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
-        )
-{
-}
-
-/*****************************************************************************/
+        ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+        ec_datagram_t *datagram /**< Datagram to use. */
+        )
+{
+}
+
+/*****************************************************************************/