42 #include "slave_config.h" |
42 #include "slave_config.h" |
43 #include "fsm_slave_config.h" |
43 #include "fsm_slave_config.h" |
44 |
44 |
45 /*****************************************************************************/ |
45 /*****************************************************************************/ |
46 |
46 |
47 /** Time difference [ns] to tolerate without setting a new system time offset. |
47 /** Maximum clock difference (in ns) before going to SAFEOP. |
48 */ |
48 * |
49 #define EC_SYSTEM_TIME_TOLERANCE_NS 100000000 |
49 * Wait for DC time difference to drop under this absolute value before |
|
50 * requesting SAFEOP. |
|
51 */ |
|
52 #define EC_DC_MAX_SYNC_DIFF_NS 5000 |
|
53 |
|
54 /** Maximum time (in ms) to wait for clock discipline. |
|
55 */ |
|
56 #define EC_DC_SYNC_WAIT_MS 5000 |
|
57 |
|
58 /** Time offset (in ns), that is added to cyclic start time. |
|
59 */ |
|
60 #define EC_DC_START_OFFSET 100000000ULL |
50 |
61 |
51 /*****************************************************************************/ |
62 /*****************************************************************************/ |
52 |
63 |
53 void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *); |
64 void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *); |
54 void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *); |
65 void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *); |
55 void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *); |
66 void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *); |
56 void ec_fsm_slave_config_state_clear_sync(ec_fsm_slave_config_t *); |
67 void ec_fsm_slave_config_state_clear_sync(ec_fsm_slave_config_t *); |
57 void ec_fsm_slave_config_state_dc_clear_assign(ec_fsm_slave_config_t *); |
68 void ec_fsm_slave_config_state_dc_clear_assign(ec_fsm_slave_config_t *); |
58 void ec_fsm_slave_config_state_dc_read_offset(ec_fsm_slave_config_t *); |
|
59 void ec_fsm_slave_config_state_dc_write_offset(ec_fsm_slave_config_t *); |
|
60 void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *); |
69 void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *); |
61 void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *); |
70 void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *); |
62 void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *); |
71 void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *); |
63 void ec_fsm_slave_config_state_soe_conf(ec_fsm_slave_config_t *); |
72 void ec_fsm_slave_config_state_soe_conf(ec_fsm_slave_config_t *); |
64 void ec_fsm_slave_config_state_watchdog_divider(ec_fsm_slave_config_t *); |
73 void ec_fsm_slave_config_state_watchdog_divider(ec_fsm_slave_config_t *); |
65 void ec_fsm_slave_config_state_watchdog(ec_fsm_slave_config_t *); |
74 void ec_fsm_slave_config_state_watchdog(ec_fsm_slave_config_t *); |
66 void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *); |
75 void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *); |
67 void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *); |
76 void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *); |
68 void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *); |
77 void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *); |
69 void ec_fsm_slave_config_state_dc_cycle(ec_fsm_slave_config_t *); |
78 void ec_fsm_slave_config_state_dc_cycle(ec_fsm_slave_config_t *); |
|
79 void ec_fsm_slave_config_state_dc_sync_check(ec_fsm_slave_config_t *); |
70 void ec_fsm_slave_config_state_dc_start(ec_fsm_slave_config_t *); |
80 void ec_fsm_slave_config_state_dc_start(ec_fsm_slave_config_t *); |
71 void ec_fsm_slave_config_state_dc_assign(ec_fsm_slave_config_t *); |
81 void ec_fsm_slave_config_state_dc_assign(ec_fsm_slave_config_t *); |
72 void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *); |
82 void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *); |
73 void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *); |
83 void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *); |
74 |
84 |
387 |
397 |
388 if (datagram->working_counter != 1) { |
398 if (datagram->working_counter != 1) { |
389 // clearing the DC assignment does not succeed on simple slaves |
399 // clearing the DC assignment does not succeed on simple slaves |
390 EC_SLAVE_DBG(fsm->slave, 1, "Failed to clear DC assignment: "); |
400 EC_SLAVE_DBG(fsm->slave, 1, "Failed to clear DC assignment: "); |
391 ec_datagram_print_wc_error(datagram); |
401 ec_datagram_print_wc_error(datagram); |
392 } |
|
393 |
|
394 // read DC system time (0x0910, 64 bit) |
|
395 // gap (64 bit) |
|
396 // and time offset (0x0920, 64 bit) |
|
397 ec_datagram_fprd(fsm->datagram, fsm->slave->station_address, 0x0910, 24); |
|
398 fsm->retries = EC_FSM_RETRIES; |
|
399 fsm->state = ec_fsm_slave_config_state_dc_read_offset; |
|
400 } |
|
401 |
|
402 /*****************************************************************************/ |
|
403 |
|
404 /** Configure 32 bit time offset. |
|
405 */ |
|
406 u64 ec_fsm_slave_config_dc_offset32( |
|
407 ec_fsm_slave_config_t *fsm, /**< slave state machine */ |
|
408 u64 system_time, /**< System time register. */ |
|
409 u64 old_offset, /**< Time offset register. */ |
|
410 unsigned long jiffies_since_read /**< Jiffies for correction. */ |
|
411 ) |
|
412 { |
|
413 ec_slave_t *slave = fsm->slave; |
|
414 u32 correction, system_time32, old_offset32, new_offset; |
|
415 s32 time_diff; |
|
416 |
|
417 system_time32 = (u32) system_time; |
|
418 old_offset32 = (u32) old_offset; |
|
419 |
|
420 // correct read system time by elapsed time since read operation |
|
421 correction = jiffies_since_read * 1000 / HZ * 1000000; |
|
422 system_time32 += correction; |
|
423 time_diff = (u32) slave->master->app_time - system_time32; |
|
424 |
|
425 EC_SLAVE_DBG(slave, 1, "Calculating DC time offset (32 bit):" |
|
426 " system_time=%u (corrected with %u), app_time=%u, diff=%i\n", |
|
427 system_time32, correction, |
|
428 (u32) slave->master->app_time, time_diff); |
|
429 |
|
430 if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) { |
|
431 new_offset = time_diff + old_offset32; |
|
432 EC_SLAVE_DBG(slave, 1, "Setting time offset to %u (was %u)\n", |
|
433 new_offset, old_offset32); |
|
434 return (u64) new_offset; |
|
435 } else { |
|
436 EC_SLAVE_DBG(slave, 1, "Not touching time offset.\n"); |
|
437 return old_offset; |
|
438 } |
|
439 } |
|
440 |
|
441 /*****************************************************************************/ |
|
442 |
|
443 /** Configure 64 bit time offset. |
|
444 */ |
|
445 u64 ec_fsm_slave_config_dc_offset64( |
|
446 ec_fsm_slave_config_t *fsm, /**< slave state machine */ |
|
447 u64 system_time, /**< System time register. */ |
|
448 u64 old_offset, /**< Time offset register. */ |
|
449 unsigned long jiffies_since_read /**< Jiffies for correction. */ |
|
450 ) |
|
451 { |
|
452 ec_slave_t *slave = fsm->slave; |
|
453 u64 new_offset, correction; |
|
454 s64 time_diff; |
|
455 |
|
456 // correct read system time by elapsed time since read operation |
|
457 correction = (u64) (jiffies_since_read * 1000 / HZ) * 1000000; |
|
458 system_time += correction; |
|
459 time_diff = fsm->slave->master->app_time - system_time; |
|
460 |
|
461 EC_SLAVE_DBG(slave, 1, "Calculating DC time offset (64 bit):" |
|
462 " system_time=%llu (corrected with %llu)," |
|
463 " app_time=%llu, diff=%lli\n", |
|
464 system_time, correction, |
|
465 slave->master->app_time, time_diff); |
|
466 |
|
467 if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) { |
|
468 new_offset = time_diff + old_offset; |
|
469 EC_SLAVE_DBG(slave, 1, "Setting time offset to %llu (was %llu)\n", |
|
470 new_offset, old_offset); |
|
471 } else { |
|
472 new_offset = old_offset; |
|
473 EC_SLAVE_DBG(slave, 1, "Not touching time offset.\n"); |
|
474 } |
|
475 |
|
476 return new_offset; |
|
477 } |
|
478 |
|
479 /*****************************************************************************/ |
|
480 |
|
481 /** Slave configuration state: DC READ OFFSET. |
|
482 */ |
|
483 void ec_fsm_slave_config_state_dc_read_offset( |
|
484 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
|
485 ) |
|
486 { |
|
487 ec_datagram_t *datagram = fsm->datagram; |
|
488 ec_slave_t *slave = fsm->slave; |
|
489 u64 system_time, old_offset, new_offset; |
|
490 unsigned long jiffies_since_read; |
|
491 |
|
492 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
|
493 return; |
|
494 |
|
495 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
496 fsm->state = ec_fsm_slave_config_state_error; |
|
497 EC_SLAVE_ERR(slave, "Failed to receive DC times datagram: "); |
|
498 ec_datagram_print_state(datagram); |
|
499 return; |
|
500 } |
|
501 |
|
502 if (datagram->working_counter != 1) { |
|
503 slave->error_flag = 1; |
|
504 fsm->state = ec_fsm_slave_config_state_error; |
|
505 EC_SLAVE_ERR(slave, "Failed to get DC times: "); |
|
506 ec_datagram_print_wc_error(datagram); |
|
507 return; |
|
508 } |
|
509 |
|
510 system_time = EC_READ_U64(datagram->data); // 0x0910 |
|
511 old_offset = EC_READ_U64(datagram->data + 16); // 0x0920 |
|
512 jiffies_since_read = jiffies - datagram->jiffies_sent; |
|
513 |
|
514 if (slave->base_dc_range == EC_DC_32) { |
|
515 new_offset = ec_fsm_slave_config_dc_offset32(fsm, |
|
516 system_time, old_offset, jiffies_since_read); |
|
517 } else { |
|
518 new_offset = ec_fsm_slave_config_dc_offset64(fsm, |
|
519 system_time, old_offset, jiffies_since_read); |
|
520 } |
|
521 |
|
522 // set DC system time offset and transmission delay |
|
523 ec_datagram_fpwr(datagram, slave->station_address, 0x0920, 12); |
|
524 EC_WRITE_U64(datagram->data, new_offset); |
|
525 EC_WRITE_U32(datagram->data + 8, slave->transmission_delay); |
|
526 fsm->retries = EC_FSM_RETRIES; |
|
527 fsm->state = ec_fsm_slave_config_state_dc_write_offset; |
|
528 } |
|
529 |
|
530 /*****************************************************************************/ |
|
531 |
|
532 /** Slave configuration state: DC WRITE OFFSET. |
|
533 */ |
|
534 void ec_fsm_slave_config_state_dc_write_offset( |
|
535 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
|
536 ) |
|
537 { |
|
538 ec_datagram_t *datagram = fsm->datagram; |
|
539 ec_slave_t *slave = fsm->slave; |
|
540 |
|
541 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
|
542 return; |
|
543 |
|
544 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
545 fsm->state = ec_fsm_slave_config_state_error; |
|
546 EC_SLAVE_ERR(slave, "Failed to receive DC system time offset" |
|
547 " datagram: "); |
|
548 ec_datagram_print_state(datagram); |
|
549 return; |
|
550 } |
|
551 |
|
552 if (datagram->working_counter != 1) { |
|
553 slave->error_flag = 1; |
|
554 fsm->state = ec_fsm_slave_config_state_error; |
|
555 EC_SLAVE_ERR(slave, "Failed to set DC system time offset: "); |
|
556 ec_datagram_print_wc_error(datagram); |
|
557 return; |
|
558 } |
402 } |
559 |
403 |
560 ec_fsm_slave_config_enter_mbox_sync(fsm); |
404 ec_fsm_slave_config_enter_mbox_sync(fsm); |
561 } |
405 } |
562 |
406 |
1317 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1161 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1318 ) |
1162 ) |
1319 { |
1163 { |
1320 ec_datagram_t *datagram = fsm->datagram; |
1164 ec_datagram_t *datagram = fsm->datagram; |
1321 ec_slave_t *slave = fsm->slave; |
1165 ec_slave_t *slave = fsm->slave; |
|
1166 ec_slave_config_t *config = slave->config; |
|
1167 |
|
1168 if (!config) { // config removed in the meantime |
|
1169 ec_fsm_slave_config_reconfigure(fsm); |
|
1170 return; |
|
1171 } |
|
1172 |
|
1173 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
|
1174 return; |
|
1175 |
|
1176 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
1177 fsm->state = ec_fsm_slave_config_state_error; |
|
1178 EC_SLAVE_ERR(slave, "Failed to receive DC cycle times datagram: "); |
|
1179 ec_datagram_print_state(datagram); |
|
1180 return; |
|
1181 } |
|
1182 |
|
1183 if (datagram->working_counter != 1) { |
|
1184 slave->error_flag = 1; |
|
1185 fsm->state = ec_fsm_slave_config_state_error; |
|
1186 EC_SLAVE_ERR(slave, "Failed to set DC cycle times: "); |
|
1187 ec_datagram_print_wc_error(datagram); |
|
1188 return; |
|
1189 } |
|
1190 |
|
1191 EC_SLAVE_DBG(slave, 1, "Checking for synchrony.\n"); |
|
1192 |
|
1193 fsm->jiffies_start = jiffies; |
|
1194 ec_datagram_fprd(datagram, slave->station_address, 0x092c, 4); |
|
1195 fsm->retries = EC_FSM_RETRIES; |
|
1196 fsm->state = ec_fsm_slave_config_state_dc_sync_check; |
|
1197 } |
|
1198 |
|
1199 /*****************************************************************************/ |
|
1200 |
|
1201 /** Slave configuration state: DC SYNC CHECK. |
|
1202 */ |
|
1203 void ec_fsm_slave_config_state_dc_sync_check( |
|
1204 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
|
1205 ) |
|
1206 { |
|
1207 ec_datagram_t *datagram = fsm->datagram; |
|
1208 ec_slave_t *slave = fsm->slave; |
1322 ec_master_t *master = slave->master; |
1209 ec_master_t *master = slave->master; |
1323 ec_slave_config_t *config = slave->config; |
1210 ec_slave_config_t *config = slave->config; |
|
1211 uint32_t abs_sync_diff; |
|
1212 unsigned long diff_ms; |
1324 ec_sync_signal_t *sync0 = &config->dc_sync[0]; |
1213 ec_sync_signal_t *sync0 = &config->dc_sync[0]; |
1325 u64 start_time; |
1214 u64 start_time; |
1326 |
1215 |
1327 if (!config) { // config removed in the meantime |
1216 if (!config) { // config removed in the meantime |
1328 ec_fsm_slave_config_reconfigure(fsm); |
1217 ec_fsm_slave_config_reconfigure(fsm); |
1332 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1221 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1333 return; |
1222 return; |
1334 |
1223 |
1335 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1224 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1336 fsm->state = ec_fsm_slave_config_state_error; |
1225 fsm->state = ec_fsm_slave_config_state_error; |
1337 EC_SLAVE_ERR(slave, "Failed to receive DC cycle times datagram: "); |
1226 EC_SLAVE_ERR(slave, "Failed to receive DC sync check datagram: "); |
1338 ec_datagram_print_state(datagram); |
1227 ec_datagram_print_state(datagram); |
1339 return; |
1228 return; |
1340 } |
1229 } |
1341 |
1230 |
1342 if (datagram->working_counter != 1) { |
1231 if (datagram->working_counter != 1) { |
1343 slave->error_flag = 1; |
1232 slave->error_flag = 1; |
1344 fsm->state = ec_fsm_slave_config_state_error; |
1233 fsm->state = ec_fsm_slave_config_state_error; |
1345 EC_SLAVE_ERR(slave, "Failed to set DC cycle times: "); |
1234 EC_SLAVE_ERR(slave, "Failed to check DC synchrony: "); |
1346 ec_datagram_print_wc_error(datagram); |
1235 ec_datagram_print_wc_error(datagram); |
1347 return; |
1236 return; |
1348 } |
1237 } |
1349 |
1238 |
|
1239 abs_sync_diff = EC_READ_U32(datagram->data) & 0x7fffffff; |
|
1240 diff_ms = (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
|
1241 |
|
1242 if (abs_sync_diff > EC_DC_MAX_SYNC_DIFF_NS) { |
|
1243 |
|
1244 if (diff_ms >= EC_DC_SYNC_WAIT_MS) { |
|
1245 EC_SLAVE_WARN(slave, "Slave did not sync after %u ms.\n", |
|
1246 (u32) diff_ms); |
|
1247 } else { |
|
1248 EC_SLAVE_DBG(slave, 1, "Sync after %4u ms: %10u ns\n", |
|
1249 (u32) diff_ms, abs_sync_diff); |
|
1250 |
|
1251 // check synchrony again |
|
1252 ec_datagram_fprd(datagram, slave->station_address, 0x092c, 4); |
|
1253 fsm->retries = EC_FSM_RETRIES; |
|
1254 return; |
|
1255 } |
|
1256 } else { |
|
1257 EC_SLAVE_DBG(slave, 1, "%u ns difference after %u ms.\n", |
|
1258 abs_sync_diff, (u32) diff_ms); |
|
1259 } |
|
1260 |
1350 // set DC start time |
1261 // set DC start time |
1351 start_time = master->app_time + 100000000ULL; // now + X ns |
1262 start_time = master->app_time + EC_DC_START_OFFSET; // now + X ns |
1352 // FIXME use slave's local system time here? |
1263 // FIXME use slave's local system time here? |
1353 |
1264 |
1354 if (sync0->cycle_time) { |
1265 if (sync0->cycle_time) { |
1355 // find correct phase |
1266 // find correct phase |
1356 if (master->has_start_time) { |
1267 if (master->has_app_time) { |
1357 u64 diff, start; |
1268 u64 diff, start; |
1358 u32 remainder; |
1269 u32 remainder; |
1359 |
1270 |
1360 diff = start_time - master->app_start_time; |
1271 diff = start_time - master->app_start_time; |
1361 remainder = do_div(diff, sync0->cycle_time); |
1272 remainder = do_div(diff, sync0->cycle_time); |
1362 |
1273 |
1363 start = start_time + |
1274 start = start_time + |
1364 sync0->cycle_time - remainder + sync0->shift_time; |
1275 sync0->cycle_time - remainder + sync0->shift_time; |
1365 |
1276 |
1366 if (master->debug_level) { |
1277 EC_SLAVE_DBG(slave, 1, "app_start_time=%llu\n", |
1367 EC_SLAVE_DBG(slave, 1, "app_start_time=%llu\n", |
1278 master->app_start_time); |
1368 master->app_start_time); |
1279 EC_SLAVE_DBG(slave, 1, " start_time=%llu\n", start_time); |
1369 EC_SLAVE_DBG(slave, 1, " start_time=%llu\n", |
1280 EC_SLAVE_DBG(slave, 1, " cycle_time=%u\n", sync0->cycle_time); |
1370 start_time); |
1281 EC_SLAVE_DBG(slave, 1, " shift_time=%u\n", sync0->shift_time); |
1371 EC_SLAVE_DBG(slave, 1, " cycle_time=%u\n", |
1282 EC_SLAVE_DBG(slave, 1, " remainder=%u\n", remainder); |
1372 sync0->cycle_time); |
1283 EC_SLAVE_DBG(slave, 1, " start=%llu\n", start); |
1373 EC_SLAVE_DBG(slave, 1, " shift_time=%u\n", |
|
1374 sync0->shift_time); |
|
1375 EC_SLAVE_DBG(slave, 1, " remainder=%u\n", |
|
1376 remainder); |
|
1377 EC_SLAVE_DBG(slave, 1, " start=%llu\n", |
|
1378 start); |
|
1379 } |
|
1380 start_time = start; |
1284 start_time = start; |
1381 } else { |
1285 } else { |
1382 EC_SLAVE_WARN(slave, "No application time supplied." |
1286 EC_SLAVE_WARN(slave, "No application time supplied." |
1383 " Cyclic start time will not be in phase.\n"); |
1287 " Cyclic start time will not be in phase.\n"); |
1384 } |
1288 } |