[etherlab-dev] r1713 - Distributed Clocks IOCTL functions

[Florian Pose]

On Fri, Apr 24, 2009 at 03:17:09PM +0200, Florian Pose wrote:
> does the attached patch make any difference?

I mean this one... ;-)

-- 
Best regards,
Florian Pose

http://etherlab.org
-------------- next part --------------
Index: master/fsm_slave_config.c
===================================================================
--- master/fsm_slave_config.c	(Revision 1721)
+++ master/fsm_slave_config.c	(Arbeitskopie)
@@ -47,6 +47,7 @@
 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_dc_clear_assign(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_dc_read_time(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 *);
@@ -390,9 +391,48 @@
         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);
+    // read DC system time
+    ec_datagram_fprd(fsm->datagram, fsm->slave->station_address, 0x0910, 8);
     fsm->retries = EC_FSM_RETRIES;
+    fsm->state = ec_fsm_slave_config_state_dc_read_time;
+}
+
+/*****************************************************************************/
+
+/** Slave configuration state: DC READ TIME.
+ */
+void ec_fsm_slave_config_state_dc_read_time(
+        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 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 system time of slave %u: ",
+                slave->ring_position);
+        ec_datagram_print_wc_error(datagram);
+        return;
+    }
+
+    fsm->system_time = EC_READ_U64(datagram->data);
+
+    // read DC system time offset
+    ec_datagram_fprd(fsm->datagram, fsm->slave->station_address, 0x0920, 8);
+    fsm->retries = EC_FSM_RETRIES;
     fsm->state = ec_fsm_slave_config_state_dc_read_offset;
 }
 
@@ -406,7 +446,7 @@
 {
     ec_datagram_t *datagram = fsm->datagram;
     ec_slave_t *slave = fsm->slave;
-    u64 system_time, old_offset, new_offset;
+    u64 old_offset, new_offset;
 
     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
         return;
@@ -428,14 +468,15 @@
         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;
+    old_offset = EC_READ_U64(datagram->data);
 
+    // FIXME compensate time since reading of system time register?
+    new_offset = slave->master->app_time - fsm->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->ring_position, fsm->system_time, old_offset,
                 slave->master->app_time, new_offset);
 
     // set DC system time offset
Index: master/fsm_slave_config.h
===================================================================
--- master/fsm_slave_config.h	(Revision 1721)
+++ master/fsm_slave_config.h	(Arbeitskopie)
@@ -65,6 +65,7 @@
     ec_sdo_request_t request_copy; /**< Copied SDO request. */
     unsigned long jiffies_start; /**< For timeout calculations. */
     unsigned int take_time; /**< Store jiffies after datagram reception. */
+    uint64_t system_time; /**< DC system time. */
 };
 
 /*****************************************************************************/