From 093311561c36810cb16957850b3a13ad9ddf4887 Mon Sep 17 00:00:00 2001
From: Andreas Karlsson <andreas.karlsson@rt-labs.com>
Date: Tue, 3 Mar 2020 10:27:37 +0100
Subject: [PATCH] Limit expected working counter per ESC

Add one to the expected working counter per
ESC/Slave, not for every syncmanager/fmmu.
Use case, if the slave define more input/output
syncmanager/fmmus it still only generate maximum
working counter of 3 for a RW command.

fixes 374
---
 soem/ethercatconfig.c | 33 ++++++++++++++++++++++++---------
 1 file changed, 24 insertions(+), 9 deletions(-)

diff --git a/soem/ethercatconfig.c b/soem/ethercatconfig.c
index af667a6..85068f3 100644
--- a/soem/ethercatconfig.c
+++ b/soem/ethercatconfig.c
@@ -900,6 +900,7 @@ static void ecx_config_create_input_mappings(ecx_contextt *context, void *pIOmap
    int ByteCount = 0;
    int FMMUsize = 0;
    int FMMUdone = 0;
+   int AddToInputsWKC = 0;
    uint8 SMc = 0;
    uint16 EndAddr;
    uint16 SMlength;
@@ -1002,8 +1003,9 @@ static void ecx_config_create_input_mappings(ecx_contextt *context, void *pIOmap
          /* 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++;
+         /* Set flag to add one for an input FMMU,
+            a single ESC can only contribute once */
+         AddToInputsWKC = 1;
       }
       if (!context->slavelist[slave].inputs)
       {
@@ -1029,6 +1031,10 @@ static void ecx_config_create_input_mappings(ecx_contextt *context, void *pIOmap
       FMMUc++;
    }
    context->slavelist[slave].FMMUunused = FMMUc;
+
+   /* Add one WKC for an input if flag is true */
+   if (AddToInputsWKC)
+      context->grouplist[group].inputsWKC++;
 }
 
 static void ecx_config_create_output_mappings(ecx_contextt *context, void *pIOmap, 
@@ -1038,6 +1044,7 @@ static void ecx_config_create_output_mappings(ecx_contextt *context, void *pIOma
    int ByteCount = 0;
    int FMMUsize = 0;
    int FMMUdone = 0;
+   int AddToOutputsWKC = 0;
    uint8 SMc = 0;
    uint16 EndAddr;
    uint16 SMlength;
@@ -1126,13 +1133,18 @@ static void ecx_config_create_output_mappings(ecx_contextt *context, void *pIOma
          *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].FMMU[FMMUc].LogLength)
+      {
+         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);
+         /* Set flag to add one for an output FMMU,
+            a single ESC can only contribute once */
+         AddToOutputsWKC = 1;
+      }
       if (!context->slavelist[slave].outputs)
       {
          if (group)
@@ -1158,6 +1170,9 @@ static void ecx_config_create_output_mappings(ecx_contextt *context, void *pIOma
       FMMUc++;
    }
    context->slavelist[slave].FMMUunused = FMMUc;
+   /* Add one WKC for an output if flag is true */
+   if (AddToOutputsWKC)
+      context->grouplist[group].outputsWKC++;
 }
 
 /** Map all PDOs in one group of slaves to IOmap with Outputs/Inputs