dcf5371cde
It's u16 in kernelspace, not WORD. Cc: Daniel Krueger <daniel.krueger@systec-electronic.com> Cc: Ronald Sieber <Ronald.Sieber@systec-electronic.com> Signed-off-by: Greg Kroah-Hartman <gregkh@suse.de>
670 lines
21 KiB
C
670 lines
21 KiB
C
/****************************************************************************
|
|
|
|
(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 (((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;
|
|
|
|
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);
|
|
|
|
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;
|
|
|
|
Event.m_EventSink = kEplEventSinkPdok;
|
|
Event.m_EventType = kEplEventTypePdoTx;
|
|
Event.m_uiSize = sizeof(tEplFrameInfo);
|
|
Event.m_pArg = pFrameInfo_p;
|
|
Ret = EplEventkPost(&Event);
|
|
|
|
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;
|
|
u16 wPdoSize;
|
|
u16 wBitOffset;
|
|
u16 wBitSize;
|
|
u16 wVarSize;
|
|
u64 qwObjectMapping;
|
|
u8 bMappSubindex;
|
|
u8 bObdSubindex;
|
|
u16 wObdMappIndex;
|
|
u16 wObdCommIndex;
|
|
u16 wPdoId;
|
|
u8 bObdData;
|
|
u8 bObjectCount;
|
|
u8 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; // u64
|
|
// 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 =
|
|
(u16) (qwObjectMapping &
|
|
0x000000000000FFFFLL);
|
|
bObdSubindex =
|
|
(u8) ((qwObjectMapping &
|
|
0x0000000000FF0000LL) >> 16);
|
|
wBitOffset =
|
|
(u16) ((qwObjectMapping &
|
|
0x0000FFFF00000000LL) >> 32);
|
|
wBitSize =
|
|
(u16) ((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; // u64
|
|
// 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 =
|
|
(u16) (qwObjectMapping &
|
|
0x000000000000FFFFLL);
|
|
bObdSubindex =
|
|
(u8) ((qwObjectMapping &
|
|
0x0000000000FF0000LL) >> 16);
|
|
wBitOffset =
|
|
(u16) ((qwObjectMapping &
|
|
0x0000FFFF00000000LL) >> 32);
|
|
wBitSize =
|
|
(u16) ((qwObjectMapping &
|
|
0xFFFF000000000000LL) >> 48);
|
|
|
|
// calculate max PDO size
|
|
ObdSize = wBitSize >> 3;
|
|
wVarSize = (wBitOffset >> 3) + (u16) 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
|