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

VxWorks: Remove usage of muxTkSend, improve abandoned frame handling. bugfix for msgQRecv return value, add cleanup on NIC close

feature/soem_140
Andreas Karlsson 2019-05-24 13:58:41 +02:00
parent c220255604
commit 49810a5adf
2 changed files with 170 additions and 129 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

295
oshw/vxworks/nicdrv.c
View File

@ -31,10 +31,12 @@
* This layer if fully transparent for the higher layers. * This layer if fully transparent for the higher layers.
*/ */
#include <vxWorks.h>
#include <ctype.h> #include <ctype.h>
#include <string.h> #include <string.h>
#include <stdio.h>
#include <muxLib.h>
#include <ipProto.h> #include <ipProto.h>
#include <vxWorks.h>
#include <wvLib.h> #include <wvLib.h>
#include "oshw.h" #include "oshw.h"
@ -146,7 +148,7 @@ int ecx_setupnic(ecx_portt *port, const char *ifname, int secondary)
pPktDev->overrun_count = 0; pPktDev->overrun_count = 0;
/* Create multi-thread support semaphores */ /* Create multi-thread support semaphores */
port->sem_get_index = semBCreate(SEM_Q_FIFO, SEM_FULL); port->sem_get_index = semMCreate(SEM_Q_PRIORITY | SEM_INVERSION_SAFE);
/* Get the dev name and unit from ifname /* Get the dev name and unit from ifname
* We assume form gei1, fei0... * We assume form gei1, fei0...
@ -172,13 +174,22 @@ int ecx_setupnic(ecx_portt *port, const char *ifname, int secondary)
* as user reference * as user reference
*/ */
/* Bind to mux */ /* Bind to mux */
pPktDev->pCookie = muxBind(ifn, unit_no, mux_rx_callback, NULL, NULL, NULL, MUX_PROTO_SNARF, "ECAT SNARF", pPktDev); pPktDev->pCookie = muxBind(ifn,
unit_no,
mux_rx_callback,
NULL,
NULL,
NULL,
MUX_PROTO_SNARF,
"ECAT SNARF",
pPktDev);
if (pPktDev->pCookie == NULL) if (pPktDev->pCookie == NULL)
{ {
/* fail */ /* fail */
NIC_LOGMSG("ecx_setupnic: muxBind init for gei: %d failed\n", unit_no, 2, 3, 4, 5, 6); NIC_LOGMSG("ecx_setupnic: muxBind init for gei: %d failed\n",
goto exit; unit_no, 2, 3, 4, 5, 6);
goto exit;
} }
/* Get reference tp END obje */ /* Get reference tp END obje */
@ -186,11 +197,11 @@ int ecx_setupnic(ecx_portt *port, const char *ifname, int secondary)
if (port->pktDev.endObj == NULL) if (port->pktDev.endObj == NULL)
{ {
/* fail */ /* fail */
NIC_LOGMSG("error_hook: endFindByName failed, device gei: %d not found\n", NIC_LOGMSG("error_hook: endFindByName failed, device gei: %d not found\n",
unit_no, 2, 3, 4, 5, 6); unit_no, 2, 3, 4, 5, 6);
goto exit; goto exit;
} }
if (secondary) if (secondary)
{ {
@ -236,13 +247,13 @@ int ecx_setupnic(ecx_portt *port, const char *ifname, int secondary)
/* Create mailboxes for each potential EtherCAT frame index */ /* Create mailboxes for each potential EtherCAT frame index */
for (i = 0; i < EC_MAXBUF; i++) for (i = 0; i < EC_MAXBUF; i++)
{ {
port->msgQId[i] = msgQCreate(1, sizeof(M_BLK_ID), MSG_Q_FIFO); port->msgQId[i] = msgQCreate(1, sizeof(M_BLK_ID), MSG_Q_FIFO);
if (port->msgQId[i] == MSG_Q_ID_NULL) if (port->msgQId[i] == MSG_Q_ID_NULL)
{ {
NIC_LOGMSG("ecx_setupnic: Failed to create MsgQ[%d]", NIC_LOGMSG("ecx_setupnic: Failed to create MsgQ[%d]",
i, 2, 3, 4, 5, 6); i, 2, 3, 4, 5, 6);
goto exit; goto exit;
} }
} }
ecx_clear_rxbufstat(&(port->rxbufstat[0])); ecx_clear_rxbufstat(&(port->rxbufstat[0]));
} }
@ -271,20 +282,31 @@ int ecx_closenic(ecx_portt *port)
{ {
int i; int i;
ETHERCAT_PKT_DEV * pPktDev; ETHERCAT_PKT_DEV * pPktDev;
M_BLK_ID trash_can;
pPktDev = &(port->pktDev); pPktDev = &(port->pktDev);
for (i = 0; i < EC_MAXBUF; i++) for (i = 0; i < EC_MAXBUF; i++)
{ {
if (port->msgQId[i] != MSG_Q_ID_NULL) if (port->msgQId[i] != MSG_Q_ID_NULL)
{ {
msgQDelete(port->msgQId[i]); if (msgQReceive(port->msgQId[i],
(char *)&trash_can,
sizeof(M_BLK_ID),
NO_WAIT) != ERROR)
{
NIC_LOGMSG("ecx_closenic: idx %d MsgQ close\n", i,
2, 3, 4, 5, 6);
/* Free resources */
netMblkClChainFree(trash_can);
}
msgQDelete(port->msgQId[i]);
} }
} }
if (pPktDev->pCookie != NULL) if (pPktDev->pCookie != NULL)
{ {
muxUnbind(pPktDev->pCookie, MUX_PROTO_SNARF, mux_rx_callback); muxUnbind(pPktDev->pCookie, MUX_PROTO_SNARF, mux_rx_callback);
} }
/* Clean redundant resources if available*/ /* Clean redundant resources if available*/
@ -295,6 +317,16 @@ int ecx_closenic(ecx_portt *port)
{ {
if (port->redport->msgQId[i] != MSG_Q_ID_NULL) if (port->redport->msgQId[i] != MSG_Q_ID_NULL)
{ {
if (msgQReceive(port->redport->msgQId[i],
(char *)&trash_can,
sizeof(M_BLK_ID),
NO_WAIT) != ERROR)
{
NIC_LOGMSG("ecx_closenic: idx %d MsgQ close\n", i,
2, 3, 4, 5, 6);
/* Free resources */
netMblkClChainFree(trash_can);
}
msgQDelete(port->redport->msgQId[i]); msgQDelete(port->redport->msgQId[i]);
} }
} }
@ -333,8 +365,6 @@ int ecx_getindex(ecx_portt *port)
{ {
int idx; int idx;
int cnt; int cnt;
MSG_Q_ID msgQId;
M_BLK_ID trash_can;
semTake(port->sem_get_index, WAIT_FOREVER); semTake(port->sem_get_index, WAIT_FOREVER);
@ -356,27 +386,6 @@ int ecx_getindex(ecx_portt *port)
} }
} }
port->rxbufstat[idx] = EC_BUF_ALLOC; port->rxbufstat[idx] = EC_BUF_ALLOC;
/* Clean up any abandoned frames */
msgQId = port->msgQId[idx];
if (msgQReceive(msgQId, (char *)&trash_can, sizeof(M_BLK_ID), NO_WAIT) == OK)
{
/* Free resources */
netMblkClChainFree(trash_can);
}
if (port->redstate != ECT_RED_NONE)
{
port->redport->rxbufstat[idx] = EC_BUF_ALLOC;
/* Clean up any abandoned frames */
msgQId = port->redport->msgQId[idx];
if (msgQReceive(msgQId, (char *)&trash_can, sizeof(M_BLK_ID), NO_WAIT) == OK)
{
/* Free resources */
netMblkClChainFree(trash_can);
}
}
port->lastidx = idx; port->lastidx = idx;
semGive(port->sem_get_index); semGive(port->sem_get_index);
@ -398,35 +407,59 @@ void ecx_setbufstat(ecx_portt *port, int idx, int bufstat)
/** Low level transmit buffer over mux layer 2 driver /** Low level transmit buffer over mux layer 2 driver
* *
* @param[in] pPktDev = packet device to send buffer over * @param[in] pPktDev = packet device to send buffer over
* @param[in] idx = index in tx buffer array
* @param[in] buf = buff to send * @param[in] buf = buff to send
* @param[in] len = bytes to send * @param[in] len = bytes to send
* @return driver send result * @return driver send result
*/ */
static int ec_outfram_send(ETHERCAT_PKT_DEV * pPktDev, void * buf, int len) static int ec_outfram_send(ETHERCAT_PKT_DEV * pPktDev, int idx, void * buf, int len)
{ {
STATUS status = OK; STATUS status = OK;
M_BLK_ID pPacket; M_BLK_ID pPacket = NULL;
int rval = 0; int rval = 0;
END_OBJ *endObj = (END_OBJ *)pPktDev->endObj;
MSG_Q_ID msgQId;
/* Allocate m_blk to send */ /* Clean up any abandoned frames and re-use the allocated buffer*/
if ((pPacket = netTupleGet(pPktDev->endObj->pNetPool, msgQId = pPktDev->port->msgQId[idx];
len, if(msgQNumMsgs(msgQId) > 0)
M_DONTWAIT,
MT_DATA,
TRUE)) == NULL)
{ {
NIC_LOGMSG("ec_outfram_send: Could not allocate MBLK memory!\n", 1, 2, 3, 4, 5, 6); pPktDev->abandoned_count++;
return ERROR; NIC_LOGMSG("ec_outfram_send: idx %d MsgQ abandoned\n", idx,
2, 3, 4, 5, 6);
if (msgQReceive(msgQId,
(char *)&pPacket,
sizeof(M_BLK_ID),
NO_WAIT) == ERROR)
{
pPacket = NULL;
NIC_LOGMSG("ec_outfram_send: idx %d MsgQ mBlk handled by receiver\n", idx,
2, 3, 4, 5, 6);
}
}
if (pPacket == NULL)
{
/* Allocate m_blk to send */
if ((pPacket = netTupleGet(endObj->pNetPool,
len,
M_DONTWAIT,
MT_DATA,
TRUE)) == NULL)
{
NIC_LOGMSG("ec_outfram_send: Could not allocate MBLK memory!\n", 1, 2, 3, 4, 5, 6);
return ERROR;
}
} }
pPacket->mBlkHdr.mLen = len; pPacket->mBlkHdr.mLen = len;
pPacket->mBlkHdr.mFlags |= M_HEADER; pPacket->mBlkHdr.mFlags |= M_PKTHDR;
pPacket->mBlkHdr.mData = pPacket->pClBlk->clNode.pClBuf; pPacket->mBlkHdr.mData = pPacket->pClBlk->clNode.pClBuf;
pPacket->mBlkPktHdr.len = len; pPacket->mBlkPktHdr.len = pPacket->m_len;
netMblkFromBufCopy(pPacket, buf, 0, pPacket->mBlkHdr.mLen, M_DONTWAIT, NULL); netMblkFromBufCopy(pPacket, buf, 0, pPacket->mBlkHdr.mLen, M_DONTWAIT, NULL);
status = muxTkSend(pPktDev->pCookie, pPacket, NULL, htons(ETH_P_ECAT), NULL); status = muxSend(endObj, pPacket);
if (status == OK) if (status == OK)
{ {
@ -475,7 +508,7 @@ int ecx_outframe(ecx_portt *port, int idx, int stacknumber)
} }
(*stack->rxbufstat)[idx] = EC_BUF_TX; (*stack->rxbufstat)[idx] = EC_BUF_TX;
rval = ec_outfram_send(pPktDev, (char*)(*stack->txbuf)[idx], rval = ec_outfram_send(pPktDev, idx, (char*)(*stack->txbuf)[idx],
(*stack->txbuflength)[idx]); (*stack->txbuflength)[idx]);
if (rval > 0) if (rval > 0)
{ {
@ -516,12 +549,12 @@ int ecx_outframe_red(ecx_portt *port, int idx)
ehp->sa1 = htons(secMAC[1]); ehp->sa1 = htons(secMAC[1]);
/* transmit over secondary interface */ /* transmit over secondary interface */
port->redport->rxbufstat[idx] = EC_BUF_TX; port->redport->rxbufstat[idx] = EC_BUF_TX;
rval = ec_outfram_send(&(port->redport->pktDev), &(port->txbuf2), port->txbuflength2); rval = ec_outfram_send(&(port->redport->pktDev), idx, &(port->txbuf2), port->txbuflength2);
if (rval <= 0) if (rval <= 0)
{ {
port->redport->rxbufstat[idx] = EC_BUF_EMPTY; port->redport->rxbufstat[idx] = EC_BUF_EMPTY;
} }
} }
return rval; return rval;
} }
@ -545,77 +578,85 @@ static int mux_rx_callback(void* pCookie, long type, M_BLK_ID pMblk, LL_HDR_INFO
MSG_Q_ID msgQId; MSG_Q_ID msgQId;
ETHERCAT_PKT_DEV * pPktDev; ETHERCAT_PKT_DEV * pPktDev;
int length; int length;
int bufstat;
/* check if it is an EtherCAT frame */ /* check if it is an EtherCAT frame */
if (type == ETH_P_ECAT) if (type == ETH_P_ECAT)
{ {
length = pMblk->mBlkHdr.mLen; length = pMblk->mBlkHdr.mLen;
tempbuf = (ec_bufT *)pMblk->mBlkHdr.mData; tempbuf = (ec_bufT *)pMblk->mBlkHdr.mData;
pPktDev = (ETHERCAT_PKT_DEV *)muxUserArg; pPktDev = (ETHERCAT_PKT_DEV *)muxUserArg;
port = pPktDev->port; port = pPktDev->port;
/* Get ethercat frame header */ /* Get ethercat frame header */
ecp = (ec_comt*)&(*tempbuf)[ETH_HEADERSIZE]; ecp = (ec_comt*)&(*tempbuf)[ETH_HEADERSIZE];
idxf = ecp->index; idxf = ecp->index;
if (idxf >= EC_MAXBUF) if (idxf >= EC_MAXBUF)
{ {
NIC_LOGMSG("mux_rx_callback: idx %d out of bounds\n", idxf, NIC_LOGMSG("mux_rx_callback: idx %d out of bounds\n", idxf,
2, 3, 4, 5, 6); 2, 3, 4, 5, 6);
return ret; return ret;
} }
/* Check if it is the redundant port or not */ /* Check if it is the redundant port or not */
if (pPktDev->redundant == 1) if (pPktDev->redundant == 1)
{ {
msgQId = port->redport->msgQId[idxf]; bufstat = port->redport->rxbufstat[idxf];
} msgQId = port->redport->msgQId[idxf];
else }
{ else
msgQId = port->msgQId[idxf]; {
} bufstat = port->rxbufstat[idxf];
msgQId = port->msgQId[idxf];
}
if (length > 0) /* Check length and if someone expects the frame */
{ if (length > 0 && bufstat == EC_BUF_TX)
/* Post the frame to the reqceive Q for the EtherCAT stack */ {
STATUS status; /* Post the frame to the receive Q for the EtherCAT stack */
status = msgQSend(msgQId, (char *)&pMblk, sizeof(M_BLK_ID), STATUS status;
NO_WAIT, MSG_PRI_NORMAL); status = msgQSend(msgQId, (char *)&pMblk, sizeof(M_BLK_ID),
NO_WAIT, MSG_PRI_NORMAL);
if (status == OK)
{
NIC_WVEVENT(ECAT_RECV_OK, (char *)&length, sizeof(length));
ret = TRUE;
}
else if ((status == ERROR) && (errno == S_objLib_OBJ_UNAVAILABLE))
{
/* Try to empty the MSGQ since we for some strange reason
* already have a frame in the MsqQ,
* is it due to timeout when receiving?
* We want the latest received frame in the buffer
*/
port->pktDev.overrun_count++;
NIC_LOGMSG("mux_rx_callback: idx %d MsgQ overrun\n", idxf,
2, 3, 4, 5, 6);
M_BLK_ID trash_can;
if (msgQReceive(msgQId,
(char *)&trash_can,
sizeof(M_BLK_ID),
NO_WAIT) != ERROR)
{
/* Free resources */
netMblkClChainFree(trash_can);
}
status = msgQSend(msgQId,
(char *)&pMblk,
sizeof(M_BLK_ID),
NO_WAIT,
MSG_PRI_NORMAL);
if (status == OK) if (status == OK)
{ {
NIC_WVEVENT(ECAT_RECV_OK, (char *)&length, sizeof(length)); NIC_WVEVENT(ECAT_RECV_RETRY_OK, (char *)&length, sizeof(length));
ret = TRUE; ret = TRUE;
} }
else if ((status == ERROR) && (errno == S_objLib_OBJ_UNAVAILABLE)) }
{ else
/* Try to empty the MSGQ since we for some strange reason {
* already have a frame in the MsqQ, NIC_WVEVENT(ECAT_RECV_FAILED, (char *)&length, sizeof(length));
* is it due to timeout when receiving? }
* We want the latest received frame in the buffer }
*/
port->pktDev.overrun_count++;
NIC_LOGMSG("mux_rx_callback: idx %d MsgQ overrun\n", idxf,
2, 3, 4, 5, 6);
M_BLK_ID trash_can;
if (msgQReceive(msgQId, (char *)&trash_can,
sizeof(M_BLK_ID), NO_WAIT) == OK)
{
/* Free resources */
netMblkClChainFree(trash_can);
}
status = msgQSend(msgQId, (char *)&pMblk, sizeof(M_BLK_ID),
NO_WAIT, MSG_PRI_NORMAL);
if (status == OK)
{
NIC_WVEVENT(ECAT_RECV_RETRY_OK, (char *)&length, sizeof(length));
ret = TRUE;
}
}
else
{
NIC_WVEVENT(ECAT_RECV_FAILED, (char *)&length, sizeof(length));
}
}
} }
return ret; return ret;
@ -912,9 +953,9 @@ int ec_outframe_red(int idx)
return ecx_outframe_red(&ecx_port, idx); return ecx_outframe_red(&ecx_port, idx);
} }
int ec_inframe(int idx, int stacknumber) int ec_inframe(int idx, int stacknumber, int timeout)
{ {
return ecx_inframe(&ecx_port, idx, stacknumber); return ecx_inframe(&ecx_port, idx, stacknumber, timeout);
} }
int ec_waitinframe(int idx, int timeout) int ec_waitinframe(int idx, int timeout)

4
oshw/vxworks/nicdrv.h
View File

@ -17,18 +17,18 @@ extern "C"
#endif #endif
#include <vxWorks.h> #include <vxWorks.h>
#include <muxLib.h>
/** structure to connect EtherCAT stack and VxWorks device */ /** structure to connect EtherCAT stack and VxWorks device */
typedef struct ETHERCAT_PKT_DEV typedef struct ETHERCAT_PKT_DEV
{ {
struct ecx_port *port; struct ecx_port *port;
void *pCookie; void *pCookie;
END_OBJ *endObj; void *endObj;
UINT32 redundant; UINT32 redundant;
UINT32 tx_count; UINT32 tx_count;
UINT32 rx_count; UINT32 rx_count;
UINT32 overrun_count; UINT32 overrun_count;
UINT32 abandoned_count;
}ETHERCAT_PKT_DEV; }ETHERCAT_PKT_DEV;
/** pointer structure to Tx and Rx stacks */ /** pointer structure to Tx and Rx stacks */