Rework FoE busy handling

See: - OpenEtherCATsociety#627 - OpenEtherCATsociety#623
synapticon · Jul 29, 2024 · 2dfe72c · 2dfe72c
1 parent ed048f4
commit 2dfe72c
Show file tree

Hide file tree

Showing 2 changed files with 42 additions and 3 deletions.
diff --git a/soem/ethercatbase.c b/soem/ethercatbase.c
@@ -325,7 +325,15 @@ int ecx_FPRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data,
    wkc = ecx_srconfirm(port, idx, timeout);
    if (wkc > 0)
    {
-      memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length);
+      // https://github.com/OpenEtherCATsociety/SOEM/pull/623/commits/ebbe5da29d969ed0847a1c312d20e8ab345e8859
+      if (length <= EC_MAXMBX)
+      {
+         memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length);
+      }
+      else
+      {
+         wkc = EC_ERROR;
+      }
    }
    ecx_setbufstat(port, idx, EC_BUF_EMPTY);
 

diff --git a/soem/ethercatfoe.c b/soem/ethercatfoe.c
@@ -19,7 +19,8 @@
 #include "ethercatmain.h"
 #include "ethercatfoe.h"
 
-#define EC_MAXFOEDATA 512
+// https://github.com/OpenEtherCATsociety/SOEM/pull/623/commits/c499b93f39bf3508abf344afbea835e4af55e4d1
+#define EC_MAXFOEDATA EC_MAXMBX
 
 /** FOE structure.
  * Used for Read, Write, Data, Ack and Error mailbox packets.
@@ -331,7 +332,37 @@ int ecx_FOEwrite(ecx_contextt *context, uint16 slave, char *filename, uint32 pas
                         }
                         psize += segmentdata;
                         p = (uint8 *)p - segmentdata;
-                        --sendpacket;
+                        // https://github.com/OpenEtherCATsociety/SOEM/pull/627
+                        sendpacket--;
+                        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;
+                        }
+                        // https://github.com/OpenEtherCATsociety/SOEM/pull/627
                      }
                      break;
                   }