697 lines
16 KiB
C
697 lines
16 KiB
C
/* qav_qbv.c - Wind River 802.1Qav and 802.1Qbv library */
|
|
/*
|
|
* Copyright (c) 2016~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
|
|
--------------------
|
|
05sep16,xxy Init version.
|
|
*/
|
|
|
|
/**
|
|
* @file qav_qbv.c
|
|
* @date Mon Sep 05 10:10:02 2010
|
|
*
|
|
* @brief Functions to interact with the network mux and NIC driver.
|
|
*
|
|
*
|
|
*/
|
|
|
|
/* includes */
|
|
|
|
#include <vxWorks.h>
|
|
#include <string.h>
|
|
#include <errno.h>
|
|
#include <endLib.h>
|
|
#include <qav_qbv.h>
|
|
|
|
/* defines */
|
|
|
|
#define CYCLE_NUMBER_MAX 8
|
|
#define CYCLE_RUN_OK 1
|
|
/* typedefs */
|
|
#ifndef BILLION
|
|
#define BILLION 1000000000L
|
|
#endif
|
|
|
|
typedef struct tsnQbvCycle
|
|
{
|
|
int cycleId;
|
|
void * ethCookie;
|
|
UINT64 cycleStartCount;
|
|
size_t cycleLen;
|
|
QBV_TIME_SLOT * QBVTsHead;
|
|
int runningFlag;
|
|
} TSN_QBV_CYCLE;
|
|
|
|
typedef struct tsnQavQbvInfo
|
|
{
|
|
int cycleNum;
|
|
TSN_QBV_CYCLE cycleOpt[CYCLE_NUMBER_MAX];
|
|
} TSN_QAVQBV_INFO;
|
|
|
|
/* globals */
|
|
|
|
/* locals */
|
|
|
|
LOCAL int qavQbvInited = 0;
|
|
LOCAL TSN_QAVQBV_INFO qbvCycleInfo;
|
|
|
|
/* forward declarations */
|
|
|
|
/******************************************************************************
|
|
*
|
|
* qbvTsCreateHead - create the QBV queue head
|
|
*
|
|
* This function creates a QBV queue head bound to a specific timer cycle. It is
|
|
* called by tsnQbvTimerCycleCreate().
|
|
*
|
|
* RETURNS: HEAD or NULL
|
|
*
|
|
* ERRNO: N/A
|
|
*/
|
|
|
|
LOCAL QBV_TIME_SLOT *qbvTsCreateHead (void)
|
|
{
|
|
QBV_TIME_SLOT *head;
|
|
head = (QBV_TIME_SLOT *)malloc(sizeof (QBV_TIME_SLOT) );
|
|
|
|
if (head == NULL)
|
|
{
|
|
return NULL;
|
|
}
|
|
|
|
head->next = NULL;
|
|
return head;
|
|
}
|
|
|
|
/******************************************************************************
|
|
*
|
|
* qbvTsInsertNod - insert a node in the QBV queue
|
|
*
|
|
* This function inserts a node in the QBV queue. It is called by
|
|
* tsnQbvTimeSlotAdd().
|
|
*
|
|
* RETURNS: OK or EINVAL
|
|
*
|
|
* ERRNO: N/A
|
|
*/
|
|
|
|
LOCAL void qbvTsInsertNod
|
|
(
|
|
QBV_TIME_SLOT *head,
|
|
QBV_TIME_SLOT *s
|
|
)
|
|
{
|
|
QBV_TIME_SLOT *p = head;
|
|
|
|
while ((p->next != NULL) && (s->slotStart > p->next->slotStart))
|
|
{
|
|
p = p->next;
|
|
}
|
|
|
|
if (p->next == NULL)
|
|
{
|
|
s->next = NULL;
|
|
p->next = s;
|
|
}
|
|
else
|
|
{
|
|
s->next = p->next;
|
|
p->next = s;
|
|
}
|
|
|
|
return ;
|
|
}
|
|
|
|
/******************************************************************************
|
|
*
|
|
* qbvTsSearchNod - search for a QBV node
|
|
*
|
|
* This function searchs for a QBV node. It is called by tsnQbvTimeSlotAdd().
|
|
*
|
|
* RETURNS: node or NULL
|
|
*
|
|
* ERRNO: N/A
|
|
*/
|
|
|
|
LOCAL QBV_TIME_SLOT *qbvTsSearchNod
|
|
(
|
|
QBV_TIME_SLOT *head,
|
|
int tsId
|
|
)
|
|
{
|
|
QBV_TIME_SLOT *p = head->next;
|
|
|
|
while ( (p != NULL) && (p->slotId != tsId))
|
|
{
|
|
p = p->next;
|
|
}
|
|
|
|
if (p == NULL)
|
|
{
|
|
return NULL;
|
|
}
|
|
|
|
return p;
|
|
}
|
|
|
|
/******************************************************************************
|
|
*
|
|
* qbvTsDeleteNod - delete a node from the QBV queue list
|
|
*
|
|
* This function deletes a node from the QBV queue list. It is called by
|
|
* tsnQbvTimeSlotDelete().
|
|
*
|
|
* RETURNS: OK or ERROR
|
|
*
|
|
* ERRNO: N/A
|
|
*/
|
|
|
|
LOCAL STATUS qbvTsDeleteNod
|
|
(
|
|
QBV_TIME_SLOT *head,
|
|
int tsId
|
|
)
|
|
{
|
|
QBV_TIME_SLOT *p1 = head->next , *p2 = head ;
|
|
|
|
while ( (p1 != NULL) && (p1->slotId != tsId))
|
|
{
|
|
p2 = p1;
|
|
p1 = p1->next;
|
|
}
|
|
|
|
if (p1 == NULL)
|
|
{
|
|
printf("node not founded!!!\n");
|
|
return ERROR;
|
|
}
|
|
|
|
if (p1->slotId == tsId)
|
|
{
|
|
p2->next = p1->next;
|
|
free(p1);
|
|
printf("delete sucess!\n");
|
|
}
|
|
|
|
return OK;
|
|
}
|
|
|
|
/*******************************************************************************
|
|
*
|
|
* tsnQbvTimerCycleCreate - create a QBV timer cycle
|
|
*
|
|
* This function creates a QBV timer cycle bound to a specific Ethernet physical
|
|
* device.
|
|
*
|
|
*
|
|
* Parameters:
|
|
* \is
|
|
* \i <pName>
|
|
* A pointer to the Ethernet device name.
|
|
* \i <index>
|
|
* Ethernet device index.
|
|
* \i <cycleStart>
|
|
* A pointer to the absolute time according to the 1588 clock when the QBV cycle
|
|
* comes into effect. The 1588 clock time can be obtained by calling
|
|
* muxIoctl(ethCookie, EIOCG1588SYSTIM, (void *)&ts). If this parameter is
|
|
* zero, or less than the current time, the QBV cycle comes into effect
|
|
* immediately. If this parameter is a future time, tsnQbvMuxSend() will not send
|
|
* the packet and return ERROR.
|
|
* \i <cycleLen>
|
|
* Length of time for the cycle. The units are us.
|
|
* \ie
|
|
*
|
|
* RETURNS: \is
|
|
* \i 'cycle ID'
|
|
* if successful.
|
|
* \i 'ERROR'
|
|
* if unsuccessful.
|
|
* \ie
|
|
*
|
|
* ERRNO:
|
|
*
|
|
*
|
|
* SYNOPSIS
|
|
* int tsnQbvTimerCycleCreate
|
|
* (
|
|
* char *pName,
|
|
* int index,
|
|
* struct timespec *cycleStart,
|
|
* size_t cycleLen
|
|
* )
|
|
*
|
|
*/
|
|
|
|
|
|
int tsnQbvTimerCycleCreate
|
|
(
|
|
char *pName, /*eth device name */
|
|
int index,/*eth device index */
|
|
struct timespec *cycleStart,/*start time of cycle*/
|
|
size_t cycleLen /*length of time cycle,unit us*/
|
|
)
|
|
{
|
|
void *ethCookie;
|
|
size_t cyclelen = cycleLen;
|
|
TSN_QBV_CYCLE *pCycle;
|
|
|
|
if (qavQbvInited == 0)
|
|
{
|
|
memset(&qbvCycleInfo, 0, sizeof(TSN_QAVQBV_INFO));
|
|
qavQbvInited = 1;
|
|
}
|
|
|
|
if (qbvCycleInfo.cycleNum >= CYCLE_NUMBER_MAX )
|
|
{
|
|
(void)printf("Achieve max cycle number.\n");
|
|
return ERROR;
|
|
}
|
|
|
|
|
|
ethCookie = (void *)endFindByName(pName, index);
|
|
|
|
if (ethCookie == NULL)
|
|
{
|
|
(void)printf("QBV Interface %s%d does not exist.\n", pName, index);
|
|
return ERROR;
|
|
}
|
|
|
|
pCycle = &qbvCycleInfo.cycleOpt[qbvCycleInfo.cycleNum];
|
|
|
|
if (pCycle->runningFlag != CYCLE_RUN_OK)
|
|
{
|
|
pCycle->cycleId = qbvCycleInfo.cycleNum;
|
|
pCycle->ethCookie = ethCookie;
|
|
|
|
pCycle->cycleStartCount = (UINT64)((((UINT64)cycleStart->tv_sec) * BILLION) +
|
|
(UINT64)cycleStart->tv_nsec);
|
|
pCycle->cycleLen = cyclelen * 1000;
|
|
pCycle->QBVTsHead = qbvTsCreateHead();
|
|
|
|
if (pCycle->QBVTsHead == NULL)
|
|
{
|
|
(void)printf("create TS head failed!\n");
|
|
return ERROR;
|
|
}
|
|
|
|
pCycle->runningFlag = CYCLE_RUN_OK;
|
|
qbvCycleInfo.cycleNum++;
|
|
return (pCycle->cycleId);
|
|
}
|
|
|
|
return ERROR;
|
|
}
|
|
|
|
|
|
|
|
/*******************************************************************************
|
|
*
|
|
* tsnQbvTimeSlotAdd - add a time slot to the timer cycle of the QBV
|
|
*
|
|
* This function adds a time slot to the timer cycle of the QBV. cycleId is the
|
|
* return value of the tsnQbvTimerCycleCreate( ) function. slotId is a unique,
|
|
* user-assigned value.
|
|
*
|
|
* Parameters:
|
|
* \is
|
|
* \i <cycleId>
|
|
* ID for the cycle. This value is returned by tsnQbvTimerCycleCreate().
|
|
* \i <slotId>
|
|
* Time slot ID. This value is a unique, user-assigned value.
|
|
* \i <slotStart>
|
|
* Offset to the start boundary for each cycle. The unit for this offset is us.
|
|
* \i <slotLen>
|
|
* Length of the time slot. The unit for this length is us.
|
|
* \ie
|
|
*
|
|
* RETURNS: \is
|
|
* \i 'OK'
|
|
* if successful.
|
|
* \i 'ERROR'
|
|
* if unsuccessful.
|
|
* \ie
|
|
*
|
|
* ERRNO:
|
|
*
|
|
*
|
|
* SYNOPSIS
|
|
* STATUS tsnQbvTimeSlotAdd
|
|
* (
|
|
* int cycleId,
|
|
* int slotId,
|
|
* int slotStart,
|
|
* size_t slotLen
|
|
* )
|
|
*/
|
|
|
|
|
|
|
|
STATUS tsnQbvTimeSlotAdd
|
|
(
|
|
int cycleId, /* ID of cycle */
|
|
int slotId, /* ID of time slot */
|
|
int slotStart, /* Offset of cycle start,unit us */
|
|
size_t slotLen /* length of time slot,unit us */
|
|
)
|
|
{
|
|
QBV_TIME_SLOT *pnod;
|
|
QBV_TIME_SLOT *tsHead;
|
|
TSN_QBV_CYCLE *pCycle;
|
|
|
|
if (cycleId >= qbvCycleInfo.cycleNum )
|
|
{
|
|
(void)printf("cycle ID %d not created.\n", cycleId);
|
|
return ERROR;
|
|
}
|
|
|
|
pCycle = &qbvCycleInfo.cycleOpt[cycleId];
|
|
|
|
tsHead = pCycle->QBVTsHead;
|
|
|
|
pnod = qbvTsSearchNod(tsHead, slotId);
|
|
|
|
if (pnod != NULL)
|
|
{
|
|
printf("slot %d alloced error!\n", slotId);
|
|
return ERROR;
|
|
}
|
|
|
|
pnod = (QBV_TIME_SLOT *)malloc(sizeof (QBV_TIME_SLOT) );
|
|
|
|
if (pnod == NULL)
|
|
{
|
|
printf("create nod failed!\n");
|
|
return ERROR;
|
|
}
|
|
|
|
pnod->slotId = slotId;
|
|
pnod->slotStart = slotStart * 1000;
|
|
pnod->slotLen = slotLen * 1000;
|
|
qbvTsInsertNod(tsHead, pnod );
|
|
return OK;
|
|
}
|
|
|
|
/*******************************************************************************
|
|
*
|
|
* tsnQbvTimeSlotDelete - delete a time slot from the timer cycle of the QBV
|
|
*
|
|
* This function deletes a time slot from the timer cycle of the QBV.
|
|
*
|
|
* Parameters:
|
|
* \is
|
|
* \i <cycleId>
|
|
* ID for the cycle. This value is returned by tsnQbvTimerCycleCreate().
|
|
* \i <slotId>
|
|
* Time slot ID. This value is the unique, user-assigned value that was passed
|
|
* to tsnQbvTimeSlotAdd.
|
|
* \ie
|
|
*
|
|
* RETURNS: \is
|
|
* \i 'OK'
|
|
* if successful.
|
|
* \i 'ERROR'
|
|
* if unsuccessful.
|
|
* \ie
|
|
*
|
|
* ERRNO:
|
|
*
|
|
* SYNOPSIS
|
|
* STATUS tsnQbvTimeSlotDelete
|
|
* (
|
|
* int cycleId,
|
|
* int slotId
|
|
* )
|
|
*
|
|
*/
|
|
|
|
|
|
|
|
STATUS tsnQbvTimeSlotDelete
|
|
(
|
|
int cycleId, /*ID of cycle*/
|
|
int slotId /*ID of time slot*/
|
|
)
|
|
{
|
|
QBV_TIME_SLOT *tsHead;
|
|
TSN_QBV_CYCLE *pCycle;
|
|
|
|
if (cycleId >= qbvCycleInfo.cycleNum )
|
|
{
|
|
(void)printf("cycle ID %d not created.\n", cycleId);
|
|
return ERROR;
|
|
}
|
|
|
|
pCycle = &qbvCycleInfo.cycleOpt[cycleId];
|
|
|
|
tsHead = pCycle->QBVTsHead;
|
|
return (qbvTsDeleteNod(tsHead, slotId));
|
|
}
|
|
|
|
/******************************************************************************
|
|
*
|
|
* tsnQbvTimeSlotShow - display time slot information for a cycle
|
|
*
|
|
* This function displays the time slot information for a cycle.
|
|
*
|
|
* RETURNS: OK or ERROR
|
|
*
|
|
* ERRNO: N/A
|
|
*/
|
|
|
|
void tsnQbvTimeSlotShow
|
|
(
|
|
int cycleId /*ID of cycle*/
|
|
)
|
|
{
|
|
QBV_TIME_SLOT *tsHead;
|
|
QBV_TIME_SLOT *p;
|
|
TSN_QBV_CYCLE *pCycle;
|
|
|
|
if (cycleId >= qbvCycleInfo.cycleNum )
|
|
{
|
|
(void)printf("cycle ID %d not created.\n", cycleId);
|
|
return;
|
|
}
|
|
|
|
pCycle = &qbvCycleInfo.cycleOpt[cycleId];
|
|
|
|
tsHead = pCycle->QBVTsHead;
|
|
|
|
p = tsHead->next;
|
|
|
|
while (p != NULL)
|
|
{
|
|
printf("ID:%d,cycle:%lu, start:%d ,len:%lu\n", p->slotId,
|
|
pCycle->cycleLen, p->slotStart, p->slotLen);
|
|
p = p->next;
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
|
|
/*******************************************************************************
|
|
*
|
|
* tsnQbvMuxSend - send an Ethernet packet by TSN QBV mux
|
|
*
|
|
* This function sends an Ethernet packet by TSN QBV mux. The cycleId is the
|
|
* return value of the tsnQbvTimerCycleCreate( ) function. The cycleOffset is
|
|
* the number of additional cycles to be added to the current running cycles.
|
|
* If the cycleOffset is zero, the TSN-scheduled send is launched in the next
|
|
* cycle adjacent to the current running cycle. The cycleOffset is used to
|
|
* ensure that the TSN-scheduled send is launched in a future time. Because
|
|
* it is limited by the NIC, the value of cycleOffset should not be a large
|
|
* value. For example, the Intel I210 chip can only hold launch time for a
|
|
* maximum of 0.5 seconds. Therefore, the value of cycleOffset is calculated
|
|
* as follows: (cycleOffset * cycleLen) < 500 ms. slotDelay is the time added
|
|
* to the start time of the slot. slotDelay is used to keep the order for
|
|
* multiple packets in the same slot. slotId is the parameter passed to
|
|
* tsnQbvTimeSlotAdd(). When calling tsnQbvMuxSend(), slotId must reuse the
|
|
* value that was previously passed to tsnQbvTimeSlotAdd().
|
|
*
|
|
*
|
|
* Parameters:
|
|
* \is
|
|
* \i <cycleId>
|
|
* ID for the cycle. This value is returned by tsnQbvTimerCycleCreate().
|
|
* \i <cycleOffset>
|
|
* The number of additional cycles to be added to the current running cycles.
|
|
* \i <slotId>
|
|
* Time slot ID. This value is the unique, user-assigned value that was passed
|
|
* to tsnQbvTimeSlotAdd.
|
|
* \i <slotDelay>
|
|
* The time added to the start time of the slot. slotDelay is used to keep the
|
|
* order for multiple packets in the same slot. The units are ns.
|
|
* \i <trafficType>
|
|
* trafficType : 0 Time-Aware(TSN-scheduled)
|
|
* 1 Credit-based A(none TSN-scheduled)
|
|
* 2 Credit-based B(none TSN-scheduled)
|
|
* 3 Prioritized(none TSN-scheduled)
|
|
* 4 Best-Effort(none TSN-scheduled)
|
|
* \i <buf>
|
|
* A pointer to the buffer to be sent.
|
|
* \i <len>
|
|
* Length of the buffer. The units are bytes.
|
|
* \ie
|
|
*
|
|
* RETURNS: \is
|
|
* \i 'OK'
|
|
* if successful.
|
|
* \i 'ERROR'
|
|
* if unsuccessful.
|
|
* \ie
|
|
*
|
|
* ERRNO:
|
|
*
|
|
* SYNOPSIS
|
|
* STATUS tsnQbvMuxSend
|
|
* (
|
|
* int cycleId,
|
|
* int cycleOffset,
|
|
* int slotId,
|
|
* int slotDelay,
|
|
* int trafficType,
|
|
* const void *buf,
|
|
* size_t len
|
|
* )
|
|
*
|
|
*/
|
|
|
|
|
|
STATUS tsnQbvMuxSend
|
|
(
|
|
int cycleId,
|
|
int cycleOffset,
|
|
int slotId,
|
|
int slotDelay,
|
|
int trafficType,
|
|
const void *buf,
|
|
size_t len
|
|
)
|
|
{
|
|
TSN_QBV_CYCLE *pCycle;
|
|
END_OBJ *pEnd;
|
|
M_BLK_ID pMblk;
|
|
QBV_TIME_SLOT *tsHead;
|
|
QBV_TIME_SLOT *pnod;
|
|
STATUS status;
|
|
UINT32 numOfBytes = 0;
|
|
struct timespec tsCycle;
|
|
UINT64 runningCounts;
|
|
|
|
if (cycleId >= qbvCycleInfo.cycleNum )
|
|
{
|
|
(void)printf("cycle ID %d not created.\n", cycleId);
|
|
return ERROR;
|
|
}
|
|
|
|
pCycle = &qbvCycleInfo.cycleOpt[cycleId];
|
|
|
|
|
|
pEnd = (END_OBJ *)pCycle->ethCookie;
|
|
|
|
if ((pMblk = endPoolTupleGet (pEnd->pNetPool)) == NULL)
|
|
{
|
|
return ERROR;
|
|
}
|
|
|
|
numOfBytes = (UINT32)netMblkFromBufCopy(pMblk,
|
|
(char *)buf,
|
|
0,
|
|
(UINT32)len,
|
|
M_WAIT,
|
|
NULL);
|
|
pMblk->mBlkHdr.mLen = numOfBytes;
|
|
pMblk->mBlkHdr.mFlags = M_EXT | M_PKTHDR;
|
|
pMblk->mBlkHdr.mType = MT_DATA;
|
|
pMblk->mBlkPktHdr.len = numOfBytes;
|
|
pMblk->mBlkPktHdr.launch_time = 0;
|
|
pMblk->mBlkPktHdr.time_flags = 0;
|
|
|
|
switch (trafficType)
|
|
{
|
|
case 0:
|
|
case 1:
|
|
pMblk->mBlkPktHdr.queue_priority = PKT_PRIORITY_FLAGS_SR;
|
|
break;
|
|
|
|
case 2:
|
|
pMblk->mBlkPktHdr.queue_priority = PKT_PRIORITY_FLAGS_SR2;
|
|
break;
|
|
|
|
case 3:
|
|
pMblk->mBlkPktHdr.queue_priority = PKT_PRIORITY_FLAGS_SP;
|
|
break;
|
|
|
|
default:
|
|
pMblk->mBlkPktHdr.queue_priority = PKT_PRIORITY_FLAGS_BE;
|
|
break;
|
|
}
|
|
|
|
if (trafficType == 0)
|
|
{
|
|
tsHead = pCycle->QBVTsHead;
|
|
pnod = qbvTsSearchNod(tsHead, slotId);
|
|
|
|
if (pnod == NULL)
|
|
{
|
|
endPoolTupleFree(pMblk);
|
|
return ERROR;
|
|
}
|
|
|
|
status = muxIoctl(pEnd, EIOCG1588SYSTIM, (void *)&tsCycle);
|
|
if (status != OK)
|
|
{
|
|
endPoolTupleFree(pMblk);
|
|
return ERROR;
|
|
}
|
|
/*Get the current running time */
|
|
runningCounts = (UINT64)((((UINT64)tsCycle.tv_sec) * BILLION) +
|
|
(UINT64)tsCycle.tv_nsec);
|
|
|
|
/*Calculate the launch time*/
|
|
if (pCycle->cycleStartCount > 0)
|
|
{
|
|
/* Check that cycle start time is reached */
|
|
|
|
if (runningCounts < pCycle->cycleStartCount)
|
|
{
|
|
endPoolTupleFree (pMblk);
|
|
return ERROR;
|
|
}
|
|
}
|
|
|
|
/* Calculate the number of cycles passed since cycle start */
|
|
|
|
runningCounts = (runningCounts - pCycle->cycleStartCount)/pCycle->cycleLen;
|
|
|
|
/* Send packet in the next cycle plus offset cycles */
|
|
|
|
runningCounts += cycleOffset + 1;
|
|
|
|
/* Time interval of send cycle relative to the cycle start in nanoseconds */
|
|
|
|
runningCounts *= pCycle->cycleLen;
|
|
|
|
/* Add cycles start time, timeslot offset and slot delay */
|
|
|
|
runningCounts += pCycle->cycleStartCount + pnod->slotStart + slotDelay;
|
|
|
|
/* Launchtime is the nanoseconds part of the absolute launchtime */
|
|
|
|
pMblk->mBlkPktHdr.launch_time = (UINT32)(runningCounts%BILLION);
|
|
|
|
pMblk->mBlkPktHdr.time_flags = PKT_TSN_TIME_LAUNCH_EN;
|
|
}
|
|
|
|
status = muxSend(pEnd, pMblk);
|
|
|
|
return status;
|
|
}
|