diff options
Diffstat (limited to 'drivers/staging/epl/EplPdok.c')
| -rw-r--r-- | drivers/staging/epl/EplPdok.c | 694 |
1 files changed, 694 insertions, 0 deletions
diff --git a/drivers/staging/epl/EplPdok.c b/drivers/staging/epl/EplPdok.c new file mode 100644 index 000000000000..15999b4f5750 --- /dev/null +++ b/drivers/staging/epl/EplPdok.c @@ -0,0 +1,694 @@ +/**************************************************************************** + + (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29 + www.systec-electronic.com + + Project: openPOWERLINK + + Description: source file for kernel PDO module + + License: + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions + are met: + + 1. Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + + 3. Neither the name of SYSTEC electronic GmbH nor the names of its + contributors may be used to endorse or promote products derived + from this software without prior written permission. For written + permission, please contact info@systec-electronic.com. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. + + Severability Clause: + + If a provision of this License is or becomes illegal, invalid or + unenforceable in any jurisdiction, that shall not affect: + 1. the validity or enforceability in that jurisdiction of any other + provision of this License; or + 2. the validity or enforceability in other jurisdictions of that or + any other provision of this License. + + ------------------------------------------------------------------------- + + $RCSfile: EplPdok.c,v $ + + $Author: D.Krueger $ + + $Revision: 1.8 $ $Date: 2008/10/17 15:32:32 $ + + $State: Exp $ + + Build Environment: + GCC V3.4 + + ------------------------------------------------------------------------- + + Revision History: + + 2006/05/22 d.k.: start of the implementation, version 1.00 + +****************************************************************************/ + +#include "kernel/EplPdok.h" +#include "kernel/EplPdokCal.h" +#include "kernel/EplEventk.h" +#include "kernel/EplObdk.h" + +#if (DEV_SYSTEM == _DEV_GNU_CF548X_) +#include "plccore.h" +#define PDO_LED 0x08 +#endif + +#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0) + +#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) == 0) + +#error 'ERROR: Missing DLLk-Modul!' + +#endif + +#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) == 0) + +#error 'ERROR: Missing OBDk-Modul!' + +#endif +/***************************************************************************/ +/* */ +/* */ +/* G L O B A L D E F I N I T I O N S */ +/* */ +/* */ +/***************************************************************************/ + +//--------------------------------------------------------------------------- +// const defines +//--------------------------------------------------------------------------- + +#define EPL_PDOK_OBD_IDX_RX_COMM_PARAM 0x1400 +#define EPL_PDOK_OBD_IDX_RX_MAPP_PARAM 0x1600 +#define EPL_PDOK_OBD_IDX_TX_COMM_PARAM 0x1800 +#define EPL_PDOK_OBD_IDX_TX_MAPP_PARAM 0x1A00 + +//--------------------------------------------------------------------------- +// local types +//--------------------------------------------------------------------------- + +//--------------------------------------------------------------------------- +// modul globale vars +//--------------------------------------------------------------------------- + +//--------------------------------------------------------------------------- +// local function prototypes +//--------------------------------------------------------------------------- + +/***************************************************************************/ +/* */ +/* */ +/* C L A S S EplPdok */ +/* */ +/* */ +/***************************************************************************/ +// +// Description: +// +// +/***************************************************************************/ + +//=========================================================================// +// // +// P R I V A T E D E F I N I T I O N S // +// // +//=========================================================================// + +//--------------------------------------------------------------------------- +// const defines +//--------------------------------------------------------------------------- + +//--------------------------------------------------------------------------- +// local types +//--------------------------------------------------------------------------- + +//--------------------------------------------------------------------------- +// local vars +//--------------------------------------------------------------------------- + +//--------------------------------------------------------------------------- +// local function prototypes +//--------------------------------------------------------------------------- + +//=========================================================================// +// // +// P U B L I C F U N C T I O N S // +// // +//=========================================================================// + +//--------------------------------------------------------------------------- +// +// Function: EplPdokAddInstance() +// +// Description: add and initialize new instance of EPL stack +// +// Parameters: none +// +// Returns: tEplKernel = error code +// +// +// State: +// +//--------------------------------------------------------------------------- + +tEplKernel EplPdokAddInstance(void) +{ + + return kEplSuccessful; +} + +//--------------------------------------------------------------------------- +// +// Function: EplPdokDelInstance() +// +// Description: deletes an instance of EPL stack +// +// Parameters: none +// +// Returns: tEplKernel = error code +// +// +// State: +// +//--------------------------------------------------------------------------- + +tEplKernel EplPdokDelInstance(void) +{ + + return kEplSuccessful; +} + +//--------------------------------------------------------------------------- +// +// Function: EplPdokCbPdoReceived +// +// Description: This function is called by DLL if PRes or PReq frame was +// received. It posts the frame to the event queue. +// It is called in states NMT_CS_READY_TO_OPERATE and NMT_CS_OPERATIONAL. +// The passed PDO needs not to be valid. +// +// Parameters: pFrameInfo_p = pointer to frame info structure +// +// Returns: tEplKernel = error code +// +// +// State: +// +//--------------------------------------------------------------------------- + +tEplKernel EplPdokCbPdoReceived(tEplFrameInfo * pFrameInfo_p) +{ + tEplKernel Ret = kEplSuccessful; + tEplEvent Event; + +#if (DEV_SYSTEM == _DEV_GNU_CF548X_) + // 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); + +#if (DEV_SYSTEM == _DEV_GNU_CF548X_) + // set LED +// MCF_GPIO_PODR_PCIBG |= PDO_LED; // Level +#endif + + return Ret; +} + +//--------------------------------------------------------------------------- +// +// Function: EplPdokCbPdoTransmitted +// +// Description: This function is called by DLL if PRes or PReq frame was +// sent. It posts the pointer to the frame to the event queue. +// It is called in NMT_CS_PRE_OPERATIONAL_2, +// NMT_CS_READY_TO_OPERATE and NMT_CS_OPERATIONAL. +// +// Parameters: pFrameInfo_p = pointer to frame info structure +// +// Returns: tEplKernel = error code +// +// +// State: +// +//--------------------------------------------------------------------------- + +tEplKernel EplPdokCbPdoTransmitted(tEplFrameInfo * pFrameInfo_p) +{ + tEplKernel Ret = kEplSuccessful; + tEplEvent Event; + +#if (DEV_SYSTEM == _DEV_GNU_CF548X_) + // 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); + +#if (DEV_SYSTEM == _DEV_GNU_CF548X_) + // set LED + MCF_GPIO_PODR_PCIBG |= PDO_LED; // Level +#endif + + return Ret; +} + +//--------------------------------------------------------------------------- +// +// Function: EplPdokCbSoa +// +// Description: This function is called by DLL if SoA frame was +// received resp. sent. It posts this event to the event queue. +// +// Parameters: pFrameInfo_p = pointer to frame info structure +// +// Returns: tEplKernel = error code +// +// +// State: +// +//--------------------------------------------------------------------------- + +tEplKernel EplPdokCbSoa(tEplFrameInfo * pFrameInfo_p) +{ + tEplKernel Ret = kEplSuccessful; + tEplEvent Event; + + Event.m_EventSink = kEplEventSinkPdok; + Event.m_EventType = kEplEventTypePdoSoa; + Event.m_uiSize = 0; + Event.m_pArg = NULL; + Ret = EplEventkPost(&Event); + + return Ret; +} + +//--------------------------------------------------------------------------- +// +// Function: EplPdokProcess +// +// Description: This function processes all received and transmitted PDOs. +// This function must not be interrupted by any other task +// except ISRs (like the ethernet driver ISR, which may call +// EplPdokCbFrameReceived() or EplPdokCbFrameTransmitted()). +// +// Parameters: pEvent_p = pointer to event structure +// +// Returns: tEplKernel = error code +// +// +// State: +// +//--------------------------------------------------------------------------- + +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; +} + +//=========================================================================// +// // +// P R I V A T E F U N C T I O N S // +// // +//=========================================================================// + +//--------------------------------------------------------------------------- +// +// Function: +// +// Description: +// +// +// +// Parameters: +// +// +// Returns: +// +// +// State: +// +//--------------------------------------------------------------------------- + +#endif // #if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0) + +// EOF |