//------------------------------------------------------------------------------ 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; }