From: Graeme Foot Date: Fri Jan 20 12:03:00 2017 +1300 Added functions to queue and read the reference slave clocks 64bit time diff --git a/include/ecrt.h b/include/ecrt.h --- a/include/ecrt.h +++ b/include/ecrt.h @@ -1100,6 +1100,33 @@ uint32_t *time /**< Pointer to store the queried system time. */ ); +/** Queues the 64bit dc reference slave clock time value datagram for sending. + * + * The datagram read the 64bit dc timestamp of the DC reference slave. + * (register \a 0x0910:0x0917). The result can be checked with the + * ecrt_master_64bit_reference_clock_time() method. + */ +void ecrt_master_64bit_reference_clock_time_queue( + ec_master_t *master /**< EtherCAT master. */ + ); + +/** Get the 64bit dc reference slave clock time. + * + * ecrt_master_64bit_reference_clock_time_queue() must be called in the cycle + * prior to calling this method + * + * \attention The returned time is the system time of the reference clock + * minus the transmission delay of the reference clock. + * + * \retval 0 success, system time was written into \a time. + * \retval -ENXIO No reference clock found. + * \retval -EIO Slave synchronization datagram was not received. + */ +int ecrt_master_64bit_reference_clock_time( + ec_master_t *master, /**< EtherCAT master. */ + uint64_t *time /**< Pointer to store the queried time. */ + ); + /** Queues the DC synchrony monitoring datagram for sending. * * The datagram broadcast-reads all "System time difference" registers (\a diff --git a/lib/master.c b/lib/master.c --- a/lib/master.c +++ b/lib/master.c @@ -787,6 +787,46 @@ /****************************************************************************/ +static int lastErr64BitRefClkQueue = 0; +void ecrt_master_64bit_reference_clock_time_queue(ec_master_t *master) +{ + int ret; + + ret = ioctl(master->fd, EC_IOCTL_64_REF_CLK_TIME_QUEUE, NULL); + // we only report the first error of its kind, otherwise the errors + // will flood the logs + if ( (ret != lastErr64BitRefClkQueue) && EC_IOCTL_IS_ERROR(ret) ) { + EC_PRINT_ERR("Failed to queue 64bit ref clock time datagram: %s\n", + strerror(EC_IOCTL_ERRNO(ret))); + } + lastErr64BitRefClkQueue = ret; +} + +/****************************************************************************/ + +static int lastErr64BitRefClk = 0; +int ecrt_master_64bit_reference_clock_time(ec_master_t *master, + uint64_t *time) +{ + int ret; + + ret = ioctl(master->fd, EC_IOCTL_64_REF_CLK_TIME, time); + + // we use EAGAIN to inform the user that the ref clock is not ready yet. + // also we only report the first error of its kind, otherwise the errors + // will flood the logs + if ( (ret != lastErr64BitRefClk) && EC_IOCTL_IS_ERROR(ret) && + (EC_IOCTL_ERRNO(ret) != EAGAIN) ) { + EC_PRINT_ERR("Failed to get 64bit reference clock time: %s\n", + strerror(EC_IOCTL_ERRNO(ret))); + } + lastErr64BitRefClk = ret; + + return ret; +} + +/****************************************************************************/ + void ecrt_master_sync_monitor_queue(ec_master_t *master) { int ret; diff --git a/master/fsm_master.c b/master/fsm_master.c --- a/master/fsm_master.c +++ b/master/fsm_master.c @@ -865,7 +865,7 @@ // Attach slave configurations ec_master_attach_slave_configs(master); - // Set DC ref slave and clac topology and transmission delays + // Set DC ref slave and calc topology and transmission delays // Note: must come after attach_slave_configs for application // selected dc_ref_config to return its slave ec_master_calc_dc(master); diff --git a/master/ioctl.c b/master/ioctl.c --- a/master/ioctl.c +++ b/master/ioctl.c @@ -2115,6 +2115,57 @@ /*****************************************************************************/ +/** Queue the 64bit dc reference slave clock datagram. + * + * \return Zero on success, otherwise a negative error code. + */ +static ATTRIBUTES int ec_ioctl_64bit_ref_clock_time_queue( + ec_master_t *master, /**< EtherCAT master. */ + void *arg, /**< ioctl() argument. */ + ec_ioctl_context_t *ctx /**< Private data structure of file handle. */ + ) +{ + if (unlikely(!ctx->requested)) { + return -EPERM; + } + + ecrt_master_64bit_reference_clock_time_queue(master); + return 0; +} + +/*****************************************************************************/ + +/** Get the 64bit system time of the reference clock. + * + * \return Zero on success, otherwise a negative error code. + */ +static ATTRIBUTES int ec_ioctl_64bit_ref_clock_time( + ec_master_t *master, /**< EtherCAT master. */ + void *arg, /**< ioctl() argument. */ + ec_ioctl_context_t *ctx /**< Private data structure of file handle. */ + ) +{ + uint64_t time; + int ret; + + if (unlikely(!ctx->requested)) { + return -EPERM; + } + + ret = ecrt_master_64bit_reference_clock_time(master, &time); + if (ret) { + return ret; + } + + if (copy_to_user((void __user *) arg, &time, sizeof(time))) { + return -EFAULT; + } + + return 0; +} + +/*****************************************************************************/ + /** Queue the sync monitoring datagram. * * \return Zero on success, otherwise a negative error code. @@ -4441,6 +4492,20 @@ } ret = ec_ioctl_ref_clock_time(master, arg, ctx); break; + case EC_IOCTL_64_REF_CLK_TIME_QUEUE: + if (!ctx->writable) { + ret = -EPERM; + break; + } + ret = ec_ioctl_64bit_ref_clock_time_queue(master, arg, ctx); + break; + case EC_IOCTL_64_REF_CLK_TIME: + if (!ctx->writable) { + ret = -EPERM; + break; + } + ret = ec_ioctl_64bit_ref_clock_time(master, arg, ctx); + break; case EC_IOCTL_SYNC_MON_QUEUE: if (!ctx->writable) { ret = -EPERM; diff --git a/master/ioctl.h b/master/ioctl.h --- a/master/ioctl.h +++ b/master/ioctl.h @@ -109,6 +109,8 @@ #define EC_IOCTL_SYNC_REF EC_IO(0x29) #define EC_IOCTL_SYNC_SLAVES EC_IO(0x2a) #define EC_IOCTL_REF_CLOCK_TIME EC_IOR(0x2b, uint32_t) +#define EC_IOCTL_64_REF_CLK_TIME_QUEUE EC_IO(0x62) +#define EC_IOCTL_64_REF_CLK_TIME EC_IOR(0x63, uint64_t) #define EC_IOCTL_SYNC_MON_QUEUE EC_IO(0x2c) #define EC_IOCTL_SYNC_MON_PROCESS EC_IOR(0x2d, uint32_t) #define EC_IOCTL_RESET EC_IO(0x2e) diff --git a/master/master.c b/master/master.c --- a/master/master.c +++ b/master/master.c @@ -292,6 +292,17 @@ goto out_clear_ref_sync; } + // init sync64 datagram + ec_datagram_init(&master->sync64_datagram); + snprintf(master->sync64_datagram.name, EC_DATAGRAM_NAME_SIZE, "sync64"); + ret = ec_datagram_prealloc(&master->sync64_datagram, 8); + if (ret < 0) { + ec_datagram_clear(&master->sync_datagram); + EC_MASTER_ERR(master, "Failed to allocate 64bit ref slave" + " system clock datagram.\n"); + goto out_clear_sync; + } + // init sync monitor datagram ec_datagram_init(&master->sync_mon_datagram); snprintf(master->sync_mon_datagram.name, EC_DATAGRAM_NAME_SIZE, @@ -301,7 +312,7 @@ ec_datagram_clear(&master->sync_mon_datagram); EC_MASTER_ERR(master, "Failed to allocate sync" " monitoring datagram.\n"); - goto out_clear_sync; + goto out_clear_sync64; } master->dc_ref_config = NULL; @@ -357,6 +368,8 @@ ec_cdev_clear(&master->cdev); out_clear_sync_mon: ec_datagram_clear(&master->sync_mon_datagram); +out_clear_sync64: + ec_datagram_clear(&master->sync64_datagram); out_clear_sync: ec_datagram_clear(&master->sync_datagram); out_clear_ref_sync: @@ -404,6 +417,7 @@ ec_master_clear_slaves(master); ec_datagram_clear(&master->sync_mon_datagram); + ec_datagram_clear(&master->sync64_datagram); ec_datagram_clear(&master->sync_datagram); ec_datagram_clear(&master->ref_sync_datagram); @@ -2082,6 +2096,8 @@ ref ? ref->station_address : 0xffff, 0x0910, 4); ec_datagram_frmw(&master->sync_datagram, ref ? ref->station_address : 0xffff, 0x0910, 4); + ec_datagram_fprd(&master->sync64_datagram, + ref ? ref->station_address : 0xffff, 0x0910, 8); } /*****************************************************************************/ @@ -2853,6 +2869,39 @@ /*****************************************************************************/ +void ecrt_master_64bit_reference_clock_time_queue(ec_master_t *master) +{ + if (master->dc_ref_clock && master->dc_offset_valid) { + ec_datagram_zero(&master->sync64_datagram); + ec_master_queue_datagram(master, &master->sync64_datagram); + } +} + +/*****************************************************************************/ + +int ecrt_master_64bit_reference_clock_time(ec_master_t *master, uint64_t *time) +{ + if (!master->dc_ref_clock) { + return -ENXIO; + } + + if (master->sync64_datagram.state != EC_DATAGRAM_RECEIVED) { + return -EIO; + } + + if (!master->dc_offset_valid) { + return -EAGAIN; + } + + // Get returned datagram time, transmission delay removed. + *time = EC_READ_U64(master->sync64_datagram.data) - + master->dc_ref_clock->transmission_delay; + + return 0; +} + +/*****************************************************************************/ + void ecrt_master_sync_monitor_queue(ec_master_t *master) { ec_datagram_zero(&master->sync_mon_datagram); @@ -3319,6 +3368,8 @@ EXPORT_SYMBOL(ecrt_master_sync_reference_clock); EXPORT_SYMBOL(ecrt_master_sync_slave_clocks); EXPORT_SYMBOL(ecrt_master_reference_clock_time); +EXPORT_SYMBOL(ecrt_master_64bit_reference_clock_time_queue); +EXPORT_SYMBOL(ecrt_master_64bit_reference_clock_time); EXPORT_SYMBOL(ecrt_master_sync_monitor_queue); EXPORT_SYMBOL(ecrt_master_sync_monitor_process); EXPORT_SYMBOL(ecrt_master_sdo_download); diff --git a/master/master.h b/master/master.h --- a/master/master.h +++ b/master/master.h @@ -244,6 +244,8 @@ reference clock to the master clock. */ ec_datagram_t sync_datagram; /**< Datagram used for DC drift compensation. */ + ec_datagram_t sync64_datagram; /**< Datagram used to retrieve 64bit ref + slave system clock time. */ ec_datagram_t sync_mon_datagram; /**< Datagram used for DC synchronisation monitoring. */ ec_slave_config_t *dc_ref_config; /**< Application-selected DC reference