--- a/master/fsm_slave_config.c Thu May 06 11:41:25 2010 +0200
+++ b/master/fsm_slave_config.c Thu May 06 11:42:52 2010 +0200
@@ -44,9 +44,20 @@
/*****************************************************************************/
-/** Time difference [ns] to tolerate without setting a new system time offset.
- */
-#define EC_SYSTEM_TIME_TOLERANCE_NS 100000000
+/** Maximum clock difference (in ns) before going to SAFEOP.
+ *
+ * Wait for DC time difference to drop under this absolute value before
+ * requesting SAFEOP.
+ */
+#define EC_DC_MAX_SYNC_DIFF_NS 5000
+
+/** Maximum time (in ms) to wait for clock discipline.
+ */
+#define EC_DC_SYNC_WAIT_MS 5000
+
+/** Time offset (in ns), that is added to cyclic start time.
+ */
+#define EC_DC_START_OFFSET 100000000ULL
/*****************************************************************************/
@@ -55,8 +66,6 @@
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_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 *);
@@ -67,6 +76,7 @@
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_cycle(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_dc_sync_check(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 *);
void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *);
@@ -391,172 +401,6 @@
ec_datagram_print_wc_error(datagram);
}
- // read DC system time (0x0910, 64 bit)
- // gap (64 bit)
- // and time offset (0x0920, 64 bit)
- 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;
-}
-
-/*****************************************************************************/
-
-/** Configure 32 bit time offset.
- */
-u64 ec_fsm_slave_config_dc_offset32(
- ec_fsm_slave_config_t *fsm, /**< slave state machine */
- u64 system_time, /**< System time register. */
- u64 old_offset, /**< Time offset register. */
- unsigned long jiffies_since_read /**< Jiffies for correction. */
- )
-{
- ec_slave_t *slave = fsm->slave;
- u32 correction, system_time32, old_offset32, new_offset;
- s32 time_diff;
-
- system_time32 = (u32) system_time;
- old_offset32 = (u32) old_offset;
-
- // correct read system time by elapsed time since read operation
- correction = jiffies_since_read * 1000 / HZ * 1000000;
- system_time32 += correction;
- time_diff = (u32) slave->master->app_time - system_time32;
-
- EC_SLAVE_DBG(slave, 1, "Calculating DC time offset (32 bit):"
- " system_time=%u (corrected with %u), app_time=%u, diff=%i\n",
- system_time32, correction,
- (u32) slave->master->app_time, time_diff);
-
- if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) {
- new_offset = time_diff + old_offset32;
- EC_SLAVE_DBG(slave, 1, "Setting time offset to %u (was %u)\n",
- new_offset, old_offset32);
- return (u64) new_offset;
- } else {
- EC_SLAVE_DBG(slave, 1, "Not touching time offset.\n");
- return old_offset;
- }
-}
-
-/*****************************************************************************/
-
-/** Configure 64 bit time offset.
- */
-u64 ec_fsm_slave_config_dc_offset64(
- ec_fsm_slave_config_t *fsm, /**< slave state machine */
- u64 system_time, /**< System time register. */
- u64 old_offset, /**< Time offset register. */
- unsigned long jiffies_since_read /**< Jiffies for correction. */
- )
-{
- ec_slave_t *slave = fsm->slave;
- u64 new_offset, correction;
- s64 time_diff;
-
- // correct read system time by elapsed time since read operation
- correction = (u64) (jiffies_since_read * 1000 / HZ) * 1000000;
- system_time += correction;
- time_diff = fsm->slave->master->app_time - system_time;
-
- EC_SLAVE_DBG(slave, 1, "Calculating DC time offset (64 bit):"
- " system_time=%llu (corrected with %llu),"
- " app_time=%llu, diff=%lli\n",
- system_time, correction,
- slave->master->app_time, time_diff);
-
- if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) {
- new_offset = time_diff + old_offset;
- EC_SLAVE_DBG(slave, 1, "Setting time offset to %llu (was %llu)\n",
- new_offset, old_offset);
- } else {
- new_offset = old_offset;
- EC_SLAVE_DBG(slave, 1, "Not touching time offset.\n");
- }
-
- return new_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;
- unsigned long jiffies_since_read;
-
- 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_SLAVE_ERR(slave, "Failed to receive DC times datagram: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
- slave->error_flag = 1;
- fsm->state = ec_fsm_slave_config_state_error;
- EC_SLAVE_ERR(slave, "Failed to get DC times: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
- system_time = EC_READ_U64(datagram->data); // 0x0910
- old_offset = EC_READ_U64(datagram->data + 16); // 0x0920
- jiffies_since_read = jiffies - datagram->jiffies_sent;
-
- if (slave->base_dc_range == EC_DC_32) {
- new_offset = ec_fsm_slave_config_dc_offset32(fsm,
- system_time, old_offset, jiffies_since_read);
- } else {
- new_offset = ec_fsm_slave_config_dc_offset64(fsm,
- system_time, old_offset, jiffies_since_read);
- }
-
- // set DC system time offset and transmission delay
- ec_datagram_fpwr(datagram, slave->station_address, 0x0920, 12);
- EC_WRITE_U64(datagram->data, new_offset);
- EC_WRITE_U32(datagram->data + 8, slave->transmission_delay);
- 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_SLAVE_ERR(slave, "Failed to receive DC system time offset"
- " datagram: ");
- ec_datagram_print_state(datagram);
- return;
- }
-
- if (datagram->working_counter != 1) {
- slave->error_flag = 1;
- fsm->state = ec_fsm_slave_config_state_error;
- EC_SLAVE_ERR(slave, "Failed to set DC system time offset: ");
- ec_datagram_print_wc_error(datagram);
- return;
- }
-
ec_fsm_slave_config_enter_mbox_sync(fsm);
}
@@ -1319,8 +1163,53 @@
{
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 (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(slave, "Failed to receive DC cycle times datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(slave, "Failed to set DC cycle times: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ EC_SLAVE_DBG(slave, 1, "Checking for synchrony.\n");
+
+ fsm->jiffies_start = jiffies;
+ ec_datagram_fprd(datagram, slave->station_address, 0x092c, 4);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_slave_config_state_dc_sync_check;
+}
+
+/*****************************************************************************/
+
+/** Slave configuration state: DC SYNC CHECK.
+ */
+void ec_fsm_slave_config_state_dc_sync_check(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
ec_slave_config_t *config = slave->config;
+ uint32_t abs_sync_diff;
+ unsigned long diff_ms;
ec_sync_signal_t *sync0 = &config->dc_sync[0];
u64 start_time;
@@ -1334,7 +1223,7 @@
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_slave_config_state_error;
- EC_SLAVE_ERR(slave, "Failed to receive DC cycle times datagram: ");
+ EC_SLAVE_ERR(slave, "Failed to receive DC sync check datagram: ");
ec_datagram_print_state(datagram);
return;
}
@@ -1342,18 +1231,40 @@
if (datagram->working_counter != 1) {
slave->error_flag = 1;
fsm->state = ec_fsm_slave_config_state_error;
- EC_SLAVE_ERR(slave, "Failed to set DC cycle times: ");
+ EC_SLAVE_ERR(slave, "Failed to check DC synchrony: ");
ec_datagram_print_wc_error(datagram);
return;
}
+ abs_sync_diff = EC_READ_U32(datagram->data) & 0x7fffffff;
+ diff_ms = (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+
+ if (abs_sync_diff > EC_DC_MAX_SYNC_DIFF_NS) {
+
+ if (diff_ms >= EC_DC_SYNC_WAIT_MS) {
+ EC_SLAVE_WARN(slave, "Slave did not sync after %u ms.\n",
+ (u32) diff_ms);
+ } else {
+ EC_SLAVE_DBG(slave, 1, "Sync after %4u ms: %10u ns\n",
+ (u32) diff_ms, abs_sync_diff);
+
+ // check synchrony again
+ ec_datagram_fprd(datagram, slave->station_address, 0x092c, 4);
+ fsm->retries = EC_FSM_RETRIES;
+ return;
+ }
+ } else {
+ EC_SLAVE_DBG(slave, 1, "%u ns difference after %u ms.\n",
+ abs_sync_diff, (u32) diff_ms);
+ }
+
// set DC start time
- start_time = master->app_time + 100000000ULL; // now + X ns
+ start_time = master->app_time + EC_DC_START_OFFSET; // now + X ns
// FIXME use slave's local system time here?
if (sync0->cycle_time) {
// find correct phase
- if (master->has_start_time) {
+ if (master->has_app_time) {
u64 diff, start;
u32 remainder;
@@ -1363,20 +1274,13 @@
start = start_time +
sync0->cycle_time - remainder + sync0->shift_time;
- if (master->debug_level) {
- EC_SLAVE_DBG(slave, 1, "app_start_time=%llu\n",
- master->app_start_time);
- EC_SLAVE_DBG(slave, 1, " start_time=%llu\n",
- start_time);
- EC_SLAVE_DBG(slave, 1, " cycle_time=%u\n",
- sync0->cycle_time);
- EC_SLAVE_DBG(slave, 1, " shift_time=%u\n",
- sync0->shift_time);
- EC_SLAVE_DBG(slave, 1, " remainder=%u\n",
- remainder);
- EC_SLAVE_DBG(slave, 1, " start=%llu\n",
- start);
- }
+ EC_SLAVE_DBG(slave, 1, "app_start_time=%llu\n",
+ master->app_start_time);
+ EC_SLAVE_DBG(slave, 1, " start_time=%llu\n", start_time);
+ EC_SLAVE_DBG(slave, 1, " cycle_time=%u\n", sync0->cycle_time);
+ EC_SLAVE_DBG(slave, 1, " shift_time=%u\n", sync0->shift_time);
+ EC_SLAVE_DBG(slave, 1, " remainder=%u\n", remainder);
+ EC_SLAVE_DBG(slave, 1, " start=%llu\n", start);
start_time = start;
} else {
EC_SLAVE_WARN(slave, "No application time supplied."