/* qbv_dkm.c - demo QBV lib routine */ /* * Copyright (c) 2017 Wind River Systems, Inc. * * The right to copy, distribute, modify, or otherwise make use * of this software may be licensed only pursuant to the terms * of an applicable Wind River license agreement. */ /* modification history -------------------- 09feb17,xxy Fix float point unavailable exception (V7NET-1263) 26Oct16 xxy Init version. */ #include #include #include #include #include #include #include #define QBV_BIG_ENDIAN #define INCLUDE_USR_IEEE_802_1_QBV_DEMO #ifdef INCLUDE_USR_IEEE_802_1_QBV_DEMO #define QBV_TEST_TX_BUFFER_LEN 1500 #define QBV_TEST_RX_BUFFER_LEN 1000 #define IEEE8021_VLAN_TYPE 0x8100 #define QBV_DEMO_TYPE 0x0800 #define IEEE8021_ETH_TYPE_POS 12 #define IEEE8021_ETH_HEAD_LEN 14 #define IEEE8021_QBV_MAX_LEN 1500 #define QBV_CYCLE_BINDED 0x55aa #define MAX_SLOT_NUMBER 8 #define SLOT_TO_INDEX_OFF 1 #ifdef QBV_BIG_ENDIAN #define QBV_SWAP16(X) ((X & 0X00FF)<<8 | (X & 0XFF00)>>8) #else #define QBV_SWAP16(X) X #endif typedef struct qbvslot { int slotId; int slotStart; int slotLen; UINT16 vlanId; UINT16 frameLen; char destMacAddr[6]; } QBVSLOT; typedef struct qbvCycle { int bindFlag; int cycleId; void *devCookie; int cycleLen; int cycleStart; QBVSLOT slot[MAX_SLOT_NUMBER] ; } QBVCYCLE; struct packet_stamp { unsigned int sequence; unsigned int cycleLen; unsigned int tv_sec; unsigned int tv_nsec; } qbvstamp; LOCAL UINT32 QbvSendDataDemoSequence = 0; LOCAL TASK_ID QbvDemo2TaskId = 0; LOCAL int GeiEndQbvDemo2TaskOff = 0; LOCAL QBVCYCLE qbvTxConfigInfo; LOCAL UINT32 currentSlotIndex = 0; /****************************************************************************** * * qbvMacStrToArray - the routine is to convert mac string to hex * * * RETURNS: OK or ERROR * * ERRNO: N/A */ LOCAL int qbvMacStrToArray( char *src, char *des ) { char c; char temp; char temp2; int i; if ( (src == NULL) && (strlen(src) < 12 ) ) { printf( "input Error,systax:aa-bb-cc-dd-ee-ff \n" ); return ERROR; } for ( i = 0; i < 6; i++ ) { temp = 0; temp2 = 0; c = *src; if ( c >= 'A' && c <= 'F' ) { temp = ( c - 'A' ) + 10; } else if ( c >= 'a' && c <= 'f' ) { temp = ( c - 'a' ) + 10; } else if ( c >= '0' && c <= '9' ) { temp = ( c - '0' ) ; } else { return ERROR; } src++; c = *src; if ( c >= 'A' && c <= 'F' ) { temp2 = ( c - 'A' ) + 10; } else if ( c >= 'a' && c <= 'f' ) { temp2 = ( c - 'a' ) + 10; } else if ( c >= '0' && c <= '9' ) { temp2 = ( c - '0' ) ; } else { return ERROR; } temp = (temp << 4); temp += temp2; src++; src++; *(des + i) = temp; } return OK; } /****************************************************************************** * * qbvTestDataSend - the routine of qbv test data send * * Parameters: * cycleID: ID of cycle * trafficType : 0 Time-Aware(scheduled) * 1 Credit-based A(none scheduled) * 2 Credit-based B(none scheduled) * 3 Prioritized(none scheduled) * 4 Best-Effort(none scheduled) * cyclePlus : cycle offset from the current * timeSlotId : time slot ID in cycle * extraDelay : extra delay time added to the time slot (unit ns) * pbuf : buffer to be send * len : length of buffer * vlanId : vlan ID * destMacAddr : destination MAC address * * RETURNS: OK or ERROR * * ERRNO: N/A */ LOCAL STATUS qbvTestDataSend ( int cycleID, int trafficType, UINT32 cyclePlus, UINT32 timeSlotId, UINT32 extraDelay, char *pbuf, UINT32 len, UINT16 vlanId, char *destMacAddr ) { END_OBJ *pEnd; UINT32 qbvlen; char *tbuf; char geiMacAddr[6]; int status; struct packet_stamp *pRec; struct timespec tsCycle; int cLen; UINT64 runningCounts; if (len > IEEE8021_QBV_MAX_LEN ) { return ERROR; } qbvlen = len + IEEE8021_ETH_HEAD_LEN + 4 ; tbuf = malloc(qbvlen); if (tbuf == NULL ) { return ERROR; } pEnd = (END_OBJ *)qbvTxConfigInfo.devCookie; status = muxIoctl(pEnd, EIOCGADDR, (void *)&geiMacAddr); if (NULL == pEnd) { free(tbuf); return ERROR; } status = muxIoctl(pEnd, EIOCGADDR, (void *)&geiMacAddr); if (status != OK) { free(tbuf); return ERROR; } (void) bcopy (geiMacAddr, &tbuf[6], 6); (void) bcopy (destMacAddr, &tbuf[0], 6); *(UINT16 *)&tbuf[IEEE8021_ETH_TYPE_POS] = QBV_SWAP16(IEEE8021_VLAN_TYPE); *(UINT16 *)&tbuf[IEEE8021_ETH_TYPE_POS + 2] = QBV_SWAP16(vlanId); *(UINT16 *)&tbuf[IEEE8021_ETH_TYPE_POS + 4] = QBV_SWAP16(QBV_DEMO_TYPE); (void) bcopy (pbuf, &tbuf[IEEE8021_ETH_HEAD_LEN + 4], len); /*get the cycle bundary and record into demo packet*/ status = muxIoctl(pEnd, EIOCG1588SYSTIM, (void *)&tsCycle); if (status != OK) { free(tbuf); return ERROR; } cLen=qbvTxConfigInfo.cycleLen; /*Get the current running time */ runningCounts = (UINT64)((((UINT64)tsCycle.tv_sec) * 1000000000) + (UINT64)tsCycle.tv_nsec); /*Calculate the launch time*/ runningCounts = ((qbvTxConfigInfo.cycleStart + cLen) + (((runningCounts - qbvTxConfigInfo.cycleStart)/cLen )*cLen) + ((cyclePlus * cLen) + (qbvTxConfigInfo.slot[timeSlotId - SLOT_TO_INDEX_OFF].slotStart) + extraDelay)); /*save the Tx sequence, cycle length, launch time into packet to be sent*/ pRec = (struct packet_stamp *)(tbuf + IEEE8021_ETH_HEAD_LEN + 4); pRec->sequence= QbvSendDataDemoSequence++; pRec->cycleLen= cLen; pRec->tv_nsec = runningCounts%1000000000 ; pRec->tv_sec = runningCounts/1000000000 ; status = tsnQbvMuxSend(cycleID, cyclePlus, timeSlotId , extraDelay, trafficType, tbuf, qbvlen); (void)printf("sequence: %d ; ", pRec->sequence); (void)printf("Tx TIME STAMP %d:%10d!\n", pRec->tv_sec, pRec->tv_nsec); free(tbuf); return status; } /****************************************************************************** * * usrQbvSend - the routine of qbv send * * Parameters: * cyclePlus : cycle offset from the current * timeSlotId : time slot ID in cycle * extraDelay : extra delay time added to the time slot (unit ns) * * RETURNS: OK or ERROR * * ERRNO: N/A */ LOCAL STATUS usrQbvSend ( UINT32 cyclePlus, UINT32 timeSlotId, UINT32 extraDelay ) { int cycleID; int status; char *pbuf=NULL ; int slotIndex; QBVSLOT * pslot; UINT32 len ; if (qbvTxConfigInfo.bindFlag != QBV_CYCLE_BINDED) { (void)printf("QBV cycle does not exist,create first.\n"); return ERROR; } if (timeSlotId > currentSlotIndex) { (void)printf("slot does not exist,add first.\n"); return ERROR; } slotIndex=timeSlotId - SLOT_TO_INDEX_OFF; pslot= &qbvTxConfigInfo.slot[slotIndex]; len=pslot->frameLen; if(len > QBV_TEST_TX_BUFFER_LEN) { (void)printf("packet too long, len: %d.\n",len); return ERROR; } cycleID=qbvTxConfigInfo.cycleId; pbuf = malloc(len); if (pbuf == NULL) { (void)printf("qbv mem alloc error.\n"); return ERROR; } status = qbvTestDataSend(cycleID, 0, cyclePlus, timeSlotId , extraDelay, pbuf, len, pslot->vlanId, pslot->destMacAddr); free(pbuf); return status; } /****************************************************************************** * * qbvVlanMacTask - task body of test send Qbv packts. * * This function is task body of send send Qbv packts . * * * Parameters: * cyclePlus : cycle offset from the current * timeSlotId : time slot ID in cycle * extraDelay : extra delay time added to the time slot (unit ns) * * RETURNS: N/A * * ERRNO: N/A */ LOCAL void qbvVlanMacTask ( UINT32 cyclePlus, UINT32 timeSlotId, UINT32 extraDelay ) { do { usrQbvSend(cyclePlus,timeSlotId,extraDelay); taskDelay(60); } while ( GeiEndQbvDemo2TaskOff == 0); } /******************************************************************************* * * demoQbvCycleCreate - create a QBV cycle in the specific device * * This function creates a QBV cycle in the specific device. It shows how to * create a QBV cycle by tsnQbvTimerCycleCreate(). When to use QBV to do the send, * first create a cycle, then add time slot into the cycle. * * Parameters: * \is * \i * A pointer to the ethernet device name. * \i * Ethernet device index. * \i * length of cycle (unit us) * * RETURNS: OK if succefully , or ERROR if not. * Ex: demoQbvCycleCreate "gei",0,1000 */ int demoQbvCycleCreate( char *pEthName , int devIndex ,int cycleLen) { struct timespec ts; void *ethCookie; if (qbvTxConfigInfo.bindFlag == QBV_CYCLE_BINDED) { return (OK); } ethCookie = (void *)endFindByName(pEthName, devIndex); if (ethCookie == NULL) { (void)printf("QBV Interface %s%d does not exist.\n", pEthName, devIndex); return ERROR; } ts.tv_nsec = 0; ts.tv_sec = 0; qbvTxConfigInfo.cycleStart = 0; qbvTxConfigInfo.cycleId = tsnQbvTimerCycleCreate( pEthName, devIndex, &ts, cycleLen); if (qbvTxConfigInfo.cycleId != -1) { qbvTxConfigInfo.bindFlag = QBV_CYCLE_BINDED; qbvTxConfigInfo.cycleLen = cycleLen *1000; qbvTxConfigInfo.devCookie = ethCookie; return (OK); } else { return ERROR; } } /******************************************************************************* * * demoQbvFlowAdd - add time slot into the QBV cycle * * This function adds time slot into the QBV cycle. When to use QBV to do the send, * first create a cycle, then add time slot into the cycle. * * Parameters: * \is * \i * Tx flow start time (unit us). * \i * Tx flow end time (unit us). * \i * length of frame in bytes. * \i * vlan ID * \i * destination MAC address * * RETURNS: OK if succefully , or ERROR if not. * * ERRNO: N/A * Ex: demoQbvFlowAdd 360,383,1000,3000,"ab:ad:ba:be:03:e9" */ int demoQbvFlowAdd( int txStart ,int txEnd, UINT16 frameLen, UINT16 vlanId,char *destMacAddr) { int slotId; if (currentSlotIndex >= MAX_SLOT_NUMBER ) { (void)printf("slot reach to max.\n"); return ERROR; } if (txEnd < txStart ) { (void)printf("slot parameter error.\n"); return ERROR; } if (qbvTxConfigInfo.bindFlag != QBV_CYCLE_BINDED) { (void)printf("QBV cycle does not exist,create first.\n"); return ERROR; } slotId=currentSlotIndex + SLOT_TO_INDEX_OFF; qbvTxConfigInfo.slot[currentSlotIndex].slotId= slotId; qbvTxConfigInfo.slot[currentSlotIndex].slotStart=txStart *1000; qbvTxConfigInfo.slot[currentSlotIndex].slotLen=(txEnd-txStart) *1000; qbvTxConfigInfo.slot[currentSlotIndex].frameLen= frameLen; qbvTxConfigInfo.slot[currentSlotIndex].vlanId= vlanId; if( ERROR==qbvMacStrToArray(destMacAddr,qbvTxConfigInfo.slot[currentSlotIndex].destMacAddr)) return ERROR; tsnQbvTimeSlotAdd( qbvTxConfigInfo.cycleId, slotId, txStart, txEnd-txStart); currentSlotIndex++; return (OK); } /*************************************************************************** * demoQbvVlanMacStart - start the task for the demo of QBV Mux send * * This function starts the task for the demo of QBV Mux send. * * Parameters: * cyclePlus : cycle offset from the current * timeSlotId : time slot ID in cycle * extraDelay : extra delay time added to the time slot (unit ns) * * * * This task may be invoked as follows: * -> demoQbvVlanMacStart 3,1,0 * * * RETURNS: N/A * * ERRNO: N/A */ STATUS demoQbvVlanMacStart ( UINT32 cyclePlus, UINT32 timeSlotId, UINT32 extraDelay ) { if (QbvDemo2TaskId == 0) { GeiEndQbvDemo2TaskOff = 0; QbvDemo2TaskId = taskSpawn ("QbvDemoTxTask", 2, #ifdef _WRS_CONFIG_ARCH_ppc VX_FP_TASK | VX_SPE_TASK , #else VX_FP_TASK, #endif 8192, (FUNCPTR) qbvVlanMacTask, (_Vx_usr_arg_t)cyclePlus, (_Vx_usr_arg_t)timeSlotId, (_Vx_usr_arg_t)extraDelay, 0, 0, 0, 0, 0, 0, 0); } return (OK); } /****************************************************************************** * * demoQbvVlanMacStop - stop task of demoQbvVlanMacStart demo. * * This function is to stop task of QbvStartdemoVlanMacTx demo. * * * RETURNS: N/A * * ERRNO: N/A */ STATUS demoQbvVlanMacStop ( void ) { GeiEndQbvDemo2TaskOff = 1; QbvDemo2TaskId = 0; return (OK); } /* For l2 MUX receive part*/ /* Supplicant Context (supp instance specific */ typedef struct Qbv_ctx_st { void *ethCookie; char dstMac[6]; } Qbv_ctx; Qbv_ctx QbvCtx; LOCAL INT64 demoQBVMuxlastRxTimeCost = 0; /*************************************************************************** * qbvMuxBindInit - init routine for QBV mux receive demo * * Parameters: * \is * \i * A pointer to the ethernet device name. * \i * Ethernet device index. * \i * vlan ID * \i * destination MAC address * * RETURNS: OK or ERROR * * ERRNO: N/A */ LOCAL int qbvMuxBindInit( char *pEthName , int devIndex, int vlanId,char *MacAddr) { Qbv_ctx *ctx = &QbvCtx; memset(&QbvCtx, 0, sizeof(QbvCtx)); ctx->ethCookie = (void *)endFindByName(pEthName, devIndex); if (ctx->ethCookie == NULL) { (void)printf("QBV Interface %s%d does not exist.\n", pEthName, devIndex); return ERROR; } if( ERROR==qbvMacStrToArray(MacAddr,ctx->dstMac)) return ERROR; return OK; } /******************************************************************************* * * tsnQbvMuxMatch - verify the packet received is belong to QBV or not * * This function verify the packet received is belong to QBV or not by * desination MAC adress. * * RETURNS: OK if match, or ERROR if not match. */ LOCAL STATUS tsnQbvMuxMatch (Qbv_ctx * ctx, void * pkt ) { UINT8 * pbuf ; Qbv_ctx * p = (Qbv_ctx *)ctx; pbuf=(UINT8 *)pkt; if(p == NULL) { return ERROR; } if ( memcmp(pbuf, p->dstMac , 6 ) != 0 ) { return ERROR; } return OK; } /******************************************************************************* * * tsnQbvMuxRx - receive a test message from a client over muxBind by QBV * * This function receives a test message from a client over muxBind with QBV. * It shows how to validate the receive packet, get out the testing database of * QBV and use the accurate receive hardware timestamp to do the LOCALs in * the MUX receive layer. The testing database is placed at start bytes behand * the type domain, and is consist by packet sequence number, QBV period * and the launch time of the packet sent. * * RETURNS: TRUE if the message is ours, or FALSE if the message not. */ LOCAL BOOL tsnQbvMuxRx(void *param, long type, M_BLK_ID mbuf, LL_HDR_INFO *llhdr, void *spare) { struct packet_stamp *pRec; UINT64 currentRxTime = 0; UINT64 currentTxTime = 0; INT32 currentTxRxCost = 0; UINT8 *pkt = (UINT8*)mbuf->mBlkHdr.mData; Qbv_ctx * p = (Qbv_ctx *)spare; UINT8 vlanoff=0; /* validate the receive packet */ if (tsnQbvMuxMatch(p, pkt) != 0) { goto error0; } /* get out the testing database */ if(*(UINT16 *)&pkt[12] == htons(ETHERTYPE_VLAN)) { vlanoff = 4; } pRec = (struct packet_stamp *)(pkt + 14 + vlanoff); /* get out the accurate hardware timestamp */ currentRxTime = (UINT64)mbuf->mBlkPktHdr.timestamp_fetch.tv_nsec + (UINT64)((UINT64)mbuf->mBlkPktHdr.timestamp_fetch.tv_sec * 1000000000); currentTxTime = (UINT64)pRec->tv_nsec + (UINT64)((UINT64)pRec->tv_sec * 1000000000); currentTxRxCost= (INT32)(currentRxTime - currentTxTime)%pRec->cycleLen; (void)printf("sequence: %d ;Tx TIME %d:%d; Rx TIME %d:%d; Tx-Rx cost: %d ns; Tx-Rx Jitter:%d ns !\n", pRec->sequence, (UINT32)pRec->tv_sec, (UINT32)pRec->tv_nsec, (UINT32)(currentRxTime/1000000000), (UINT32)(currentRxTime%1000000000), (INT32)currentTxRxCost, (demoQBVMuxlastRxTimeCost>0) ? (INT32)(currentTxRxCost - demoQBVMuxlastRxTimeCost) :0 ); demoQBVMuxlastRxTimeCost = currentTxRxCost; demoQBVMuxlastRxTimeCost = currentTxRxCost; /* Message is ours */ netMblkClChainFree(mbuf); return TRUE; error0: /* way to say not our message */ return FALSE; } /*************************************************************************** * demoQbvMuxReceive - start the demo of QBV Mux receive * * * This function starts the demo of QBV Mux receive. It is the peer function * of the demoQbvVlanMacStart. * * This task may be invoked as follows: * -> demoQbvMuxReceive "gei",0,3000,"ab:ad:ba:be:03:e9" * * Parameters: * \is * \i * A pointer to the ethernet device name. * \i * Ethernet device index. * \i * vlan ID * \i * destination MAC address * * RETURNS: OK, or ERROR if the Mux receive of QBV could not be setup. * * ERRNO: N/A */ STATUS demoQbvMuxReceive( char *pName , int index, int vlanId,char *MacAddr) { int status; Qbv_ctx *ctx = &QbvCtx; if (qbvMuxBindInit(pName, index, vlanId,MacAddr) != OK) { return (ERROR); } /* bind the destination MAC address to the END driver */ if (muxIoctl(ctx->ethCookie, EIOCMULTIADD, (void *)ctx->dstMac) != OK) { printf ("error,%s not add MAC address to device \n",pName,index); return (ERROR); } /* bind the service to the END driver */ status=(int)muxBind(pName, index, tsnQbvMuxRx, NULL,NULL, NULL, MUX_PROTO_SNARF, "QbvRx", (void *)ctx ); return status; } #endif /* GEI_TEMP_Q_CONFIG */