diff --git a/include/ecrt.h b/include/ecrt.h index 6f5c8ad2..21339bff 100644 --- a/include/ecrt.h +++ b/include/ecrt.h @@ -1125,6 +1125,34 @@ int ecrt_master_reference_clock_time( uint32_t *time /**< Pointer to store the queried system time. */ ); +/** Queues the 64 bit DC reference slave clock time value datagram for + * sending. + * + * The datagram read the 64 bit 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 64 bit 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/common.c b/lib/common.c index 40854f6b..88bcabe5 100644 --- a/lib/common.c +++ b/lib/common.c @@ -85,6 +85,8 @@ ec_master_t *ecrt_open_master(unsigned int master_index) master->process_data_size = 0; master->first_domain = NULL; master->first_config = NULL; + master->last_err_64bit_ref_clk_queue = 0; + master->last_err_64bit_ref_clk = 0; snprintf(path, MAX_PATH_LEN - 1, #ifdef USE_RTDM diff --git a/lib/master.c b/lib/master.c index b1b0abda..0b587a8d 100644 --- a/lib/master.c +++ b/lib/master.c @@ -813,6 +813,46 @@ int ecrt_master_reference_clock_time(ec_master_t *master, uint32_t *time) /****************************************************************************/ +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 != master->last_err_64bit_ref_clk_queue) + && EC_IOCTL_IS_ERROR(ret)) { + EC_PRINT_ERR("Failed to queue 64-bit ref clock time datagram: %s\n", + strerror(EC_IOCTL_ERRNO(ret))); + } + master->last_err_64bit_ref_clk_queue = ret; +} + +/****************************************************************************/ + +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 != master->last_err_64bit_ref_clk && + EC_IOCTL_IS_ERROR(ret) && + EC_IOCTL_ERRNO(ret) != EAGAIN) { + EC_PRINT_ERR("Failed to get 64-bit reference clock time: %s\n", + strerror(EC_IOCTL_ERRNO(ret))); + } + master->last_err_64bit_ref_clk = ret; + + return ret; +} + +/****************************************************************************/ + void ecrt_master_sync_monitor_queue(ec_master_t *master) { int ret; diff --git a/lib/master.h b/lib/master.h index fbd85110..9e8b9e34 100644 --- a/lib/master.h +++ b/lib/master.h @@ -39,6 +39,9 @@ struct ec_master { ec_domain_t *first_domain; ec_slave_config_t *first_config; + + int last_err_64bit_ref_clk_queue; + int last_err_64bit_ref_clk; }; /*****************************************************************************/ diff --git a/master/fsm_master.c b/master/fsm_master.c index 07703ced..c171828a 100644 --- a/master/fsm_master.c +++ b/master/fsm_master.c @@ -1168,7 +1168,7 @@ void ec_fsm_master_state_scan_slave( // 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 index b674efc6..cf823832 100755 --- a/master/ioctl.c +++ b/master/ioctl.c @@ -2306,6 +2306,57 @@ static ATTRIBUTES int ec_ioctl_ref_clock_time( /*****************************************************************************/ +/** 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. @@ -4749,6 +4800,20 @@ long EC_IOCTL( } 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 index 52f56068..3bb8d054 100644 --- a/master/ioctl.h +++ b/master/ioctl.h @@ -152,6 +152,8 @@ #define EC_IOCTL_SC_OVERLAPPING_IO EC_IOW(0x5d, ec_ioctl_config_t) #define EC_IOCTL_SETUP_DOMAIN_MEMORY EC_IOR(0x5e, ec_ioctl_master_activate_t) #define EC_IOCTL_DEACTIVATE_SLAVES EC_IO(0x5f) +#define EC_IOCTL_64_REF_CLK_TIME_QUEUE EC_IO(0x60) +#define EC_IOCTL_64_REF_CLK_TIME EC_IOR(0x61, uint64_t) /*****************************************************************************/ diff --git a/master/master.c b/master/master.c index 748f4368..30afdeed 100644 --- a/master/master.c +++ b/master/master.c @@ -304,6 +304,17 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */ 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, @@ -313,7 +324,7 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */ 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; @@ -369,6 +380,8 @@ out_clear_cdev: 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: @@ -416,6 +429,7 @@ void ec_master_clear( 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); @@ -2135,6 +2149,8 @@ void ec_master_find_dc_ref_clock( 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); } /*****************************************************************************/ @@ -2934,6 +2950,39 @@ void ecrt_master_sync_slave_clocks(ec_master_t *master) /*****************************************************************************/ +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); @@ -3401,6 +3450,8 @@ EXPORT_SYMBOL(ecrt_master_sync_reference_clock); EXPORT_SYMBOL(ecrt_master_sync_reference_clock_to); 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 index abb5b472..5f421e1a 100644 --- a/master/master.h +++ b/master/master.h @@ -242,6 +242,8 @@ struct ec_master { 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