1043 slave->ring_position); |
1043 slave->ring_position); |
1044 } |
1044 } |
1045 |
1045 |
1046 // set DC cycle times |
1046 // set DC cycle times |
1047 ec_datagram_fpwr(datagram, slave->station_address, 0x09A0, 8); |
1047 ec_datagram_fpwr(datagram, slave->station_address, 0x09A0, 8); |
1048 EC_WRITE_U32(datagram->data, config->dc_sync_cycle_times[0]); |
1048 EC_WRITE_U32(datagram->data, config->dc_sync[0].cycle_time); |
1049 EC_WRITE_U32(datagram->data + 4, config->dc_sync_cycle_times[1]); |
1049 EC_WRITE_U32(datagram->data + 4, config->dc_sync[1].cycle_time); |
1050 fsm->retries = EC_FSM_RETRIES; |
1050 fsm->retries = EC_FSM_RETRIES; |
1051 fsm->state = ec_fsm_slave_config_state_dc_cycle; |
1051 fsm->state = ec_fsm_slave_config_state_dc_cycle; |
1052 } else { |
1052 } else { |
1053 // DC are unused |
1053 // DC are unused |
1054 ec_fsm_slave_config_enter_safeop(fsm); |
1054 ec_fsm_slave_config_enter_safeop(fsm); |
1065 { |
1065 { |
1066 ec_datagram_t *datagram = fsm->datagram; |
1066 ec_datagram_t *datagram = fsm->datagram; |
1067 ec_slave_t *slave = fsm->slave; |
1067 ec_slave_t *slave = fsm->slave; |
1068 ec_master_t *master = slave->master; |
1068 ec_master_t *master = slave->master; |
1069 ec_slave_config_t *config = slave->config; |
1069 ec_slave_config_t *config = slave->config; |
|
1070 ec_sync_signal_t *sync0 = &config->dc_sync[0]; |
1070 u64 start_time; |
1071 u64 start_time; |
1071 |
1072 |
1072 if (!config) { // config removed in the meantime |
1073 if (!config) { // config removed in the meantime |
1073 ec_fsm_slave_config_reconfigure(fsm); |
1074 ec_fsm_slave_config_reconfigure(fsm); |
1074 return; |
1075 return; |
1096 |
1097 |
1097 // set DC start time |
1098 // set DC start time |
1098 start_time = master->app_time + 100000000ULL; // now + X ns |
1099 start_time = master->app_time + 100000000ULL; // now + X ns |
1099 // FIXME use slave's local system time here? |
1100 // FIXME use slave's local system time here? |
1100 |
1101 |
1101 if (config->dc_sync_cycle_times[0]) { |
1102 if (sync0->cycle_time) { |
1102 // find correct phase |
1103 // find correct phase |
1103 if (master->has_start_time) { |
1104 if (master->has_start_time) { |
1104 u32 cycle_time, shift_time, remainder; |
1105 u64 diff, start; |
1105 u64 start, diff; |
1106 u32 remainder; |
1106 |
1107 |
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; |
1108 diff = start_time - master->app_start_time; |
1110 remainder = do_div(diff, cycle_time); |
1109 remainder = do_div(diff, sync0->cycle_time); |
1111 |
1110 |
1112 start = start_time + cycle_time - remainder + shift_time; |
1111 start = start_time + |
|
1112 sync0->cycle_time - remainder + sync0->shift_time; |
1113 |
1113 |
1114 if (master->debug_level) { |
1114 if (master->debug_level) { |
1115 EC_DBG("app_start_time=%llu\n", master->app_start_time); |
1115 EC_DBG("app_start_time=%llu\n", master->app_start_time); |
1116 EC_DBG(" start_time=%llu\n", start_time); |
1116 EC_DBG(" start_time=%llu\n", start_time); |
1117 EC_DBG(" cycle_time=%u\n", cycle_time); |
1117 EC_DBG(" cycle_time=%u\n", sync0->cycle_time); |
1118 EC_DBG(" shift_time=%u\n", shift_time); |
1118 EC_DBG(" shift_time=%u\n", sync0->shift_time); |
1119 EC_DBG(" remainder=%u\n", remainder); |
1119 EC_DBG(" remainder=%u\n", remainder); |
1120 EC_DBG(" start=%llu\n", start); |
1120 EC_DBG(" start=%llu\n", start); |
1121 } |
1121 } |
1122 start_time = start; |
1122 start_time = start; |
1123 } else { |
1123 } else { |