//------------------------------------------------------------------------------
static tOplkError insertDataBlock(tDllCalQueueInstance pDllCalQueue_p,
BYTE* pData_p, UINT* pDataSize_p)
{
tOplkError ret = kErrorOk;
tCircBufError error;
tDllCalCircBufInstance* pDllCalCircBufInstance =
(tDllCalCircBufInstance*)pDllCalQueue_p;
if (pDllCalCircBufInstance == NULL)
{
ret = kErrorInvalidInstanceParam;
goto Exit;
}
error = circbuf_writeData(pDllCalCircBufInstance->pCircBufInstance,
pData_p, *pDataSize_p);
switch (error)
{
case kCircBufOk:
break;
case kCircBufExceedDataSizeLimit:
case kCircBufBufferFull:
ret = kErrorDllAsyncTxBufferFull;
break;
case kCircBufInvalidArg:
default:
ret = kErrorNoResource;
break;
}
Exit:
return ret;
}
//------------------------------------------------------------------------------
tOplkError dllkcal_issueRequest(tDllReqServiceId service_p, UINT nodeId_p,
BYTE soaFlag1_p)
{
tOplkError ret = kErrorOk;
tCircBufError err;
if (soaFlag1_p != 0xFF)
{
ret = dllk_setFlag1OfNode(nodeId_p, soaFlag1_p);
if (ret != kErrorOk)
{
goto Exit;
}
}
// add node to appropriate request queue
switch (service_p)
{
case kDllReqServiceIdent:
err = circbuf_writeData(instance_l.pQueueIdentReq, &nodeId_p, sizeof(nodeId_p));
if (err != kCircBufOk)
{ // queue is full
ret = kErrorDllAsyncTxBufferFull;
goto Exit;
}
break;
case kDllReqServiceStatus:
err = circbuf_writeData(instance_l.pQueueStatusReq, &nodeId_p, sizeof(nodeId_p));
if (err != kCircBufOk)
{ // queue is full
ret = kErrorDllAsyncTxBufferFull;
goto Exit;
}
break;
default:
ret = kErrorDllInvalidParam;
goto Exit;
}
Exit:
return ret;
}
//------------------------------------------------------------------------------
tOplkError dllkcal_setAsyncPendingRequests(UINT nodeId_p,
tDllAsyncReqPriority asyncReqPrio_p,
UINT count_p)
{
tOplkError ret = kErrorOk;
tCircBufError err;
UINT* pLocalRequestCnt;
tCircBufInstance* pTargetQueue;
// get local request count for the node and the target queue
switch (asyncReqPrio_p)
{
case kDllAsyncReqPrioNmt:
pLocalRequestCnt = &instance_l.aCnRequestCntNmt[nodeId_p-1];
pTargetQueue = instance_l.pQueueCnRequestNmt;
break;
default:
pLocalRequestCnt = &instance_l.aCnRequestCntGen[nodeId_p-1];
pTargetQueue = instance_l.pQueueCnRequestGen;
break;
}
// compare the node request count with the locally stored one
if (*pLocalRequestCnt < count_p)
{
// The node has added some requests, but post only one for fair
// scheduling among the other nodes.
err = circbuf_writeData(pTargetQueue, &nodeId_p, sizeof(nodeId_p));
if (err == kCircBufOk)
(*pLocalRequestCnt)++; // increment locally only by successful post
}
else
{
// the node's request count is equal or less the local one
*pLocalRequestCnt = count_p;
}
return ret;
}
//------------------------------------------------------------------------------
static tOplkError insertDataBlock(tDllCalQueueInstance pDllCalQueue_p,
const UINT8* pData_p,
UINT dataSize_p)
{
tOplkError ret = kErrorOk;
tCircBufError error;
tDllCalCircBufInstance* pDllCalCircBufInstance =
(tDllCalCircBufInstance*)pDllCalQueue_p;
// Check parameter validity
ASSERT(pData_p != NULL);
if (pDllCalCircBufInstance == NULL)
{
ret = kErrorInvalidInstanceParam;
goto Exit;
}
error = circbuf_writeData(pDllCalCircBufInstance->pCircBufInstance,
pData_p,
dataSize_p);
switch (error)
{
case kCircBufOk:
break;
case kCircBufBufferFull:
ret = kErrorDllAsyncTxBufferFull;
break;
case kCircBufInvalidArg:
default:
ret = kErrorNoResource;
break;
}
Exit:
return ret;
}
//------------------------------------------------------------------------------
tOplkError sdotestcom_sendFrame(UINT nodeId_p, tSdoType sdoType_p,
tAsySdoCom* pSdoCom_p, size_t sdoSize_p)
{
tOplkError ret;
tSdoTestComCon* pCmdCon;
tEvent Event;
tCircBufError CbError;
tPlkFrame* pFrame;
tAsySdoCom* pSdoCom_Dst;
size_t FrameSize;
ret = kErrorOk;
pCmdCon = &sdoTestComInst.tCmdCon;
FrameSize = PLK_FRAME_OFFSET_SDO_COMU + sdoSize_p;
// Check if parameters are valid
if (FrameSize > C_DLL_MAX_ASYNC_MTU)
{
return kErrorInvalidOperation;
}
if (kOplkTestSdoComStateIdle != pCmdCon->tState)
{
// If the connection is already in use, node ID and SDO type have to match
if ((pCmdCon->nodeId != nodeId_p) ||
(pCmdCon->tSdoType != sdoType_p))
{
return kErrorInvalidOperation;
}
}
// Get frame buffer
pFrame = (tPlkFrame *)OPLK_MALLOC(FrameSize);
if (pFrame == NULL)
{
ret = kErrorNoResource;
}
// Generate frame
pSdoCom_Dst = &pFrame->data.asnd.payload.sdoSequenceFrame.sdoSeqPayload;
OPLK_MEMSET(pFrame, 0, FrameSize);
OPLK_MEMCPY(pSdoCom_Dst, pSdoCom_p, sdoSize_p);
// Save frame in shared buffer
CbError = circbuf_writeData(pCmdCon->pCbBufInst,
pFrame, FrameSize);
OPLK_FREE(pFrame);
if (kCircBufOk != CbError)
{
ret = kErrorInvalidOperation;
}
else
{
// Sequence layer handling
// Either create a new connection, or reuse existing one
switch (pCmdCon->tState)
{
case kOplkTestSdoComStateIdle:
// Get new sequence layer connection
// When the connection is ready, the callback will trigger sending
ret = sdoseq_initCon(&pCmdCon->tSeqHdl, nodeId_p, sdoType_p);
pCmdCon->tState = kOplkTestSdoComStateWaitInit;
pCmdCon->tSdoType = sdoType_p;
pCmdCon->nodeId = nodeId_p;
break;
case kOplkTestSdoComStateWaitInit:
// Connection setup is already in progress
// Nothing left to do
break;
case kOplkTestSdoComStateConnected:
// Connection is already up and running,
// just trigger frame send event
OPLK_MEMSET(&Event.netTime, 0x00, sizeof(Event.netTime));
Event.eventType = kEventTypeSdoAsySend;
Event.pEventArg = pCmdCon;
Event.eventArgSize = sizeof(*pCmdCon);
Event.eventSink = kEventSinkSdoTest;
ret = eventu_postEvent(&Event);
break;
default:
// Reject unknown states
ret = kErrorInvalidOperation;
break;
}
}
return ret;
}