From 13e1e469e1c6e482ead89045a7dc56d72a4757c7 Mon Sep 17 00:00:00 2001 From: Bjarne von Horn Date: Fri, 12 Jan 2024 12:18:28 +0100 Subject: [PATCH] Use RT Mutex instead of semaphore for RT ioctl locking. --- master/ioctl.c | 54 ++++++++++++++++++++++++++++++++++--------------- master/master.c | 22 +++++++++++--------- master/master.h | 3 ++- 3 files changed, 53 insertions(+), 26 deletions(-) diff --git a/master/ioctl.c b/master/ioctl.c index 013e44ab..45db98fa 100644 --- a/master/ioctl.c +++ b/master/ioctl.c @@ -50,6 +50,19 @@ #define ATTRIBUTES #endif +#ifdef EC_IOCTL_RTDM +/* RTDM does not support locking yet, + * therefore no send/receive callbacks are set too. */ +# define ec_ioctl_lock(lock) do {} while(0) +# define ec_ioctl_unlock(lock) do {} while(0) +# define ec_ioctl_lock_interruptible(lock) (0) +#else +# define ec_ioctl_lock(lock) rt_mutex_lock(lock) +# define ec_ioctl_unlock(lock) rt_mutex_unlock(lock) +# define ec_ioctl_lock_interruptible(lock) \ + rt_mutex_lock_interruptible(lock) +#endif // EC_IOCTL_RTDM + /*****************************************************************************/ /** Copies a string to an ioctl structure. @@ -1812,6 +1825,7 @@ static ATTRIBUTES int ec_ioctl_activate( io.process_data_size = ctx->process_data_size; #ifndef EC_IOCTL_RTDM + /* RTDM does not support locking yet. */ ecrt_master_callbacks(master, ec_master_internal_send_cb, ec_master_internal_receive_cb, master); #endif @@ -1894,9 +1908,10 @@ static ATTRIBUTES int ec_ioctl_send( return -EPERM; } - down( & master->io_sem ); + if (ec_ioctl_lock_interruptible(&master->io_mutex)) + return -EINTR; ecrt_master_send(master); - up( & master->io_sem ); + ec_ioctl_unlock(&master->io_mutex); return 0; } @@ -1916,9 +1931,10 @@ static ATTRIBUTES int ec_ioctl_receive( return -EPERM; } - down( & master->io_sem ); + if (ec_ioctl_lock_interruptible(&master->io_mutex)) + return -EINTR; ecrt_master_receive(master); - up( & master->io_sem ); + ec_ioctl_unlock(&master->io_mutex); return 0; } @@ -2017,9 +2033,10 @@ static ATTRIBUTES int ec_ioctl_sync_ref( return -EPERM; } - down( & master->io_sem ); + if (ec_ioctl_lock_interruptible(&master->io_mutex)) + return -EINTR; ecrt_master_sync_reference_clock(master); - up( & master->io_sem ); + ec_ioctl_unlock(&master->io_mutex); return 0; } @@ -2044,9 +2061,10 @@ static ATTRIBUTES int ec_ioctl_sync_ref_to( return -EFAULT; } - down( & master->io_sem ); + if (ec_ioctl_lock_interruptible(&master->io_mutex)) + return -EINTR; ecrt_master_sync_reference_clock_to(master, time); - up( & master->io_sem ); + ec_ioctl_unlock(&master->io_mutex); return 0; } @@ -2066,9 +2084,10 @@ static ATTRIBUTES int ec_ioctl_sync_slaves( return -EPERM; } - down( & master->io_sem ); + if (ec_ioctl_lock_interruptible(&master->io_mutex)) + return -EINTR; ecrt_master_sync_slave_clocks(master); - up( & master->io_sem ); + ec_ioctl_unlock(&master->io_mutex); return 0; } @@ -2119,9 +2138,10 @@ static ATTRIBUTES int ec_ioctl_sync_mon_queue( return -EPERM; } - down( & master->io_sem ); + if (ec_ioctl_lock_interruptible(&master->io_mutex)) + return -EINTR; ecrt_master_sync_monitor_queue(master); - up( & master->io_sem ); + ec_ioctl_unlock(&master->io_mutex); return 0; } @@ -3185,9 +3205,10 @@ static ATTRIBUTES int ec_ioctl_domain_queue( return -ENOENT; } - down( & master->io_sem ); + if (ec_ioctl_lock_interruptible(&master->io_mutex)) + return -EINTR; ecrt_domain_queue(domain); - up( & master->io_sem ); + ec_ioctl_unlock(&master->io_mutex); return 0; } @@ -3905,9 +3926,10 @@ static ATTRIBUTES int ec_ioctl_voe_exec( return -ENOENT; } - down( & master->io_sem ); + if (ec_ioctl_lock_interruptible(&master->io_mutex)) + return -EINTR; data.state = ecrt_voe_handler_execute(voe); - up( & master->io_sem ); + ec_ioctl_unlock(&master->io_mutex); if (data.state == EC_REQUEST_SUCCESS && voe->dir == EC_DIR_INPUT) data.size = ecrt_voe_handler_data_size(voe); else diff --git a/master/master.c b/master/master.c index b675486e..3d533f4f 100644 --- a/master/master.c +++ b/master/master.c @@ -231,7 +231,7 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */ INIT_LIST_HEAD(&master->eoe_handlers); #endif - sema_init(&master->io_sem, 1); + rt_mutex_init(&master->io_mutex); master->send_cb = NULL; master->receive_cb = NULL; master->cb_data = NULL; @@ -523,9 +523,10 @@ void ec_master_internal_send_cb( ) { ec_master_t *master = (ec_master_t *) cb_data; - down(&master->io_sem); + if (rt_mutex_lock_interruptible(&master->io_mutex)) + return; ecrt_master_send_ext(master); - up(&master->io_sem); + rt_mutex_unlock(&master->io_mutex); } /*****************************************************************************/ @@ -537,9 +538,10 @@ void ec_master_internal_receive_cb( ) { ec_master_t *master = (ec_master_t *) cb_data; - down(&master->io_sem); + if (rt_mutex_lock_interruptible(&master->io_mutex)) + return; ecrt_master_receive(master); - up(&master->io_sem); + rt_mutex_unlock(&master->io_mutex); } /*****************************************************************************/ @@ -1517,9 +1519,10 @@ static int ec_master_idle_thread(void *priv_data) ec_datagram_output_stats(&master->fsm_datagram); // receive - down(&master->io_sem); + if (rt_mutex_lock_interruptible(&master->io_mutex)) + break; ecrt_master_receive(master); - up(&master->io_sem); + rt_mutex_unlock(&master->io_mutex); // execute master & slave state machines if (down_interruptible(&master->master_sem)) { @@ -1533,7 +1536,8 @@ static int ec_master_idle_thread(void *priv_data) up(&master->master_sem); // queue and send - down(&master->io_sem); + if (rt_mutex_lock_interruptible(&master->io_mutex)) + break; if (fsm_exec) { ec_master_queue_datagram(master, &master->fsm_datagram); } @@ -1542,7 +1546,7 @@ static int ec_master_idle_thread(void *priv_data) sent_bytes = master->devices[EC_DEVICE_MAIN].tx_skb[ master->devices[EC_DEVICE_MAIN].tx_ring_index]->len; #endif - up(&master->io_sem); + rt_mutex_unlock(&master->io_mutex); if (ec_fsm_master_idle(&master->fsm)) { #ifdef EC_USE_HRTIMER diff --git a/master/master.h b/master/master.h index 18281815..afdbde42 100644 --- a/master/master.h +++ b/master/master.h @@ -37,6 +37,7 @@ #include #include +#include #include #include "device.h" @@ -279,7 +280,7 @@ struct ec_master { struct list_head eoe_handlers; /**< Ethernet over EtherCAT handlers. */ #endif - struct semaphore io_sem; /**< Semaphore used in \a IDLE phase. */ + struct rt_mutex io_mutex; /**< Mutex used in \a IDLE and \a OP phase. */ void (*send_cb)(void *); /**< Current send datagrams callback. */ void (*receive_cb)(void *); /**< Current receive datagrams callback. */