--- a/master/master.c	Thu Apr 29 14:05:15 2010 +0200
+++ b/master/master.c	Thu Apr 29 16:33:32 2010 +0200
@@ -902,7 +902,7 @@
 {
     ec_datagram_t *datagram, *next;
     size_t datagram_size;
-    uint8_t *frame_data, *cur_data;
+    uint8_t *frame_data, *cur_data, *frame_datagram_data;
     void *follows_word;
 #ifdef EC_HAVE_CYCLES
     cycles_t cycles_start, cycles_sent, cycles_end;
@@ -910,6 +910,7 @@
     unsigned long jiffies_sent;
     unsigned int frame_count, more_datagrams_waiting;
     struct list_head sent_datagrams;
+    ec_fmmu_config_t* domain_fmmu;
 
 #ifdef EC_HAVE_CYCLES
     cycles_start = get_cycles();
@@ -959,7 +960,28 @@
             cur_data += EC_DATAGRAM_HEADER_SIZE;
 
             // EtherCAT datagram data
-            memcpy(cur_data, datagram->data, datagram->data_size);
+            frame_datagram_data = cur_data;
+            if (datagram->domain) {
+                unsigned int datagram_address = EC_READ_U32(datagram->address);
+                int i = 0;
+                uint8_t *domain_data = datagram->data;
+                list_for_each_entry(domain_fmmu, &datagram->domain->fmmu_configs, list) {
+                    if (domain_fmmu->dir == EC_DIR_OUTPUT ) {
+                        unsigned int frame_offset = domain_fmmu->logical_start_address-datagram_address;
+                        memcpy(frame_datagram_data+frame_offset, domain_data, domain_fmmu->data_size);
+                        if (unlikely(master->debug_level > 1)) {
+                            EC_DBG("sending dg 0x%02X fmmu %u fp=%u dp=%u size=%u\n",
+                                   datagram->index, i,frame_offset,domain_data-datagram->data,domain_fmmu->data_size);
+                            ec_print_data(domain_data, domain_fmmu->data_size);
+                        }
+                    }
+                    domain_data += domain_fmmu->data_size;
+                    ++i;
+                }
+            }
+            else {
+                memcpy(frame_datagram_data, datagram->data, datagram->data_size);
+            }
             cur_data += datagram->data_size;
 
             // EtherCAT datagram footer
@@ -1031,8 +1053,9 @@
     size_t frame_size, data_size;
     uint8_t datagram_type, datagram_index;
     unsigned int cmd_follows, matched;
-    const uint8_t *cur_data;
+    const uint8_t *cur_data, *frame_datagram_data;
     ec_datagram_t *datagram;
+    ec_fmmu_config_t* domain_fmmu;
 
     if (unlikely(size < EC_FRAME_HEADER_SIZE)) {
         if (master->debug_level) {
@@ -1113,9 +1136,29 @@
             cur_data += data_size + EC_DATAGRAM_FOOTER_SIZE;
             continue;
         }
-
-        // copy received data into the datagram memory
-        memcpy(datagram->data, cur_data, data_size);
+        frame_datagram_data = cur_data;
+        if (datagram->domain) {
+            size_t datagram_address = EC_READ_U32(datagram->address);
+            int i = 0;
+            uint8_t *domain_data = datagram->data;
+            list_for_each_entry(domain_fmmu, &datagram->domain->fmmu_configs, list) {
+                if (domain_fmmu->dir == EC_DIR_INPUT ) {
+                    unsigned int frame_offset = domain_fmmu->logical_start_address-datagram_address;
+                    memcpy(domain_data, frame_datagram_data+frame_offset, domain_fmmu->data_size);
+                    if (unlikely(master->debug_level > 1)) {
+                        EC_DBG("receiving dg 0x%02X fmmu %u fp=%u dp=%u size=%u\n",
+                               datagram->index, i,frame_offset,domain_data-datagram->data,domain_fmmu->data_size);
+                        ec_print_data(domain_data, domain_fmmu->data_size);
+                    }
+                }
+                domain_data += domain_fmmu->data_size;
+                ++i;
+            }
+        }
+        else {
+            // copy received data into the datagram memory
+            memcpy(datagram->data, frame_datagram_data, data_size);
+        }
         cur_data += data_size;
 
         // set the datagram's working counter