//------------------------------------------------------------------------------
static tOplkError handleRxAsndFrame(const tFrameInfo* pFrameInfo_p)
{
tMsgType msgType;
UINT asndServiceId;
tOplkError ret = kErrorOk;
msgType = (tMsgType)ami_getUint8Le(&pFrameInfo_p->frame.pBuffer->messageType);
if (msgType != kMsgTypeAsnd)
{
ret = kErrorInvalidOperation;
goto Exit;
}
asndServiceId = (UINT)ami_getUint8Le(&pFrameInfo_p->frame.pBuffer->data.asnd.serviceId);
if (asndServiceId < DLL_MAX_ASND_SERVICE_ID)
{ // ASnd service ID is valid
if (instance_l.apfnDlluCbAsnd[asndServiceId] != NULL)
{ // handler was registered
ret = instance_l.apfnDlluCbAsnd[asndServiceId](pFrameInfo_p);
}
}
Exit:
return ret;
}
//------------------------------------------------------------------------------
static tOplkError handleNotRxAsndFrame(const tDllAsndNotRx* pAsndNotRx_p)
{
tOplkError ret = kErrorOk;
UINT8 aBuffer[DLLUCAL_NOTRX_FRAME_SIZE];
tPlkFrame* pFrame = (tPlkFrame*)aBuffer;
tFrameInfo frameInfo;
UINT asndServiceId;
ami_setUint8Le(&pFrame->srcNodeId, pAsndNotRx_p->nodeId);
ami_setUint8Le(&pFrame->messageType, (UINT8)kMsgTypeAsnd);
ami_setUint8Le(&pFrame->data.asnd.serviceId, pAsndNotRx_p->serviceId);
frameInfo.frameSize = DLLUCAL_NOTRX_FRAME_SIZE;
frameInfo.frame.pBuffer = pFrame;
asndServiceId = (UINT)ami_getUint8Le(&pFrame->data.asnd.serviceId);
if (asndServiceId < DLL_MAX_ASND_SERVICE_ID)
{ // ASnd service ID is valid
if (instance_l.apfnDlluCbAsnd[asndServiceId] != NULL)
{ // handler was registered
ret = instance_l.apfnDlluCbAsnd[asndServiceId](&frameInfo);
}
}
return ret;
}
//------------------------------------------------------------------------------
static tOplkError syncResponseCb(const tFrameInfo* pFrameInfo_p)
{
tOplkError ret = kErrorOk;
UINT nodeId;
UINT index;
tSyncuCbResponse pfnCbResponse;
nodeId = ami_getUint8Le(&pFrameInfo_p->frame.pBuffer->srcNodeId);
index = nodeId - 1;
if (index < tabentries(syncuInstance_g.aSyncRespQueue))
{
// memorize pointer to callback function
pfnCbResponse = readResponseQueue(nodeId);
if (pfnCbResponse == NULL)
{ // response was not requested
return ret;
}
if (pFrameInfo_p->frameSize < C_DLL_MINSIZE_SYNCRES)
{ // SyncResponse not received or it has invalid size
ret = pfnCbResponse(nodeId, NULL);
}
else
{ // SyncResponse received
ret = pfnCbResponse(nodeId, &pFrameInfo_p->frame.pBuffer->data.asnd.payload.syncResponse);
}
}
return ret;
}
//------------------------------------------------------------------------------
static tNmtCommand getNmtCommand(tFrameInfo* pFrameInfo_p)
{
tNmtCommand nmtCommand;
tNmtCommandService* pNmtCommandService;
pNmtCommandService = &pFrameInfo_p->frame.pBuffer->data.asnd.payload.nmtCommandService;
nmtCommand = (tNmtCommand)ami_getUint8Le(&pNmtCommandService->nmtCommandId);
return nmtCommand;
}
//------------------------------------------------------------------------------
static tOplkError downloadObject(tCfmNodeInfo* pNodeInfo_p)
{
tOplkError ret = kErrorOk;
// forward data pointer for last transfer
pNodeInfo_p->pDataConciseDcf += pNodeInfo_p->curDataSize;
pNodeInfo_p->bytesRemaining -= pNodeInfo_p->curDataSize;
if (pNodeInfo_p->entriesRemaining > 0)
{
if (pNodeInfo_p->bytesRemaining < CDC_OFFSET_DATA)
{
// not enough bytes left in ConciseDCF
pNodeInfo_p->eventCnProgress.error = kErrorCfmInvalidDcf;
if ((ret = callCbProgress(pNodeInfo_p)) != kErrorOk)
return ret;
return finishConfig(pNodeInfo_p, kNmtNodeCommandConfErr);
}
// fetch next item from ConciseDCF
pNodeInfo_p->eventCnProgress.objectIndex = ami_getUint16Le(&pNodeInfo_p->pDataConciseDcf[CDC_OFFSET_INDEX]);
pNodeInfo_p->eventCnProgress.objectSubIndex = ami_getUint8Le(&pNodeInfo_p->pDataConciseDcf[CDC_OFFSET_SUBINDEX]);
pNodeInfo_p->curDataSize = (UINT)ami_getUint32Le(&pNodeInfo_p->pDataConciseDcf[CDC_OFFSET_SIZE]);
pNodeInfo_p->pDataConciseDcf += CDC_OFFSET_DATA;
pNodeInfo_p->bytesRemaining -= CDC_OFFSET_DATA;
pNodeInfo_p->eventCnProgress.bytesDownloaded += CDC_OFFSET_DATA;
if ((pNodeInfo_p->bytesRemaining < pNodeInfo_p->curDataSize) ||
(pNodeInfo_p->curDataSize == 0))
{
// not enough bytes left in ConciseDCF
pNodeInfo_p->eventCnProgress.error = kErrorCfmInvalidDcf;
if ((ret = callCbProgress(pNodeInfo_p)) != kErrorOk)
return ret;
return finishConfig(pNodeInfo_p, kNmtNodeCommandConfErr);
}
pNodeInfo_p->entriesRemaining--;
ret = sdoWriteObject(pNodeInfo_p, pNodeInfo_p->pDataConciseDcf, pNodeInfo_p->curDataSize);
if (ret != kErrorOk)
return ret;
}
else
{ // download finished
#if defined(CONFIG_INCLUDE_NMT_RMN)
ret = downloadNetConf(pNodeInfo_p);
#else
ret = finishDownload(pNodeInfo_p);
#endif
if (ret != kErrorOk)
return ret;
}
return ret;
}
//------------------------------------------------------------------------------
static tOplkError processCdc(tObdCdcInfo* pCdcInfo_p)
{
tOplkError ret = kErrorOk;
UINT32 entriesRemaining;
UINT objectIndex;
UINT objectSubIndex;
size_t curDataSize;
if ((ret = loadNextBuffer(pCdcInfo_p, sizeof(UINT32))) != kErrorOk)
return ret;
entriesRemaining = ami_getUint32Le(pCdcInfo_p->pCurBuffer);
if (entriesRemaining == 0)
{
ret = eventu_postError(kEventSourceObdu, kErrorObdNoConfigData, 0, NULL);
return ret;
}
for (; entriesRemaining != 0; entriesRemaining--)
{
if ((ret = loadNextBuffer(pCdcInfo_p, CDC_OFFSET_DATA)) != kErrorOk)
return ret;
objectIndex = ami_getUint16Le(&pCdcInfo_p->pCurBuffer[CDC_OFFSET_INDEX]);
objectSubIndex = ami_getUint8Le(&pCdcInfo_p->pCurBuffer[CDC_OFFSET_SUBINDEX]);
curDataSize = (size_t)ami_getUint32Le(&pCdcInfo_p->pCurBuffer[CDC_OFFSET_SIZE]);
DEBUG_LVL_OBD_TRACE("%s: Reading object 0x%04X/%u with size %u from CDC\n",
__func__, objectIndex, objectSubIndex, curDataSize);
if ((ret = loadNextBuffer(pCdcInfo_p, curDataSize)) != kErrorOk)
{
DEBUG_LVL_OBD_TRACE("%s: Reading the corresponding data from CDC failed with 0x%02X\n", __func__, ret);
return ret;
}
ret = obd_writeEntryFromLe(objectIndex, objectSubIndex, pCdcInfo_p->pCurBuffer,
(tObdSize)curDataSize);
if (ret != kErrorOk)
{
tEventObdError obdError;
obdError.index = objectIndex;
obdError.subIndex = objectSubIndex;
DEBUG_LVL_OBD_TRACE("%s: Writing object 0x%04X/%u to local OBD failed with 0x%02X\n",
__func__, objectIndex, objectSubIndex, ret);
ret = eventu_postError(kEventSourceObdu, ret, sizeof(tEventObdError), &obdError);
if (ret != kErrorOk)
return ret;
}
}
return ret;
}
//------------------------------------------------------------------------------
static tOplkError sdoAsndCb(const tFrameInfo* pFrameInfo_p)
{
tOplkError ret = kErrorOk;
UINT count;
UINT* pConnection;
UINT nodeId;
UINT freeEntry = 0xFFFF;
tSdoConHdl sdoConHdl;
tPlkFrame* pFrame;
pFrame = pFrameInfo_p->frame.pBuffer;
nodeId = ami_getUint8Le(&pFrame->srcNodeId);
// search corresponding entry in control structure
count = 0;
pConnection = &sdoAsndInstance_l.aSdoAsndConnection[0];
while (count < CONFIG_SDO_MAX_CONNECTION_ASND)
{
if (nodeId == *pConnection)
break;
else if ((*pConnection == 0) && (freeEntry == 0xFFFF))
{ // free entry
freeEntry = count;
}
count++;
pConnection++;
}
if (count == CONFIG_SDO_MAX_CONNECTION_ASND)
{
if (freeEntry != 0xFFFF)
{
pConnection = &sdoAsndInstance_l.aSdoAsndConnection[freeEntry];
*pConnection = nodeId;
count = freeEntry;
}
else
{
DEBUG_LVL_SDO_TRACE("%s(): no free handle\n", __func__);
return ret;
}
}
sdoConHdl = (tSdoConHdl)(count | SDO_ASND_HANDLE);
sdoAsndInstance_l.pfnSdoAsySeqCb(sdoConHdl,
&pFrame->data.asnd.payload.sdoSequenceFrame,
(pFrameInfo_p->frameSize - 18));
return ret;
}
//------------------------------------------------------------------------------
static BOOL checkNodeIdList(UINT8* pbNmtCommandDate_p)
{
BOOL fNodeIdInList;
UINT byteOffset = nmtCnuInstance_g.extNmtCmdByteOffset;
UINT8 bitMask = nmtCnuInstance_g.extNmtCmdBitMask;
UINT8 nodeListByte;
nodeListByte = ami_getUint8Le(&pbNmtCommandDate_p[byteOffset]);
if ((nodeListByte & bitMask) == 0)
fNodeIdInList = FALSE;
else
fNodeIdInList = TRUE;
return fNodeIdInList;
}
//------------------------------------------------------------------------------
tOplkError dllk_deleteNodeFilter(tDllkNodeInfo* pIntNodeInfo_p, tDllNodeOpType nodeOpType_p,
BOOL fUpdateEdrv_p)
{
tOplkError ret = kErrorOk;
BYTE bPresFilterFlags = 0;
switch (nodeOpType_p)
{
case kDllNodeOpTypeFilterPdo:
bPresFilterFlags = DLLK_FILTER_FLAG_PDO;
break;
case kDllNodeOpTypeFilterHeartbeat:
bPresFilterFlags = DLLK_FILTER_FLAG_HB;
break;
default:
ret = kErrorDllInvalidParam;
goto Exit;
}
pIntNodeInfo_p->presFilterFlags &= ~bPresFilterFlags;
if (fUpdateEdrv_p != FALSE)
{
if ((pIntNodeInfo_p->presFilterFlags & (DLLK_FILTER_FLAG_PDO | DLLK_FILTER_FLAG_HB)) == 0)
{
#if CONFIG_DLL_PRES_FILTER_COUNT < 0
if (dllkInstance_g.usedPresFilterCount > 0)
dllkInstance_g.usedPresFilterCount--;
if (dllkInstance_g.usedPresFilterCount == 0)
{
// disable PRes Rx filter
dllkInstance_g.aFilter[DLLK_FILTER_PRES].fEnable = FALSE;
ret = edrv_changeRxFilter(dllkInstance_g.aFilter, DLLK_FILTER_COUNT,
DLLK_FILTER_PRES, EDRV_FILTER_CHANGE_STATE);
if (ret != kErrorOk)
goto Exit;
}
#else
UINT handle;
for (handle = DLLK_FILTER_PRES; handle < DLLK_FILTER_COUNT; handle++)
{
if (ami_getUint8Le(&dllkInstance_g.aFilter[handle].aFilterValue[16]) ==
pIntNodeInfo_p->nodeId)
{
ami_setUint8Be(&dllkInstance_g.aFilter[handle].aFilterValue[16], C_ADR_INVALID);
dllkInstance_g.aFilter[handle].fEnable = FALSE;
ret = edrv_changeRxFilter(dllkInstance_g.aFilter, DLLK_FILTER_COUNT,
handle, EDRV_FILTER_CHANGE_STATE);
if (ret != kErrorOk)
goto Exit;
break;
}
}
#endif
}
}
Exit:
return ret;
}
//---------------------------------------------------------------------------
static tOplkError copyTxPdo(tPlkFrame* pFrame_p, UINT frameSize_p, BOOL fReadyFlag_p)
{
tOplkError ret = kErrorOk;
BYTE flag1;
UINT nodeId;
tMsgType msgType;
tPdoChannel* pPdoChannel;
UINT channelId;
UINT16 pdoSize;
// set TPDO invalid, so that only fully processed TPDOs are sent as valid
flag1 = ami_getUint8Le(&pFrame_p->data.pres.flag1);
ami_setUint8Le(&pFrame_p->data.pres.flag1, (flag1 & ~PLK_FRAME_FLAG1_RD));
// retrieve POWERLINK message type
msgType = (tMsgType)ami_getUint8Le(&pFrame_p->messageType);
if (msgType == kMsgTypePres)
{ // TPDO is PRes frame
nodeId = PDO_PRES_NODE_ID; // 0x00
}
else
{ // TPDO is PReq frame
// retrieve node ID
nodeId = ami_getUint8Le(&pFrame_p->dstNodeId);
}
if (pdokInstance_g.fRunning)
{
// Get PDO channel reference
channelId = pdokInstance_g.aTpdoChannelIdLut[nodeId];
pPdoChannel = &pdokInstance_g.pdoChannels.pTxPdoChannel[channelId];
// valid TPDO found
if ((pPdoChannel->nodeId == nodeId) &&
((unsigned int)(pPdoChannel->pdoSize + 24) <= frameSize_p))
{
/*
TRACE("%s() Channel:%d Node:%d MapObjectCnt:%d PdoSize:%d\n",
__func__, channelId, nodeId, pPdoChannel->mappObjectCount,
pPdoChannel->pdoSize);
*/
// set PDO version in frame
ami_setUint8Le(&pFrame_p->data.pres.pdoVersion, pPdoChannel->mappingVersion);
pdokcal_readTxPdo(channelId, &pFrame_p->data.pres.aPayload[0],
pPdoChannel->pdoSize);
// set PDO size in frame
pdoSize = pPdoChannel->pdoSize;
}
else
{ // TPDO is too short or invalid
// $$$ raise PDO error, set ret
pdoSize = 0;
}
}
else
{
// set PDO size in frame to zero, because no TPDO mapped
pdoSize = 0;
}
// set PDO size in frame
ami_setUint16Le(&pFrame_p->data.pres.sizeLe, pdoSize);
if (fReadyFlag_p != FALSE)
{
// set TPDO valid
ami_setUint8Le(&pFrame_p->data.pres.flag1, (flag1 | PLK_FRAME_FLAG1_RD));
}
return ret;
}
//------------------------------------------------------------------------------
tOplkError pdok_processRxPdo(tPlkFrame* pFrame_p, UINT frameSize_p)
{
tOplkError ret = kErrorOk;
BYTE frameData;
UINT nodeId;
tMsgType msgType;
tPdoChannel* pPdoChannel;
UINT channelId;
// check if received RPDO is valid
frameData = ami_getUint8Le(&pFrame_p->data.pres.flag1);
if ((frameData & PLK_FRAME_FLAG1_RD) == 0)
{ // RPDO invalid
goto Exit;
}
// retrieve POWERLINK message type
msgType = (tMsgType)ami_getUint8Le(&pFrame_p->messageType);
if (msgType == kMsgTypePreq)
{ // RPDO is PReq frame
nodeId = PDO_PREQ_NODE_ID; // 0x00
}
else
{ // RPDO is PRes frame
// retrieve node ID
nodeId = ami_getUint8Le(&pFrame_p->srcNodeId);
}
if (pdokInstance_g.fRunning)
{
// Get PDO channel reference
channelId = pdokInstance_g.aRpdoChannelIdLut[nodeId];
pPdoChannel = &pdokInstance_g.pdoChannels.pRxPdoChannel[channelId];
if (pPdoChannel->nodeId != nodeId)
{ // we received a PDO which we aren't interested in
// discard it
goto Exit;
}
// retrieve PDO version from frame
frameData = ami_getUint8Le(&pFrame_p->data.pres.pdoVersion);
if ((pPdoChannel->mappingVersion & PLK_VERSION_MAIN) != (frameData & PLK_VERSION_MAIN))
{ // PDO versions do not match
// $$$ raise PDO error
// terminate processing of this RPDO
goto Exit;
}
// valid RPDO found
if ((unsigned int)(pPdoChannel->pdoSize + PLK_FRAME_OFFSET_PDO_PAYLOAD) > frameSize_p)
{ // RPDO is too short
// $$$ raise PDO error, set Ret
goto Exit;
}
/*
TRACE("%s() Channel:%d Node:%d MapObjectCnt:%d PdoSize:%d\n",
__func__, channelId, nodeId, pPdoChannel->mappObjectCount,
pPdoChannel->pdoSize);
*/
pdokcal_writeRxPdo(channelId,
&pFrame_p->data.pres.aPayload[0],
pPdoChannel->pdoSize);
}
Exit:
#if CONFIG_DLL_DEFERRED_RXFRAME_RELEASE_SYNC != FALSE
dllk_releaseRxFrame(pFrame_p, frameSize_p);
// $$$ return value?
#endif
return ret;
}
