Explicit use of slave configuration FSM in ecrt_master_activate() and ecrt_master_release().

master/fsm.c

@@ -68,6 +68,7 @@ void ec_fsm_slavescan_datalink(ec_fsm_t *);
 void ec_fsm_slavescan_eeprom_size(ec_fsm_t *);
 void ec_fsm_slavescan_eeprom_data(ec_fsm_t *);
 
+void ec_fsm_slaveconf_start(ec_fsm_t *);
 void ec_fsm_slaveconf_init(ec_fsm_t *);
 void ec_fsm_slaveconf_sync(ec_fsm_t *);
 void ec_fsm_slaveconf_preop(ec_fsm_t *);
@@ -136,6 +137,17 @@ int ec_fsm_exec(ec_fsm_t *fsm /**< finite state machine */)
 {
     fsm->master_state(fsm);
 
+    return ec_fsm_running(fsm);
+}
+
+/*****************************************************************************/
+
+/**
+   \return false, if state machine has terminated
+*/
+
+int ec_fsm_running(ec_fsm_t *fsm /**< finite state machine */)
+{
     return fsm->master_state != ec_fsm_master_end
         && fsm->master_state != ec_fsm_master_error;
 }
@@ -303,28 +315,25 @@ void ec_fsm_master_action_process_states(ec_fsm_t *fsm
             || !slave->online
             || slave->requested_state == EC_SLAVE_STATE_UNKNOWN
             || (slave->current_state == slave->requested_state
-                && (slave->configured
-                    || slave->current_state == EC_SLAVE_STATE_INIT))) continue;
+                && slave->configured)) continue;
 
         if (master->debug_level) {
             ec_state_string(slave->current_state, old_state);
-            if (!slave->configured
-                && slave->current_state != EC_SLAVE_STATE_INIT) {
-                EC_INFO("Reconfiguring slave %i (%s).\n",
-                        slave->ring_position, old_state);
-            }
-            else if (slave->current_state != slave->requested_state) {
+            if (slave->current_state != slave->requested_state) {
                 ec_state_string(slave->requested_state, new_state);
-                EC_INFO("Changing state of slave %i (%s -> %s).\n",
-                        slave->ring_position, old_state, new_state);
+                EC_DBG("Changing state of slave %i (%s -> %s).\n",
+                       slave->ring_position, old_state, new_state);
+            }
+            else if (!slave->configured) {
+                EC_DBG("Reconfiguring slave %i (%s).\n",
+                       slave->ring_position, old_state);
             }
         }
 
-        fsm->slave = slave;
-        fsm->slave_state = ec_fsm_slaveconf_init;
-        ec_fsm_change_start(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_INIT);
         fsm->master_state = ec_fsm_master_configure_slave;
-        fsm->master_state(fsm); // execute immediately
+        fsm->slave = slave;
+        fsm->slave_state = ec_fsm_slaveconf_start;
+        fsm->slave_state(fsm); // execute immediately
         return;
     }
 
@@ -1192,6 +1201,23 @@ end:
  *  slave configuration state machine
  *****************************************************************************/
 
+/**
+   Slave configuration state: START.
+*/
+
+void ec_fsm_slaveconf_start(ec_fsm_t *fsm /**< finite state machine */)
+{
+    if (fsm->master->debug_level) {
+        EC_DBG("Configuring slave %i...\n", fsm->slave->ring_position);
+    }
+
+    ec_fsm_change_start(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_INIT);
+    ec_fsm_change_exec(&fsm->fsm_change);
+    fsm->slave_state = ec_fsm_slaveconf_init;
+}
+
+/*****************************************************************************/
+
 /**
    Slave configuration state: INIT.
 */

master/fsm.h

@@ -86,8 +86,14 @@ int ec_fsm_init(ec_fsm_t *, ec_master_t *);
 void ec_fsm_clear(ec_fsm_t *);
 
 int ec_fsm_exec(ec_fsm_t *);
+int ec_fsm_running(ec_fsm_t *);
 int ec_fsm_success(ec_fsm_t *);
 
+// FIXME
+void ec_fsm_slaveconf_start(ec_fsm_t *);
+void ec_fsm_slave_end(ec_fsm_t *);
+void ec_fsm_slave_error(ec_fsm_t *);
+
 /*****************************************************************************/
 
 #endif

master/master.c

@@ -373,14 +373,11 @@ int ec_master_enter_operation_mode(ec_master_t *master /**< EtherCAT master */)
         ecrt_master_receive(master);
     }
 
-    // finish running FSM
-    while (ec_fsm_exec(&master->fsm)) {
-        ec_master_sync_io(master);
-    }
-
-    if (!ec_fsm_success(&master->fsm)) {
-        EC_ERR("Master state machine failure!\n");
-        goto out_idle;
+    // finish running master FSM
+    if (ec_fsm_running(&master->fsm)) {
+        while (ec_fsm_exec(&master->fsm)) {
+            ec_master_sync_io(master);
+        }
     }
 
     if (master->debug_level) {
@@ -427,14 +424,26 @@ void ec_master_leave_operation_mode(ec_master_t *master
         ecrt_master_receive(master);
     }
 
+    // finish running master FSM
+    if (ec_fsm_running(fsm)) {
+        while (ec_fsm_exec(fsm)) {
+            ec_master_sync_io(master);
+        }
+    }
+
     // set states for all slaves
     list_for_each_entry(slave, &master->slaves, list) {
         ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP);
-    }
 
-    // execute master FSM to deactivate slaves
-    while (ec_fsm_exec(fsm)) {
-        ec_master_sync_io(master);
+        fsm->slave = slave;
+        fsm->slave_state = ec_fsm_slaveconf_start;
+
+        do {
+            fsm->slave_state(fsm);
+            ec_master_sync_io(master);
+        }
+        while (fsm->slave_state != ec_fsm_slave_end
+               && fsm->slave_state != ec_fsm_slave_error);
     }
 
     ec_master_destroy_domains(master);
@@ -1319,19 +1328,19 @@ int ecrt_master_activate(ec_master_t *master /**< EtherCAT master */)
         domain_offset += domain->data_size;
     }
 
-    // execute master FSM until termination
-    while (ec_fsm_exec(fsm)) {
-        ec_master_sync_io(master);
-    }
+    // configure all slaves
+    list_for_each_entry(slave, &master->slaves, list) {
+        fsm->slave = slave;
+        fsm->slave_state = ec_fsm_slaveconf_start;
 
-    if (!ec_fsm_success(fsm)) {
-        EC_ERR("Error in master state machine.\n");
-        return -1;
-    }
+        do {
+            fsm->slave_state(fsm);
+            ec_master_sync_io(master);
+        }
+        while (fsm->slave_state != ec_fsm_slave_end
+               && fsm->slave_state != ec_fsm_slave_error);
 
-    // check, if all slaves have been configured
-    list_for_each_entry(slave, &master->slaves, list) {
-        if (slave->error_flag || !slave->online) {
+        if (fsm->slave_state == ec_fsm_slave_error) {
             EC_ERR("Failed to configure slave %i!\n", slave->ring_position);
             return -1;
         }