--- a/include/ecrt.h	Mon Nov 30 12:26:24 2009 +0100
+++ b/include/ecrt.h	Mon Nov 30 12:26:56 2009 +0100
@@ -698,6 +698,15 @@
         ec_master_t *master /**< EtherCAT master. */
         );
 
+
+/** Set max. number of databytes in a cycle
+ *
+ */
+int ecrt_master_set_max_cycle_size(
+        ec_master_t *master, /**< EtherCAT master. */
+        size_t max_cycle_data_size /**< Max. number of databytes in a cycle */
+        );
+
 /** Sends all datagrams in the queue.
  *
  * This method takes all datagrams, that have been queued for transmission,

--- a/lib/master.c	Mon Nov 30 12:26:24 2009 +0100
+++ b/lib/master.c	Mon Nov 30 12:26:56 2009 +0100
@@ -344,6 +344,21 @@
     }
 }
 
+
+/*****************************************************************************/
+
+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",
+                strerror(errno));
+        return -1; // FIXME
+    }
+    return 0;
+}
+
+
 /*****************************************************************************/
 
 void ecrt_master_send(ec_master_t *master)

--- a/master/cdev.c	Mon Nov 30 12:26:24 2009 +0100
+++ b/master/cdev.c	Mon Nov 30 12:26:56 2009 +0100
@@ -1673,6 +1673,32 @@
     return 0;
 }
 
+
+/*****************************************************************************/
+
+/** 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;
+    up(&master->master_sem);
+
+    return 0;
+}
+
+
 /*****************************************************************************/
 
 /** Send frames.
@@ -3466,6 +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);
         default:
             return -ENOTTY;
     }

--- a/master/globals.h	Mon Nov 30 12:26:24 2009 +0100
+++ b/master/globals.h	Mon Nov 30 12:26:56 2009 +0100
@@ -48,6 +48,12 @@
 /** Datagram timeout in microseconds. */
 #define EC_IO_TIMEOUT 500
 
+/** SDO injection timeout in microseconds. */
+#define EC_SDO_INJECTION_TIMEOUT 10000
+
+/** time to send a byte in nanoseconds. */
+#define EC_BYTE_TRANSMITION_TIME 80LL
+
 /** Number of state machine retries on datagram timeout. */
 #define EC_FSM_RETRIES 3
 

--- a/master/ioctl.h	Mon Nov 30 12:26:24 2009 +0100
+++ b/master/ioctl.h	Mon Nov 30 12:26:56 2009 +0100
@@ -123,6 +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)
 
 /*****************************************************************************/
 

--- a/master/master.c	Mon Nov 30 12:26:24 2009 +0100
+++ b/master/master.c	Mon Nov 30 12:26:56 2009 +0100
@@ -59,13 +59,14 @@
 /** Frame timeout in cycles.
  */
 static cycles_t timeout_cycles;
-
+static cycles_t sdo_injection_timeout_cycles;
 #else
 
 /** Frame timeout in jiffies.
  */
 static unsigned long timeout_jiffies;
-    
+static unsigned long sdo_injection_timeout_jiffies;
+
 #endif
 
 /*****************************************************************************/
@@ -87,9 +88,11 @@
 {
 #ifdef EC_HAVE_CYCLES
     timeout_cycles = (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000);
+    sdo_injection_timeout_cycles = (cycles_t) EC_SDO_INJECTION_TIMEOUT /* us */ * (cpu_khz / 1000);
 #else
     // one jiffy may always elapse between time measurement
     timeout_jiffies = max(EC_IO_TIMEOUT * HZ / 1000000, 1);
+    sdo_injection_timeout_jiffies = max(EC_SDO_INJECTION_TIMEOUT * HZ / 1000000, 1);
 #endif
 }
 
@@ -152,6 +155,7 @@
     sema_init(&master->ext_queue_sem, 1);
 
     INIT_LIST_HEAD(&master->sdo_datagram_queue);
+    master->max_queue_size = EC_MAX_DATA_SIZE;
 
     INIT_LIST_HEAD(&master->domains);
 
@@ -695,12 +699,56 @@
         ec_master_t *master /**< EtherCAT master */
         )
 {
-    ec_datagram_t *sdo_datagram, *n;
-    list_for_each_entry_safe(sdo_datagram, n, &master->sdo_datagram_queue, queue) {
-        list_del_init(&sdo_datagram->queue);
-        if (master->debug_level)
-            EC_DBG("queuing SDO datagram %08x\n",(unsigned int)sdo_datagram);
-        ec_master_queue_datagram(master, sdo_datagram);
+    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->sdo_datagram_queue, queue) {
+        queue_size += datagram->data_size;
+        if (queue_size <= master->max_queue_size) {
+            list_del_init(&datagram->queue);
+            if (master->debug_level) {
+                EC_DBG("Injecting SDO datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size);
+            }
+#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("SDO 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) {
+#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;
+#ifdef EC_HAVE_CYCLES
+                    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 SDO datagram %08x size=%u, max_queue_size=%u\n",time_us,(unsigned int)datagram,datagram->data_size,master->max_queue_size);
+                }
+                else  {
+                    if (master->debug_level) {
+                        EC_DBG("Deferred injecting of SDO datagram %08x size=%u, queue_size=%u\n",(unsigned int)datagram,datagram->data_size,queue_size);
+                    }
+                }
+            }
+        }
     }
 }
 
@@ -713,9 +761,17 @@
         ec_datagram_t *datagram /**< datagram */
         )
 {
+    if (master->debug_level) {
+        EC_DBG("Requesting SDO datagram %08x size=%u\n",(unsigned int)datagram,datagram->data_size);
+    }
+    datagram->state = EC_DATAGRAM_QUEUED;
+#ifdef EC_HAVE_CYCLES
+    datagram->cycles_sent = get_cycles();
+#endif
+    datagram->jiffies_sent = jiffies;
+
     down(&master->io_sem);
     list_add_tail(&datagram->queue, &master->sdo_datagram_queue);
-    datagram->state = EC_DATAGRAM_QUEUED;
     up(&master->io_sem);
 }
 

--- a/master/master.h	Mon Nov 30 12:26:24 2009 +0100
+++ b/master/master.h	Mon Nov 30 12:26:56 2009 +0100
@@ -155,7 +155,7 @@
                                       ext_datagram_queue. */
 
     struct list_head sdo_datagram_queue; /**< SDO Datagram queue. */
-
+    size_t max_queue_size; /** max. size of datagram queue */
     struct list_head domains; /**< List of domains. */
 
     unsigned int debug_level; /**< Master debug level. */