# HG changeset patch # User Martin Troxler # Date 1260870936 -3600 # Node ID e36c92cf58a9600cc5fc61bbf7dcc6cc36d29735 # Parent fe338fe379235062c543dae0ea954a871b1edd6f use given send interval to limit SDO/FOE traffic diff -r fe338fe37923 -r e36c92cf58a9 include/ecrt.h --- a/include/ecrt.h Tue Dec 15 10:53:28 2009 +0100 +++ b/include/ecrt.h Tue Dec 15 10:55:36 2009 +0100 @@ -699,12 +699,12 @@ ); -/** Set max. number of databytes in a cycle - * - */ -int ecrt_master_set_max_cycle_size( +/** Set interval between calls to ecrt_master_send + * + */ +int ecrt_master_set_send_interval( ec_master_t *master, /**< EtherCAT master. */ - size_t max_cycle_data_size /**< Max. number of databytes in a cycle */ + size_t send_interval /**< Send interval in us */ ); /** Sends all datagrams in the queue. diff -r fe338fe37923 -r e36c92cf58a9 lib/master.c --- a/lib/master.c Tue Dec 15 10:53:28 2009 +0100 +++ b/lib/master.c Tue Dec 15 10:55:36 2009 +0100 @@ -347,11 +347,11 @@ /*****************************************************************************/ -int ecrt_master_set_max_cycle_size(ec_master_t *master,size_t max_cycle_data_size) -{ - if (ioctl(master->fd, EC_IOCTL_SET_MAX_CYCLE_SIZE, - &max_cycle_data_size) == -1) { - fprintf(stderr, "Failed to activate master: %s\n", +int ecrt_master_set_send_interval(ec_master_t *master,size_t send_interval_us) +{ + if (ioctl(master->fd, EC_IOCTL_SET_SEND_INTERVAL, + &send_interval_us) == -1) { + fprintf(stderr, "Failed to set send interval: %s\n", strerror(errno)); return -1; // FIXME } diff -r fe338fe37923 -r e36c92cf58a9 master/cdev.c --- a/master/cdev.c Tue Dec 15 10:53:28 2009 +0100 +++ b/master/cdev.c Tue Dec 15 10:55:36 2009 +0100 @@ -1678,21 +1678,21 @@ /** Set max. number of databytes in a cycle */ -int ec_cdev_ioctl_set_max_cycle_size( - ec_master_t *master, /**< EtherCAT master. */ - unsigned long arg, /**< ioctl() argument. */ - ec_cdev_priv_t *priv /**< Private data structure of file handle. */ - ) -{ - size_t max_cycle_size; - - if (copy_from_user(&max_cycle_size, (void __user *) arg, sizeof(max_cycle_size))) { - return -EFAULT; - } - - if (down_interruptible(&master->master_sem)) - return -EINTR; - master->max_queue_size = max_cycle_size; +int ec_cdev_ioctl_set_send_interval( + ec_master_t *master, /**< EtherCAT master. */ + unsigned long arg, /**< ioctl() argument. */ + ec_cdev_priv_t *priv /**< Private data structure of file handle. */ + ) +{ + size_t send_interval; + + if (copy_from_user(&send_interval, (void __user *) arg, sizeof(send_interval))) { + return -EFAULT; + } + + if (down_interruptible(&master->master_sem)) + return -EINTR; + ec_master_set_send_interval(master,send_interval); up(&master->master_sem); return 0; @@ -3492,10 +3492,10 @@ return ec_cdev_ioctl_voe_exec(master, arg, priv); case EC_IOCTL_VOE_DATA: return ec_cdev_ioctl_voe_data(master, arg, priv); - case EC_IOCTL_SET_MAX_CYCLE_SIZE: - if (!(filp->f_mode & FMODE_WRITE)) - return -EPERM; - return ec_cdev_ioctl_set_max_cycle_size(master,arg,priv); + case EC_IOCTL_SET_SEND_INTERVAL: + if (!(filp->f_mode & FMODE_WRITE)) + return -EPERM; + return ec_cdev_ioctl_set_send_interval(master,arg,priv); default: return -ENOTTY; } diff -r fe338fe37923 -r e36c92cf58a9 master/globals.h --- a/master/globals.h Tue Dec 15 10:53:28 2009 +0100 +++ b/master/globals.h Tue Dec 15 10:55:36 2009 +0100 @@ -52,7 +52,7 @@ #define EC_SDO_INJECTION_TIMEOUT 10000 /** time to send a byte in nanoseconds. */ -#define EC_BYTE_TRANSMITION_TIME 80LL +#define EC_BYTE_TRANSMITION_TIME 80 /** Number of state machine retries on datagram timeout. */ #define EC_FSM_RETRIES 3 diff -r fe338fe37923 -r e36c92cf58a9 master/ioctl.h --- a/master/ioctl.h Tue Dec 15 10:53:28 2009 +0100 +++ b/master/ioctl.h Tue Dec 15 10:55:36 2009 +0100 @@ -123,7 +123,7 @@ #define EC_IOCTL_VOE_WRITE EC_IOWR(0x3f, ec_ioctl_voe_t) #define EC_IOCTL_VOE_EXEC EC_IOWR(0x40, ec_ioctl_voe_t) #define EC_IOCTL_VOE_DATA EC_IOWR(0x41, ec_ioctl_voe_t) -#define EC_IOCTL_SET_MAX_CYCLE_SIZE EC_IOW(0x42, size_t) +#define EC_IOCTL_SET_SEND_INTERVAL EC_IOW(0x42, size_t) /*****************************************************************************/ diff -r fe338fe37923 -r e36c92cf58a9 master/master.c --- a/master/master.c Tue Dec 15 10:53:28 2009 +0100 +++ b/master/master.c Tue Dec 15 10:55:36 2009 +0100 @@ -41,7 +41,7 @@ #include #include #include - +#include #include "globals.h" #include "slave.h" #include "slave_config.h" @@ -159,7 +159,7 @@ sema_init(&master->ext_queue_sem, 1); INIT_LIST_HEAD(&master->external_datagram_queue); - master->max_queue_size = EC_MAX_DATA_SIZE; + ec_master_set_send_interval(master,1000000 / HZ); // send interval in IDLE phase INIT_LIST_HEAD(&master->domains); @@ -692,65 +692,81 @@ /** Injects external datagrams that fit into the datagram queue */ void ec_master_inject_external_datagrams( - ec_master_t *master /**< EtherCAT master */ - ) -{ - ec_datagram_t *datagram, *n; - size_t queue_size = 0; - list_for_each_entry(datagram, &master->datagram_queue, queue) { - queue_size += datagram->data_size; - } - list_for_each_entry_safe(datagram, n, &master->external_datagram_queue, queue) { - queue_size += datagram->data_size; - if (queue_size <= master->max_queue_size) { - list_del_init(&datagram->queue); + ec_master_t *master /**< EtherCAT master */ + ) +{ + ec_datagram_t *datagram, *n; + size_t queue_size = 0; + list_for_each_entry(datagram, &master->datagram_queue, queue) { + queue_size += datagram->data_size; + } + list_for_each_entry_safe(datagram, n, &master->external_datagram_queue, queue) { + queue_size += datagram->data_size; + if (queue_size <= master->max_queue_size) { + list_del_init(&datagram->queue); #if DEBUG_INJECT - if (master->debug_level) { - EC_DBG("Injecting external datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size); - } + if (master->debug_level) { + EC_DBG("Injecting external datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size); + } #endif #ifdef EC_HAVE_CYCLES - datagram->cycles_sent = 0; -#endif - datagram->jiffies_sent = 0; - ec_master_queue_datagram(master, datagram); - } - else { - if (datagram->data_size > master->max_queue_size) { - list_del_init(&datagram->queue); - datagram->state = EC_DATAGRAM_ERROR; - EC_ERR("External datagram %08x is too large, size=%u, max_queue_size=%u\n",(unsigned int)datagram,datagram->data_size,master->max_queue_size); - } - else { + datagram->cycles_sent = 0; +#endif + datagram->jiffies_sent = 0; + ec_master_queue_datagram(master, datagram); + } + else { + if (datagram->data_size > master->max_queue_size) { + list_del_init(&datagram->queue); + datagram->state = EC_DATAGRAM_ERROR; + EC_ERR("External datagram %08x is too large, size=%u, max_queue_size=%u\n",(unsigned int)datagram,datagram->data_size,master->max_queue_size); + } + else { #ifdef EC_HAVE_CYCLES - cycles_t cycles_now = get_cycles(); - if (cycles_now - datagram->cycles_sent - > sdo_injection_timeout_cycles) { + cycles_t cycles_now = get_cycles(); + if (cycles_now - datagram->cycles_sent + > sdo_injection_timeout_cycles) { #else - if (jiffies - datagram->jiffies_sent - > sdo_injection_timeout_jiffies) { -#endif - unsigned int time_us; - list_del_init(&datagram->queue); - datagram->state = EC_DATAGRAM_ERROR; + if (jiffies - datagram->jiffies_sent + > sdo_injection_timeout_jiffies) { +#endif + unsigned int time_us; + list_del_init(&datagram->queue); + datagram->state = EC_DATAGRAM_ERROR; #ifdef EC_HAVE_CYCLES - time_us = (unsigned int) ((cycles_now - datagram->cycles_sent) * 1000LL) / cpu_khz; + time_us = (unsigned int) ((cycles_now - datagram->cycles_sent) * 1000LL) / cpu_khz; #else - time_us = (unsigned int) ((jiffies - datagram->jiffies_sent) * 1000000 / HZ); -#endif - EC_ERR("Timeout %u us: injecting external datagram %08x size=%u, max_queue_size=%u\n",time_us,(unsigned int)datagram,datagram->data_size,master->max_queue_size); - } - else { + time_us = (unsigned int) ((jiffies - datagram->jiffies_sent) * 1000000 / HZ); +#endif + EC_ERR("Timeout %u us: injecting external datagram %08x size=%u, max_queue_size=%u\n",time_us,(unsigned int)datagram,datagram->data_size,master->max_queue_size); + } + else { #if DEBUG_INJECT - if (master->debug_level) { - EC_DBG("Deferred injecting of external datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size); - } -#endif - } - } - } - } -} + if (master->debug_level) { + EC_DBG("Deferred injecting of external datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size); + } +#endif + } + } + } + } +} + +/*****************************************************************************/ + +/** sets the expected interval between calls to ecrt_master_send + and calculates the maximum amount of data to queue + */ +void ec_master_set_send_interval( + ec_master_t *master, /**< EtherCAT master */ + size_t send_interval /**< send interval */ + ) +{ + master->send_interval = send_interval; + master->max_queue_size = (send_interval * 1000) / EC_BYTE_TRANSMITION_TIME; + master->max_queue_size -= master->max_queue_size / 10; +} + /*****************************************************************************/ @@ -761,22 +777,30 @@ ec_datagram_t *datagram /**< datagram */ ) { + ec_datagram_t *queued_datagram; + + down(&master->io_sem); + // check, if the datagram is already queued + list_for_each_entry(queued_datagram, &master->external_datagram_queue, queue) { + if (queued_datagram == datagram) { + datagram->state = EC_DATAGRAM_QUEUED; + return; + } + } #if DEBUG_INJECT - if (master->debug_level) { - EC_DBG("Requesting external datagram %08x size=%u\n",(unsigned int)datagram,datagram->data_size); - } -#endif - datagram->state = EC_DATAGRAM_QUEUED; + if (master->debug_level) { + EC_DBG("Requesting external datagram %08x size=%u\n",(unsigned int)datagram,datagram->data_size); + } +#endif + list_add_tail(&datagram->queue, &master->external_datagram_queue); + datagram->state = EC_DATAGRAM_QUEUED; #ifdef EC_HAVE_CYCLES - datagram->cycles_sent = get_cycles(); -#endif - datagram->jiffies_sent = jiffies; - - master->fsm.idle = 0; - - down(&master->io_sem); - list_add_tail(&datagram->queue, &master->external_datagram_queue); - up(&master->io_sem); + datagram->cycles_sent = get_cycles(); +#endif + datagram->jiffies_sent = jiffies; + + master->fsm.idle = 0; + up(&master->io_sem); } /*****************************************************************************/ @@ -1091,6 +1115,49 @@ } } + +/*****************************************************************************/ +/* + * Sleep related functions: + */ +static enum hrtimer_restart ec_master_nanosleep_wakeup(struct hrtimer *timer) +{ + struct hrtimer_sleeper *t = + container_of(timer, struct hrtimer_sleeper, timer); + struct task_struct *task = t->task; + + t->task = NULL; + if (task) + wake_up_process(task); + + return HRTIMER_NORESTART; +} + +void ec_master_nanosleep(const unsigned long nsecs) +{ + struct hrtimer_sleeper t; + enum hrtimer_mode mode = HRTIMER_MODE_REL; + hrtimer_init(&t.timer, CLOCK_MONOTONIC,mode); + t.timer.function = ec_master_nanosleep_wakeup; + t.task = current; +#ifdef CONFIG_HIGH_RES_TIMERS + t.timer.cb_mode = HRTIMER_CB_IRQSAFE_NO_RESTART; +#endif + t.timer.expires = ktime_set(0,nsecs); + do { + set_current_state(TASK_INTERRUPTIBLE); + hrtimer_start(&t.timer, t.timer.expires, mode); + + if (likely(t.task)) + schedule(); + + hrtimer_cancel(&t.timer); + mode = HRTIMER_MODE_ABS; + + } while (t.task && !signal_pending(current)); +} + + /*****************************************************************************/ /** Master kernel thread function for IDLE phase. @@ -1100,8 +1167,9 @@ ec_master_t *master = (ec_master_t *) priv_data; ec_slave_t *slave = NULL; int fsm_exec; - if (master->debug_level) - EC_DBG("Idle thread running.\n"); + ec_master_set_send_interval(master,1000000 / HZ); // send interval in IDLE phase + if (master->debug_level) + EC_DBG("Idle thread running with send interval = %d us, max data size=%d\n",master->send_interval,master->max_queue_size); while (!kthread_should_stop()) { ec_datagram_output_stats(&master->fsm_datagram); @@ -1132,13 +1200,10 @@ ecrt_master_send(master); up(&master->io_sem); - if (ec_fsm_master_idle(&master->fsm)) { - set_current_state(TASK_INTERRUPTIBLE); - schedule_timeout(1); - } - else { - schedule(); - } + if (ec_fsm_master_idle(&master->fsm)) + ec_master_nanosleep(master->send_interval*1000); + else + schedule(); } if (master->debug_level) @@ -1156,7 +1221,7 @@ ec_slave_t *slave = NULL; int fsm_exec; if (master->debug_level) - EC_DBG("Operation thread running.\n"); + EC_DBG("Operation thread running with fsm interval = %d us, max data size=%d\n",master->send_interval,master->max_queue_size); while (!kthread_should_stop()) { ec_datagram_output_stats(&master->fsm_datagram); @@ -1180,14 +1245,9 @@ if (fsm_exec) master->injection_seq_fsm++; } - if (ec_fsm_master_idle(&master->fsm)) { - set_current_state(TASK_INTERRUPTIBLE); - schedule_timeout(1); - } - else { - schedule(); - } - } + // the op thread should not work faster than the sending RT thread + ec_master_nanosleep(master->send_interval*1000); + } if (master->debug_level) EC_DBG("Master OP thread exiting...\n"); diff -r fe338fe37923 -r e36c92cf58a9 master/master.h --- a/master/master.h Tue Dec 15 10:53:28 2009 +0100 +++ b/master/master.h Tue Dec 15 10:55:36 2009 +0100 @@ -155,6 +155,7 @@ ext_datagram_queue. */ struct list_head external_datagram_queue; /**< External Datagram queue. */ + size_t send_interval; /* interval between calls to ecrt_master_send */ size_t max_queue_size; /** max. size of datagram queue */ struct list_head domains; /**< List of domains. */ @@ -220,6 +221,7 @@ void ec_master_inject_external_datagrams(ec_master_t *); // misc. +void ec_master_set_send_interval(ec_master_t *,size_t); void ec_master_attach_slave_configs(ec_master_t *); ec_slave_t *ec_master_find_slave(ec_master_t *, uint16_t, uint16_t); const ec_slave_t *ec_master_find_slave_const(const ec_master_t *, uint16_t,