Merged changes by Martin Troxler.
authorFlorian Pose <fp@igh-essen.com>
Mon, 14 Dec 2009 13:25:50 +0100
changeset 1586 eb9185dfa8ac
parent 1581 e51cf2af3ff9 (current diff)
parent 1585 1f640e321ee4 (diff)
child 1587 d613db7ca61c
Merged changes by Martin Troxler.
master/cdev.c
master/fsm_master.c
master/globals.h
master/master.c
master/module.c
master/slave.c
--- a/include/ecrt.h	Mon Dec 14 13:11:36 2009 +0100
+++ b/include/ecrt.h	Mon Dec 14 13:25:50 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 Dec 14 13:11:36 2009 +0100
+++ b/lib/master.c	Mon Dec 14 13:25:50 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/Kbuild.in	Mon Dec 14 13:11:36 2009 +0100
+++ b/master/Kbuild.in	Mon Dec 14 13:25:50 2009 +0100
@@ -44,6 +44,7 @@
 	fsm_coe.o \
 	fsm_foe.o \
 	fsm_master.o \
+	fsm_slave.o \
 	fsm_pdo.o \
 	fsm_pdo_entry.o \
 	fsm_sii.o \
--- a/master/Makefile.am	Mon Dec 14 13:11:36 2009 +0100
+++ b/master/Makefile.am	Mon Dec 14 13:25:50 2009 +0100
@@ -43,6 +43,7 @@
 	fsm_coe.c fsm_coe.h \
 	fsm_foe.c fsm_foe.h \
 	fsm_master.c fsm_master.h \
+	fsm_slave.c fsm_slave.h \
 	fsm_pdo.c fsm_pdo.h \
 	fsm_pdo_entry.c fsm_pdo_entry.h \
 	fsm_sii.c fsm_sii.h \
--- a/master/cdev.c	Mon Dec 14 13:11:36 2009 +0100
+++ b/master/cdev.c	Mon Dec 14 13:25:50 2009 +0100
@@ -805,6 +805,8 @@
         return -EINVAL;
     }
 
+    if (master->debug_level)
+        EC_DBG("Schedule SDO upload request for slave %u\n",request.slave->ring_position);
     // schedule request.
     list_add_tail(&request.list, &master->slave_sdo_requests);
 
@@ -816,7 +818,7 @@
         // interrupted by signal
         down(&master->master_sem);
         if (request.req.state == EC_INT_REQUEST_QUEUED) {
-            list_del(&request.req.list);
+            list_del(&request.list);
             up(&master->master_sem);
             ec_sdo_request_clear(&request.req);
             return -EINTR;
@@ -828,6 +830,9 @@
     // wait until master FSM has finished processing
     wait_event(master->sdo_queue, request.req.state != EC_INT_REQUEST_BUSY);
 
+    if (master->debug_level)
+        EC_DBG("Scheduled SDO upload request for slave %u done\n",request.slave->ring_position);
+
     data.abort_code = request.req.abort_code;
 
     if (request.req.state != EC_INT_REQUEST_SUCCESS) {
@@ -906,6 +911,8 @@
         return -EINVAL;
     }
     
+    if (master->debug_level)
+        EC_DBG("Schedule SDO download request for slave %u\n",request.slave->ring_position);
     // schedule request.
     list_add_tail(&request.list, &master->slave_sdo_requests);
 
@@ -917,7 +924,7 @@
         // interrupted by signal
         down(&master->master_sem);
         if (request.req.state == EC_INT_REQUEST_QUEUED) {
-            list_del(&request.req.list);
+            list_del(&request.list);
             up(&master->master_sem);
             ec_sdo_request_clear(&request.req);
             return -EINTR;
@@ -929,6 +936,9 @@
     // wait until master FSM has finished processing
     wait_event(master->sdo_queue, request.req.state != EC_INT_REQUEST_BUSY);
 
+    if (master->debug_level)
+        EC_DBG("Scheduled SDO download request for slave %u done\n",request.slave->ring_position);
+
     data.abort_code = request.req.abort_code;
 
     retval = request.req.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO;
@@ -1663,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.
@@ -3456,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/fsm_master.c	Mon Dec 14 13:11:36 2009 +0100
+++ b/master/fsm_master.c	Mon Dec 14 13:25:50 2009 +0100
@@ -60,6 +60,7 @@
 void ec_fsm_master_state_reg_request(ec_fsm_master_t *);
 void ec_fsm_master_state_foe_request(ec_fsm_master_t *);
 
+
 /*****************************************************************************/
 
 /** Constructor.
@@ -115,18 +116,21 @@
  *
  * If the state machine's datagram is not sent or received yet, the execution
  * of the state machine is delayed to the next cycle.
- */
-void ec_fsm_master_exec(
+ *
+ * \return true, if the state machine was executed
+ */
+int ec_fsm_master_exec(
         ec_fsm_master_t *fsm /**< Master state machine. */
         )
 {
     if (fsm->datagram->state == EC_DATAGRAM_SENT
         || fsm->datagram->state == EC_DATAGRAM_QUEUED) {
         // datagram was not sent or received yet.
-        return;
+        return 0;
     }
 
     fsm->state(fsm);
+    return 1;
 }
 
 /*****************************************************************************/
@@ -411,7 +415,6 @@
     ec_master_t *master = fsm->master;
     ec_slave_t *slave;
     ec_sdo_request_t *req;
-    ec_master_sdo_request_t *request;
 
     // search for internal requests to be processed
     for (slave = master->slaves;
@@ -425,7 +428,7 @@
                 if (ec_sdo_request_timed_out(req)) {
                     req->state = EC_INT_REQUEST_FAILURE;
                     if (master->debug_level)
-                        EC_DBG("SDO request for slave %u timed out...\n",
+                        EC_DBG("Internal SDO request for slave %u timed out...\n",
                                 slave->ring_position);
                     continue;
                 }
@@ -437,7 +440,7 @@
 
                 req->state = EC_INT_REQUEST_BUSY;
                 if (master->debug_level)
-                    EC_DBG("Processing SDO request for slave %u...\n",
+                    EC_DBG("Processing internal SDO request for slave %u...\n",
                             slave->ring_position);
 
                 fsm->idle = 0;
@@ -450,42 +453,6 @@
             }
         }
     }
-
-    // search the first external request to be processed
-    while (1) {
-        if (list_empty(&master->slave_sdo_requests))
-            break;
-
-        // get first request
-        request = list_entry(master->slave_sdo_requests.next,
-                ec_master_sdo_request_t, list);
-        list_del_init(&request->list); // dequeue
-        request->req.state = EC_INT_REQUEST_BUSY;
-
-        slave = request->slave;
-        if (slave->current_state == EC_SLAVE_STATE_INIT) {
-            EC_ERR("Discarding SDO request, slave %u is in INIT.\n",
-                    slave->ring_position);
-            request->req.state = EC_INT_REQUEST_FAILURE;
-            wake_up(&master->sdo_queue);
-            continue;
-        }
-
-        // Found pending SDO request. Execute it!
-        if (master->debug_level)
-            EC_DBG("Processing SDO request for slave %u...\n",
-                    slave->ring_position);
-
-        // Start uploading SDO
-        fsm->idle = 0;
-        fsm->sdo_request = &request->req;
-        fsm->slave = slave;
-        fsm->state = ec_fsm_master_state_sdo_request;
-        ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req);
-        ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
-        return 1;
-    }
-
     return 0;
 }
 
@@ -1047,7 +1014,7 @@
     if (ec_fsm_coe_exec(&fsm->fsm_coe)) return;
 
     if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
-        EC_DBG("Failed to process SDO request for slave %u.\n",
+        EC_DBG("Failed to process internal SDO request for slave %u.\n",
                 fsm->slave->ring_position);
         request->state = EC_INT_REQUEST_FAILURE;
         wake_up(&master->sdo_queue);
@@ -1060,7 +1027,7 @@
     wake_up(&master->sdo_queue);
 
     if (master->debug_level)
-        EC_DBG("Finished SDO request for slave %u.\n",
+        EC_DBG("Finished internal SDO request for slave %u.\n",
                 fsm->slave->ring_position);
 
     // check for another SDO request
--- a/master/fsm_master.h	Mon Dec 14 13:11:36 2009 +0100
+++ b/master/fsm_master.h	Mon Dec 14 13:25:50 2009 +0100
@@ -133,7 +133,7 @@
 void ec_fsm_master_init(ec_fsm_master_t *, ec_master_t *, ec_datagram_t *);
 void ec_fsm_master_clear(ec_fsm_master_t *);
 
-void ec_fsm_master_exec(ec_fsm_master_t *);
+int ec_fsm_master_exec(ec_fsm_master_t *);
 int ec_fsm_master_idle(const ec_fsm_master_t *);
 
 /*****************************************************************************/
--- a/master/fsm_sii.c	Mon Dec 14 13:11:36 2009 +0100
+++ b/master/fsm_sii.c	Mon Dec 14 13:25:50 2009 +0100
@@ -248,7 +248,6 @@
    SII state: READ FETCH.
    Fetches the result of an SII-read datagram.
 */
-
 void ec_fsm_sii_state_read_fetch(
         ec_fsm_sii_t *fsm /**< finite state machine */
         )
@@ -302,6 +301,7 @@
         }
 
         // issue check/fetch datagram again
+        fsm->datagram->state = EC_DATAGRAM_INIT;
         fsm->retries = EC_FSM_RETRIES;
         return;
     }
@@ -435,6 +435,7 @@
 #endif
         // issue check datagram again
         fsm->retries = EC_FSM_RETRIES;
+        fsm->datagram->state = EC_DATAGRAM_INIT;
         return;
     }
 
@@ -453,6 +454,7 @@
 
         // issue check datagram again
         fsm->retries = EC_FSM_RETRIES;
+        fsm->datagram->state = EC_DATAGRAM_INIT;
         return;
     }
 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/master/fsm_slave.c	Mon Dec 14 13:25:50 2009 +0100
@@ -0,0 +1,185 @@
+/******************************************************************************
+ *
+ *  $Id$
+ *
+ *  Copyright (C) 2006-2008  Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ *  This file is part of the IgH EtherCAT Master.
+ *
+ *  The IgH EtherCAT Master is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU General Public License version 2, as
+ *  published by the Free Software Foundation.
+ *
+ *  The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General
+ *  Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License along
+ *  with the IgH EtherCAT Master; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ *  ---
+ *
+ *  The license mentioned above concerns the source code only. Using the
+ *  EtherCAT technology and brand is only permitted in compliance with the
+ *  industrial property and similar rights of Beckhoff Automation GmbH.
+ *
+ *****************************************************************************/
+
+/** \file
+ * EtherCAT slave (SDO) state machine.
+ */
+
+/*****************************************************************************/
+
+#include "globals.h"
+#include "master.h"
+#include "mailbox.h"
+
+#include "fsm_slave.h"
+
+/*****************************************************************************/
+
+void ec_fsm_slave_state_idle(ec_fsm_slave_t *);
+void ec_fsm_slave_state_sdo_request(ec_fsm_slave_t *);
+
+
+/*****************************************************************************/
+
+/** Constructor.
+ */
+void ec_fsm_slave_init(
+        ec_fsm_slave_t *fsm, /**< Slave state machine. */
+        ec_slave_t *slave, /**< EtherCAT slave. */
+        ec_datagram_t *datagram /**< Datagram object to use. */
+        )
+{
+    fsm->slave = slave;
+    fsm->datagram = datagram;
+    fsm->datagram->data_size = 0;
+    fsm->state = ec_fsm_slave_state_idle;
+
+    // init sub-state-machines
+    ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram);
+}
+
+/*****************************************************************************/
+
+/** Destructor.
+ */
+void ec_fsm_slave_clear(
+        ec_fsm_slave_t *fsm /**< Master state machine. */
+        )
+{
+    // clear sub-state machines
+    ec_fsm_coe_clear(&fsm->fsm_coe);
+}
+
+/*****************************************************************************/
+
+/** Executes the current state of the state machine.
+ *
+ * If the state machine's datagram is not sent or received yet, the execution
+ * of the state machine is delayed to the next cycle.
+ */
+void ec_fsm_slave_exec(
+        ec_fsm_slave_t *fsm /**< Slave state machine. */
+        )
+{
+    if (fsm->datagram->state == EC_DATAGRAM_SENT
+        || fsm->datagram->state == EC_DATAGRAM_QUEUED) {
+        // datagram was not sent or received yet.
+        return;
+    }
+
+    fsm->state(fsm);
+    return;
+}
+
+/******************************************************************************
+ * Slave state machine
+ *****************************************************************************/
+
+/** Slave state: IDLE.
+ *
+ * Check for pending SDO requests and process one.
+ *
+ */
+void ec_fsm_slave_state_idle(
+        ec_fsm_slave_t *fsm /**< Slave state machine. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+    ec_master_t *master = slave->master;
+    ec_master_sdo_request_t *request, *next;
+
+    // search the first matching external request to be processed
+    list_for_each_entry_safe(request, next, &master->slave_sdo_requests, list) {
+        if (request->slave != slave)
+            continue;
+        list_del_init(&request->list); // dequeue
+        request->req.state = EC_INT_REQUEST_BUSY;
+
+        if (slave->current_state == EC_SLAVE_STATE_INIT) {
+            EC_ERR("Discarding SDO request, slave %u is in INIT.\n",
+                    slave->ring_position);
+            request->req.state = EC_INT_REQUEST_FAILURE;
+            wake_up(&master->sdo_queue);
+            continue;
+        }
+
+        // Found pending SDO request. Execute it!
+        if (master->debug_level)
+            EC_DBG("Processing SDO request for slave %u...\n",
+                    slave->ring_position);
+
+        // Start SDO transfer
+        fsm->sdo_request = &request->req;
+        fsm->state = ec_fsm_slave_state_sdo_request;
+        ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req);
+        ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
+        ec_master_queue_sdo_datagram(fsm->slave->master,fsm->datagram);
+        return;
+    }
+}
+
+/*****************************************************************************/
+
+/** Slave state: SDO_REQUEST.
+ */
+void ec_fsm_slave_state_sdo_request(
+        ec_fsm_slave_t *fsm /**< Slave state machine. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+    ec_master_t *master = slave->master;
+    ec_sdo_request_t *request = fsm->sdo_request;
+
+    if (ec_fsm_coe_exec(&fsm->fsm_coe))
+    {
+        ec_master_queue_sdo_datagram(fsm->slave->master,fsm->datagram);
+        return;
+    }
+    if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
+        EC_DBG("Failed to process SDO request for slave %u.\n",
+                fsm->slave->ring_position);
+        request->state = EC_INT_REQUEST_FAILURE;
+        wake_up(&master->sdo_queue);
+        fsm->sdo_request = NULL;
+        fsm->state = ec_fsm_slave_state_idle;
+        return;
+    }
+
+    // SDO request finished
+    request->state = EC_INT_REQUEST_SUCCESS;
+    wake_up(&master->sdo_queue);
+
+    if (master->debug_level)
+        EC_DBG("Finished SDO request for slave %u.\n",
+                fsm->slave->ring_position);
+
+    fsm->sdo_request = NULL;
+    fsm->datagram->data_size = 0;
+    fsm->state = ec_fsm_slave_state_idle;
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/master/fsm_slave.h	Mon Dec 14 13:25:50 2009 +0100
@@ -0,0 +1,70 @@
+/******************************************************************************
+ *
+ *  $Id$
+ *
+ *  Copyright (C) 2006-2008  Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ *  This file is part of the IgH EtherCAT Master.
+ *
+ *  The IgH EtherCAT Master is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU General Public License version 2, as
+ *  published by the Free Software Foundation.
+ *
+ *  The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General
+ *  Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License along
+ *  with the IgH EtherCAT Master; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ *  ---
+ *
+ *  The license mentioned above concerns the source code only. Using the
+ *  EtherCAT technology and brand is only permitted in compliance with the
+ *  industrial property and similar rights of Beckhoff Automation GmbH.
+ *
+ *****************************************************************************/
+
+/**
+   \file
+   EtherCAT slave request (SDO) state machine.
+*/
+
+/*****************************************************************************/
+#ifndef __EC_FSM_SLAVE_H__
+#define __EC_FSM_SLAVE_H__
+
+#include "globals.h"
+#include "datagram.h"
+#include "sdo_request.h"
+#include "fsm_coe.h"
+
+
+typedef struct ec_fsm_slave ec_fsm_slave_t; /**< \see ec_fsm_slave */
+
+/** Finite state machine of an EtherCAT slave.
+ */
+struct ec_fsm_slave {
+    ec_slave_t *slave; /**< slave the FSM runs on */
+    ec_datagram_t *datagram; /**< datagram used in the state machine */
+
+    void (*state)(ec_fsm_slave_t *); /**< master state function */
+    ec_sdo_request_t *sdo_request; /**< SDO request to process. */
+
+    ec_fsm_coe_t fsm_coe; /**< CoE state machine */
+};
+
+/*****************************************************************************/
+
+void ec_fsm_slave_init(ec_fsm_slave_t *, ec_slave_t *, ec_datagram_t *);
+void ec_fsm_slave_clear(ec_fsm_slave_t *);
+
+void ec_fsm_slave_exec(ec_fsm_slave_t *);
+int ec_fsm_slave_idle(const ec_fsm_slave_t *);
+
+/*****************************************************************************/
+
+
+#endif // __EC_FSM_SLAVE_H__
--- a/master/globals.h	Mon Dec 14 13:11:36 2009 +0100
+++ b/master/globals.h	Mon Dec 14 13:25:50 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 Dec 14 13:11:36 2009 +0100
+++ b/master/ioctl.h	Mon Dec 14 13:25:50 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 Dec 14 13:11:36 2009 +0100
+++ b/master/master.c	Mon Dec 14 13:25:50 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
 }
 
@@ -151,6 +154,9 @@
     INIT_LIST_HEAD(&master->ext_datagram_queue);
     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);
 
     master->debug_level = debug_level;
@@ -684,6 +690,91 @@
     master->phase = EC_IDLE;
 }
 
+
+/*****************************************************************************/
+
+/** Injects sdo datagrams that fit into the datagram queue
+ */
+void ec_master_inject_sdo_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->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);
+                    }
+                }
+            }
+        }
+    }
+}
+
+/*****************************************************************************/
+
+/** Places a sdo datagram in the sdo datagram queue.
+ */
+void ec_master_queue_sdo_datagram(
+        ec_master_t *master, /**< EtherCAT master */
+        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);
+    up(&master->io_sem);
+}
+
 /*****************************************************************************/
 
 /** Places a datagram in the datagram queue.
@@ -695,6 +786,8 @@
 {
     ec_datagram_t *queued_datagram;
 
+    if (datagram->state == EC_DATAGRAM_SENT)
+        return;
     // check, if the datagram is already queued
     list_for_each_entry(queued_datagram, &master->datagram_queue, queue) {
         if (queued_datagram == datagram) {
@@ -1001,7 +1094,8 @@
 static int ec_master_idle_thread(void *priv_data)
 {
     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");
 
@@ -1013,22 +1107,27 @@
         ecrt_master_receive(master);
         up(&master->io_sem);
 
-        if (master->fsm_datagram.state == EC_DATAGRAM_SENT)
-            goto schedule;
-
-        // execute master state machine
+        fsm_exec = 0;
+        // execute master & slave state machines
         if (down_interruptible(&master->master_sem))
             break;
-        ec_fsm_master_exec(&master->fsm);
+        fsm_exec = ec_fsm_master_exec(&master->fsm);
+        for (slave = master->slaves;
+                slave < master->slaves + master->slave_count;
+                slave++) {
+            ec_fsm_slave_exec(&slave->fsm);
+        }
         up(&master->master_sem);
 
         // queue and send
         down(&master->io_sem);
-        ec_master_queue_datagram(master, &master->fsm_datagram);
+        if (fsm_exec) {
+            ec_master_queue_datagram(master, &master->fsm_datagram);
+        }
+        ec_master_inject_sdo_datagrams(master);
         ecrt_master_send(master);
         up(&master->io_sem);
-        
-schedule:
+
         if (ec_fsm_master_idle(&master->fsm)) {
             set_current_state(TASK_INTERRUPTIBLE);
             schedule_timeout(1);
@@ -1045,35 +1144,38 @@
 
 /*****************************************************************************/
 
-/** Master kernel thread function for IDLE phase.
+/** Master kernel thread function for OPERATION phase.
  */
 static int ec_master_operation_thread(void *priv_data)
 {
     ec_master_t *master = (ec_master_t *) priv_data;
-
+    ec_slave_t *slave = NULL;
+    int fsm_exec;
     if (master->debug_level)
         EC_DBG("Operation thread running.\n");
 
     while (!kthread_should_stop()) {
         ec_datagram_output_stats(&master->fsm_datagram);
-        if (master->injection_seq_rt != master->injection_seq_fsm ||
-                master->fsm_datagram.state == EC_DATAGRAM_SENT ||
-                master->fsm_datagram.state == EC_DATAGRAM_QUEUED)
-            goto schedule;
-
-        // output statistics
-        ec_master_output_stats(master);
-
-        // execute master state machine
-        if (down_interruptible(&master->master_sem))
-            break;
-        ec_fsm_master_exec(&master->fsm);
-        up(&master->master_sem);
-
-        // inject datagram
-        master->injection_seq_fsm++;
-
-schedule:
+        if (master->injection_seq_rt == master->injection_seq_fsm) {
+            // output statistics
+            ec_master_output_stats(master);
+
+            fsm_exec = 0;
+            // execute master & slave state machines
+            if (down_interruptible(&master->master_sem))
+                break;
+            fsm_exec += ec_fsm_master_exec(&master->fsm);
+            for (slave = master->slaves;
+                    slave < master->slaves + master->slave_count;
+                    slave++) {
+                ec_fsm_slave_exec(&slave->fsm);
+            }
+            up(&master->master_sem);
+
+            // inject datagrams (let the rt thread queue them, see ecrt_master_send)
+            if (fsm_exec)
+                master->injection_seq_fsm++;
+        }
         if (ec_fsm_master_idle(&master->fsm)) {
             set_current_state(TASK_INTERRUPTIBLE);
             schedule_timeout(1);
@@ -1797,10 +1899,11 @@
     ec_datagram_t *datagram, *n;
 
     if (master->injection_seq_rt != master->injection_seq_fsm) {
-        // inject datagram produced by master FSM
+        // inject datagrams produced by master & slave FSMs
         ec_master_queue_datagram(master, &master->fsm_datagram);
         master->injection_seq_rt = master->injection_seq_fsm;
     }
+    ec_master_inject_sdo_datagrams(master);
 
     if (unlikely(!master->main_device.link_state)) {
         // link is down, no datagram can be sent
--- a/master/master.h	Mon Dec 14 13:11:36 2009 +0100
+++ b/master/master.h	Mon Dec 14 13:25:50 2009 +0100
@@ -114,6 +114,7 @@
     unsigned int injection_seq_rt; /**< Datagram injection sequence number
                                      for the realtime side. */
 
+
     ec_slave_t *slaves; /**< Array of slaves on the bus. */
     unsigned int slave_count; /**< Number of slaves on the bus. */
 
@@ -153,6 +154,8 @@
     struct semaphore ext_queue_sem; /**< Semaphore protecting the \a
                                       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. */
@@ -220,6 +223,8 @@
 void ec_master_receive_datagrams(ec_master_t *, const uint8_t *, size_t);
 void ec_master_queue_datagram(ec_master_t *, ec_datagram_t *);
 void ec_master_queue_datagram_ext(ec_master_t *, ec_datagram_t *);
+void ec_master_queue_sdo_datagram(ec_master_t *, ec_datagram_t *);
+void ec_master_inject_sdo_datagrams(ec_master_t *);
 
 // misc.
 void ec_master_attach_slave_configs(ec_master_t *);
--- a/master/module.c	Mon Dec 14 13:11:36 2009 +0100
+++ b/master/module.c	Mon Dec 14 13:25:50 2009 +0100
@@ -318,6 +318,8 @@
 /*****************************************************************************/
 
 /** Outputs frame contents for debugging purposes.
+ * If the data block is larger than 256 bytes, only the first 128
+ * and the last 128 bytes will be shown
  */
 void ec_print_data(const uint8_t *data, /**< pointer to data */
                    size_t size /**< number of bytes to output */
@@ -332,6 +334,12 @@
             printk("\n");
             EC_DBG("");
         }
+        if (i+1 == 128 && size > 256)
+        {
+            printk("dropped %d bytes\n",size-128-i);
+            i = size - 128;
+            EC_DBG("");
+        }
     }
     printk("\n");
 }
--- a/master/slave.c	Mon Dec 14 13:11:36 2009 +0100
+++ b/master/slave.c	Mon Dec 14 13:25:50 2009 +0100
@@ -67,6 +67,7 @@
         )
 {
     unsigned int i;
+    int ret;
 
     slave->master = master;
     slave->ring_position = ring_position;
@@ -147,6 +148,20 @@
 
     slave->sdo_dictionary_fetched = 0;
     slave->jiffies_preop = 0;
+
+    // init state machine datagram
+    ec_datagram_init(&slave->fsm_datagram);
+    snprintf(slave->fsm_datagram.name, EC_DATAGRAM_NAME_SIZE, "slave%u-fsm",slave->ring_position);
+    ret = ec_datagram_prealloc(&slave->fsm_datagram, EC_MAX_DATA_SIZE);
+    if (ret < 0) {
+        ec_datagram_clear(&slave->fsm_datagram);
+        EC_ERR("Failed to allocate Slave %u FSM datagram.\n",slave->ring_position);
+        return;
+    }
+
+    // create state machine object
+    ec_fsm_slave_init(&slave->fsm, slave, &slave->fsm_datagram);
+
 }
 
 /*****************************************************************************/
@@ -191,6 +206,9 @@
 
     if (slave->sii_words)
         kfree(slave->sii_words);
+    ec_fsm_slave_clear(&slave->fsm);
+    ec_datagram_clear(&slave->fsm_datagram);
+
 }
 
 /*****************************************************************************/
--- a/master/slave.h	Mon Dec 14 13:11:36 2009 +0100
+++ b/master/slave.h	Mon Dec 14 13:25:50 2009 +0100
@@ -45,6 +45,7 @@
 #include "pdo.h"
 #include "sync.h"
 #include "sdo.h"
+#include "fsm_slave.h"
 
 /*****************************************************************************/
 
@@ -157,6 +158,9 @@
     struct list_head sdo_dictionary; /**< SDO dictionary list */
     uint8_t sdo_dictionary_fetched; /**< Dictionary has been fetched. */
     unsigned long jiffies_preop; /**< Time, the slave went to PREOP. */
+
+    ec_fsm_slave_t fsm; /**< Slave state machine. */
+    ec_datagram_t fsm_datagram; /**< Datagram used for state machines. */
 };
 
 /*****************************************************************************/