--- a/master/domain.c	Thu Mar 26 17:41:57 2015 -0400
+++ b/master/domain.c	Thu Apr 02 16:49:36 2015 -0400
@@ -79,6 +79,7 @@
     domain->working_counter_changes = 0;
     domain->redundancy_active = 0;
     domain->notify_jiffies = 0;
+    memset(domain->offset_used, 0, sizeof(domain->offset_used));
 }
 
 /*****************************************************************************/
@@ -124,14 +125,43 @@
         ec_fmmu_config_t *fmmu /**< FMMU configuration. */
         )
 {
+    const ec_slave_config_t *sc;
+    uint32_t logical_domain_offset;
+    unsigned fmmu_data_size;
+
     fmmu->domain = domain;
-
-    domain->data_size += fmmu->data_size;
+    sc = fmmu->sc;
+
+    fmmu_data_size = ec_pdo_list_total_size(
+        &sc->sync_configs[fmmu->sync_index].pdos);
+
+    if (sc->allow_overlapping_pdos && sc->used_fmmus) {
+        // If we permit overlapped PDOs, and we already have an allocated FMMU
+        // for this slave, alocate the subsequent FMMU offsets by direction
+        logical_domain_offset = domain->offset_used[fmmu->dir];
+    } else {
+        // otherwise, allocate to the furthest extent of any allocated
+        // FMMU on this domain.
+        logical_domain_offset = max(domain->offset_used[EC_DIR_INPUT],
+            domain->offset_used[EC_DIR_OUTPUT]);
+        // rebase the free offsets to the current position
+        domain->offset_used[EC_DIR_INPUT] = logical_domain_offset;
+        domain->offset_used[EC_DIR_OUTPUT] = logical_domain_offset;
+    }
+    // consume the offset space for this FMMU's direction
+    domain->offset_used[fmmu->dir] += fmmu_data_size;
+
+    ec_fmmu_set_domain_offset_size(fmmu, logical_domain_offset, fmmu_data_size);
+
     list_add_tail(&fmmu->list, &domain->fmmu_configs);
+    
+    // Determine domain size from furthest extent of FMMU data
+    domain->data_size = max(domain->offset_used[EC_DIR_INPUT],
+            domain->offset_used[EC_DIR_OUTPUT]);
 
     EC_MASTER_DBG(domain->master, 1, "Domain %u:"
-            " Added %u bytes.\n",
-            domain->index, fmmu->data_size);
+            " Added %u bytes at %u.\n",
+            domain->index, fmmu->data_size, logical_domain_offset);
 }
 
 /*****************************************************************************/
@@ -237,15 +267,6 @@
     const ec_datagram_pair_t *datagram_pair;
     int ret;
 
-#if 0
-    // Determine domain size from furthest extent of FMMU data
-    domain->data_size = 0;
-    list_for_each_entry(fmmu, &domain->fmmu_configs, list) {
-        domain->data_size = max(domain->data_size,
-            fmmu->logical_domain_offset + fmmu->data_size);
-    }
-#endif
-
     domain->logical_base_address = base_address;
 
     if (domain->data_size && domain->data_origin == EC_ORIG_INTERNAL) {

--- a/master/domain.h	Thu Mar 26 17:41:57 2015 -0400
+++ b/master/domain.h	Thu Apr 02 16:49:36 2015 -0400
@@ -72,6 +72,8 @@
                                              since last notification. */
     unsigned int redundancy_active; /**< Non-zero, if redundancy is in use. */
     unsigned long notify_jiffies; /**< Time of last notification. */
+    uint32_t offset_used[EC_DIR_COUNT]; /**< Next available domain offset of
+        PDO, by direction */
 };
 
 /*****************************************************************************/

--- a/master/fmmu_config.c	Thu Mar 26 17:41:57 2015 -0400
+++ b/master/fmmu_config.c	Thu Apr 02 16:49:36 2015 -0400
@@ -60,13 +60,23 @@
     fmmu->sync_index = sync_index;
     fmmu->dir = dir;
 
-    fmmu->logical_domain_offset = domain->data_size;
-    fmmu->data_size = ec_pdo_list_total_size(
-            &sc->sync_configs[sync_index].pdos);
+    fmmu->logical_domain_offset = 0;
+    fmmu->data_size = 0;
 
     ec_domain_add_fmmu_config(domain, fmmu);
 }
 
+void ec_fmmu_set_domain_offset_size(
+        ec_fmmu_config_t *fmmu, /**< EtherCAT FMMU configuration. */
+        uint32_t logical_domain_offset, /**< Logical offset address
+            relative to domain->logical_base_address. */
+        unsigned data_size /**< Covered PDO size. */
+        )
+{
+    fmmu->logical_domain_offset = logical_domain_offset;
+    fmmu->data_size = data_size;
+}
+
 /*****************************************************************************/
 
 /** Initializes an FMMU configuration page.

--- a/master/fmmu_config.h	Thu Mar 26 17:41:57 2015 -0400
+++ b/master/fmmu_config.h	Thu Apr 02 16:49:36 2015 -0400
@@ -59,6 +59,15 @@
 void ec_fmmu_config_init(ec_fmmu_config_t *, ec_slave_config_t *,
         ec_domain_t *, uint8_t, ec_direction_t);
 
+/**
+ * @param fmmu EtherCAT FMMU configuration.
+ * @param logical_domain_offset Logical offset address 
+        relative to domain->logical_base_address.
+ * @param data_size Covered PDO size.
+*/        
+void ec_fmmu_set_domain_offset_size(ec_fmmu_config_t *fmmu, 
+        uint32_t logical_domain_offset, unsigned data_size);
+
 void ec_fmmu_config_page(const ec_fmmu_config_t *, const ec_sync_t *,
         uint8_t *);