398 EC_DBG("Failed to clear DC assignment of slave %u: ", |
404 EC_DBG("Failed to clear DC assignment of slave %u: ", |
399 fsm->slave->ring_position); |
405 fsm->slave->ring_position); |
400 ec_datagram_print_wc_error(datagram); |
406 ec_datagram_print_wc_error(datagram); |
401 } |
407 } |
402 |
408 |
403 // read DC system time and system time offset |
409 // read DC system time (0x0910, 64 bit) |
|
410 // gap (64 bit) |
|
411 // and time offset (0x0920, 64 bit) |
404 ec_datagram_fprd(fsm->datagram, fsm->slave->station_address, 0x0910, 24); |
412 ec_datagram_fprd(fsm->datagram, fsm->slave->station_address, 0x0910, 24); |
405 fsm->retries = EC_FSM_RETRIES; |
413 fsm->retries = EC_FSM_RETRIES; |
406 fsm->state = ec_fsm_slave_config_state_dc_read_offset; |
414 fsm->state = ec_fsm_slave_config_state_dc_read_offset; |
407 } |
415 } |
408 |
416 |
409 /*****************************************************************************/ |
417 /*****************************************************************************/ |
410 |
418 |
|
419 #define ABS(X) ((X) >= 0 ? (X) : -(X)) |
|
420 |
|
421 /** Configure 32 bit time offset. |
|
422 */ |
|
423 u64 ec_fsm_slave_config_dc_offset32( |
|
424 ec_fsm_slave_config_t *fsm, /**< slave state machine */ |
|
425 u64 system_time, /**< System time register. */ |
|
426 u64 old_offset, /**< Time offset register. */ |
|
427 unsigned long jiffies_since_read /**< Jiffies for correction. */ |
|
428 ) |
|
429 { |
|
430 ec_slave_t *slave = fsm->slave; |
|
431 u32 correction, system_time32, old_offset32, new_offset; |
|
432 s32 time_diff; |
|
433 |
|
434 system_time32 = (u32) system_time; |
|
435 old_offset32 = (u32) old_offset; |
|
436 |
|
437 // correct read system time by elapsed time since read operation |
|
438 correction = jiffies_since_read * 1000 / HZ * 1000000; |
|
439 system_time32 += correction; |
|
440 time_diff = (u32) slave->master->app_time - system_time32; |
|
441 |
|
442 if (slave->master->debug_level) |
|
443 EC_DBG("Slave %u: system_time=%u (corrected with %u)," |
|
444 " app_time=%u, diff=%i\n", |
|
445 slave->ring_position, system_time32, correction, |
|
446 (u32) slave->master->app_time, time_diff); |
|
447 |
|
448 if (ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) { |
|
449 new_offset = time_diff + old_offset32; |
|
450 if (slave->master->debug_level) |
|
451 EC_DBG("Slave %u: Setting time offset to %u (was %u)\n", |
|
452 slave->ring_position, new_offset, old_offset32); |
|
453 return (u64) new_offset; |
|
454 } else { |
|
455 if (slave->master->debug_level) |
|
456 EC_DBG("Slave %u: Not touching time offset.\n", |
|
457 slave->ring_position); |
|
458 return old_offset; |
|
459 } |
|
460 } |
|
461 |
|
462 /*****************************************************************************/ |
|
463 |
|
464 /** Configure 64 bit time offset. |
|
465 */ |
|
466 u64 ec_fsm_slave_config_dc_offset64( |
|
467 ec_fsm_slave_config_t *fsm, /**< slave state machine */ |
|
468 u64 system_time, /**< System time register. */ |
|
469 u64 old_offset, /**< Time offset register. */ |
|
470 unsigned long jiffies_since_read /**< Jiffies for correction. */ |
|
471 ) |
|
472 { |
|
473 ec_slave_t *slave = fsm->slave; |
|
474 u64 new_offset, correction; |
|
475 s64 time_diff; |
|
476 |
|
477 // correct read system time by elapsed time since read operation |
|
478 correction = (u64) (jiffies_since_read * 1000 / HZ) * 1000000; |
|
479 system_time += correction; |
|
480 time_diff = fsm->slave->master->app_time - system_time; |
|
481 |
|
482 if (slave->master->debug_level) |
|
483 EC_DBG("Slave %u: system_time=%llu (corrected with %llu)," |
|
484 " app_time=%llu, diff=%lli\n", |
|
485 slave->ring_position, system_time, correction, |
|
486 slave->master->app_time, time_diff); |
|
487 |
|
488 |
|
489 if (ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) { |
|
490 new_offset = time_diff + old_offset; |
|
491 if (slave->master->debug_level) |
|
492 EC_DBG("Slave %u: Setting time offset to %llu (was %llu)\n", |
|
493 slave->ring_position, new_offset, old_offset); |
|
494 } else { |
|
495 new_offset = old_offset; |
|
496 if (slave->master->debug_level) |
|
497 EC_DBG("Slave %u: Not touching time offset.\n", |
|
498 slave->ring_position); |
|
499 } |
|
500 |
|
501 return new_offset; |
|
502 } |
|
503 |
|
504 /*****************************************************************************/ |
|
505 |
411 /** Slave configuration state: DC READ OFFSET. |
506 /** Slave configuration state: DC READ OFFSET. |
412 */ |
507 */ |
413 void ec_fsm_slave_config_state_dc_read_offset( |
508 void ec_fsm_slave_config_state_dc_read_offset( |
414 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
509 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
415 ) |
510 ) |
416 { |
511 { |
417 ec_datagram_t *datagram = fsm->datagram; |
512 ec_datagram_t *datagram = fsm->datagram; |
418 ec_slave_t *slave = fsm->slave; |
513 ec_slave_t *slave = fsm->slave; |
419 u64 system_time, old_offset, new_offset; |
514 u64 system_time, old_offset, new_offset; |
|
515 unsigned long jiffies_since_read; |
420 |
516 |
421 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
517 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
422 return; |
518 return; |
423 |
519 |
424 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
520 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
436 slave->ring_position); |
532 slave->ring_position); |
437 ec_datagram_print_wc_error(datagram); |
533 ec_datagram_print_wc_error(datagram); |
438 return; |
534 return; |
439 } |
535 } |
440 |
536 |
441 system_time = EC_READ_U64(datagram->data); |
537 system_time = EC_READ_U64(datagram->data); // 0x0910 |
442 old_offset = EC_READ_U64(datagram->data + 16); |
538 old_offset = EC_READ_U64(datagram->data + 16); // 0x0920 |
443 new_offset = slave->master->app_time - system_time + old_offset; |
539 jiffies_since_read = jiffies - datagram->jiffies_sent; |
444 |
540 |
445 if (slave->master->debug_level) |
541 if (slave->base_dc_range == EC_DC_32) { |
446 EC_DBG("Slave %u: DC system_time=%llu old_offset=%llu, " |
542 new_offset = ec_fsm_slave_config_dc_offset32(fsm, |
447 "app_time=%llu, new_offset=%llu\n", |
543 system_time, old_offset, jiffies_since_read); |
448 slave->ring_position, system_time, old_offset, |
544 } else { |
449 slave->master->app_time, new_offset); |
545 new_offset = ec_fsm_slave_config_dc_offset64(fsm, |
|
546 system_time, old_offset, jiffies_since_read); |
|
547 } |
450 |
548 |
451 // set DC system time offset and transmission delay |
549 // set DC system time offset and transmission delay |
452 ec_datagram_fpwr(datagram, slave->station_address, 0x0920, 12); |
550 ec_datagram_fpwr(datagram, slave->station_address, 0x0920, 12); |
453 EC_WRITE_U64(datagram->data, new_offset); |
551 EC_WRITE_U64(datagram->data, new_offset); |
454 EC_WRITE_U32(datagram->data + 8, slave->transmission_delay); |
552 EC_WRITE_U32(datagram->data + 8, slave->transmission_delay); |