From 91616ac408e77372b92e685ce93d36d0e7593228 Mon Sep 17 00:00:00 2001
From: Sebastian Block <github@world-wi.de>
Date: Sat, 9 Jul 2022 15:15:53 +0200
Subject: [PATCH] FoE: resend packet on BUSY answer while writing

---
 soem/ethercatfoe.c | 40 ++++++++++++++++++++++++++++++++++++----
 1 file changed, 36 insertions(+), 4 deletions(-)

diff --git a/soem/ethercatfoe.c b/soem/ethercatfoe.c
index 573cfc0c..7f264553 100644
--- a/soem/ethercatfoe.c
+++ b/soem/ethercatfoe.c
@@ -309,13 +309,45 @@ int ecx_FOEwrite(ecx_contextt *context, uint16 slave, char *filename, uint32 pas
                      /* otherwise ignore */
                      if (sendpacket)
                      {
-                        if (!psize)
-                        {
-                           dofinalzero = TRUE;
-                        }
                         psize += segmentdata;
                         p = (uint8 *)p - segmentdata;
                         --sendpacket;
+                        tsize = psize;
+                        if (tsize > maxdata)
+                        {
+                           tsize = maxdata;
+                        }
+                        if(tsize || dofinalzero)
+                        {
+                           worktodo = TRUE;
+                           dofinalzero = FALSE;
+                           segmentdata = tsize;
+                           psize -= segmentdata;
+                           /* if last packet was full size, add a zero size packet as final */
+                           /* EOF is defined as packetsize < full packetsize */
+                           if (!psize && (segmentdata == maxdata))
+                           {
+                             dofinalzero = TRUE;
+                           }
+                           FOEp->MbxHeader.length = htoes(0x0006 + segmentdata);
+                           FOEp->MbxHeader.address = htoes(0x0000);
+                           FOEp->MbxHeader.priority = 0x00;
+                           /* get new mailbox count value */
+                           cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
+                           context->slavelist[slave].mbx_cnt = cnt;
+                           FOEp->MbxHeader.mbxtype = ECT_MBXT_FOE + (cnt << 4); /* FoE */
+                           FOEp->OpCode = ECT_FOE_DATA;
+                           sendpacket++;
+                           FOEp->PacketNumber = htoel(sendpacket);
+                           memcpy(&FOEp->Data[0], p, segmentdata);
+                           p = (uint8 *)p + segmentdata;
+                           /* send FoE data to slave */
+                           wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
+                           if (wkc <= 0)
+                           {
+                              worktodo = FALSE;
+                           }
+                        }
                      }
                      break;
                   }