DC sync reference clock to application time. TBC...

include/ecrt.h

@@ -67,9 +67,11 @@
 #ifdef __KERNEL__
 #include <asm/byteorder.h>
 #include <linux/types.h>
+#include <linux/time.h>
 #else
 #include <stdlib.h> // for size_t
 #include <stdint.h>
+#include <sys/time.h> // for struct timeval
 #endif
 
 /******************************************************************************
@@ -505,9 +507,13 @@ void ecrt_master_state(
         );
 
 /** Queues the DC drift compensation datagram for sending.
+ *
+ * The reference clock will by synchronized to the \a app_time, while the
+ * other slaves will by synchronized to the reference clock.
  */
 void ecrt_master_sync(
-        ec_master_t *master /**< EtherCAT master. */
+        ec_master_t *master, /**< EtherCAT master. */
+        const struct timeval *app_time /**< Application time. */
         );
 
 /******************************************************************************
@@ -685,14 +691,14 @@ int ecrt_slave_config_reg_pdo_entry(
  *
  * The AssignActivate word is vendor-specific and can be taken from the XML
  * device description file (Device -> Dc -> AssignActivate). Set this to zero,
- * if the slave shall be not operated without distributed clocks (default).
+ * if the slave shall be operated without distributed clocks (default).
  */
 void ecrt_slave_config_dc_assign_activate(
 		ec_slave_config_t *sc, /**< Slave configuration. */
         uint16_t assign_activate /**< AssignActivate word. */
 		);
 
-/** Sets the cylce times for the SYNC0 and SYNC1 signals.
+/** Sets the cycle times for the SYNC0 and SYNC1 signals.
  */
 void ecrt_slave_config_dc_sync_cycle_times(
 		ec_slave_config_t *sc, /**< Slave configuration. */

master/fsm_slave_config.c

@@ -52,6 +52,8 @@ 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 *);
@@ -66,8 +68,7 @@ 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_cycle(ec_fsm_slave_config_t *);
-void ec_fsm_slave_config_enter_dc_assign(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_enter_dc_read(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 *);
@@ -811,7 +812,7 @@ void ec_fsm_slave_config_enter_fmmu(
     }
 
     if (!slave->base_fmmu_count) { // skip FMMU configuration
-        ec_fsm_slave_config_enter_dc_cycle(fsm);
+        ec_fsm_slave_config_enter_dc_read(fsm);
         return;
     }
 
@@ -867,28 +868,111 @@ void ec_fsm_slave_config_state_fmmu(
         return;
     }
 
-	ec_fsm_slave_config_enter_dc_cycle(fsm);
+    ec_fsm_slave_config_enter_dc_read(fsm);
 }
 
 /*****************************************************************************/
 
 /** Check for DCs to be configured.
  */
-void ec_fsm_slave_config_enter_dc_cycle(
+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;
-    ec_slave_config_t *config = slave->config;
+    u64 system_time, old_offset, new_offset;
 
-    if (config->dc_assign_activate) {
-        if (!slave->base_dc_supported) {
-            EC_WARN("Attempt to enable synchronized mode for slave %u,"
-                    " that seems not to support distributed clocks!\n",
-                    slave->ring_position);
-        }
+    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);
+        return;
+    }
+
+    if (config->dc_assign_activate) {
         // set DC cycle times
         ec_datagram_fpwr(datagram, slave->station_address, 0x09A0, 8);
         EC_WRITE_U32(datagram->data, config->dc_sync_cycle_times[0]);
@@ -896,7 +980,7 @@ void ec_fsm_slave_config_enter_dc_cycle(
         fsm->retries = EC_FSM_RETRIES;
         fsm->state = ec_fsm_slave_config_state_dc_cycle;
     } else {
-        ec_fsm_slave_config_enter_dc_assign(fsm);
+        ec_fsm_slave_config_enter_safeop(fsm);
     }
 }
 
@@ -910,6 +994,7 @@ void ec_fsm_slave_config_state_dc_cycle(
 {
     ec_datagram_t *datagram = fsm->datagram;
     ec_slave_t *slave = fsm->slave;
+    u64 start_time;
 
     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
         return;
@@ -932,8 +1017,13 @@ void ec_fsm_slave_config_state_dc_cycle(
     }
 
     // set DC start time
+    start_time = slave->master->app_time + 1000000000; // now plus 1 s
+    if (slave->master->debug_level)
+        EC_DBG("Slave %u: Setting DC cyclic operation start time to %llu.\n",
+                slave->ring_position, start_time);
+
     ec_datagram_fpwr(datagram, slave->station_address, 0x0990, 8);
-    EC_WRITE_U64(datagram->data, 0x37E11D600ULL); // 15 s, FIXME
+    EC_WRITE_U64(datagram->data, start_time);
     fsm->retries = EC_FSM_RETRIES;
     fsm->state = ec_fsm_slave_config_state_dc_start;
 }
@@ -948,6 +1038,7 @@ void ec_fsm_slave_config_state_dc_start(
 {
     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;
@@ -969,21 +1060,6 @@ void ec_fsm_slave_config_state_dc_start(
         return;
     }
 
-    ec_fsm_slave_config_enter_dc_assign(fsm);
-}
-
-/*****************************************************************************/
-
-/** Set the DC AssignActivate word.
- */
-void ec_fsm_slave_config_enter_dc_assign(
-        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;
-
     // assign sync unit to EtherCAT or PDI
     ec_datagram_fpwr(datagram, slave->station_address, 0x0980, 2);
     EC_WRITE_U16(datagram->data, config->dc_assign_activate);

master/globals.h

@@ -251,6 +251,13 @@ enum {
         .name = EC_STR(NAME), .owner = THIS_MODULE, .mode = S_IRUGO | S_IWUSR \
     }
 
+/** Timeval to nanoseconds conversion.
+ *
+ * \param TV Pointer to struct timeval.
+ */
+#define EC_TIMEVAL2NANO(TV) \
+    (((TV)->tv_sec - 946684800ULL) * 1000000000ULL + (TV)->tv_usec * 1000ULL)
+
 /*****************************************************************************/
 
 extern char *ec_master_version_str;

master/master.c

@@ -198,6 +198,16 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
     // create state machine object
     ec_fsm_master_init(&master->fsm, master, &master->fsm_datagram);
 
+    // init reference sync datagram
+    ec_datagram_init(&master->ref_sync_datagram);
+    snprintf(master->ref_sync_datagram.name, EC_DATAGRAM_NAME_SIZE, "refsync");
+    ret = ec_datagram_apwr(&master->ref_sync_datagram, 0, 0x0910, 8);
+    if (ret < 0) {
+        ec_datagram_clear(&master->ref_sync_datagram);
+        EC_ERR("Failed to allocate reference synchronisation datagram.\n");
+        goto out_clear_fsm;
+    }
+
     // init sync datagram
     ec_datagram_init(&master->sync_datagram);
     snprintf(master->sync_datagram.name, EC_DATAGRAM_NAME_SIZE, "sync");
@@ -205,7 +215,7 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
     if (ret < 0) {
         ec_datagram_clear(&master->sync_datagram);
         EC_ERR("Failed to allocate synchronisation datagram.\n");
-        goto out_clear_fsm;
+        goto out_clear_ref_sync;
     }
 
     // init character device
@@ -242,6 +252,8 @@ out_clear_cdev:
     ec_cdev_clear(&master->cdev);
 out_clear_sync:
     ec_datagram_clear(&master->sync_datagram);
+out_clear_ref_sync:
+    ec_datagram_clear(&master->ref_sync_datagram);
 out_clear_fsm:
     ec_fsm_master_clear(&master->fsm);
     ec_datagram_clear(&master->fsm_datagram);
@@ -277,6 +289,7 @@ void ec_master_clear(
     ec_master_clear_slaves(master);
 
     ec_datagram_clear(&master->sync_datagram);
+    ec_datagram_clear(&master->ref_sync_datagram);
     ec_fsm_master_clear(&master->fsm);
     ec_datagram_clear(&master->fsm_datagram);
     ec_device_clear(&master->backup_device);
@@ -1607,10 +1620,19 @@ void ecrt_master_state(const ec_master_t *master, ec_master_state_t *state)
 
 /*****************************************************************************/
 
-void ecrt_master_sync(ec_master_t *master)
+void ecrt_master_sync(ec_master_t *master, const struct timeval *app_time)
 {
+    master->app_time = EC_TIMEVAL2NANO(app_time);
+
+#if 1
+    EC_WRITE_U32(master->ref_sync_datagram.data, master->app_time);
+    ec_master_queue_datagram(master, &master->ref_sync_datagram);
+#endif
+
+#if 1
     ec_datagram_zero(&master->sync_datagram);
     ec_master_queue_datagram(master, &master->sync_datagram);
+#endif
 }
 
 /*****************************************************************************/

master/master.h

@@ -117,6 +117,9 @@ struct ec_master {
 
     struct list_head configs; /**< List of slave configurations. */
     
+    u64 app_time; /**< Time of the last ecrt_master_sync() call. */
+    ec_datagram_t ref_sync_datagram; /**< Datagram used for synchronizing the
+                                       reference clock to the master clock. */
     ec_datagram_t sync_datagram; /**< Datagram used for DC drift
                                    compensation. */