From 49810a5adf1a2e578d1824544ee2cf14e9227d8a Mon Sep 17 00:00:00 2001 From: Andreas Karlsson Date: Fri, 24 May 2019 13:58:41 +0200 Subject: [PATCH] VxWorks: Remove usage of muxTkSend, improve abandoned frame handling. bugfix for msgQRecv return value, add cleanup on NIC close --- oshw/vxworks/nicdrv.c | 295 ++++++++++++++++++++++++------------------ oshw/vxworks/nicdrv.h | 4 +- 2 files changed, 170 insertions(+), 129 deletions(-) diff --git a/oshw/vxworks/nicdrv.c b/oshw/vxworks/nicdrv.c index e749023..fb4392e 100644 --- a/oshw/vxworks/nicdrv.c +++ b/oshw/vxworks/nicdrv.c @@ -31,10 +31,12 @@ * This layer if fully transparent for the higher layers. */ +#include #include #include +#include +#include #include -#include #include #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) diff --git a/oshw/vxworks/nicdrv.h b/oshw/vxworks/nicdrv.h index 277d055..e009399 100644 --- a/oshw/vxworks/nicdrv.h +++ b/oshw/vxworks/nicdrv.h @@ -17,18 +17,18 @@ extern "C" #endif #include -#include /** 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 */