Added config_map_aligned to use byte aligned outputs and inputs (#645)

OpenEtherCATsociety · Oct 19, 2022 · 6a5babf · 6a5babf
1 parent 26fc5dd
commit 6a5babf
Show file tree

Hide file tree

Showing 3 changed files with 87 additions and 11 deletions.
diff --git a/soem/ethercatconfig.c b/soem/ethercatconfig.c
@@ -1175,16 +1175,7 @@ static void ecx_config_create_output_mappings(ecx_contextt *context, void *pIOma
       context->grouplist[group].outputsWKC++;
 }
 
-/** 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
- * @param[in]  group      = group to map, 0 = all groups
- * @return IOmap size
- */
-int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group)
+static int ecx_main_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group, boolean forceByteAlignment)
 {
    uint16 slave, configadr;
    uint8 BitPos;
@@ -1218,6 +1209,17 @@ int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group)
             if (context->slavelist[slave].Obits)
             {
                ecx_config_create_output_mappings (context, pIOmap, group, slave, &LogAddr, &BitPos);
+
+               if (forceByteAlignment)
+               {
+                  /* Force byte alignment if the output is < 8 bits */
+                  if (BitPos)
+                  {
+                     LogAddr++;
+                     BitPos = 0;
+                  }
+               } 
+
                diff = LogAddr - oLogAddr;
                oLogAddr = LogAddr;
                if ((segmentsize + diff) > (EC_MAXLRWDATA - EC_FIRSTDCDATAGRAM))
@@ -1278,6 +1280,17 @@ int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group)
             {
 
                ecx_config_create_input_mappings(context, pIOmap, group, slave, &LogAddr, &BitPos);
+
+               if (forceByteAlignment)
+               {
+                  /* Force byte alignment if the input is < 8 bits */
+                  if (BitPos)
+                  {
+                     LogAddr++;
+                     BitPos = 0;
+                  }
+               } 
+
                diff = LogAddr - oLogAddr;
                oLogAddr = LogAddr;
                if ((segmentsize + diff) > (EC_MAXLRWDATA - EC_FIRSTDCDATAGRAM))
@@ -1354,6 +1367,34 @@ 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
+* in sequential order (legacy SOEM way).
+*
+ *
+ * @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_map_group(ecx_contextt *context, void *pIOmap, uint8 group)
+{
+   return ecx_main_config_map_group(context, pIOmap, group, FALSE);
+}
+
+/** Map all PDOs in one group of slaves to IOmap with Outputs/Inputs
+* in sequential order (legacy SOEM way) and force byte alignement.
+*
+ *
+ * @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_map_group_aligned(ecx_contextt *context, void *pIOmap, uint8 group)
+{
+   return ecx_main_config_map_group(context, pIOmap, group, TRUE);
+}
+
 /** Map all PDOs in one group of slaves to IOmap with Outputs/Inputs
  * overlapping. NOTE: Must use this for TI ESC when using LRW.
  *
@@ -1659,6 +1700,19 @@ int ec_config_overlap_map_group(void *pIOmap, uint8 group)
    return ecx_config_overlap_map_group(&ecx_context, pIOmap, group);
 }
 
+/** Map all PDOs in one group of slaves to IOmap with Outputs/Inputs
+ * in sequential order (legacy SOEM way) and force byte alignment.
+ *
+ * @param[out] pIOmap     = pointer to IOmap
+ * @param[in]  group      = group to map, 0 = all groups
+ * @return IOmap size
+ * @see ecx_config_map_group
+ */
+int ec_config_map_group_aligned(void *pIOmap, uint8 group)
+{
+   return ecx_config_map_group_aligned(&ecx_context, pIOmap, group);
+}
+
 /** Map all PDOs from slaves to IOmap with Outputs/Inputs
  * in sequential order (legacy SOEM way).
  *
@@ -1681,6 +1735,17 @@ int ec_config_overlap_map(void *pIOmap)
    return ec_config_overlap_map_group(pIOmap, 0);
 }
 
+/** Map all PDOs from slaves to IOmap with Outputs/Inputs
+ * in sequential order (legacy SOEM way) and force byte alignment.
+ *
+ * @param[out] pIOmap     = pointer to IOmap
+ * @return IOmap size
+ */
+int ec_config_map_aligned(void *pIOmap)
+{
+   return ec_config_map_group_aligned(pIOmap, 0);
+}
+
 /** Enumerate / map and init all slaves.
  *
  * @param[in] usetable    = TRUE when using configtable to init slaves, FALSE otherwise

diff --git a/soem/ethercatconfig.h b/soem/ethercatconfig.h
@@ -23,8 +23,10 @@ extern "C"
 int ec_config_init(uint8 usetable);
 int ec_config_map(void *pIOmap);
 int ec_config_overlap_map(void *pIOmap);
+int ec_config_map_aligned(void *pIOmap);
 int ec_config_map_group(void *pIOmap, uint8 group);
 int ec_config_overlap_map_group(void *pIOmap, uint8 group);
+int ec_config_map_group_aligned(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);
@@ -34,6 +36,7 @@ int ec_reconfig_slave(uint16 slave, int timeout);
 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_config_map_group_aligned(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/test/linux/simple_test/simple_test.c b/test/linux/simple_test/simple_test.c
@@ -24,6 +24,7 @@ boolean needlf;
 volatile int wkc;
 boolean inOP;
 uint8 currentgroup = 0;
+boolean forceByteAlignment = FALSE;
 
 void simpletest(char *ifname)
 {
@@ -44,7 +45,14 @@ void simpletest(char *ifname)
       {
          printf("%d slaves found and configured.\n",ec_slavecount);
 
-         ec_config_map(&IOmap);
+         if (forceByteAlignment)
+         {
+            ec_config_map_aligned(&IOmap);
+         }
+         else
+         {
+            ec_config_map(&IOmap);
+         }
 
          ec_configdc();