diff --git a/soem/ethercatconfig.c b/soem/ethercatconfig.c
index 34b9e974..b31c04f2 100644
--- a/soem/ethercatconfig.c
+++ b/soem/ethercatconfig.c
@@ -820,7 +820,309 @@ static int ecx_get_threadcount(void)
    return thrc;
 }
 
-/** Map all PDOs in one group of slaves to IOmap.
+static void ecx_config_find_mappings(ecx_contextt *context, uint8 group)
+{
+   int thrn, thrc;
+   uint16 slave;
+
+   for (thrn = 0; thrn < EC_MAX_MAPT; thrn++)
+   {
+      ecx_mapt[thrn].running = 0;
+   }
+   /* find CoE and SoE mapping of slaves in multiple threads */
+   for (slave = 1; slave <= *(context->slavecount); slave++)
+   {
+      if (!group || (group == context->slavelist[slave].group))
+      {
+         if (EC_MAX_MAPT <= 1)
+         {
+            /* serialised version */
+            ecx_map_coe_soe(context, slave, 0);
+         }
+         else
+         {
+            /* multi-threaded version */
+            while ((thrn = ecx_find_mapt()) < 0)
+            {
+               osal_usleep(1000);
+            }
+            ecx_mapt[thrn].context = context;
+            ecx_mapt[thrn].slave = slave;
+            ecx_mapt[thrn].thread_n = thrn;
+            ecx_mapt[thrn].running = 1;
+            osal_thread_create(&(ecx_threadh[thrn]), 128000,
+               &ecx_mapper_thread, &(ecx_mapt[thrn]));
+         }
+      }
+   }
+   /* wait for all threads to finish */
+   do
+   {
+      thrc = ecx_get_threadcount();
+      if (thrc)
+      {
+         osal_usleep(1000);
+      }
+   } while (thrc);
+   /* find SII mapping of slave and program SM */
+   for (slave = 1; slave <= *(context->slavecount); slave++)
+   {
+      if (!group || (group == context->slavelist[slave].group))
+      {
+         ecx_map_sii(context, slave);
+         ecx_map_sm(context, slave);
+      }
+   }
+}
+
+static void ecx_config_create_input_mappings(ecx_contextt *context, void *pIOmap, 
+   uint8 group, int16 slave, uint32 * LogAddr, uint8 * BitPos)
+{
+   int BitCount = 0;
+   int ByteCount = 0;
+   int FMMUsize = 0;
+   int FMMUdone = 0;
+   uint8 SMc = 0;
+   uint16 EndAddr;
+   uint16 SMlength;
+   uint16 configadr;
+   uint8 FMMUc;
+
+   EC_PRINT(" =Slave %d, INPUT MAPPING\n", slave);
+
+   configadr = context->slavelist[slave].configadr;
+   FMMUc = context->slavelist[slave].FMMUunused;
+   if (context->slavelist[slave].Obits) /* find free FMMU */
+   {
+      while (context->slavelist[slave].FMMU[FMMUc].LogStart)
+      {
+         FMMUc++;
+      }
+   }
+   /* search for SM that contribute to the input mapping */
+   while ((SMc < (EC_MAXSM - 1)) && (FMMUdone < ((context->slavelist[slave].Ibits + 7) / 8)))
+   {
+      EC_PRINT("    FMMU %d\n", FMMUc);
+      while ((SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 4))
+      {
+         SMc++;
+      }
+      EC_PRINT("      SM%d\n", SMc);
+      context->slavelist[slave].FMMU[FMMUc].PhysStart =
+         context->slavelist[slave].SM[SMc].StartAddr;
+      SMlength = etohs(context->slavelist[slave].SM[SMc].SMlength);
+      ByteCount += SMlength;
+      BitCount += SMlength * 8;
+      EndAddr = etohs(context->slavelist[slave].SM[SMc].StartAddr) + SMlength;
+      while ((BitCount < context->slavelist[slave].Ibits) && (SMc < (EC_MAXSM - 1))) /* more SM for input */
+      {
+         SMc++;
+         while ((SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 4))
+         {
+            SMc++;
+         }
+         /* if addresses from more SM connect use one FMMU otherwise break up in mutiple FMMU */
+         if (etohs(context->slavelist[slave].SM[SMc].StartAddr) > EndAddr)
+         {
+            break;
+         }
+         EC_PRINT("      SM%d\n", SMc);
+         SMlength = etohs(context->slavelist[slave].SM[SMc].SMlength);
+         ByteCount += SMlength;
+         BitCount += SMlength * 8;
+         EndAddr = etohs(context->slavelist[slave].SM[SMc].StartAddr) + SMlength;
+      }
+
+      /* bit oriented slave */
+      if (!context->slavelist[slave].Ibytes)
+      {
+         context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(*LogAddr);
+         context->slavelist[slave].FMMU[FMMUc].LogStartbit = *BitPos;
+         *BitPos += context->slavelist[slave].Ibits - 1;
+         if (*BitPos > 7)
+         {
+            *LogAddr += 1;
+            *BitPos -= 8;
+         }
+         FMMUsize = *LogAddr - etohl(context->slavelist[slave].FMMU[FMMUc].LogStart) + 1;
+         context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize);
+         context->slavelist[slave].FMMU[FMMUc].LogEndbit = *BitPos;
+         *BitPos += 1;
+         if (*BitPos > 7)
+         {
+            *LogAddr += 1;
+            *BitPos -= 8;
+         }
+      }
+      /* byte oriented slave */
+      else
+      {
+         if (*BitPos)
+         {
+            *LogAddr += 1;
+            *BitPos = 0;
+         }
+         context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(*LogAddr);
+         context->slavelist[slave].FMMU[FMMUc].LogStartbit = *BitPos;
+         *BitPos = 7;
+         FMMUsize = ByteCount;
+         if ((FMMUsize + FMMUdone)> (int)context->slavelist[slave].Ibytes)
+         {
+            FMMUsize = context->slavelist[slave].Ibytes - FMMUdone;
+         }
+         *LogAddr += FMMUsize;
+         context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize);
+         context->slavelist[slave].FMMU[FMMUc].LogEndbit = *BitPos;
+         *BitPos = 0;
+      }
+      FMMUdone += FMMUsize;
+      if (context->slavelist[slave].FMMU[FMMUc].LogLength)
+      {
+         context->slavelist[slave].FMMU[FMMUc].PhysStartBit = 0;
+         context->slavelist[slave].FMMU[FMMUc].FMMUtype = 1;
+         context->slavelist[slave].FMMU[FMMUc].FMMUactive = 1;
+         /* program FMMU for input */
+         ecx_FPWR(context->port, configadr, ECT_REG_FMMU0 + (sizeof(ec_fmmut) * FMMUc),
+            sizeof(ec_fmmut), &(context->slavelist[slave].FMMU[FMMUc]), EC_TIMEOUTRET3);
+         /* add one for an input FMMU */
+         context->grouplist[group].inputsWKC++;
+      }
+      if (!context->slavelist[slave].inputs)
+      {
+         context->slavelist[slave].inputs =
+            (uint8 *)(pIOmap)+etohl(context->slavelist[slave].FMMU[FMMUc].LogStart);
+         context->slavelist[slave].Istartbit =
+            context->slavelist[slave].FMMU[FMMUc].LogStartbit;
+         EC_PRINT("    Inputs %p startbit %d\n",
+            context->slavelist[slave].inputs,
+            context->slavelist[slave].Istartbit);
+      }
+      FMMUc++;
+   }
+   context->slavelist[slave].FMMUunused = FMMUc;
+}
+
+static void ecx_config_create_output_mappings(ecx_contextt *context, void *pIOmap, 
+   uint8 group, int16 slave, uint32 * LogAddr, uint8 * BitPos)
+{
+   int BitCount = 0;
+   int ByteCount = 0;
+   int FMMUsize = 0;
+   int FMMUdone = 0;
+   uint8 SMc = 0;
+   uint16 EndAddr;
+   uint16 SMlength;
+   uint16 configadr;
+   uint8 FMMUc;
+
+   EC_PRINT("  OUTPUT MAPPING\n");
+
+   FMMUc = context->slavelist[slave].FMMUunused;
+   configadr = context->slavelist[slave].configadr;
+
+   /* search for SM that contribute to the output mapping */
+   while ((SMc < (EC_MAXSM - 1)) && (FMMUdone < ((context->slavelist[slave].Obits + 7) / 8)))
+   {
+      EC_PRINT("    FMMU %d\n", FMMUc);
+      while ((SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 3))
+      {
+         SMc++;
+      }
+      EC_PRINT("      SM%d\n", SMc);
+      context->slavelist[slave].FMMU[FMMUc].PhysStart =
+         context->slavelist[slave].SM[SMc].StartAddr;
+      SMlength = etohs(context->slavelist[slave].SM[SMc].SMlength);
+      ByteCount += SMlength;
+      BitCount += SMlength * 8;
+      EndAddr = etohs(context->slavelist[slave].SM[SMc].StartAddr) + SMlength;
+      while ((BitCount < context->slavelist[slave].Obits) && (SMc < (EC_MAXSM - 1))) /* more SM for output */
+      {
+         SMc++;
+         while ((SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 3))
+         {
+            SMc++;
+         }
+         /* if addresses from more SM connect use one FMMU otherwise break up in mutiple FMMU */
+         if (etohs(context->slavelist[slave].SM[SMc].StartAddr) > EndAddr)
+         {
+            break;
+         }
+         EC_PRINT("      SM%d\n", SMc);
+         SMlength = etohs(context->slavelist[slave].SM[SMc].SMlength);
+         ByteCount += SMlength;
+         BitCount += SMlength * 8;
+         EndAddr = etohs(context->slavelist[slave].SM[SMc].StartAddr) + SMlength;
+      }
+
+      /* bit oriented slave */
+      if (!context->slavelist[slave].Obytes)
+      {
+         context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(*LogAddr);
+         context->slavelist[slave].FMMU[FMMUc].LogStartbit = *BitPos;
+         *BitPos += context->slavelist[slave].Obits - 1;
+         if (*BitPos > 7)
+         {
+            *LogAddr += 1;
+            *BitPos -= 8;
+         }
+         FMMUsize = *LogAddr - etohl(context->slavelist[slave].FMMU[FMMUc].LogStart) + 1;
+         context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize);
+         context->slavelist[slave].FMMU[FMMUc].LogEndbit = *BitPos;
+         *BitPos += 1;
+         if (*BitPos > 7)
+         {
+            *LogAddr += 1;
+            *BitPos -= 8;
+         }
+      }
+      /* byte oriented slave */
+      else
+      {
+         if (*BitPos)
+         {
+            *LogAddr += 1;
+            *BitPos = 0;
+         }
+         context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(*LogAddr);
+         context->slavelist[slave].FMMU[FMMUc].LogStartbit = *BitPos;
+         *BitPos = 7;
+         FMMUsize = ByteCount;
+         if ((FMMUsize + FMMUdone)> (int)context->slavelist[slave].Obytes)
+         {
+            FMMUsize = context->slavelist[slave].Obytes - FMMUdone;
+         }
+         *LogAddr += FMMUsize;
+         context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize);
+         context->slavelist[slave].FMMU[FMMUc].LogEndbit = *BitPos;
+         *BitPos = 0;
+      }
+      FMMUdone += FMMUsize;
+      context->slavelist[slave].FMMU[FMMUc].PhysStartBit = 0;
+      context->slavelist[slave].FMMU[FMMUc].FMMUtype = 2;
+      context->slavelist[slave].FMMU[FMMUc].FMMUactive = 1;
+      /* program FMMU for output */
+      ecx_FPWR(context->port, configadr, ECT_REG_FMMU0 + (sizeof(ec_fmmut) * FMMUc),
+         sizeof(ec_fmmut), &(context->slavelist[slave].FMMU[FMMUc]), EC_TIMEOUTRET3);
+      context->grouplist[group].outputsWKC++;
+      if (!context->slavelist[slave].outputs)
+      {
+         context->slavelist[slave].outputs =
+            (uint8 *)(pIOmap)+etohl(context->slavelist[slave].FMMU[FMMUc].LogStart);
+         context->slavelist[slave].Ostartbit =
+            context->slavelist[slave].FMMU[FMMUc].LogStartbit;
+         EC_PRINT("    slave %d Outputs %p startbit %d\n",
+            slave,
+            context->slavelist[slave].outputs,
+            context->slavelist[slave].Ostartbit);
+      }
+      FMMUc++;
+   }
+   context->slavelist[slave].FMMUunused = FMMUc;
+}
+
+/** Map all PDOs in one group of slaves to IOmap with Outputs/Inputs
+* in sequential order (legacy SOEM way).
+*
  *
  * @param[in]  context    = context struct
  * @param[out] pIOmap     = pointer to IOmap
@@ -830,16 +1132,12 @@ static int ecx_get_threadcount(void)
 int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group)
 {
    uint16 slave, configadr;
-   int BitCount, ByteCount, FMMUsize, FMMUdone;
-   uint16 SMlength, EndAddr;
    uint8 BitPos;
-   uint8 SMc, FMMUc;
    uint32 LogAddr = 0;
    uint32 oLogAddr = 0;
    uint32 diff;
    uint16 currentsegment = 0;
    uint32 segmentsize = 0;
-   int thrn, thrc;
 
    if ((*(context->slavecount) > 0) && (group < context->maxgroup))
    {
@@ -851,165 +1149,20 @@ int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group)
       context->grouplist[group].outputsWKC = 0;
       context->grouplist[group].inputsWKC = 0;
 
-      for(thrn = 0 ; thrn < EC_MAX_MAPT ; thrn++)
-      {
-         ecx_mapt[thrn].running = 0;
-      }
-      /* find CoE and SoE mapping of slaves in multiple threads */
-      for (slave = 1; slave <= *(context->slavecount); slave++)
-      {
-         if (!group || (group == context->slavelist[slave].group))
-         {
-            if(EC_MAX_MAPT <= 1)
-            {
-               /* serialised version */
-               ecx_map_coe_soe(context, slave, 0);
-            }
-            else
-            {
-               /* multi-threaded version */
-               while((thrn = ecx_find_mapt()) < 0)
-               {
-                  osal_usleep(1000);
-               }
-               ecx_mapt[thrn].context = context;
-               ecx_mapt[thrn].slave = slave;
-               ecx_mapt[thrn].thread_n = thrn;
-               ecx_mapt[thrn].running = 1;
-               osal_thread_create(&(ecx_threadh[thrn]), 128000,
-                  &ecx_mapper_thread, &(ecx_mapt[thrn]));
-            }
-         }
-      }
-      /* wait for all threads to finish */
-      do
-      {
-         thrc = ecx_get_threadcount();
-         if(thrc)
-         {
-            osal_usleep(1000);
-         }
-      } while(thrc);
-      /* find SII mapping of slave and program SM */
-      for (slave = 1; slave <= *(context->slavecount); slave++)
-      {
-         if (!group || (group == context->slavelist[slave].group))
-         {
-            ecx_map_sii(context, slave);
-            ecx_map_sm(context, slave);
-         }
-      }
+      /* Find mappings and program syncmanagers */
+      ecx_config_find_mappings(context, group);
 
-      /* do input mapping of slave and program FMMUs */
+      /* do output mapping of slave and program FMMUs */
       for (slave = 1; slave <= *(context->slavecount); slave++)
       {
          configadr = context->slavelist[slave].configadr;
 
          if (!group || (group == context->slavelist[slave].group))
          {
-            FMMUc = context->slavelist[slave].FMMUunused;
-            SMc = 0;
-            BitCount = 0;
-            ByteCount = 0;
-            EndAddr = 0;
-            FMMUsize = 0;
-            FMMUdone = 0;
             /* create output mapping */
             if (context->slavelist[slave].Obits)
             {
-               EC_PRINT("  OUTPUT MAPPING\n");
-               /* search for SM that contribute to the output mapping */
-               while ( (SMc < (EC_MAXSM - 1)) && (FMMUdone < ((context->slavelist[slave].Obits + 7) / 8)))
-               {
-                  EC_PRINT("    FMMU %d\n", FMMUc);
-                  while ( (SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 3)) SMc++;
-                  EC_PRINT("      SM%d\n", SMc);
-                  context->slavelist[slave].FMMU[FMMUc].PhysStart =
-                     context->slavelist[slave].SM[SMc].StartAddr;
-                  SMlength = etohs(context->slavelist[slave].SM[SMc].SMlength);
-                  ByteCount += SMlength;
-                  BitCount += SMlength * 8;
-                  EndAddr = etohs(context->slavelist[slave].SM[SMc].StartAddr) + SMlength;
-                  while ( (BitCount < context->slavelist[slave].Obits) && (SMc < (EC_MAXSM - 1)) ) /* more SM for output */
-                  {
-                     SMc++;
-                     while ( (SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 3)) SMc++;
-                     /* if addresses from more SM connect use one FMMU otherwise break up in mutiple FMMU */
-                     if ( etohs(context->slavelist[slave].SM[SMc].StartAddr) > EndAddr )
-                     {
-                        break;
-                     }
-                     EC_PRINT("      SM%d\n", SMc);
-                     SMlength = etohs(context->slavelist[slave].SM[SMc].SMlength);
-                     ByteCount += SMlength;
-                     BitCount += SMlength * 8;
-                     EndAddr = etohs(context->slavelist[slave].SM[SMc].StartAddr) + SMlength;
-                  }
-
-                  /* bit oriented slave */
-                  if (!context->slavelist[slave].Obytes)
-                  {
-                     context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(LogAddr);
-                     context->slavelist[slave].FMMU[FMMUc].LogStartbit = BitPos;
-                     BitPos += context->slavelist[slave].Obits - 1;
-                     if (BitPos > 7)
-                     {
-                        LogAddr++;
-                        BitPos -= 8;
-                     }
-                     FMMUsize = LogAddr - etohl(context->slavelist[slave].FMMU[FMMUc].LogStart) + 1;
-                     context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize);
-                     context->slavelist[slave].FMMU[FMMUc].LogEndbit = BitPos;
-                     BitPos ++;
-                     if (BitPos > 7)
-                     {
-                        LogAddr++;
-                        BitPos -= 8;
-                     }
-                  }
-                  /* byte oriented slave */
-                  else
-                  {
-                     if (BitPos)
-                     {
-                        LogAddr++;
-                        BitPos = 0;
-                     }
-                     context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(LogAddr);
-                     context->slavelist[slave].FMMU[FMMUc].LogStartbit = BitPos;
-                     BitPos = 7;
-                     FMMUsize = ByteCount;
-                     if ((FMMUsize + FMMUdone)> (int)context->slavelist[slave].Obytes)
-                     {
-                        FMMUsize = context->slavelist[slave].Obytes - FMMUdone;
-                     }
-                     LogAddr += FMMUsize;
-                     context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize);
-                     context->slavelist[slave].FMMU[FMMUc].LogEndbit = BitPos;
-                     BitPos = 0;
-                  }
-                  FMMUdone += FMMUsize;
-                  context->slavelist[slave].FMMU[FMMUc].PhysStartBit = 0;
-                  context->slavelist[slave].FMMU[FMMUc].FMMUtype = 2;
-                  context->slavelist[slave].FMMU[FMMUc].FMMUactive = 1;
-                  /* program FMMU for output */
-                  ecx_FPWR(context->port, configadr, ECT_REG_FMMU0 + (sizeof(ec_fmmut) * FMMUc),
-                     sizeof(ec_fmmut), &(context->slavelist[slave].FMMU[FMMUc]), EC_TIMEOUTRET3);
-                  context->grouplist[group].outputsWKC++;
-                  if (!context->slavelist[slave].outputs)
-                  {
-                     context->slavelist[slave].outputs =
-                        (uint8 *)(pIOmap) + etohl(context->slavelist[slave].FMMU[FMMUc].LogStart);
-                     context->slavelist[slave].Ostartbit =
-                        context->slavelist[slave].FMMU[FMMUc].LogStartbit;
-                     EC_PRINT("    slave %d Outputs %p startbit %d\n",
-                        slave,
-                        context->slavelist[slave].outputs,
-                        context->slavelist[slave].Ostartbit);
-                  }
-                  FMMUc++;
-               }
-               context->slavelist[slave].FMMUunused = FMMUc;
+               ecx_config_create_output_mappings (context, pIOmap, group, slave, &LogAddr, &BitPos);
                diff = LogAddr - oLogAddr;
                oLogAddr = LogAddr;
                if ((segmentsize + diff) > (EC_MAXLRWDATA - EC_FIRSTDCDATAGRAM))
@@ -1064,116 +1217,11 @@ int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group)
          configadr = context->slavelist[slave].configadr;
          if (!group || (group == context->slavelist[slave].group))
          {
-            FMMUc = context->slavelist[slave].FMMUunused;
-            if (context->slavelist[slave].Obits) /* find free FMMU */
-            {
-               while ( context->slavelist[slave].FMMU[FMMUc].LogStart ) FMMUc++;
-            }
-            SMc = 0;
-            BitCount = 0;
-            ByteCount = 0;
-            EndAddr = 0;
-            FMMUsize = 0;
-            FMMUdone = 0;
             /* create input mapping */
             if (context->slavelist[slave].Ibits)
             {
-               EC_PRINT(" =Slave %d, INPUT MAPPING\n", slave);
-               /* search for SM that contribute to the input mapping */
-               while ( (SMc < (EC_MAXSM - 1)) && (FMMUdone < ((context->slavelist[slave].Ibits + 7) / 8)))
-               {
-                  EC_PRINT("    FMMU %d\n", FMMUc);
-                  while ( (SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 4)) SMc++;
-                  EC_PRINT("      SM%d\n", SMc);
-                  context->slavelist[slave].FMMU[FMMUc].PhysStart =
-                     context->slavelist[slave].SM[SMc].StartAddr;
-                  SMlength = etohs(context->slavelist[slave].SM[SMc].SMlength);
-                  ByteCount += SMlength;
-                  BitCount += SMlength * 8;
-                  EndAddr = etohs(context->slavelist[slave].SM[SMc].StartAddr) + SMlength;
-                  while ( (BitCount < context->slavelist[slave].Ibits) && (SMc < (EC_MAXSM - 1)) ) /* more SM for input */
-                  {
-                     SMc++;
-                     while ( (SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 4)) SMc++;
-                     /* if addresses from more SM connect use one FMMU otherwise break up in mutiple FMMU */
-                     if ( etohs(context->slavelist[slave].SM[SMc].StartAddr) > EndAddr )
-                     {
-                        break;
-                     }
-                     EC_PRINT("      SM%d\n", SMc);
-                     SMlength = etohs(context->slavelist[slave].SM[SMc].SMlength);
-                     ByteCount += SMlength;
-                     BitCount += SMlength * 8;
-                     EndAddr = etohs(context->slavelist[slave].SM[SMc].StartAddr) + SMlength;
-                  }
-
-                  /* bit oriented slave */
-                  if (!context->slavelist[slave].Ibytes)
-                  {
-                     context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(LogAddr);
-                     context->slavelist[slave].FMMU[FMMUc].LogStartbit = BitPos;
-                     BitPos += context->slavelist[slave].Ibits - 1;
-                     if (BitPos > 7)
-                     {
-                        LogAddr++;
-                        BitPos -= 8;
-                     }
-                     FMMUsize = LogAddr - etohl(context->slavelist[slave].FMMU[FMMUc].LogStart) + 1;
-                     context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize);
-                     context->slavelist[slave].FMMU[FMMUc].LogEndbit = BitPos;
-                     BitPos ++;
-                     if (BitPos > 7)
-                     {
-                        LogAddr++;
-                        BitPos -= 8;
-                     }
-                  }
-                  /* byte oriented slave */
-                  else
-                  {
-                     if (BitPos)
-                     {
-                        LogAddr++;
-                        BitPos = 0;
-                     }
-                     context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(LogAddr);
-                     context->slavelist[slave].FMMU[FMMUc].LogStartbit = BitPos;
-                     BitPos = 7;
-                     FMMUsize = ByteCount;
-                     if ((FMMUsize + FMMUdone)> (int)context->slavelist[slave].Ibytes)
-                     {
-                        FMMUsize = context->slavelist[slave].Ibytes - FMMUdone;
-                     }
-                     LogAddr += FMMUsize;
-                     context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize);
-                     context->slavelist[slave].FMMU[FMMUc].LogEndbit = BitPos;
-                     BitPos = 0;
-                  }
-                  FMMUdone += FMMUsize;
-                  if (context->slavelist[slave].FMMU[FMMUc].LogLength)
-                  {
-                     context->slavelist[slave].FMMU[FMMUc].PhysStartBit = 0;
-                     context->slavelist[slave].FMMU[FMMUc].FMMUtype = 1;
-                     context->slavelist[slave].FMMU[FMMUc].FMMUactive = 1;
-                     /* program FMMU for input */
-                     ecx_FPWR(context->port, configadr, ECT_REG_FMMU0 + (sizeof(ec_fmmut) * FMMUc),
-                        sizeof(ec_fmmut), &(context->slavelist[slave].FMMU[FMMUc]), EC_TIMEOUTRET3);
-                     /* add one for an input FMMU */
-                     context->grouplist[group].inputsWKC++;
-                  }
-                  if (!context->slavelist[slave].inputs)
-                  {
-                     context->slavelist[slave].inputs =
-                        (uint8 *)(pIOmap) + etohl(context->slavelist[slave].FMMU[FMMUc].LogStart);
-                     context->slavelist[slave].Istartbit =
-                        context->slavelist[slave].FMMU[FMMUc].LogStartbit;
-                     EC_PRINT("    Inputs %p startbit %d\n",
-                        context->slavelist[slave].inputs,
-                        context->slavelist[slave].Istartbit);
-                  }
-                  FMMUc++;
-               }
-               context->slavelist[slave].FMMUunused = FMMUc;
+ 
+               ecx_config_create_input_mappings(context, pIOmap, group, slave, &LogAddr, &BitPos);
                diff = LogAddr - oLogAddr;
                oLogAddr = LogAddr;
                if ((segmentsize + diff) > (EC_MAXLRWDATA - EC_FIRSTDCDATAGRAM))
@@ -1238,6 +1286,136 @@ int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group)
    return 0;
 }
 
+/** Map all PDOs in one group of slaves to IOmap with Outputs/Inputs
+ * overlapping. NOTE: Must use this for TI ESC when using LRW.
+ *
+ * @param[in]  context    = context struct
+ * @param[out] pIOmap     = pointer to IOmap
+ * @param[in]  group      = group to map, 0 = all groups
+ * @return IOmap size
+ */
+int ecx_config_overlap_map_group(ecx_contextt *context, void *pIOmap, uint8 group)
+{
+   uint16 slave, configadr;
+   uint8 BitPos;
+   uint32 mLogAddr = 0;
+   uint32 siLogAddr = 0;
+   uint32 soLogAddr = 0;
+   uint32 tempLogAddr;
+   uint32 diff;
+   uint16 currentsegment = 0;
+   uint32 segmentsize = 0;
+
+   if ((*(context->slavecount) > 0) && (group < context->maxgroup))
+   {
+      EC_PRINT("ec_config_map_group IOmap:%p group:%d\n", pIOmap, group);
+      mLogAddr = context->grouplist[group].logstartaddr;
+      siLogAddr = mLogAddr;
+      soLogAddr = mLogAddr;
+      BitPos = 0;
+      context->grouplist[group].nsegments = 0;
+      context->grouplist[group].outputsWKC = 0;
+      context->grouplist[group].inputsWKC = 0;
+
+      /* Find mappings and program syncmanagers */
+      ecx_config_find_mappings(context, group);
+      
+      /* do IO mapping of slave and program FMMUs */
+      for (slave = 1; slave <= *(context->slavecount); slave++)
+      {
+         configadr = context->slavelist[slave].configadr;
+         siLogAddr = soLogAddr = mLogAddr;
+
+         if (!group || (group == context->slavelist[slave].group))
+         {
+            /* create output mapping */
+            if (context->slavelist[slave].Obits)
+            {
+               
+               ecx_config_create_output_mappings(context, pIOmap, group, 
+                  slave, &soLogAddr, &BitPos);
+               if (BitPos)
+               {
+                  soLogAddr++;
+                  BitPos = 0;
+               }
+            }
+
+            /* create input mapping */
+            if (context->slavelist[slave].Ibits)
+            {
+               ecx_config_create_input_mappings(context, pIOmap, group, 
+                  slave, &siLogAddr, &BitPos);
+               if (BitPos)
+               {
+                  siLogAddr++;
+                  BitPos = 0;
+               }
+            }
+
+            tempLogAddr = (siLogAddr > soLogAddr) ?  siLogAddr : soLogAddr;
+            diff = tempLogAddr - mLogAddr;
+            mLogAddr = tempLogAddr;
+
+            if ((segmentsize + diff) > (EC_MAXLRWDATA - EC_FIRSTDCDATAGRAM))
+            {
+               context->grouplist[group].IOsegment[currentsegment] = segmentsize;
+               if (currentsegment < (EC_MAXIOSEGMENTS - 1))
+               {
+                  currentsegment++;
+                  segmentsize = diff;
+               }
+            }
+            else
+            {
+               segmentsize += diff;
+            }
+
+            ecx_eeprom2pdi(context, slave); /* set Eeprom control to PDI */
+            ecx_FPWRw(context->port, configadr, ECT_REG_ALCTL, htoes(EC_STATE_SAFE_OP), EC_TIMEOUTRET3); /* set safeop status */
+
+            if (context->slavelist[slave].blockLRW)
+            {
+               context->grouplist[group].blockLRW++;
+            }
+            context->grouplist[group].Ebuscurrent += context->slavelist[slave].Ebuscurrent;
+
+         }
+      }
+
+      context->grouplist[group].IOsegment[currentsegment] = segmentsize;
+      context->grouplist[group].nsegments = currentsegment + 1;
+      context->grouplist[group].Isegment = 0;
+      context->grouplist[group].Ioffset = 0;
+
+      context->grouplist[group].Obytes = soLogAddr;
+      context->grouplist[group].Ibytes = siLogAddr;
+      context->grouplist[group].outputs = pIOmap;
+      context->grouplist[group].inputs = (uint8 *)pIOmap + context->grouplist[group].Obytes;
+
+      /* Move calculated inputs with OBytes offset*/
+      for (slave = 1; slave <= *(context->slavecount); slave++)
+      {
+         context->slavelist[slave].inputs += context->grouplist[group].Obytes;
+      }
+
+      if (!group)
+      {
+         context->slavelist[0].outputs = pIOmap;
+         context->slavelist[0].Obytes = soLogAddr; /* store output bytes in master record */
+         context->slavelist[0].inputs = (uint8 *)pIOmap + context->slavelist[0].Obytes;
+         context->slavelist[0].Ibytes = siLogAddr;
+      }
+
+      EC_PRINT("IOmapSize %d\n", context->grouplist[group].Obytes + context->grouplist[group].Ibytes);
+
+      return (context->grouplist[group].Obytes + context->grouplist[group].Ibytes);
+   }
+
+   return 0;
+}
+
+
 /** Recover slave.
  *
  * @param[in] context = context struct
@@ -1368,7 +1546,8 @@ int ec_config_init(uint8 usetable)
    return ecx_config_init(&ecx_context, usetable);
 }
 
-/** Map all PDOs in one group of slaves to IOmap.
+/** Map all PDOs in one group of slaves to IOmap with Outputs/Inputs
+ * in sequential order (legacy SOEM way).
  *
  * @param[out] pIOmap     = pointer to IOmap
  * @param[in]  group      = group to map, 0 = all groups
@@ -1380,7 +1559,21 @@ int ec_config_map_group(void *pIOmap, uint8 group)
    return ecx_config_map_group(&ecx_context, pIOmap, group);
 }
 
-/** Map all PDOs from slaves to IOmap.
+/** Map all PDOs in one group of slaves to IOmap with Outputs/Inputs
+* overlapping. NOTE: Must use this for TI ESC when using LRW.
+*
+* @param[out] pIOmap     = pointer to IOmap
+* @param[in]  group      = group to map, 0 = all groups
+* @return IOmap size
+* @see ecx_config_overlap_map_group
+*/
+int ec_config_overlap_map_group(void *pIOmap, uint8 group)
+{
+   return ecx_config_overlap_map_group(&ecx_context, pIOmap, group);
+}
+
+/** Map all PDOs from slaves to IOmap with Outputs/Inputs
+ * in sequential order (legacy SOEM way).
  *
  * @param[out] pIOmap     = pointer to IOmap
  * @return IOmap size
@@ -1390,6 +1583,17 @@ int ec_config_map(void *pIOmap)
    return ec_config_map_group(pIOmap, 0);
 }
 
+/** Map all PDOs from slaves to IOmap with Outputs/Inputs
+* overlapping. NOTE: Must use this for TI ESC when using LRW.
+*
+* @param[out] pIOmap     = pointer to IOmap
+* @return IOmap size
+*/
+int ec_config_overlap_map(void *pIOmap)
+{
+   return ec_config_overlap_map_group(pIOmap, 0);
+}
+
 /** Enumerate / map and init all slaves.
  *
  * @param[in] usetable    = TRUE when using configtable to init slaves, FALSE otherwise
@@ -1407,6 +1611,23 @@ int ec_config(uint8 usetable, void *pIOmap)
    return wkc;
 }
 
+/** Enumerate / map and init all slaves.
+*
+* @param[in] usetable    = TRUE when using configtable to init slaves, FALSE otherwise
+* @param[out] pIOmap     = pointer to IOmap
+* @return Workcounter of slave discover datagram = number of slaves found
+*/
+int ec_config_overlap(uint8 usetable, void *pIOmap)
+{
+   int wkc;
+   wkc = ec_config_init(usetable);
+   if (wkc)
+   {
+      ec_config_overlap_map(pIOmap);
+   }
+   return wkc;
+}
+
 /** Recover slave.
  *
  * @param[in] slave   = slave to recover
diff --git a/soem/ethercatconfig.h b/soem/ethercatconfig.h
index 38e939f8..957376cf 100644
--- a/soem/ethercatconfig.h
+++ b/soem/ethercatconfig.h
@@ -22,14 +22,18 @@ extern "C"
 #ifdef EC_VER1
 int ec_config_init(uint8 usetable);
 int ec_config_map(void *pIOmap);
+int ec_config_overlap_map(void *pIOmap);
 int ec_config_map_group(void *pIOmap, uint8 group);
+int ec_config_overlap_map_group(void *pIOmap, uint8 group);
 int ec_config(uint8 usetable, void *pIOmap);
+int ec_config_overlap(uint8 usetable, void *pIOmap);
 int ec_recover_slave(uint16 slave, int timeout);
 int ec_reconfig_slave(uint16 slave, int timeout);
 #endif
 
 int ecx_config_init(ecx_contextt *context, uint8 usetable);
 int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group);
+int ecx_config_overlap_map_group(ecx_contextt *context, void *pIOmap, uint8 group);
 int ecx_recover_slave(ecx_contextt *context, uint16 slave, int timeout);
 int ecx_reconfig_slave(ecx_contextt *context, uint16 slave, int timeout);
 
diff --git a/soem/ethercatmain.c b/soem/ethercatmain.c
index c2b562dd..666325d8 100644
--- a/soem/ethercatmain.c
+++ b/soem/ethercatmain.c
@@ -1671,7 +1671,7 @@ static void ecx_clearindex(ecx_contextt *context)  {
  * @param[in]  group          = group number
  * @return >0 if processdata is transmitted.
  */
-int ecx_send_processdata_group(ecx_contextt *context, uint8 group)
+static int ecx_main_send_processdata(ecx_contextt *context, uint8 group, boolean use_overlap_io)
 {
    uint32 LogAdr;
    uint16 w1, w2;
@@ -1681,20 +1681,36 @@ int ecx_send_processdata_group(ecx_contextt *context, uint8 group)
    uint8* data;
    boolean first=FALSE;
    uint16 currentsegment = 0;
+   uint32 iomapinputoffset;
 
    wkc = 0;
    if(context->grouplist[group].hasdc)
    {
       first = TRUE;
    }
-   length = context->grouplist[group].Obytes + context->grouplist[group].Ibytes;
+
+   /* For overlapping IO map use the biggest */
+   if(use_overlap_io == TRUE)
+   {
+      /* For overlap IOmap make the frame EQ big to biggest part */
+      length = (context->grouplist[group].Obytes > context->grouplist[group].Ibytes) ?
+         context->grouplist[group].Obytes : context->grouplist[group].Ibytes;
+      /* Save the offset used to compensate where to save inputs when frame returns */
+      iomapinputoffset = context->grouplist[group].Obytes;
+   }
+   else
+   {
+      length = context->grouplist[group].Obytes + context->grouplist[group].Ibytes;
+      iomapinputoffset = 0;
+   }
+   
    LogAdr = context->grouplist[group].logstartaddr;
-   if (length)
+   if(length)
    {
 
       wkc = 1;
       /* LRW blocked by one or more slaves ? */
-      if (context->grouplist[group].blockLRW)
+      if(context->grouplist[group].blockLRW)
       {
          /* if inputs available generate LRD */
          if(context->grouplist[group].Ibytes)
@@ -1786,6 +1802,8 @@ int ecx_send_processdata_group(ecx_contextt *context, uint8 group)
          else
          {
             data = context->grouplist[group].inputs;
+            /* Clear offset, don't compensate for overlapping IOmap if we only got inputs */
+            iomapinputoffset = 0;
          }
          /* segment transfer if needed */
          do
@@ -1807,8 +1825,12 @@ int ecx_send_processdata_group(ecx_contextt *context, uint8 group)
             }
             /* send frame */
             ecx_outframe_red(context->port, idx);
-            /* push index and data pointer on stack */
-            ecx_pushindex(context, idx, data, sublength);
+            /* push index and data pointer on stack.
+             * the iomapinputoffset compensate for where the inputs are stored 
+             * in the IOmap if we use an overlapping IOmap. If a regular IOmap
+             * is used it should always be 0.
+             */
+            ecx_pushindex(context, idx, (data + iomapinputoffset), sublength);      
             length -= sublength;
             LogAdr += sublength;
             data += sublength;
@@ -1819,6 +1841,40 @@ int ecx_send_processdata_group(ecx_contextt *context, uint8 group)
    return wkc;
 }
 
+/** Transmit processdata to slaves.
+* Uses LRW, or LRD/LWR if LRW is not allowed (blockLRW).
+* Both the input and output processdata are transmitted in the overlapped IOmap.
+* The outputs with the actual data, the inputs replace the output data in the
+* returning frame. The inputs are gathered with the receive processdata function.
+* In contrast to the base LRW function this function is non-blocking.
+* If the processdata does not fit in one datagram, multiple are used.
+* In order to recombine the slave response, a stack is used.
+* @param[in]  context        = context struct
+* @param[in]  group          = group number
+* @return >0 if processdata is transmitted.
+*/
+int ecx_send_overlap_processdata_group(ecx_contextt *context, uint8 group)
+{
+   return ecx_main_send_processdata(context, group, TRUE);
+}
+
+/** Transmit processdata to slaves.
+* Uses LRW, or LRD/LWR if LRW is not allowed (blockLRW).
+* Both the input and output processdata are transmitted.
+* The outputs with the actual data, the inputs have a placeholder.
+* The inputs are gathered with the receive processdata function.
+* In contrast to the base LRW function this function is non-blocking.
+* If the processdata does not fit in one datagram, multiple are used.
+* In order to recombine the slave response, a stack is used.
+* @param[in]  context        = context struct
+* @param[in]  group          = group number
+* @return >0 if processdata is transmitted.
+*/
+int ecx_send_processdata_group(ecx_contextt *context, uint8 group)
+{
+   return ecx_main_send_processdata(context, group, FALSE);
+}
+
 /** Receive processdata from slaves.
  * Second part from ec_send_processdata().
  * Received datagrams are recombined with the processdata with help from the stack.
@@ -1911,6 +1967,11 @@ int ecx_send_processdata(ecx_contextt *context)
    return ecx_send_processdata_group(context, 0);
 }
 
+int ecx_send_overlap_processdata(ecx_contextt *context)
+{
+   return ecx_send_overlap_processdata_group(context, 0);
+}
+
 int ecx_receive_processdata(ecx_contextt *context, int timeout)
 {
    return ecx_receive_processdata_group(context, 0, timeout);
@@ -2265,6 +2326,24 @@ int ec_send_processdata_group(uint8 group)
    return ecx_send_processdata_group (&ecx_context, group);
 }
 
+/** Transmit processdata to slaves.
+* Uses LRW, or LRD/LWR if LRW is not allowed (blockLRW).
+* Both the input and output processdata are transmitted in the overlapped IOmap.
+* The outputs with the actual data, the inputs replace the output data in the
+* returning frame. The inputs are gathered with the receive processdata function.
+* In contrast to the base LRW function this function is non-blocking.
+* If the processdata does not fit in one datagram, multiple are used.
+* In order to recombine the slave response, a stack is used.
+* @param[in]  context        = context struct
+* @param[in]  group          = group number
+* @return >0 if processdata is transmitted.
+* @see ecx_send_overlap_processdata_group
+*/
+int ec_send_overlap_processdata_group(uint8 group)
+{
+   return ecx_send_overlap_processdata_group(&ecx_context, group);
+}
+
 /** Receive processdata from slaves.
  * Second part from ec_send_processdata().
  * Received datagrams are recombined with the processdata with help from the stack.
@@ -2284,6 +2363,11 @@ int ec_send_processdata(void)
    return ec_send_processdata_group(0);
 }
 
+int ec_send_overlap_processdata(void)
+{
+   return ec_send_overlap_processdata_group(0);
+}
+
 int ec_receive_processdata(int timeout)
 {
    return ec_receive_processdata_group(0, timeout);
diff --git a/soem/ethercatmain.h b/soem/ethercatmain.h
index 9c87b546..b8a3059c 100644
--- a/soem/ethercatmain.h
+++ b/soem/ethercatmain.h
@@ -467,8 +467,10 @@ int ec_writeeepromFP(uint16 configadr, uint16 eeproma, uint16 data, int timeout)
 void ec_readeeprom1(uint16 slave, uint16 eeproma);
 uint32 ec_readeeprom2(uint16 slave, int timeout);
 int ec_send_processdata_group(uint8 group);
+int ec_send_overlap_processdata_group(uint8 group);
 int ec_receive_processdata_group(uint8 group, int timeout);
 int ec_send_processdata(void);
+int ec_send_overlap_processdata(void);
 int ec_receive_processdata(int timeout);
 #endif
 
@@ -507,9 +509,10 @@ uint64 ecx_readeepromFP(ecx_contextt *context, uint16 configadr, uint16 eeproma,
 int ecx_writeeepromFP(ecx_contextt *context, uint16 configadr, uint16 eeproma, uint16 data, int timeout);
 void ecx_readeeprom1(ecx_contextt *context, uint16 slave, uint16 eeproma);
 uint32 ecx_readeeprom2(ecx_contextt *context, uint16 slave, int timeout);
-int ecx_send_processdata_group(ecx_contextt *context, uint8 group);
+int ecx_send_overlap_processdata_group(ecx_contextt *context, uint8 group);
 int ecx_receive_processdata_group(ecx_contextt *context, uint8 group, int timeout);
 int ecx_send_processdata(ecx_contextt *context);
+int ecx_send_overlap_processdata(ecx_contextt *context);
 int ecx_receive_processdata(ecx_contextt *context, int timeout);
 
 #ifdef __cplusplus