libs/SOEM
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
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.
*/
#include <vxWorks.h>
#include <ctype.h>
#include <string.h>
#include <stdio.h>
#include <muxLib.h>
#include <ipProto.h>
#include <vxWorks.h>
#include <wvLib.h>
#include "oshw.h"
@ -146,7 +148,7 @@ int ecx_setupnic(ecx_portt *port, const char *ifname, int secondary)
pPktDev->overrun_count = 0;
/* 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
* We assume form gei1, fei0...
@ -172,13 +174,22 @@ int ecx_setupnic(ecx_portt *port, const char *ifname, int secondary)
* as user reference
*/
/* 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)
{
/* fail */
NIC_LOGMSG("ecx_setupnic: muxBind init for gei: %d failed\n", unit_no, 2, 3, 4, 5, 6);
goto exit;
/* fail */
NIC_LOGMSG("ecx_setupnic: muxBind init for gei: %d failed\n",
unit_no, 2, 3, 4, 5, 6);
goto exit;
}
/* 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)
{
/* fail */
NIC_LOGMSG("error_hook: endFindByName failed, device gei: %d not found\n",
unit_no, 2, 3, 4, 5, 6);
goto exit;
}
/* fail */
NIC_LOGMSG("error_hook: endFindByName failed, device gei: %d not found\n",
unit_no, 2, 3, 4, 5, 6);
goto exit;
}
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 */
for (i = 0; i < EC_MAXBUF; i++)
{
port->msgQId[i] = msgQCreate(1, sizeof(M_BLK_ID), MSG_Q_FIFO);
if (port->msgQId[i] == MSG_Q_ID_NULL)
{
NIC_LOGMSG("ecx_setupnic: Failed to create MsgQ[%d]",
i, 2, 3, 4, 5, 6);
goto exit;
}
port->msgQId[i] = msgQCreate(1, sizeof(M_BLK_ID), MSG_Q_FIFO);
if (port->msgQId[i] == MSG_Q_ID_NULL)
{
NIC_LOGMSG("ecx_setupnic: Failed to create MsgQ[%d]",
i, 2, 3, 4, 5, 6);
goto exit;
}
}
ecx_clear_rxbufstat(&(port->rxbufstat[0]));
}
@ -271,20 +282,31 @@ int ecx_closenic(ecx_portt *port)
{
int i;
ETHERCAT_PKT_DEV * pPktDev;
M_BLK_ID trash_can;
pPktDev = &(port->pktDev);
for (i = 0; i < EC_MAXBUF; i++)
{
if (port->msgQId[i] != MSG_Q_ID_NULL)
{
msgQDelete(port->msgQId[i]);
if (port->msgQId[i] != MSG_Q_ID_NULL)
{
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)
{
muxUnbind(pPktDev->pCookie, MUX_PROTO_SNARF, mux_rx_callback);
muxUnbind(pPktDev->pCookie, MUX_PROTO_SNARF, mux_rx_callback);
}
/* 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 (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]);
}
}
@ -333,8 +365,6 @@ int ecx_getindex(ecx_portt *port)
{
int idx;
int cnt;
MSG_Q_ID msgQId;
M_BLK_ID trash_can;
semTake(port->sem_get_index, WAIT_FOREVER);
@ -356,27 +386,6 @@ int ecx_getindex(ecx_portt *port)
}
}
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;
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
*
* @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] len = bytes to send
* @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;
M_BLK_ID pPacket;
int rval = 0;
STATUS status = OK;
M_BLK_ID pPacket = NULL;
int rval = 0;
END_OBJ *endObj = (END_OBJ *)pPktDev->endObj;
MSG_Q_ID msgQId;
/* Allocate m_blk to send */
if ((pPacket = netTupleGet(pPktDev->endObj->pNetPool,
len,
M_DONTWAIT,
MT_DATA,
TRUE)) == NULL)
/* Clean up any abandoned frames and re-use the allocated buffer*/
msgQId = pPktDev->port->msgQId[idx];
if(msgQNumMsgs(msgQId) > 0)
{
NIC_LOGMSG("ec_outfram_send: Could not allocate MBLK memory!\n", 1, 2, 3, 4, 5, 6);
return ERROR;
pPktDev->abandoned_count++;
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.mFlags |= M_HEADER;
pPacket->mBlkHdr.mFlags |= M_PKTHDR;
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);
status = muxTkSend(pPktDev->pCookie, pPacket, NULL, htons(ETH_P_ECAT), NULL);
status = muxSend(endObj, pPacket);
if (status == OK)
{
@ -475,7 +508,7 @@ int ecx_outframe(ecx_portt *port, int idx, int stacknumber)
}
(*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]);
if (rval > 0)
{
@ -516,12 +549,12 @@ int ecx_outframe_red(ecx_portt *port, int idx)
ehp->sa1 = htons(secMAC[1]);
/* transmit over secondary interface */
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)
{
port->redport->rxbufstat[idx] = EC_BUF_EMPTY;
}
}
}
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;
ETHERCAT_PKT_DEV * pPktDev;
int length;
int bufstat;
/* check if it is an EtherCAT frame */
if (type == ETH_P_ECAT)
{
length = pMblk->mBlkHdr.mLen;
tempbuf = (ec_bufT *)pMblk->mBlkHdr.mData;
pPktDev = (ETHERCAT_PKT_DEV *)muxUserArg;
port = pPktDev->port;
length = pMblk->mBlkHdr.mLen;
tempbuf = (ec_bufT *)pMblk->mBlkHdr.mData;
pPktDev = (ETHERCAT_PKT_DEV *)muxUserArg;
port = pPktDev->port;
/* Get ethercat frame header */
ecp = (ec_comt*)&(*tempbuf)[ETH_HEADERSIZE];
idxf = ecp->index;
if (idxf >= EC_MAXBUF)
{
NIC_LOGMSG("mux_rx_callback: idx %d out of bounds\n", idxf,
2, 3, 4, 5, 6);
return ret;
}
/* Get ethercat frame header */
ecp = (ec_comt*)&(*tempbuf)[ETH_HEADERSIZE];
idxf = ecp->index;
if (idxf >= EC_MAXBUF)
{
NIC_LOGMSG("mux_rx_callback: idx %d out of bounds\n", idxf,
2, 3, 4, 5, 6);
return ret;
}
/* Check if it is the redundant port or not */
if (pPktDev->redundant == 1)
{
msgQId = port->redport->msgQId[idxf];
}
else
{
msgQId = port->msgQId[idxf];
}
/* Check if it is the redundant port or not */
if (pPktDev->redundant == 1)
{
bufstat = port->redport->rxbufstat[idxf];
msgQId = port->redport->msgQId[idxf];
}
else
{
bufstat = port->rxbufstat[idxf];
msgQId = port->msgQId[idxf];
}
if (length > 0)
{
/* Post the frame to the reqceive Q for the EtherCAT stack */
STATUS status;
status = msgQSend(msgQId, (char *)&pMblk, sizeof(M_BLK_ID),
NO_WAIT, MSG_PRI_NORMAL);
/* Check length and if someone expects the frame */
if (length > 0 && bufstat == EC_BUF_TX)
{
/* Post the frame to the receive Q for the EtherCAT stack */
STATUS status;
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)
{
NIC_WVEVENT(ECAT_RECV_OK, (char *)&length, sizeof(length));
ret = TRUE;
NIC_WVEVENT(ECAT_RECV_RETRY_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) == 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));
}
}
}
else
{
NIC_WVEVENT(ECAT_RECV_FAILED, (char *)&length, sizeof(length));
}
}
}
return ret;
@ -912,9 +953,9 @@ int ec_outframe_red(int 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)

4
oshw/vxworks/nicdrv.h
View File

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