420 */ |
420 */ |
421 u64 ec_fsm_slave_config_dc_offset32( |
421 u64 ec_fsm_slave_config_dc_offset32( |
422 ec_fsm_slave_config_t *fsm, /**< slave state machine */ |
422 ec_fsm_slave_config_t *fsm, /**< slave state machine */ |
423 u64 system_time, /**< System time register. */ |
423 u64 system_time, /**< System time register. */ |
424 u64 old_offset, /**< Time offset register. */ |
424 u64 old_offset, /**< Time offset register. */ |
425 unsigned long jiffies_since_read /**< Jiffies for correction. */ |
425 u64 correction /**< Correction. */ |
426 ) |
426 ) |
427 { |
427 { |
428 ec_slave_t *slave = fsm->slave; |
428 ec_slave_t *slave = fsm->slave; |
429 u32 correction, system_time32, old_offset32, new_offset; |
429 u32 correction32, system_time32, old_offset32, new_offset; |
430 s32 time_diff; |
430 s32 time_diff; |
431 |
431 |
432 system_time32 = (u32) system_time; |
432 system_time32 = (u32) system_time; |
433 old_offset32 = (u32) old_offset; |
433 old_offset32 = (u32) old_offset; |
434 |
434 |
435 // correct read system time by elapsed time since read operation |
435 // correct read system time by elapsed time since read operation |
436 correction = jiffies_since_read * 1000 / HZ * 1000000; |
436 correction32 = (u32)correction; |
437 system_time32 += correction; |
437 system_time32 -= correction32; |
438 time_diff = (u32) slave->master->app_time - system_time32; |
438 time_diff = (u32) slave->master->app_time - system_time32; |
439 |
439 |
440 if (slave->master->debug_level) |
440 if (slave->master->debug_level) |
441 EC_DBG("Slave %u: system_time=%u (corrected with %u)," |
441 EC_DBG("Slave %u: system_time=%u (corrected with %u)," |
442 " app_time=%u, diff=%i\n", |
442 " app_time=%u, diff=%i\n", |
443 slave->ring_position, system_time32, correction, |
443 slave->ring_position, system_time32, correction32, |
444 (u32) slave->master->app_time, time_diff); |
444 (u32) slave->master->app_time, time_diff); |
445 |
445 |
446 if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) { |
446 if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) { |
447 new_offset = time_diff + old_offset32; |
447 new_offset = time_diff + old_offset32; |
448 if (slave->master->debug_level) |
448 if (slave->master->debug_level) |
463 */ |
463 */ |
464 u64 ec_fsm_slave_config_dc_offset64( |
464 u64 ec_fsm_slave_config_dc_offset64( |
465 ec_fsm_slave_config_t *fsm, /**< slave state machine */ |
465 ec_fsm_slave_config_t *fsm, /**< slave state machine */ |
466 u64 system_time, /**< System time register. */ |
466 u64 system_time, /**< System time register. */ |
467 u64 old_offset, /**< Time offset register. */ |
467 u64 old_offset, /**< Time offset register. */ |
468 unsigned long jiffies_since_read /**< Jiffies for correction. */ |
468 u64 correction /**< Correction. */ |
469 ) |
469 ) |
470 { |
470 { |
471 ec_slave_t *slave = fsm->slave; |
471 ec_slave_t *slave = fsm->slave; |
472 u64 new_offset, correction; |
472 u64 new_offset; |
473 s64 time_diff; |
473 s64 time_diff; |
474 |
474 |
475 // correct read system time by elapsed time since read operation |
475 system_time -= correction; |
476 correction = (u64) (jiffies_since_read * 1000 / HZ) * 1000000; |
|
477 system_time += correction; |
|
478 time_diff = fsm->slave->master->app_time - system_time; |
476 time_diff = fsm->slave->master->app_time - system_time; |
479 |
477 |
480 if (slave->master->debug_level) |
478 if (slave->master->debug_level) |
481 EC_DBG("Slave %u: system_time=%llu (corrected with %llu)," |
479 EC_DBG("Slave %u: system_time=%llu (corrected with %llu)," |
482 " app_time=%llu, diff=%lli\n", |
480 " app_time=%llu, diff=%lli\n", |
507 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
505 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
508 ) |
506 ) |
509 { |
507 { |
510 ec_datagram_t *datagram = fsm->datagram; |
508 ec_datagram_t *datagram = fsm->datagram; |
511 ec_slave_t *slave = fsm->slave; |
509 ec_slave_t *slave = fsm->slave; |
512 u64 system_time, old_offset, new_offset; |
510 u64 system_time, old_offset, new_offset, correction; |
513 unsigned long jiffies_since_read; |
|
514 |
|
515 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
511 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
516 return; |
512 return; |
517 |
513 |
518 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
514 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
519 fsm->state = ec_fsm_slave_config_state_error; |
515 fsm->state = ec_fsm_slave_config_state_error; |
532 return; |
528 return; |
533 } |
529 } |
534 |
530 |
535 system_time = EC_READ_U64(datagram->data); // 0x0910 |
531 system_time = EC_READ_U64(datagram->data); // 0x0910 |
536 old_offset = EC_READ_U64(datagram->data + 16); // 0x0920 |
532 old_offset = EC_READ_U64(datagram->data + 16); // 0x0920 |
537 jiffies_since_read = jiffies - datagram->jiffies_sent; |
533 /* correct read system time by elapsed time since read operation |
|
534 and the app_time set time */ |
|
535 #ifdef EC_HAVE_CYCLES |
|
536 correction = |
|
537 (datagram->cycles_sent - slave->master->dc_cycles_app_time) |
|
538 * 1000000LL; |
|
539 do_div(correction,cpu_khz); |
|
540 #else |
|
541 correction = |
|
542 (u64) ((datagram->jiffies_sent-slave->master->dc_jiffies_app_time) * 1000 / HZ) |
|
543 * 1000000; |
|
544 #endif |
538 |
545 |
539 if (slave->base_dc_range == EC_DC_32) { |
546 if (slave->base_dc_range == EC_DC_32) { |
540 new_offset = ec_fsm_slave_config_dc_offset32(fsm, |
547 new_offset = ec_fsm_slave_config_dc_offset32(fsm, |
541 system_time, old_offset, jiffies_since_read); |
548 system_time, old_offset, correction); |
542 } else { |
549 } else { |
543 new_offset = ec_fsm_slave_config_dc_offset64(fsm, |
550 new_offset = ec_fsm_slave_config_dc_offset64(fsm, |
544 system_time, old_offset, jiffies_since_read); |
551 system_time, old_offset, correction); |
545 } |
552 } |
546 |
553 |
547 // set DC system time offset and transmission delay |
554 // set DC system time offset and transmission delay |
548 ec_datagram_fpwr(datagram, slave->station_address, 0x0920, 12); |
555 ec_datagram_fpwr(datagram, slave->station_address, 0x0920, 12); |
549 EC_WRITE_U64(datagram->data, new_offset); |
556 EC_WRITE_U64(datagram->data, new_offset); |