diff options
Diffstat (limited to 'drivers/staging/epl/EplPdok.c')
-rw-r--r-- | drivers/staging/epl/EplPdok.c | 718 |
1 files changed, 360 insertions, 358 deletions
diff --git a/drivers/staging/epl/EplPdok.c b/drivers/staging/epl/EplPdok.c index 83c2f29..15999b4 100644 --- a/drivers/staging/epl/EplPdok.c +++ b/drivers/staging/epl/EplPdok.c @@ -82,13 +82,13 @@ #if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) == 0) - #error 'ERROR: Missing DLLk-Modul!' +#error 'ERROR: Missing DLLk-Modul!' #endif #if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) == 0) - #error 'ERROR: Missing OBDk-Modul!' +#error 'ERROR: Missing OBDk-Modul!' #endif /***************************************************************************/ @@ -120,7 +120,6 @@ // local function prototypes //--------------------------------------------------------------------------- - /***************************************************************************/ /* */ /* */ @@ -134,7 +133,6 @@ // /***************************************************************************/ - //=========================================================================// // // // P R I V A T E D E F I N I T I O N S // @@ -153,12 +151,10 @@ // local vars //--------------------------------------------------------------------------- - //--------------------------------------------------------------------------- // local function prototypes //--------------------------------------------------------------------------- - //=========================================================================// // // // P U B L I C F U N C T I O N S // @@ -183,7 +179,7 @@ tEplKernel EplPdokAddInstance(void) { - return kEplSuccessful; + return kEplSuccessful; } //--------------------------------------------------------------------------- @@ -204,10 +200,9 @@ tEplKernel EplPdokAddInstance(void) tEplKernel EplPdokDelInstance(void) { - return kEplSuccessful; + return kEplSuccessful; } - //--------------------------------------------------------------------------- // // Function: EplPdokCbPdoReceived @@ -228,27 +223,27 @@ tEplKernel EplPdokDelInstance(void) tEplKernel EplPdokCbPdoReceived(tEplFrameInfo * pFrameInfo_p) { -tEplKernel Ret = kEplSuccessful; -tEplEvent Event; + tEplKernel Ret = kEplSuccessful; + tEplEvent Event; #if (DEV_SYSTEM == _DEV_GNU_CF548X_) - // reset LED + // reset LED // MCF_GPIO_PODR_PCIBG &= ~PDO_LED; // Level #endif - Event.m_EventSink = kEplEventSinkPdok; - Event.m_EventType = kEplEventTypePdoRx; - // limit copied data to size of PDO (because from some CNs the frame is larger than necessary) - Event.m_uiSize = AmiGetWordFromLe(&pFrameInfo_p->m_pFrame->m_Data.m_Pres.m_le_wSize) + 24; // pFrameInfo_p->m_uiFrameSize; - Event.m_pArg = pFrameInfo_p->m_pFrame; - Ret = EplEventkPost(&Event); + Event.m_EventSink = kEplEventSinkPdok; + Event.m_EventType = kEplEventTypePdoRx; + // limit copied data to size of PDO (because from some CNs the frame is larger than necessary) + Event.m_uiSize = AmiGetWordFromLe(&pFrameInfo_p->m_pFrame->m_Data.m_Pres.m_le_wSize) + 24; // pFrameInfo_p->m_uiFrameSize; + Event.m_pArg = pFrameInfo_p->m_pFrame; + Ret = EplEventkPost(&Event); #if (DEV_SYSTEM == _DEV_GNU_CF548X_) - // set LED + // set LED // MCF_GPIO_PODR_PCIBG |= PDO_LED; // Level #endif - return Ret; + return Ret; } //--------------------------------------------------------------------------- @@ -271,26 +266,26 @@ tEplEvent Event; tEplKernel EplPdokCbPdoTransmitted(tEplFrameInfo * pFrameInfo_p) { -tEplKernel Ret = kEplSuccessful; -tEplEvent Event; + tEplKernel Ret = kEplSuccessful; + tEplEvent Event; #if (DEV_SYSTEM == _DEV_GNU_CF548X_) - // reset LED - MCF_GPIO_PODR_PCIBG &= ~PDO_LED; // Level + // reset LED + MCF_GPIO_PODR_PCIBG &= ~PDO_LED; // Level #endif - Event.m_EventSink = kEplEventSinkPdok; - Event.m_EventType = kEplEventTypePdoTx; - Event.m_uiSize = sizeof (tEplFrameInfo); - Event.m_pArg = pFrameInfo_p; - Ret = EplEventkPost(&Event); + Event.m_EventSink = kEplEventSinkPdok; + Event.m_EventType = kEplEventTypePdoTx; + Event.m_uiSize = sizeof(tEplFrameInfo); + Event.m_pArg = pFrameInfo_p; + Ret = EplEventkPost(&Event); #if (DEV_SYSTEM == _DEV_GNU_CF548X_) - // set LED - MCF_GPIO_PODR_PCIBG |= PDO_LED; // Level + // set LED + MCF_GPIO_PODR_PCIBG |= PDO_LED; // Level #endif - return Ret; + return Ret; } //--------------------------------------------------------------------------- @@ -311,16 +306,16 @@ tEplEvent Event; tEplKernel EplPdokCbSoa(tEplFrameInfo * pFrameInfo_p) { -tEplKernel Ret = kEplSuccessful; -tEplEvent Event; + tEplKernel Ret = kEplSuccessful; + tEplEvent Event; - Event.m_EventSink = kEplEventSinkPdok; - Event.m_EventType = kEplEventTypePdoSoa; - Event.m_uiSize = 0; - Event.m_pArg = NULL; - Ret = EplEventkPost(&Event); + Event.m_EventSink = kEplEventSinkPdok; + Event.m_EventType = kEplEventTypePdoSoa; + Event.m_uiSize = 0; + Event.m_pArg = NULL; + Ret = EplEventkPost(&Event); - return Ret; + return Ret; } //--------------------------------------------------------------------------- @@ -343,323 +338,331 @@ tEplEvent Event; tEplKernel EplPdokProcess(tEplEvent * pEvent_p) { -tEplKernel Ret = kEplSuccessful; -WORD wPdoSize; -WORD wBitOffset; -WORD wBitSize; -WORD wVarSize; -QWORD qwObjectMapping; -BYTE bMappSubindex; -BYTE bObdSubindex; -WORD wObdMappIndex; -WORD wObdCommIndex; -WORD wPdoId; -BYTE bObdData; -BYTE bObjectCount; -BYTE bFrameData; -BOOL fValid; -tEplObdSize ObdSize; -tEplFrame *pFrame; -tEplFrameInfo *pFrameInfo; -unsigned int uiNodeId; -tEplMsgType MsgType; - - // 0xFF=invalid, RPDO: 0x00=PReq, localNodeId=PRes, remoteNodeId=PRes - // TPDO: 0x00=PRes, MN: CnNodeId=PReq - - switch (pEvent_p->m_EventType) - { - case kEplEventTypePdoRx: // RPDO received - pFrame = (tEplFrame *) pEvent_p->m_pArg; - - // check if received RPDO is valid - bFrameData = AmiGetByteFromLe(&pFrame->m_Data.m_Pres.m_le_bFlag1); - if ((bFrameData & EPL_FRAME_FLAG1_RD) == 0) - { // RPDO invalid - goto Exit; - } - - // retrieve EPL message type - MsgType = AmiGetByteFromLe(&pFrame->m_le_bMessageType); - if (MsgType == kEplMsgTypePreq) - { // RPDO is PReq frame - uiNodeId = EPL_PDO_PREQ_NODE_ID; // 0x00 - } - else - { // RPDO is PRes frame - // retrieve node ID - uiNodeId = AmiGetByteFromLe(&pFrame->m_le_bSrcNodeId); - } - - // search for appropriate valid RPDO in OD - wObdMappIndex = EPL_PDOK_OBD_IDX_RX_MAPP_PARAM; - for (wObdCommIndex = EPL_PDOK_OBD_IDX_RX_COMM_PARAM; - wObdCommIndex < (EPL_PDOK_OBD_IDX_RX_COMM_PARAM + 0x00FF); - wObdCommIndex++, wObdMappIndex++) - { - ObdSize = 1; - // read node ID from OD - Ret = EplObdReadEntry(wObdCommIndex, 0x01, &bObdData, &ObdSize); - if ((Ret == kEplObdIndexNotExist) - || (Ret == kEplObdSubindexNotExist) - || (Ret == kEplObdIllegalPart)) - { // PDO does not exist; last PDO reached - Ret = kEplSuccessful; - goto Exit; - } - else if (Ret != kEplSuccessful) - { // other fatal error occured - goto Exit; - } - // entry read successfully - if (bObdData != uiNodeId) - { // node ID does not equal - wrong PDO, try next PDO in OD - continue; - } - ObdSize = 1; - // read number of mapped objects from OD; this indicates if the PDO is valid - Ret = EplObdReadEntry(wObdMappIndex, 0x00, &bObjectCount, &ObdSize); - if ((Ret == kEplObdIndexNotExist) - || (Ret == kEplObdSubindexNotExist) - || (Ret == kEplObdIllegalPart)) - { // PDO does not exist; last PDO reached - Ret = kEplSuccessful; - goto Exit; - } - else if (Ret != kEplSuccessful) - { // other fatal error occured - goto Exit; - } - // entry read successfully - if (bObjectCount == 0) - { // PDO in OD not valid, try next PDO in OD - continue; - } - - ObdSize = 1; - // check PDO mapping version - Ret = EplObdReadEntry(wObdCommIndex, 0x02, &bObdData, &ObdSize); - if (Ret != kEplSuccessful) - { // other fatal error occured - goto Exit; - } - // entry read successfully - // retrieve PDO version from frame - bFrameData = AmiGetByteFromLe(&pFrame->m_Data.m_Pres.m_le_bPdoVersion); - if ((bObdData & EPL_VERSION_MAIN) != (bFrameData & EPL_VERSION_MAIN)) - { // PDO versions do not match - // $$$ raise PDO error - // termiate processing of this RPDO - goto Exit; - } - - // valid RPDO found - - // retrieve PDO size - wPdoSize = AmiGetWordFromLe(&pFrame->m_Data.m_Pres.m_le_wSize); - - // process mapping - for (bMappSubindex = 1; bMappSubindex <= bObjectCount; bMappSubindex++) - { - ObdSize = 8; // QWORD - // read object mapping from OD - Ret = EplObdReadEntry(wObdMappIndex, bMappSubindex, &qwObjectMapping, &ObdSize); - if (Ret != kEplSuccessful) - { // other fatal error occured - goto Exit; - } - - // check if object mapping entry is valid, i.e. unequal zero, because "empty" entries are allowed - if (qwObjectMapping == 0) - { // invalid entry, continue with next entry - continue; - } - - // decode object mapping - wObdCommIndex = (WORD) (qwObjectMapping & 0x000000000000FFFFLL); - bObdSubindex = (BYTE) ((qwObjectMapping & 0x0000000000FF0000LL) >> 16); - wBitOffset = (WORD) ((qwObjectMapping & 0x0000FFFF00000000LL) >> 32); - wBitSize = (WORD) ((qwObjectMapping & 0xFFFF000000000000LL) >> 48); - - // check if object exceeds PDO size - if (((wBitOffset + wBitSize) >> 3) > wPdoSize) - { // wrong object mapping; PDO size is too low - // $$$ raise PDO error - // terminate processing of this RPDO - goto Exit; - } - - // copy object from RPDO to process/OD variable - ObdSize = wBitSize >> 3; - Ret = EplObdWriteEntryFromLe(wObdCommIndex, bObdSubindex, &pFrame->m_Data.m_Pres.m_le_abPayload[(wBitOffset >> 3)], ObdSize); - if (Ret != kEplSuccessful) - { // other fatal error occured - goto Exit; - } - - } - - // processing finished successfully - goto Exit; - } - break; - - case kEplEventTypePdoTx: // TPDO transmitted - pFrameInfo = (tEplFrameInfo *) pEvent_p->m_pArg; - pFrame = pFrameInfo->m_pFrame; - - // set TPDO invalid, so that only fully processed TPDOs are sent as valid - bFrameData = AmiGetByteFromLe(&pFrame->m_Data.m_Pres.m_le_bFlag1); - AmiSetByteToLe(&pFrame->m_Data.m_Pres.m_le_bFlag1, (bFrameData & ~EPL_FRAME_FLAG1_RD)); - - // retrieve EPL message type - MsgType = AmiGetByteFromLe(&pFrame->m_le_bMessageType); - if (MsgType == kEplMsgTypePres) - { // TPDO is PRes frame - uiNodeId = EPL_PDO_PRES_NODE_ID; // 0x00 - } - else - { // TPDO is PReq frame - // retrieve node ID - uiNodeId = AmiGetByteFromLe(&pFrame->m_le_bDstNodeId); - } - - // search for appropriate valid TPDO in OD - wObdMappIndex = EPL_PDOK_OBD_IDX_TX_MAPP_PARAM; - wObdCommIndex = EPL_PDOK_OBD_IDX_TX_COMM_PARAM; - for (wPdoId = 0; ; wPdoId++, wObdCommIndex++, wObdMappIndex++) - { - ObdSize = 1; - // read node ID from OD - Ret = EplObdReadEntry(wObdCommIndex, 0x01, &bObdData, &ObdSize); - if ((Ret == kEplObdIndexNotExist) - || (Ret == kEplObdSubindexNotExist) - || (Ret == kEplObdIllegalPart)) - { // PDO does not exist; last PDO reached - Ret = kEplSuccessful; - goto Exit; - } - else if (Ret != kEplSuccessful) - { // other fatal error occured - goto Exit; - } - // entry read successfully - if (bObdData != uiNodeId) - { // node ID does not equal - wrong PDO, try next PDO in OD - continue; - } - ObdSize = 1; - // read number of mapped objects from OD; this indicates if the PDO is valid - Ret = EplObdReadEntry(wObdMappIndex, 0x00, &bObjectCount, &ObdSize); - if ((Ret == kEplObdIndexNotExist) - || (Ret == kEplObdSubindexNotExist) - || (Ret == kEplObdIllegalPart)) - { // PDO does not exist; last PDO reached - Ret = kEplSuccessful; - goto Exit; - } - else if (Ret != kEplSuccessful) - { // other fatal error occured - goto Exit; - } - // entry read successfully - if (bObjectCount == 0) - { // PDO in OD not valid, try next PDO in OD - continue; - } - - // valid TPDO found - - ObdSize = 1; - // get PDO mapping version from OD - Ret = EplObdReadEntry(wObdCommIndex, 0x02, &bObdData, &ObdSize); - if (Ret != kEplSuccessful) - { // other fatal error occured - goto Exit; - } - // entry read successfully - // set PDO version in frame - AmiSetByteToLe(&pFrame->m_Data.m_Pres.m_le_bPdoVersion, bObdData); - - // calculate PDO size - wPdoSize = 0; - - // process mapping - for (bMappSubindex = 1; bMappSubindex <= bObjectCount; bMappSubindex++) - { - ObdSize = 8; // QWORD - // read object mapping from OD - Ret = EplObdReadEntry(wObdMappIndex, bMappSubindex, &qwObjectMapping, &ObdSize); - if (Ret != kEplSuccessful) - { // other fatal error occured - goto Exit; - } - - // check if object mapping entry is valid, i.e. unequal zero, because "empty" entries are allowed - if (qwObjectMapping == 0) - { // invalid entry, continue with next entry - continue; - } - - // decode object mapping - wObdCommIndex = (WORD) (qwObjectMapping & 0x000000000000FFFFLL); - bObdSubindex = (BYTE) ((qwObjectMapping & 0x0000000000FF0000LL) >> 16); - wBitOffset = (WORD) ((qwObjectMapping & 0x0000FFFF00000000LL) >> 32); - wBitSize = (WORD) ((qwObjectMapping & 0xFFFF000000000000LL) >> 48); - - // calculate max PDO size - ObdSize = wBitSize >> 3; - wVarSize = (wBitOffset >> 3) + (WORD) ObdSize; - if ((unsigned int)(wVarSize + 24) > pFrameInfo->m_uiFrameSize) - { // TPDO is too short - // $$$ raise PDO error, set Ret - goto Exit; - } - if (wVarSize > wPdoSize) - { // memorize new PDO size - wPdoSize = wVarSize; - } - - // copy object from process/OD variable to TPDO - Ret = EplObdReadEntryToLe(wObdCommIndex, bObdSubindex, &pFrame->m_Data.m_Pres.m_le_abPayload[(wBitOffset >> 3)], &ObdSize); - if (Ret != kEplSuccessful) - { // other fatal error occured - goto Exit; - } - - } - - // set PDO size in frame - AmiSetWordToLe(&pFrame->m_Data.m_Pres.m_le_wSize, wPdoSize); - - Ret = EplPdokCalAreTpdosValid(&fValid); - if (fValid != FALSE) - { - // set TPDO valid - bFrameData = AmiGetByteFromLe(&pFrame->m_Data.m_Pres.m_le_bFlag1); - AmiSetByteToLe(&pFrame->m_Data.m_Pres.m_le_bFlag1, (bFrameData | EPL_FRAME_FLAG1_RD)); - } - - // processing finished successfully - - goto Exit; - } - break; - - case kEplEventTypePdoSoa: // SoA received - - // invalidate TPDOs - Ret = EplPdokCalSetTpdosValid(FALSE); - break; - - default: - { - ASSERTMSG(FALSE, "EplPdokProcess(): unhandled event type!\n"); - } - } - -Exit: - return Ret; + tEplKernel Ret = kEplSuccessful; + WORD wPdoSize; + WORD wBitOffset; + WORD wBitSize; + WORD wVarSize; + QWORD qwObjectMapping; + BYTE bMappSubindex; + BYTE bObdSubindex; + WORD wObdMappIndex; + WORD wObdCommIndex; + WORD wPdoId; + BYTE bObdData; + BYTE bObjectCount; + BYTE bFrameData; + BOOL fValid; + tEplObdSize ObdSize; + tEplFrame *pFrame; + tEplFrameInfo *pFrameInfo; + unsigned int uiNodeId; + tEplMsgType MsgType; + + // 0xFF=invalid, RPDO: 0x00=PReq, localNodeId=PRes, remoteNodeId=PRes + // TPDO: 0x00=PRes, MN: CnNodeId=PReq + + switch (pEvent_p->m_EventType) { + case kEplEventTypePdoRx: // RPDO received + pFrame = (tEplFrame *) pEvent_p->m_pArg; + + // check if received RPDO is valid + bFrameData = + AmiGetByteFromLe(&pFrame->m_Data.m_Pres.m_le_bFlag1); + if ((bFrameData & EPL_FRAME_FLAG1_RD) == 0) { // RPDO invalid + goto Exit; + } + // retrieve EPL message type + MsgType = AmiGetByteFromLe(&pFrame->m_le_bMessageType); + if (MsgType == kEplMsgTypePreq) { // RPDO is PReq frame + uiNodeId = EPL_PDO_PREQ_NODE_ID; // 0x00 + } else { // RPDO is PRes frame + // retrieve node ID + uiNodeId = AmiGetByteFromLe(&pFrame->m_le_bSrcNodeId); + } + + // search for appropriate valid RPDO in OD + wObdMappIndex = EPL_PDOK_OBD_IDX_RX_MAPP_PARAM; + for (wObdCommIndex = EPL_PDOK_OBD_IDX_RX_COMM_PARAM; + wObdCommIndex < (EPL_PDOK_OBD_IDX_RX_COMM_PARAM + 0x00FF); + wObdCommIndex++, wObdMappIndex++) { + ObdSize = 1; + // read node ID from OD + Ret = + EplObdReadEntry(wObdCommIndex, 0x01, &bObdData, + &ObdSize); + if ((Ret == kEplObdIndexNotExist) + || (Ret == kEplObdSubindexNotExist) + || (Ret == kEplObdIllegalPart)) { // PDO does not exist; last PDO reached + Ret = kEplSuccessful; + goto Exit; + } else if (Ret != kEplSuccessful) { // other fatal error occured + goto Exit; + } + // entry read successfully + if (bObdData != uiNodeId) { // node ID does not equal - wrong PDO, try next PDO in OD + continue; + } + ObdSize = 1; + // read number of mapped objects from OD; this indicates if the PDO is valid + Ret = + EplObdReadEntry(wObdMappIndex, 0x00, &bObjectCount, + &ObdSize); + if ((Ret == kEplObdIndexNotExist) + || (Ret == kEplObdSubindexNotExist) + || (Ret == kEplObdIllegalPart)) { // PDO does not exist; last PDO reached + Ret = kEplSuccessful; + goto Exit; + } else if (Ret != kEplSuccessful) { // other fatal error occured + goto Exit; + } + // entry read successfully + if (bObjectCount == 0) { // PDO in OD not valid, try next PDO in OD + continue; + } + + ObdSize = 1; + // check PDO mapping version + Ret = + EplObdReadEntry(wObdCommIndex, 0x02, &bObdData, + &ObdSize); + if (Ret != kEplSuccessful) { // other fatal error occured + goto Exit; + } + // entry read successfully + // retrieve PDO version from frame + bFrameData = + AmiGetByteFromLe(&pFrame->m_Data.m_Pres. + m_le_bPdoVersion); + if ((bObdData & EPL_VERSION_MAIN) != (bFrameData & EPL_VERSION_MAIN)) { // PDO versions do not match + // $$$ raise PDO error + // termiate processing of this RPDO + goto Exit; + } + // valid RPDO found + + // retrieve PDO size + wPdoSize = + AmiGetWordFromLe(&pFrame->m_Data.m_Pres.m_le_wSize); + + // process mapping + for (bMappSubindex = 1; bMappSubindex <= bObjectCount; + bMappSubindex++) { + ObdSize = 8; // QWORD + // read object mapping from OD + Ret = + EplObdReadEntry(wObdMappIndex, + bMappSubindex, + &qwObjectMapping, &ObdSize); + if (Ret != kEplSuccessful) { // other fatal error occured + goto Exit; + } + // check if object mapping entry is valid, i.e. unequal zero, because "empty" entries are allowed + if (qwObjectMapping == 0) { // invalid entry, continue with next entry + continue; + } + // decode object mapping + wObdCommIndex = + (WORD) (qwObjectMapping & + 0x000000000000FFFFLL); + bObdSubindex = + (BYTE) ((qwObjectMapping & + 0x0000000000FF0000LL) >> 16); + wBitOffset = + (WORD) ((qwObjectMapping & + 0x0000FFFF00000000LL) >> 32); + wBitSize = + (WORD) ((qwObjectMapping & + 0xFFFF000000000000LL) >> 48); + + // check if object exceeds PDO size + if (((wBitOffset + wBitSize) >> 3) > wPdoSize) { // wrong object mapping; PDO size is too low + // $$$ raise PDO error + // terminate processing of this RPDO + goto Exit; + } + // copy object from RPDO to process/OD variable + ObdSize = wBitSize >> 3; + Ret = + EplObdWriteEntryFromLe(wObdCommIndex, + bObdSubindex, + &pFrame->m_Data. + m_Pres. + m_le_abPayload[(wBitOffset >> 3)], ObdSize); + if (Ret != kEplSuccessful) { // other fatal error occured + goto Exit; + } + + } + + // processing finished successfully + goto Exit; + } + break; + + case kEplEventTypePdoTx: // TPDO transmitted + pFrameInfo = (tEplFrameInfo *) pEvent_p->m_pArg; + pFrame = pFrameInfo->m_pFrame; + + // set TPDO invalid, so that only fully processed TPDOs are sent as valid + bFrameData = + AmiGetByteFromLe(&pFrame->m_Data.m_Pres.m_le_bFlag1); + AmiSetByteToLe(&pFrame->m_Data.m_Pres.m_le_bFlag1, + (bFrameData & ~EPL_FRAME_FLAG1_RD)); + + // retrieve EPL message type + MsgType = AmiGetByteFromLe(&pFrame->m_le_bMessageType); + if (MsgType == kEplMsgTypePres) { // TPDO is PRes frame + uiNodeId = EPL_PDO_PRES_NODE_ID; // 0x00 + } else { // TPDO is PReq frame + // retrieve node ID + uiNodeId = AmiGetByteFromLe(&pFrame->m_le_bDstNodeId); + } + + // search for appropriate valid TPDO in OD + wObdMappIndex = EPL_PDOK_OBD_IDX_TX_MAPP_PARAM; + wObdCommIndex = EPL_PDOK_OBD_IDX_TX_COMM_PARAM; + for (wPdoId = 0;; wPdoId++, wObdCommIndex++, wObdMappIndex++) { + ObdSize = 1; + // read node ID from OD + Ret = + EplObdReadEntry(wObdCommIndex, 0x01, &bObdData, + &ObdSize); + if ((Ret == kEplObdIndexNotExist) + || (Ret == kEplObdSubindexNotExist) + || (Ret == kEplObdIllegalPart)) { // PDO does not exist; last PDO reached + Ret = kEplSuccessful; + goto Exit; + } else if (Ret != kEplSuccessful) { // other fatal error occured + goto Exit; + } + // entry read successfully + if (bObdData != uiNodeId) { // node ID does not equal - wrong PDO, try next PDO in OD + continue; + } + ObdSize = 1; + // read number of mapped objects from OD; this indicates if the PDO is valid + Ret = + EplObdReadEntry(wObdMappIndex, 0x00, &bObjectCount, + &ObdSize); + if ((Ret == kEplObdIndexNotExist) + || (Ret == kEplObdSubindexNotExist) + || (Ret == kEplObdIllegalPart)) { // PDO does not exist; last PDO reached + Ret = kEplSuccessful; + goto Exit; + } else if (Ret != kEplSuccessful) { // other fatal error occured + goto Exit; + } + // entry read successfully + if (bObjectCount == 0) { // PDO in OD not valid, try next PDO in OD + continue; + } + // valid TPDO found + + ObdSize = 1; + // get PDO mapping version from OD + Ret = + EplObdReadEntry(wObdCommIndex, 0x02, &bObdData, + &ObdSize); + if (Ret != kEplSuccessful) { // other fatal error occured + goto Exit; + } + // entry read successfully + // set PDO version in frame + AmiSetByteToLe(&pFrame->m_Data.m_Pres.m_le_bPdoVersion, + bObdData); + + // calculate PDO size + wPdoSize = 0; + + // process mapping + for (bMappSubindex = 1; bMappSubindex <= bObjectCount; + bMappSubindex++) { + ObdSize = 8; // QWORD + // read object mapping from OD + Ret = + EplObdReadEntry(wObdMappIndex, + bMappSubindex, + &qwObjectMapping, &ObdSize); + if (Ret != kEplSuccessful) { // other fatal error occured + goto Exit; + } + // check if object mapping entry is valid, i.e. unequal zero, because "empty" entries are allowed + if (qwObjectMapping == 0) { // invalid entry, continue with next entry + continue; + } + // decode object mapping + wObdCommIndex = + (WORD) (qwObjectMapping & + 0x000000000000FFFFLL); + bObdSubindex = + (BYTE) ((qwObjectMapping & + 0x0000000000FF0000LL) >> 16); + wBitOffset = + (WORD) ((qwObjectMapping & + 0x0000FFFF00000000LL) >> 32); + wBitSize = + (WORD) ((qwObjectMapping & + 0xFFFF000000000000LL) >> 48); + + // calculate max PDO size + ObdSize = wBitSize >> 3; + wVarSize = (wBitOffset >> 3) + (WORD) ObdSize; + if ((unsigned int)(wVarSize + 24) > pFrameInfo->m_uiFrameSize) { // TPDO is too short + // $$$ raise PDO error, set Ret + goto Exit; + } + if (wVarSize > wPdoSize) { // memorize new PDO size + wPdoSize = wVarSize; + } + // copy object from process/OD variable to TPDO + Ret = + EplObdReadEntryToLe(wObdCommIndex, + bObdSubindex, + &pFrame->m_Data.m_Pres. + m_le_abPayload[(wBitOffset >> 3)], &ObdSize); + if (Ret != kEplSuccessful) { // other fatal error occured + goto Exit; + } + + } + + // set PDO size in frame + AmiSetWordToLe(&pFrame->m_Data.m_Pres.m_le_wSize, + wPdoSize); + + Ret = EplPdokCalAreTpdosValid(&fValid); + if (fValid != FALSE) { + // set TPDO valid + bFrameData = + AmiGetByteFromLe(&pFrame->m_Data.m_Pres. + m_le_bFlag1); + AmiSetByteToLe(&pFrame->m_Data.m_Pres. + m_le_bFlag1, + (bFrameData | + EPL_FRAME_FLAG1_RD)); + } + // processing finished successfully + + goto Exit; + } + break; + + case kEplEventTypePdoSoa: // SoA received + + // invalidate TPDOs + Ret = EplPdokCalSetTpdosValid(FALSE); + break; + + default: + { + ASSERTMSG(FALSE, + "EplPdokProcess(): unhandled event type!\n"); + } + } + + Exit: + return Ret; } //=========================================================================// @@ -689,4 +692,3 @@ Exit: #endif // #if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0) // EOF - |