//------------------------------------------------------------------------------ tOplkError sdoasnd_sendData(tSdoConHdl sdoConHandle_p, tPlkFrame* pSrcData_p, size_t dataSize_p) { tOplkError ret; UINT array; tFrameInfo frameInfo; array = ((UINT)sdoConHandle_p & ~SDO_ASY_HANDLE_MASK); if (array > CONFIG_SDO_MAX_CONNECTION_ASND) return kErrorSdoAsndInvalidHandle; // fill Asnd header // own node id not needed -> filled by DLL ami_setUint8Le(&pSrcData_p->messageType, (UINT8)kMsgTypeAsnd); // ASnd == 0x06 ami_setUint8Le(&pSrcData_p->dstNodeId, (UINT8)sdoAsndInstance_l.aSdoAsndConnection[array]); ami_setUint8Le(&pSrcData_p->srcNodeId, 0x00); // set source-nodeid (filled by DLL 0) // calc size (add Ethernet and ASnd header size) dataSize_p += (size_t)((UINT8*)&pSrcData_p->data.asnd.payload.sdoSequenceFrame - (UINT8*)pSrcData_p); // send function of DLL frameInfo.frameSize = (UINT)dataSize_p; frameInfo.frame.pBuffer = pSrcData_p; ret = dllucal_sendAsyncFrame(&frameInfo, kDllAsyncReqPrioGeneric); return ret; }
//------------------------------------------------------------------------------ static tOplkError processPresReady(tNmtState nmtState_p) { tOplkError ret = kErrorOk; tPlkFrame* pTxFrame; // post PRes to transmit FIFO if (nmtState_p != kNmtCsBasicEthernet) { // Does PRes exist? if (dllkInstance_g.pTxBuffer[DLLK_TXFRAME_PRES + dllkInstance_g.curTxBufferOffsetCycle].pBuffer != NULL) { // PRes does exist pTxFrame = (tPlkFrame*)dllkInstance_g.pTxBuffer[DLLK_TXFRAME_PRES + dllkInstance_g.curTxBufferOffsetCycle].pBuffer; // update frame (NMT state, RD, RS, PR, MS, EN flags) if (nmtState_p < kNmtCsPreOperational2) { // NMT state is not PreOp2, ReadyToOp or Op // fake NMT state PreOp2, because PRes will be sent only in PreOp2 or greater nmtState_p = kNmtCsPreOperational2; } ami_setUint8Le(&pTxFrame->data.pres.nmtStatus, (UINT8)nmtState_p); ami_setUint8Le(&pTxFrame->data.pres.flag2, dllkInstance_g.flag2); if (nmtState_p != kNmtCsOperational) { // mark PDO as invalid in all NMT state but Op // $$$ reset only RD flag; set other flags appropriately ami_setUint8Le(&pTxFrame->data.pres.flag1, 0); } // $$$ make function that updates Pres, StatusRes // mark PRes frame as ready for transmission ret = edrv_setTxBufferReady(&dllkInstance_g.pTxBuffer[DLLK_TXFRAME_PRES + dllkInstance_g.curTxBufferOffsetCycle]); } } 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; }
//------------------------------------------------------------------------------ tOplkError dllknode_deleteNodeIsochronous(tDllkNodeInfo* pIntNodeInfo_p) { tOplkError ret = kErrorOk; tDllkNodeInfo** ppIntNodeInfo; tPlkFrame * pTxFrame; if ((pIntNodeInfo_p->pPreqTxBuffer == NULL) && (pIntNodeInfo_p->nodeId != dllkInstance_g.dllConfigParam.nodeId)) { ppIntNodeInfo = &dllkInstance_g.pFirstPrcNodeInfo; } else { ppIntNodeInfo = &dllkInstance_g.pFirstNodeInfo; } // search node in whole list while ((*ppIntNodeInfo != NULL) && (*ppIntNodeInfo != pIntNodeInfo_p)) { ppIntNodeInfo = &(*ppIntNodeInfo)->pNextNodeInfo; } if ((*ppIntNodeInfo == NULL) || (*ppIntNodeInfo != pIntNodeInfo_p)) { // node was not found in list // $$$ d.k. maybe this should be an error return ret; } // remove node from list *ppIntNodeInfo = pIntNodeInfo_p->pNextNodeInfo; if (pIntNodeInfo_p->pPreqTxBuffer != NULL) { // disable TPDO pTxFrame = (tPlkFrame*)pIntNodeInfo_p->pPreqTxBuffer[0].pBuffer; if (pTxFrame != NULL) { // frame does exist // update frame (disable RD in Flag1) ami_setUint8Le(&pTxFrame->data.preq.flag1, 0); } pTxFrame = (tPlkFrame*)pIntNodeInfo_p->pPreqTxBuffer[1].pBuffer; if (pTxFrame != NULL) { // frame does exist // update frame (disable RD in Flag1) ami_setUint8Le(&pTxFrame->data.preq.flag1, 0); } } return ret; }
//------------------------------------------------------------------------------ static tOplkError processSyncMn(tNmtState nmtState_p, BOOL fReadyFlag_p) { tOplkError ret = kErrorOk; tPlkFrame * pTxFrame; tEdrvTxBuffer* pTxBuffer; UINT index = 0; UINT32 nextTimeOffsetNs = 0; UINT nextTxBufferOffset = dllkInstance_g.curTxBufferOffsetCycle ^ 1; pTxBuffer = &dllkInstance_g.pTxBuffer[DLLK_TXFRAME_SOC + nextTxBufferOffset]; pTxBuffer->timeOffsetNs = nextTimeOffsetNs; pTxFrame = (tPlkFrame*)pTxBuffer->pBuffer; #if defined(CONFIG_INCLUDE_SOC_TIME_FORWARD) // Forward SoC time information to timesync module. Note that this SoC time // info is sent after the current cycle is completed (due to double buffers)! ret = timesynck_setSocTime(&dllkInstance_g.socTime); if (ret != kErrorOk) return ret; #endif // Set SoC relative time ami_setUint64Le(&pTxFrame->data.soc.relativeTimeLe, dllkInstance_g.socTime.relTime); dllkInstance_g.socTime.relTime += dllkInstance_g.dllConfigParam.cycleLen; if (!dllkInstance_g.socTime.fRelTimeValid) { // SoC time information is valid from now on... dllkInstance_g.socTime.fRelTimeValid = TRUE; } // Update SOC Prescaler Flag ami_setUint8Le(&pTxFrame->data.soc.flag1, dllkInstance_g.mnFlag1 & (PLK_FRAME_FLAG1_PS | PLK_FRAME_FLAG1_MC)); if (dllkInstance_g.ppTxBufferList == NULL) return ret; dllkInstance_g.ppTxBufferList[index] = pTxBuffer; index++; ret = dllknode_setupSyncPhase(nmtState_p, fReadyFlag_p, nextTxBufferOffset, &nextTimeOffsetNs, &index); if (ret != kErrorOk) return ret; dllknode_setupAsyncPhase(nmtState_p, nextTxBufferOffset, nextTimeOffsetNs, &index); // set last list element to NULL dllkInstance_g.ppTxBufferList[index] = NULL; index++; ret = edrvcyclic_setNextTxBufferList(dllkInstance_g.ppTxBufferList, index); return ret; }
//------------------------------------------------------------------------------ tOplkError nmtcnu_sendNmtRequestEx(UINT nodeId_p, tNmtCommand nmtCommand_p, void* pNmtCommandData_p, UINT dataSize_p) { tOplkError ret; tFrameInfo nmtRequestFrameInfo; tPlkFrame nmtRequestFrame; ret = kErrorOk; // build frame OPLK_MEMSET(&nmtRequestFrame.aDstMac[0], 0x00, sizeof(nmtRequestFrame.aDstMac)); // set by DLL OPLK_MEMSET(&nmtRequestFrame.aSrcMac[0], 0x00, sizeof(nmtRequestFrame.aSrcMac)); // set by DLL ami_setUint16Be(&nmtRequestFrame.etherType, C_DLL_ETHERTYPE_EPL); ami_setUint8Le(&nmtRequestFrame.dstNodeId, (BYTE)C_ADR_MN_DEF_NODE_ID); // node id of the MN ami_setUint8Le(&nmtRequestFrame.messageType, (BYTE)kMsgTypeAsnd); ami_setUint8Le(&nmtRequestFrame.data.asnd.serviceId, (BYTE)kDllAsndNmtRequest); ami_setUint8Le(&nmtRequestFrame.data.asnd.payload.nmtRequestService.nmtCommandId, (BYTE)nmtCommand_p); ami_setUint8Le(&nmtRequestFrame.data.asnd.payload.nmtRequestService.targetNodeId, (BYTE)nodeId_p); // target for the nmt command OPLK_MEMSET(&nmtRequestFrame.data.asnd.payload.nmtRequestService.aNmtCommandData[0], 0x00, sizeof(nmtRequestFrame.data.asnd.payload.nmtRequestService.aNmtCommandData)); if (pNmtCommandData_p && (dataSize_p != 0)) { OPLK_MEMCPY(&nmtRequestFrame.data.asnd.payload.nmtRequestService.aNmtCommandData[0], pNmtCommandData_p, min(dataSize_p, sizeof(nmtRequestFrame.data.asnd.payload.nmtRequestService.aNmtCommandData))); } // build info-structure nmtRequestFrameInfo.pFrame = &nmtRequestFrame; nmtRequestFrameInfo.frameSize = C_DLL_MINSIZE_NMTREQ; // sizeof(nmtRequestFrame); // send NMT request ret = dllucal_sendAsyncFrame(&nmtRequestFrameInfo, kDllAsyncReqPrioNmt); return ret; }
//------------------------------------------------------------------------------ static tOplkError generateHistoryEntryNodeId(UINT16 errorCode_p, tNetTime netTime_p, UINT nodeId_p) { tOplkError ret; tErrHistoryEntry historyEntry; historyEntry.entryType = ERR_ENTRYTYPE_MODE_OCCURRED | ERR_ENTRYTYPE_PROF_PLK | ERR_ENTRYTYPE_HISTORY; historyEntry.errorCode = errorCode_p; historyEntry.timeStamp = netTime_p; ami_setUint8Le(&historyEntry.aAddInfo[0], (BYTE)nodeId_p); ret = postHistoryEntryEvent(&historyEntry); return ret; }
//------------------------------------------------------------------------------ tOplkError sdotestseq_sendFrame(UINT nodeId_p, tSdoType sdoType_p, tAsySdoSeq* pSdoSeq_p, size_t sdoSize_p) { tOplkError ret = kErrorOk; tSdoTestSeqCon* pCon; size_t FrameSize; tPlkFrame* pFrame; tAsySdoSeq* pSequDst; // Check parameters FrameSize = sdoSize_p + PLK_FRAME_OFFSET_SDO_SEQU; if (FrameSize > C_DLL_MAX_ASYNC_MTU) { return kErrorInvalidOperation; } // Try to get a valid lower layer connection pCon = &sdoTestSeqInst.seqCon; if (pCon->state == kOplkTestSdoSequConIdle) { // We need a new connection switch (sdoType_p) { case kSdoTypeUdp: #if defined (CONFIG_INCLUDE_SDO_UDP) ret = sdoudp_initCon(&pCon->sdoConHandle, nodeId_p); #else ret = kErrorSdoSeqUnsupportedProt; #endif if (ret != kErrorOk) { return ret; } #if defined (CONFIG_INCLUDE_SDO_UDP) pCon->pFuncTable = &sdoTestSeqUdpFuncs; #endif break; case kSdoTypeAsnd: #if defined (CONFIG_INCLUDE_SDO_ASND) ret = sdoasnd_initCon(&pCon->sdoConHandle, nodeId_p); #else ret = kErrorSdoSeqUnsupportedProt; #endif if (ret != kErrorOk) { return ret; } #if defined (CONFIG_INCLUDE_SDO_ASND) pCon->pFuncTable = &sdoTestSeqAsndFuncs; #endif break; default: case kSdoTypeAuto: case kSdoTypePdo: // Current implementation only supports Asnd and UDP return kErrorSdoSeqUnsupportedProt; } // Save parameters pCon->state = kOplkTestSdoSequConActive; pCon->sdoType = sdoType_p; pCon->nodeId = nodeId_p; } else { // Connection exists, check parameters if ((nodeId_p != pCon->nodeId) || (sdoType_p != pCon->sdoType)) { return kErrorInvalidOperation; } } // Get frame buffer pFrame = (tPlkFrame*)OPLK_MALLOC(FrameSize); if (pFrame == NULL) { ret = kErrorNoResource; } else { // Set up frame OPLK_MEMSET(pFrame, 0, FrameSize); pSequDst = &pFrame->data.asnd.payload.sdoSequenceFrame; OPLK_MEMCPY(pSequDst, pSdoSeq_p, sdoSize_p); ami_setUint8Le(&pFrame->data.asnd.serviceId, (BYTE)kDllAsndSdo); // Send data ret = pCon->pFuncTable->pfnSendData(pCon->sdoConHandle, pFrame, sdoSize_p); OPLK_FREE(pFrame); } 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 dllknode_setupSyncPhase(tNmtState nmtState_p, BOOL fReadyFlag_p, UINT nextTxBufferOffset_p, UINT32* pNextTimeOffsetNs_p, UINT* pIndex_p) { tOplkError ret = kErrorOk; BYTE* pCnNodeId; UINT32 accFrameLenNs = 0; tPlkFrame* pTxFrame; tEdrvTxBuffer* pTxBuffer; tFrameInfo FrameInfo; tDllkNodeInfo* pIntNodeInfo; BYTE flag1; // calculate WaitSoCPReq delay if (dllkInstance_g.dllConfigParam.waitSocPreq != 0) { *pNextTimeOffsetNs_p = dllkInstance_g.dllConfigParam.waitSocPreq + C_DLL_T_PREAMBLE + C_DLL_T_MIN_FRAME + C_DLL_T_IFG; } else { accFrameLenNs = C_DLL_T_PREAMBLE + C_DLL_T_MIN_FRAME + C_DLL_T_IFG; } pCnNodeId = &dllkInstance_g.aCnNodeIdList[nextTxBufferOffset_p][0]; if (nmtState_p != kNmtMsOperational) fReadyFlag_p = FALSE; pIntNodeInfo = dllkInstance_g.pFirstNodeInfo; while (pIntNodeInfo != NULL) { pTxBuffer = &pIntNodeInfo->pPreqTxBuffer[nextTxBufferOffset_p]; if ((pTxBuffer != NULL) && (pTxBuffer->pBuffer != NULL)) { // PReq does exist pTxFrame = (tPlkFrame*)pTxBuffer->pBuffer; flag1 = pIntNodeInfo->soaFlag1 & PLK_FRAME_FLAG1_EA; // $$$ d.k. set PLK_FRAME_FLAG1_MS if necessary // update frame (Flag1) ami_setUint8Le(&pTxFrame->data.preq.flag1, flag1); // process TPDO FrameInfo.pFrame = pTxFrame; FrameInfo.frameSize = pTxBuffer->txFrameSize; ret = dllkframe_processTpdo(&FrameInfo, fReadyFlag_p); if (ret != kErrorOk) return ret; pTxBuffer->timeOffsetNs = *pNextTimeOffsetNs_p; dllkInstance_g.ppTxBufferList[*pIndex_p] = pTxBuffer; (*pIndex_p)++; if (pTxBuffer == &dllkInstance_g.pTxBuffer[DLLK_TXFRAME_PRES + nextTxBufferOffset_p]) { // PRes of MN will be sent // update NMT state ami_setUint8Le(&pTxFrame->data.pres.nmtStatus, (BYTE) nmtState_p); *pNextTimeOffsetNs_p = pIntNodeInfo->presTimeoutNs; { tDllkNodeInfo* pIntPrcNodeInfo; pIntPrcNodeInfo = dllkInstance_g.pFirstPrcNodeInfo; while (pIntPrcNodeInfo != NULL) { *pCnNodeId = (BYTE)pIntPrcNodeInfo->nodeId; pCnNodeId++; *pNextTimeOffsetNs_p = pIntNodeInfo->presTimeoutNs; pIntPrcNodeInfo = pIntPrcNodeInfo->pNextNodeInfo; } *pCnNodeId = C_ADR_BROADCAST; // mark this entry as PRC slot finished pCnNodeId++; } } else { // PReq to CN *pCnNodeId = (BYTE)pIntNodeInfo->nodeId; pCnNodeId++; *pNextTimeOffsetNs_p = pIntNodeInfo->presTimeoutNs; } if (*pNextTimeOffsetNs_p == 0) { // add SoC frame length accFrameLenNs += C_DLL_T_PREAMBLE + (pTxBuffer->txFrameSize * C_DLL_T_BITTIME) + C_DLL_T_IFG; } else { *pNextTimeOffsetNs_p += accFrameLenNs; accFrameLenNs = 0; } } pIntNodeInfo = pIntNodeInfo->pNextNodeInfo; } *pCnNodeId = C_ADR_INVALID; // mark last entry in node-ID list return ret; }
//------------------------------------------------------------------------------ tOplkError dllknode_addNodeIsochronous(tDllkNodeInfo* pIntNodeInfo_p) { tOplkError ret = kErrorOk; tDllkNodeInfo** ppIntNodeInfo; tPlkFrame * pTxFrame; if (pIntNodeInfo_p->nodeId == dllkInstance_g.dllConfigParam.nodeId) { // we shall send PRes ourself // insert our node as first entry in the list ppIntNodeInfo = &dllkInstance_g.pFirstNodeInfo; if (*ppIntNodeInfo != NULL) { if ((*ppIntNodeInfo)->nodeId == pIntNodeInfo_p->nodeId) { // node was already added to list // $$$ d.k. maybe this should be an error goto Exit; } } // set "PReq"-TxBuffer to PRes-TxBuffer pIntNodeInfo_p->pPreqTxBuffer = &dllkInstance_g.pTxBuffer[DLLK_TXFRAME_PRES]; // Reset PRC Slot Timeout // which is required if falling back to PreOp1 pIntNodeInfo_p->presTimeoutNs = 0; } else { // normal CN shall be added to isochronous phase // insert node into list in ascending order if (pIntNodeInfo_p->pPreqTxBuffer == NULL) { ppIntNodeInfo = &dllkInstance_g.pFirstPrcNodeInfo; } else { ppIntNodeInfo = &dllkInstance_g.pFirstNodeInfo; } while ((*ppIntNodeInfo != NULL) && (((*ppIntNodeInfo)->nodeId < pIntNodeInfo_p->nodeId) || ((*ppIntNodeInfo)->nodeId == dllkInstance_g.dllConfigParam.nodeId))) { ppIntNodeInfo = &(*ppIntNodeInfo)->pNextNodeInfo; } if ((*ppIntNodeInfo != NULL) && ((*ppIntNodeInfo)->nodeId == pIntNodeInfo_p->nodeId)) { // node was already added to list // $$$ d.k. maybe this should be an error goto Exit; } if (pIntNodeInfo_p->pPreqTxBuffer != NULL) { // TxBuffer entry exists tEvent event; pTxFrame = (tPlkFrame*)pIntNodeInfo_p->pPreqTxBuffer[0].pBuffer; // set up destination MAC address OPLK_MEMCPY(pTxFrame->aDstMac, pIntNodeInfo_p->aMacAddr, 6); // set destination node-ID in PReq ami_setUint8Le(&pTxFrame->dstNodeId, (UINT8)pIntNodeInfo_p->nodeId); // do the same for second frame buffer pTxFrame = (tPlkFrame*)pIntNodeInfo_p->pPreqTxBuffer[1].pBuffer; // set up destination MAC address OPLK_MEMCPY(pTxFrame->aDstMac, pIntNodeInfo_p->aMacAddr, 6); // set destination node-ID in PReq ami_setUint8Le(&pTxFrame->dstNodeId, (UINT8) pIntNodeInfo_p->nodeId); event.eventSink = kEventSinkNmtMnu; event.eventType = kEventTypeNmtMnuNodeAdded; event.eventArgSize = sizeof(pIntNodeInfo_p->nodeId); event.pEventArg = &pIntNodeInfo_p->nodeId; ret = eventk_postEvent(&event); if (ret != kErrorOk) goto Exit; } ret = errhndk_resetCnError(pIntNodeInfo_p->nodeId); } // initialize elements of internal node info structure pIntNodeInfo_p->fSoftDelete = FALSE; pIntNodeInfo_p->nmtState = kNmtCsNotActive; pIntNodeInfo_p->dllErrorEvents = 0L; // add node to list pIntNodeInfo_p->pNextNodeInfo = *ppIntNodeInfo; *ppIntNodeInfo = pIntNodeInfo_p; Exit: return ret; }