FoE: resend packet on BUSY answer while writing

Change-Id: Ifbcfd90628fd92297434ecc3dba4a1299cdcd7a9

diff --git a/soem/ethercatfoe.c b/soem/ethercatfoe.c
index 4e1976df4b140b808936d415070f14d6e8bf132c..a832193f5dd78b4d0a9e413e3747652f23c1af76 100644 (file)
--- a/soem/ethercatfoe.c
+++ b/soem/ethercatfoe.c
@@ -323,13 +323,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((uint16)(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 + MBX_HDR_SET_CNT(cnt); /* 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;
                   }