# HG changeset patch
# User Florian Pose <fp@igh-essen.com>
# Date 1240235037 0
# Node ID e3974f72d31e73b331e853c4bdd95cb92cca746d
# Parent  8686c5b3d14bd3403eef6f48734985c7f4b3ce2f
Improved DC configuration.

diff -r 8686c5b3d14b -r e3974f72d31e master/fsm_slave_config.c
--- a/master/fsm_slave_config.c	Mon Apr 20 11:53:17 2009 +0000
+++ b/master/fsm_slave_config.c	Mon Apr 20 13:43:57 2009 +0000
@@ -46,15 +46,15 @@
 void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_clear_sync(ec_fsm_slave_config_t *);
-void ec_fsm_slave_config_state_clear_dc_assign(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_dc_clear_assign(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_dc_read_offset(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_dc_write_offset(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *);
-void ec_fsm_slave_config_state_dc_read(ec_fsm_slave_config_t *);
-void ec_fsm_slave_config_state_dc_offset(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_dc_cycle(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_dc_start(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_dc_assign(ec_fsm_slave_config_t *);
@@ -63,14 +63,14 @@
 
 void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_enter_clear_sync(ec_fsm_slave_config_t *);
-void ec_fsm_slave_config_enter_clear_dc_assign(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_enter_dc_clear_assign(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *);
-void ec_fsm_slave_config_enter_dc_read(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_enter_dc_cycle(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *);
 
 void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *);
@@ -288,7 +288,7 @@
 
     if (!slave->sii.sync_count) { // FIXME use base_sync_count?
         // no sync managers
-        ec_fsm_slave_config_enter_clear_dc_assign(fsm);
+        ec_fsm_slave_config_enter_dc_clear_assign(fsm);
         return;
     }
 
@@ -334,14 +334,14 @@
         return;
     }
 
-    ec_fsm_slave_config_enter_clear_dc_assign(fsm);
+    ec_fsm_slave_config_enter_dc_clear_assign(fsm);
 }
 
 /*****************************************************************************/
 
 /** Clear the DC assignment.
  */
-void ec_fsm_slave_config_enter_clear_dc_assign(
+void ec_fsm_slave_config_enter_dc_clear_assign(
         ec_fsm_slave_config_t *fsm /**< slave state machine */
         )
 {
@@ -360,14 +360,14 @@
     ec_datagram_fpwr(datagram, slave->station_address, 0x0980, 2);
     ec_datagram_zero(datagram);
     fsm->retries = EC_FSM_RETRIES;
-    fsm->state = ec_fsm_slave_config_state_clear_dc_assign;
+    fsm->state = ec_fsm_slave_config_state_dc_clear_assign;
 }
 
 /*****************************************************************************/
 
 /** Slave configuration state: CLEAR DC ASSIGN.
  */
-void ec_fsm_slave_config_state_clear_dc_assign(
+void ec_fsm_slave_config_state_dc_clear_assign(
         ec_fsm_slave_config_t *fsm /**< slave state machine */
         )
 {
@@ -383,11 +383,95 @@
         return;
     }
 
+    if (fsm->slave->master->debug_level && datagram->working_counter != 1) {
+        // clearing the DC assignment does not succeed on simple slaves
+        EC_DBG("Failed to clear DC assignment of slave %u: ",
+               fsm->slave->ring_position);
+        ec_datagram_print_wc_error(datagram);
+    }
+
+    // read DC system time and system time offset
+    ec_datagram_fprd(fsm->datagram, fsm->slave->station_address, 0x0910, 24);
+    fsm->retries = EC_FSM_RETRIES;
+    fsm->state = ec_fsm_slave_config_state_dc_read_offset;
+}
+
+/*****************************************************************************/
+
+/** Slave configuration state: DC READ OFFSET.
+ */
+void ec_fsm_slave_config_state_dc_read_offset(
+        ec_fsm_slave_config_t *fsm /**< slave state machine */
+        )
+{
+    ec_datagram_t *datagram = fsm->datagram;
+    ec_slave_t *slave = fsm->slave;
+    u64 system_time, old_offset, new_offset;
+
+    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+        return;
+
+    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+        fsm->state = ec_fsm_slave_config_state_error;
+        EC_ERR("Failed to receive DC times datagram for slave %u"
+                " (datagram state %u).\n",
+                slave->ring_position, datagram->state);
+        return;
+    }
+
     if (datagram->working_counter != 1) {
-        fsm->slave->error_flag = 1;
-        fsm->state = ec_fsm_slave_config_state_error;
-        EC_ERR("Failed to clear DC assignment of slave %u: ",
-               fsm->slave->ring_position);
+        slave->error_flag = 1;
+        fsm->state = ec_fsm_slave_config_state_error;
+        EC_ERR("Failed to get DC times of slave %u: ",
+                slave->ring_position);
+        ec_datagram_print_wc_error(datagram);
+        return;
+    }
+
+    system_time = EC_READ_U64(datagram->data);
+    old_offset = EC_READ_U64(datagram->data + 16);
+    new_offset = slave->master->app_time - system_time + old_offset;
+
+    if (slave->master->debug_level)
+        EC_DBG("Slave %u: DC system_time=%llu old_offset=%llu, "
+                "app_time=%llu, new_offset=%llu\n",
+                slave->ring_position, system_time, old_offset,
+                slave->master->app_time, new_offset);
+
+    // set DC system time offset
+    ec_datagram_fpwr(datagram, slave->station_address, 0x0920, 8);
+    EC_WRITE_U64(datagram->data, new_offset);
+    fsm->retries = EC_FSM_RETRIES;
+    fsm->state = ec_fsm_slave_config_state_dc_write_offset;
+}
+
+/*****************************************************************************/
+
+/** Slave configuration state: DC WRITE OFFSET.
+ */
+void ec_fsm_slave_config_state_dc_write_offset(
+        ec_fsm_slave_config_t *fsm /**< slave state machine */
+        )
+{
+    ec_datagram_t *datagram = fsm->datagram;
+    ec_slave_t *slave = fsm->slave;
+
+    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+        return;
+
+    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+        fsm->state = ec_fsm_slave_config_state_error;
+        EC_ERR("Failed to receive DC system time offset datagram for slave %u"
+                " (datagram state %u).\n",
+                slave->ring_position, datagram->state);
+        return;
+    }
+
+    if (datagram->working_counter != 1) {
+        slave->error_flag = 1;
+        fsm->state = ec_fsm_slave_config_state_error;
+        EC_ERR("Failed to set DC system time offset of slave %u: ",
+                slave->ring_position);
         ec_datagram_print_wc_error(datagram);
         return;
     }
@@ -744,6 +828,8 @@
         ec_fsm_slave_config_t *fsm /**< slave state machine */
         )
 {
+	// TODO check for config here
+
     if (ec_fsm_pdo_exec(fsm->fsm_pdo))
         return;
 
@@ -840,11 +926,6 @@
         return;
     }
 
-    if (!fsm->slave->config) { // config removed in the meantime
-        ec_fsm_slave_config_reconfigure(fsm);
-        return;
-    }
-
     ec_fsm_slave_config_enter_fmmu(fsm);
 }
 
@@ -862,6 +943,11 @@
     const ec_fmmu_config_t *fmmu;
     const ec_sync_t *sync;
 
+    if (!slave->config) { // config removed in the meantime
+        ec_fsm_slave_config_reconfigure(fsm);
+        return;
+    }
+
     if (slave->base_fmmu_count < slave->config->used_fmmus) {
         slave->error_flag = 1;
         fsm->state = ec_fsm_slave_config_state_error;
@@ -872,7 +958,7 @@
     }
 
     if (!slave->base_fmmu_count) { // skip FMMU configuration
-        ec_fsm_slave_config_enter_dc_read(fsm);
+        ec_fsm_slave_config_enter_dc_cycle(fsm);
         return;
     }
 
@@ -928,111 +1014,32 @@
         return;
     }
 
-    ec_fsm_slave_config_enter_dc_read(fsm);
-}
-
-/*****************************************************************************/
-
-/** Check for DCs to be configured.
- */
-void ec_fsm_slave_config_enter_dc_read(
-        ec_fsm_slave_config_t *fsm /**< slave state machine */
-        )
-{
-    ec_slave_t *slave = fsm->slave;
-
-    if (slave->base_dc_supported) {
-        // read DC system time and system time offset
-        ec_datagram_fprd(fsm->datagram, slave->station_address, 0x0910, 24);
-        fsm->retries = EC_FSM_RETRIES;
-        fsm->state = ec_fsm_slave_config_state_dc_read;
-    } else {
-        ec_fsm_slave_config_enter_safeop(fsm);
-    }
-}
-
-/*****************************************************************************/
-
-/** Slave configuration state: DC READ.
- */
-void ec_fsm_slave_config_state_dc_read(
-        ec_fsm_slave_config_t *fsm /**< slave state machine */
-        )
-{
-    ec_datagram_t *datagram = fsm->datagram;
-    ec_slave_t *slave = fsm->slave;
-    u64 system_time, old_offset, new_offset;
-
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return;
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
-        fsm->state = ec_fsm_slave_config_state_error;
-        EC_ERR("Failed to receive DC times datagram for slave %u"
-                " (datagram state %u).\n",
-                slave->ring_position, datagram->state);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
-        slave->error_flag = 1;
-        fsm->state = ec_fsm_slave_config_state_error;
-        EC_ERR("Failed to get DC times of slave %u: ",
-                slave->ring_position);
-        ec_datagram_print_wc_error(datagram);
-        return;
-    }
-
-    system_time = EC_READ_U64(datagram->data);
-    old_offset = EC_READ_U64(datagram->data + 16);
-    new_offset = slave->master->app_time - system_time + old_offset;
-
-    if (slave->master->debug_level)
-        EC_DBG("Slave %u: DC system_time=%llu old_offset=%llu, "
-                "app_time=%llu, new_offset=%llu\n",
-                slave->ring_position, system_time, old_offset,
-                slave->master->app_time, new_offset);
-
-    // set DC system time offset
-    ec_datagram_fpwr(datagram, slave->station_address, 0x0920, 8);
-    EC_WRITE_U64(datagram->data, new_offset);
-    fsm->retries = EC_FSM_RETRIES;
-    fsm->state = ec_fsm_slave_config_state_dc_offset;
-}
-
-/*****************************************************************************/
-
-/** Slave configuration state: DC OFFSET.
- */
-void ec_fsm_slave_config_state_dc_offset(
-        ec_fsm_slave_config_t *fsm /**< slave state machine */
-        )
-{
-    ec_datagram_t *datagram = fsm->datagram;
-    ec_slave_t *slave = fsm->slave;
-    ec_slave_config_t *config = slave->config; // FIXME
-
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
-        return;
-
-    if (datagram->state != EC_DATAGRAM_RECEIVED) {
-        fsm->state = ec_fsm_slave_config_state_error;
-        EC_ERR("Failed to receive DC system time offset datagram for slave %u"
-                " (datagram state %u).\n",
-                slave->ring_position, datagram->state);
-        return;
-    }
-
-    if (datagram->working_counter != 1) {
-        slave->error_flag = 1;
-        fsm->state = ec_fsm_slave_config_state_error;
-        EC_ERR("Failed to set DC system time offset of slave %u: ",
-                slave->ring_position);
-        ec_datagram_print_wc_error(datagram);
+    ec_fsm_slave_config_enter_dc_cycle(fsm);
+}
+
+/*****************************************************************************/
+
+/** Check for DC to be configured.
+ */
+void ec_fsm_slave_config_enter_dc_cycle(
+        ec_fsm_slave_config_t *fsm /**< slave state machine */
+        )
+{
+    ec_datagram_t *datagram = fsm->datagram;
+    ec_slave_t *slave = fsm->slave;
+    ec_slave_config_t *config = slave->config;
+
+    if (!config) { // config removed in the meantime
+        ec_fsm_slave_config_reconfigure(fsm);
         return;
     }
 
     if (config->dc_assign_activate) {
+        if (!slave->base_dc_supported) {
+            EC_WARN("Slave %u seems not to support distributed clocks!\n",
+                    slave->ring_position);
+        }
+
         // set DC cycle times
         ec_datagram_fpwr(datagram, slave->station_address, 0x09A0, 8);
         EC_WRITE_U32(datagram->data, config->dc_sync_cycle_times[0]);
@@ -1078,7 +1085,7 @@
     }
 
     // set DC start time
-    start_time = slave->master->app_time + 1000000000; // now plus 1 s
+    start_time = slave->master->app_time + 10000000ULL; // now + 100 ms
     if (slave->master->debug_level)
         EC_DBG("Slave %u: Setting DC cyclic operation start time to %llu.\n",
                 slave->ring_position, start_time);
@@ -1099,7 +1106,12 @@
 {
     ec_datagram_t *datagram = fsm->datagram;
     ec_slave_t *slave = fsm->slave;
-    ec_slave_config_t *config = slave->config; // FIXME
+    ec_slave_config_t *config = slave->config;
+
+    if (!config) { // config removed in the meantime
+        ec_fsm_slave_config_reconfigure(fsm);
+        return;
+    }
 
     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
         return;
@@ -1153,7 +1165,7 @@
     if (datagram->working_counter != 1) {
         slave->error_flag = 1;
         fsm->state = ec_fsm_slave_config_state_error;
-        EC_ERR("Failed to set DC cyclia operation state of slave %u: ",
+        EC_ERR("Failed to set DC cyclic operation state of slave %u: ",
                 slave->ring_position);
         ec_datagram_print_wc_error(datagram);
         return;