diff --git a/master/domain.c b/master/domain.c
index 0abbe7ec..4476f5ef 100644
--- a/master/domain.c
+++ b/master/domain.c
@@ -203,6 +203,7 @@ int ec_domain_finish(
     unsigned int datagram_count;
     unsigned int datagram_used[EC_DIR_COUNT];
     ec_fmmu_config_t *fmmu;
+    ec_fmmu_config_t *fmmu_temp;
     const ec_datagram_t *datagram;
     int ret;
 
@@ -225,15 +226,21 @@ int ec_domain_finish(
     datagram_count = 0;
     datagram_used[EC_DIR_OUTPUT] = 0;
     datagram_used[EC_DIR_INPUT] = 0;
-
+    list_for_each_entry(fmmu_temp, &domain->fmmu_configs, list) {
+       ec_slave_config_t *sc = (ec_slave_config_t *)fmmu_temp->sc; // we have to remove the constness, sorry
+       sc->used_for_fmmu_datagram[fmmu_temp->dir] = 0;
+    }
     list_for_each_entry(fmmu, &domain->fmmu_configs, list) {
         // Correct logical FMMU address
         fmmu->logical_start_address += base_address;
 
         // Increment Input/Output counter to determine datagram types
         // and calculate expected working counters
-        datagram_used[fmmu->dir]++;
-
+        if (fmmu->sc->used_for_fmmu_datagram[fmmu->dir] == 0) {
+            ec_slave_config_t *sc = (ec_slave_config_t *)fmmu->sc;
+            datagram_used[fmmu->dir]++;
+            sc->used_for_fmmu_datagram[fmmu->dir] = 1;
+        }
         // If the current FMMU's data do not fit in the current datagram,
         // allocate a new one.
         if (datagram_size + fmmu->data_size > EC_MAX_DATA_SIZE) {
@@ -248,6 +255,10 @@ int ec_domain_finish(
             datagram_count++;
             datagram_used[EC_DIR_OUTPUT] = 0;
             datagram_used[EC_DIR_INPUT] = 0;
+            list_for_each_entry(fmmu_temp, &domain->fmmu_configs, list) {
+                ec_slave_config_t *sc = (ec_slave_config_t *)fmmu_temp->sc;
+               sc->used_for_fmmu_datagram[fmmu_temp->dir] = 0;
+            }
         }
 
         datagram_size += fmmu->data_size;
diff --git a/master/slave_config.c b/master/slave_config.c
index 42d38779..a30dd1ee 100644
--- a/master/slave_config.c
+++ b/master/slave_config.c
@@ -75,7 +75,8 @@ void ec_slave_config_init(
         ec_sync_config_init(&sc->sync_configs[i]);
 
     sc->used_fmmus = 0;
-
+    sc->used_for_fmmu_datagram[EC_DIR_INPUT] = 0;
+    sc->used_for_fmmu_datagram[EC_DIR_OUTPUT] = 0;
     sc->dc_assign_activate = 0x0000;
     sc->dc_sync[0].cycle_time = 0x00000000;
     sc->dc_sync[1].cycle_time = 0x00000000;
diff --git a/master/slave_config.h b/master/slave_config.h
index 4954a8f1..599c1b38 100644
--- a/master/slave_config.h
+++ b/master/slave_config.h
@@ -70,7 +70,7 @@ struct ec_slave_config {
                                                    configurations. */
     ec_fmmu_config_t fmmu_configs[EC_MAX_FMMUS]; /**< FMMU configurations. */
     uint8_t used_fmmus; /**< Number of FMMUs used. */
-
+    unsigned int used_for_fmmu_datagram[EC_DIR_COUNT];
     uint16_t dc_assign_activate; /**< Vendor-specific AssignActivate word. */
     ec_sync_signal_t dc_sync[EC_SYNC_SIGNAL_COUNT]; /**< DC sync signals. */