ln-dotnet
/
SOEM
3
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Fix tx/rx processdata functions

pull/343/merge
Arthur Ketels 2019-11-12 17:03:48 +01:00
parent 2a8c07c32b
commit 9ec8635943
2 changed files with 33 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
soem/ethercatmain.c
View File

@ -1641,14 +1641,16 @@ uint32 ecx_readeeprom2(ecx_contextt *context, uint16 slave, int timeout)
* @param[in] idx = Used datagram index. * @param[in] idx = Used datagram index.
* @param[in] data = Pointer to process data segment. * @param[in] data = Pointer to process data segment.
* @param[in] length = Length of data segment in bytes. * @param[in] length = Length of data segment in bytes.
* @param[in] dcoffset = Offset position of DC frame.
*/ */
static void ecx_pushindex(ecx_contextt *context, uint8 idx, void *data, uint16 length) static void ecx_pushindex(ecx_contextt *context, uint8 idx, void *data, uint16 length, uint16 DCO)
{ {
if(context->idxstack->pushed < EC_MAXBUF) if(context->idxstack->pushed < EC_MAXBUF)
{ {
context->idxstack->idx[context->idxstack->pushed] = idx; context->idxstack->idx[context->idxstack->pushed] = idx;
context->idxstack->data[context->idxstack->pushed] = data; context->idxstack->data[context->idxstack->pushed] = data;
context->idxstack->length[context->idxstack->pushed] = length; context->idxstack->length[context->idxstack->pushed] = length;
context->idxstack->dcoffset[context->idxstack->pulled] = DCO;
context->idxstack->pushed++; context->idxstack->pushed++;
} }
} }
@ -1705,6 +1707,7 @@ static int ecx_main_send_processdata(ecx_contextt *context, uint8 group, boolean
boolean first=FALSE; boolean first=FALSE;
uint16 currentsegment = 0; uint16 currentsegment = 0;
uint32 iomapinputoffset; uint32 iomapinputoffset;
uint16 DCO;
wkc = 0; wkc = 0;
if(context->grouplist[group].hasdc) if(context->grouplist[group].hasdc)
@ -1757,12 +1760,12 @@ static int ecx_main_send_processdata(ecx_contextt *context, uint8 group, boolean
idx = ecx_getindex(context->port); idx = ecx_getindex(context->port);
w1 = LO_WORD(LogAdr); w1 = LO_WORD(LogAdr);
w2 = HI_WORD(LogAdr); w2 = HI_WORD(LogAdr);
DCO = 0;
ecx_setupdatagram(context->port, &(context->port->txbuf[idx]), EC_CMD_LRD, idx, w1, w2, sublength, data); ecx_setupdatagram(context->port, &(context->port->txbuf[idx]), EC_CMD_LRD, idx, w1, w2, sublength, data);
if(first) if(first)
{ {
context->DCl = sublength;
/* FPRMW in second datagram */ /* FPRMW in second datagram */
context->DCtO = ecx_adddatagram(context->port, &(context->port->txbuf[idx]), EC_CMD_FRMW, idx, FALSE, DCO = ecx_adddatagram(context->port, &(context->port->txbuf[idx]), EC_CMD_FRMW, idx, FALSE,
context->slavelist[context->grouplist[group].DCnext].configadr, context->slavelist[context->grouplist[group].DCnext].configadr,
ECT_REG_DCSYSTIME, sizeof(int64), context->DCtime); ECT_REG_DCSYSTIME, sizeof(int64), context->DCtime);
first = FALSE; first = FALSE;
@ -1770,7 +1773,7 @@ static int ecx_main_send_processdata(ecx_contextt *context, uint8 group, boolean
/* send frame */ /* send frame */
ecx_outframe_red(context->port, idx); ecx_outframe_red(context->port, idx);
/* push index and data pointer on stack */ /* push index and data pointer on stack */
ecx_pushindex(context, idx, data, sublength); ecx_pushindex(context, idx, data, sublength, DCO);
length -= sublength; length -= sublength;
LogAdr += sublength; LogAdr += sublength;
data += sublength; data += sublength;
@ -1795,12 +1798,12 @@ static int ecx_main_send_processdata(ecx_contextt *context, uint8 group, boolean
idx = ecx_getindex(context->port); idx = ecx_getindex(context->port);
w1 = LO_WORD(LogAdr); w1 = LO_WORD(LogAdr);
w2 = HI_WORD(LogAdr); w2 = HI_WORD(LogAdr);
DCO = 0;
ecx_setupdatagram(context->port, &(context->port->txbuf[idx]), EC_CMD_LWR, idx, w1, w2, sublength, data); ecx_setupdatagram(context->port, &(context->port->txbuf[idx]), EC_CMD_LWR, idx, w1, w2, sublength, data);
if(first) if(first)
{ {
context->DCl = sublength;
/* FPRMW in second datagram */ /* FPRMW in second datagram */
context->DCtO = ecx_adddatagram(context->port, &(context->port->txbuf[idx]), EC_CMD_FRMW, idx, FALSE, DCO = ecx_adddatagram(context->port, &(context->port->txbuf[idx]), EC_CMD_FRMW, idx, FALSE,
context->slavelist[context->grouplist[group].DCnext].configadr, context->slavelist[context->grouplist[group].DCnext].configadr,
ECT_REG_DCSYSTIME, sizeof(int64), context->DCtime); ECT_REG_DCSYSTIME, sizeof(int64), context->DCtime);
first = FALSE; first = FALSE;
@ -1808,7 +1811,7 @@ static int ecx_main_send_processdata(ecx_contextt *context, uint8 group, boolean
/* send frame */ /* send frame */
ecx_outframe_red(context->port, idx); ecx_outframe_red(context->port, idx);
/* push index and data pointer on stack */ /* push index and data pointer on stack */
ecx_pushindex(context, idx, data, sublength); ecx_pushindex(context, idx, data, sublength, DCO);
length -= sublength; length -= sublength;
LogAdr += sublength; LogAdr += sublength;
data += sublength; data += sublength;
@ -1836,12 +1839,12 @@ static int ecx_main_send_processdata(ecx_contextt *context, uint8 group, boolean
idx = ecx_getindex(context->port); idx = ecx_getindex(context->port);
w1 = LO_WORD(LogAdr); w1 = LO_WORD(LogAdr);
w2 = HI_WORD(LogAdr); w2 = HI_WORD(LogAdr);
DCO = 0;
ecx_setupdatagram(context->port, &(context->port->txbuf[idx]), EC_CMD_LRW, idx, w1, w2, sublength, data); ecx_setupdatagram(context->port, &(context->port->txbuf[idx]), EC_CMD_LRW, idx, w1, w2, sublength, data);
if(first) if(first)
{ {
context->DCl = sublength;
/* FPRMW in second datagram */ /* FPRMW in second datagram */
context->DCtO = ecx_adddatagram(context->port, &(context->port->txbuf[idx]), EC_CMD_FRMW, idx, FALSE, DCO = ecx_adddatagram(context->port, &(context->port->txbuf[idx]), EC_CMD_FRMW, idx, FALSE,
context->slavelist[context->grouplist[group].DCnext].configadr, context->slavelist[context->grouplist[group].DCnext].configadr,
ECT_REG_DCSYSTIME, sizeof(int64), context->DCtime); ECT_REG_DCSYSTIME, sizeof(int64), context->DCtime);
first = FALSE; first = FALSE;
@ -1853,7 +1856,7 @@ static int ecx_main_send_processdata(ecx_contextt *context, uint8 group, boolean
* in the IOmap if we use an overlapping IOmap. If a regular IOmap * in the IOmap if we use an overlapping IOmap. If a regular IOmap
* is used it should always be 0. * is used it should always be 0.
*/ */
ecx_pushindex(context, idx, (data + iomapinputoffset), sublength); ecx_pushindex(context, idx, (data + iomapinputoffset), sublength, DCO);
length -= sublength; length -= sublength;
LogAdr += sublength; LogAdr += sublength;
data += sublength; data += sublength;
@ -1914,51 +1917,51 @@ int ecx_receive_processdata_group(ecx_contextt *context, uint8 group, int timeou
uint16 le_wkc = 0; uint16 le_wkc = 0;
int valid_wkc = 0; int valid_wkc = 0;
int64 le_DCtime; int64 le_DCtime;
boolean first = FALSE; ec_idxstackT *idxstack;
ec_bufT *rxbuf;
if(context->grouplist[group].hasdc) /* just to prevent compiler warning for unused group */
{ wkc2 = group;
first = TRUE;
} idxstack = context->idxstack;
rxbuf = context->port->rxbuf;
/* get first index */ /* get first index */
pos = ecx_pullindex(context); pos = ecx_pullindex(context);
/* read the same number of frames as send */ /* read the same number of frames as send */
while (pos >= 0) while (pos >= 0)
{ {
idx = context->idxstack->idx[pos]; idx = idxstack->idx[pos];
wkc2 = ecx_waitinframe(context->port, context->idxstack->idx[pos], timeout); wkc2 = ecx_waitinframe(context->port, idx, timeout);
/* check if there is input data in frame */ /* check if there is input data in frame */
if (wkc2 > EC_NOFRAME) if (wkc2 > EC_NOFRAME)
{ {
if((context->port->rxbuf[idx][EC_CMDOFFSET]==EC_CMD_LRD) || (context->port->rxbuf[idx][EC_CMDOFFSET]==EC_CMD_LRW)) if((rxbuf[idx][EC_CMDOFFSET]==EC_CMD_LRD) || (rxbuf[idx][EC_CMDOFFSET]==EC_CMD_LRW))
{ {
if(first) if(idxstack->dcoffset[pos] > 0)
{ {
memcpy(context->idxstack->data[pos], &(context->port->rxbuf[idx][EC_HEADERSIZE]), context->DCl); memcpy(idxstack->data[pos], &(rxbuf[idx][EC_HEADERSIZE]), idxstack->length[pos]);
memcpy(&le_wkc, &(context->port->rxbuf[idx][EC_HEADERSIZE + context->DCl]), EC_WKCSIZE); memcpy(&le_wkc, &(rxbuf[idx][EC_HEADERSIZE + idxstack->length[pos]]), EC_WKCSIZE);
wkc = etohs(le_wkc); wkc = etohs(le_wkc);
memcpy(&le_DCtime, &(context->port->rxbuf[idx][context->DCtO]), sizeof(le_DCtime)); memcpy(&le_DCtime, &(rxbuf[idx][idxstack->dcoffset[pos]]), sizeof(le_DCtime));
*(context->DCtime) = etohll(le_DCtime); *(context->DCtime) = etohll(le_DCtime);
first = FALSE;
} }
else else
{ {
/* copy input data back to process data buffer */ /* copy input data back to process data buffer */
memcpy(context->idxstack->data[pos], &(context->port->rxbuf[idx][EC_HEADERSIZE]), context->idxstack->length[pos]); memcpy(idxstack->data[pos], &(rxbuf[idx][EC_HEADERSIZE]), idxstack->length[pos]);
wkc += wkc2; wkc += wkc2;
} }
valid_wkc = 1; valid_wkc = 1;
} }
else if(context->port->rxbuf[idx][EC_CMDOFFSET]==EC_CMD_LWR) else if(rxbuf[idx][EC_CMDOFFSET]==EC_CMD_LWR)
{ {
if(first) if(idxstack->dcoffset[pos] > 0)
{ {
memcpy(&le_wkc, &(context->port->rxbuf[idx][EC_HEADERSIZE + context->DCl]), EC_WKCSIZE); memcpy(&le_wkc, &(rxbuf[idx][EC_HEADERSIZE + idxstack->length[pos]]), EC_WKCSIZE);
/* output WKC counts 2 times when using LRW, emulate the same for LWR */ /* output WKC counts 2 times when using LRW, emulate the same for LWR */
wkc = etohs(le_wkc) * 2; wkc = etohs(le_wkc) * 2;
memcpy(&le_DCtime, &(context->port->rxbuf[idx][context->DCtO]), sizeof(le_DCtime)); memcpy(&le_DCtime, &(rxbuf[idx][idxstack->dcoffset[pos]]), sizeof(le_DCtime));
*(context->DCtime) = etohll(le_DCtime); *(context->DCtime) = etohll(le_DCtime);
first = FALSE;
} }
else else
{ {

1
soem/ethercatmain.h
View File

@ -340,6 +340,7 @@ typedef struct ec_idxstack
uint8 idx[EC_MAXBUF]; uint8 idx[EC_MAXBUF];
void *data[EC_MAXBUF]; void *data[EC_MAXBUF];
uint16 length[EC_MAXBUF]; uint16 length[EC_MAXBUF];
uint16 dcoffset[EC_MAXBUF];
} ec_idxstackT; } ec_idxstackT;
/** ringbuf for error storage */ /** ringbuf for error storage */