--- 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,