46 

    47 /*****************************************************************************/

    48 

    49 /** Time difference [ns] to tolerate without setting a new system time offset.

    50  */

    51 #define EC_SYSTEM_TIME_TOLERANCE_NS 100000000

    63 void ec_fsm_master_state_dc_read_offset(ec_fsm_master_t *);

    64 void ec_fsm_master_state_dc_write_offset(ec_fsm_master_t *);

    70 void ec_fsm_master_enter_write_system_times(ec_fsm_master_t *);

   846 }

   847 

   848 /*****************************************************************************/

   849 

   850 /** Start writing DC system times.

   851  */

   852 void ec_fsm_master_enter_write_system_times(

   853         ec_fsm_master_t *fsm /**< Master state machine. */

   854         )

   855 {

   856     ec_master_t *master = fsm->master;

   857 

   858     EC_MASTER_DBG(master, 1, "Writing system time offsets...\n");

   859 

   860     if (master->has_app_time) {

   861         while (fsm->slave < master->slaves + master->slave_count) {

   862             if (!fsm->slave->base_dc_supported

   863                     || !fsm->slave->has_dc_system_time) {

   864                 fsm->slave++;

   865                 continue;

   866             }

   867 

   868             // read DC system time (0x0910, 64 bit)

   869             //                         gap (64 bit)

   870             //     and time offset (0x0920, 64 bit)

   871             ec_datagram_fprd(fsm->datagram, fsm->slave->station_address,

   872                     0x0910, 24);

   873             fsm->retries = EC_FSM_RETRIES;

   874             fsm->state = ec_fsm_master_state_dc_read_offset;

   875             return;

   876         }

   877     } else {

   878         EC_MASTER_DBG(master, 1, "No app_time received up to now.\n");

   879     }

   880 

   881     ec_master_request_op(master);

   882     ec_fsm_master_restart(fsm);

   883 }

   884 

   885 /*****************************************************************************/

   886 

   887 /** Configure 32 bit time offset.

   888  */

   889 u64 ec_fsm_master_dc_offset32(

   890         ec_fsm_master_t *fsm, /**< Master state machine. */

   891         u64 system_time, /**< System time register. */

   892         u64 old_offset, /**< Time offset register. */

   893         unsigned long jiffies_since_read /**< Jiffies for correction. */

   894         )

   895 {

   896     ec_slave_t *slave = fsm->slave;

   897     u32 correction, system_time32, old_offset32, new_offset;

   898     s32 time_diff;

   899 

   900     system_time32 = (u32) system_time;

   901     old_offset32 = (u32) old_offset;

   902 

   903     // correct read system time by elapsed time since read operation

   904     correction = jiffies_since_read * 1000 / HZ * 1000000;

   905     system_time32 += correction;

   906     time_diff = (u32) slave->master->app_time - system_time32;

   907 

   908     EC_SLAVE_DBG(slave, 1, "DC system time offset calculation:"

   909             " system_time=%u (corrected with %u),"

   910             " app_time=%u, diff=%i\n",

   911             system_time32, correction,

   912             (u32) slave->master->app_time, time_diff);

   913 

   914     if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) {

   915         new_offset = time_diff + old_offset32;

   916         EC_SLAVE_DBG(slave, 1, "Setting time offset to %u (was %u)\n",

   917                 new_offset, old_offset32);

   918         return (u64) new_offset;

   919     } else {

   920         EC_SLAVE_DBG(slave, 1, "Not touching time offset.\n");

   921         return old_offset;

   922     }

   923 }

   924 

   925 /*****************************************************************************/

   926 

   927 /** Configure 64 bit time offset.

   928  */

   929 u64 ec_fsm_master_dc_offset64(

   930         ec_fsm_master_t *fsm, /**< Master state machine. */

   931         u64 system_time, /**< System time register. */

   932         u64 old_offset, /**< Time offset register. */

   933         unsigned long jiffies_since_read /**< Jiffies for correction. */

   934         )

   935 {

   936     ec_slave_t *slave = fsm->slave;

   937     u64 new_offset, correction;

   938     s64 time_diff;

   939 

   940     // correct read system time by elapsed time since read operation

   941     correction = (u64) (jiffies_since_read * 1000 / HZ) * 1000000;

   942     system_time += correction;

   943     time_diff = fsm->slave->master->app_time - system_time;

   944 

   945     EC_SLAVE_DBG(slave, 1, "DC system time offset calculation:"

   946             " system_time=%llu (corrected with %llu),"

   947             " app_time=%llu, diff=%lli\n",

   948             system_time, correction,

   949             slave->master->app_time, time_diff);

   950 

   951     if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) {

   952         new_offset = time_diff + old_offset;

   953         EC_SLAVE_DBG(slave, 1, "Setting time offset to %llu (was %llu)\n",

   954                 new_offset, old_offset);

   955     } else {

   956         new_offset = old_offset;

   957         EC_SLAVE_DBG(slave, 1, "Not touching time offset.\n");

   958     }

   959 

   960     return new_offset;

   961 }

   962 

   963 /*****************************************************************************/

   964 

   965 /** Master state: DC READ OFFSET.

   966  */

   967 void ec_fsm_master_state_dc_read_offset(

   968         ec_fsm_master_t *fsm /**< Master state machine. */

   969         )

   970 {

   971     ec_datagram_t *datagram = fsm->datagram;

   972     ec_slave_t *slave = fsm->slave;

   973     u64 system_time, old_offset, new_offset;

   974     unsigned long jiffies_since_read;

   975 

   976     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)

   977         return;

   978 

   979     if (datagram->state != EC_DATAGRAM_RECEIVED) {

   980         EC_SLAVE_ERR(slave, "Failed to receive DC times datagram: ");

   981         ec_datagram_print_state(datagram);

   982         fsm->slave++;

   983         ec_fsm_master_enter_write_system_times(fsm);

   984         return;

   985     }

   986 

   987     if (datagram->working_counter != 1) {

   988         EC_SLAVE_WARN(slave, "Failed to get DC times: ");

   989         ec_datagram_print_wc_error(datagram);

   990         fsm->slave++;

   991         ec_fsm_master_enter_write_system_times(fsm);

   992         return;

   993     }

   994 

   995     system_time = EC_READ_U64(datagram->data);     // 0x0910

   996     old_offset = EC_READ_U64(datagram->data + 16); // 0x0920

   997     jiffies_since_read = jiffies - datagram->jiffies_sent;

   998 

   999     if (slave->base_dc_range == EC_DC_32) {

  1000         new_offset = ec_fsm_master_dc_offset32(fsm,

  1001                 system_time, old_offset, jiffies_since_read);

  1002     } else {

  1003         new_offset = ec_fsm_master_dc_offset64(fsm,

  1004                 system_time, old_offset, jiffies_since_read);

  1005     }

  1006 

  1007     // set DC system time offset and transmission delay

  1008     ec_datagram_fpwr(datagram, slave->station_address, 0x0920, 12);

  1009     EC_WRITE_U64(datagram->data, new_offset);

  1010     EC_WRITE_U32(datagram->data + 8, slave->transmission_delay);

  1011     fsm->retries = EC_FSM_RETRIES;

  1012     fsm->state = ec_fsm_master_state_dc_write_offset;

  1013 }

  1014 

  1015 /*****************************************************************************/

  1016 

  1017 /** Master state: DC WRITE OFFSET.

  1018  */

  1019 void ec_fsm_master_state_dc_write_offset(

  1020         ec_fsm_master_t *fsm /**< Master state machine. */

  1021         )

  1022 {

  1023     ec_datagram_t *datagram = fsm->datagram;

  1024     ec_slave_t *slave = fsm->slave;

  1025 

  1026     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)

  1027         return;

  1028 

  1029     if (datagram->state != EC_DATAGRAM_RECEIVED) {

  1030         EC_SLAVE_ERR(slave,

  1031                 "Failed to receive DC system time offset datagram: ");

  1032         ec_datagram_print_state(datagram);

  1033         fsm->slave++;

  1034         ec_fsm_master_enter_write_system_times(fsm);

  1035         return;

  1036     }

  1037 

  1038     if (datagram->working_counter != 1) {

  1039         EC_SLAVE_ERR(slave, "Failed to set DC system time offset: ");

  1040         ec_datagram_print_wc_error(datagram);

  1041         fsm->slave++;

  1042         ec_fsm_master_enter_write_system_times(fsm);

  1043         return;

  1044     }

  1045 

  1046     fsm->slave++;

  1047     ec_fsm_master_enter_write_system_times(fsm);