32 * EtherCAT slave configuration state machine. |
32 * EtherCAT slave configuration state machine. |
33 */ |
33 */ |
34 |
34 |
35 /*****************************************************************************/ |
35 /*****************************************************************************/ |
36 |
36 |
|
37 #include <asm/div64.h> |
|
38 |
37 #include "globals.h" |
39 #include "globals.h" |
38 #include "master.h" |
40 #include "master.h" |
39 #include "mailbox.h" |
41 #include "mailbox.h" |
40 #include "slave_config.h" |
42 #include "slave_config.h" |
41 #include "fsm_slave_config.h" |
43 #include "fsm_slave_config.h" |
1061 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1063 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1062 ) |
1064 ) |
1063 { |
1065 { |
1064 ec_datagram_t *datagram = fsm->datagram; |
1066 ec_datagram_t *datagram = fsm->datagram; |
1065 ec_slave_t *slave = fsm->slave; |
1067 ec_slave_t *slave = fsm->slave; |
|
1068 ec_master_t *master = slave->master; |
1066 ec_slave_config_t *config = slave->config; |
1069 ec_slave_config_t *config = slave->config; |
1067 u64 start_time; |
1070 u64 start_time; |
1068 |
1071 |
1069 if (!config) { // config removed in the meantime |
1072 if (!config) { // config removed in the meantime |
1070 ec_fsm_slave_config_reconfigure(fsm); |
1073 ec_fsm_slave_config_reconfigure(fsm); |
1090 ec_datagram_print_wc_error(datagram); |
1093 ec_datagram_print_wc_error(datagram); |
1091 return; |
1094 return; |
1092 } |
1095 } |
1093 |
1096 |
1094 // set DC start time |
1097 // set DC start time |
1095 start_time = slave->master->app_time + |
1098 start_time = master->app_time + 100000000ULL; // now + X ns |
1096 config->dc_sync_shift_times[0] + 100000000ULL; // now + shift + x ns |
1099 // FIXME use slave's local system time here? |
1097 if (slave->master->debug_level) |
1100 |
|
1101 if (config->dc_sync_cycle_times[0]) { |
|
1102 // find correct phase |
|
1103 if (master->has_start_time) { |
|
1104 u32 cycle_time, shift_time, remainder; |
|
1105 u64 start, diff; |
|
1106 |
|
1107 cycle_time = config->dc_sync_cycle_times[0]; |
|
1108 shift_time = config->dc_sync_shift_times[0]; |
|
1109 diff = start_time - master->app_start_time; |
|
1110 remainder = do_div(diff, cycle_time); |
|
1111 |
|
1112 start = start_time + cycle_time - remainder + shift_time; |
|
1113 |
|
1114 if (master->debug_level) { |
|
1115 EC_DBG("app_start_time=%llu\n", master->app_start_time); |
|
1116 EC_DBG(" start_time=%llu\n", start_time); |
|
1117 EC_DBG(" cycle_time=%u\n", cycle_time); |
|
1118 EC_DBG(" shift_time=%u\n", shift_time); |
|
1119 EC_DBG(" remainder=%u\n", remainder); |
|
1120 EC_DBG(" start=%llu\n", start); |
|
1121 } |
|
1122 start_time = start; |
|
1123 } else { |
|
1124 EC_WARN("No application time supplied. Cyclic start time will " |
|
1125 "not be in phase for slave %u.", slave->ring_position); |
|
1126 } |
|
1127 } |
|
1128 |
|
1129 if (master->debug_level) |
1098 EC_DBG("Slave %u: Setting DC cyclic operation start time to %llu.\n", |
1130 EC_DBG("Slave %u: Setting DC cyclic operation start time to %llu.\n", |
1099 slave->ring_position, start_time); |
1131 slave->ring_position, start_time); |
1100 |
1132 |
1101 ec_datagram_fpwr(datagram, slave->station_address, 0x0990, 8); |
1133 ec_datagram_fpwr(datagram, slave->station_address, 0x0990, 8); |
1102 EC_WRITE_U64(datagram->data, start_time); |
1134 EC_WRITE_U64(datagram->data, start_time); |