--- a/master/datagram.c	Thu Jan 12 12:14:33 2012 +0100
+++ b/master/datagram.c	Thu Jan 12 13:55:15 2012 +0100
@@ -88,6 +88,7 @@
 void ec_datagram_init(ec_datagram_t *datagram /**< EtherCAT datagram. */)
 {
     INIT_LIST_HEAD(&datagram->queue); // mark as unqueued
+    datagram->device_index = EC_DEVICE_MAIN;
     datagram->type = EC_DATAGRAM_NONE;
     memset(datagram->address, 0x00, EC_ADDR_LEN);
     datagram->data = NULL;

--- a/master/datagram.h	Thu Jan 12 12:14:33 2012 +0100
+++ b/master/datagram.h	Thu Jan 12 13:55:15 2012 +0100
@@ -88,6 +88,8 @@
     struct list_head list; /**< Needed by domain datagram lists. */
     struct list_head queue; /**< Master datagram queue item. */
     struct list_head sent; /**< Master list item for sent datagrams. */
+    ec_device_index_t device_index; /**< Device via which the datagram shall
+                                      be / was sent. */
     ec_datagram_type_t type; /**< Datagram type (APRD, BWR, etc.). */
     uint8_t address[EC_ADDR_LEN]; /**< Recipient address. */
     uint8_t *data; /**< Datagram payload. */

--- a/master/domain.c	Thu Jan 12 12:14:33 2012 +0100
+++ b/master/domain.c	Thu Jan 12 13:55:15 2012 +0100
@@ -438,7 +438,7 @@
     ec_datagram_t *datagram;
 
     list_for_each_entry(datagram, &domain->datagrams, list) {
-        ec_master_queue_datagram(domain->master, datagram);
+        ec_master_queue_datagram(domain->master, datagram, EC_DEVICE_MAIN);
     }
 }
 

--- a/master/fsm_slave.c	Thu Jan 12 12:14:33 2012 +0100
+++ b/master/fsm_slave.c	Thu Jan 12 13:55:15 2012 +0100
@@ -203,7 +203,8 @@
         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_external_datagram(fsm->slave->master,fsm->datagram);
+        ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram,
+                EC_DEVICE_MAIN);
         return 1;
     }
     return 0;
@@ -220,9 +221,9 @@
     ec_slave_t *slave = fsm->slave;
     ec_sdo_request_t *request = fsm->sdo_request;
 
-    if (ec_fsm_coe_exec(&fsm->fsm_coe))
-    {
-        ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram);
+    if (ec_fsm_coe_exec(&fsm->fsm_coe)) {
+        ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram,
+                EC_DEVICE_MAIN);
         return;
     }
     if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
@@ -277,7 +278,8 @@
         fsm->state = ec_fsm_slave_state_foe_request;
         ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req);
         ec_fsm_foe_exec(&fsm->fsm_foe);
-        ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram);
+        ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram,
+                EC_DEVICE_MAIN);
         return 1;
     }
     return 0;
@@ -294,9 +296,9 @@
     ec_slave_t *slave = fsm->slave;
     ec_foe_request_t *request = fsm->foe_request;
 
-    if (ec_fsm_foe_exec(&fsm->fsm_foe))
-    {
-        ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram);
+    if (ec_fsm_foe_exec(&fsm->fsm_foe)) {
+        ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram,
+                EC_DEVICE_MAIN);
         return;
     }
 
@@ -364,7 +366,8 @@
         fsm->state = ec_fsm_slave_state_soe_request;
         ec_fsm_soe_transfer(&fsm->fsm_soe, slave, &req->req);
         ec_fsm_soe_exec(&fsm->fsm_soe); // execute immediately
-        ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
+        ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram,
+                EC_DEVICE_MAIN);
         return 1;
     }
     return 0;
@@ -382,7 +385,8 @@
     ec_soe_request_t *request = fsm->soe_request;
 
     if (ec_fsm_soe_exec(&fsm->fsm_soe)) {
-        ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
+        ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram,
+                EC_DEVICE_MAIN);
         return;
     }
 

--- a/master/fsm_slave_config.c	Thu Jan 12 12:14:33 2012 +0100
+++ b/master/fsm_slave_config.c	Thu Jan 12 13:55:15 2012 +0100
@@ -744,7 +744,7 @@
             ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy);
             ec_fsm_soe_exec(fsm_soe); // execute immediately
             ec_master_queue_external_datagram(slave->master,
-                    fsm_soe->datagram);
+                    fsm_soe->datagram, EC_DEVICE_MAIN);
             return;
         }
     }
@@ -765,7 +765,8 @@
     ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe;
 
     if (ec_fsm_soe_exec(fsm_soe)) {
-        ec_master_queue_external_datagram(slave->master, fsm_soe->datagram);
+        ec_master_queue_external_datagram(slave->master, fsm_soe->datagram,
+                EC_DEVICE_MAIN);
         return;
     }
 
@@ -791,7 +792,7 @@
             ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy);
             ec_fsm_soe_exec(fsm_soe); // execute immediately
             ec_master_queue_external_datagram(slave->master,
-                    fsm_soe->datagram);
+                    fsm_soe->datagram, EC_DEVICE_MAIN);
             return;
         }
     }
@@ -1473,7 +1474,7 @@
             ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy);
             ec_fsm_soe_exec(fsm_soe); // execute immediately
             ec_master_queue_external_datagram(slave->master,
-                    fsm_soe->datagram);
+                    fsm_soe->datagram, EC_DEVICE_MAIN);
             return;
         }
     }
@@ -1494,7 +1495,8 @@
     ec_fsm_soe_t *fsm_soe = &slave->fsm.fsm_soe;
 
     if (ec_fsm_soe_exec(fsm_soe)) {
-        ec_master_queue_external_datagram(slave->master, fsm_soe->datagram);
+        ec_master_queue_external_datagram(slave->master, fsm_soe->datagram,
+                EC_DEVICE_MAIN);
         return;
     }
 
@@ -1520,7 +1522,7 @@
             ec_fsm_soe_transfer(fsm_soe, fsm->slave, &fsm->soe_request_copy);
             ec_fsm_soe_exec(fsm_soe); // execute immediately
             ec_master_queue_external_datagram(slave->master,
-                    fsm_soe->datagram);
+                    fsm_soe->datagram, EC_DEVICE_MAIN);
             return;
         }
     }

--- a/master/globals.h	Thu Jan 12 12:14:33 2012 +0100
+++ b/master/globals.h	Thu Jan 12 13:55:15 2012 +0100
@@ -218,11 +218,11 @@
 
 /** Master devices.
  */
-enum {
+typedef enum {
     EC_DEVICE_MAIN, /**< Main device. */
     EC_DEVICE_BACKUP, /**< Backup device */
     EC_NUM_DEVICES /**< Number of devices. */
-};
+} ec_device_index_t;
 
 /*****************************************************************************/
 

--- a/master/master.c	Thu Jan 12 12:14:33 2012 +0100
+++ b/master/master.c	Thu Jan 12 13:55:15 2012 +0100
@@ -720,9 +720,9 @@
             datagram->cycles_sent = 0;
 #endif
             datagram->jiffies_sent = 0;
-            ec_master_queue_datagram(master, datagram);
-        }
-        else {
+            ec_master_queue_datagram(master, datagram,
+                    datagram->device_index);
+        } else {
             if (datagram->data_size > master->max_queue_size) {
                 list_del_init(&datagram->queue);
                 datagram->state = EC_DATAGRAM_ERROR;
@@ -793,7 +793,8 @@
  */
 void ec_master_queue_external_datagram(
         ec_master_t *master, /**< EtherCAT master */
-        ec_datagram_t *datagram /**< datagram */
+        ec_datagram_t *datagram, /**< datagram */
+        ec_device_index_t device_index /**< Device index. */
         )
 {
     ec_datagram_t *queued_datagram;
@@ -816,6 +817,7 @@
 
     list_add_tail(&datagram->queue, &master->external_datagram_queue);
     datagram->state = EC_DATAGRAM_QUEUED;
+    datagram->device_index = device_index;
 #ifdef EC_HAVE_CYCLES
     datagram->cycles_sent = get_cycles();
 #endif
@@ -831,7 +833,8 @@
  */
 void ec_master_queue_datagram(
         ec_master_t *master, /**< EtherCAT master */
-        ec_datagram_t *datagram /**< datagram */
+        ec_datagram_t *datagram, /**< datagram */
+        ec_device_index_t device_index /**< Device index. */
         )
 {
     ec_datagram_t *queued_datagram;
@@ -871,6 +874,7 @@
 
     list_add_tail(&datagram->queue, &master->datagram_queue);
     datagram->state = EC_DATAGRAM_QUEUED;
+    datagram->device_index = device_index;
 }
 
 /*****************************************************************************/
@@ -889,14 +893,17 @@
 
 /*****************************************************************************/
 
-/** Sends the datagrams in the queue.
+/** Sends the datagrams in the queue for a certain device.
  *
  */
-void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */)
+void ec_master_send_datagrams(
+        ec_master_t *master, /**< EtherCAT master */
+        ec_device_index_t device_index /**< Device index. */
+        )
 {
     ec_datagram_t *datagram, *next;
     size_t datagram_size;
-    uint8_t *frame_data, *cur_data;
+    uint8_t *frame_data, *cur_data = NULL;
     void *follows_word;
 #ifdef EC_HAVE_CYCLES
     cycles_t cycles_start, cycles_sent, cycles_end;
@@ -911,18 +918,27 @@
     frame_count = 0;
     INIT_LIST_HEAD(&sent_datagrams);
 
-    EC_MASTER_DBG(master, 2, "ec_master_send_datagrams\n");
+    EC_MASTER_DBG(master, 2, "%s(device_index = %u)\n",
+            __func__, device_index);
 
     do {
-        // fetch pointer to transmit socket buffer
-        frame_data = ec_device_tx_data(&master->devices[EC_DEVICE_MAIN]);
-        cur_data = frame_data + EC_FRAME_HEADER_SIZE;
+        frame_data = NULL;
         follows_word = NULL;
         more_datagrams_waiting = 0;
 
         // fill current frame with datagrams
         list_for_each_entry(datagram, &master->datagram_queue, queue) {
-            if (datagram->state != EC_DATAGRAM_QUEUED) continue;
+            if (datagram->state != EC_DATAGRAM_QUEUED ||
+                    datagram->device_index != device_index) {
+                continue;
+            }
+
+            if (!frame_data) {
+                // fetch pointer to transmit socket buffer
+                frame_data =
+                    ec_device_tx_data(&master->devices[device_index]);
+                cur_data = frame_data + EC_FRAME_HEADER_SIZE;
+            }
 
             // does the current datagram fit in the frame?
             datagram_size = EC_DATAGRAM_HEADER_SIZE + datagram->data_size
@@ -935,16 +951,17 @@
             list_add_tail(&datagram->sent, &sent_datagrams);
             datagram->index = master->datagram_index++;
 
-            EC_MASTER_DBG(master, 2, "adding datagram 0x%02X\n",
+            EC_MASTER_DBG(master, 2, "Adding datagram 0x%02X\n",
                     datagram->index);
 
-            // set "datagram following" flag in previous frame
-            if (follows_word)
+            // set "datagram following" flag in previous datagram
+            if (follows_word) {
                 EC_WRITE_U16(follows_word,
                         EC_READ_U16(follows_word) | 0x8000);
+            }
 
             // EtherCAT datagram header
-            EC_WRITE_U8 (cur_data,     datagram->type);
+            EC_WRITE_U8 (cur_data, datagram->type);
             EC_WRITE_U8 (cur_data + 1, datagram->index);
             memcpy(cur_data + 2, datagram->address, EC_ADDR_LEN);
             EC_WRITE_U16(cur_data + 6, datagram->data_size & 0x7FF);
@@ -977,7 +994,7 @@
         EC_MASTER_DBG(master, 2, "frame size: %zu\n", cur_data - frame_data);
 
         // send frame
-        ec_device_send(&master->devices[EC_DEVICE_MAIN],
+        ec_device_send(&master->devices[device_index],
                 cur_data - frame_data);
 #ifdef EC_HAVE_CYCLES
         cycles_sent = get_cycles();
@@ -1001,8 +1018,8 @@
 #ifdef EC_HAVE_CYCLES
     if (unlikely(master->debug_level > 1)) {
         cycles_end = get_cycles();
-        EC_MASTER_DBG(master, 0, "ec_master_send_datagrams"
-                " sent %u frames in %uus.\n", frame_count,
+        EC_MASTER_DBG(master, 0, "%s()"
+                " sent %u frames in %uus.\n", __func__, frame_count,
                (unsigned int) (cycles_end - cycles_start) * 1000 / cpu_khz);
     }
 #endif
@@ -1363,7 +1380,8 @@
         // queue and send
         down(&master->io_sem);
         if (fsm_exec) {
-            ec_master_queue_datagram(master, &master->fsm_datagram);
+            ec_master_queue_datagram(master, &master->fsm_datagram,
+                    EC_DEVICE_MAIN);
         }
         ec_master_inject_external_datagrams(master);
         ecrt_master_send(master);
@@ -2190,10 +2208,12 @@
 void ecrt_master_send(ec_master_t *master)
 {
     ec_datagram_t *datagram, *n;
+    unsigned int i;
 
     if (master->injection_seq_rt != master->injection_seq_fsm) {
         // inject datagrams produced by master & slave FSMs
-        ec_master_queue_datagram(master, &master->fsm_datagram);
+        ec_master_queue_datagram(master, &master->fsm_datagram,
+                EC_DEVICE_MAIN);
         master->injection_seq_rt = master->injection_seq_fsm;
     }
     ec_master_inject_external_datagrams(master);
@@ -2215,7 +2235,11 @@
     }
 
     // send frames
-    ec_master_send_datagrams(master);
+    for (i = 0; i < EC_NUM_DEVICES; i++) {
+        if (master->devices[i].dev) {
+            ec_master_send_datagrams(master, i);
+        }
+    }
 }
 
 /*****************************************************************************/
@@ -2275,7 +2299,7 @@
     list_for_each_entry_safe(datagram, next, &master->ext_datagram_queue,
             queue) {
         list_del(&datagram->queue);
-        ec_master_queue_datagram(master, datagram);
+        ec_master_queue_datagram(master, datagram, EC_DEVICE_MAIN);
     }
 
     ecrt_master_send(master);
@@ -2441,7 +2465,8 @@
 void ecrt_master_sync_reference_clock(ec_master_t *master)
 {
     EC_WRITE_U32(master->ref_sync_datagram.data, master->app_time);
-    ec_master_queue_datagram(master, &master->ref_sync_datagram);
+    ec_master_queue_datagram(master, &master->ref_sync_datagram,
+            EC_DEVICE_MAIN);
 }
 
 /*****************************************************************************/
@@ -2449,7 +2474,8 @@
 void ecrt_master_sync_slave_clocks(ec_master_t *master)
 {
     ec_datagram_zero(&master->sync_datagram);
-    ec_master_queue_datagram(master, &master->sync_datagram);
+    ec_master_queue_datagram(master, &master->sync_datagram,
+            EC_DEVICE_MAIN);
 }
 
 /*****************************************************************************/
@@ -2457,7 +2483,8 @@
 void ecrt_master_sync_monitor_queue(ec_master_t *master)
 {
     ec_datagram_zero(&master->sync_mon_datagram);
-    ec_master_queue_datagram(master, &master->sync_mon_datagram);
+    ec_master_queue_datagram(master, &master->sync_mon_datagram,
+            EC_DEVICE_MAIN);
 }
 
 /*****************************************************************************/

--- a/master/master.h	Thu Jan 12 12:14:33 2012 +0100
+++ b/master/master.h	Thu Jan 12 13:55:15 2012 +0100
@@ -242,7 +242,8 @@
                                       ext_datagram_queue. */
 
     struct list_head external_datagram_queue; /**< External Datagram queue. */
-    unsigned int send_interval; /**< Interval between calls to ecrt_master_send */
+    unsigned int send_interval; /**< Interval between calls to
+                                  ecrt_master_send */
     size_t max_queue_size; /**< Maximum size of datagram queue */
 
     unsigned int debug_level; /**< Master debug level. */
@@ -298,9 +299,11 @@
 
 // datagram IO
 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(ec_master_t *, ec_datagram_t *,
+        ec_device_index_t);
 void ec_master_queue_datagram_ext(ec_master_t *, ec_datagram_t *);
-void ec_master_queue_external_datagram(ec_master_t *, ec_datagram_t *);
+void ec_master_queue_external_datagram(ec_master_t *, ec_datagram_t *,
+        ec_device_index_t);
 void ec_master_inject_external_datagrams(ec_master_t *);
 
 // misc.

--- a/master/voe_handler.c	Thu Jan 12 12:14:33 2012 +0100
+++ b/master/voe_handler.c	Thu Jan 12 13:55:15 2012 +0100
@@ -191,7 +191,8 @@
     if (voe->config->slave) { // FIXME locking?
         voe->state(voe);
         if (voe->request_state == EC_INT_REQUEST_BUSY)
-            ec_master_queue_datagram(voe->config->master, &voe->datagram);
+            ec_master_queue_datagram(voe->config->master, &voe->datagram,
+                    EC_DEVICE_MAIN);
     } else {
         voe->state = ec_voe_handler_state_error;
         voe->request_state = EC_INT_REQUEST_FAILURE;