bjayesh/m241Neelima
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
ac0de9605242fc6efcc9db3068c13d7a33e65f6b
m241Neelima/components/ip_net2-6.9/tsn/demo/qbv_dkm.c
T
neelimap afc7070c1f original files of m241
2026-07-30 11:19:44 +05:30

805 lines
18 KiB
C
Raw Blame History

/* 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 <vxWorks.h>
#include <string.h>
#include <errno.h>
#include <netinet/ip.h>
#include <endLib.h>
#include <net/ethernet.h>
#include <qav_qbv.h>
#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 <pName>
* A pointer to the ethernet device name.
* \i <index>
* Ethernet device index.
* \i <cycleLen>
* 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 <txStart>
* Tx flow start time (unit us).
* \i <txEnd>
* Tx flow end time (unit us).
* \i <frameLen>
* length of frame in bytes.
* \i <vlanId>
* vlan ID
* \i <destMacAddr>
* 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 <pName>
* A pointer to the ethernet device name.
* \i <devIndex>
* Ethernet device index.
* \i <vlanId>
* vlan ID
* \i <MacAddr>
* 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 <pName>
* A pointer to the ethernet device name.
* \i <index>
* Ethernet device index.
* \i <vlanId>
* vlan ID
* \i <destMacAddr>
* 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 */
Reference in New Issue View Git Blame Copy Permalink