//------------------------------------------------------------------------------
static tOplkError handleInvalidFormat(tEvent* pEvent_p)
{
tOplkError ret;
tEventDllError* pErrorHandlerEvent = (tEventDllError*)pEvent_p->pEventArg;
// check if invalid format error occurred (only direct reaction)
if ((pErrorHandlerEvent->dllErrorEvents & DLL_ERR_INVALID_FORMAT) == 0)
return kErrorOk;
ret = generateHistoryEntryNodeId(E_DLL_INVALID_FORMAT,
pEvent_p->netTime,
pErrorHandlerEvent->nodeId);
if (ret != kErrorOk)
return ret;
BENCHMARK_MOD_02_TOGGLE(7);
#ifdef CONFIG_INCLUDE_NMT_MN
if (NMT_IF_ACTIVE_MN(pErrorHandlerEvent->nmtState))
{ // MN is active
if (pErrorHandlerEvent->nodeId != 0)
{
tDllNodeOpParam NodeOpParam;
NodeOpParam.opNodeType = kDllNodeOpTypeIsochronous;
NodeOpParam.nodeId = pErrorHandlerEvent->nodeId;
// remove node from isochronous phase
dllk_deleteNode(&NodeOpParam);
// inform NmtMnu module about state change, which shall send
// NMT command ResetNode to this CN
postHeartbeatEvent(pErrorHandlerEvent->nodeId,
kNmtCsNotActive,
E_DLL_INVALID_FORMAT);
}
// $$$ and else should lead to InternComError
}
else
{
postNmtEvent(kNmtEventInternComError);
}
#else
// CN is active
postNmtEvent(kNmtEventInternComError);
#endif
return kErrorOk;
}
//------------------------------------------------------------------------------
tOplkError pdok_deAllocChannelMem(void)
{
tOplkError Ret = kErrorOk;
#if NMT_MAX_NODE_ID > 0
tDllNodeOpParam NodeOpParam;
NodeOpParam.opNodeType = kDllNodeOpTypeFilterPdo;
NodeOpParam.nodeId = C_ADR_BROADCAST;
Ret = dllk_deleteNode(&NodeOpParam);
if (Ret != kErrorOk)
{
DEBUG_LVL_PDO_TRACE("%s() dllk_deleteNode failed (%s)\n",
__func__, debugstr_getRetValStr(Ret));
return Ret;
}
#endif // NMT_MAX_NODE_ID > 0
// de-allocate mem for RX PDO channels
if (pdokInstance_g.pdoChannels.allocation.rxPdoChannelCount != 0)
{
pdokInstance_g.pdoChannels.allocation.rxPdoChannelCount = 0;
if (pdokInstance_g.pdoChannels.pRxPdoChannel != NULL)
{
OPLK_FREE(pdokInstance_g.pdoChannels.pRxPdoChannel);
pdokInstance_g.pdoChannels.pRxPdoChannel = NULL;
}
}
// de-allocate mem for TX PDO channels
if (pdokInstance_g.pdoChannels.allocation.txPdoChannelCount != 0)
{
pdokInstance_g.pdoChannels.allocation.txPdoChannelCount = 0;
if (pdokInstance_g.pdoChannels.pTxPdoChannel != NULL)
{
OPLK_FREE(pdokInstance_g.pdoChannels.pTxPdoChannel);
pdokInstance_g.pdoChannels.pTxPdoChannel = NULL;
}
}
return Ret;
}
//------------------------------------------------------------------------------
tOplkError dllkcal_process(tEvent* pEvent_p)
{
tOplkError ret = kErrorOk;
tDllCalAsndServiceIdFilter* pServFilter;
tDllIdentParam* pIdentParam;
tDllConfigParam* pConfigParam;
#if defined(CONFIG_INCLUDE_NMT_MN)
tDllCalIssueRequest* pIssueReq;
#endif
#if NMT_MAX_NODE_ID > 0
tDllNodeInfo* pNodeInfo;
tDllNodeOpParam* pNodeOpParam;
#endif
#if CONFIG_DLL_DEFERRED_RXFRAME_RELEASE_ASYNC == TRUE
tFrameInfo* pFrameInfo;
#endif
switch (pEvent_p->eventType)
{
case kEventTypeDllkServFilter:
pServFilter = (tDllCalAsndServiceIdFilter*)pEvent_p->eventArg.pEventArg;
ret = dllk_setAsndServiceIdFilter(pServFilter->serviceId,
pServFilter->filter);
break;
#if defined(CONFIG_INCLUDE_NMT_MN)
case kEventTypeDllkIssueReq:
pIssueReq = (tDllCalIssueRequest*)pEvent_p->eventArg.pEventArg;
ret = dllkcal_issueRequest(pIssueReq->service, pIssueReq->nodeId,
pIssueReq->soaFlag1);
break;
#endif
#if NMT_MAX_NODE_ID > 0
case kEventTypeDllkConfigNode:
pNodeInfo = (tDllNodeInfo*)pEvent_p->eventArg.pEventArg;
ret = dllk_configNode(pNodeInfo);
break;
case kEventTypeDllkAddNode:
pNodeOpParam = (tDllNodeOpParam*)pEvent_p->eventArg.pEventArg;
ret = dllk_addNode(pNodeOpParam);
break;
case kEventTypeDllkDelNode:
pNodeOpParam = (tDllNodeOpParam*)pEvent_p->eventArg.pEventArg;
ret = dllk_deleteNode(pNodeOpParam);
break;
#endif // NMT_MAX_NODE_ID > 0
case kEventTypeDllkIdentity:
pIdentParam = (tDllIdentParam*)pEvent_p->eventArg.pEventArg;
if (pIdentParam->sizeOfStruct > pEvent_p->eventArgSize)
{
pIdentParam->sizeOfStruct = pEvent_p->eventArgSize;
}
ret = dllk_setIdentity(pIdentParam);
break;
case kEventTypeDllkConfig:
pConfigParam = (tDllConfigParam*)pEvent_p->eventArg.pEventArg;
if (pConfigParam->sizeOfStruct > pEvent_p->eventArgSize)
{
pConfigParam->sizeOfStruct = pEvent_p->eventArgSize;
}
ret = dllk_config(pConfigParam);
break;
#if CONFIG_DLL_DEFERRED_RXFRAME_RELEASE_ASYNC == TRUE
case kEventTypeReleaseRxFrame:
pFrameInfo = (tFrameInfo*)pEvent_p->eventArg.pEventArg;
ret = dllk_releaseRxFrame(pFrameInfo->frame.pBuffer, pFrameInfo->frameSize);
if (ret == kErrorOk)
{
instance_l.asyncFrameFreed++;
instance_l.statistics.curRxFrameCount = instance_l.asyncFrameReceived - instance_l.asyncFrameFreed;
}
break;
#endif
default:
ret = kErrorInvalidEvent;
break;
}
return ret;
}
//------------------------------------------------------------------------------
static BOOL getMnSyncRequest(tDllReqServiceId* pReqServiceId_p, UINT* pNodeId_p,
tSoaPayload* pSoaPayload_p)
{
tOplkError ret;
ULONG syncReqCount = 0;
UINT syncReqSize = 0;
tDllSyncRequest syncRequest;
tDllNodeOpParam nodeOpParam;
// next queue will be CnGenReq queue
instance_l.nextRequestQueue = 0;
ret = instance_l.pTxSyncFuncs->pfnGetDataBlockCount(
instance_l.dllCalQueueTxSync, &syncReqCount);
if (ret != kErrorOk)
{
return TRUE;
}
if (syncReqCount > 0)
{
syncReqSize = sizeof(syncRequest);
ret = instance_l.pTxSyncFuncs->pfnGetDataBlock(
instance_l.dllCalQueueTxSync,
(BYTE *)&syncRequest, &syncReqSize);
if (ret != kErrorOk)
{
return TRUE;
}
if (syncReqSize > offsetof(tDllSyncRequest, syncControl))
{
ami_setUint32Le(&pSoaPayload_p->syncRequest.syncControlLe,
syncRequest.syncControl);
if ((syncRequest.syncControl & PLK_SYNC_PRES_MODE_SET) != 0)
{
nodeOpParam.opNodeType = kDllNodeOpTypeIsochronous;
nodeOpParam.nodeId = syncRequest.nodeId;
ret = dllk_addNode(&nodeOpParam);
if (ret != kErrorOk)
{
return TRUE;
}
}
if ((syncRequest.syncControl & PLK_SYNC_PRES_MODE_RESET) != 0)
{
nodeOpParam.opNodeType = kDllNodeOpTypeIsochronous;
nodeOpParam.nodeId = syncRequest.nodeId;
ret = dllk_deleteNode(&nodeOpParam);
if (ret != kErrorOk)
{
return TRUE;
}
}
}
if (syncReqSize > offsetof(tDllSyncRequest, pResTimeFirst))
{
ami_setUint32Le(&pSoaPayload_p->syncRequest.presTimeFirstLe,
syncRequest.pResTimeFirst);
}
if (syncReqSize > offsetof(tDllSyncRequest, pResFallBackTimeout))
{
ami_setUint32Le(&pSoaPayload_p->syncRequest.presFallBackTimeoutLe,
syncRequest.pResFallBackTimeout);
}
if ((syncRequest.syncControl & PLK_SYNC_DEST_MAC_ADDRESS_VALID) != 0)
{
ret = dllk_getCnMacAddress(syncRequest.nodeId,
&pSoaPayload_p->syncRequest.aDestMacAddress[0]);
if (ret != kErrorOk)
{
return TRUE;
}
}
*pNodeId_p = syncRequest.nodeId;
*pReqServiceId_p = kDllReqServiceSync;
return TRUE;
}
return FALSE;
}
//------------------------------------------------------------------------------
static tOplkError handleMnCnLossPres(tEvent* pEvent_p)
{
tOplkError ret;
UINT nodeIdx;
tDllNodeOpParam nodeOpParam;
tEventDllError* pErrorHandlerEvent = (tEventDllError*)pEvent_p->pEventArg;
UINT32 threshold, thresholdCnt, cumulativeCnt;
if ((pErrorHandlerEvent->dllErrorEvents & DLL_ERR_MN_CN_LOSS_PRES) == 0)
return kErrorOk;
nodeIdx = pErrorHandlerEvent->nodeId - 1;
//if (nodeIdx >= tabentries(pErrorObjects_p->m_adwMnCnLossPresCumCnt))
// return kErrorOk;
errhndkcal_getMnCnLossPresError(nodeIdx, &cumulativeCnt,
&thresholdCnt, &threshold);
if (instance_l.aMnCnLossPresEvent[nodeIdx] !=
ERRORHANDLERK_CN_LOSS_PRES_EVENT_NONE)
return kErrorOk;
cumulativeCnt++;
// According to spec threshold counting is disabled by setting threshold to 0
if (threshold > 0)
{
thresholdCnt += 8;
if (thresholdCnt >= threshold)
{
instance_l.aMnCnLossPresEvent[nodeIdx] =
ERRORHANDLERK_CN_LOSS_PRES_EVENT_THR;
ret = generateHistoryEntryNodeId(E_DLL_LOSS_PRES_TH,
pEvent_p->netTime,
pErrorHandlerEvent->nodeId);
if (ret != kErrorOk)
{
errhndkcal_setMnCnLossPresCounters(nodeIdx, cumulativeCnt,
thresholdCnt);
return ret;
}
// remove node from isochronous phase
nodeOpParam.opNodeType = kDllNodeOpTypeIsochronous;
nodeOpParam.nodeId = pErrorHandlerEvent->nodeId;
ret = dllk_deleteNode(&nodeOpParam);
// inform NmtMnu module about state change, which shall send
// NMT command ResetNode to this CN
postHeartbeatEvent(pErrorHandlerEvent->nodeId, kNmtCsNotActive,
E_DLL_LOSS_PRES_TH);
}
else
{
instance_l.aMnCnLossPresEvent[nodeIdx] =
ERRORHANDLERK_CN_LOSS_PRES_EVENT_OCC;
}
}
errhndkcal_setMnCnLossPresCounters(nodeIdx, cumulativeCnt, thresholdCnt);
return kErrorOk;
}
//------------------------------------------------------------------------------
tOplkError pdok_configureChannel(tPdoChannelConf* pChannelConf_p)
{
tOplkError Ret = kErrorOk;
tPdoChannel* pDestPdoChannel;
if (pChannelConf_p->fTx == FALSE)
{ // RPDO
#if NMT_MAX_NODE_ID > 0
tDllNodeOpParam NodeOpParam;
NodeOpParam.opNodeType = kDllNodeOpTypeFilterPdo;
#endif
if (pChannelConf_p->channelId >= pdokInstance_g.pdoChannels.allocation.rxPdoChannelCount)
{
Ret = kErrorPdoNotExist;
goto Exit;
}
pDestPdoChannel = &pdokInstance_g.pdoChannels.pRxPdoChannel[pChannelConf_p->channelId];
// copy channel configuration to local structure
OPLK_MEMCPY(pDestPdoChannel, &pChannelConf_p->pdoChannel,
sizeof(pChannelConf_p->pdoChannel));
// Store channel ID for fast access
pdokInstance_g.aRpdoChannelIdLut[pDestPdoChannel->nodeId] = pChannelConf_p->channelId;
#if NMT_MAX_NODE_ID > 0
if ((pDestPdoChannel->nodeId != PDO_INVALID_NODE_ID) &&
(pDestPdoChannel->nodeId != PDO_PREQ_NODE_ID))
{ // disable old PRes filter in DLL
NodeOpParam.nodeId = pDestPdoChannel->nodeId;
Ret = dllk_deleteNode(&NodeOpParam);
if (Ret != kErrorOk)
{
goto Exit;
}
}
#endif // NMT_MAX_NODE_ID > 0
#if NMT_MAX_NODE_ID > 0
if ((pDestPdoChannel->nodeId != PDO_INVALID_NODE_ID) &&
(pDestPdoChannel->nodeId != PDO_PREQ_NODE_ID))
{ // enable new PRes filter in DLL
NodeOpParam.nodeId = pDestPdoChannel->nodeId;
Ret = dllk_addNode(&NodeOpParam);
if (Ret != kErrorOk)
{
goto Exit;
}
}
#endif // NMT_MAX_NODE_ID > 0
}
else
{ // TPDO
if (pChannelConf_p->channelId >= pdokInstance_g.pdoChannels.allocation.txPdoChannelCount)
{
Ret = kErrorPdoNotExist;
goto Exit;
}
pDestPdoChannel = &pdokInstance_g.pdoChannels.pTxPdoChannel[pChannelConf_p->channelId];
// copy channel to local structure
OPLK_MEMCPY(pDestPdoChannel, &pChannelConf_p->pdoChannel,
sizeof(pChannelConf_p->pdoChannel));
// Store channel ID for fast access
pdokInstance_g.aTpdoChannelIdLut[pDestPdoChannel->nodeId] = pChannelConf_p->channelId;
}
pdokInstance_g.fRunning = FALSE;
Exit:
return Ret;
}
//------------------------------------------------------------------------------
tOplkError pdok_allocChannelMem(tPdoAllocationParam* pAllocationParam_p)
{
tOplkError ret = kErrorOk;
#if NMT_MAX_NODE_ID > 0
tDllNodeOpParam nodeOpParam;
nodeOpParam.opNodeType = kDllNodeOpTypeFilterPdo;
nodeOpParam.nodeId = C_ADR_BROADCAST;
ret = dllk_deleteNode(&nodeOpParam);
if (ret != kErrorOk)
{
goto Exit;
}
#endif // NMT_MAX_NODE_ID > 0
if (pdokInstance_g.pdoChannels.allocation.rxPdoChannelCount != pAllocationParam_p->rxPdoChannelCount)
{ // allocation should be changed
pdokInstance_g.pdoChannels.allocation.rxPdoChannelCount = pAllocationParam_p->rxPdoChannelCount;
if (pdokInstance_g.pdoChannels.pRxPdoChannel != NULL)
{
OPLK_FREE(pdokInstance_g.pdoChannels.pRxPdoChannel);
pdokInstance_g.pdoChannels.pRxPdoChannel = NULL;
}
if (pAllocationParam_p->rxPdoChannelCount > 0)
{
pdokInstance_g.pdoChannels.pRxPdoChannel =
(tPdoChannel*)OPLK_MALLOC(sizeof(*pdokInstance_g.pdoChannels.pRxPdoChannel) *
pAllocationParam_p->rxPdoChannelCount);
if (pdokInstance_g.pdoChannels.pRxPdoChannel == NULL)
{
ret = kErrorPdoInitError;
goto Exit;
}
}
}
disablePdoChannels(pdokInstance_g.pdoChannels.pRxPdoChannel,
pdokInstance_g.pdoChannels.allocation.rxPdoChannelCount);
if (pdokInstance_g.pdoChannels.allocation.txPdoChannelCount != pAllocationParam_p->txPdoChannelCount)
{ // allocation should be changed
pdokInstance_g.pdoChannels.allocation.txPdoChannelCount = pAllocationParam_p->txPdoChannelCount;
if (pdokInstance_g.pdoChannels.pTxPdoChannel != NULL)
{
OPLK_FREE(pdokInstance_g.pdoChannels.pTxPdoChannel);
pdokInstance_g.pdoChannels.pTxPdoChannel = NULL;
}
if (pAllocationParam_p->txPdoChannelCount > 0)
{
pdokInstance_g.pdoChannels.pTxPdoChannel =
(tPdoChannel*)OPLK_MALLOC(sizeof(*pdokInstance_g.pdoChannels.pTxPdoChannel) *
pAllocationParam_p->txPdoChannelCount);
if (pdokInstance_g.pdoChannels.pTxPdoChannel == NULL)
{
ret = kErrorPdoInitError;
goto Exit;
}
}
}
disablePdoChannels(pdokInstance_g.pdoChannels.pTxPdoChannel,
pdokInstance_g.pdoChannels.allocation.txPdoChannelCount);
Exit:
return ret;
}
