From b22a70977e12d601b16a47b5b73190ad75a5e933 Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Wed, 25 Aug 2021 11:10:35 +0200 Subject: [PATCH 01/25] Avoided warning. --- devices/igb/igb_main-4.19-ethercat.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/devices/igb/igb_main-4.19-ethercat.c b/devices/igb/igb_main-4.19-ethercat.c index a4169472..26919387 100644 --- a/devices/igb/igb_main-4.19-ethercat.c +++ b/devices/igb/igb_main-4.19-ethercat.c @@ -6059,6 +6059,7 @@ static int igb_tx_map(struct igb_ring *tx_ring, u32 tx_flags = first->tx_flags; u32 cmd_type = igb_tx_cmd_type(skb, tx_flags); u16 i = tx_ring->next_to_use; + struct igb_adapter *adapter = netdev_priv(tx_ring->netdev); tx_desc = IGB_TX_DESC(tx_ring, i); @@ -6186,7 +6187,6 @@ dma_error: DMA_TO_DEVICE); dma_unmap_len_set(tx_buffer, len, 0); - struct igb_adapter *adapter = netdev_priv(tx_ring->netdev); if (!adapter->ecdev) { dev_kfree_skb_any(tx_buffer->skb); tx_buffer->skb = NULL; From a5a30f7dbf88ec34a555da25946e1de48cb4e1e7 Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Wed, 25 Aug 2021 11:58:22 +0200 Subject: [PATCH 02/25] Avoided warning about unreasonable use of strncpy(). --- master/flag.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/master/flag.c b/master/flag.c index c0e23c31..53252999 100644 --- a/master/flag.c +++ b/master/flag.c @@ -47,7 +47,7 @@ int ec_flag_init( return -ENOMEM; } - strncpy(flag->key, key, strlen(key)); + strcpy(flag->key, key); flag->value = value; return 0; } From 7e71d03fda34e942f787ad5fcee7ed28c98aa532 Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Tue, 24 Aug 2021 16:04:31 +0000 Subject: [PATCH 03/25] Merge branch 'fix-make-clean' into 'stable-1.5' Fix make clean See merge request etherlab.org/ethercat!29 (cherry picked from commit dde91be4be1937ddf086920a1fa29744fe5dbdb3) 89f81d63 Fix make clean --- Makefile.kbuild | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/Makefile.kbuild b/Makefile.kbuild index a419b939..79515ed2 100644 --- a/Makefile.kbuild +++ b/Makefile.kbuild @@ -13,15 +13,14 @@ modules_install: $(KBUILD) modules_install -# Call Kbuild clean *before* automake `clean` (see #5 for the rationale) -# by adding `kbuild-clean` to the clean prerequisites. Having targets -# with the same name will result in merging the prerequisites, i.e.: -# ``` -# # Equivalent to 'clean: prerequisite1 prerequisite2' -# clean: prerequisite1 -# clean: prerequisite2 -# ``` +# Override default `clean` target to call Kbuild clean *before* automake +# `clean` (see #5 for the rationale). clean: kbuild-clean + @if test -z "$(SUBDIRS)"; then \ + $(MAKE) $(AM_MAKEFLAGS) clean-am ; \ + else \ + $(MAKE) $(AM_MAKEFLAGS) clean-recursive ; \ + fi kbuild-clean: $(KBUILD) clean From ffbb1eed9fea58a2c17acfa30c42235c022b4db8 Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Wed, 25 Aug 2021 09:45:31 +0200 Subject: [PATCH 04/25] Define set_normal_priority() inside #ifdef EC_EOE. Fixes #12. --- master/master.c | 30 ++++++++++++++++-------------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/master/master.c b/master/master.c index 252ae57a..862f3b3e 100644 --- a/master/master.c +++ b/master/master.c @@ -1443,20 +1443,6 @@ void ec_master_nanosleep(const unsigned long nsecs) /*****************************************************************************/ -/* compatibility for priority changes */ -static inline void set_normal_priority(struct task_struct *p, int nice) -{ -#if LINUX_VERSION_CODE >= KERNEL_VERSION(5, 9, 0) - sched_set_normal(p, nice); -#else - struct sched_param param = { .sched_priority = 0 }; - sched_setscheduler(p, SCHED_NORMAL, ¶m); - set_user_nice(p, nice); -#endif -} - -/*****************************************************************************/ - /** Execute slave FSMs. */ void ec_master_exec_slave_fsms( @@ -1667,6 +1653,21 @@ static int ec_master_operation_thread(void *priv_data) /*****************************************************************************/ #ifdef EC_EOE + +/* compatibility for priority changes */ +static inline void set_normal_priority(struct task_struct *p, int nice) +{ +#if LINUX_VERSION_CODE >= KERNEL_VERSION(5, 9, 0) + sched_set_normal(p, nice); +#else + struct sched_param param = { .sched_priority = 0 }; + sched_setscheduler(p, SCHED_NORMAL, ¶m); + set_user_nice(p, nice); +#endif +} + +/*****************************************************************************/ + /** Starts Ethernet over EtherCAT processing on demand. */ void ec_master_eoe_start(ec_master_t *master /**< EtherCAT master */) @@ -1778,6 +1779,7 @@ schedule: EC_MASTER_DBG(master, 1, "EoE thread exiting...\n"); return 0; } + #endif /*****************************************************************************/ From c9c43ac439bfd97410988090ffaf970a6bb42063 Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 23 Jun 2022 10:06:04 +0200 Subject: [PATCH 05/25] Added pre-commit configuration. --- .pre-commit-config.yaml | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 .pre-commit-config.yaml diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 00000000..77fcc5b0 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,14 @@ +# See https://pre-commit.com for more information +# See https://pre-commit.com/hooks.html for more hooks +repos: +- repo: https://github.com/pre-commit/pre-commit-hooks + rev: v3.2.0 + hooks: + - id: check-added-large-files + - id: check-byte-order-marker + - id: check-case-conflict + - id: check-merge-conflict + - id: end-of-file-fixer + - id: mixed-line-ending + - id: trailing-whitespace + - id: trailing-whitespace From b4f8c0e750d495be83a59ba95a9407ac291cad7a Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 23 Jun 2022 10:07:34 +0200 Subject: [PATCH 06/25] Added 0001-Distributed-Clock-fixes-and-helpers.patch --- include/ecrt.h | 27 ++++++ lib/master.c | 61 +++++++++++++- master/fsm_master.c | 9 +- master/ioctl.c | 198 ++++++++++++++++++++++++++++++++++++-------- master/ioctl.h | 2 + master/master.c | 96 +++++++++++++++++---- 6 files changed, 338 insertions(+), 55 deletions(-) diff --git a/include/ecrt.h b/include/ecrt.h index 91fc3a0b..6f5c8ad2 100644 --- a/include/ecrt.h +++ b/include/ecrt.h @@ -675,6 +675,20 @@ ec_domain_t *ecrt_master_create_domain( ec_master_t *master /**< EtherCAT master. */ ); +/** setup the domain's process data memory. + * + * Call this after all PDO entries have been registered and before activating + * the master. + * + * Call this if you need to access the domain memory before activating the + * master + * + * \return 0 on success, else non-zero. + */ +int ecrt_master_setup_domain_memory( + ec_master_t *master /**< EtherCAT master. */ + ); + /** Obtains a slave configuration. * * Creates a slave configuration object for the given \a alias and \a position @@ -933,6 +947,19 @@ int ecrt_master_activate( ec_master_t *master /**< EtherCAT master. */ ); +/** Deactivates the slaves distributed clocks and sends the slaves into PREOP. + * + * This can be called prior to ecrt_master_deactivate to avoid the slaves + * getting sync errors. + * + * This method should be called in realtime context. + * + * Note: EoE slaves will not be changed to PREOP. + */ +void ecrt_master_deactivate_slaves( + ec_master_t *master /**< EtherCAT master. */ + ); + /** Deactivates the master. * * Removes the bus configuration. All objects created by diff --git a/lib/master.c b/lib/master.c index 5275fcb9..d07bb4ac 100644 --- a/lib/master.c +++ b/lib/master.c @@ -540,6 +540,48 @@ int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position, /****************************************************************************/ +int ecrt_master_setup_domain_memory(ec_master_t *master) +{ + ec_ioctl_master_activate_t io; + int ret; + + ret = ioctl(master->fd, EC_IOCTL_SETUP_DOMAIN_MEMORY, &io); + if (EC_IOCTL_IS_ERROR(ret)) { + fprintf(stderr, "Failed to activate master: %s\n", + strerror(EC_IOCTL_ERRNO(ret))); + return -EC_IOCTL_ERRNO(ret); + } + + // will return 0 process_data_size if domain data has already been set up + if (io.process_data_size) { + master->process_data_size = io.process_data_size; + +#ifdef USE_RTDM + /* memory-mapping was already done in kernel. The user-space addess is + * provided in the ioctl data. + */ + master->process_data = io.process_data; +#else + master->process_data = mmap(0, master->process_data_size, + PROT_READ | PROT_WRITE, MAP_SHARED, master->fd, 0); + if (master->process_data == MAP_FAILED) { + fprintf(stderr, "Failed to map process data: %s\n", + strerror(errno)); + master->process_data = NULL; + master->process_data_size = 0; + return -errno; + } +#endif + + // Access the mapped region to cause the initial page fault + master->process_data[0] = 0x00; + } + + return 0; +} + +/*****************************************************************************/ + int ecrt_master_activate(ec_master_t *master) { ec_ioctl_master_activate_t io; @@ -552,9 +594,10 @@ int ecrt_master_activate(ec_master_t *master) return -EC_IOCTL_ERRNO(ret); } - master->process_data_size = io.process_data_size; + // will return 0 process_data_size if domain data has already been set up + if (io.process_data_size) { + master->process_data_size = io.process_data_size; - if (master->process_data_size) { #ifdef USE_RTDM /* memory-mapping was already done in kernel. The user-space addess is * provided in the ioctl data. @@ -581,6 +624,20 @@ int ecrt_master_activate(ec_master_t *master) /****************************************************************************/ +void ecrt_master_deactivate_slaves(ec_master_t *master) +{ + int ret; + + ret = ioctl(master->fd, EC_IOCTL_DEACTIVATE_SLAVES, NULL); + if (EC_IOCTL_IS_ERROR(ret)) { + fprintf(stderr, "Failed to deactivate slaves: %s\n", + strerror(EC_IOCTL_IS_ERROR(ret))); + return; + } +} + +/*****************************************************************************/ + void ecrt_master_deactivate(ec_master_t *master) { int ret; diff --git a/master/fsm_master.c b/master/fsm_master.c index c62d328f..3a90e335 100644 --- a/master/fsm_master.c +++ b/master/fsm_master.c @@ -48,7 +48,7 @@ /** Time difference [ns] to tolerate without setting a new system time offset. */ -#define EC_SYSTEM_TIME_TOLERANCE_NS 1000000 +#define EC_SYSTEM_TIME_TOLERANCE_NS 10000 /*****************************************************************************/ @@ -1163,11 +1163,14 @@ void ec_fsm_master_state_scan_slave( master->scan_busy = 0; wake_up_interruptible(&master->scan_queue); - ec_master_calc_dc(master); - // Attach slave configurations ec_master_attach_slave_configs(master); + // Set DC ref slave and clac 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); + #ifdef EC_EOE // check if EoE processing has to be started ec_master_eoe_start(master); diff --git a/master/ioctl.c b/master/ioctl.c index 01020687..e1f35a29 100755 --- a/master/ioctl.c +++ b/master/ioctl.c @@ -1834,6 +1834,92 @@ out_return: /*****************************************************************************/ +/** Sets up domain memory. + * + * \return Zero on success, otherwise a negative error code. + */ +static ATTRIBUTES int ec_ioctl_setup_domain_memory( + ec_master_t *master, /**< EtherCAT master. */ + void *arg, /**< ioctl() argument. */ + ec_ioctl_context_t *ctx /**< Private data structure of file handle. */ + ) +{ + ec_ioctl_master_activate_t io; + ec_domain_t *domain; + off_t offset; +#ifdef EC_IOCTL_RTDM + int ret; +#endif + + if (unlikely(!ctx->requested)) + return -EPERM; + + + if (!ctx->process_data) + { + io.process_data = NULL; + + /* Get the sum of the domains' process data sizes. */ + + ctx->process_data_size = 0; + + if (down_interruptible(&master->master_sem)) + return -EINTR; + + list_for_each_entry(domain, &master->domains, list) { + ctx->process_data_size += ecrt_domain_size(domain); + } + + up(&master->master_sem); + + if (ctx->process_data_size) { + ctx->process_data = vmalloc(ctx->process_data_size); + if (!ctx->process_data) { + ctx->process_data_size = 0; + return -ENOMEM; + } + + /* Set the memory as external process data memory for the + * domains. + */ + offset = 0; + list_for_each_entry(domain, &master->domains, list) { + ecrt_domain_external_memory(domain, + ctx->process_data + offset); + offset += ecrt_domain_size(domain); + } + +#ifdef EC_IOCTL_RTDM + /* RTDM uses a different approach for memory-mapping, which has to be + * initiated by the kernel. + */ + ret = ec_rtdm_mmap(ctx, &io.process_data); + if (ret < 0) { + EC_MASTER_ERR(master, "Failed to map process data" + " memory to user space (code %i).\n", ret); + return ret; + } +#endif + } + + io.process_data_size = ctx->process_data_size; + } + else + { + io.process_data = NULL; + io.process_data_size = 0; + } + + + if (copy_to_user((void __user *) arg, &io, + sizeof(ec_ioctl_master_activate_t))) + return -EFAULT; + + return 0; +} + +/*****************************************************************************/ + /** Activates the master. * * \return Zero on success, otherwise a negative error code. @@ -1852,52 +1938,61 @@ static ATTRIBUTES int ec_ioctl_activate( if (unlikely(!ctx->requested)) return -EPERM; - io.process_data = NULL; - /* Get the sum of the domains' process data sizes. */ + if (!ctx->process_data) + { + io.process_data = NULL; - ctx->process_data_size = 0; + /* Get the sum of the domains' process data sizes. */ - if (down_interruptible(&master->master_sem)) - return -EINTR; + ctx->process_data_size = 0; - list_for_each_entry(domain, &master->domains, list) { - ctx->process_data_size += ecrt_domain_size(domain); - } + if (down_interruptible(&master->master_sem)) + return -EINTR; - up(&master->master_sem); - - if (ctx->process_data_size) { - ctx->process_data = vmalloc(ctx->process_data_size); - if (!ctx->process_data) { - ctx->process_data_size = 0; - return -ENOMEM; - } - - /* Set the memory as external process data memory for the - * domains. - */ - offset = 0; list_for_each_entry(domain, &master->domains, list) { - ecrt_domain_external_memory(domain, - ctx->process_data + offset); - offset += ecrt_domain_size(domain); + ctx->process_data_size += ecrt_domain_size(domain); } + up(&master->master_sem); + + if (ctx->process_data_size) { + ctx->process_data = vmalloc(ctx->process_data_size); + if (!ctx->process_data) { + ctx->process_data_size = 0; + return -ENOMEM; + } + + /* Set the memory as external process data memory for the + * domains. + */ + offset = 0; + list_for_each_entry(domain, &master->domains, list) { + ecrt_domain_external_memory(domain, + ctx->process_data + offset); + offset += ecrt_domain_size(domain); + } + #ifdef EC_IOCTL_RTDM - /* RTDM uses a different approach for memory-mapping, which has to be - * initiated by the kernel. - */ - ret = ec_rtdm_mmap(ctx, &io.process_data); - if (ret < 0) { - EC_MASTER_ERR(master, "Failed to map process data" - " memory to user space (code %i).\n", ret); - return ret; - } + /* RTDM uses a different approach for memory-mapping, which has to be + * initiated by the kernel. + */ + ret = ec_rtdm_mmap(ctx, &io.process_data); + if (ret < 0) { + EC_MASTER_ERR(master, "Failed to map process data" + " memory to user space (code %i).\n", ret); + return ret; + } #endif - } + } - io.process_data_size = ctx->process_data_size; + io.process_data_size = ctx->process_data_size; + } + else + { + io.process_data = NULL; + io.process_data_size = 0; + } #ifndef EC_IOCTL_RTDM ecrt_master_callbacks(master, ec_master_internal_send_cb, @@ -1917,6 +2012,25 @@ static ATTRIBUTES int ec_ioctl_activate( /*****************************************************************************/ +/** Deactivates the slaves. + * + * \return Zero on success, otherwise a negative error code. + */ +static ATTRIBUTES int ec_ioctl_deactivate_slaves( + 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_deactivate_slaves(master); + return 0; +} + +/*****************************************************************************/ + /** Deactivates the master. * * \return Zero on success, otherwise a negative error code. @@ -4548,6 +4662,13 @@ long EC_IOCTL( } ret = ec_ioctl_select_ref_clock(master, arg, ctx); break; + case EC_IOCTL_SETUP_DOMAIN_MEMORY: + if (!ctx->writable) { + ret = -EPERM; + break; + } + ret = ec_ioctl_setup_domain_memory(master, arg, ctx); + break; case EC_IOCTL_ACTIVATE: if (!ctx->writable) { ret = -EPERM; @@ -4555,6 +4676,13 @@ long EC_IOCTL( } ret = ec_ioctl_activate(master, arg, ctx); break; + case EC_IOCTL_DEACTIVATE_SLAVES: + if (!ctx->writable) { + ret = -EPERM; + break; + } + ret = ec_ioctl_deactivate_slaves(master, arg, ctx); + break; case EC_IOCTL_DEACTIVATE: if (!ctx->writable) { ret = -EPERM; diff --git a/master/ioctl.h b/master/ioctl.h index 67ea95ff..727c2fb1 100644 --- a/master/ioctl.h +++ b/master/ioctl.h @@ -148,6 +148,8 @@ #define EC_IOCTL_VOE_DATA EC_IOWR(0x5b, ec_ioctl_voe_t) #define EC_IOCTL_SET_SEND_INTERVAL EC_IOW(0x5c, size_t) #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) /*****************************************************************************/ diff --git a/master/master.c b/master/master.c index 862f3b3e..9ed5241c 100644 --- a/master/master.c +++ b/master/master.c @@ -2079,25 +2079,29 @@ void ec_master_find_dc_ref_clock( ec_slave_t *slave, *ref = NULL; if (master->dc_ref_config) { - // Use application-selected reference clock + // Check application-selected reference clock slave = master->dc_ref_config->slave; if (slave) { if (slave->base_dc_supported && slave->has_dc_system_time) { ref = slave; + EC_MASTER_INFO(master, "Using slave %u as application selected" + " DC reference clock.\n", ref->ring_position); } else { - EC_MASTER_WARN(master, "Slave %u can not act as a" - " DC reference clock!", slave->ring_position); + EC_MASTER_WARN(master, "Application selected slave %u can not" + " act as a DC reference clock!", slave->ring_position); } } else { - EC_MASTER_WARN(master, "DC reference clock config (%u-%u)" - " has no slave attached!\n", master->dc_ref_config->alias, + EC_MASTER_WARN(master, "Application selected DC reference clock" + " config (%u-%u) has no slave attached!\n", + master->dc_ref_config->alias, master->dc_ref_config->position); } } - else { + + if (!ref) { // Use first slave with DC support as reference clock for (slave = master->slaves; slave < master->slaves + master->slave_count; @@ -2316,6 +2320,14 @@ ec_domain_t *ecrt_master_create_domain( /*****************************************************************************/ +int ecrt_master_setup_domain_memory(ec_master_t *master) +{ + // not currently supported + return -ENOMEM; // FIXME +} + +/*****************************************************************************/ + int ecrt_master_activate(ec_master_t *master) { uint32_t domain_offset; @@ -2390,6 +2402,54 @@ int ecrt_master_activate(ec_master_t *master) /*****************************************************************************/ +void ecrt_master_deactivate_slaves(ec_master_t *master) +{ + ec_slave_t *slave; + ec_slave_config_t *sc, *next; +#ifdef EC_EOE + ec_eoe_t *eoe; +#endif + + EC_MASTER_DBG(master, 1, "%s(master = 0x%p)\n", __func__, master); + + if (!master->active) { + EC_MASTER_WARN(master, "%s: Master not active.\n", __func__); + return; + } + + + // clear dc settings on all slaves + list_for_each_entry_safe(sc, next, &master->configs, list) { + if (sc->dc_assign_activate) { + ecrt_slave_config_dc(sc, 0x0000, 0, 0, 0, 0); + } + } + + + for (slave = master->slaves; + slave < master->slaves + master->slave_count; + slave++) { + + // set states for all slaves + ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP); + + // mark for reconfiguration, because the master could have no + // possibility for a reconfiguration between two sequential operation + // phases. + slave->force_config = 1; + } + +#ifdef EC_EOE + // ... but leave EoE slaves in OP + list_for_each_entry(eoe, &master->eoe_handlers, list) { + if (ec_eoe_is_open(eoe)) + ec_slave_request_state(eoe->slave, EC_SLAVE_STATE_OP); + } +#endif +} + +/*****************************************************************************/ + void ecrt_master_deactivate(ec_master_t *master) { ec_slave_t *slave; @@ -2650,18 +2710,22 @@ ec_slave_config_t *ecrt_master_slave_config(ec_master_t *master, int ecrt_master_select_reference_clock(ec_master_t *master, ec_slave_config_t *sc) { - if (sc) { - ec_slave_t *slave = sc->slave; + master->dc_ref_config = sc; - // output an early warning - if (slave && - (!slave->base_dc_supported || !slave->has_dc_system_time)) { - EC_MASTER_WARN(master, "Slave %u can not act as" - " a reference clock!", slave->ring_position); - } + if (master->dc_ref_config) { + EC_MASTER_INFO(master, "Application selected DC reference clock" + " config (%u-%u) set by application.\n", + master->dc_ref_config->alias, + master->dc_ref_config->position); + } + else { + EC_MASTER_INFO(master, "Application selected DC reference clock" + " config cleared by application.\n"); } - master->dc_ref_config = sc; + // update dc datagrams + ec_master_find_dc_ref_clock(master); + return 0; } @@ -3307,7 +3371,9 @@ void ecrt_master_reset(ec_master_t *master) /** \cond */ EXPORT_SYMBOL(ecrt_master_create_domain); +EXPORT_SYMBOL(ecrt_master_setup_domain_memory); EXPORT_SYMBOL(ecrt_master_activate); +EXPORT_SYMBOL(ecrt_master_deactivate_slaves); EXPORT_SYMBOL(ecrt_master_deactivate); EXPORT_SYMBOL(ecrt_master_send); EXPORT_SYMBOL(ecrt_master_send_ext); From 17eddce68ddaab4c81222abf46b5b8eabab84864 Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 23 Jun 2022 10:12:12 +0200 Subject: [PATCH 07/25] Applied 0002-Distributed-Clock-fixes-from-Jun-Yuan.patch --- lib/master.c | 2 +- master/datagram.c | 1 + master/datagram.h | 1 + master/fsm_master.c | 62 +++++++++++++++++++-------------------- master/fsm_slave_config.c | 4 ++- master/fsm_slave_scan.c | 1 + master/master.c | 31 +++++++++++++------- master/master.h | 1 + 8 files changed, 59 insertions(+), 44 deletions(-) diff --git a/lib/master.c b/lib/master.c index d07bb4ac..81d100db 100644 --- a/lib/master.c +++ b/lib/master.c @@ -800,7 +800,7 @@ int ecrt_master_reference_clock_time(ec_master_t *master, uint32_t *time) ret = ioctl(master->fd, EC_IOCTL_REF_CLOCK_TIME, time); if (EC_IOCTL_IS_ERROR(ret)) { ret = EC_IOCTL_ERRNO(ret); - if (ret != EIO && ret != ENXIO) { + if (ret != EIO && ret != ENXIO && ret != EAGAIN) { // do not log if no refclk or no refclk time yet fprintf(stderr, "Failed to get reference clock time: %s\n", strerror(ret)); diff --git a/master/datagram.c b/master/datagram.c index f6163274..da508509 100644 --- a/master/datagram.c +++ b/master/datagram.c @@ -102,6 +102,7 @@ void ec_datagram_init(ec_datagram_t *datagram /**< EtherCAT datagram. */) datagram->cycles_sent = 0; #endif datagram->jiffies_sent = 0; + datagram->app_time_sent = 0; #ifdef EC_HAVE_CYCLES datagram->cycles_received = 0; #endif diff --git a/master/datagram.h b/master/datagram.h index e902fbd4..96af32fa 100644 --- a/master/datagram.h +++ b/master/datagram.h @@ -102,6 +102,7 @@ typedef struct { cycles_t cycles_sent; /**< Time, when the datagram was sent. */ #endif unsigned long jiffies_sent; /**< Jiffies, when the datagram was sent. */ + uint64_t app_time_sent; /**< App time, when the datagram was sent. */ #ifdef EC_HAVE_CYCLES cycles_t cycles_received; /**< Time, when the datagram was received. */ #endif diff --git a/master/fsm_master.c b/master/fsm_master.c index 3a90e335..07703ced 100644 --- a/master/fsm_master.c +++ b/master/fsm_master.c @@ -48,7 +48,7 @@ /** Time difference [ns] to tolerate without setting a new system time offset. */ -#define EC_SYSTEM_TIME_TOLERANCE_NS 10000 +#define EC_SYSTEM_TIME_TOLERANCE_NS 1000 /*****************************************************************************/ @@ -417,6 +417,7 @@ void ec_fsm_master_state_broadcast( // application applied configurations if (master->config_changed) { master->config_changed = 0; + master->dc_offset_valid = 0; EC_MASTER_DBG(master, 1, "Configuration changed.\n"); @@ -805,6 +806,7 @@ void ec_fsm_master_action_configure( if (master->config_changed) { master->config_changed = 0; + master->dc_offset_valid = 0; // abort iterating through slaves, // first compensate DC system time offsets, @@ -1178,6 +1180,7 @@ void ec_fsm_master_state_scan_slave( if (master->slave_count) { master->config_changed = 0; + master->dc_offset_valid = 0; fsm->slave = master->slaves; // begin with first slave ec_fsm_master_enter_write_system_times(fsm); @@ -1233,7 +1236,7 @@ void ec_fsm_master_enter_write_system_times( { ec_master_t *master = fsm->master; - if (master->dc_ref_time) { + if (master->active) { while (fsm->slave < master->slaves + master->slave_count) { if (!fsm->slave->base_dc_supported @@ -1249,19 +1252,16 @@ void ec_fsm_master_enter_write_system_times( // and time offset (0x0920, 64 bit) ec_datagram_fprd(fsm->datagram, fsm->slave->station_address, 0x0910, 24); + ec_datagram_zero(fsm->datagram); fsm->datagram->device_index = fsm->slave->device_index; fsm->retries = EC_FSM_RETRIES; fsm->state = ec_fsm_master_state_dc_read_offset; return; } + master->dc_offset_valid = 1; } else { - if (master->active) { - EC_MASTER_WARN(master, "No application time received up to now," - " but master already active.\n"); - } else { - EC_MASTER_DBG(master, 1, "No app_time received up to now.\n"); - } + EC_MASTER_DBG(master, 1, "No app_time received up to now.\n"); } // scanning and setting system times complete @@ -1279,26 +1279,21 @@ u64 ec_fsm_master_dc_offset32( ec_fsm_master_t *fsm, /**< Master state machine. */ u64 system_time, /**< System time register. */ u64 old_offset, /**< Time offset register. */ - unsigned long jiffies_since_read /**< Jiffies for correction. */ + u64 app_time_sent /**< Master app time by reading. */ ) { ec_slave_t *slave = fsm->slave; - u32 correction, system_time32, old_offset32, new_offset; + u32 system_time32, old_offset32, new_offset; s32 time_diff; system_time32 = (u32) system_time; old_offset32 = (u32) old_offset; - // correct read system time by elapsed time since read operation - correction = jiffies_since_read * 1000 / HZ * 1000000; - system_time32 += correction; - time_diff = (u32) slave->master->app_time - system_time32; + time_diff = (u32) app_time_sent - system_time32; EC_SLAVE_DBG(slave, 1, "DC 32 bit system time offset calculation:" - " system_time=%u (corrected with %u)," - " app_time=%llu, diff=%i\n", - system_time32, correction, - slave->master->app_time, time_diff); + " system_time=%u, app_time=%llu, diff=%i\n", + system_time32, app_time_sent, time_diff); if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) { new_offset = time_diff + old_offset32; @@ -1321,23 +1316,18 @@ u64 ec_fsm_master_dc_offset64( ec_fsm_master_t *fsm, /**< Master state machine. */ u64 system_time, /**< System time register. */ u64 old_offset, /**< Time offset register. */ - unsigned long jiffies_since_read /**< Jiffies for correction. */ + u64 app_time_sent /**< Master app time by reading. */ ) { ec_slave_t *slave = fsm->slave; - u64 new_offset, correction; + u64 new_offset; s64 time_diff; - // correct read system time by elapsed time since read operation - correction = (u64) (jiffies_since_read * 1000 / HZ) * 1000000; - system_time += correction; - time_diff = fsm->slave->master->app_time - system_time; + time_diff = app_time_sent - system_time; EC_SLAVE_DBG(slave, 1, "DC 64 bit system time offset calculation:" - " system_time=%llu (corrected with %llu)," - " app_time=%llu, diff=%lli\n", - system_time, correction, - slave->master->app_time, time_diff); + " system_time=%llu, app_time=%llu, diff=%lli\n", + system_time, app_time_sent, time_diff); if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) { new_offset = time_diff + old_offset; @@ -1361,8 +1351,8 @@ void ec_fsm_master_state_dc_read_offset( { ec_datagram_t *datagram = fsm->datagram; ec_slave_t *slave = fsm->slave; + ec_master_t *master = fsm->master; u64 system_time, old_offset, new_offset; - unsigned long jiffies_since_read; if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) return; @@ -1383,16 +1373,24 @@ void ec_fsm_master_state_dc_read_offset( return; } + if (unlikely(!master->dc_ref_time)) { + EC_MASTER_WARN(master, "No app_time received up to now," + " abort DC time offset calculation.\n"); + // scanning and setting system times complete + ec_master_request_op(master); + ec_fsm_master_restart(fsm); + return; + } + system_time = EC_READ_U64(datagram->data); // 0x0910 old_offset = EC_READ_U64(datagram->data + 16); // 0x0920 - jiffies_since_read = jiffies - datagram->jiffies_sent; if (slave->base_dc_range == EC_DC_32) { new_offset = ec_fsm_master_dc_offset32(fsm, - system_time, old_offset, jiffies_since_read); + system_time, old_offset, datagram->app_time_sent); } else { new_offset = ec_fsm_master_dc_offset64(fsm, - system_time, old_offset, jiffies_since_read); + system_time, old_offset, datagram->app_time_sent); } // set DC system time offset and transmission delay diff --git a/master/fsm_slave_config.c b/master/fsm_slave_config.c index 49d916f4..865daba4 100755 --- a/master/fsm_slave_config.c +++ b/master/fsm_slave_config.c @@ -1372,6 +1372,7 @@ void ec_fsm_slave_config_state_dc_cycle( fsm->jiffies_start = jiffies; ec_datagram_fprd(datagram, slave->station_address, 0x092c, 4); + ec_datagram_zero(datagram); fsm->retries = EC_FSM_RETRIES; fsm->state = ec_fsm_slave_config_state_dc_sync_check; } @@ -1430,11 +1431,12 @@ void ec_fsm_slave_config_state_dc_sync_check( if ((diff_ms < last_diff_ms) || (diff_ms >= (last_diff_ms + 100))) { last_diff_ms = diff_ms; EC_SLAVE_DBG(slave, 1, "Sync after %4lu ms: %10u ns\n", - diff_ms, abs_sync_diff); + diff_ms, EC_READ_U32(datagram->data) & 0x80000000 ? -abs_sync_diff: abs_sync_diff); } // check synchrony again ec_datagram_fprd(datagram, slave->station_address, 0x092c, 4); + ec_datagram_zero(datagram); fsm->retries = EC_FSM_RETRIES; return; } diff --git a/master/fsm_slave_scan.c b/master/fsm_slave_scan.c index 708fe891..e24fefe2 100644 --- a/master/fsm_slave_scan.c +++ b/master/fsm_slave_scan.c @@ -957,6 +957,7 @@ void ec_fsm_slave_scan_enter_preop( * sizes. */ ec_datagram_fprd(fsm->datagram, slave->station_address, 0x0800, EC_SYNC_PAGE_SIZE * 2); + ec_datagram_zero(fsm->datagram); fsm->retries = EC_FSM_RETRIES; fsm->state = ec_fsm_slave_scan_state_sync; } diff --git a/master/master.c b/master/master.c index 9ed5241c..748f4368 100644 --- a/master/master.c +++ b/master/master.c @@ -188,6 +188,7 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */ master->app_time = 0ULL; master->dc_ref_time = 0ULL; + master->dc_offset_valid = 0; master->scan_busy = 0; master->allow_scan = 1; @@ -1092,6 +1093,7 @@ size_t ec_master_send_datagrams( datagram->cycles_sent = cycles_sent; #endif datagram->jiffies_sent = jiffies_sent; + datagram->app_time_sent = master->app_time; list_del_init(&datagram->sent); // empty list of sent datagrams } @@ -1237,14 +1239,17 @@ void ec_master_receive_datagrams( datagram->working_counter = EC_READ_U16(cur_data); cur_data += EC_DATAGRAM_FOOTER_SIZE; - // dequeue the received datagram - datagram->state = EC_DATAGRAM_RECEIVED; #ifdef EC_HAVE_CYCLES datagram->cycles_received = master->devices[EC_DEVICE_MAIN].cycles_poll; #endif datagram->jiffies_received = master->devices[EC_DEVICE_MAIN].jiffies_poll; + + barrier(); /* reordering might lead to races */ + + // dequeue the received datagram + datagram->state = EC_DATAGRAM_RECEIVED; list_del_init(&datagram->queue); } } @@ -2396,6 +2401,7 @@ int ecrt_master_activate(ec_master_t *master) // notify state machine, that the configuration shall now be applied master->config_changed = 1; + master->dc_offset_valid = 0; return 0; } @@ -2500,6 +2506,13 @@ void ecrt_master_deactivate(ec_master_t *master) master->app_time = 0ULL; master->dc_ref_time = 0ULL; + master->dc_offset_valid = 0; + + /* Disallow scanning to get into the same state like after a master + * request (after ec_master_enter_operation_phase() is called). */ + master->allow_scan = 0; + + master->active = 0; #ifdef EC_EOE if (eoe_was_running) { @@ -2510,12 +2523,6 @@ void ecrt_master_deactivate(ec_master_t *master) "EtherCAT-IDLE")) { EC_MASTER_WARN(master, "Failed to restart master thread!\n"); } - - /* Disallow scanning to get into the same state like after a master - * request (after ec_master_enter_operation_phase() is called). */ - master->allow_scan = 0; - - master->active = 0; } /*****************************************************************************/ @@ -2881,6 +2888,10 @@ int ecrt_master_reference_clock_time(ec_master_t *master, uint32_t *time) return -EIO; } + if (!master->dc_offset_valid) { + return -EAGAIN; + } + // Get returned datagram time, transmission delay removed. *time = EC_READ_U32(master->sync_datagram.data) - master->dc_ref_clock->transmission_delay; @@ -2892,7 +2903,7 @@ int ecrt_master_reference_clock_time(ec_master_t *master, uint32_t *time) void ecrt_master_sync_reference_clock(ec_master_t *master) { - if (master->dc_ref_clock) { + if (master->dc_ref_clock && master->dc_offset_valid) { EC_WRITE_U32(master->ref_sync_datagram.data, master->app_time); ec_master_queue_datagram(master, &master->ref_sync_datagram); } @@ -2915,7 +2926,7 @@ void ecrt_master_sync_reference_clock_to( void ecrt_master_sync_slave_clocks(ec_master_t *master) { - if (master->dc_ref_clock) { + if (master->dc_ref_clock && master->dc_offset_valid) { ec_datagram_zero(&master->sync_datagram); ec_master_queue_datagram(master, &master->sync_datagram); } diff --git a/master/master.h b/master/master.h index ca83d01f..abb5b472 100644 --- a/master/master.h +++ b/master/master.h @@ -237,6 +237,7 @@ struct ec_master { u64 app_time; /**< Time of the last ecrt_master_sync() call. */ u64 dc_ref_time; /**< Common reference timestamp for DC start times. */ + u8 dc_offset_valid; /**< DC slaves have valid system time offsets*/ ec_datagram_t ref_sync_datagram; /**< Datagram used for synchronizing the reference clock to the master clock. */ ec_datagram_t sync_datagram; /**< Datagram used for DC drift From d0d4535df00a1439c25534ae5b4003dfa14df48b Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 23 Jun 2022 10:13:00 +0200 Subject: [PATCH 08/25] Applied 0003-print-sync-signed.patch --- master/fsm_slave_config.c | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/master/fsm_slave_config.c b/master/fsm_slave_config.c index 865daba4..80956432 100755 --- a/master/fsm_slave_config.c +++ b/master/fsm_slave_config.c @@ -1389,6 +1389,7 @@ void ec_fsm_slave_config_state_dc_sync_check( ec_slave_t *slave = fsm->slave; ec_master_t *master = slave->master; ec_slave_config_t *config = slave->config; + bool negative; uint32_t abs_sync_diff; unsigned long diff_ms; ec_sync_signal_t *sync0 = &config->dc_sync[0]; @@ -1419,6 +1420,7 @@ void ec_fsm_slave_config_state_dc_sync_check( } abs_sync_diff = EC_READ_U32(datagram->data) & 0x7fffffff; + negative = (EC_READ_U32(datagram->data) & 0x80000000) != 0; diff_ms = (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; if (abs_sync_diff > EC_DC_MAX_SYNC_DIFF_NS) { @@ -1430,8 +1432,8 @@ void ec_fsm_slave_config_state_dc_sync_check( static unsigned long last_diff_ms = 0; if ((diff_ms < last_diff_ms) || (diff_ms >= (last_diff_ms + 100))) { last_diff_ms = diff_ms; - EC_SLAVE_DBG(slave, 1, "Sync after %4lu ms: %10u ns\n", - diff_ms, EC_READ_U32(datagram->data) & 0x80000000 ? -abs_sync_diff: abs_sync_diff); + EC_SLAVE_DBG(slave, 1, "Sync after %4lu ms: %10d ns\n", + diff_ms, negative ? -abs_sync_diff: abs_sync_diff); } // check synchrony again @@ -1441,8 +1443,8 @@ void ec_fsm_slave_config_state_dc_sync_check( return; } } else { - EC_SLAVE_DBG(slave, 1, "%u ns difference after %lu ms.\n", - abs_sync_diff, diff_ms); + EC_SLAVE_DBG(slave, 1, "%d ns difference after %lu ms.\n", + negative ? -abs_sync_diff: abs_sync_diff, diff_ms); } // set DC start time (roughly in the future, not in-phase) From efcd95ed9ceecc119dbed0ef25ec5378df7211bc Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 23 Jun 2022 10:13:49 +0200 Subject: [PATCH 09/25] Applied 0004-fix-eoe-clear.patch --- master/fsm_slave.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/master/fsm_slave.c b/master/fsm_slave.c index fc21574f..e959beb1 100644 --- a/master/fsm_slave.c +++ b/master/fsm_slave.c @@ -113,7 +113,7 @@ void ec_fsm_slave_clear( } if (fsm->eoe_request) { - fsm->soe_request->state = EC_INT_REQUEST_FAILURE; + fsm->eoe_request->state = EC_INT_REQUEST_FAILURE; wake_up_all(&fsm->slave->master->request_queue); } From 405c88f6d6fe3a6f07f19d2f99a3b944c71904f7 Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 23 Jun 2022 10:25:36 +0200 Subject: [PATCH 10/25] Applied 0005-disable-eoe.patch --- master/Kbuild.in | 4 ++-- master/fsm_slave.c | 15 ++++++++++++++- master/fsm_slave.h | 8 +++++++- master/ioctl.c | 4 ++++ master/ioctl.h | 4 ++++ tool/CommandIp.h | 1 + tool/Makefile.am | 21 +++++++++++---------- tool/MasterDevice.cpp | 2 ++ tool/MasterDevice.h | 6 +++--- tool/main.cpp | 6 +++++- 10 files changed, 53 insertions(+), 18 deletions(-) diff --git a/master/Kbuild.in b/master/Kbuild.in index 14f6e300..f2bb094c 100644 --- a/master/Kbuild.in +++ b/master/Kbuild.in @@ -40,13 +40,11 @@ ec_master-objs := \ datagram_pair.o \ device.o \ domain.o \ - eoe_request.o \ flag.o \ fmmu_config.o \ foe_request.o \ fsm_change.o \ fsm_coe.o \ - fsm_eoe.o \ fsm_foe.o \ fsm_master.o \ fsm_pdo.o \ @@ -76,6 +74,8 @@ ec_master-objs := \ voe_handler.o ifeq (@ENABLE_EOE@,1) +ec_master-objs += eoe_request.o +ec_master-objs += fsm_eoe.o ec_master-objs += ethernet.o endif diff --git a/master/fsm_slave.c b/master/fsm_slave.c index e959beb1..9f85d487 100644 --- a/master/fsm_slave.c +++ b/master/fsm_slave.c @@ -52,8 +52,10 @@ int ec_fsm_slave_action_process_foe(ec_fsm_slave_t *, ec_datagram_t *); void ec_fsm_slave_state_foe_request(ec_fsm_slave_t *, ec_datagram_t *); int ec_fsm_slave_action_process_soe(ec_fsm_slave_t *, ec_datagram_t *); void ec_fsm_slave_state_soe_request(ec_fsm_slave_t *, ec_datagram_t *); +#ifdef EC_EOE int ec_fsm_slave_action_process_eoe(ec_fsm_slave_t *, ec_datagram_t *); void ec_fsm_slave_state_eoe_request(ec_fsm_slave_t *, ec_datagram_t *); +#endif /*****************************************************************************/ @@ -73,13 +75,17 @@ void ec_fsm_slave_init( fsm->reg_request = NULL; fsm->foe_request = NULL; fsm->soe_request = NULL; +#ifdef EC_EOE fsm->eoe_request = NULL; +#endif // Init sub-state-machines ec_fsm_coe_init(&fsm->fsm_coe); ec_fsm_foe_init(&fsm->fsm_foe); ec_fsm_soe_init(&fsm->fsm_soe); +#ifdef EC_EOE ec_fsm_eoe_init(&fsm->fsm_eoe); +#endif } /*****************************************************************************/ @@ -112,16 +118,20 @@ void ec_fsm_slave_clear( wake_up_all(&fsm->slave->master->request_queue); } +#ifdef EC_EOE if (fsm->eoe_request) { fsm->eoe_request->state = EC_INT_REQUEST_FAILURE; wake_up_all(&fsm->slave->master->request_queue); } +#endif // clear sub-state machines ec_fsm_coe_clear(&fsm->fsm_coe); ec_fsm_foe_clear(&fsm->fsm_foe); ec_fsm_soe_clear(&fsm->fsm_soe); +#ifdef EC_EOE ec_fsm_eoe_clear(&fsm->fsm_eoe); +#endif } /*****************************************************************************/ @@ -233,10 +243,12 @@ void ec_fsm_slave_state_ready( return; } +#ifdef EC_EOE // Check for pending EoE IP parameter requests if (ec_fsm_slave_action_process_eoe(fsm, datagram)) { return; } +#endif } /*****************************************************************************/ @@ -607,7 +619,7 @@ void ec_fsm_slave_state_soe_request( } /*****************************************************************************/ - +#ifdef EC_EOE /** Check for pending EoE IP parameter requests and process one. * * \return non-zero, if a request is processed. @@ -687,5 +699,6 @@ void ec_fsm_slave_state_eoe_request( fsm->eoe_request = NULL; fsm->state = ec_fsm_slave_state_ready; } +#endif /*****************************************************************************/ diff --git a/master/fsm_slave.h b/master/fsm_slave.h index f051d5bb..acc9ee2e 100644 --- a/master/fsm_slave.h +++ b/master/fsm_slave.h @@ -45,7 +45,9 @@ #include "fsm_coe.h" #include "fsm_foe.h" #include "fsm_soe.h" +#ifdef EC_EOE #include "fsm_eoe.h" +#endif /*****************************************************************************/ @@ -64,12 +66,16 @@ struct ec_fsm_slave { ec_foe_request_t *foe_request; /**< FoE request to process. */ off_t foe_index; /**< Index to FoE write request data. */ ec_soe_request_t *soe_request; /**< SoE request to process. */ - ec_eoe_request_t *eoe_request; /**< SoE request to process. */ +#ifdef EC_EOE + ec_eoe_request_t *eoe_request; /**< EoE request to process. */ +#endif ec_fsm_coe_t fsm_coe; /**< CoE state machine. */ ec_fsm_foe_t fsm_foe; /**< FoE state machine. */ ec_fsm_soe_t fsm_soe; /**< SoE state machine. */ +#ifdef EC_EOE ec_fsm_eoe_t fsm_eoe; /**< EoE state machine. */ +#endif }; /*****************************************************************************/ diff --git a/master/ioctl.c b/master/ioctl.c index e1f35a29..b674efc6 100755 --- a/master/ioctl.c +++ b/master/ioctl.c @@ -1615,6 +1615,7 @@ static ATTRIBUTES int ec_ioctl_eoe_handler( /*****************************************************************************/ +#ifdef EC_EOE /** Request EoE IP parameter setting. * * \return Zero on success, otherwise a negative error code. @@ -1695,6 +1696,7 @@ static ATTRIBUTES int ec_ioctl_slave_eoe_ip_param( return req.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO; } +#endif /*****************************************************************************/ @@ -4597,6 +4599,7 @@ long EC_IOCTL( case EC_IOCTL_SLAVE_SOE_READ: ret = ec_ioctl_slave_soe_read(master, arg); break; +#ifdef EC_EOE case EC_IOCTL_SLAVE_EOE_IP_PARAM: if (!ctx->writable) { ret = -EPERM; @@ -4604,6 +4607,7 @@ long EC_IOCTL( } ret = ec_ioctl_slave_eoe_ip_param(master, arg); break; +#endif case EC_IOCTL_SLAVE_SOE_WRITE: if (!ctx->writable) { ret = -EPERM; diff --git a/master/ioctl.h b/master/ioctl.h index 727c2fb1..52f56068 100644 --- a/master/ioctl.h +++ b/master/ioctl.h @@ -74,7 +74,9 @@ #define EC_IOCTL_SLAVE_FOE_WRITE EC_IOW(0x15, ec_ioctl_slave_foe_t) #define EC_IOCTL_SLAVE_SOE_READ EC_IOWR(0x16, ec_ioctl_slave_soe_read_t) #define EC_IOCTL_SLAVE_SOE_WRITE EC_IOWR(0x17, ec_ioctl_slave_soe_write_t) +#ifdef EC_EOE #define EC_IOCTL_SLAVE_EOE_IP_PARAM EC_IOW(0x18, ec_ioctl_slave_eoe_ip_t) +#endif #define EC_IOCTL_CONFIG EC_IOWR(0x19, ec_ioctl_config_t) #define EC_IOCTL_CONFIG_PDO EC_IOWR(0x1a, ec_ioctl_config_pdo_t) #define EC_IOCTL_CONFIG_PDO_ENTRY EC_IOWR(0x1b, ec_ioctl_config_pdo_entry_t) @@ -614,6 +616,7 @@ typedef struct { #endif #endif +#ifdef EC_EOE typedef struct { // input uint16_t slave_position; @@ -636,6 +639,7 @@ typedef struct { uint16_t result; } ec_ioctl_slave_eoe_ip_t; +#endif /*****************************************************************************/ typedef struct { diff --git a/tool/CommandIp.h b/tool/CommandIp.h index 204bd5e0..f5665bc9 100644 --- a/tool/CommandIp.h +++ b/tool/CommandIp.h @@ -27,6 +27,7 @@ * ****************************************************************************/ + #ifndef __COMMANDIP_H__ #define __COMMANDIP_H__ diff --git a/tool/Makefile.am b/tool/Makefile.am index 5961aabf..6269a383 100644 --- a/tool/Makefile.am +++ b/tool/Makefile.am @@ -49,7 +49,6 @@ ethercat_SOURCES = \ CommandFoeRead.cpp \ CommandFoeWrite.cpp \ CommandGraph.cpp \ - CommandIp.cpp \ CommandMaster.cpp \ CommandPdos.cpp \ CommandRegRead.cpp \ @@ -74,12 +73,6 @@ ethercat_SOURCES = \ main.cpp \ sii_crc.cpp -if ENABLE_EOE -ethercat_SOURCES += CommandEoe.cpp -else -EXTRA_DIST += CommandEoe.cpp -endif - noinst_HEADERS = \ Command.h \ CommandAlias.h \ @@ -93,7 +86,6 @@ noinst_HEADERS = \ CommandFoeRead.h \ CommandFoeWrite.h \ CommandGraph.h \ - CommandIp.h \ CommandMaster.h \ CommandPdos.h \ CommandRegRead.h \ @@ -118,9 +110,18 @@ noinst_HEADERS = \ sii_crc.h if ENABLE_EOE -noinst_HEADERS += CommandEoe.h +ethercat_SOURCES += \ + CommandEoe.cpp \ + CommandIp.cpp +noinst_HEADERS += \ + CommandEoe.h \ + CommandIp.h else -EXTRA_DIST += CommandEoe.h +EXTRA_DIST += \ + CommandEoe.cpp \ + CommandIp.cpp \ + CommandEoe.h \ + CommandIp.h endif REV = `if test -s $(top_srcdir)/revision; then \ diff --git a/tool/MasterDevice.cpp b/tool/MasterDevice.cpp index 4a8c18ad..00dae90c 100644 --- a/tool/MasterDevice.cpp +++ b/tool/MasterDevice.cpp @@ -605,6 +605,7 @@ void MasterDevice::writeSoe(ec_ioctl_slave_soe_write_t *data) /****************************************************************************/ +#ifdef EC_EOE void MasterDevice::setIpParam(ec_ioctl_slave_eoe_ip_t *data) { if (ioctl(fd, EC_IOCTL_SLAVE_EOE_IP_PARAM, data) < 0) { @@ -617,5 +618,6 @@ void MasterDevice::setIpParam(ec_ioctl_slave_eoe_ip_t *data) } } } +#endif /*****************************************************************************/ diff --git a/tool/MasterDevice.h b/tool/MasterDevice.h index c0d6c694..0684fc9e 100644 --- a/tool/MasterDevice.h +++ b/tool/MasterDevice.h @@ -93,7 +93,7 @@ class MasterDeviceSoeException: }; /****************************************************************************/ - +#ifdef EC_EOE class MasterDeviceEoeException: public MasterDeviceException { @@ -108,7 +108,7 @@ class MasterDeviceEoeException: MasterDeviceException("EoE set IP parameter failed."), result(result) {}; }; - +#endif /****************************************************************************/ class MasterDevice @@ -160,10 +160,10 @@ class MasterDevice void writeFoe(ec_ioctl_slave_foe_t *); #ifdef EC_EOE void getEoeHandler(ec_ioctl_eoe_handler_t *, uint16_t); + void setIpParam(ec_ioctl_slave_eoe_ip_t *); #endif void readSoe(ec_ioctl_slave_soe_read_t *); void writeSoe(ec_ioctl_slave_soe_write_t *); - void setIpParam(ec_ioctl_slave_eoe_ip_t *); unsigned int getMasterCount() const {return masterCount;} diff --git a/tool/main.cpp b/tool/main.cpp index dc756071..0f6a0cb5 100644 --- a/tool/main.cpp +++ b/tool/main.cpp @@ -49,7 +49,9 @@ using namespace std; #include "CommandFoeRead.h" #include "CommandFoeWrite.h" #include "CommandGraph.h" -#include "CommandIp.h" +#ifdef EC_EOE +# include "CommandIp.h" +#endif #include "CommandMaster.h" #include "CommandPdos.h" #include "CommandRegRead.h" @@ -291,7 +293,9 @@ int main(int argc, char **argv) commandList.push_back(new CommandFoeRead()); commandList.push_back(new CommandFoeWrite()); commandList.push_back(new CommandGraph()); +#ifdef EC_EOE commandList.push_back(new CommandIp()); +#endif commandList.push_back(new CommandMaster()); commandList.push_back(new CommandPdos()); commandList.push_back(new CommandRegRead()); From 234695fe44a81a99cefc62fef25f2dbaa905ceb1 Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 23 Jun 2022 10:26:43 +0200 Subject: [PATCH 11/25] Applied 0006-avoid-ssize_t.patch --- master/globals.h | 4 ++-- master/module.c | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/master/globals.h b/master/globals.h index ffd89711..3779b3ec 100755 --- a/master/globals.h +++ b/master/globals.h @@ -77,7 +77,7 @@ /** Resulting maximum data size of a single datagram in a frame. */ #ifdef DEBUG_DATAGRAM_OVERFLOW -// Define a runt datagram which can be easily overflowed on +// Define a runt datagram which can be easily overflowed on // available hardware for use when testing ec_domain_finish() #define EC_MAX_DATA_SIZE (128) #else @@ -266,7 +266,7 @@ unsigned int ec_master_count(void); void ec_print_data(const uint8_t *, size_t); void ec_print_data_diff(const uint8_t *, const uint8_t *, size_t); size_t ec_state_string(uint8_t, char *, uint8_t); -ssize_t ec_mac_print(const uint8_t *, char *); +size_t ec_mac_print(const uint8_t *, char *); int ec_mac_is_zero(const uint8_t *); ec_master_t *ecrt_request_master_err(unsigned int); diff --git a/master/module.c b/master/module.c index 573a6949..250f6121 100644 --- a/master/module.c +++ b/master/module.c @@ -239,12 +239,12 @@ int ec_mac_equal( * * \return number of bytes written. */ -ssize_t ec_mac_print( +size_t ec_mac_print( const uint8_t *mac, /**< MAC address */ char *buffer /**< Target buffer. */ ) { - off_t off = 0; + size_t off = 0; unsigned int i; for (i = 0; i < ETH_ALEN; i++) { From 6cac716122a74352c306e55209c4f0a42077436d Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 23 Jun 2022 10:31:41 +0200 Subject: [PATCH 12/25] Applied 0007-replace-fprintf-calls-with-EC_PRINT_ERR.patch --- lib/common.c | 8 +++--- lib/domain.c | 10 +++---- lib/ioctl.h | 10 ++++++- lib/master.c | 70 +++++++++++++++++++++++----------------------- lib/reg_request.c | 8 +++--- lib/sdo_request.c | 14 +++++----- lib/slave_config.c | 60 +++++++++++++++++++-------------------- lib/voe_handler.c | 16 +++++------ 8 files changed, 102 insertions(+), 94 deletions(-) diff --git a/lib/common.c b/lib/common.c index a70f3559..40854f6b 100644 --- a/lib/common.c +++ b/lib/common.c @@ -77,7 +77,7 @@ ec_master_t *ecrt_open_master(unsigned int master_index) master = malloc(sizeof(ec_master_t)); if (!master) { - fprintf(stderr, "Failed to allocate memory.\n"); + EC_PRINT_ERR("Failed to allocate memory.\n"); return 0; } @@ -100,20 +100,20 @@ ec_master_t *ecrt_open_master(unsigned int master_index) master->fd = open(path, O_RDWR); #endif if (EC_IOCTL_IS_ERROR(master->fd)) { - fprintf(stderr, "Failed to open %s: %s\n", path, + EC_PRINT_ERR("Failed to open %s: %s\n", path, strerror(EC_IOCTL_ERRNO(master->fd))); goto out_clear; } ret = ioctl(master->fd, EC_IOCTL_MODULE, &module_data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to get module information from %s: %s\n", + EC_PRINT_ERR("Failed to get module information from %s: %s\n", path, strerror(EC_IOCTL_ERRNO(ret))); goto out_clear; } if (module_data.ioctl_version_magic != EC_IOCTL_VERSION_MAGIC) { - fprintf(stderr, "ioctl() version magic is differing:" + EC_PRINT_ERR("ioctl() version magic is differing:" " %s: %u, libethercat: %u.\n", path, module_data.ioctl_version_magic, EC_IOCTL_VERSION_MAGIC); diff --git a/lib/domain.c b/lib/domain.c index 1ee838b8..86548f29 100644 --- a/lib/domain.c +++ b/lib/domain.c @@ -82,7 +82,7 @@ size_t ecrt_domain_size(const ec_domain_t *domain) ret = ioctl(domain->master->fd, EC_IOCTL_DOMAIN_SIZE, domain->index); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to get domain size: %s\n", + EC_PRINT_ERR("Failed to get domain size: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } @@ -99,7 +99,7 @@ uint8_t *ecrt_domain_data(ec_domain_t *domain) offset = ioctl(domain->master->fd, EC_IOCTL_DOMAIN_OFFSET, domain->index); if (EC_IOCTL_IS_ERROR(offset)) { - fprintf(stderr, "Failed to get domain offset: %s\n", + EC_PRINT_ERR("Failed to get domain offset: %s\n", strerror(EC_IOCTL_ERRNO(offset))); return NULL; } @@ -118,7 +118,7 @@ void ecrt_domain_process(ec_domain_t *domain) ret = ioctl(domain->master->fd, EC_IOCTL_DOMAIN_PROCESS, domain->index); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to process domain: %s\n", + EC_PRINT_ERR("Failed to process domain: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } @@ -131,7 +131,7 @@ void ecrt_domain_queue(ec_domain_t *domain) ret = ioctl(domain->master->fd, EC_IOCTL_DOMAIN_QUEUE, domain->index); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to queue domain: %s\n", + EC_PRINT_ERR("Failed to queue domain: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } @@ -148,7 +148,7 @@ void ecrt_domain_state(const ec_domain_t *domain, ec_domain_state_t *state) ret = ioctl(domain->master->fd, EC_IOCTL_DOMAIN_STATE, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to get domain state: %s\n", + EC_PRINT_ERR("Failed to get domain state: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } diff --git a/lib/ioctl.h b/lib/ioctl.h index c79a9a26..7f387e4a 100644 --- a/lib/ioctl.h +++ b/lib/ioctl.h @@ -53,6 +53,11 @@ #define EC_IOCTL_IS_ERROR(X) ((X) < 0) #define EC_IOCTL_ERRNO(X) (-(X)) +/* print errors to syslog */ +#define KERN_ERR "<3>" +#define EC_PRINT_ERR(fmt, args...) \ + rt_printk(KERN_ERR "EtherCAT ERROR: " fmt, ##args) + #else #define ioctl ioctl @@ -61,6 +66,10 @@ #define EC_IOCTL_IS_ERROR(X) ((X) == -1) #define EC_IOCTL_ERRNO(X) (errno) +/* print errors to stderr */ +#define EC_PRINT_ERR(fmt, args...) \ + fprintf(stderr, fmt, ##args) + #include #endif @@ -70,4 +79,3 @@ #endif /* __EC_LIB_IOCTL_H__ */ /*****************************************************************************/ - diff --git a/lib/master.c b/lib/master.c index 81d100db..b1b0abda 100644 --- a/lib/master.c +++ b/lib/master.c @@ -44,7 +44,7 @@ int ecrt_master_reserve(ec_master_t *master) { int ret = ioctl(master->fd, EC_IOCTL_REQUEST, NULL); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to reserve master: %s\n", + EC_PRINT_ERR("Failed to reserve master: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -128,13 +128,13 @@ ec_domain_t *ecrt_master_create_domain(ec_master_t *master) domain = malloc(sizeof(ec_domain_t)); if (!domain) { - fprintf(stderr, "Failed to allocate memory.\n"); + EC_PRINT_ERR("Failed to allocate memory.\n"); return 0; } index = ioctl(master->fd, EC_IOCTL_CREATE_DOMAIN, NULL); if (EC_IOCTL_IS_ERROR(index)) { - fprintf(stderr, "Failed to create domain: %s\n", + EC_PRINT_ERR("Failed to create domain: %s\n", strerror(EC_IOCTL_ERRNO(index))); free(domain); return 0; @@ -177,7 +177,7 @@ ec_slave_config_t *ecrt_master_slave_config(ec_master_t *master, sc = malloc(sizeof(ec_slave_config_t)); if (!sc) { - fprintf(stderr, "Failed to allocate memory.\n"); + EC_PRINT_ERR("Failed to allocate memory.\n"); return 0; } @@ -188,7 +188,7 @@ ec_slave_config_t *ecrt_master_slave_config(ec_master_t *master, ret = ioctl(master->fd, EC_IOCTL_CREATE_SLAVE_CONFIG, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to create slave config: %s\n", + EC_PRINT_ERR("Failed to create slave config: %s\n", strerror(EC_IOCTL_ERRNO(ret))); free(sc); return 0; @@ -225,7 +225,7 @@ int ecrt_master_select_reference_clock(ec_master_t *master, ret = ioctl(master->fd, EC_IOCTL_SELECT_REF_CLOCK, config_index); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to select reference clock: %s\n", + EC_PRINT_ERR("Failed to select reference clock: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -242,7 +242,7 @@ int ecrt_master(ec_master_t *master, ec_master_info_t *master_info) ret = ioctl(master->fd, EC_IOCTL_MASTER, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to get master info: %s\n", + EC_PRINT_ERR("Failed to get master info: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -266,7 +266,7 @@ int ecrt_master_get_slave(ec_master_t *master, uint16_t slave_position, ret = ioctl(master->fd, EC_IOCTL_SLAVE, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to get slave info: %s\n", + EC_PRINT_ERR("Failed to get slave info: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -316,7 +316,7 @@ int ecrt_master_get_sync_manager(ec_master_t *master, uint16_t slave_position, ret = ioctl(master->fd, EC_IOCTL_SLAVE_SYNC, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to get sync manager information: %s\n", + EC_PRINT_ERR("Failed to get sync manager information: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -350,7 +350,7 @@ int ecrt_master_get_pdo(ec_master_t *master, uint16_t slave_position, ret = ioctl(master->fd, EC_IOCTL_SLAVE_SYNC_PDO, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to get pdo information: %s\n", + EC_PRINT_ERR("Failed to get pdo information: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -382,7 +382,7 @@ int ecrt_master_get_pdo_entry(ec_master_t *master, uint16_t slave_position, ret = ioctl(master->fd, EC_IOCTL_SLAVE_SYNC_PDO_ENTRY, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to get pdo entry information: %s\n", + EC_PRINT_ERR("Failed to get pdo entry information: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -415,7 +415,7 @@ int ecrt_master_sdo_download(ec_master_t *master, uint16_t slave_position, if (EC_IOCTL_ERRNO(ret) == EIO && abort_code) { *abort_code = download.abort_code; } - fprintf(stderr, "Failed to execute SDO download: %s\n", + EC_PRINT_ERR("Failed to execute SDO download: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -444,7 +444,7 @@ int ecrt_master_sdo_download_complete(ec_master_t *master, if (EC_IOCTL_ERRNO(ret) == EIO && abort_code) { *abort_code = download.abort_code; } - fprintf(stderr, "Failed to execute SDO download: %s\n", + EC_PRINT_ERR("Failed to execute SDO download: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -472,7 +472,7 @@ int ecrt_master_sdo_upload(ec_master_t *master, uint16_t slave_position, if (EC_IOCTL_ERRNO(ret) == EIO && abort_code) { *abort_code = upload.abort_code; } - fprintf(stderr, "Failed to execute SDO upload: %s\n", + EC_PRINT_ERR("Failed to execute SDO upload: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -501,7 +501,7 @@ int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position, if (EC_IOCTL_ERRNO(ret) == EIO && error_code) { *error_code = io.error_code; } - fprintf(stderr, "Failed to write IDN: %s\n", + EC_PRINT_ERR("Failed to write IDN: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -529,7 +529,7 @@ int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position, if (EC_IOCTL_ERRNO(ret) == EIO && error_code) { *error_code = io.error_code; } - fprintf(stderr, "Failed to read IDN: %s\n", + EC_PRINT_ERR("Failed to read IDN: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -547,7 +547,7 @@ int ecrt_master_setup_domain_memory(ec_master_t *master) ret = ioctl(master->fd, EC_IOCTL_SETUP_DOMAIN_MEMORY, &io); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to activate master: %s\n", + EC_PRINT_ERR("Failed to activate master: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -565,7 +565,7 @@ int ecrt_master_setup_domain_memory(ec_master_t *master) master->process_data = mmap(0, master->process_data_size, PROT_READ | PROT_WRITE, MAP_SHARED, master->fd, 0); if (master->process_data == MAP_FAILED) { - fprintf(stderr, "Failed to map process data: %s\n", + EC_PRINT_ERR("Failed to map process data: %s\n", strerror(errno)); master->process_data = NULL; master->process_data_size = 0; @@ -589,7 +589,7 @@ int ecrt_master_activate(ec_master_t *master) ret = ioctl(master->fd, EC_IOCTL_ACTIVATE, &io); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to activate master: %s\n", + EC_PRINT_ERR("Failed to activate master: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -607,7 +607,7 @@ int ecrt_master_activate(ec_master_t *master) master->process_data = mmap(0, master->process_data_size, PROT_READ | PROT_WRITE, MAP_SHARED, master->fd, 0); if (master->process_data == MAP_FAILED) { - fprintf(stderr, "Failed to map process data: %s\n", + EC_PRINT_ERR("Failed to map process data: %s\n", strerror(errno)); master->process_data = NULL; master->process_data_size = 0; @@ -630,7 +630,7 @@ void ecrt_master_deactivate_slaves(ec_master_t *master) ret = ioctl(master->fd, EC_IOCTL_DEACTIVATE_SLAVES, NULL); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to deactivate slaves: %s\n", + EC_PRINT_ERR("Failed to deactivate slaves: %s\n", strerror(EC_IOCTL_IS_ERROR(ret))); return; } @@ -644,7 +644,7 @@ void ecrt_master_deactivate(ec_master_t *master) ret = ioctl(master->fd, EC_IOCTL_DEACTIVATE, NULL); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to deactivate master: %s\n", + EC_PRINT_ERR("Failed to deactivate master: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return; } @@ -661,7 +661,7 @@ int ecrt_master_set_send_interval(ec_master_t *master, ret = ioctl(master->fd, EC_IOCTL_SET_SEND_INTERVAL, &send_interval_us); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to set send interval: %s\n", + EC_PRINT_ERR("Failed to set send interval: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -678,7 +678,7 @@ size_t ecrt_master_send(ec_master_t *master) ret = ioctl(master->fd, EC_IOCTL_SEND, &sent_bytes); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to send: %s\n", + EC_PRINT_ERR("Failed to send: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } @@ -693,7 +693,7 @@ void ecrt_master_receive(ec_master_t *master) ret = ioctl(master->fd, EC_IOCTL_RECEIVE, NULL); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to receive: %s\n", + EC_PRINT_ERR("Failed to receive: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } @@ -706,7 +706,7 @@ void ecrt_master_state(const ec_master_t *master, ec_master_state_t *state) ret = ioctl(master->fd, EC_IOCTL_MASTER_STATE, state); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to get master state: %s\n", + EC_PRINT_ERR("Failed to get master state: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } @@ -724,7 +724,7 @@ int ecrt_master_link_state(const ec_master_t *master, unsigned int dev_idx, ret = ioctl(master->fd, EC_IOCTL_MASTER_LINK_STATE, &io); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to get link state: %s\n", + EC_PRINT_ERR("Failed to get link state: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -743,7 +743,7 @@ void ecrt_master_application_time(ec_master_t *master, uint64_t app_time) ret = ioctl(master->fd, EC_IOCTL_APP_TIME, &time); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to set application time: %s\n", + EC_PRINT_ERR("Failed to set application time: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } @@ -756,7 +756,7 @@ void ecrt_master_sync_reference_clock(ec_master_t *master) ret = ioctl(master->fd, EC_IOCTL_SYNC_REF, NULL); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to sync reference clock: %s\n", + EC_PRINT_ERR("Failed to sync reference clock: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } @@ -773,7 +773,7 @@ void ecrt_master_sync_reference_clock_to(ec_master_t *master, ret = ioctl(master->fd, EC_IOCTL_SYNC_REF_TO, &time); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to sync reference clock: %s\n", + EC_PRINT_ERR("Failed to sync reference clock: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } @@ -786,7 +786,7 @@ void ecrt_master_sync_slave_clocks(ec_master_t *master) ret = ioctl(master->fd, EC_IOCTL_SYNC_SLAVES, NULL); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to sync slave clocks: %s\n", + EC_PRINT_ERR("Failed to sync slave clocks: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } @@ -802,7 +802,7 @@ int ecrt_master_reference_clock_time(ec_master_t *master, uint32_t *time) ret = EC_IOCTL_ERRNO(ret); if (ret != EIO && ret != ENXIO && ret != EAGAIN) { // do not log if no refclk or no refclk time yet - fprintf(stderr, "Failed to get reference clock time: %s\n", + EC_PRINT_ERR("Failed to get reference clock time: %s\n", strerror(ret)); } return -ret; @@ -819,7 +819,7 @@ void ecrt_master_sync_monitor_queue(ec_master_t *master) ret = ioctl(master->fd, EC_IOCTL_SYNC_MON_QUEUE, NULL); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to queue sync monitor datagram: %s\n", + EC_PRINT_ERR("Failed to queue sync monitor datagram: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } @@ -834,7 +834,7 @@ uint32_t ecrt_master_sync_monitor_process(ec_master_t *master) ret = ioctl(master->fd, EC_IOCTL_SYNC_MON_PROCESS, &time_diff); if (EC_IOCTL_IS_ERROR(ret)) { time_diff = 0xffffffff; - fprintf(stderr, "Failed to process sync monitor datagram: %s\n", + EC_PRINT_ERR("Failed to process sync monitor datagram: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } @@ -849,7 +849,7 @@ void ecrt_master_reset(ec_master_t *master) ret = ioctl(master->fd, EC_IOCTL_RESET, NULL); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to reset master: %s\n", + EC_PRINT_ERR("Failed to reset master: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } diff --git a/lib/reg_request.c b/lib/reg_request.c index 8fa72243..2eb6bca5 100644 --- a/lib/reg_request.c +++ b/lib/reg_request.c @@ -71,7 +71,7 @@ ec_request_state_t ecrt_reg_request_state(ec_reg_request_t *reg) ret = ioctl(reg->config->master->fd, EC_IOCTL_REG_REQUEST_STATE, &io); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to get register request state: %s\n", + EC_PRINT_ERR("Failed to get register request state: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return EC_REQUEST_ERROR; } @@ -84,7 +84,7 @@ ec_request_state_t ecrt_reg_request_state(ec_reg_request_t *reg) ret = ioctl(reg->config->master->fd, EC_IOCTL_REG_REQUEST_DATA, &io); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to get register data: %s\n", + EC_PRINT_ERR("Failed to get register data: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return EC_REQUEST_ERROR; } @@ -109,7 +109,7 @@ void ecrt_reg_request_write(ec_reg_request_t *reg, uint16_t address, ret = ioctl(reg->config->master->fd, EC_IOCTL_REG_REQUEST_WRITE, &io); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to command an register write operation: %s\n", + EC_PRINT_ERR("Failed to command an register write operation: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } @@ -129,7 +129,7 @@ void ecrt_reg_request_read(ec_reg_request_t *reg, uint16_t address, ret = ioctl(reg->config->master->fd, EC_IOCTL_REG_REQUEST_READ, &io); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to command an register read operation: %s\n", + EC_PRINT_ERR("Failed to command an register read operation: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } diff --git a/lib/sdo_request.c b/lib/sdo_request.c index ad1abbe2..1117cbb4 100644 --- a/lib/sdo_request.c +++ b/lib/sdo_request.c @@ -67,7 +67,7 @@ void ecrt_sdo_request_index(ec_sdo_request_t *req, uint16_t index, ret = ioctl(req->config->master->fd, EC_IOCTL_SDO_REQUEST_INDEX, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to set SDO request index/subindex: %s\n", + EC_PRINT_ERR("Failed to set SDO request index/subindex: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } @@ -85,7 +85,7 @@ void ecrt_sdo_request_timeout(ec_sdo_request_t *req, uint32_t timeout) ret = ioctl(req->config->master->fd, EC_IOCTL_SDO_REQUEST_TIMEOUT, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to set SDO request timeout: %s\n", + EC_PRINT_ERR("Failed to set SDO request timeout: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } @@ -116,14 +116,14 @@ ec_request_state_t ecrt_sdo_request_state(ec_sdo_request_t *req) ret = ioctl(req->config->master->fd, EC_IOCTL_SDO_REQUEST_STATE, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to get SDO request state: %s\n", + EC_PRINT_ERR("Failed to get SDO request state: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return EC_REQUEST_ERROR; } if (data.size) { // new data waiting to be copied if (req->mem_size < data.size) { - fprintf(stderr, "Received %zu bytes do not fit info SDO data" + EC_PRINT_ERR("Received %zu bytes do not fit info SDO data" " memory (%zu bytes)!\n", data.size, req->mem_size); return EC_REQUEST_ERROR; } @@ -133,7 +133,7 @@ ec_request_state_t ecrt_sdo_request_state(ec_sdo_request_t *req) ret = ioctl(req->config->master->fd, EC_IOCTL_SDO_REQUEST_DATA, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to get SDO data: %s\n", + EC_PRINT_ERR("Failed to get SDO data: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return EC_REQUEST_ERROR; } @@ -155,7 +155,7 @@ void ecrt_sdo_request_read(ec_sdo_request_t *req) ret = ioctl(req->config->master->fd, EC_IOCTL_SDO_REQUEST_READ, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to command an SDO read operation : %s\n", + EC_PRINT_ERR("Failed to command an SDO read operation : %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } @@ -174,7 +174,7 @@ void ecrt_sdo_request_write(ec_sdo_request_t *req) ret = ioctl(req->config->master->fd, EC_IOCTL_SDO_REQUEST_WRITE, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to command an SDO write operation : %s\n", + EC_PRINT_ERR("Failed to command an SDO write operation : %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } diff --git a/lib/slave_config.c b/lib/slave_config.c index 7a3bc96d..74b2e9f9 100644 --- a/lib/slave_config.c +++ b/lib/slave_config.c @@ -94,7 +94,7 @@ int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index, ret = ioctl(sc->master->fd, EC_IOCTL_SC_SYNC, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to config sync manager: %s\n", + EC_PRINT_ERR("Failed to config sync manager: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -117,7 +117,7 @@ void ecrt_slave_config_watchdog(ec_slave_config_t *sc, ret = ioctl(sc->master->fd, EC_IOCTL_SC_WATCHDOG, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to config watchdog: %s\n", + EC_PRINT_ERR("Failed to config watchdog: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } @@ -134,7 +134,7 @@ void ecrt_slave_config_overlapping_pdos(ec_slave_config_t *sc, data.allow_overlapping_pdos = allow_overlapping_pdos; if (ioctl(sc->master->fd, EC_IOCTL_SC_OVERLAPPING_IO, &data) == -1) { - fprintf(stderr, "Failed to config overlapping PDOs: %s\n", + EC_PRINT_ERR("Failed to config overlapping PDOs: %s\n", strerror(errno)); } } @@ -153,7 +153,7 @@ int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc, ret = ioctl(sc->master->fd, EC_IOCTL_SC_ADD_PDO, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to add PDO: %s\n", + EC_PRINT_ERR("Failed to add PDO: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -174,7 +174,7 @@ void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc, ret = ioctl(sc->master->fd, EC_IOCTL_SC_CLEAR_PDOS, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to clear PDOs: %s\n", + EC_PRINT_ERR("Failed to clear PDOs: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } @@ -196,7 +196,7 @@ int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc, ret = ioctl(sc->master->fd, EC_IOCTL_SC_ADD_ENTRY, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to add PDO entry: %s\n", + EC_PRINT_ERR("Failed to add PDO entry: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -217,7 +217,7 @@ void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc, ret = ioctl(sc->master->fd, EC_IOCTL_SC_CLEAR_ENTRIES, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to clear PDO entries: %s\n", + EC_PRINT_ERR("Failed to clear PDO entries: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } @@ -243,7 +243,7 @@ int ecrt_slave_config_pdos(ec_slave_config_t *sc, break; if (sync_info->index >= EC_MAX_SYNC_MANAGERS) { - fprintf(stderr, "Invalid sync manager index %u!\n", + EC_PRINT_ERR("Invalid sync manager index %u!\n", sync_info->index); return -ENOENT; } @@ -306,7 +306,7 @@ int ecrt_slave_config_reg_pdo_entry( ret = ioctl(sc->master->fd, EC_IOCTL_SC_REG_PDO_ENTRY, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to register PDO entry: %s\n", + EC_PRINT_ERR("Failed to register PDO entry: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -315,7 +315,7 @@ int ecrt_slave_config_reg_pdo_entry( *bit_position = data.bit_position; } else { if (data.bit_position) { - fprintf(stderr, "PDO entry 0x%04X:%02X does not byte-align " + EC_PRINT_ERR("PDO entry 0x%04X:%02X does not byte-align " "in config %u:%u.\n", index, subindex, sc->alias, sc->position); return -EFAULT; @@ -347,7 +347,7 @@ int ecrt_slave_config_reg_pdo_entry_pos( ret = ioctl(sc->master->fd, EC_IOCTL_SC_REG_PDO_POS, &io); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to register PDO entry: %s\n", + EC_PRINT_ERR("Failed to register PDO entry: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -356,7 +356,7 @@ int ecrt_slave_config_reg_pdo_entry_pos( *bit_position = io.bit_position; } else { if (io.bit_position) { - fprintf(stderr, "PDO entry %u/%u/%u does not byte-align " + EC_PRINT_ERR("PDO entry %u/%u/%u does not byte-align " "in config %u:%u.\n", sync_index, pdo_pos, entry_pos, sc->alias, sc->position); return -EFAULT; @@ -384,7 +384,7 @@ void ecrt_slave_config_dc(ec_slave_config_t *sc, uint16_t assign_activate, ret = ioctl(sc->master->fd, EC_IOCTL_SC_DC, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to set DC parameters: %s\n", + EC_PRINT_ERR("Failed to set DC parameters: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } @@ -406,7 +406,7 @@ int ecrt_slave_config_sdo(ec_slave_config_t *sc, uint16_t index, ret = ioctl(sc->master->fd, EC_IOCTL_SC_SDO, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to configure SDO: %s\n", + EC_PRINT_ERR("Failed to configure SDO: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -431,7 +431,7 @@ int ecrt_slave_config_complete_sdo(ec_slave_config_t *sc, uint16_t index, ret = ioctl(sc->master->fd, EC_IOCTL_SC_SDO, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to configure SDO: %s\n", + EC_PRINT_ERR("Failed to configure SDO: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -484,7 +484,7 @@ int ecrt_slave_config_emerg_size(ec_slave_config_t *sc, size_t elements) ret = ioctl(sc->master->fd, EC_IOCTL_SC_EMERG_SIZE, &io); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to set emergency ring size: %s\n", + EC_PRINT_ERR("Failed to set emergency ring size: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -505,7 +505,7 @@ int ecrt_slave_config_emerg_pop(ec_slave_config_t *sc, uint8_t *target) ret = ioctl(sc->master->fd, EC_IOCTL_SC_EMERG_POP, &io); if (EC_IOCTL_IS_ERROR(ret)) { if (EC_IOCTL_ERRNO(ret) != ENOENT) { - fprintf(stderr, "Failed to get emergency message: %s\n", + EC_PRINT_ERR("Failed to get emergency message: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } return -EC_IOCTL_ERRNO(ret); @@ -525,7 +525,7 @@ int ecrt_slave_config_emerg_clear(ec_slave_config_t *sc) ret = ioctl(sc->master->fd, EC_IOCTL_SC_EMERG_CLEAR, &io); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to clear emergency ring: %s\n", + EC_PRINT_ERR("Failed to clear emergency ring: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -544,7 +544,7 @@ int ecrt_slave_config_emerg_overruns(ec_slave_config_t *sc) ret = ioctl(sc->master->fd, EC_IOCTL_SC_EMERG_OVERRUNS, &io); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to get emergency overruns: %s\n", + EC_PRINT_ERR("Failed to get emergency overruns: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } @@ -579,14 +579,14 @@ ec_sdo_request_t *ecrt_slave_config_create_sdo_request(ec_slave_config_t *sc, req = malloc(sizeof(ec_sdo_request_t)); if (!req) { - fprintf(stderr, "Failed to allocate memory.\n"); + EC_PRINT_ERR("Failed to allocate memory.\n"); return 0; } if (size) { req->data = malloc(size); if (!req->data) { - fprintf(stderr, "Failed to allocate %zu bytes of SDO data" + EC_PRINT_ERR("Failed to allocate %zu bytes of SDO data" " memory.\n", size); free(req); return 0; @@ -602,7 +602,7 @@ ec_sdo_request_t *ecrt_slave_config_create_sdo_request(ec_slave_config_t *sc, ret = ioctl(sc->master->fd, EC_IOCTL_SC_SDO_REQUEST, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to create SDO request: %s\n", + EC_PRINT_ERR("Failed to create SDO request: %s\n", strerror(EC_IOCTL_ERRNO(ret))); ec_sdo_request_clear(req); free(req); @@ -649,14 +649,14 @@ ec_reg_request_t *ecrt_slave_config_create_reg_request(ec_slave_config_t *sc, reg = malloc(sizeof(ec_reg_request_t)); if (!reg) { - fprintf(stderr, "Failed to allocate memory.\n"); + EC_PRINT_ERR("Failed to allocate memory.\n"); return NULL; } if (size) { reg->data = malloc(size); if (!reg->data) { - fprintf(stderr, "Failed to allocate %zu bytes of register data" + EC_PRINT_ERR("Failed to allocate %zu bytes of register data" " memory.\n", size); free(reg); return 0; @@ -670,7 +670,7 @@ ec_reg_request_t *ecrt_slave_config_create_reg_request(ec_slave_config_t *sc, ret = ioctl(sc->master->fd, EC_IOCTL_SC_REG_REQUEST, &io); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to create register request: %s\n", + EC_PRINT_ERR("Failed to create register request: %s\n", strerror(EC_IOCTL_ERRNO(ret))); ec_reg_request_clear(reg); free(reg); @@ -714,14 +714,14 @@ ec_voe_handler_t *ecrt_slave_config_create_voe_handler(ec_slave_config_t *sc, voe = malloc(sizeof(ec_voe_handler_t)); if (!voe) { - fprintf(stderr, "Failed to allocate memory.\n"); + EC_PRINT_ERR("Failed to allocate memory.\n"); return 0; } if (size) { voe->data = malloc(size); if (!voe->data) { - fprintf(stderr, "Failed to allocate %zu bytes of VoE data" + EC_PRINT_ERR("Failed to allocate %zu bytes of VoE data" " memory.\n", size); free(voe); return 0; @@ -735,7 +735,7 @@ ec_voe_handler_t *ecrt_slave_config_create_voe_handler(ec_slave_config_t *sc, ret = ioctl(sc->master->fd, EC_IOCTL_SC_VOE, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to create VoE handler: %s\n", + EC_PRINT_ERR("Failed to create VoE handler: %s\n", strerror(EC_IOCTL_ERRNO(ret))); ec_voe_handler_clear(voe); free(voe); @@ -766,7 +766,7 @@ void ecrt_slave_config_state(const ec_slave_config_t *sc, ret = ioctl(sc->master->fd, EC_IOCTL_SC_STATE, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to get slave configuration state: %s\n", + EC_PRINT_ERR("Failed to get slave configuration state: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } @@ -788,7 +788,7 @@ int ecrt_slave_config_idn(ec_slave_config_t *sc, uint8_t drive_no, ret = ioctl(sc->master->fd, EC_IOCTL_SC_IDN, &io); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to configure IDN: %s\n", + EC_PRINT_ERR("Failed to configure IDN: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } diff --git a/lib/voe_handler.c b/lib/voe_handler.c index 723f530a..c6382d5f 100644 --- a/lib/voe_handler.c +++ b/lib/voe_handler.c @@ -66,7 +66,7 @@ void ecrt_voe_handler_send_header(ec_voe_handler_t *voe, uint32_t vendor_id, ret = ioctl(voe->config->master->fd, EC_IOCTL_VOE_SEND_HEADER, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to set VoE send header: %s\n", + EC_PRINT_ERR("Failed to set VoE send header: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } @@ -86,7 +86,7 @@ void ecrt_voe_handler_received_header(const ec_voe_handler_t *voe, ret = ioctl(voe->config->master->fd, EC_IOCTL_VOE_REC_HEADER, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to get received VoE header: %s\n", + EC_PRINT_ERR("Failed to get received VoE header: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } @@ -117,7 +117,7 @@ void ecrt_voe_handler_read(ec_voe_handler_t *voe) ret = ioctl(voe->config->master->fd, EC_IOCTL_VOE_READ, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to initiate VoE reading: %s\n", + EC_PRINT_ERR("Failed to initiate VoE reading: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } @@ -134,7 +134,7 @@ void ecrt_voe_handler_read_nosync(ec_voe_handler_t *voe) ret = ioctl(voe->config->master->fd, EC_IOCTL_VOE_READ_NOSYNC, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to initiate VoE reading: %s\n", + EC_PRINT_ERR("Failed to initiate VoE reading: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } @@ -153,7 +153,7 @@ void ecrt_voe_handler_write(ec_voe_handler_t *voe, size_t size) ret = ioctl(voe->config->master->fd, EC_IOCTL_VOE_WRITE, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to initiate VoE writing: %s\n", + EC_PRINT_ERR("Failed to initiate VoE writing: %s\n", strerror(EC_IOCTL_ERRNO(ret))); } } @@ -170,14 +170,14 @@ ec_request_state_t ecrt_voe_handler_execute(ec_voe_handler_t *voe) ret = ioctl(voe->config->master->fd, EC_IOCTL_VOE_EXEC, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to execute VoE handler: %s\n", + EC_PRINT_ERR("Failed to execute VoE handler: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return EC_REQUEST_ERROR; } if (data.size) { // new data waiting to be copied if (voe->mem_size < data.size) { - fprintf(stderr, "Received %zu bytes do not fit info VoE data" + EC_PRINT_ERR("Received %zu bytes do not fit info VoE data" " memory (%zu bytes)!\n", data.size, voe->mem_size); return EC_REQUEST_ERROR; } @@ -186,7 +186,7 @@ ec_request_state_t ecrt_voe_handler_execute(ec_voe_handler_t *voe) ret = ioctl(voe->config->master->fd, EC_IOCTL_VOE_DATA, &data); if (EC_IOCTL_IS_ERROR(ret)) { - fprintf(stderr, "Failed to get VoE data: %s\n", + EC_PRINT_ERR("Failed to get VoE data: %s\n", strerror(EC_IOCTL_ERRNO(ret))); return EC_REQUEST_ERROR; } From 225af9f7876c669449a0175a1d2d042e41d878d7 Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 23 Jun 2022 10:45:16 +0200 Subject: [PATCH 13/25] Applied 0008-read-reference-slave-clock-64bit-time.patch --- include/ecrt.h | 28 +++++++++++++++++++ lib/common.c | 2 ++ lib/master.c | 40 ++++++++++++++++++++++++++++ lib/master.h | 3 +++ master/fsm_master.c | 2 +- master/ioctl.c | 65 +++++++++++++++++++++++++++++++++++++++++++++ master/ioctl.h | 2 ++ master/master.c | 53 +++++++++++++++++++++++++++++++++++- master/master.h | 2 ++ 9 files changed, 195 insertions(+), 2 deletions(-) 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 From 90f3f7e841e48cd74937d54e614a722102e6c378 Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 23 Jun 2022 10:53:56 +0200 Subject: [PATCH 14/25] Applied 0009-Avoid-changing-running-slaves-DC-offset.patch --- master/fsm_master.c | 69 ++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 68 insertions(+), 1 deletion(-) diff --git a/master/fsm_master.c b/master/fsm_master.c index c171828a..4a6ae576 100644 --- a/master/fsm_master.c +++ b/master/fsm_master.c @@ -69,6 +69,8 @@ void ec_fsm_master_state_dc_measure_delays(ec_fsm_master_t *); void ec_fsm_master_state_scan_slave(ec_fsm_master_t *); void ec_fsm_master_state_dc_read_offset(ec_fsm_master_t *); void ec_fsm_master_state_dc_write_offset(ec_fsm_master_t *); +void ec_fsm_master_state_dc_reset_filter(ec_fsm_master_t *); + void ec_fsm_master_state_assign_sii(ec_fsm_master_t *); void ec_fsm_master_state_write_sii(ec_fsm_master_t *); void ec_fsm_master_state_sdo_dictionary(ec_fsm_master_t *); @@ -1250,8 +1252,9 @@ void ec_fsm_master_enter_write_system_times( // read DC system time (0x0910, 64 bit) // gap (64 bit) // and time offset (0x0920, 64 bit) + // and receive delay (0x0928, 32 bit) ec_datagram_fprd(fsm->datagram, fsm->slave->station_address, - 0x0910, 24); + 0x0910, 28); ec_datagram_zero(fsm->datagram); fsm->datagram->device_index = fsm->slave->device_index; fsm->retries = EC_FSM_RETRIES; @@ -1353,6 +1356,7 @@ void ec_fsm_master_state_dc_read_offset( ec_slave_t *slave = fsm->slave; ec_master_t *master = fsm->master; u64 system_time, old_offset, new_offset; + u32 old_delay; if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) return; @@ -1384,6 +1388,7 @@ void ec_fsm_master_state_dc_read_offset( system_time = EC_READ_U64(datagram->data); // 0x0910 old_offset = EC_READ_U64(datagram->data + 16); // 0x0920 + old_delay = EC_READ_U32(datagram->data + 24); // 0x0928 if (slave->base_dc_range == EC_DC_32) { new_offset = ec_fsm_master_dc_offset32(fsm, @@ -1393,6 +1398,23 @@ void ec_fsm_master_state_dc_read_offset( system_time, old_offset, datagram->app_time_sent); } + if (new_offset != old_offset + && slave->current_state >= EC_SLAVE_STATE_SAFEOP) { + // Slave is already active; changing the system time offset could + // cause problems. Leave the offset alone in this case and just + // let the normal cyclic sync process gradually adjust it to the + // correct time. + EC_SLAVE_DBG(slave, 1, "Slave is running; ignoring DC offset change.\n"); + new_offset = old_offset; + } + + if (new_offset == old_offset && slave->transmission_delay == old_delay) { + // offsets have not changed; skip write to avoid possible trouble + fsm->slave++; + ec_fsm_master_enter_write_system_times(fsm); + return; + } + // set DC system time offset and transmission delay ec_datagram_fpwr(datagram, slave->station_address, 0x0920, 12); EC_WRITE_U64(datagram->data, new_offset); @@ -1433,6 +1455,51 @@ void ec_fsm_master_state_dc_write_offset( return; } + // reset DC filter after changing offsets + if (slave->current_state >= EC_SLAVE_STATE_SAFEOP) { + EC_SLAVE_DBG(slave, 1, "Slave is running; not resetting DC filter.\n"); + fsm->slave++; + ec_fsm_master_enter_write_system_times(fsm); + } else { + ec_datagram_fpwr(datagram, slave->station_address, 0x0930, 2); + EC_WRITE_U16(datagram->data, 0x1000); + fsm->datagram->device_index = slave->device_index; + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_master_state_dc_reset_filter; + } +} + +/*****************************************************************************/ + +/** Master state: DC RESET FILTER. + */ +void ec_fsm_master_state_dc_reset_filter( + ec_fsm_master_t *fsm /**< Master state machine. */ + ) +{ + ec_datagram_t *datagram = fsm->datagram; + ec_slave_t *slave = fsm->slave; + + if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) + return; + + if (datagram->state != EC_DATAGRAM_RECEIVED) { + EC_SLAVE_ERR(slave, + "Failed to receive DC reset filter datagram: "); + ec_datagram_print_state(datagram); + fsm->slave++; + ec_fsm_master_enter_write_system_times(fsm); + return; + } + + if (datagram->working_counter != 1) { + EC_SLAVE_ERR(slave, "Failed to reset DC filter: "); + ec_datagram_print_wc_error(datagram); + fsm->slave++; + ec_fsm_master_enter_write_system_times(fsm); + return; + } + fsm->slave++; ec_fsm_master_enter_write_system_times(fsm); } From d1dc6795f379af37adf783954e8def757357dbcf Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 23 Jun 2022 10:55:03 +0200 Subject: [PATCH 15/25] Applied 0010-Make-busy-logging-a-little-less-irritating.patch --- master/fsm_master.c | 2 +- master/fsm_slave.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/master/fsm_master.c b/master/fsm_master.c index 4a6ae576..56164f27 100644 --- a/master/fsm_master.c +++ b/master/fsm_master.c @@ -527,7 +527,7 @@ int ec_fsm_master_action_process_sdo( } if (!ec_fsm_slave_is_ready(&slave->fsm)) { - EC_SLAVE_DBG(slave, 1, "Busy - processing external request!\n"); + EC_SLAVE_DBG(slave, 2, "Busy - processing external request!\n"); continue; } diff --git a/master/fsm_slave.c b/master/fsm_slave.c index 9f85d487..904da960 100644 --- a/master/fsm_slave.c +++ b/master/fsm_slave.c @@ -217,7 +217,7 @@ void ec_fsm_slave_state_ready( if (slave->config) { list_for_each_entry(req, &slave->config->sdo_requests, list) { if (req->state == EC_INT_REQUEST_QUEUED || req->state == EC_INT_REQUEST_BUSY) { - EC_SLAVE_DBG(slave, 1, "Busy - processing internal SDO request!\n"); + EC_SLAVE_DBG(slave, 2, "Busy - processing internal SDO request %04X:%02X!\n", req->index, req->subindex); return; } } From 57342c99ca6dd85d7b92624bf708abc29ced4e63 Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 23 Jun 2022 10:55:57 +0200 Subject: [PATCH 16/25] Applied 0011-Reduced-printing-to-avoid-syslog-spam.patch --- lib/master.c | 2 -- master/ioctl.c | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/lib/master.c b/lib/master.c index 0b587a8d..f4410a95 100644 --- a/lib/master.c +++ b/lib/master.c @@ -266,8 +266,6 @@ int ecrt_master_get_slave(ec_master_t *master, uint16_t slave_position, ret = ioctl(master->fd, EC_IOCTL_SLAVE, &data); if (EC_IOCTL_IS_ERROR(ret)) { - EC_PRINT_ERR("Failed to get slave info: %s\n", - strerror(EC_IOCTL_ERRNO(ret))); return -EC_IOCTL_ERRNO(ret); } diff --git a/master/ioctl.c b/master/ioctl.c index cf823832..d13b2c20 100755 --- a/master/ioctl.c +++ b/master/ioctl.c @@ -216,7 +216,7 @@ static ATTRIBUTES int ec_ioctl_slave( if (!(slave = ec_master_find_slave_const( master, 0, data.position))) { up(&master->master_sem); - EC_MASTER_ERR(master, "Slave %u does not exist!\n", data.position); + EC_MASTER_DBG(master, 1, "Slave %u does not exist!\n", data.position); return -EINVAL; } From 8db3e5abf424e7b4a393825766f4bdc2dd5e1b1a Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 23 Jun 2022 10:56:33 +0200 Subject: [PATCH 17/25] Applied 0012-Added-newline-to-syslog-message-MAC-address-derived.patch --- master/ethernet.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/master/ethernet.c b/master/ethernet.c index ae40ce9d..8593d5ef 100644 --- a/master/ethernet.c +++ b/master/ethernet.c @@ -231,7 +231,7 @@ int ec_eoe_init( // interfaces and the NIC part can be used for the EoE // interface MAC. EC_SLAVE_INFO(slave, "%s MAC address derived from" - " NIC part of %s MAC address", + " NIC part of %s MAC address\n", eoe->dev->name, dev->name); eoe->dev->dev_addr[1] = dev->dev_addr[3]; eoe->dev->dev_addr[2] = dev->dev_addr[4]; @@ -245,7 +245,7 @@ int ec_eoe_init( if (eoe->dev->addr_len == ETH_ALEN) { if (use_master_mac) { EC_SLAVE_INFO(slave, "%s MAC address derived" - " from NIC part of %s MAC address", + " from NIC part of %s MAC address\n", eoe->dev->name, slave->master->devices[EC_DEVICE_MAIN].dev->name); eoe->dev->dev_addr[1] = From d09daadfbbc6809acfdf0fa988e89ea26cb15109 Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 23 Jun 2022 10:58:57 +0200 Subject: [PATCH 18/25] Applied 0013-Do-not-reuse-the-index-of-a-pending-datagram.patch --- master/master.c | 23 ++++++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/master/master.c b/master/master.c index 30afdeed..e2d931f8 100644 --- a/master/master.c +++ b/master/master.c @@ -989,6 +989,15 @@ void ec_master_queue_datagram_ext( /*****************************************************************************/ +static int index_in_use(ec_master_t *master, uint8_t index) +{ + ec_datagram_t *datagram; + list_for_each_entry(datagram, &master->datagram_queue, queue) + if (datagram->state == EC_DATAGRAM_SENT && datagram->index == index) + return 1; + return 0; +} + /** Sends the datagrams in the queue for a certain device. * */ @@ -1008,6 +1017,7 @@ size_t ec_master_send_datagrams( unsigned int frame_count, more_datagrams_waiting; struct list_head sent_datagrams; size_t sent_bytes = 0; + uint8_t last_index; #ifdef EC_HAVE_CYCLES cycles_start = get_cycles(); @@ -1045,9 +1055,19 @@ size_t ec_master_send_datagrams( break; } - list_add_tail(&datagram->sent, &sent_datagrams); + // do not reuse the index of a pending datagram to avoid confusion + // in ec_master_receive_datagrams() + last_index = master->datagram_index; + while (index_in_use(master, master->datagram_index)) { + if (++master->datagram_index == last_index) { + EC_MASTER_ERR(master, "No free datagram index, sending delayed\n"); + goto break_send; + } + } datagram->index = master->datagram_index++; + list_add_tail(&datagram->sent, &sent_datagrams); + EC_MASTER_DBG(master, 2, "Adding datagram 0x%02X\n", datagram->index); @@ -1075,6 +1095,7 @@ size_t ec_master_send_datagrams( cur_data += EC_DATAGRAM_FOOTER_SIZE; } +break_send: if (list_empty(&sent_datagrams)) { EC_MASTER_DBG(master, 2, "nothing to send.\n"); break; From a3fa7cecafcab5470a23ab96d5efa8145c1ad7c1 Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 23 Jun 2022 10:59:45 +0200 Subject: [PATCH 19/25] Applied 0014-Fix-NOHZ-local_softirq_pending-08-warning.patch --- master/debug.c | 2 +- master/ethernet.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/master/debug.c b/master/debug.c index ad5f90eb..b45df896 100644 --- a/master/debug.c +++ b/master/debug.c @@ -192,7 +192,7 @@ void ec_debug_send( skb->dev = dbg->dev; skb->protocol = eth_type_trans(skb, dbg->dev); skb->ip_summed = CHECKSUM_UNNECESSARY; - netif_rx(skb); + netif_rx_ni(skb); } /****************************************************************************** diff --git a/master/ethernet.c b/master/ethernet.c index 8593d5ef..37bb1afe 100644 --- a/master/ethernet.c +++ b/master/ethernet.c @@ -688,7 +688,7 @@ void ec_eoe_state_rx_fetch(ec_eoe_t *eoe /**< EoE handler */) eoe->rx_skb->dev = eoe->dev; eoe->rx_skb->protocol = eth_type_trans(eoe->rx_skb, eoe->dev); eoe->rx_skb->ip_summed = CHECKSUM_UNNECESSARY; - if (netif_rx(eoe->rx_skb)) { + if (netif_rx_ni(eoe->rx_skb)) { EC_SLAVE_WARN(eoe->slave, "EoE RX netif_rx failed.\n"); } eoe->rx_skb = NULL; From 667e93284d4438a02c3dfa4392d7a7962e3ed6b8 Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 23 Jun 2022 11:00:45 +0200 Subject: [PATCH 20/25] Applied 0015-Clear-configuration-on-deactivate-even-if-not-activa.patch --- master/master.c | 1 + 1 file changed, 1 insertion(+) diff --git a/master/master.c b/master/master.c index e2d931f8..dfe39081 100644 --- a/master/master.c +++ b/master/master.c @@ -2505,6 +2505,7 @@ void ecrt_master_deactivate(ec_master_t *master) if (!master->active) { EC_MASTER_WARN(master, "%s: Master not active.\n", __func__); + ec_master_clear_config(master); return; } From 9955855f7e37d42659e1b1b269005560d2f41f5e Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 23 Jun 2022 11:21:51 +0200 Subject: [PATCH 21/25] Applied 0016-If-enable-rtmutex-use-rtmutexes-instead-of-semaphore.patch --- configure.ac | 29 ++++ master/Makefile.am | 1 + master/device.c | 4 +- master/domain.c | 4 +- master/ethernet.c | 16 +- master/ethernet.h | 9 +- master/fsm_master.c | 10 +- master/ioctl.c | 350 +++++++++++++++++++++--------------------- master/locks.h | 80 ++++++++++ master/master.c | 142 ++++++++--------- master/master.h | 19 +-- master/module.c | 24 +-- master/slave_config.c | 46 +++--- 13 files changed, 417 insertions(+), 317 deletions(-) create mode 100644 master/locks.h diff --git a/configure.ac b/configure.ac index 61bf31e1..8a1d71e4 100644 --- a/configure.ac +++ b/configure.ac @@ -812,6 +812,35 @@ else AC_MSG_RESULT([no]) fi +#------------------------------------------------------------------------------ +# Realtime Mutex support +#------------------------------------------------------------------------------ + +AC_MSG_CHECKING([whether to use rtmutexes]) + +AC_ARG_ENABLE([rtmutex], + AS_HELP_STRING([--enable-rtmutex], + [Use rtmutexes for synchronization (default: no)]), + [ + case "${enableval}" in + yes) rtmutex=1 + ;; + no) rtmutex=0 + ;; + *) AC_MSG_ERROR([Invalid value for --enable-rtmutex]) + ;; + esac + ], + [rtmutex=0] +) + +if test "x${rtmutex}" = "x1"; then + AC_DEFINE([EC_USE_RTMUTEX], [1], [Use rtmutex for synchronization]) + AC_MSG_RESULT([yes]) +else + AC_MSG_RESULT([no]) +fi + #------------------------------------------------------------------------------ # High-resolution timer support #------------------------------------------------------------------------------ diff --git a/master/Makefile.am b/master/Makefile.am index 505b6a7f..382fab77 100644 --- a/master/Makefile.am +++ b/master/Makefile.am @@ -57,6 +57,7 @@ noinst_HEADERS = \ fsm_soe.c fsm_soe.h \ globals.h \ ioctl.c ioctl.h \ + locks.h \ mailbox.c mailbox.h \ master.c master.h \ module.c \ diff --git a/master/device.c b/master/device.c index deb938fa..337dea40 100644 --- a/master/device.c +++ b/master/device.c @@ -559,9 +559,9 @@ void ecdev_withdraw(ec_device_t *device /**< EtherCAT device */) EC_MASTER_INFO(master, "Releasing %s device %s.\n", dev_str, mac_str); - down(&master->device_sem); + ec_lock_down(&master->device_sem); ec_device_detach(device); - up(&master->device_sem); + ec_lock_up(&master->device_sem); } /*****************************************************************************/ diff --git a/master/domain.c b/master/domain.c index 96db67e5..1ac08341 100755 --- a/master/domain.c +++ b/master/domain.c @@ -512,14 +512,14 @@ void ecrt_domain_external_memory(ec_domain_t *domain, uint8_t *mem) EC_MASTER_DBG(domain->master, 1, "ecrt_domain_external_memory(" "domain = 0x%p, mem = 0x%p)\n", domain, mem); - down(&domain->master->master_sem); + ec_lock_down(&domain->master->master_sem); ec_domain_clear_data(domain); domain->data = mem; domain->data_origin = EC_ORIG_EXTERNAL; - up(&domain->master->master_sem); + ec_lock_up(&domain->master->master_sem); } /*****************************************************************************/ diff --git a/master/ethernet.c b/master/ethernet.c index 37bb1afe..f51dc92e 100644 --- a/master/ethernet.c +++ b/master/ethernet.c @@ -153,7 +153,7 @@ int ec_eoe_init( eoe->tx_queue_size = EC_EOE_TX_QUEUE_SIZE; eoe->tx_queued_frames = 0; - sema_init(&eoe->tx_queue_sem, 1); + ec_lock_init(&eoe->tx_queue_sem); eoe->tx_frame_number = 0xFF; memset(&eoe->stats, 0, sizeof(struct net_device_stats)); @@ -323,7 +323,7 @@ void ec_eoe_flush(ec_eoe_t *eoe /**< EoE handler */) { ec_eoe_frame_t *frame, *next; - down(&eoe->tx_queue_sem); + ec_lock_down(&eoe->tx_queue_sem); list_for_each_entry_safe(frame, next, &eoe->tx_queue, queue) { list_del(&frame->queue); @@ -332,7 +332,7 @@ void ec_eoe_flush(ec_eoe_t *eoe /**< EoE handler */) } eoe->tx_queued_frames = 0; - up(&eoe->tx_queue_sem); + ec_lock_up(&eoe->tx_queue_sem); } /*****************************************************************************/ @@ -727,10 +727,10 @@ void ec_eoe_state_tx_start(ec_eoe_t *eoe /**< EoE handler */) return; } - down(&eoe->tx_queue_sem); + ec_lock_down(&eoe->tx_queue_sem); if (!eoe->tx_queued_frames || list_empty(&eoe->tx_queue)) { - up(&eoe->tx_queue_sem); + ec_lock_up(&eoe->tx_queue_sem); eoe->tx_idle = 1; // no data available. // start a new receive immediately. @@ -751,7 +751,7 @@ void ec_eoe_state_tx_start(ec_eoe_t *eoe /**< EoE handler */) } eoe->tx_queued_frames--; - up(&eoe->tx_queue_sem); + ec_lock_up(&eoe->tx_queue_sem); eoe->tx_idle = 0; @@ -923,14 +923,14 @@ int ec_eoedev_tx(struct sk_buff *skb, /**< transmit socket buffer */ frame->skb = skb; - down(&eoe->tx_queue_sem); + ec_lock_down(&eoe->tx_queue_sem); list_add_tail(&frame->queue, &eoe->tx_queue); eoe->tx_queued_frames++; if (eoe->tx_queued_frames == eoe->tx_queue_size) { netif_stop_queue(dev); eoe->tx_queue_active = 0; } - up(&eoe->tx_queue_sem); + ec_lock_up(&eoe->tx_queue_sem); #if EOE_DEBUG_LEVEL >= 2 EC_SLAVE_DBG(eoe->slave, 0, "EoE %s TX queued frame" diff --git a/master/ethernet.h b/master/ethernet.h index 024ec9a3..85a31822 100644 --- a/master/ethernet.h +++ b/master/ethernet.h @@ -40,13 +40,8 @@ #include #include -#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27) -#include -#else -#include -#endif - #include "globals.h" +#include "locks.h" #include "slave.h" #include "datagram.h" @@ -109,7 +104,7 @@ struct ec_eoe unsigned int tx_queue_size; /**< Transmit queue size. */ unsigned int tx_queue_active; /**< kernel netif queue started */ unsigned int tx_queued_frames; /**< number of frames in the queue */ - struct semaphore tx_queue_sem; /**< Semaphore for the send queue. */ + ec_lock_t tx_queue_sem; /**< Semaphore for the send queue. */ ec_eoe_frame_t *tx_frame; /**< current TX frame */ uint8_t tx_frame_number; /**< number of the transmitted frame */ uint8_t tx_fragment_number; /**< number of the fragment */ diff --git a/master/fsm_master.c b/master/fsm_master.c index 56164f27..1ae6466e 100644 --- a/master/fsm_master.c +++ b/master/fsm_master.c @@ -334,15 +334,15 @@ void ec_fsm_master_state_broadcast( } if (fsm->rescan_required) { - down(&master->scan_sem); + ec_lock_down(&master->scan_sem); if (!master->allow_scan) { - up(&master->scan_sem); + ec_lock_up(&master->scan_sem); } else { unsigned int count = 0, next_dev_slave, ring_position; ec_device_index_t dev_idx; master->scan_busy = 1; - up(&master->scan_sem); + ec_lock_up(&master->scan_sem); // clear all slaves and scan the bus fsm->rescan_required = 0; @@ -826,9 +826,9 @@ void ec_fsm_master_action_configure( || slave->force_config) && !slave->error_flag) { // Start slave configuration - down(&master->config_sem); + ec_lock_down(&master->config_sem); master->config_busy = 1; - up(&master->config_sem); + ec_lock_up(&master->config_sem); if (master->debug_level) { char old_state[EC_STATE_STRING_SIZE], diff --git a/master/ioctl.c b/master/ioctl.c index d13b2c20..afd8e902 100755 --- a/master/ioctl.c +++ b/master/ioctl.c @@ -109,7 +109,7 @@ static ATTRIBUTES int ec_ioctl_master( ec_ioctl_master_t io; unsigned int dev_idx, j; - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { return -EINTR; } @@ -123,9 +123,9 @@ static ATTRIBUTES int ec_ioctl_master( io.active = (uint8_t) master->active; io.scan_busy = master->scan_busy; - up(&master->master_sem); + ec_lock_up(&master->master_sem); - if (down_interruptible(&master->device_sem)) { + if (ec_lock_down_interruptible(&master->device_sem)) { return -EINTR; } @@ -177,7 +177,7 @@ static ATTRIBUTES int ec_ioctl_master( master->device_stats.loss_rates[j]; } - up(&master->device_sem); + ec_lock_up(&master->device_sem); io.app_time = master->app_time; io.dc_ref_time = master->dc_ref_time; @@ -210,12 +210,12 @@ static ATTRIBUTES int ec_ioctl_slave( return -EFAULT; } - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; if (!(slave = ec_master_find_slave_const( master, 0, data.position))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_DBG(master, 1, "Slave %u does not exist!\n", data.position); return -EINVAL; } @@ -270,7 +270,7 @@ static ATTRIBUTES int ec_ioctl_slave( ec_ioctl_strcpy(data.order, slave->sii.order); ec_ioctl_strcpy(data.name, slave->sii.name); - up(&master->master_sem); + ec_lock_up(&master->master_sem); if (copy_to_user((void __user *) arg, &data, sizeof(data))) return -EFAULT; @@ -297,19 +297,19 @@ static ATTRIBUTES int ec_ioctl_slave_sync( return -EFAULT; } - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; if (!(slave = ec_master_find_slave_const( master, 0, data.slave_position))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Slave %u does not exist!\n", data.slave_position); return -EINVAL; } if (data.sync_index >= slave->sii.sync_count) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_SLAVE_ERR(slave, "Sync manager %u does not exist!\n", data.sync_index); return -EINVAL; @@ -323,7 +323,7 @@ static ATTRIBUTES int ec_ioctl_slave_sync( data.enable = sync->enable; data.pdo_count = ec_pdo_list_count(&sync->pdos); - up(&master->master_sem); + ec_lock_up(&master->master_sem); if (copy_to_user((void __user *) arg, &data, sizeof(data))) return -EFAULT; @@ -351,19 +351,19 @@ static ATTRIBUTES int ec_ioctl_slave_sync_pdo( return -EFAULT; } - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; if (!(slave = ec_master_find_slave_const( master, 0, data.slave_position))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Slave %u does not exist!\n", data.slave_position); return -EINVAL; } if (data.sync_index >= slave->sii.sync_count) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_SLAVE_ERR(slave, "Sync manager %u does not exist!\n", data.sync_index); return -EINVAL; @@ -372,7 +372,7 @@ static ATTRIBUTES int ec_ioctl_slave_sync_pdo( sync = &slave->sii.syncs[data.sync_index]; if (!(pdo = ec_pdo_list_find_pdo_by_pos_const( &sync->pdos, data.pdo_pos))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_SLAVE_ERR(slave, "Sync manager %u does not contain a PDO with " "position %u!\n", data.sync_index, data.pdo_pos); return -EINVAL; @@ -382,7 +382,7 @@ static ATTRIBUTES int ec_ioctl_slave_sync_pdo( data.entry_count = ec_pdo_entry_count(pdo); ec_ioctl_strcpy(data.name, pdo->name); - up(&master->master_sem); + ec_lock_up(&master->master_sem); if (copy_to_user((void __user *) arg, &data, sizeof(data))) return -EFAULT; @@ -411,19 +411,19 @@ static ATTRIBUTES int ec_ioctl_slave_sync_pdo_entry( return -EFAULT; } - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; if (!(slave = ec_master_find_slave_const( master, 0, data.slave_position))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Slave %u does not exist!\n", data.slave_position); return -EINVAL; } if (data.sync_index >= slave->sii.sync_count) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_SLAVE_ERR(slave, "Sync manager %u does not exist!\n", data.sync_index); return -EINVAL; @@ -432,7 +432,7 @@ static ATTRIBUTES int ec_ioctl_slave_sync_pdo_entry( sync = &slave->sii.syncs[data.sync_index]; if (!(pdo = ec_pdo_list_find_pdo_by_pos_const( &sync->pdos, data.pdo_pos))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_SLAVE_ERR(slave, "Sync manager %u does not contain a PDO with " "position %u!\n", data.sync_index, data.pdo_pos); return -EINVAL; @@ -440,7 +440,7 @@ static ATTRIBUTES int ec_ioctl_slave_sync_pdo_entry( if (!(entry = ec_pdo_find_entry_by_pos_const( pdo, data.entry_pos))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_SLAVE_ERR(slave, "PDO 0x%04X does not contain an entry with " "position %u!\n", data.pdo_pos, data.entry_pos); return -EINVAL; @@ -451,7 +451,7 @@ static ATTRIBUTES int ec_ioctl_slave_sync_pdo_entry( data.bit_length = entry->bit_length; ec_ioctl_strcpy(data.name, entry->name); - up(&master->master_sem); + ec_lock_up(&master->master_sem); if (copy_to_user((void __user *) arg, &data, sizeof(data))) return -EFAULT; @@ -478,11 +478,11 @@ static ATTRIBUTES int ec_ioctl_domain( return -EFAULT; } - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; if (!(domain = ec_master_find_domain_const(master, data.index))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Domain %u does not exist!\n", data.index); return -EINVAL; } @@ -496,7 +496,7 @@ static ATTRIBUTES int ec_ioctl_domain( data.expected_working_counter = domain->expected_working_counter; data.fmmu_count = ec_domain_fmmu_count(domain); - up(&master->master_sem); + ec_lock_up(&master->master_sem); if (copy_to_user((void __user *) arg, &data, sizeof(data))) return -EFAULT; @@ -523,18 +523,18 @@ static ATTRIBUTES int ec_ioctl_domain_fmmu( return -EFAULT; } - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; if (!(domain = ec_master_find_domain_const(master, data.domain_index))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Domain %u does not exist!\n", data.domain_index); return -EINVAL; } if (!(fmmu = ec_domain_find_fmmu(domain, data.fmmu_index))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Domain %u has less than %u" " fmmu configurations.\n", data.domain_index, data.fmmu_index + 1); @@ -548,7 +548,7 @@ static ATTRIBUTES int ec_ioctl_domain_fmmu( data.logical_address = fmmu->domain->logical_base_address + fmmu->logical_domain_offset; data.data_size = fmmu->data_size; - up(&master->master_sem); + ec_lock_up(&master->master_sem); if (copy_to_user((void __user *) arg, &data, sizeof(data))) return -EFAULT; @@ -574,18 +574,18 @@ static ATTRIBUTES int ec_ioctl_domain_data( return -EFAULT; } - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; if (!(domain = ec_master_find_domain_const(master, data.domain_index))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Domain %u does not exist!\n", data.domain_index); return -EINVAL; } if (domain->data_size != data.data_size) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Data size mismatch %u/%zu!\n", data.data_size, domain->data_size); return -EFAULT; @@ -593,11 +593,11 @@ static ATTRIBUTES int ec_ioctl_domain_data( if (copy_to_user((void __user *) data.target, domain->data, domain->data_size)) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); return -EFAULT; } - up(&master->master_sem); + ec_lock_up(&master->master_sem); return 0; } @@ -648,12 +648,12 @@ static ATTRIBUTES int ec_ioctl_slave_state( return -EFAULT; } - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; if (!(slave = ec_master_find_slave( master, 0, data.slave_position))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Slave %u does not exist!\n", data.slave_position); return -EINVAL; @@ -661,7 +661,7 @@ static ATTRIBUTES int ec_ioctl_slave_state( ec_slave_request_state(slave, data.al_state); - up(&master->master_sem); + ec_lock_up(&master->master_sem); return 0; } @@ -684,12 +684,12 @@ static ATTRIBUTES int ec_ioctl_slave_sdo( return -EFAULT; } - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; if (!(slave = ec_master_find_slave_const( master, 0, data.slave_position))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Slave %u does not exist!\n", data.slave_position); return -EINVAL; @@ -697,7 +697,7 @@ static ATTRIBUTES int ec_ioctl_slave_sdo( if (!(sdo = ec_slave_get_sdo_by_pos_const( slave, data.sdo_position))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_SLAVE_ERR(slave, "SDO %u does not exist!\n", data.sdo_position); return -EINVAL; } @@ -706,7 +706,7 @@ static ATTRIBUTES int ec_ioctl_slave_sdo( data.max_subindex = sdo->max_subindex; ec_ioctl_strcpy(data.name, sdo->name); - up(&master->master_sem); + ec_lock_up(&master->master_sem); if (copy_to_user((void __user *) arg, &data, sizeof(data))) return -EFAULT; @@ -734,12 +734,12 @@ static ATTRIBUTES int ec_ioctl_slave_sdo_entry( return -EFAULT; } - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; if (!(slave = ec_master_find_slave_const( master, 0, data.slave_position))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Slave %u does not exist!\n", data.slave_position); return -EINVAL; @@ -748,14 +748,14 @@ static ATTRIBUTES int ec_ioctl_slave_sdo_entry( if (data.sdo_spec <= 0) { if (!(sdo = ec_slave_get_sdo_by_pos_const( slave, -data.sdo_spec))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_SLAVE_ERR(slave, "SDO %u does not exist!\n", -data.sdo_spec); return -EINVAL; } } else { if (!(sdo = ec_slave_get_sdo_const( slave, data.sdo_spec))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_SLAVE_ERR(slave, "SDO 0x%04X does not exist!\n", data.sdo_spec); return -EINVAL; @@ -764,7 +764,7 @@ static ATTRIBUTES int ec_ioctl_slave_sdo_entry( if (!(entry = ec_sdo_get_entry_const( sdo, data.sdo_entry_subindex))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_SLAVE_ERR(slave, "SDO entry 0x%04X:%02X does not exist!\n", sdo->index, data.sdo_entry_subindex); return -EINVAL; @@ -786,7 +786,7 @@ static ATTRIBUTES int ec_ioctl_slave_sdo_entry( entry->write_access[EC_SDO_ENTRY_ACCESS_OP]; ec_ioctl_strcpy(data.description, entry->description); - up(&master->master_sem); + ec_lock_up(&master->master_sem); if (copy_to_user((void __user *) arg, &data, sizeof(data))) return -EFAULT; @@ -907,12 +907,12 @@ static ATTRIBUTES int ec_ioctl_slave_sii_read( return -EFAULT; } - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; if (!(slave = ec_master_find_slave_const( master, 0, data.slave_position))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Slave %u does not exist!\n", data.slave_position); return -EINVAL; @@ -920,7 +920,7 @@ static ATTRIBUTES int ec_ioctl_slave_sii_read( if (!data.nwords || data.offset + data.nwords > slave->sii_nwords) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_SLAVE_ERR(slave, "Invalid SII read offset/size %u/%u for slave SII" " size %zu!\n", data.offset, data.nwords, slave->sii_nwords); return -EINVAL; @@ -932,7 +932,7 @@ static ATTRIBUTES int ec_ioctl_slave_sii_read( else retval = 0; - up(&master->master_sem); + ec_lock_up(&master->master_sem); return retval; } @@ -974,14 +974,14 @@ static ATTRIBUTES int ec_ioctl_slave_sii_write( return -EFAULT; } - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { kfree(words); return -EINTR; } if (!(slave = ec_master_find_slave( master, 0, data.slave_position))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Slave %u does not exist!\n", data.slave_position); kfree(words); @@ -999,21 +999,21 @@ static ATTRIBUTES int ec_ioctl_slave_sii_write( // schedule SII write request. list_add_tail(&request.list, &master->sii_requests); - up(&master->master_sem); + ec_lock_up(&master->master_sem); // wait for processing through FSM if (wait_event_interruptible(master->request_queue, request.state != EC_INT_REQUEST_QUEUED)) { // interrupted by signal - down(&master->master_sem); + ec_lock_down(&master->master_sem); if (request.state == EC_INT_REQUEST_QUEUED) { // abort request list_del(&request.list); - up(&master->master_sem); + ec_lock_up(&master->master_sem); kfree(words); return -EINTR; } - up(&master->master_sem); + ec_lock_up(&master->master_sem); } // wait until master FSM has finished processing @@ -1056,14 +1056,14 @@ static ATTRIBUTES int ec_ioctl_slave_reg_read( ecrt_reg_request_read(&request, io.address, io.size); - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { ec_reg_request_clear(&request); return -EINTR; } if (!(slave = ec_master_find_slave( master, 0, io.slave_position))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); ec_reg_request_clear(&request); EC_MASTER_ERR(master, "Slave %u does not exist!\n", io.slave_position); @@ -1073,21 +1073,21 @@ static ATTRIBUTES int ec_ioctl_slave_reg_read( // schedule request. list_add_tail(&request.list, &slave->reg_requests); - up(&master->master_sem); + ec_lock_up(&master->master_sem); // wait for processing through FSM if (wait_event_interruptible(master->request_queue, request.state != EC_INT_REQUEST_QUEUED)) { // interrupted by signal - down(&master->master_sem); + ec_lock_down(&master->master_sem); if (request.state == EC_INT_REQUEST_QUEUED) { // abort request list_del(&request.list); - up(&master->master_sem); + ec_lock_up(&master->master_sem); ec_reg_request_clear(&request); return -EINTR; } - up(&master->master_sem); + ec_lock_up(&master->master_sem); } // wait until master FSM has finished processing @@ -1140,7 +1140,7 @@ static ATTRIBUTES int ec_ioctl_slave_reg_write( ecrt_reg_request_write(&request, io.address, io.size); - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { ec_reg_request_clear(&request); return -EINTR; } @@ -1152,7 +1152,7 @@ static ATTRIBUTES int ec_ioctl_slave_reg_write( } else { if (!(slave = ec_master_find_slave(master, 0, io.slave_position))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); ec_reg_request_clear(&request); EC_MASTER_ERR(master, "Slave %u does not exist!\n", io.slave_position); @@ -1163,21 +1163,21 @@ static ATTRIBUTES int ec_ioctl_slave_reg_write( list_add_tail(&request.list, &slave->reg_requests); } - up(&master->master_sem); + ec_lock_up(&master->master_sem); // wait for processing through FSM if (wait_event_interruptible(master->request_queue, request.state != EC_INT_REQUEST_QUEUED)) { // interrupted by signal - down(&master->master_sem); + ec_lock_down(&master->master_sem); if (request.state == EC_INT_REQUEST_QUEUED) { // abort request list_del(&request.list); - up(&master->master_sem); + ec_lock_up(&master->master_sem); ec_reg_request_clear(&request); return -EINTR; } - up(&master->master_sem); + ec_lock_up(&master->master_sem); } // wait until master FSM has finished processing @@ -1207,12 +1207,12 @@ static ATTRIBUTES int ec_ioctl_config( return -EFAULT; } - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; if (!(sc = ec_master_get_config_const( master, data.config_index))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Slave config %u does not exist!\n", data.config_index); return -EINVAL; @@ -1239,7 +1239,7 @@ static ATTRIBUTES int ec_ioctl_config( data.dc_sync[i] = sc->dc_sync[i]; } - up(&master->master_sem); + ec_lock_up(&master->master_sem); if (copy_to_user((void __user *) arg, &data, sizeof(data))) return -EFAULT; @@ -1272,12 +1272,12 @@ static ATTRIBUTES int ec_ioctl_config_pdo( return -EINVAL; } - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; if (!(sc = ec_master_get_config_const( master, data.config_index))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Slave config %u does not exist!\n", data.config_index); return -EINVAL; @@ -1286,7 +1286,7 @@ static ATTRIBUTES int ec_ioctl_config_pdo( if (!(pdo = ec_pdo_list_find_pdo_by_pos_const( &sc->sync_configs[data.sync_index].pdos, data.pdo_pos))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Invalid PDO position!\n"); return -EINVAL; } @@ -1295,7 +1295,7 @@ static ATTRIBUTES int ec_ioctl_config_pdo( data.entry_count = ec_pdo_entry_count(pdo); ec_ioctl_strcpy(data.name, pdo->name); - up(&master->master_sem); + ec_lock_up(&master->master_sem); if (copy_to_user((void __user *) arg, &data, sizeof(data))) return -EFAULT; @@ -1329,12 +1329,12 @@ static ATTRIBUTES int ec_ioctl_config_pdo_entry( return -EINVAL; } - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; if (!(sc = ec_master_get_config_const( master, data.config_index))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Slave config %u does not exist!\n", data.config_index); return -EINVAL; @@ -1343,14 +1343,14 @@ static ATTRIBUTES int ec_ioctl_config_pdo_entry( if (!(pdo = ec_pdo_list_find_pdo_by_pos_const( &sc->sync_configs[data.sync_index].pdos, data.pdo_pos))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Invalid PDO position!\n"); return -EINVAL; } if (!(entry = ec_pdo_find_entry_by_pos_const( pdo, data.entry_pos))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Entry not found!\n"); return -EINVAL; } @@ -1360,7 +1360,7 @@ static ATTRIBUTES int ec_ioctl_config_pdo_entry( data.bit_length = entry->bit_length; ec_ioctl_strcpy(data.name, entry->name); - up(&master->master_sem); + ec_lock_up(&master->master_sem); if (copy_to_user((void __user *) arg, &data, sizeof(data))) return -EFAULT; @@ -1392,14 +1392,14 @@ static ATTRIBUTES int ec_ioctl_config_sdo( return -EFAULT; } - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { kfree(ioctl); return -EINTR; } if (!(sc = ec_master_get_config_const( master, ioctl->config_index))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Slave config %u does not exist!\n", ioctl->config_index); kfree(ioctl); @@ -1408,7 +1408,7 @@ static ATTRIBUTES int ec_ioctl_config_sdo( if (!(req = ec_slave_config_get_sdo_by_pos_const( sc, ioctl->sdo_pos))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Invalid SDO position!\n"); kfree(ioctl); return -EINVAL; @@ -1421,7 +1421,7 @@ static ATTRIBUTES int ec_ioctl_config_sdo( min((u32) ioctl->size, (u32) EC_MAX_SDO_DATA_SIZE)); ioctl->complete_access = req->complete_access; - up(&master->master_sem); + ec_lock_up(&master->master_sem); if (copy_to_user((void __user *) arg, ioctl, sizeof(*ioctl))) { kfree(ioctl); @@ -1456,14 +1456,14 @@ static ATTRIBUTES int ec_ioctl_config_idn( return -EFAULT; } - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { kfree(ioctl); return -EINTR; } if (!(sc = ec_master_get_config_const( master, ioctl->config_index))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Slave config %u does not exist!\n", ioctl->config_index); kfree(ioctl); @@ -1472,7 +1472,7 @@ static ATTRIBUTES int ec_ioctl_config_idn( if (!(req = ec_slave_config_get_idn_by_pos_const( sc, ioctl->idn_pos))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Invalid IDN position!\n"); kfree(ioctl); return -EINVAL; @@ -1485,7 +1485,7 @@ static ATTRIBUTES int ec_ioctl_config_idn( memcpy(ioctl->data, req->data, min((u32) ioctl->size, (u32) EC_MAX_IDN_DATA_SIZE)); - up(&master->master_sem); + ec_lock_up(&master->master_sem); if (copy_to_user((void __user *) arg, ioctl, sizeof(*ioctl))) { kfree(ioctl); @@ -1579,11 +1579,11 @@ static ATTRIBUTES int ec_ioctl_eoe_handler( return -EFAULT; } - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; if (!(eoe = ec_master_get_eoe_handler_const(master, data.eoe_index))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "EoE handler %u does not exist!\n", data.eoe_index); return -EINVAL; @@ -1603,7 +1603,7 @@ static ATTRIBUTES int ec_ioctl_eoe_handler( data.tx_queued_frames = eoe->tx_queued_frames; data.tx_queue_size = eoe->tx_queue_size; - up(&master->master_sem); + ec_lock_up(&master->master_sem); if (copy_to_user((void __user *) arg, &data, sizeof(data))) return -EFAULT; @@ -1652,13 +1652,13 @@ static ATTRIBUTES int ec_ioctl_slave_eoe_ip_param( req.state = EC_INT_REQUEST_QUEUED; - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { return -EINTR; } if (!(slave = ec_master_find_slave( master, 0, io.slave_position))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Slave %u does not exist!\n", io.slave_position); return -EINVAL; @@ -1669,20 +1669,20 @@ static ATTRIBUTES int ec_ioctl_slave_eoe_ip_param( // schedule request. list_add_tail(&req.list, &slave->eoe_requests); - up(&master->master_sem); + ec_lock_up(&master->master_sem); // wait for processing through FSM if (wait_event_interruptible(master->request_queue, req.state != EC_INT_REQUEST_QUEUED)) { // interrupted by signal - down(&master->master_sem); + ec_lock_down(&master->master_sem); if (req.state == EC_INT_REQUEST_QUEUED) { // abort request list_del(&req.list); - up(&master->master_sem); + ec_lock_up(&master->master_sem); return -EINTR; } - up(&master->master_sem); + ec_lock_up(&master->master_sem); } // wait until master FSM has finished processing @@ -1776,7 +1776,7 @@ static ATTRIBUTES int ec_ioctl_create_slave_config( data.config_index = 0; - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; list_for_each_entry(entry, &master->configs, list) { @@ -1785,7 +1785,7 @@ static ATTRIBUTES int ec_ioctl_create_slave_config( data.config_index++; } - up(&master->master_sem); + ec_lock_up(&master->master_sem); if (copy_to_user((void __user *) arg, &data, sizeof(data))) return -EFAULT; @@ -1814,7 +1814,7 @@ static ATTRIBUTES int ec_ioctl_select_ref_clock( goto out_return; } - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { ret = -EINTR; goto out_return; } @@ -1829,7 +1829,7 @@ static ATTRIBUTES int ec_ioctl_select_ref_clock( ecrt_master_select_reference_clock(master, sc); out_up: - up(&master->master_sem); + ec_lock_up(&master->master_sem); out_return: return ret; } @@ -1865,14 +1865,14 @@ static ATTRIBUTES int ec_ioctl_setup_domain_memory( ctx->process_data_size = 0; - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; list_for_each_entry(domain, &master->domains, list) { ctx->process_data_size += ecrt_domain_size(domain); } - up(&master->master_sem); + ec_lock_up(&master->master_sem); if (ctx->process_data_size) { ctx->process_data = vmalloc(ctx->process_data_size); @@ -1949,14 +1949,14 @@ static ATTRIBUTES int ec_ioctl_activate( ctx->process_data_size = 0; - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; list_for_each_entry(domain, &master->domains, list) { ctx->process_data_size += ecrt_domain_size(domain); } - up(&master->master_sem); + ec_lock_up(&master->master_sem); if (ctx->process_data_size) { ctx->process_data = vmalloc(ctx->process_data_size); @@ -2073,12 +2073,12 @@ static ATTRIBUTES int ec_ioctl_set_send_interval( return -EFAULT; } - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; ec_master_set_send_interval(master, send_interval); - up(&master->master_sem); + ec_lock_up(&master->master_sem); return 0; } @@ -2412,9 +2412,9 @@ static ATTRIBUTES int ec_ioctl_reset( ec_ioctl_context_t *ctx /**< Private data structure of file handle. */ ) { - down(&master->master_sem); + ec_lock_down(&master->master_sem); ecrt_master_reset(master); - up(&master->master_sem); + ec_lock_up(&master->master_sem); return 0; } @@ -2445,7 +2445,7 @@ static ATTRIBUTES int ec_ioctl_sc_sync( goto out_return; } - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { ret = -EINTR; goto out_return; } @@ -2466,7 +2466,7 @@ static ATTRIBUTES int ec_ioctl_sc_sync( } out_up: - up(&master->master_sem); + ec_lock_up(&master->master_sem); out_return: return ret; } @@ -2497,7 +2497,7 @@ static ATTRIBUTES int ec_ioctl_sc_watchdog( goto out_return; } - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { ret = -EINTR; goto out_return; } @@ -2511,7 +2511,7 @@ static ATTRIBUTES int ec_ioctl_sc_watchdog( data.watchdog_divider, data.watchdog_intervals); out_up: - up(&master->master_sem); + ec_lock_up(&master->master_sem); out_return: return ret; } @@ -2540,7 +2540,7 @@ static ATTRIBUTES int ec_ioctl_sc_allow_overlapping_pdos( goto out_return; } - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { ret = -EINTR; goto out_return; } @@ -2554,7 +2554,7 @@ static ATTRIBUTES int ec_ioctl_sc_allow_overlapping_pdos( data.allow_overlapping_pdos); out_up: - up(&master->master_sem); + ec_lock_up(&master->master_sem); out_return: return ret; } @@ -2579,15 +2579,15 @@ static ATTRIBUTES int ec_ioctl_sc_add_pdo( if (copy_from_user(&data, (void __user *) arg, sizeof(data))) return -EFAULT; - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; if (!(sc = ec_master_get_config(master, data.config_index))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); return -ENOENT; } - up(&master->master_sem); /** \todo sc could be invalidated */ + ec_lock_up(&master->master_sem); /** \todo sc could be invalidated */ return ecrt_slave_config_pdo_assign_add(sc, data.sync_index, data.index); } @@ -2613,15 +2613,15 @@ static ATTRIBUTES int ec_ioctl_sc_clear_pdos( if (copy_from_user(&data, (void __user *) arg, sizeof(data))) return -EFAULT; - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; if (!(sc = ec_master_get_config(master, data.config_index))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); return -ENOENT; } - up(&master->master_sem); /** \todo sc could be invalidated */ + ec_lock_up(&master->master_sem); /** \todo sc could be invalidated */ ecrt_slave_config_pdo_assign_clear(sc, data.sync_index); return 0; @@ -2648,15 +2648,15 @@ static ATTRIBUTES int ec_ioctl_sc_add_entry( if (copy_from_user(&data, (void __user *) arg, sizeof(data))) return -EFAULT; - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; if (!(sc = ec_master_get_config(master, data.config_index))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); return -ENOENT; } - up(&master->master_sem); /** \todo sc could be invalidated */ + ec_lock_up(&master->master_sem); /** \todo sc could be invalidated */ return ecrt_slave_config_pdo_mapping_add(sc, data.pdo_index, data.entry_index, data.entry_subindex, data.entry_bit_length); @@ -2683,15 +2683,15 @@ static ATTRIBUTES int ec_ioctl_sc_clear_entries( if (copy_from_user(&data, (void __user *) arg, sizeof(data))) return -EFAULT; - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; if (!(sc = ec_master_get_config(master, data.config_index))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); return -ENOENT; } - up(&master->master_sem); /** \todo sc could be invalidated */ + ec_lock_up(&master->master_sem); /** \todo sc could be invalidated */ ecrt_slave_config_pdo_mapping_clear(sc, data.index); return 0; @@ -2720,20 +2720,20 @@ static ATTRIBUTES int ec_ioctl_sc_reg_pdo_entry( if (copy_from_user(&data, (void __user *) arg, sizeof(data))) return -EFAULT; - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; if (!(sc = ec_master_get_config(master, data.config_index))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); return -ENOENT; } if (!(domain = ec_master_find_domain(master, data.domain_index))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); return -ENOENT; } - up(&master->master_sem); /** \todo sc or domain could be invalidated */ + ec_lock_up(&master->master_sem); /** \todo sc or domain could be invalidated */ ret = ecrt_slave_config_reg_pdo_entry(sc, data.entry_index, data.entry_subindex, domain, &data.bit_position); @@ -2769,21 +2769,21 @@ static ATTRIBUTES int ec_ioctl_sc_reg_pdo_pos( return -EFAULT; } - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { return -EINTR; } if (!(sc = ec_master_get_config(master, io.config_index))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); return -ENOENT; } if (!(domain = ec_master_find_domain(master, io.domain_index))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); return -ENOENT; } - up(&master->master_sem); /** \todo sc or domain could be invalidated */ + ec_lock_up(&master->master_sem); /** \todo sc or domain could be invalidated */ ret = ecrt_slave_config_reg_pdo_entry_pos(sc, io.sync_index, io.pdo_pos, io.entry_pos, domain, &io.bit_position); @@ -2815,11 +2815,11 @@ static ATTRIBUTES int ec_ioctl_sc_dc( if (copy_from_user(&data, (void __user *) arg, sizeof(data))) return -EFAULT; - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; if (!(sc = ec_master_get_config(master, data.config_index))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); return -ENOENT; } @@ -2829,7 +2829,7 @@ static ATTRIBUTES int ec_ioctl_sc_dc( data.dc_sync[1].cycle_time, data.dc_sync[1].shift_time); - up(&master->master_sem); + ec_lock_up(&master->master_sem); return 0; } @@ -2869,18 +2869,18 @@ static ATTRIBUTES int ec_ioctl_sc_sdo( return -EFAULT; } - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { kfree(sdo_data); return -EINTR; } if (!(sc = ec_master_get_config(master, data.config_index))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); kfree(sdo_data); return -ENOENT; } - up(&master->master_sem); /** \todo sc could be invalidated */ + ec_lock_up(&master->master_sem); /** \todo sc could be invalidated */ if (data.complete_access) { ret = ecrt_slave_config_complete_sdo(sc, @@ -2915,18 +2915,18 @@ static ATTRIBUTES int ec_ioctl_sc_emerg_size( if (copy_from_user(&io, (void __user *) arg, sizeof(io))) return -EFAULT; - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { return -EINTR; } if (!(sc = ec_master_get_config(master, io.config_index))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); return -ENOENT; } ret = ecrt_slave_config_emerg_size(sc, io.size); - up(&master->master_sem); + ec_lock_up(&master->master_sem); return ret; } @@ -3078,12 +3078,12 @@ static ATTRIBUTES int ec_ioctl_sc_create_sdo_request( data.request_index = 0; - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; sc = ec_master_get_config(master, data.config_index); if (!sc) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); return -ENOENT; } @@ -3091,7 +3091,7 @@ static ATTRIBUTES int ec_ioctl_sc_create_sdo_request( data.request_index++; } - up(&master->master_sem); /** \todo sc could be invalidated */ + ec_lock_up(&master->master_sem); /** \todo sc could be invalidated */ req = ecrt_slave_config_create_sdo_request_err(sc, data.sdo_index, data.sdo_subindex, data.size); @@ -3130,13 +3130,13 @@ static ATTRIBUTES int ec_ioctl_sc_create_reg_request( io.request_index = 0; - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { return -EINTR; } sc = ec_master_get_config(master, io.config_index); if (!sc) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); return -ENOENT; } @@ -3144,7 +3144,7 @@ static ATTRIBUTES int ec_ioctl_sc_create_reg_request( io.request_index++; } - up(&master->master_sem); /** \todo sc could be invalidated */ + ec_lock_up(&master->master_sem); /** \todo sc could be invalidated */ reg = ecrt_slave_config_create_reg_request_err(sc, io.mem_size); if (IS_ERR(reg)) { @@ -3183,12 +3183,12 @@ static ATTRIBUTES int ec_ioctl_sc_create_voe_handler( data.voe_index = 0; - if (down_interruptible(&master->master_sem)) + if (ec_lock_down_interruptible(&master->master_sem)) return -EINTR; sc = ec_master_get_config(master, data.config_index); if (!sc) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); return -ENOENT; } @@ -3196,7 +3196,7 @@ static ATTRIBUTES int ec_ioctl_sc_create_voe_handler( data.voe_index++; } - up(&master->master_sem); /** \todo sc could be invalidated */ + ec_lock_up(&master->master_sem); /** \todo sc could be invalidated */ voe = ecrt_slave_config_create_voe_handler_err(sc, data.size); if (IS_ERR(voe)) @@ -3281,18 +3281,18 @@ static ATTRIBUTES int ec_ioctl_sc_idn( return -EFAULT; } - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { kfree(data); return -EINTR; } if (!(sc = ec_master_get_config(master, ioctl.config_index))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); kfree(data); return -ENOENT; } - up(&master->master_sem); /** \todo sc could be invalidated */ + ec_lock_up(&master->master_sem); /** \todo sc could be invalidated */ ret = ecrt_slave_config_idn( sc, ioctl.drive_no, ioctl.idn, ioctl.al_state, data, ioctl.size); @@ -3374,19 +3374,19 @@ static ATTRIBUTES int ec_ioctl_domain_size( return -EPERM; } - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { return -EINTR; } list_for_each_entry(domain, &master->domains, list) { if (domain->index == (unsigned long) arg) { size_t size = ecrt_domain_size(domain); - up(&master->master_sem); + ec_lock_up(&master->master_sem); return size; } } - up(&master->master_sem); + ec_lock_up(&master->master_sem); return -ENOENT; } @@ -3408,19 +3408,19 @@ static ATTRIBUTES int ec_ioctl_domain_offset( if (unlikely(!ctx->requested)) return -EPERM; - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { return -EINTR; } list_for_each_entry(domain, &master->domains, list) { if (domain->index == (unsigned long) arg) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); return offset; } offset += ecrt_domain_size(domain); } - up(&master->master_sem); + ec_lock_up(&master->master_sem); return -ENOENT; } @@ -4275,13 +4275,13 @@ static ATTRIBUTES int ec_ioctl_slave_foe_read( ec_foe_request_read(&request); - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { ec_foe_request_clear(&request); return -EINTR; } if (!(slave = ec_master_find_slave(master, 0, io.slave_position))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); ec_foe_request_clear(&request); EC_MASTER_ERR(master, "Slave %u does not exist!\n", io.slave_position); @@ -4293,21 +4293,21 @@ static ATTRIBUTES int ec_ioctl_slave_foe_read( // schedule request. list_add_tail(&request.list, &slave->foe_requests); - up(&master->master_sem); + ec_lock_up(&master->master_sem); // wait for processing through FSM if (wait_event_interruptible(master->request_queue, request.state != EC_INT_REQUEST_QUEUED)) { // interrupted by signal - down(&master->master_sem); + ec_lock_down(&master->master_sem); if (request.state == EC_INT_REQUEST_QUEUED) { list_del(&request.list); - up(&master->master_sem); + ec_lock_up(&master->master_sem); ec_foe_request_clear(&request); return -EINTR; } // request already processing: interrupt not possible. - up(&master->master_sem); + ec_lock_up(&master->master_sem); } // wait until master FSM has finished processing @@ -4379,13 +4379,13 @@ static ATTRIBUTES int ec_ioctl_slave_foe_write( request.data_size = io.buffer_size; ec_foe_request_write(&request); - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { ec_foe_request_clear(&request); return -EINTR; } if (!(slave = ec_master_find_slave(master, 0, io.slave_position))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Slave %u does not exist!\n", io.slave_position); ec_foe_request_clear(&request); @@ -4397,21 +4397,21 @@ static ATTRIBUTES int ec_ioctl_slave_foe_write( // schedule FoE write request. list_add_tail(&request.list, &slave->foe_requests); - up(&master->master_sem); + ec_lock_up(&master->master_sem); // wait for processing through FSM if (wait_event_interruptible(master->request_queue, request.state != EC_INT_REQUEST_QUEUED)) { // interrupted by signal - down(&master->master_sem); + ec_lock_down(&master->master_sem); if (request.state == EC_INT_REQUEST_QUEUED) { // abort request list_del(&request.list); - up(&master->master_sem); + ec_lock_up(&master->master_sem); ec_foe_request_clear(&request); return -EINTR; } - up(&master->master_sem); + ec_lock_up(&master->master_sem); } // wait until master FSM has finished processing diff --git a/master/locks.h b/master/locks.h new file mode 100644 index 00000000..ecbe2ff1 --- /dev/null +++ b/master/locks.h @@ -0,0 +1,80 @@ +/****************************************************************************** + * + * $Id$ + * + * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH + * + * This file is part of the IgH EtherCAT Master. + * + * The IgH EtherCAT Master is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License version 2, as + * published by the Free Software Foundation. + * + * The IgH EtherCAT Master is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with the IgH EtherCAT Master; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + * --- + * + * The license mentioned above concerns the source code only. Using the + * EtherCAT technology and brand is only permitted in compliance with the + * industrial property and similar rights of Beckhoff Automation GmbH. + * + *****************************************************************************/ + +/** + \file + Abstract locks +*/ + +/*****************************************************************************/ + +#ifndef __EC_LOCKS_H__ +#define __EC_LOCKS_H__ + +#include "globals.h" + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27) +#include +#else +#include +#endif + +/*****************************************************************************/ + +#ifdef EC_USE_RTMUTEX + +#include + +typedef struct rt_mutex ec_lock_t; + +static inline void ec_lock_init(ec_lock_t *sem) { rt_mutex_init(sem); } +static inline void ec_lock_down(ec_lock_t *sem) { rt_mutex_lock(sem); } +#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 14, 34) +static inline int ec_lock_down_interruptible(ec_lock_t *sem) { return rt_mutex_lock_interruptible(sem); } +#else +static inline int ec_lock_down_interruptible(ec_lock_t *sem) { return rt_mutex_lock_interruptible(sem, 1); } +#endif +static inline void ec_lock_up(ec_lock_t *sem) { rt_mutex_unlock(sem); } + +#else + +typedef struct semaphore ec_lock_t; + +static inline void ec_lock_init(ec_lock_t *sem) { sema_init(sem, 1); } +static inline void ec_lock_down(ec_lock_t *sem) { down(sem); } +static inline int ec_lock_down_interruptible(ec_lock_t *sem) { return down_interruptible(sem); } +static inline void ec_lock_up(ec_lock_t *sem) { up(sem); } + +#endif + +/*****************************************************************************/ + +#endif + +/*****************************************************************************/ diff --git a/master/master.c b/master/master.c index dfe39081..db1e1082 100644 --- a/master/master.c +++ b/master/master.c @@ -153,7 +153,7 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */ master->index = index; master->reserved = 0; - sema_init(&master->master_sem, 1); + ec_lock_init(&master->master_sem); for (dev_idx = EC_DEVICE_MAIN; dev_idx < EC_MAX_NUM_DEVICES; dev_idx++) { master->macs[dev_idx] = NULL; @@ -172,7 +172,7 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */ ec_master_clear_device_stats(master); - sema_init(&master->device_sem, 1); + ec_lock_init(&master->device_sem); master->phase = EC_ORPHANED; master->active = 0; @@ -192,18 +192,18 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */ master->scan_busy = 0; master->allow_scan = 1; - sema_init(&master->scan_sem, 1); + ec_lock_init(&master->scan_sem); init_waitqueue_head(&master->scan_queue); master->config_busy = 0; - sema_init(&master->config_sem, 1); + ec_lock_init(&master->config_sem); init_waitqueue_head(&master->config_queue); INIT_LIST_HEAD(&master->datagram_queue); master->datagram_index = 0; INIT_LIST_HEAD(&master->ext_datagram_queue); - sema_init(&master->ext_queue_sem, 1); + ec_lock_init(&master->ext_queue_sem); master->ext_ring_idx_rt = 0; master->ext_ring_idx_fsm = 0; @@ -235,7 +235,7 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */ INIT_LIST_HEAD(&master->eoe_handlers); #endif - sema_init(&master->io_sem, 1); + ec_lock_init(&master->io_sem); master->send_cb = NULL; master->receive_cb = NULL; master->cb_data = NULL; @@ -548,10 +548,10 @@ void ec_master_clear_config( ec_master_t *master /**< EtherCAT master. */ ) { - down(&master->master_sem); + ec_lock_down(&master->master_sem); ec_master_clear_domains(master); ec_master_clear_slave_configs(master); - up(&master->master_sem); + ec_lock_up(&master->master_sem); } /*****************************************************************************/ @@ -563,9 +563,9 @@ void ec_master_internal_send_cb( ) { ec_master_t *master = (ec_master_t *) cb_data; - down(&master->io_sem); + ec_lock_down(&master->io_sem); ecrt_master_send_ext(master); - up(&master->io_sem); + ec_lock_up(&master->io_sem); } /*****************************************************************************/ @@ -577,9 +577,9 @@ void ec_master_internal_receive_cb( ) { ec_master_t *master = (ec_master_t *) cb_data; - down(&master->io_sem); + ec_lock_down(&master->io_sem); ecrt_master_receive(master); - up(&master->io_sem); + ec_lock_up(&master->io_sem); } /*****************************************************************************/ @@ -688,9 +688,9 @@ void ec_master_leave_idle_phase(ec_master_t *master /**< EtherCAT master */) #endif ec_master_thread_stop(master); - down(&master->master_sem); + ec_lock_down(&master->master_sem); ec_master_clear_slaves(master); - up(&master->master_sem); + ec_lock_up(&master->master_sem); ec_fsm_master_reset(&master->fsm); } @@ -710,9 +710,9 @@ int ec_master_enter_operation_phase( EC_MASTER_DBG(master, 1, "IDLE -> OPERATION.\n"); - down(&master->config_sem); + ec_lock_down(&master->config_sem); if (master->config_busy) { - up(&master->config_sem); + ec_lock_up(&master->config_sem); // wait for slave configuration to complete ret = wait_event_interruptible(master->config_queue, @@ -726,15 +726,15 @@ int ec_master_enter_operation_phase( EC_MASTER_DBG(master, 1, "Waiting for pending slave" " configuration returned.\n"); } else { - up(&master->config_sem); + ec_lock_up(&master->config_sem); } - down(&master->scan_sem); + ec_lock_down(&master->scan_sem); master->allow_scan = 0; // 'lock' the slave list if (!master->scan_busy) { - up(&master->scan_sem); + ec_lock_up(&master->scan_sem); } else { - up(&master->scan_sem); + ec_lock_up(&master->scan_sem); // wait for slave scan to complete ret = wait_event_interruptible(master->scan_queue, @@ -982,9 +982,9 @@ void ec_master_queue_datagram_ext( ec_datagram_t *datagram /**< datagram */ ) { - down(&master->ext_queue_sem); + ec_lock_down(&master->ext_queue_sem); list_add_tail(&datagram->queue, &master->ext_datagram_queue); - up(&master->ext_queue_sem); + ec_lock_up(&master->ext_queue_sem); } /*****************************************************************************/ @@ -1592,12 +1592,12 @@ static int ec_master_idle_thread(void *priv_data) ec_datagram_output_stats(&master->fsm_datagram); // receive - down(&master->io_sem); + ec_lock_down(&master->io_sem); ecrt_master_receive(master); - up(&master->io_sem); + ec_lock_up(&master->io_sem); // execute master & slave state machines - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { break; } @@ -1605,15 +1605,15 @@ static int ec_master_idle_thread(void *priv_data) ec_master_exec_slave_fsms(master); - up(&master->master_sem); + ec_lock_up(&master->master_sem); // queue and send - down(&master->io_sem); + ec_lock_down(&master->io_sem); if (fsm_exec) { ec_master_queue_datagram(master, &master->fsm_datagram); } sent_bytes = ecrt_master_send(master); - up(&master->io_sem); + ec_lock_up(&master->io_sem); if (ec_fsm_master_idle(&master->fsm)) { #ifdef EC_USE_HRTIMER @@ -1657,7 +1657,7 @@ static int ec_master_operation_thread(void *priv_data) ec_master_output_stats(master); // execute master & slave state machines - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { break; } @@ -1669,7 +1669,7 @@ static int ec_master_operation_thread(void *priv_data) ec_master_exec_slave_fsms(master); - up(&master->master_sem); + ec_lock_up(&master->master_sem); } #ifdef EC_USE_HRTIMER @@ -1802,9 +1802,9 @@ static int ec_master_eoe_thread(void *priv_data) ec_eoe_queue(eoe); } // (try to) send datagrams - down(&master->ext_queue_sem); + ec_lock_down(&master->ext_queue_sem); master->send_cb(master->cb_data); - up(&master->ext_queue_sem); + ec_lock_up(&master->ext_queue_sem); } schedule: @@ -2331,7 +2331,7 @@ ec_domain_t *ecrt_master_create_domain_err( return ERR_PTR(-ENOMEM); } - down(&master->master_sem); + ec_lock_down(&master->master_sem); if (list_empty(&master->domains)) { index = 0; @@ -2343,7 +2343,7 @@ ec_domain_t *ecrt_master_create_domain_err( ec_domain_init(domain, master, index); list_add_tail(&domain->list, &master->domains); - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_DBG(master, 1, "Created domain %u.\n", domain->index); @@ -2386,21 +2386,21 @@ int ecrt_master_activate(ec_master_t *master) return 0; } - down(&master->master_sem); + ec_lock_down(&master->master_sem); // finish all domains domain_offset = 0; list_for_each_entry(domain, &master->domains, list) { ret = ec_domain_finish(domain, domain_offset); if (ret < 0) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Failed to finish domain 0x%p!\n", domain); return ret; } domain_offset += domain->data_size; } - up(&master->master_sem); + ec_lock_up(&master->master_sem); // restart EoE process and master thread with new locking @@ -2726,14 +2726,14 @@ ec_slave_config_t *ecrt_master_slave_config_err(ec_master_t *master, ec_slave_config_init(sc, master, alias, position, vendor_id, product_code); - down(&master->master_sem); + ec_lock_down(&master->master_sem); // try to find the addressed slave ec_slave_config_attach(sc); ec_slave_config_load_default_sync_config(sc); list_add_tail(&sc->list, &master->configs); - up(&master->master_sem); + ec_lock_up(&master->master_sem); } return sc; @@ -2797,7 +2797,7 @@ int ecrt_master_get_slave(ec_master_t *master, uint16_t slave_position, unsigned int i; int ret = 0; - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { return -EINTR; } @@ -2845,7 +2845,7 @@ int ecrt_master_get_slave(ec_master_t *master, uint16_t slave_position, } out_get_slave: - up(&master->master_sem); + ec_lock_up(&master->master_sem); return ret; } @@ -3055,13 +3055,13 @@ int ecrt_master_sdo_download(ec_master_t *master, uint16_t slave_position, request.data_size = data_size; ecrt_sdo_request_write(&request); - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { ec_sdo_request_clear(&request); return -EINTR; } if (!(slave = ec_master_find_slave(master, 0, slave_position))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position); ec_sdo_request_clear(&request); return -EINVAL; @@ -3072,21 +3072,21 @@ int ecrt_master_sdo_download(ec_master_t *master, uint16_t slave_position, // schedule request. list_add_tail(&request.list, &slave->sdo_requests); - up(&master->master_sem); + ec_lock_up(&master->master_sem); // wait for processing through FSM if (wait_event_interruptible(master->request_queue, request.state != EC_INT_REQUEST_QUEUED)) { // interrupted by signal - down(&master->master_sem); + ec_lock_down(&master->master_sem); if (request.state == EC_INT_REQUEST_QUEUED) { list_del(&request.list); - up(&master->master_sem); + ec_lock_up(&master->master_sem); ec_sdo_request_clear(&request); return -EINTR; } // request already processing: interrupt not possible. - up(&master->master_sem); + ec_lock_up(&master->master_sem); } // wait until master FSM has finished processing @@ -3140,13 +3140,13 @@ int ecrt_master_sdo_download_complete(ec_master_t *master, request.data_size = data_size; ecrt_sdo_request_write(&request); - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { ec_sdo_request_clear(&request); return -EINTR; } if (!(slave = ec_master_find_slave(master, 0, slave_position))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position); ec_sdo_request_clear(&request); return -EINVAL; @@ -3158,21 +3158,21 @@ int ecrt_master_sdo_download_complete(ec_master_t *master, // schedule request. list_add_tail(&request.list, &slave->sdo_requests); - up(&master->master_sem); + ec_lock_up(&master->master_sem); // wait for processing through FSM if (wait_event_interruptible(master->request_queue, request.state != EC_INT_REQUEST_QUEUED)) { // interrupted by signal - down(&master->master_sem); + ec_lock_down(&master->master_sem); if (request.state == EC_INT_REQUEST_QUEUED) { list_del(&request.list); - up(&master->master_sem); + ec_lock_up(&master->master_sem); ec_sdo_request_clear(&request); return -EINTR; } // request already processing: interrupt not possible. - up(&master->master_sem); + ec_lock_up(&master->master_sem); } // wait until master FSM has finished processing @@ -3213,13 +3213,13 @@ int ecrt_master_sdo_upload(ec_master_t *master, uint16_t slave_position, ecrt_sdo_request_index(&request, index, subindex); ecrt_sdo_request_read(&request); - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { ec_sdo_request_clear(&request); return -EINTR; } if (!(slave = ec_master_find_slave(master, 0, slave_position))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); ec_sdo_request_clear(&request); EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position); return -EINVAL; @@ -3230,21 +3230,21 @@ int ecrt_master_sdo_upload(ec_master_t *master, uint16_t slave_position, // schedule request. list_add_tail(&request.list, &slave->sdo_requests); - up(&master->master_sem); + ec_lock_up(&master->master_sem); // wait for processing through FSM if (wait_event_interruptible(master->request_queue, request.state != EC_INT_REQUEST_QUEUED)) { // interrupted by signal - down(&master->master_sem); + ec_lock_down(&master->master_sem); if (request.state == EC_INT_REQUEST_QUEUED) { list_del(&request.list); - up(&master->master_sem); + ec_lock_up(&master->master_sem); ec_sdo_request_clear(&request); return -EINTR; } // request already processing: interrupt not possible. - up(&master->master_sem); + ec_lock_up(&master->master_sem); } // wait until master FSM has finished processing @@ -3304,13 +3304,13 @@ int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position, request.data_size = data_size; ec_soe_request_write(&request); - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { ec_soe_request_clear(&request); return -EINTR; } if (!(slave = ec_master_find_slave(master, 0, slave_position))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position); ec_soe_request_clear(&request); @@ -3322,21 +3322,21 @@ int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position, // schedule SoE write request. list_add_tail(&request.list, &slave->soe_requests); - up(&master->master_sem); + ec_lock_up(&master->master_sem); // wait for processing through FSM if (wait_event_interruptible(master->request_queue, request.state != EC_INT_REQUEST_QUEUED)) { // interrupted by signal - down(&master->master_sem); + ec_lock_down(&master->master_sem); if (request.state == EC_INT_REQUEST_QUEUED) { // abort request list_del(&request.list); - up(&master->master_sem); + ec_lock_up(&master->master_sem); ec_soe_request_clear(&request); return -EINTR; } - up(&master->master_sem); + ec_lock_up(&master->master_sem); } // wait until master FSM has finished processing @@ -3371,13 +3371,13 @@ int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position, ec_soe_request_set_idn(&request, idn); ec_soe_request_read(&request); - if (down_interruptible(&master->master_sem)) { + if (ec_lock_down_interruptible(&master->master_sem)) { ec_soe_request_clear(&request); return -EINTR; } if (!(slave = ec_master_find_slave(master, 0, slave_position))) { - up(&master->master_sem); + ec_lock_up(&master->master_sem); ec_soe_request_clear(&request); EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position); return -EINVAL; @@ -3388,21 +3388,21 @@ int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position, // schedule request. list_add_tail(&request.list, &slave->soe_requests); - up(&master->master_sem); + ec_lock_up(&master->master_sem); // wait for processing through FSM if (wait_event_interruptible(master->request_queue, request.state != EC_INT_REQUEST_QUEUED)) { // interrupted by signal - down(&master->master_sem); + ec_lock_down(&master->master_sem); if (request.state == EC_INT_REQUEST_QUEUED) { list_del(&request.list); - up(&master->master_sem); + ec_lock_up(&master->master_sem); ec_soe_request_clear(&request); return -EINTR; } // request already processing: interrupt not possible. - up(&master->master_sem); + ec_lock_up(&master->master_sem); } // wait until master FSM has finished processing diff --git a/master/master.h b/master/master.h index 5f421e1a..89debd1f 100644 --- a/master/master.h +++ b/master/master.h @@ -43,16 +43,11 @@ #include #include -#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27) -#include -#else -#include -#endif - #include "device.h" #include "domain.h" #include "ethernet.h" #include "fsm_master.h" +#include "locks.h" #include "cdev.h" #ifdef EC_RTDM @@ -206,7 +201,7 @@ struct ec_master { ec_rtdm_dev_t rtdm_dev; /**< RTDM device. */ #endif - struct semaphore master_sem; /**< Master semaphore. */ + ec_lock_t master_sem; /**< Master semaphore. */ ec_device_t devices[EC_MAX_NUM_DEVICES]; /**< EtherCAT devices. */ const uint8_t *macs[EC_MAX_NUM_DEVICES]; /**< Device MAC addresses. */ @@ -215,7 +210,7 @@ struct ec_master { ec_master_num_devices(), because it may be optimized! */ #endif - struct semaphore device_sem; /**< Device semaphore. */ + ec_lock_t device_sem; /**< Device semaphore. */ ec_device_stats_t device_stats; /**< Device statistics. */ ec_fsm_master_t fsm; /**< Master state machine. */ @@ -252,13 +247,13 @@ struct ec_master { unsigned int scan_busy; /**< Current scan state. */ unsigned int allow_scan; /**< \a True, if slave scanning is allowed. */ - struct semaphore scan_sem; /**< Semaphore protecting the \a scan_busy + ec_lock_t scan_sem; /**< Semaphore protecting the \a scan_busy variable and the \a allow_scan flag. */ wait_queue_head_t scan_queue; /**< Queue for processes that wait for slave scanning. */ unsigned int config_busy; /**< State of slave configuration. */ - struct semaphore config_sem; /**< Semaphore protecting the \a config_busy + ec_lock_t config_sem; /**< Semaphore protecting the \a config_busy variable and the allow_config flag. */ wait_queue_head_t config_queue; /**< Queue for processes that wait for slave configuration. */ @@ -268,7 +263,7 @@ struct ec_master { struct list_head ext_datagram_queue; /**< Queue for non-application datagrams. */ - struct semaphore ext_queue_sem; /**< Semaphore protecting the \a + ec_lock_t ext_queue_sem; /**< Semaphore protecting the \a ext_datagram_queue. */ ec_datagram_t ext_datagram_ring[EC_EXT_RING_SIZE]; /**< External datagram @@ -295,7 +290,7 @@ struct ec_master { struct list_head eoe_handlers; /**< Ethernet over EtherCAT handlers. */ #endif - struct semaphore io_sem; /**< Semaphore used in \a IDLE phase. */ + ec_lock_t io_sem; /**< Semaphore used in \a IDLE phase. */ void (*send_cb)(void *); /**< Current send datagrams callback. */ void (*receive_cb)(void *); /**< Current receive datagrams callback. */ diff --git a/master/module.c b/master/module.c index 250f6121..3d818480 100644 --- a/master/module.c +++ b/master/module.c @@ -61,7 +61,7 @@ static unsigned int backup_count; /**< Number of backup devices. */ static unsigned int debug_level; /**< Debug level parameter. */ static ec_master_t *masters; /**< Array of masters. */ -static struct semaphore master_sem; /**< Master semaphore. */ +static ec_lock_t master_sem; /**< Master semaphore. */ dev_t device_number; /**< Device number for master cdevs. */ struct class *class; /**< Device class. */ @@ -101,7 +101,7 @@ int __init ec_init_module(void) EC_INFO("Master driver %s\n", EC_MASTER_VERSION); - sema_init(&master_sem, 1); + ec_lock_init(&master_sem); if (master_count) { if (alloc_chrdev_region(&device_number, @@ -484,7 +484,7 @@ ec_device_t *ecdev_offer( master = &masters[i]; ec_mac_print(net_dev->dev_addr, str); - if (down_interruptible(&master->device_sem)) { + if (ec_lock_down_interruptible(&master->device_sem)) { EC_MASTER_WARN(master, "%s() interrupted!\n", __func__); return NULL; } @@ -500,7 +500,7 @@ ec_device_t *ecdev_offer( ec_device_attach(&master->devices[dev_idx], net_dev, poll, module); - up(&master->device_sem); + ec_lock_up(&master->device_sem); snprintf(net_dev->name, IFNAMSIZ, "ec%c%u", ec_device_names[dev_idx != 0][0], master->index); @@ -509,7 +509,7 @@ ec_device_t *ecdev_offer( } } - up(&master->device_sem); + ec_lock_up(&master->device_sem); EC_MASTER_DBG(master, 1, "Master declined device %s.\n", str); } @@ -543,27 +543,27 @@ ec_master_t *ecrt_request_master_err( } master = &masters[master_index]; - if (down_interruptible(&master_sem)) { + if (ec_lock_down_interruptible(&master_sem)) { errptr = ERR_PTR(-EINTR); goto out_return; } if (master->reserved) { - up(&master_sem); + ec_lock_up(&master_sem); EC_MASTER_ERR(master, "Master already in use!\n"); errptr = ERR_PTR(-EBUSY); goto out_return; } master->reserved = 1; - up(&master_sem); + ec_lock_up(&master_sem); - if (down_interruptible(&master->device_sem)) { + if (ec_lock_down_interruptible(&master->device_sem)) { errptr = ERR_PTR(-EINTR); goto out_release; } if (master->phase != EC_IDLE) { - up(&master->device_sem); + ec_lock_up(&master->device_sem); EC_MASTER_ERR(master, "Master still waiting for devices!\n"); errptr = ERR_PTR(-ENODEV); goto out_release; @@ -572,14 +572,14 @@ ec_master_t *ecrt_request_master_err( for (; dev_idx < ec_master_num_devices(master); dev_idx++) { ec_device_t *device = &master->devices[dev_idx]; if (!try_module_get(device->module)) { - up(&master->device_sem); + ec_lock_up(&master->device_sem); EC_MASTER_ERR(master, "Device module is unloading!\n"); errptr = ERR_PTR(-ENODEV); goto out_module_put; } } - up(&master->device_sem); + ec_lock_up(&master->device_sem); if (ec_master_enter_operation_phase(master)) { EC_MASTER_ERR(master, "Failed to enter OPERATION phase!\n"); diff --git a/master/slave_config.c b/master/slave_config.c index cf88a637..35bb951c 100755 --- a/master/slave_config.c +++ b/master/slave_config.c @@ -202,7 +202,7 @@ int ec_slave_config_prepare_fmmu( fmmu = &sc->fmmu_configs[sc->used_fmmus]; - down(&sc->master->master_sem); + ec_lock_down(&sc->master->master_sem); ec_fmmu_config_init(fmmu, sc, domain, sync_index, dir); #if 0 //TODO overlapping PDOs @@ -235,7 +235,7 @@ int ec_slave_config_prepare_fmmu( #endif sc->used_fmmus++; - up(&sc->master->master_sem); + ec_lock_up(&sc->master->master_sem); return fmmu->logical_domain_offset; } @@ -731,18 +731,18 @@ int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc, return -EINVAL; } - down(&sc->master->master_sem); + ec_lock_down(&sc->master->master_sem); pdo = ec_pdo_list_add_pdo(&sc->sync_configs[sync_index].pdos, pdo_index); if (IS_ERR(pdo)) { - up(&sc->master->master_sem); + ec_lock_up(&sc->master->master_sem); return PTR_ERR(pdo); } pdo->sync_index = sync_index; ec_slave_config_load_default_mapping(sc, pdo); - up(&sc->master->master_sem); + ec_lock_up(&sc->master->master_sem); return 0; } @@ -759,9 +759,9 @@ void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc, return; } - down(&sc->master->master_sem); + ec_lock_down(&sc->master->master_sem); ec_pdo_list_clear_pdos(&sc->sync_configs[sync_index].pdos); - up(&sc->master->master_sem); + ec_lock_up(&sc->master->master_sem); } /*****************************************************************************/ @@ -787,10 +787,10 @@ int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc, break; if (pdo) { - down(&sc->master->master_sem); + ec_lock_down(&sc->master->master_sem); entry = ec_pdo_add_entry(pdo, entry_index, entry_subindex, entry_bit_length); - up(&sc->master->master_sem); + ec_lock_up(&sc->master->master_sem); if (IS_ERR(entry)) retval = PTR_ERR(entry); } else { @@ -818,9 +818,9 @@ void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc, break; if (pdo) { - down(&sc->master->master_sem); + ec_lock_down(&sc->master->master_sem); ec_pdo_clear_entries(pdo); - up(&sc->master->master_sem); + ec_lock_up(&sc->master->master_sem); } else { EC_CONFIG_WARN(sc, "PDO 0x%04X is not assigned.\n", pdo_index); } @@ -1066,9 +1066,9 @@ int ecrt_slave_config_sdo(ec_slave_config_t *sc, uint16_t index, return ret; } - down(&sc->master->master_sem); + ec_lock_down(&sc->master->master_sem); list_add_tail(&req->list, &sc->sdo_configs); - up(&sc->master->master_sem); + ec_lock_up(&sc->master->master_sem); return 0; } @@ -1151,9 +1151,9 @@ int ecrt_slave_config_complete_sdo(ec_slave_config_t *sc, uint16_t index, return ret; } - down(&sc->master->master_sem); + ec_lock_down(&sc->master->master_sem); list_add_tail(&req->list, &sc->sdo_configs); - up(&sc->master->master_sem); + ec_lock_up(&sc->master->master_sem); return 0; } @@ -1220,9 +1220,9 @@ ec_sdo_request_t *ecrt_slave_config_create_sdo_request_err( memset(req->data, 0x00, size); req->data_size = size; - down(&sc->master->master_sem); + ec_lock_down(&sc->master->master_sem); list_add_tail(&req->list, &sc->sdo_requests); - up(&sc->master->master_sem); + ec_lock_up(&sc->master->master_sem); return req; } @@ -1263,9 +1263,9 @@ ec_reg_request_t *ecrt_slave_config_create_reg_request_err( return ERR_PTR(ret); } - down(&sc->master->master_sem); + ec_lock_down(&sc->master->master_sem); list_add_tail(®->list, &sc->reg_requests); - up(&sc->master->master_sem); + ec_lock_up(&sc->master->master_sem); return reg; } @@ -1305,9 +1305,9 @@ ec_voe_handler_t *ecrt_slave_config_create_voe_handler_err( return ERR_PTR(ret); } - down(&sc->master->master_sem); + ec_lock_down(&sc->master->master_sem); list_add_tail(&voe->list, &sc->voe_handlers); - up(&sc->master->master_sem); + ec_lock_up(&sc->master->master_sem); return voe; } @@ -1388,9 +1388,9 @@ int ecrt_slave_config_idn(ec_slave_config_t *sc, uint8_t drive_no, return ret; } - down(&sc->master->master_sem); + ec_lock_down(&sc->master->master_sem); list_add_tail(&req->list, &sc->soe_configs); - up(&sc->master->master_sem); + ec_lock_up(&sc->master->master_sem); return 0; } From d1e3d2e0a32a0307ddd774c210cb3ecea2ab9b4f Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 23 Jun 2022 11:22:08 +0200 Subject: [PATCH 22/25] Removed duplicate pre-commit rule. --- .pre-commit-config.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 77fcc5b0..7bccd120 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -11,4 +11,3 @@ repos: - id: end-of-file-fixer - id: mixed-line-ending - id: trailing-whitespace - - id: trailing-whitespace From cef602586cd3b7acbd944dfd4b4ab69801c7f6f6 Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 23 Jun 2022 11:26:20 +0200 Subject: [PATCH 23/25] Applied 0017-Master-locks-to-avoid-corrupted-datagram-queue.patch --- master/ioctl.c | 43 +++++++++++++++++++++++++++++++++++++++---- 1 file changed, 39 insertions(+), 4 deletions(-) diff --git a/master/ioctl.c b/master/ioctl.c index afd8e902..64c411f7 100755 --- a/master/ioctl.c +++ b/master/ioctl.c @@ -57,6 +57,16 @@ #define ATTRIBUTES #endif +/** Ioctl locking is disabled for RTDM as the RT app needs to use RTAI locks. + */ +#ifdef EC_IOCTL_RTDM +#define ec_ioctl_lock_down_interruptible(p) 0 +#define ec_ioctl_lock_up(p) do {} while (0) +#else +#define ec_ioctl_lock_down_interruptible(p) ec_lock_down_interruptible(p) +#define ec_ioctl_lock_up(p) ec_lock_up(p) +#endif + /*****************************************************************************/ /** Copies a string to an ioctl structure. @@ -2100,8 +2110,15 @@ static ATTRIBUTES int ec_ioctl_send( return -EPERM; } + /* Locking added as send is likely to be used by more than + one application tasks */ + if (ec_ioctl_lock_down_interruptible(&master->master_sem)) + return -EINTR; + sent_bytes = ecrt_master_send(master); + ec_ioctl_lock_up(&master->master_sem); + if (copy_to_user((void __user *) arg, &sent_bytes, sizeof(sent_bytes))) { return -EFAULT; } @@ -2125,7 +2142,14 @@ static ATTRIBUTES int ec_ioctl_receive( return -EPERM; } + /* Locking added as receive is likely to be used by more than + one application tasks */ + if (ec_ioctl_lock_down_interruptible(&master->master_sem)) + return -EINTR; + ecrt_master_receive(master); + + ec_ioctl_lock_up(&master->master_sem); return 0; } @@ -3441,14 +3465,19 @@ static ATTRIBUTES int ec_ioctl_domain_process( if (unlikely(!ctx->requested)) return -EPERM; - /* no locking of master_sem needed, because domain will not be deleted in - * the meantime. */ + /* Locking added as domain processing is likely to be used by more than + one application tasks */ + if (ec_ioctl_lock_down_interruptible(&master->master_sem)) { + return -EINTR; + } if (!(domain = ec_master_find_domain(master, (unsigned long) arg))) { + ec_ioctl_lock_up(&master->master_sem); return -ENOENT; } ecrt_domain_process(domain); + ec_ioctl_lock_up(&master->master_sem); return 0; } @@ -3469,14 +3498,20 @@ static ATTRIBUTES int ec_ioctl_domain_queue( if (unlikely(!ctx->requested)) return -EPERM; - /* no locking of master_sem needed, because domain will not be deleted in - * the meantime. */ + /* Locking added as domain queing is likely to be used by more than + one application tasks */ + if (ec_ioctl_lock_down_interruptible(&master->master_sem)) + return -EINTR; if (!(domain = ec_master_find_domain(master, (unsigned long) arg))) { + ec_ioctl_lock_up(&master->master_sem); return -ENOENT; } ecrt_domain_queue(domain); + + ec_ioctl_lock_up(&master->master_sem); + return 0; } From 429ba9356e4afe3b1116c79a715f9f2614d971b0 Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 23 Jun 2022 11:27:28 +0200 Subject: [PATCH 24/25] Applied 0018-Use-call-back-functions.patch --- master/ioctl.c | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/master/ioctl.c b/master/ioctl.c index 64c411f7..bddf77ba 100755 --- a/master/ioctl.c +++ b/master/ioctl.c @@ -2115,7 +2115,11 @@ static ATTRIBUTES int ec_ioctl_send( if (ec_ioctl_lock_down_interruptible(&master->master_sem)) return -EINTR; - sent_bytes = ecrt_master_send(master); + if (master->send_cb != NULL) { + master->send_cb(master->cb_data); + sent_bytes = 0; + } else + sent_bytes = ecrt_master_send(master); ec_ioctl_lock_up(&master->master_sem); @@ -2147,7 +2151,10 @@ static ATTRIBUTES int ec_ioctl_receive( if (ec_ioctl_lock_down_interruptible(&master->master_sem)) return -EINTR; - ecrt_master_receive(master); + if (master->receive_cb != NULL) + master->receive_cb(master->cb_data); + else + ecrt_master_receive(master); ec_ioctl_lock_up(&master->master_sem); return 0; From f943348d986426d698c8d3ccb00942a8fb86d1ac Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 23 Jun 2022 11:54:10 +0200 Subject: [PATCH 25/25] Applied 0019-Support-for-multiple-mailbox-protocols.patch --- master/datagram.c | 104 +++++++ master/datagram.h | 18 +- master/ethernet.c | 66 +++- master/fsm_coe.c | 617 +++++++++++++++++++++++++++++++------- master/fsm_eoe.c | 82 ++++- master/fsm_foe.c | 182 +++++++++-- master/fsm_slave_config.c | 7 + master/fsm_slave_scan.c | 3 + master/fsm_soe.c | 184 ++++++++++-- master/mailbox.c | 19 +- master/mailbox.h | 2 +- master/master.c | 98 +++++- master/slave.c | 53 ++++ master/slave.h | 15 + master/voe_handler.c | 95 +++++- 15 files changed, 1325 insertions(+), 220 deletions(-) diff --git a/master/datagram.c b/master/datagram.c index da508509..e051ac8b 100644 --- a/master/datagram.c +++ b/master/datagram.c @@ -587,6 +587,9 @@ void ec_datagram_print_state( case EC_DATAGRAM_ERROR: printk("error"); break; + case EC_DATAGRAM_INVALID: + printk(KERN_CONT "invalid"); + break; default: printk("???"); } @@ -648,3 +651,104 @@ const char *ec_datagram_type_string( } /*****************************************************************************/ + +/** Initialize mailbox response data. + * + */ +void ec_mbox_data_init( + ec_mbox_data_t *mbox_data /**< Mailbox response data. */ + ) +{ + mbox_data->data = NULL; + mbox_data->data_size = 0; + mbox_data->payload_size = 0; +} + + +/*****************************************************************************/ + +/** Free internal memory for mailbox response data. + * + */ +void ec_mbox_data_clear( + ec_mbox_data_t *mbox_data /**< Mailbox response data. */ + ) +{ + if (mbox_data->data) { + kfree(mbox_data->data); + mbox_data->data = NULL; + mbox_data->data_size = 0; + } +} + + +/*****************************************************************************/ + +/** Allocates internal memory for mailbox response data. + * + * \return 0 in case of success, otherwise \a -ENOMEM. + */ +int ec_mbox_data_prealloc( + ec_mbox_data_t *mbox_data, /**< Mailbox response data. */ + size_t size /**< Mailbox size in bytes. */ + ) +{ + if (mbox_data->data) { + kfree(mbox_data->data); + mbox_data->data = NULL; + mbox_data->data_size = 0; + } + + if (!(mbox_data->data = kmalloc(size, GFP_KERNEL))) { + EC_ERR("Failed to allocate %zu bytes of mailbox data memory!\n", size); + return -ENOMEM; + } + mbox_data->data_size = size; + return 0; +} + + +/*****************************************************************************/ + +/** Allocates internal memory for mailbox response data for all slave + * supported mailbox protocols . + * + */ +void ec_mbox_prot_data_prealloc( + ec_slave_t *slave, /**< EtherCAT slave. */ + uint16_t protocols, /**< Supported protocols. */ + size_t size /**< Mailbox size in bytes. */ + ) +{ + if ((size > 0) && (size <= EC_MAX_DATA_SIZE)) { +#ifdef EC_EOE + if (protocols & EC_MBOX_EOE) { + ec_mbox_data_prealloc(&slave->mbox_eoe_data, size); + } else { + ec_mbox_data_clear(&slave->mbox_eoe_data); + } +#endif + if (protocols & EC_MBOX_COE) { + ec_mbox_data_prealloc(&slave->mbox_coe_data, size); + } else { + ec_mbox_data_clear(&slave->mbox_coe_data); + } + if (protocols & EC_MBOX_FOE) { + ec_mbox_data_prealloc(&slave->mbox_foe_data, size); + } else { + ec_mbox_data_clear(&slave->mbox_foe_data); + } + if (protocols & EC_MBOX_SOE) { + ec_mbox_data_prealloc(&slave->mbox_soe_data, size); + } else { + ec_mbox_data_clear(&slave->mbox_soe_data); + } + if (protocols & EC_MBOX_VOE) { + ec_mbox_data_prealloc(&slave->mbox_voe_data, size); + } else { + ec_mbox_data_clear(&slave->mbox_voe_data); + } + } +} + +/*****************************************************************************/ diff --git a/master/datagram.h b/master/datagram.h index 96af32fa..eea19a37 100644 --- a/master/datagram.h +++ b/master/datagram.h @@ -77,7 +77,8 @@ typedef enum { EC_DATAGRAM_SENT, /**< Sent (still in the queue). */ EC_DATAGRAM_RECEIVED, /**< Received (dequeued). */ EC_DATAGRAM_TIMED_OUT, /**< Timed out (dequeued). */ - EC_DATAGRAM_ERROR /**< Error while sending/receiving (dequeued). */ + EC_DATAGRAM_ERROR, /**< Error while sending/receiving (dequeued). */ + EC_DATAGRAM_INVALID /**< Unused and should not be queued (dequeued). */ } ec_datagram_state_t; /*****************************************************************************/ @@ -113,6 +114,17 @@ typedef struct { char name[EC_DATAGRAM_NAME_SIZE]; /**< Description of the datagram. */ } ec_datagram_t; + +/*****************************************************************************/ + +/** EtherCAT mailbox response data. + */ +typedef struct { + uint8_t *data; /**< Mailbox response data. */ + size_t data_size; /**< Size of the mailbox response data buffer. */ + size_t payload_size; /**< Size of the mailbox response payload data. */ +} ec_mbox_data_t; + /*****************************************************************************/ void ec_datagram_init(ec_datagram_t *); @@ -144,6 +156,10 @@ void ec_datagram_print_wc_error(const ec_datagram_t *); void ec_datagram_output_stats(ec_datagram_t *); const char *ec_datagram_type_string(const ec_datagram_t *); +void ec_mbox_data_init(ec_mbox_data_t *); +void ec_mbox_data_clear(ec_mbox_data_t *); +void ec_mbox_prot_data_prealloc(ec_slave_t *, uint16_t, size_t); + /*****************************************************************************/ #endif diff --git a/master/ethernet.c b/master/ethernet.c index f51dc92e..b70094e8 100644 --- a/master/ethernet.c +++ b/master/ethernet.c @@ -71,6 +71,7 @@ void ec_eoe_flush(ec_eoe_t *); void ec_eoe_state_rx_start(ec_eoe_t *); void ec_eoe_state_rx_check(ec_eoe_t *); void ec_eoe_state_rx_fetch(ec_eoe_t *); +void ec_eoe_state_rx_fetch_data(ec_eoe_t *); void ec_eoe_state_tx_start(ec_eoe_t *); void ec_eoe_state_tx_sent(ec_eoe_t *); @@ -494,9 +495,14 @@ void ec_eoe_state_rx_start(ec_eoe_t *eoe /**< EoE handler */) return; } - ec_slave_mbox_prepare_check(eoe->slave, &eoe->datagram); - eoe->queue_datagram = 1; - eoe->state = ec_eoe_state_rx_check; + // mailbox read check is skipped if a read request is already ongoing + if (ec_read_mbox_locked(eoe->slave)) { + eoe->state = ec_eoe_state_rx_fetch_data; + } else { + ec_slave_mbox_prepare_check(eoe->slave, &eoe->datagram); + eoe->queue_datagram = 1; + eoe->state = ec_eoe_state_rx_check; + } } /*****************************************************************************/ @@ -515,12 +521,20 @@ void ec_eoe_state_rx_check(ec_eoe_t *eoe /**< EoE handler */) " check datagram for %s.\n", eoe->dev->name); #endif eoe->state = ec_eoe_state_tx_start; + ec_read_mbox_lock_clear(eoe->slave); return; } if (!ec_slave_mbox_check(&eoe->datagram)) { eoe->rx_idle = 1; - eoe->state = ec_eoe_state_tx_start; + ec_read_mbox_lock_clear(eoe->slave); + // check that data is not already received by another read request + if (eoe->slave->mbox_eoe_data.payload_size > 0) { + eoe->state = ec_eoe_state_rx_fetch_data; + eoe->state(eoe); + } else { + eoe->state = ec_eoe_state_tx_start; + } return; } @@ -538,6 +552,32 @@ void ec_eoe_state_rx_check(ec_eoe_t *eoe /**< EoE handler */) * datagram. */ void ec_eoe_state_rx_fetch(ec_eoe_t *eoe /**< EoE handler */) +{ + + if (eoe->datagram.state != EC_DATAGRAM_RECEIVED) { + eoe->stats.rx_errors++; +#if EOE_DEBUG_LEVEL >= 1 + EC_SLAVE_WARN(eoe->slave, "Failed to receive mbox" + " fetch datagram for %s.\n", eoe->dev->name); +#endif + eoe->state = ec_eoe_state_tx_start; + ec_read_mbox_lock_clear(eoe->slave); + return; + } + ec_read_mbox_lock_clear(eoe->slave); + eoe->state = ec_eoe_state_rx_fetch_data; + eoe->state(eoe); +} + + + +/*****************************************************************************/ + +/** State: RX_FETCH DATA. + * + * Processes the EoE data. + */ +void ec_eoe_state_rx_fetch_data(ec_eoe_t *eoe /**< EoE handler */) { size_t rec_size, data_size; uint8_t *data, frame_type, last_fragment, time_appended, mbox_prot; @@ -550,17 +590,19 @@ void ec_eoe_state_rx_fetch(ec_eoe_t *eoe /**< EoE handler */) unsigned int i; #endif - if (eoe->datagram.state != EC_DATAGRAM_RECEIVED) { - eoe->stats.rx_errors++; -#if EOE_DEBUG_LEVEL >= 1 - EC_SLAVE_WARN(eoe->slave, "Failed to receive mbox" - " fetch datagram for %s.\n", eoe->dev->name); -#endif - eoe->state = ec_eoe_state_tx_start; + if (eoe->slave->mbox_eoe_data.payload_size > 0) { + eoe->slave->mbox_eoe_data.payload_size = 0; + } else { + // initiate a new mailbox read check if required data is not available + if (!ec_read_mbox_locked(eoe->slave)) { + ec_slave_mbox_prepare_check(eoe->slave, &eoe->datagram); + eoe->queue_datagram = 1; + eoe->state = ec_eoe_state_rx_check; + } return; } - data = ec_slave_mbox_fetch(eoe->slave, &eoe->datagram, + data = ec_slave_mbox_fetch(eoe->slave, &eoe->slave->mbox_eoe_data, &mbox_prot, &rec_size); if (IS_ERR(data)) { eoe->stats.rx_errors++; diff --git a/master/fsm_coe.c b/master/fsm_coe.c index b3116b6a..b0d7d2da 100644 --- a/master/fsm_coe.c +++ b/master/fsm_coe.c @@ -71,27 +71,34 @@ void ec_fsm_coe_dict_start(ec_fsm_coe_t *, ec_datagram_t *); void ec_fsm_coe_dict_request(ec_fsm_coe_t *, ec_datagram_t *); void ec_fsm_coe_dict_check(ec_fsm_coe_t *, ec_datagram_t *); void ec_fsm_coe_dict_response(ec_fsm_coe_t *, ec_datagram_t *); +void ec_fsm_coe_dict_response_data(ec_fsm_coe_t *, ec_datagram_t *); void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *, ec_datagram_t *); void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *, ec_datagram_t *); void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *, ec_datagram_t *); +void ec_fsm_coe_dict_desc_response_data(ec_fsm_coe_t *, ec_datagram_t *); void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *, ec_datagram_t *); void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *, ec_datagram_t *); void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *, ec_datagram_t *); +void ec_fsm_coe_dict_entry_response_data(ec_fsm_coe_t *, ec_datagram_t *); void ec_fsm_coe_down_start(ec_fsm_coe_t *, ec_datagram_t *); void ec_fsm_coe_down_request(ec_fsm_coe_t *, ec_datagram_t *); void ec_fsm_coe_down_check(ec_fsm_coe_t *, ec_datagram_t *); void ec_fsm_coe_down_response(ec_fsm_coe_t *, ec_datagram_t *); +void ec_fsm_coe_down_response_data(ec_fsm_coe_t *, ec_datagram_t *); void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *, ec_datagram_t *); void ec_fsm_coe_down_seg_response(ec_fsm_coe_t *, ec_datagram_t *); +void ec_fsm_coe_down_seg_response_data(ec_fsm_coe_t *, ec_datagram_t *); void ec_fsm_coe_up_start(ec_fsm_coe_t *, ec_datagram_t *); void ec_fsm_coe_up_request(ec_fsm_coe_t *, ec_datagram_t *); void ec_fsm_coe_up_check(ec_fsm_coe_t *, ec_datagram_t *); void ec_fsm_coe_up_response(ec_fsm_coe_t *, ec_datagram_t *); +void ec_fsm_coe_up_response_data(ec_fsm_coe_t *, ec_datagram_t *); void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *, ec_datagram_t *); void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *, ec_datagram_t *); void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *, ec_datagram_t *); +void ec_fsm_coe_up_seg_response_data(ec_fsm_coe_t *, ec_datagram_t *); void ec_fsm_coe_end(ec_fsm_coe_t *, ec_datagram_t *); void ec_fsm_coe_error(ec_fsm_coe_t *, ec_datagram_t *); @@ -401,9 +408,16 @@ void ec_fsm_coe_dict_request( fsm->jiffies_start = fsm->datagram->jiffies_sent; - ec_slave_mbox_prepare_check(slave, datagram); // can not fail. - fsm->retries = EC_FSM_RETRIES; - fsm->state = ec_fsm_coe_dict_check; + // mailbox read check is skipped if a read request is already ongoing + if (ec_read_mbox_locked(slave)) { + fsm->state = ec_fsm_coe_dict_response_data; + // the datagram is not used and marked as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_coe_dict_check; + } } /*****************************************************************************/ @@ -424,6 +438,7 @@ void ec_fsm_coe_dict_check( if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); ec_datagram_print_state(fsm->datagram); return; @@ -431,6 +446,7 @@ void ec_fsm_coe_dict_check( if (fsm->datagram->working_counter != 1) { fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave,"Reception of CoE mailbox check" " datagram failed: "); ec_datagram_print_wc_error(fsm->datagram); @@ -438,11 +454,22 @@ void ec_fsm_coe_dict_check( } if (!ec_slave_mbox_check(fsm->datagram)) { - unsigned long diff_ms = - (fsm->datagram->jiffies_received - fsm->jiffies_start) * - 1000 / HZ; + unsigned long diff_ms = 0; + + // check that data is not already received by another read request + if (slave->mbox_coe_data.payload_size > 0) { + ec_read_mbox_lock_clear(slave); + fsm->state = ec_fsm_coe_dict_response_data; + fsm->state(fsm, datagram); + return; + } + + diff_ms = (fsm->datagram->jiffies_received - fsm->jiffies_start) * + 1000 / HZ; + if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Timeout while waiting for" " SDO dictionary list response.\n"); return; @@ -500,13 +527,6 @@ void ec_fsm_coe_dict_response( ) { ec_slave_t *slave = fsm->slave; - uint8_t *data, mbox_prot; - size_t rec_size; - unsigned int sdo_count, i; - uint16_t sdo_index, fragments_left; - ec_sdo_t *sdo; - bool first_segment; - size_t index_list_offset; if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. @@ -515,6 +535,7 @@ void ec_fsm_coe_dict_response( if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary" " response datagram: "); ec_datagram_print_state(fsm->datagram); @@ -522,13 +543,58 @@ void ec_fsm_coe_dict_response( } if (fsm->datagram->working_counter != 1) { - fsm->state = ec_fsm_coe_error; - EC_SLAVE_ERR(slave, "Reception of CoE dictionary response failed: "); - ec_datagram_print_wc_error(fsm->datagram); + // only an error if data has not already been read by another read request + if (slave->mbox_coe_data.payload_size == 0) { + fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); + EC_SLAVE_ERR(slave, "Reception of CoE dictionary response failed: "); + ec_datagram_print_wc_error(fsm->datagram); + return; + } + } + ec_read_mbox_lock_clear(slave); + fsm->state = ec_fsm_coe_dict_response_data; + fsm->state(fsm, datagram); +} + + +/*****************************************************************************/ + +/** + CoE state: DICT RESPONSE DATA. + +*/ + +void ec_fsm_coe_dict_response_data( + ec_fsm_coe_t *fsm, /**< Finite state machine. */ + ec_datagram_t *datagram /**< Datagram to use. */ + ) +{ + ec_slave_t *slave = fsm->slave; + uint8_t *data, mbox_prot; + size_t rec_size; + unsigned int sdo_count, i; + uint16_t sdo_index, fragments_left; + ec_sdo_t *sdo; + bool first_segment; + size_t index_list_offset; + + // process the data available or initiate a new mailbox read check + if (slave->mbox_coe_data.payload_size > 0) { + slave->mbox_coe_data.payload_size = 0; + } else { + // initiate a new mailbox read check if required data is not available + if (ec_read_mbox_locked(slave)) { + // await current read request and mark the datagram as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->state = ec_fsm_coe_dict_check; + } return; } - data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); + data = ec_slave_mbox_fetch(slave, &slave->mbox_coe_data, &mbox_prot, &rec_size); if (IS_ERR(data)) { fsm->state = ec_fsm_coe_error; return; @@ -681,9 +747,16 @@ void ec_fsm_coe_dict_desc_request( fsm->jiffies_start = fsm->datagram->jiffies_sent; - ec_slave_mbox_prepare_check(slave, datagram); // can not fail. - fsm->retries = EC_FSM_RETRIES; - fsm->state = ec_fsm_coe_dict_desc_check; + // mailbox read check is skipped if a read request is already ongoing + if (ec_read_mbox_locked(slave)) { + fsm->state = ec_fsm_coe_dict_desc_response_data; + // the datagram is not used and marked as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_coe_dict_desc_check; + } } /*****************************************************************************/ @@ -706,6 +779,7 @@ void ec_fsm_coe_dict_desc_check( if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); ec_datagram_print_state(fsm->datagram); return; @@ -713,6 +787,7 @@ void ec_fsm_coe_dict_desc_check( if (fsm->datagram->working_counter != 1) { fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" " datagram failed: "); ec_datagram_print_wc_error(fsm->datagram); @@ -720,11 +795,22 @@ void ec_fsm_coe_dict_desc_check( } if (!ec_slave_mbox_check(fsm->datagram)) { - unsigned long diff_ms = - (fsm->datagram->jiffies_received - fsm->jiffies_start) * - 1000 / HZ; + unsigned long diff_ms = 0; + + // check that data is not already received by another read request + if (slave->mbox_coe_data.payload_size > 0) { + ec_read_mbox_lock_clear(slave); + fsm->state = ec_fsm_coe_dict_desc_response_data; + fsm->state(fsm, datagram); + return; + } + + diff_ms = (fsm->datagram->jiffies_received - fsm->jiffies_start) * + 1000 / HZ; + if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Timeout while waiting for" " SDO 0x%04x object description response.\n", fsm->sdo->index); @@ -785,9 +871,6 @@ void ec_fsm_coe_dict_desc_response( ) { ec_slave_t *slave = fsm->slave; - ec_sdo_t *sdo = fsm->sdo; - uint8_t *data, mbox_prot; - size_t rec_size, name_size; if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. @@ -796,6 +879,7 @@ void ec_fsm_coe_dict_desc_response( if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Failed to receive CoE SDO description" " response datagram: "); ec_datagram_print_state(fsm->datagram); @@ -803,14 +887,54 @@ void ec_fsm_coe_dict_desc_response( } if (fsm->datagram->working_counter != 1) { - fsm->state = ec_fsm_coe_error; - EC_SLAVE_ERR(slave, "Reception of CoE SDO description" + // only an error if data has not already been read by another read request + if (slave->mbox_coe_data.payload_size == 0) { + fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); + EC_SLAVE_ERR(slave, "Reception of CoE SDO description" " response failed: "); - ec_datagram_print_wc_error(fsm->datagram); + ec_datagram_print_wc_error(fsm->datagram); + return; + } + } + ec_read_mbox_lock_clear(slave); + fsm->state = ec_fsm_coe_dict_desc_response_data; + fsm->state(fsm, datagram); +} + +/*****************************************************************************/ + +/** + CoE state: DICT DESC RESPONSE DATA. + +*/ + +void ec_fsm_coe_dict_desc_response_data( + ec_fsm_coe_t *fsm, /**< Finite state machine */ + ec_datagram_t *datagram /**< Datagram to use. */ + ) +{ + ec_slave_t *slave = fsm->slave; + ec_sdo_t *sdo = fsm->sdo; + uint8_t *data, mbox_prot; + size_t rec_size, name_size; + + // process the data available or initiate a new mailbox read check + if (slave->mbox_coe_data.payload_size > 0) { + slave->mbox_coe_data.payload_size = 0; + } else { + // initiate a new mailbox read check if required data is not available + if (ec_read_mbox_locked(slave)) { + // await current read request and mark the datagram as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->state = ec_fsm_coe_dict_desc_check; + } return; } - data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); + data = ec_slave_mbox_fetch(slave, &slave->mbox_coe_data, &mbox_prot, &rec_size); if (IS_ERR(data)) { fsm->state = ec_fsm_coe_error; return; @@ -945,9 +1069,16 @@ void ec_fsm_coe_dict_entry_request( fsm->jiffies_start = fsm->datagram->jiffies_sent; - ec_slave_mbox_prepare_check(slave, datagram); // can not fail - fsm->retries = EC_FSM_RETRIES; - fsm->state = ec_fsm_coe_dict_entry_check; + // mailbox read check is skipped if a read request is already ongoing + if (ec_read_mbox_locked(slave)) { + fsm->state = ec_fsm_coe_dict_entry_response_data; + // the datagram is not used and marked as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_coe_dict_entry_check; + } } /*****************************************************************************/ @@ -970,6 +1101,7 @@ void ec_fsm_coe_dict_entry_check( if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); ec_datagram_print_state(fsm->datagram); return; @@ -977,6 +1109,7 @@ void ec_fsm_coe_dict_entry_check( if (fsm->datagram->working_counter != 1) { fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" " datagram failed: "); ec_datagram_print_wc_error(fsm->datagram); @@ -984,11 +1117,22 @@ void ec_fsm_coe_dict_entry_check( } if (!ec_slave_mbox_check(fsm->datagram)) { - unsigned long diff_ms = - (fsm->datagram->jiffies_received - fsm->jiffies_start) * - 1000 / HZ; + unsigned long diff_ms = 0; + + // check that data is not already received by another read request + if (slave->mbox_coe_data.payload_size > 0) { + ec_read_mbox_lock_clear(slave); + fsm->state = ec_fsm_coe_dict_entry_response_data; + fsm->state(fsm, datagram); + return; + } + + diff_ms = (fsm->datagram->jiffies_received - fsm->jiffies_start) * + 1000 / HZ; + if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Timeout while waiting for" " SDO entry 0x%04x:%x description response.\n", fsm->sdo->index, fsm->subindex); @@ -1019,11 +1163,6 @@ void ec_fsm_coe_dict_entry_response( ) { ec_slave_t *slave = fsm->slave; - ec_sdo_t *sdo = fsm->sdo; - uint8_t *data, mbox_prot; - size_t rec_size, data_size; - ec_sdo_entry_t *entry; - u16 word; if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. @@ -1032,6 +1171,7 @@ void ec_fsm_coe_dict_entry_response( if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Failed to receive CoE SDO" " description response datagram: "); ec_datagram_print_state(fsm->datagram); @@ -1039,14 +1179,56 @@ void ec_fsm_coe_dict_entry_response( } if (fsm->datagram->working_counter != 1) { - fsm->state = ec_fsm_coe_error; - EC_SLAVE_ERR(slave, "Reception of CoE SDO description" + // only an error if data has not already been read by another read request + if (slave->mbox_coe_data.payload_size == 0) { + fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); + EC_SLAVE_ERR(slave, "Reception of CoE SDO description" " response failed: "); - ec_datagram_print_wc_error(fsm->datagram); + ec_datagram_print_wc_error(fsm->datagram); + return; + } + } + ec_read_mbox_lock_clear(slave); + fsm->state = ec_fsm_coe_dict_entry_response_data; + fsm->state(fsm, datagram); +} + +/*****************************************************************************/ + +/** + CoE state: DICT ENTRY RESPONSE DATA. + +*/ + +void ec_fsm_coe_dict_entry_response_data( + ec_fsm_coe_t *fsm, /**< Finite state machine. */ + ec_datagram_t *datagram /**< Datagram to use. */ + ) +{ + ec_slave_t *slave = fsm->slave; + ec_sdo_t *sdo = fsm->sdo; + uint8_t *data, mbox_prot; + size_t rec_size, data_size; + ec_sdo_entry_t *entry; + u16 word; + + // process the data available or initiate a new mailbox read check + if (slave->mbox_coe_data.payload_size > 0) { + slave->mbox_coe_data.payload_size = 0; + } else { + // initiate a new mailbox read check if required data is not available + if (ec_read_mbox_locked(slave)) { + // await current read request and mark the datagram as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->state = ec_fsm_coe_dict_entry_check; + } return; } - data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); + data = ec_slave_mbox_fetch(slave, &slave->mbox_coe_data, &mbox_prot, &rec_size); if (IS_ERR(data)) { fsm->state = ec_fsm_coe_error; return; @@ -1396,9 +1578,16 @@ void ec_fsm_coe_down_request( fsm->jiffies_start = fsm->datagram->jiffies_sent; - ec_slave_mbox_prepare_check(slave, datagram); // can not fail. - fsm->retries = EC_FSM_RETRIES; - fsm->state = ec_fsm_coe_down_check; + // mailbox read check is skipped if a read request is already ongoing + if (ec_read_mbox_locked(slave)) { + fsm->state = ec_fsm_coe_down_response_data; + // the datagram is not used and marked as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_coe_down_check; + } } /*****************************************************************************/ @@ -1420,6 +1609,7 @@ void ec_fsm_coe_down_check( if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { fsm->request->errno = EIO; fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check" " datagram: "); ec_datagram_print_state(fsm->datagram); @@ -1429,6 +1619,7 @@ void ec_fsm_coe_down_check( if (fsm->datagram->working_counter != 1) { fsm->request->errno = EIO; fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" " datagram failed: "); ec_datagram_print_wc_error(fsm->datagram); @@ -1436,12 +1627,23 @@ void ec_fsm_coe_down_check( } if (!ec_slave_mbox_check(fsm->datagram)) { - unsigned long diff_ms = - (fsm->datagram->jiffies_received - fsm->jiffies_start) * - 1000 / HZ; + unsigned long diff_ms = 0; + + // check that data is not already received by another read request + if (slave->mbox_coe_data.payload_size > 0) { + ec_read_mbox_lock_clear(slave); + fsm->state = ec_fsm_coe_down_response_data; + fsm->state(fsm, datagram); + return; + } + + diff_ms = (fsm->datagram->jiffies_received - fsm->jiffies_start) * + 1000 / HZ; + if (diff_ms >= fsm->request->response_timeout) { fsm->request->errno = EIO; fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting" " for SDO 0x%04x:%x download response.\n", diff_ms, fsm->request->index, fsm->request->subindex); @@ -1535,8 +1737,6 @@ void ec_fsm_coe_down_response( ) { ec_slave_t *slave = fsm->slave; - uint8_t *data, mbox_prot; - size_t rec_size; ec_sdo_request_t *request = fsm->request; if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { @@ -1554,14 +1754,55 @@ void ec_fsm_coe_down_response( } if (fsm->datagram->working_counter != 1) { - request->errno = EIO; - fsm->state = ec_fsm_coe_error; - EC_SLAVE_ERR(slave, "Reception of CoE download response failed: "); - ec_datagram_print_wc_error(fsm->datagram); + // only an error if data has not already been read by another read request + if (slave->mbox_coe_data.payload_size == 0) { + request->errno = EIO; + fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); + EC_SLAVE_ERR(slave, "Reception of CoE download response failed: "); + ec_datagram_print_wc_error(fsm->datagram); + return; + } + } + ec_read_mbox_lock_clear(slave); + fsm->state = ec_fsm_coe_down_response_data; + fsm->state(fsm, datagram); +} + +/*****************************************************************************/ + +/** + CoE state: DOWN RESPONSE DATA. + +*/ + +void ec_fsm_coe_down_response_data( + ec_fsm_coe_t *fsm, /**< Finite state machine. */ + ec_datagram_t *datagram /**< Datagram to use. */ + ) +{ + ec_slave_t *slave = fsm->slave; + uint8_t *data, mbox_prot; + size_t rec_size; + ec_sdo_request_t *request = fsm->request; + + // process the data available or initiate a new mailbox read check + if (slave->mbox_coe_data.payload_size > 0) { + slave->mbox_coe_data.payload_size = 0; + } else { + // initiate a new mailbox read check if required data is not available + if (ec_read_mbox_locked(slave)) { + // await current read request and mark the datagram as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->state = ec_fsm_coe_down_check; + } return; } - data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); + data = ec_slave_mbox_fetch(slave, &slave->mbox_coe_data, &mbox_prot, &rec_size); + if (IS_ERR(data)) { request->errno = PTR_ERR(data); fsm->state = ec_fsm_coe_error; @@ -1663,6 +1904,7 @@ void ec_fsm_coe_down_seg_check( if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { fsm->request->errno = EIO; fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); ec_datagram_print_state(fsm->datagram); return; @@ -1671,6 +1913,7 @@ void ec_fsm_coe_down_seg_check( if (fsm->datagram->working_counter != 1) { fsm->request->errno = EIO; fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Reception of CoE mailbox segment check" " datagram failed: "); ec_datagram_print_wc_error(fsm->datagram); @@ -1678,12 +1921,23 @@ void ec_fsm_coe_down_seg_check( } if (!ec_slave_mbox_check(fsm->datagram)) { - unsigned long diff_ms = - (fsm->datagram->jiffies_received - fsm->jiffies_start) * - 1000 / HZ; + unsigned long diff_ms = 0; + + // check that data is not already received by another read request + if (slave->mbox_coe_data.payload_size > 0) { + ec_read_mbox_lock_clear(slave); + fsm->state = ec_fsm_coe_down_seg_response_data; + fsm->state(fsm, datagram); + return; + } + + diff_ms = (fsm->datagram->jiffies_received - fsm->jiffies_start) * + 1000 / HZ; + if (diff_ms >= fsm->request->response_timeout) { fsm->request->errno = EIO; fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Timeout while waiting for SDO download" " segment response.\n"); return; @@ -1713,8 +1967,6 @@ void ec_fsm_coe_down_seg_response( ) { ec_slave_t *slave = fsm->slave; - uint8_t *data, mbox_prot; - size_t rec_size; ec_sdo_request_t *request = fsm->request; if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { @@ -1725,6 +1977,7 @@ void ec_fsm_coe_down_seg_response( if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { request->errno = EIO; fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Failed to receive CoE download response" " datagram: "); ec_datagram_print_state(fsm->datagram); @@ -1732,14 +1985,54 @@ void ec_fsm_coe_down_seg_response( } if (fsm->datagram->working_counter != 1) { - request->errno = EIO; - fsm->state = ec_fsm_coe_error; - EC_SLAVE_ERR(slave, "Reception of CoE download response failed: "); - ec_datagram_print_wc_error(fsm->datagram); + // only an error if data has not already been read by another read request + if (slave->mbox_coe_data.payload_size == 0) { + request->errno = EIO; + fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); + EC_SLAVE_ERR(slave, "Reception of CoE download response failed: "); + ec_datagram_print_wc_error(fsm->datagram); + return; + } + } + ec_read_mbox_lock_clear(slave); + fsm->state = ec_fsm_coe_down_seg_response_data; + fsm->state(fsm, datagram); +} + +/*****************************************************************************/ + +/** + CoE state: DOWN SEG RESPONSE DATA. + +*/ + +void ec_fsm_coe_down_seg_response_data( + ec_fsm_coe_t *fsm, /**< Finite state machine. */ + ec_datagram_t *datagram /**< Datagram to use. */ + ) +{ + ec_slave_t *slave = fsm->slave; + uint8_t *data, mbox_prot; + size_t rec_size; + ec_sdo_request_t *request = fsm->request; + + // process the data available or initiate a new mailbox read check + if (slave->mbox_coe_data.payload_size > 0) { + slave->mbox_coe_data.payload_size = 0; + } else { + // initiate a new mailbox read check if required data is not available + if (ec_read_mbox_locked(slave)) { + // await current read request and mark the datagram as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->state = ec_fsm_coe_down_seg_check; + } return; } - data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); + data = ec_slave_mbox_fetch(slave, &slave->mbox_coe_data, &mbox_prot, &rec_size); if (IS_ERR(data)) { request->errno = PTR_ERR(data); fsm->state = ec_fsm_coe_error; @@ -1964,10 +2257,16 @@ void ec_fsm_coe_up_request( #endif fsm->jiffies_start = fsm->datagram->jiffies_sent; - - ec_slave_mbox_prepare_check(slave, datagram); // can not fail. - fsm->retries = EC_FSM_RETRIES; - fsm->state = ec_fsm_coe_up_check; + // mailbox read check is skipped if a read request is already ongoing + if (ec_read_mbox_locked(slave)) { + fsm->state = ec_fsm_coe_up_response_data; + // the datagram is not used and marked as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_coe_up_check; + } } /*****************************************************************************/ @@ -1991,6 +2290,7 @@ void ec_fsm_coe_up_check( if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { fsm->request->errno = EIO; fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); ec_datagram_print_state(fsm->datagram); return; @@ -1999,6 +2299,7 @@ void ec_fsm_coe_up_check( if (fsm->datagram->working_counter != 1) { fsm->request->errno = EIO; fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" " datagram failed: "); ec_datagram_print_wc_error(fsm->datagram); @@ -2006,12 +2307,23 @@ void ec_fsm_coe_up_check( } if (!ec_slave_mbox_check(fsm->datagram)) { - unsigned long diff_ms = - (fsm->datagram->jiffies_received - fsm->jiffies_start) * - 1000 / HZ; + unsigned long diff_ms = 0; + + // check that data is not already received by another read request + if (slave->mbox_coe_data.payload_size > 0) { + ec_read_mbox_lock_clear(slave); + fsm->state = ec_fsm_coe_up_response_data; + fsm->state(fsm, datagram); + return; + } + + diff_ms = (fsm->datagram->jiffies_received - fsm->jiffies_start) * + 1000 / HZ; + if (diff_ms >= fsm->request->response_timeout) { fsm->request->errno = EIO; fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for" " SDO 0x%04x:%x upload response.\n", diff_ms, fsm->request->index, fsm->request->subindex); @@ -2071,13 +2383,7 @@ void ec_fsm_coe_up_response( ) { ec_slave_t *slave = fsm->slave; - ec_master_t *master = slave->master; - uint16_t rec_index; - uint8_t *data, mbox_prot, rec_subindex; - size_t rec_size, data_size; ec_sdo_request_t *request = fsm->request; - unsigned int expedited, size_specified; - int ret; if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. @@ -2087,6 +2393,7 @@ void ec_fsm_coe_up_response( if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { request->errno = EIO; fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Failed to receive CoE upload response" " datagram: "); ec_datagram_print_state(fsm->datagram); @@ -2094,14 +2401,60 @@ void ec_fsm_coe_up_response( } if (fsm->datagram->working_counter != 1) { - request->errno = EIO; - fsm->state = ec_fsm_coe_error; - EC_SLAVE_ERR(slave, "Reception of CoE upload response failed: "); - ec_datagram_print_wc_error(fsm->datagram); + // only an error if data has not already been read by another read request + if (slave->mbox_coe_data.payload_size == 0) { + request->errno = EIO; + fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); + EC_SLAVE_ERR(slave, "Reception of CoE upload response failed: "); + ec_datagram_print_wc_error(fsm->datagram); + return; + } + } + ec_read_mbox_lock_clear(slave); + fsm->state = ec_fsm_coe_up_response_data; + fsm->state(fsm, datagram); +} + + +/*****************************************************************************/ + +/** + CoE state: UP RESPONSE DATA. + +*/ + +void ec_fsm_coe_up_response_data( + ec_fsm_coe_t *fsm, /**< Finite state machine. */ + ec_datagram_t *datagram /**< Datagram to use. */ + ) +{ + ec_slave_t *slave = fsm->slave; + ec_master_t *master = slave->master; + uint16_t rec_index; + uint8_t *data, mbox_prot, rec_subindex; + size_t rec_size, data_size; + ec_sdo_request_t *request = fsm->request; + unsigned int expedited, size_specified; + int ret; + + // process the data available or initiate a new mailbox read check + if (slave->mbox_coe_data.payload_size > 0) { + slave->mbox_coe_data.payload_size = 0; + } else { + // initiate a new mailbox read check if required data is not available + if (ec_read_mbox_locked(slave)) { + // await current read request and mark the datagram as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->state = ec_fsm_coe_up_check; + } return; } - data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); + data = ec_slave_mbox_fetch(slave, &slave->mbox_coe_data, &mbox_prot, &rec_size); + if (IS_ERR(data)) { request->errno = PTR_ERR(data); fsm->state = ec_fsm_coe_error; @@ -2299,10 +2652,16 @@ void ec_fsm_coe_up_seg_request( } fsm->jiffies_start = fsm->datagram->jiffies_sent; - - ec_slave_mbox_prepare_check(slave, datagram); // can not fail. - fsm->retries = EC_FSM_RETRIES; - fsm->state = ec_fsm_coe_up_seg_check; + // mailbox read check is skipped if a read request is already ongoing + if (ec_read_mbox_locked(slave)) { + fsm->state = ec_fsm_coe_up_seg_response_data; + // the datagram is not used and marked as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_coe_up_seg_check; + } } /*****************************************************************************/ @@ -2326,6 +2685,7 @@ void ec_fsm_coe_up_seg_check( if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { fsm->request->errno = EIO; fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check" " datagram: "); ec_datagram_print_state(fsm->datagram); @@ -2335,6 +2695,7 @@ void ec_fsm_coe_up_seg_check( if (fsm->datagram->working_counter != 1) { fsm->request->errno = EIO; fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Reception of CoE mailbox check datagram" " failed: "); ec_datagram_print_wc_error(fsm->datagram); @@ -2342,12 +2703,23 @@ void ec_fsm_coe_up_seg_check( } if (!ec_slave_mbox_check(fsm->datagram)) { - unsigned long diff_ms = - (fsm->datagram->jiffies_received - fsm->jiffies_start) * - 1000 / HZ; + unsigned long diff_ms = 0; + + // check that data is not already received by another read request + if (slave->mbox_coe_data.payload_size > 0) { + ec_read_mbox_lock_clear(slave); + fsm->state = ec_fsm_coe_up_seg_response_data; + fsm->state(fsm, datagram); + return; + } + + diff_ms = (fsm->datagram->jiffies_received - fsm->jiffies_start) * + 1000 / HZ; + if (diff_ms >= fsm->request->response_timeout) { fsm->request->errno = EIO; fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Timeout while waiting for SDO upload" " segment response.\n"); return; @@ -2377,11 +2749,7 @@ void ec_fsm_coe_up_seg_response( ) { ec_slave_t *slave = fsm->slave; - ec_master_t *master = slave->master; - uint8_t *data, mbox_prot; - size_t rec_size, data_size; ec_sdo_request_t *request = fsm->request; - unsigned int last_segment; if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. @@ -2391,6 +2759,7 @@ void ec_fsm_coe_up_seg_response( if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { request->errno = EIO; fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Failed to receive CoE upload segment" " response datagram: "); ec_datagram_print_state(fsm->datagram); @@ -2398,15 +2767,59 @@ void ec_fsm_coe_up_seg_response( } if (fsm->datagram->working_counter != 1) { - request->errno = EIO; - fsm->state = ec_fsm_coe_error; - EC_SLAVE_ERR(slave, "Reception of CoE upload segment" + // only an error if data has not already been read by another read request + if (slave->mbox_coe_data.payload_size == 0) { + request->errno = EIO; + fsm->state = ec_fsm_coe_error; + ec_read_mbox_lock_clear(slave); + EC_SLAVE_ERR(slave, "Reception of CoE upload segment" " response failed: "); - ec_datagram_print_wc_error(fsm->datagram); + ec_datagram_print_wc_error(fsm->datagram); + return; + } + } + ec_read_mbox_lock_clear(slave); + fsm->state = ec_fsm_coe_up_seg_response_data; + fsm->state(fsm, datagram); +} + + +/*****************************************************************************/ + +/** + CoE state: UP RESPONSE DATA. + +*/ + +void ec_fsm_coe_up_seg_response_data( + ec_fsm_coe_t *fsm, /**< Finite state machine. */ + ec_datagram_t *datagram /**< Datagram to use. */ + ) +{ + ec_slave_t *slave = fsm->slave; + ec_master_t *master = slave->master; + uint8_t *data, mbox_prot; + size_t rec_size, data_size; + ec_sdo_request_t *request = fsm->request; + unsigned int last_segment; + + // process the data available or initiate a new mailbox read check + if (slave->mbox_coe_data.payload_size > 0) { + slave->mbox_coe_data.payload_size = 0; + } else { + // initiate a new mailbox read check if required data is not available + if (ec_read_mbox_locked(slave)) { + // await current read request and mark the datagram as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->state = ec_fsm_coe_up_seg_check; + } return; } - data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); + data = ec_slave_mbox_fetch(slave, &slave->mbox_coe_data, &mbox_prot, &rec_size); + if (IS_ERR(data)) { request->errno = PTR_ERR(data); fsm->state = ec_fsm_coe_error; diff --git a/master/fsm_eoe.c b/master/fsm_eoe.c index 4788ab67..873c49c5 100644 --- a/master/fsm_eoe.c +++ b/master/fsm_eoe.c @@ -51,6 +51,7 @@ void ec_fsm_eoe_set_ip_start(ec_fsm_eoe_t *, ec_datagram_t *); void ec_fsm_eoe_set_ip_request(ec_fsm_eoe_t *, ec_datagram_t *); void ec_fsm_eoe_set_ip_check(ec_fsm_eoe_t *, ec_datagram_t *); void ec_fsm_eoe_set_ip_response(ec_fsm_eoe_t *, ec_datagram_t *); +void ec_fsm_eoe_set_ip_response_data(ec_fsm_eoe_t *, ec_datagram_t *); void ec_fsm_eoe_end(ec_fsm_eoe_t *, ec_datagram_t *); void ec_fsm_eoe_error(ec_fsm_eoe_t *, ec_datagram_t *); @@ -325,9 +326,17 @@ void ec_fsm_eoe_set_ip_request( } fsm->jiffies_start = fsm->datagram->jiffies_sent; - ec_slave_mbox_prepare_check(slave, datagram); // can not fail. - fsm->retries = EC_FSM_RETRIES; - fsm->state = ec_fsm_eoe_set_ip_check; + + // mailbox read check is skipped if a read request is already ongoing + if (ec_read_mbox_locked(slave)) { + fsm->state = ec_fsm_eoe_set_ip_response_data; + // the datagram is not used and marked as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_eoe_set_ip_check; + } } /*****************************************************************************/ @@ -348,6 +357,7 @@ void ec_fsm_eoe_set_ip_check( if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_eoe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Failed to receive EoE mailbox check datagram: "); ec_datagram_print_state(fsm->datagram); return; @@ -355,6 +365,7 @@ void ec_fsm_eoe_set_ip_check( if (fsm->datagram->working_counter != 1) { fsm->state = ec_fsm_eoe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Reception of EoE mailbox check" " datagram failed: "); ec_datagram_print_wc_error(fsm->datagram); @@ -362,11 +373,21 @@ void ec_fsm_eoe_set_ip_check( } if (!ec_slave_mbox_check(fsm->datagram)) { - unsigned long diff_ms = - (fsm->datagram->jiffies_received - fsm->jiffies_start) * + unsigned long diff_ms; + + // check that data is not already received by another read request + if (slave->mbox_eoe_data.payload_size > 0) { + ec_read_mbox_lock_clear(slave); + fsm->state = ec_fsm_eoe_set_ip_response_data; + fsm->state(fsm, datagram); + return; + } + + diff_ms = (fsm->datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; if (diff_ms >= EC_EOE_RESPONSE_TIMEOUT) { fsm->state = ec_fsm_eoe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for" " set IP parameter response.\n", diff_ms); return; @@ -393,10 +414,6 @@ void ec_fsm_eoe_set_ip_response( ) { ec_slave_t *slave = fsm->slave; - ec_master_t *master = slave->master; - uint8_t *data, mbox_prot, frame_type; - size_t rec_size; - ec_eoe_request_t *req = fsm->request; if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. @@ -405,19 +422,58 @@ void ec_fsm_eoe_set_ip_response( if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_eoe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Failed to receive EoE read response datagram: "); ec_datagram_print_state(fsm->datagram); return; } if (fsm->datagram->working_counter != 1) { - fsm->state = ec_fsm_eoe_error; - EC_SLAVE_ERR(slave, "Reception of EoE read response failed: "); - ec_datagram_print_wc_error(fsm->datagram); + // only an error if data has not already been read by another read request + if (slave->mbox_eoe_data.payload_size == 0) { + fsm->state = ec_fsm_eoe_error; + ec_read_mbox_lock_clear(slave); + EC_SLAVE_ERR(slave, "Reception of EoE read response failed: "); + ec_datagram_print_wc_error(fsm->datagram); + return; + } + } + ec_read_mbox_lock_clear(slave); + fsm->state = ec_fsm_eoe_set_ip_response_data; + fsm->state(fsm, datagram); +} + +/*****************************************************************************/ + +/** EoE state: SET IP RESPONSE DATA. + */ +void ec_fsm_eoe_set_ip_response_data( + ec_fsm_eoe_t *fsm, /**< finite state machine */ + ec_datagram_t *datagram /**< Datagram to use. */ + ) +{ + ec_slave_t *slave = fsm->slave; + ec_master_t *master = slave->master; + uint8_t *data, mbox_prot, frame_type; + size_t rec_size; + ec_eoe_request_t *req = fsm->request; + + // process the data available or initiate a new mailbox read check + if (slave->mbox_eoe_data.payload_size > 0) { + slave->mbox_eoe_data.payload_size = 0; + } else { + // initiate a new mailbox read check if required data is not available + if (ec_read_mbox_locked(slave)) { + // await current read request and mark the datagram as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->state = ec_fsm_eoe_set_ip_check; + } return; } - data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); + data = ec_slave_mbox_fetch(slave, &slave->mbox_eoe_data, &mbox_prot, &rec_size); if (IS_ERR(data)) { fsm->state = ec_fsm_eoe_error; return; diff --git a/master/fsm_foe.c b/master/fsm_foe.c index 4470c91c..dff70cd5 100644 --- a/master/fsm_foe.c +++ b/master/fsm_foe.c @@ -87,11 +87,13 @@ void ec_fsm_foe_state_rrq_sent(ec_fsm_foe_t *, ec_datagram_t *); void ec_fsm_foe_state_ack_check(ec_fsm_foe_t *, ec_datagram_t *); void ec_fsm_foe_state_ack_read(ec_fsm_foe_t *, ec_datagram_t *); +void ec_fsm_foe_state_ack_read_data(ec_fsm_foe_t *, ec_datagram_t *); void ec_fsm_foe_state_data_sent(ec_fsm_foe_t *, ec_datagram_t *); void ec_fsm_foe_state_data_check(ec_fsm_foe_t *, ec_datagram_t *); void ec_fsm_foe_state_data_read(ec_fsm_foe_t *, ec_datagram_t *); +void ec_fsm_foe_state_data_read_data(ec_fsm_foe_t *, ec_datagram_t *); void ec_fsm_foe_state_sent_ack(ec_fsm_foe_t *, ec_datagram_t *); void ec_fsm_foe_write_start(ec_fsm_foe_t *, ec_datagram_t *); @@ -359,6 +361,7 @@ void ec_fsm_foe_state_ack_check( if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR); + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Failed to receive FoE mailbox check datagram: "); ec_datagram_print_state(fsm->datagram); return; @@ -366,6 +369,7 @@ void ec_fsm_foe_state_ack_check( if (fsm->datagram->working_counter != 1) { ec_foe_set_rx_error(fsm, FOE_WC_ERROR); + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Reception of FoE mailbox check datagram" " failed: "); ec_datagram_print_wc_error(fsm->datagram); @@ -374,9 +378,19 @@ void ec_fsm_foe_state_ack_check( if (!ec_slave_mbox_check(fsm->datagram)) { // slave did not put anything in the mailbox yet + + // check that data is not already received by another read request + if (slave->mbox_foe_data.payload_size > 0) { + ec_read_mbox_lock_clear(slave); + fsm->state = ec_fsm_foe_state_ack_read_data; + fsm->state(fsm, datagram); + return; + } + if (time_after(fsm->datagram->jiffies_received, fsm->jiffies_start + EC_FSM_FOE_TIMEOUT_JIFFIES)) { ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR); + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Timeout while waiting for ack response.\n"); return; } @@ -403,9 +417,6 @@ void ec_fsm_foe_state_ack_read( ) { ec_slave_t *slave = fsm->slave; - uint8_t *data, mbox_prot; - uint8_t opCode; - size_t rec_size; #ifdef DEBUG_FOE EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__); @@ -413,19 +424,57 @@ void ec_fsm_foe_state_ack_read( if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR); + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Failed to receive FoE ack response datagram: "); ec_datagram_print_state(fsm->datagram); return; } if (fsm->datagram->working_counter != 1) { - ec_foe_set_rx_error(fsm, FOE_WC_ERROR); - EC_SLAVE_ERR(slave, "Reception of FoE ack response failed: "); - ec_datagram_print_wc_error(fsm->datagram); + // only an error if data has not already been read by another read request + if (slave->mbox_foe_data.payload_size == 0) { + ec_foe_set_rx_error(fsm, FOE_WC_ERROR); + ec_read_mbox_lock_clear(slave); + EC_SLAVE_ERR(slave, "Reception of FoE ack response failed: "); + ec_datagram_print_wc_error(fsm->datagram); + return; + } + } + ec_read_mbox_lock_clear(slave); + fsm->state = ec_fsm_foe_state_ack_read_data; + fsm->state(fsm, datagram); +} + +/*****************************************************************************/ + +/** Process a read operation. + */ +void ec_fsm_foe_state_ack_read_data( + ec_fsm_foe_t *fsm, /**< FoE statemachine. */ + ec_datagram_t *datagram /**< Datagram to use. */ + ) +{ + ec_slave_t *slave = fsm->slave; + uint8_t *data, mbox_prot; + uint8_t opCode; + size_t rec_size; + + // process the data available or initiate a new mailbox read check + if (slave->mbox_foe_data.payload_size > 0) { + slave->mbox_foe_data.payload_size = 0; + } else { + // initiate a new mailbox read check if required data is not available + if (ec_read_mbox_locked(slave)) { + // await current read request and mark the datagram as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->state = ec_fsm_foe_state_ack_check; + } return; } - data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); + data = ec_slave_mbox_fetch(slave, &slave->mbox_foe_data, &mbox_prot, &rec_size); if (IS_ERR(data)) { ec_foe_set_tx_error(fsm, FOE_PROT_ERROR); return; @@ -522,10 +571,16 @@ void ec_fsm_foe_state_wrq_sent( fsm->jiffies_start = fsm->datagram->jiffies_sent; - ec_slave_mbox_prepare_check(slave, datagram); // can not fail. - - fsm->retries = EC_FSM_RETRIES; - fsm->state = ec_fsm_foe_state_ack_check; + // mailbox read check is skipped if a read request is already ongoing + if (ec_read_mbox_locked(slave)) { + fsm->state = ec_fsm_foe_state_ack_read_data; + // the datagram is not used and marked as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_foe_state_ack_check; + } } /*****************************************************************************/ @@ -560,10 +615,18 @@ void ec_fsm_foe_state_data_sent( return; } - ec_slave_mbox_prepare_check(slave, datagram); fsm->jiffies_start = jiffies; - fsm->retries = EC_FSM_RETRIES; - fsm->state = ec_fsm_foe_state_ack_check; + + // mailbox read check is skipped if a read request is already ongoing + if (ec_read_mbox_locked(slave)) { + fsm->state = ec_fsm_foe_state_ack_read_data; + // the datagram is not used and marked as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_foe_state_ack_check; + } } /*****************************************************************************/ @@ -652,6 +715,7 @@ void ec_fsm_foe_state_rrq_sent( if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR); + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Failed to send FoE RRQ: "); ec_datagram_print_state(fsm->datagram); return; @@ -660,6 +724,7 @@ void ec_fsm_foe_state_rrq_sent( if (fsm->datagram->working_counter != 1) { // slave did not put anything in the mailbox yet ec_foe_set_rx_error(fsm, FOE_WC_ERROR); + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Reception of FoE RRQ failed: "); ec_datagram_print_wc_error(fsm->datagram); return; @@ -667,10 +732,16 @@ void ec_fsm_foe_state_rrq_sent( fsm->jiffies_start = fsm->datagram->jiffies_sent; - ec_slave_mbox_prepare_check(slave, datagram); // can not fail. - - fsm->retries = EC_FSM_RETRIES; - fsm->state = ec_fsm_foe_state_data_check; + // mailbox read check is skipped if a read request is already ongoing + if (ec_read_mbox_locked(slave)) { + fsm->state = ec_fsm_foe_state_data_read_data; + // the datagram is not used and marked as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_foe_state_data_check; + } } /*****************************************************************************/ @@ -723,6 +794,7 @@ void ec_fsm_foe_state_data_check( if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR); + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Failed to send FoE DATA READ: "); ec_datagram_print_state(fsm->datagram); return; @@ -730,15 +802,25 @@ void ec_fsm_foe_state_data_check( if (fsm->datagram->working_counter != 1) { ec_foe_set_rx_error(fsm, FOE_WC_ERROR); + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Reception of FoE DATA READ: "); ec_datagram_print_wc_error(fsm->datagram); return; } if (!ec_slave_mbox_check(fsm->datagram)) { + // check that data is not already received by another read request + if (slave->mbox_foe_data.payload_size > 0) { + ec_read_mbox_lock_clear(slave); + fsm->state = ec_fsm_foe_state_data_read_data; + fsm->state(fsm, datagram); + return; + } + if (time_after(fsm->datagram->jiffies_received, fsm->jiffies_start + EC_FSM_FOE_TIMEOUT_JIFFIES)) { ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR); + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Timeout while waiting for ack response.\n"); return; } @@ -764,10 +846,6 @@ void ec_fsm_foe_state_data_read( ec_datagram_t *datagram /**< Datagram to use. */ ) { - size_t rec_size; - uint32_t packet_no; - uint8_t *data, opCode, mbox_prot; - ec_slave_t *slave = fsm->slave; #ifdef DEBUG_FOE @@ -776,19 +854,56 @@ void ec_fsm_foe_state_data_read( if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR); + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Failed to receive FoE DATA READ datagram: "); ec_datagram_print_state(fsm->datagram); return; } if (fsm->datagram->working_counter != 1) { - ec_foe_set_rx_error(fsm, FOE_WC_ERROR); - EC_SLAVE_ERR(slave, "Reception of FoE DATA READ failed: "); - ec_datagram_print_wc_error(fsm->datagram); + // only an error if data has not already been read by another read request + if (slave->mbox_foe_data.payload_size == 0) { + ec_foe_set_rx_error(fsm, FOE_WC_ERROR); + ec_read_mbox_lock_clear(slave); + EC_SLAVE_ERR(slave, "Reception of FoE DATA READ failed: "); + ec_datagram_print_wc_error(fsm->datagram); + return; + } + } + ec_read_mbox_lock_clear(slave); + fsm->state = ec_fsm_foe_state_data_read_data; + fsm->state(fsm, datagram); +} + +/*****************************************************************************/ + +/** Process a read data operation. + */ +void ec_fsm_foe_state_data_read_data( + ec_fsm_foe_t *fsm, /**< FoE statemachine. */ + ec_datagram_t *datagram /**< Datagram to use. */ + ) +{ + ec_slave_t *slave = fsm->slave; + size_t rec_size; + uint8_t *data, opCode, packet_no, mbox_prot; + + // process the data available or initiate a new mailbox read check + if (slave->mbox_foe_data.payload_size > 0) { + slave->mbox_foe_data.payload_size = 0; + } else { + // initiate a new mailbox read check if required data is not available + if (ec_read_mbox_locked(slave)) { + // await current read request and mark the datagram as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->state = ec_fsm_foe_state_data_check; + } return; } - data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); + data = ec_slave_mbox_fetch(slave, &slave->mbox_foe_data, &mbox_prot, &rec_size); if (IS_ERR(data)) { ec_foe_set_rx_error(fsm, FOE_MBOX_FETCH_ERROR); return; @@ -923,8 +1038,6 @@ void ec_fsm_foe_state_sent_ack( fsm->jiffies_start = fsm->datagram->jiffies_sent; - ec_slave_mbox_prepare_check(slave, datagram); // can not fail. - if (fsm->rx_last_packet) { fsm->rx_expected_packet_no = 0; fsm->request->data_size = fsm->rx_buffer_offset; @@ -932,8 +1045,17 @@ void ec_fsm_foe_state_sent_ack( } else { fsm->rx_expected_packet_no++; - fsm->retries = EC_FSM_RETRIES; - fsm->state = ec_fsm_foe_state_data_check; + + // mailbox read check is skipped if a read request is already ongoing + if (ec_read_mbox_locked(slave)) { + fsm->state = ec_fsm_foe_state_data_read_data; + // the datagram is not used and marked as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_foe_state_data_check; + } } } diff --git a/master/fsm_slave_config.c b/master/fsm_slave_config.c index 80956432..e9014d84 100755 --- a/master/fsm_slave_config.c +++ b/master/fsm_slave_config.c @@ -272,6 +272,10 @@ void ec_fsm_slave_config_state_init( ec_datagram_zero(datagram); fsm->retries = EC_FSM_RETRIES; fsm->state = ec_fsm_slave_config_state_clear_fmmus; + + EC_SLAVE_DBG(slave, 1, "Clearing mailbox check flag...\n"); + + ec_read_mbox_lock_clear(slave); } /*****************************************************************************/ @@ -542,6 +546,9 @@ void ec_fsm_slave_config_enter_mbox_sync( slave->sii.std_tx_mailbox_size; } + // allocate memory for mailbox response data for supported mailbox protocols + ec_mbox_prot_data_prealloc(slave, slave->sii.mailbox_protocols, slave->configured_tx_mailbox_size); + fsm->take_time = 1; fsm->retries = EC_FSM_RETRIES; diff --git a/master/fsm_slave_scan.c b/master/fsm_slave_scan.c index e24fefe2..6637a285 100644 --- a/master/fsm_slave_scan.c +++ b/master/fsm_slave_scan.c @@ -1047,6 +1047,9 @@ void ec_fsm_slave_scan_state_sync( slave->configured_tx_mailbox_offset, slave->configured_tx_mailbox_size); + // allocate memory for mailbox response data for supported mailbox protocols + ec_mbox_prot_data_prealloc(slave, slave->sii.mailbox_protocols, slave->configured_tx_mailbox_size); + ec_fsm_slave_scan_enter_pdos(fsm); } diff --git a/master/fsm_soe.c b/master/fsm_soe.c index 8ffbd5c3..a626de7d 100644 --- a/master/fsm_soe.c +++ b/master/fsm_soe.c @@ -66,11 +66,13 @@ void ec_fsm_soe_read_start(ec_fsm_soe_t *, ec_datagram_t *); void ec_fsm_soe_read_request(ec_fsm_soe_t *, ec_datagram_t *); void ec_fsm_soe_read_check(ec_fsm_soe_t *, ec_datagram_t *); void ec_fsm_soe_read_response(ec_fsm_soe_t *, ec_datagram_t *); +void ec_fsm_soe_read_response_data(ec_fsm_soe_t *, ec_datagram_t *); void ec_fsm_soe_write_start(ec_fsm_soe_t *, ec_datagram_t *); void ec_fsm_soe_write_request(ec_fsm_soe_t *, ec_datagram_t *); void ec_fsm_soe_write_check(ec_fsm_soe_t *, ec_datagram_t *); void ec_fsm_soe_write_response(ec_fsm_soe_t *, ec_datagram_t *); +void ec_fsm_soe_write_response_data(ec_fsm_soe_t *, ec_datagram_t *); void ec_fsm_soe_end(ec_fsm_soe_t *, ec_datagram_t *); void ec_fsm_soe_error(ec_fsm_soe_t *, ec_datagram_t *); @@ -326,9 +328,17 @@ void ec_fsm_soe_read_request( } fsm->jiffies_start = fsm->datagram->jiffies_sent; - ec_slave_mbox_prepare_check(slave, datagram); // can not fail. - fsm->retries = EC_FSM_RETRIES; - fsm->state = ec_fsm_soe_read_check; + + // mailbox read check is skipped if a read request is already ongoing + if (ec_read_mbox_locked(slave)) { + fsm->state = ec_fsm_soe_read_response_data; + // the datagram is not used and marked as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_soe_read_check; + } } /*****************************************************************************/ @@ -349,6 +359,7 @@ void ec_fsm_soe_read_check( if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_soe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Failed to receive SoE mailbox check datagram: "); ec_datagram_print_state(fsm->datagram); ec_fsm_soe_print_error(fsm); @@ -357,6 +368,7 @@ void ec_fsm_soe_read_check( if (fsm->datagram->working_counter != 1) { fsm->state = ec_fsm_soe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Reception of SoE mailbox check" " datagram failed: "); ec_datagram_print_wc_error(fsm->datagram); @@ -365,11 +377,22 @@ void ec_fsm_soe_read_check( } if (!ec_slave_mbox_check(fsm->datagram)) { - unsigned long diff_ms = - (fsm->datagram->jiffies_received - fsm->jiffies_start) * - 1000 / HZ; + unsigned long diff_ms = 0; + + // check that data is not already received by another read request + if (slave->mbox_soe_data.payload_size > 0) { + ec_read_mbox_lock_clear(slave); + fsm->state = ec_fsm_soe_read_response_data; + fsm->state(fsm, datagram); + return; + } + + diff_ms = (fsm->datagram->jiffies_received - fsm->jiffies_start) * + 1000 / HZ; + if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) { fsm->state = ec_fsm_soe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for" " read response.\n", diff_ms); ec_fsm_soe_print_error(fsm); @@ -397,11 +420,6 @@ void ec_fsm_soe_read_response( ) { ec_slave_t *slave = fsm->slave; - ec_master_t *master = slave->master; - uint8_t *data, mbox_prot, header, opcode, incomplete, error_flag, - value_included; - size_t rec_size, data_size; - ec_soe_request_t *req = fsm->request; if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. @@ -410,6 +428,7 @@ void ec_fsm_soe_read_response( if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_soe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Failed to receive SoE read response datagram: "); ec_datagram_print_state(fsm->datagram); ec_fsm_soe_print_error(fsm); @@ -417,14 +436,57 @@ void ec_fsm_soe_read_response( } if (fsm->datagram->working_counter != 1) { - fsm->state = ec_fsm_soe_error; - EC_SLAVE_ERR(slave, "Reception of SoE read response failed: "); - ec_datagram_print_wc_error(fsm->datagram); - ec_fsm_soe_print_error(fsm); + // only an error if data has not already been read by another read request + if (slave->mbox_soe_data.payload_size == 0) { + fsm->state = ec_fsm_soe_error; + ec_read_mbox_lock_clear(slave); + EC_SLAVE_ERR(slave, "Reception of SoE read response failed: "); + ec_datagram_print_wc_error(fsm->datagram); + ec_fsm_soe_print_error(fsm); + return; + } + } + ec_read_mbox_lock_clear(slave); + fsm->state = ec_fsm_soe_read_response_data; + fsm->state(fsm, datagram); +} + + +/*****************************************************************************/ + +/** + SoE state: READ RESPONSE DATA. + +*/ + +void ec_fsm_soe_read_response_data( + ec_fsm_soe_t *fsm, /**< finite state machine */ + ec_datagram_t *datagram /**< Datagram to use. */ + ) +{ + ec_slave_t *slave = fsm->slave; + ec_master_t *master = slave->master; + uint8_t *data, mbox_prot, header, opcode, incomplete, error_flag, + value_included; + size_t rec_size, data_size; + ec_soe_request_t *req = fsm->request; + + // process the data available or initiate a new mailbox read check + if (slave->mbox_soe_data.payload_size > 0) { + slave->mbox_soe_data.payload_size = 0; + } else { + // initiate a new mailbox read check if required data is not available + if (ec_read_mbox_locked(slave)) { + // await current read request and mark the datagram as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->state = ec_fsm_soe_read_check; + } return; } - data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); + data = ec_slave_mbox_fetch(slave, &slave->mbox_soe_data, &mbox_prot, &rec_size); if (IS_ERR(data)) { fsm->state = ec_fsm_soe_error; ec_fsm_soe_print_error(fsm); @@ -650,9 +712,17 @@ void ec_fsm_soe_write_request( } else { // all fragments sent; query response fsm->jiffies_start = fsm->datagram->jiffies_sent; - ec_slave_mbox_prepare_check(slave, datagram); // can not fail. - fsm->retries = EC_FSM_RETRIES; - fsm->state = ec_fsm_soe_write_check; + + // mailbox read check is skipped if a read request is already ongoing + if (ec_read_mbox_locked(slave)) { + fsm->state = ec_fsm_soe_write_response_data; + // the datagram is not used and marked as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_soe_write_check; + } } } @@ -674,6 +744,7 @@ void ec_fsm_soe_write_check( if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_soe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Failed to receive SoE write request datagram: "); ec_datagram_print_state(fsm->datagram); ec_fsm_soe_print_error(fsm); @@ -682,6 +753,7 @@ void ec_fsm_soe_write_check( if (fsm->datagram->working_counter != 1) { fsm->state = ec_fsm_soe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Reception of SoE write request datagram: "); ec_datagram_print_wc_error(fsm->datagram); ec_fsm_soe_print_error(fsm); @@ -689,10 +761,21 @@ void ec_fsm_soe_write_check( } if (!ec_slave_mbox_check(fsm->datagram)) { - unsigned long diff_ms = - (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; + unsigned long diff_ms = 0; + + // check that data is not already received by another read request + if (slave->mbox_soe_data.payload_size > 0) { + ec_read_mbox_lock_clear(slave); + fsm->state = ec_fsm_soe_write_response_data; + fsm->state(fsm, datagram); + return; + } + + diff_ms = (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; + if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) { fsm->state = ec_fsm_soe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting" " for write response.\n", diff_ms); ec_fsm_soe_print_error(fsm); @@ -720,11 +803,6 @@ void ec_fsm_soe_write_response( ) { ec_slave_t *slave = fsm->slave; - ec_master_t *master = slave->master; - ec_soe_request_t *req = fsm->request; - uint8_t *data, mbox_prot, opcode, error_flag; - uint16_t idn; - size_t rec_size; if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. @@ -733,6 +811,7 @@ void ec_fsm_soe_write_response( if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_soe_error; + ec_read_mbox_lock_clear(slave); EC_SLAVE_ERR(slave, "Failed to receive SoE write" " response datagram: "); ec_datagram_print_state(fsm->datagram); @@ -741,14 +820,57 @@ void ec_fsm_soe_write_response( } if (fsm->datagram->working_counter != 1) { - fsm->state = ec_fsm_soe_error; - EC_SLAVE_ERR(slave, "Reception of SoE write response failed: "); - ec_datagram_print_wc_error(fsm->datagram); - ec_fsm_soe_print_error(fsm); + // only an error if data has not already been read by another read request + if (slave->mbox_soe_data.payload_size == 0) { + fsm->state = ec_fsm_soe_error; + ec_read_mbox_lock_clear(slave); + EC_SLAVE_ERR(slave, "Reception of SoE write response failed: "); + ec_datagram_print_wc_error(fsm->datagram); + ec_fsm_soe_print_error(fsm); + return; + } + } + ec_read_mbox_lock_clear(slave); + fsm->state = ec_fsm_soe_write_response_data; + fsm->state(fsm, datagram); +} + + +/*****************************************************************************/ + +/** + SoE state: WRITE RESPONSE DATA. + +*/ + +void ec_fsm_soe_write_response_data( + ec_fsm_soe_t *fsm, /**< finite state machine */ + ec_datagram_t *datagram /**< Datagram to use. */ + ) +{ + ec_slave_t *slave = fsm->slave; + ec_master_t *master = slave->master; + ec_soe_request_t *req = fsm->request; + uint8_t *data, mbox_prot, opcode, error_flag; + uint16_t idn; + size_t rec_size; + + // process the data available or initiate a new mailbox read check + if (slave->mbox_soe_data.payload_size > 0) { + slave->mbox_soe_data.payload_size = 0; + } else { + // initiate a new mailbox read check if required data is not available + if (ec_read_mbox_locked(slave)) { + // await current read request and mark the datagram as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + fsm->state = ec_fsm_soe_write_check; + } return; } - data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); + data = ec_slave_mbox_fetch(slave, &slave->mbox_soe_data, &mbox_prot, &rec_size); if (IS_ERR(data)) { fsm->state = ec_fsm_soe_error; ec_fsm_soe_print_error(fsm); diff --git a/master/mailbox.c b/master/mailbox.c index d894bae5..8ceda622 100644 --- a/master/mailbox.c +++ b/master/mailbox.c @@ -164,18 +164,23 @@ const ec_code_msg_t mbox_error_messages[] = { * \return Pointer to the received data, or ERR_PTR() code. */ uint8_t *ec_slave_mbox_fetch(const ec_slave_t *slave, /**< slave */ - const ec_datagram_t *datagram, /**< datagram */ + ec_mbox_data_t *response_data, /**< response data */ uint8_t *type, /**< expected mailbox protocol */ size_t *size /**< size of the received data */ ) { size_t data_size; - data_size = EC_READ_U16(datagram->data); + if (!response_data->data) { + EC_SLAVE_ERR(slave, "No mailbox response data received!\n"); + return ERR_PTR(-EPROTO); + } + + data_size = EC_READ_U16(response_data->data); if (data_size + EC_MBOX_HEADER_SIZE > slave->configured_tx_mailbox_size) { EC_SLAVE_ERR(slave, "Corrupt mailbox response received!\n"); - ec_print_data(datagram->data, slave->configured_tx_mailbox_size); + ec_print_data(response_data->data, slave->configured_tx_mailbox_size); return ERR_PTR(-EPROTO); } @@ -186,12 +191,12 @@ uint8_t *ec_slave_mbox_fetch(const ec_slave_t *slave, /**< slave */ } #endif - *type = EC_READ_U8(datagram->data + 5) & 0x0F; + *type = EC_READ_U8(response_data->data + 5) & 0x0F; *size = data_size; if (*type == 0x00) { const ec_code_msg_t *mbox_msg; - uint16_t code = EC_READ_U16(datagram->data + 8); + uint16_t code = EC_READ_U16(response_data->data + 8); EC_SLAVE_ERR(slave, "Mailbox error response received - "); @@ -208,13 +213,13 @@ uint8_t *ec_slave_mbox_fetch(const ec_slave_t *slave, /**< slave */ } if (slave->master->debug_level && data_size > 0) { - ec_print_data(datagram->data + EC_MBOX_HEADER_SIZE, data_size); + ec_print_data(response_data->data + EC_MBOX_HEADER_SIZE, data_size); } return ERR_PTR(-EPROTO); } - return datagram->data + EC_MBOX_HEADER_SIZE; + return response_data->data + EC_MBOX_HEADER_SIZE; } /*****************************************************************************/ diff --git a/master/mailbox.h b/master/mailbox.h index 18a5d4ab..fc1f0a6f 100644 --- a/master/mailbox.h +++ b/master/mailbox.h @@ -64,7 +64,7 @@ uint8_t *ec_slave_mbox_prepare_send(const ec_slave_t *, ec_datagram_t *, int ec_slave_mbox_prepare_check(const ec_slave_t *, ec_datagram_t *); int ec_slave_mbox_check(const ec_datagram_t *); int ec_slave_mbox_prepare_fetch(const ec_slave_t *, ec_datagram_t *); -uint8_t *ec_slave_mbox_fetch(const ec_slave_t *, const ec_datagram_t *, +uint8_t *ec_slave_mbox_fetch(const ec_slave_t *, ec_mbox_data_t *, uint8_t *, size_t *); /*****************************************************************************/ diff --git a/master/master.c b/master/master.c index db1e1082..2936a646 100644 --- a/master/master.c +++ b/master/master.c @@ -48,6 +48,7 @@ #include "slave_config.h" #include "device.h" #include "datagram.h" +#include "mailbox.h" #ifdef EC_EOE #if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 11, 0) @@ -969,8 +970,10 @@ void ec_master_queue_datagram( } } - list_add_tail(&datagram->queue, &master->datagram_queue); - datagram->state = EC_DATAGRAM_QUEUED; + if (datagram->state != EC_DATAGRAM_INVALID) { + list_add_tail(&datagram->queue, &master->datagram_queue); + datagram->state = EC_DATAGRAM_QUEUED; + } } /*****************************************************************************/ @@ -1163,10 +1166,11 @@ void ec_master_receive_datagrams( ) { size_t frame_size, data_size; - uint8_t datagram_type, datagram_index; - unsigned int cmd_follows, matched; + uint8_t datagram_type, datagram_index, datagram_mbox_prot; + unsigned int cmd_follows, datagram_slave_addr, datagram_offset_addr, datagram_wc, matched; const uint8_t *cur_data; ec_datagram_t *datagram; + ec_slave_t *slave; if (unlikely(size < EC_FRAME_HEADER_SIZE)) { if (master->debug_level || FORCE_OUTPUT_CORRUPTED) { @@ -1208,6 +1212,8 @@ void ec_master_receive_datagrams( // process datagram header datagram_type = EC_READ_U8 (cur_data); datagram_index = EC_READ_U8 (cur_data + 1); + datagram_slave_addr = EC_READ_U16(cur_data + 2); + datagram_offset_addr = EC_READ_U16(cur_data + 4); data_size = EC_READ_U16(cur_data + 6) & 0x07FF; cmd_follows = EC_READ_U16(cur_data + 6) & 0x8000; cur_data += EC_DATAGRAM_HEADER_SIZE; @@ -1264,9 +1270,84 @@ void ec_master_receive_datagrams( datagram->type != EC_DATAGRAM_FPWR && datagram->type != EC_DATAGRAM_BWR && datagram->type != EC_DATAGRAM_LWR) { - // copy received data into the datagram memory, - // if something has been read - memcpy(datagram->data, cur_data, data_size); + + // common mailbox dispatcher for mailboxes read using the physical slave address + if (datagram->type == EC_DATAGRAM_FPRD) { + datagram_wc = EC_READ_U16(cur_data + data_size); + if (datagram_wc) { + if (master->slaves != NULL) { + for (slave = master->slaves; slave < master->slaves + master->slave_count; slave++) { + if (slave->station_address == datagram_slave_addr) { + break; + } + } + if (slave->station_address == datagram_slave_addr) { + if (slave->configured_tx_mailbox_offset != 0) { + if (datagram_offset_addr == slave->configured_tx_mailbox_offset) { + datagram_mbox_prot = EC_READ_U8(cur_data + 5) & 0x0F; + switch (datagram_mbox_prot) { +#ifdef EC_EOE + case EC_MBOX_TYPE_EOE: + if ((slave->mbox_eoe_data.data) && (data_size <= slave->mbox_eoe_data.data_size)) { + memcpy(slave->mbox_eoe_data.data, cur_data, data_size); + slave->mbox_eoe_data.payload_size = data_size; + } + break; +#endif + case EC_MBOX_TYPE_COE: + if ((slave->mbox_coe_data.data) && (data_size <= slave->mbox_coe_data.data_size)) { + memcpy(slave->mbox_coe_data.data, cur_data, data_size); + slave->mbox_coe_data.payload_size = data_size; + } + break; + case EC_MBOX_TYPE_FOE: + if ((slave->mbox_foe_data.data) && (data_size <= slave->mbox_foe_data.data_size)) { + memcpy(slave->mbox_foe_data.data, cur_data, data_size); + slave->mbox_foe_data.payload_size = data_size; + } + break; + case EC_MBOX_TYPE_SOE: + if ((slave->mbox_soe_data.data) && (data_size <= slave->mbox_soe_data.data_size)) { + memcpy(slave->mbox_soe_data.data, cur_data, data_size); + slave->mbox_soe_data.payload_size = data_size; + } + break; + case EC_MBOX_TYPE_VOE: + if ((slave->mbox_voe_data.data) && (data_size <= slave->mbox_voe_data.data_size)) { + memcpy(slave->mbox_voe_data.data, cur_data, data_size); + slave->mbox_voe_data.payload_size = data_size; + } + break; + default: + EC_MASTER_DBG(master, 1, "Unknown mailbox protocol from slave: %u Protocol: %u\n", datagram_slave_addr, datagram_mbox_prot); + // copy instead received data into the datagram memory. + memcpy(datagram->data, cur_data, data_size); + break; + } + } else { + // copy instead received data into the datagram memory. + memcpy(datagram->data, cur_data, data_size); + } + } else { + // copy instead received data into the datagram memory. + memcpy(datagram->data, cur_data, data_size); + } + } else { + EC_MASTER_DBG(master, 1, "No slave matching datagram slave address: %u\n", datagram_slave_addr); + } + } else { + EC_MASTER_DBG(master, 1, "No configured slaves!\n"); + // copy instead received data into the datagram memory. + memcpy(datagram->data, cur_data, data_size); + } + } else { + // copy instead received data into the datagram memory. + memcpy(datagram->data, cur_data, data_size); + } + } else { + // copy instead received data into the datagram memory. + memcpy(datagram->data, cur_data, data_size); + } } cur_data += data_size; @@ -2528,6 +2609,9 @@ void ecrt_master_deactivate(ec_master_t *master) // set states for all slaves ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP); + // clear read_mbox_busy flag in case slave CoE FSM were interrupted + ec_read_mbox_lock_clear(slave); + // mark for reconfiguration, because the master could have no // possibility for a reconfiguration between two sequential operation // phases. diff --git a/master/slave.c b/master/slave.c index db294c6a..0736e586 100644 --- a/master/slave.c +++ b/master/slave.c @@ -164,8 +164,52 @@ void ec_slave_init( // create state machine object ec_fsm_slave_init(&slave->fsm, slave); + + slave->read_mbox_busy = 0; + rt_mutex_init(&slave->mbox_sem); + +#ifdef EC_EOE + ec_mbox_data_init(&slave->mbox_eoe_data); +#endif + ec_mbox_data_init(&slave->mbox_coe_data); + ec_mbox_data_init(&slave->mbox_foe_data); + ec_mbox_data_init(&slave->mbox_soe_data); + ec_mbox_data_init(&slave->mbox_voe_data); } + +/*****************************************************************************/ + +/** + Clears the mailbox lock. +*/ +void ec_read_mbox_lock_clear(ec_slave_t *slave) +{ + rt_mutex_lock(&slave->mbox_sem); + slave->read_mbox_busy = 0; + rt_mutex_unlock(&slave->mbox_sem); +} + + +/*****************************************************************************/ + +/** + Return the current mailbox lock status and lock it if not locked. +*/ +int ec_read_mbox_locked(ec_slave_t *slave) +{ + int rc; + + rt_mutex_lock(&slave->mbox_sem); + rc = slave->read_mbox_busy; + if (!slave->read_mbox_busy) { + slave->read_mbox_busy = 1; + } + rt_mutex_unlock(&slave->mbox_sem); + return rc; +} + + /*****************************************************************************/ /** @@ -260,6 +304,15 @@ void ec_slave_clear(ec_slave_t *slave /**< EtherCAT slave */) kfree(slave->sii_words); } + // free mailbox response data +#ifdef EC_EOE + ec_mbox_data_clear(&slave->mbox_eoe_data); +#endif + ec_mbox_data_clear(&slave->mbox_coe_data); + ec_mbox_data_clear(&slave->mbox_foe_data); + ec_mbox_data_clear(&slave->mbox_soe_data); + ec_mbox_data_clear(&slave->mbox_voe_data); + ec_fsm_slave_clear(&slave->fsm); } diff --git a/master/slave.h b/master/slave.h index 8a0ddbac..46ece170 100644 --- a/master/slave.h +++ b/master/slave.h @@ -39,6 +39,7 @@ #include #include +#include #include "globals.h" #include "datagram.h" @@ -255,6 +256,17 @@ struct ec_slave struct list_head eoe_requests; /**< EoE set IP parameter requests. */ ec_fsm_slave_t fsm; /**< Slave state machine. */ + + uint8_t read_mbox_busy; /**< Flag set during a mailbox read request. */ + struct rt_mutex mbox_sem; /**< Semaphore protecting the check_mbox variable. */ + +#ifdef EC_EOE + ec_mbox_data_t mbox_eoe_data; /**< Received mailbox data for EoE. */ +#endif + ec_mbox_data_t mbox_coe_data; /**< Received mailbox data for CoE. */ + ec_mbox_data_t mbox_foe_data; /**< Received mailbox data for FoE. */ + ec_mbox_data_t mbox_soe_data; /**< Received mailbox data for SoE. */ + ec_mbox_data_t mbox_voe_data; /**< Received mailbox data for VoE. */ }; /*****************************************************************************/ @@ -292,6 +304,9 @@ void ec_slave_attach_pdo_names(ec_slave_t *); void ec_slave_calc_port_delays(ec_slave_t *); void ec_slave_calc_transmission_delays_rec(ec_slave_t *, uint32_t *); +void ec_read_mbox_lock_clear(ec_slave_t *); +int ec_read_mbox_locked(ec_slave_t *); + /*****************************************************************************/ #endif diff --git a/master/voe_handler.c b/master/voe_handler.c index 6e0a39b1..ea67c2e4 100644 --- a/master/voe_handler.c +++ b/master/voe_handler.c @@ -56,6 +56,7 @@ void ec_voe_handler_state_write_response(ec_voe_handler_t *); void ec_voe_handler_state_read_start(ec_voe_handler_t *); void ec_voe_handler_state_read_check(ec_voe_handler_t *); void ec_voe_handler_state_read_response(ec_voe_handler_t *); +void ec_voe_handler_state_read_response_data(ec_voe_handler_t *); void ec_voe_handler_state_read_nosync_start(ec_voe_handler_t *); void ec_voe_handler_state_read_nosync_response(ec_voe_handler_t *); @@ -303,11 +304,19 @@ void ec_voe_handler_state_read_start(ec_voe_handler_t *voe) return; } - ec_slave_mbox_prepare_check(slave, datagram); // can not fail. - voe->jiffies_start = jiffies; - voe->retries = EC_FSM_RETRIES; - voe->state = ec_voe_handler_state_read_check; + + // mailbox read check is skipped if a read request is already ongoing + if (ec_read_mbox_locked(slave)) { + voe->state = ec_voe_handler_state_read_response_data; + // the datagram is not used and marked as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + voe->jiffies_start = jiffies; + voe->retries = EC_FSM_RETRIES; + voe->state = ec_voe_handler_state_read_check; + } } /*****************************************************************************/ @@ -324,6 +333,7 @@ void ec_voe_handler_state_read_check(ec_voe_handler_t *voe) if (datagram->state != EC_DATAGRAM_RECEIVED) { voe->state = ec_voe_handler_state_error; + ec_read_mbox_lock_clear(slave); voe->request_state = EC_INT_REQUEST_FAILURE; EC_SLAVE_ERR(slave, "Failed to receive VoE mailbox check datagram: "); ec_datagram_print_state(datagram); @@ -332,6 +342,7 @@ void ec_voe_handler_state_read_check(ec_voe_handler_t *voe) if (datagram->working_counter != 1) { voe->state = ec_voe_handler_state_error; + ec_read_mbox_lock_clear(slave); voe->request_state = EC_INT_REQUEST_FAILURE; EC_SLAVE_ERR(slave, "Reception of VoE mailbox check" " datagram failed: "); @@ -340,10 +351,21 @@ void ec_voe_handler_state_read_check(ec_voe_handler_t *voe) } if (!ec_slave_mbox_check(datagram)) { - unsigned long diff_ms = - (datagram->jiffies_received - voe->jiffies_start) * 1000 / HZ; + unsigned long diff_ms = 0; + + // check that data is not already received by another read request + if (slave->mbox_voe_data.payload_size > 0) { + ec_read_mbox_lock_clear(slave); + voe->state = ec_voe_handler_state_read_response_data; + voe->state(voe); + return; + } + + diff_ms = (datagram->jiffies_received - voe->jiffies_start) * 1000 / HZ; + if (diff_ms >= EC_VOE_RESPONSE_TIMEOUT) { voe->state = ec_voe_handler_state_error; + ec_read_mbox_lock_clear(slave); voe->request_state = EC_INT_REQUEST_FAILURE; EC_SLAVE_ERR(slave, "Timeout while waiting for VoE data.\n"); return; @@ -368,15 +390,13 @@ void ec_voe_handler_state_read_response(ec_voe_handler_t *voe) { ec_datagram_t *datagram = &voe->datagram; ec_slave_t *slave = voe->config->slave; - ec_master_t *master = voe->config->master; - uint8_t *data, mbox_prot; - size_t rec_size; if (datagram->state == EC_DATAGRAM_TIMED_OUT && voe->retries--) return; if (datagram->state != EC_DATAGRAM_RECEIVED) { voe->state = ec_voe_handler_state_error; + ec_read_mbox_lock_clear(slave); voe->request_state = EC_INT_REQUEST_FAILURE; EC_SLAVE_ERR(slave, "Failed to receive VoE read datagram: "); ec_datagram_print_state(datagram); @@ -384,14 +404,53 @@ void ec_voe_handler_state_read_response(ec_voe_handler_t *voe) } if (datagram->working_counter != 1) { - voe->state = ec_voe_handler_state_error; - voe->request_state = EC_INT_REQUEST_FAILURE; - EC_SLAVE_ERR(slave, "Reception of VoE read response failed: "); - ec_datagram_print_wc_error(datagram); + // only an error if data has not already been read by another read request + if (slave->mbox_voe_data.payload_size == 0) { + voe->state = ec_voe_handler_state_error; + ec_read_mbox_lock_clear(slave); + voe->request_state = EC_INT_REQUEST_FAILURE; + EC_SLAVE_ERR(slave, "Reception of VoE read response failed: "); + ec_datagram_print_wc_error(datagram); + return; + } + } + ec_read_mbox_lock_clear(slave); + voe->state = ec_voe_handler_state_read_response_data; + voe->state(voe); +} + + +/*****************************************************************************/ + +/** + VoE state: READ RESPONSE DATA. + +*/ + +void ec_voe_handler_state_read_response_data(ec_voe_handler_t *voe) +{ + ec_datagram_t *datagram = &voe->datagram; + ec_slave_t *slave = voe->config->slave; + ec_master_t *master = voe->config->master; + uint8_t *data, mbox_prot; + size_t rec_size; + + // process the data available or initiate a new mailbox read check + if (slave->mbox_voe_data.payload_size > 0) { + slave->mbox_voe_data.payload_size = 0; + } else { + // initiate a new mailbox read check if required data is not available + if (ec_read_mbox_locked(slave)) { + // await current read request and mark the datagram as invalid + datagram->state = EC_DATAGRAM_INVALID; + } else { + ec_slave_mbox_prepare_check(slave, datagram); // can not fail. + voe->state = ec_voe_handler_state_read_check; + } return; } - data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); + data = ec_slave_mbox_fetch(slave, &slave->mbox_voe_data, &mbox_prot, &rec_size); if (IS_ERR(data)) { voe->state = ec_voe_handler_state_error; voe->request_state = EC_INT_REQUEST_FAILURE; @@ -421,6 +480,7 @@ void ec_voe_handler_state_read_response(ec_voe_handler_t *voe) } voe->data_size = rec_size - EC_VOE_HEADER_SIZE; + memcpy(voe->datagram.data + EC_MBOX_HEADER_SIZE, data, rec_size); voe->request_state = EC_INT_REQUEST_SUCCESS; voe->state = ec_voe_handler_state_end; // success } @@ -489,8 +549,10 @@ void ec_voe_handler_state_read_nosync_response(ec_voe_handler_t *voe) return; } - if (!(data = ec_slave_mbox_fetch(slave, datagram, - &mbox_prot, &rec_size))) { + if (slave->mbox_voe_data.payload_size > 0) { + slave->mbox_voe_data.payload_size = 0; + data = ec_slave_mbox_fetch(slave, &slave->mbox_voe_data, &mbox_prot, &rec_size); + } else { voe->state = ec_voe_handler_state_error; voe->request_state = EC_INT_REQUEST_FAILURE; return; @@ -519,6 +581,7 @@ void ec_voe_handler_state_read_nosync_response(ec_voe_handler_t *voe) } voe->data_size = rec_size - EC_VOE_HEADER_SIZE; + memcpy(voe->datagram.data + EC_MBOX_HEADER_SIZE, data, rec_size); voe->request_state = EC_INT_REQUEST_SUCCESS; voe->state = ec_voe_handler_state_end; // success }