use given send interval to limit SDO/FOE traffic
authorMartin Troxler <martin.troxler@komaxgroup.com>
Tue, 15 Dec 2009 10:55:36 +0100
changeset 1600 e36c92cf58a9
parent 1599 fe338fe37923
child 1601 a784812c787f
use given send interval to limit SDO/FOE traffic
include/ecrt.h
lib/master.c
master/cdev.c
master/globals.h
master/ioctl.h
master/master.c
master/master.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.
--- 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
     }
--- 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;
     }
--- 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
--- 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)
 
 /*****************************************************************************/
 
--- 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 <linux/delay.h>
 #include <linux/device.h>
 #include <linux/version.h>
-
+#include <linux/hrtimer.h>
 #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");
--- 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,