--- a/master/fsm_mapping.c	Tue Feb 19 16:03:04 2008 +0000
+++ b/master/fsm_mapping.c	Tue Feb 19 16:09:58 2008 +0000
@@ -150,7 +150,7 @@
         return;
     }
 
-    fsm->dir = EC_DIR_OUTPUT;
+    fsm->sync = NULL;
     ec_fsm_mapping_next_sync(fsm);
 }
 
@@ -162,31 +162,38 @@
         ec_fsm_mapping_t *fsm /**< mapping state machine */
         )
 {
-    while (1) {
-        if (fsm->dir > EC_DIR_INPUT) {
-            // no more directions to configure mappings for
-            fsm->state = ec_fsm_mapping_state_end;
-            return;
-        }
-
-        if (!(fsm->sync = ec_slave_get_pdo_sync(fsm->slave, fsm->dir))) {
-            EC_WARN("next_sync(): No sync manager!\n");
-            fsm->dir++;
+    ec_direction_t dir;
+    const ec_sync_t *sync;
+    const ec_pdo_mapping_t *map;
+
+    for (dir = EC_DIR_OUTPUT; dir <= EC_DIR_INPUT; dir++) {
+        if (!(sync = ec_slave_get_pdo_sync(fsm->slave, dir))) {
+            EC_WARN("No sync manager for direction %u!\n", dir);
             continue;
         }
 
-        fsm->mapping = &fsm->slave->config->mapping[fsm->dir];
-
-        if (ec_pdo_mapping_equal(&fsm->sync->mapping, fsm->mapping)) {
-            // the mapping for this direction does not have to be altered
-            fsm->dir++;
-            continue;
+        if (fsm->sync) { // there is a last SM
+            if (sync == fsm->sync) // this is the last SM
+                fsm->sync = NULL; // take the next one
         } else {
-            fsm->dir++;
+            map = &fsm->slave->config->mapping[dir];
+            if (ec_pdo_mapping_equal(&sync->mapping, map))
+                continue;
+
+            fsm->sync = sync;
+            fsm->mapping = map;
             break;
         }
     }
 
+    if (!sync) {
+        if (fsm->slave->master->debug_level)
+            EC_DBG("Pdo mapping finished for slave %u.\n",
+                    fsm->slave->ring_position);
+        fsm->state = ec_fsm_mapping_state_end;
+        return;
+    }
+
     if (fsm->slave->master->debug_level) {
         EC_DBG("Configuring PDO mapping for SM%u of slave %u.\n",
                 fsm->sync->index, fsm->slave->ring_position);
@@ -222,6 +229,27 @@
 
 /*****************************************************************************/
 
+/** Add a Pdo to the mapping.
+ */
+void ec_fsm_mapping_add_pdo(
+        ec_fsm_mapping_t *fsm /**< mapping state machine */
+        )
+{
+    fsm->sdodata.subindex = fsm->pdo_count;
+    EC_WRITE_U16(&fsm->sdo_value, fsm->pdo->index);
+    fsm->sdodata.size = 2;
+
+    if (fsm->slave->master->debug_level)
+        EC_DBG("Mapping PDO 0x%04X at position %u.\n",
+                fsm->pdo->index, fsm->sdodata.subindex);
+    
+    fsm->state = ec_fsm_mapping_state_add_pdo;
+    ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata);
+    ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+}
+
+/*****************************************************************************/
+
 /** Set the number of mapped PDOs to zero.
  */
 void ec_fsm_mapping_state_zero_count(
@@ -250,17 +278,7 @@
 
     // add first PDO to mapping
     fsm->pdo_count = 1;
-    fsm->sdodata.subindex = fsm->pdo_count;
-    EC_WRITE_U16(&fsm->sdo_value, fsm->pdo->index);
-    fsm->sdodata.size = 2;
-
-    if (fsm->slave->master->debug_level)
-        EC_DBG("Mapping PDO 0x%04X at position %u.\n",
-                fsm->pdo->index, fsm->sdodata.subindex);
-    
-    fsm->state = ec_fsm_mapping_state_add_pdo;
-    ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata);
-    ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+    ec_fsm_mapping_add_pdo(fsm);
 }
 
 /*****************************************************************************/
@@ -299,16 +317,7 @@
 
     // add next PDO to mapping
     fsm->pdo_count++;
-    fsm->sdodata.subindex = fsm->pdo_count;
-    EC_WRITE_U16(&fsm->sdo_value, fsm->pdo->index);
-    fsm->sdodata.size = 2;
-
-    if (fsm->slave->master->debug_level)
-        EC_DBG("Mapping PDO 0x%04X at position %u.\n",
-                fsm->pdo->index, fsm->sdodata.subindex);
-
-    ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata);
-    ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
+    ec_fsm_mapping_add_pdo(fsm);
 }
 
 /*****************************************************************************/

--- a/master/fsm_mapping.h	Tue Feb 19 16:03:04 2008 +0000
+++ b/master/fsm_mapping.h	Tue Feb 19 16:09:58 2008 +0000
@@ -54,18 +54,17 @@
  */
 struct ec_fsm_mapping
 {
-    void (*state)(ec_fsm_mapping_t *); /**< state function */
-    ec_fsm_coe_t *fsm_coe; /**< CoE state machine to use */
+    void (*state)(ec_fsm_mapping_t *); /**< State function. */
+    ec_fsm_coe_t *fsm_coe; /**< CoE state machine to use. */
+    ec_slave_t *slave; /**< Slave the FSM runs on. */
 
-    ec_slave_t *slave; /**< slave the FSM runs on */
+    const ec_sync_t *sync; /**< Current sync manager. */
+    const ec_pdo_mapping_t *mapping; /**< Target Pdo mapping. */
+    const ec_pdo_t *pdo; /**< Current Pdo. */
 
-    ec_direction_t dir; /**< current PDO direction */
-    ec_sync_t *sync; /**< current sync manager */
-    const ec_pdo_mapping_t *mapping; /**< Mapping to assign. */
-    ec_pdo_t *pdo; /**< current PDO */
-    ec_sdo_data_t sdodata; /**< SDO configuration data */
-    uint16_t sdo_value; /**< SDO value */
-    unsigned int pdo_count; /**< number of mapped PDOs */
+    ec_sdo_data_t sdodata; /**< SDO configuration data. */
+    uint16_t sdo_value; /**< SDO value. */
+    unsigned int pdo_count; /**< Number of mapped Pdos. */
 };
 
 /*****************************************************************************/

--- a/master/fsm_pdo_config.c	Tue Feb 19 16:03:04 2008 +0000
+++ b/master/fsm_pdo_config.c	Tue Feb 19 16:09:58 2008 +0000
@@ -167,27 +167,28 @@
         map = &fsm->slave->config->mapping[dir];
 
         list_for_each_entry(pdo, &map->pdos, list) {
-            if (fsm->pdo) {
-                if (pdo == fsm->pdo)
-                    fsm->pdo = NULL;
+            if (fsm->pdo) { // there was a Pdo configured in the last run
+                if (pdo == fsm->pdo) // this is the last Pdo
+                    fsm->pdo = NULL; // take the next one
             } else {
                 if ((mapped_pdo = ec_slave_find_pdo(fsm->slave, pdo->index)))
                     if (ec_pdo_equal_entries(pdo, mapped_pdo))
                         continue; // Pdo configured correctly
 
                 fsm->pdo = pdo;
-                goto configure;
+                break;
             }
         }
     }
 
-    if (fsm->slave->master->debug_level)
-        EC_DBG("Pdo configuration finished for slave %u.\n",
-                fsm->slave->ring_position);
-    fsm->state = ec_fsm_pdo_config_state_end;
-    return;
-
-configure:
+    if (!fsm->pdo) {
+        if (fsm->slave->master->debug_level)
+            EC_DBG("Pdo configuration finished for slave %u.\n",
+                    fsm->slave->ring_position);
+        fsm->state = ec_fsm_pdo_config_state_end;
+        return;
+    }
+
     if (fsm->slave->master->debug_level) {
         EC_DBG("Changing configuration of Pdo 0x%04X of slave %u.\n",
                 fsm->pdo->index, fsm->slave->ring_position);

--- a/master/fsm_pdo_config.h	Tue Feb 19 16:03:04 2008 +0000
+++ b/master/fsm_pdo_config.h	Tue Feb 19 16:09:58 2008 +0000
@@ -31,10 +31,9 @@
  *
  *****************************************************************************/
 
-/**
-   \file
-   EtherCAT PDO configuration state machine structures.
-*/
+/** \file
+ * EtherCAT PDO configuration state machine structures.
+ */
 
 /*****************************************************************************/