tEplKernel EplDlluProcess(tEplFrameInfo * pFrameInfo_p)
{
tEplKernel Ret = kEplSuccessful;
tEplMsgType MsgType;
unsigned int uiAsndServiceId;
MsgType = (tEplMsgType)AmiGetByteFromLe(&pFrameInfo_p->m_pFrame->m_le_bMessageType);
if (MsgType != kEplMsgTypeAsnd)
{
Ret = kEplInvalidOperation; // $$$ kEplDllInvalidFrame
goto Exit;
}
uiAsndServiceId = (unsigned int) AmiGetByteFromLe(&pFrameInfo_p->m_pFrame->m_Data.m_Asnd.m_le_bServiceId);
if (uiAsndServiceId < EPL_DLL_MAX_ASND_SERVICE_ID)
{ // ASnd service ID is valid
if (EplDlluInstance_g.m_apfnDlluCbAsnd[uiAsndServiceId] != NULL)
{ // handler was registered
Ret = EplDlluInstance_g.m_apfnDlluCbAsnd[uiAsndServiceId](pFrameInfo_p);
}
}
Exit:
return Ret;
}
//---------------------------------------------------------------------------
//
// Function: EplStatusuCbStatusResponse
//
// Description: callback funktion for StatusResponse
//
//
//
// Parameters: pFrameInfo_p = Frame with the StatusResponse
//
//
// Returns: tEplKernel = error code
//
//
// State:
//
//---------------------------------------------------------------------------
static tEplKernel PUBLIC EplStatusuCbStatusResponse(tEplFrameInfo *
pFrameInfo_p)
{
tEplKernel Ret = kEplSuccessful;
unsigned int uiNodeId;
unsigned int uiIndex;
tEplStatusuCbResponse pfnCbResponse;
uiNodeId = AmiGetByteFromLe(&pFrameInfo_p->m_pFrame->m_le_bSrcNodeId);
uiIndex = uiNodeId - 1;
if (uiIndex < tabentries(EplStatusuInstance_g.m_apfnCbResponse)) {
// memorize pointer to callback function
pfnCbResponse = EplStatusuInstance_g.m_apfnCbResponse[uiIndex];
if (pfnCbResponse == NULL) { // response was not requested
goto Exit;
}
// reset callback function pointer so that caller may issue next request
EplStatusuInstance_g.m_apfnCbResponse[uiIndex] = NULL;
if (pFrameInfo_p->m_uiFrameSize < EPL_C_DLL_MINSIZE_STATUSRES) { // StatusResponse not received or it has invalid size
Ret = pfnCbResponse(uiNodeId, NULL);
} else { // StatusResponse received
Ret =
pfnCbResponse(uiNodeId,
&pFrameInfo_p->m_pFrame->m_Data.
m_Asnd.m_Payload.m_StatusResponse);
}
}
Exit:
return Ret;
}
static tEplKernel EplIdentuCbIdentResponse(tEplFrameInfo *pFrameInfo_p)
{
tEplKernel Ret = kEplSuccessful;
unsigned int uiNodeId;
unsigned int uiIndex;
tEplIdentuCbResponse pfnCbResponse;
uiNodeId = AmiGetByteFromLe(&pFrameInfo_p->m_pFrame->m_le_bSrcNodeId);
uiIndex = uiNodeId - 1;
if (uiIndex < tabentries(EplIdentuInstance_g.m_apfnCbResponse)) {
// memorize pointer to callback function
pfnCbResponse = EplIdentuInstance_g.m_apfnCbResponse[uiIndex];
// reset callback function pointer so that caller may issue next request immediately
EplIdentuInstance_g.m_apfnCbResponse[uiIndex] = NULL;
if (pFrameInfo_p->m_uiFrameSize < EPL_C_DLL_MINSIZE_IDENTRES) { // IdentResponse not received or it has invalid size
if (pfnCbResponse == NULL) { // response was not requested
goto Exit;
}
Ret = pfnCbResponse(uiNodeId, NULL);
} else { // IdentResponse received
if (EplIdentuInstance_g.m_apIdentResponse[uiIndex] == NULL) { // memory for IdentResponse must be allocated
EplIdentuInstance_g.m_apIdentResponse[uiIndex] =
EPL_MALLOC(sizeof(tEplIdentResponse));
if (EplIdentuInstance_g.m_apIdentResponse[uiIndex] == NULL) { // malloc failed
if (pfnCbResponse == NULL) { // response was not requested
goto Exit;
}
Ret =
pfnCbResponse(uiNodeId,
&pFrameInfo_p->
m_pFrame->m_Data.
m_Asnd.m_Payload.
m_IdentResponse);
goto Exit;
}
}
// copy IdentResponse to instance structure
EPL_MEMCPY(EplIdentuInstance_g.
m_apIdentResponse[uiIndex],
&pFrameInfo_p->m_pFrame->m_Data.m_Asnd.
m_Payload.m_IdentResponse,
sizeof(tEplIdentResponse));
if (pfnCbResponse == NULL) { // response was not requested
goto Exit;
}
Ret =
pfnCbResponse(uiNodeId,
EplIdentuInstance_g.
m_apIdentResponse[uiIndex]);
}
}
Exit:
return Ret;
}
//---------------------------------------------------------------------------
//
// Function: EplSdoAsnduCb
//
// Description: callback function for SDO ASnd frames
//
//
//
// Parameters: pFrameInfo_p = Frame with SDO payload
//
//
// Returns: tEplKernel = errorcode
//
//
// State:
//
//---------------------------------------------------------------------------
tEplKernel PUBLIC EplSdoAsnduCb(tEplFrameInfo * pFrameInfo_p)
{
tEplKernel Ret = kEplSuccessful;
unsigned int uiCount;
unsigned int* puiConnection;
unsigned int uiNodeId;
unsigned int uiFreeEntry = 0xFFFF;
tEplSdoConHdl SdoConHdl;
tEplFrame* pFrame;
pFrame = pFrameInfo_p->m_pFrame;
uiNodeId = AmiGetByteFromLe(&pFrame->m_le_bSrcNodeId);
// search corresponding entry in control structure
uiCount = 0;
puiConnection = &SdoAsndInstance_g.m_auiSdoAsndConnection[0];
while (uiCount < EPL_SDO_MAX_CONNECTION_ASND)
{
if (uiNodeId == *puiConnection)
{
break;
}
else if ((*puiConnection == 0)
&& (uiFreeEntry == 0xFFFF))
{ // free entry
uiFreeEntry = uiCount;
}
uiCount++;
puiConnection++;
}
if (uiCount == EPL_SDO_MAX_CONNECTION_ASND)
{
if (uiFreeEntry != 0xFFFF)
{
puiConnection = &SdoAsndInstance_g.m_auiSdoAsndConnection[uiFreeEntry];
*puiConnection = uiNodeId;
uiCount = uiFreeEntry;
}
else
{
EPL_DBGLVL_SDO_TRACE0("EplSdoAsnduCb(): no free handle\n");
goto Exit;
}
}
// if (uiNodeId == *puiConnection)
{ // entry found or created
SdoConHdl = (uiCount | EPL_SDO_ASND_HANDLE );
SdoAsndInstance_g.m_fpSdoAsySeqCb(SdoConHdl, &pFrame->m_Data.m_Asnd.m_Payload.m_SdoSequenceFrame, (pFrameInfo_p->m_uiFrameSize - 18));
}
Exit:
return Ret;
}
tEplKernel EplDlluCalProcess(tEplEvent * pEvent_p)
{
tEplKernel Ret = kEplSuccessful;
tEplMsgType MsgType;
unsigned int uiAsndServiceId;
tEplFrameInfo FrameInfo;
if (pEvent_p->m_EventType == kEplEventTypeAsndRx) {
FrameInfo.m_pFrame = (tEplFrame *) pEvent_p->m_pArg;
FrameInfo.m_uiFrameSize = pEvent_p->m_uiSize;
// extract NetTime
FrameInfo.m_NetTime = pEvent_p->m_NetTime;
MsgType =
(tEplMsgType) AmiGetByteFromLe(&FrameInfo.m_pFrame->
m_le_bMessageType);
if (MsgType != kEplMsgTypeAsnd) {
Ret = kEplInvalidOperation;
goto Exit;
}
uiAsndServiceId =
(unsigned int)AmiGetByteFromLe(&FrameInfo.m_pFrame->m_Data.
m_Asnd.m_le_bServiceId);
if (uiAsndServiceId < EPL_DLL_MAX_ASND_SERVICE_ID) { // ASnd service ID is valid
if (EplDlluCalInstance_g.m_apfnDlluCbAsnd[uiAsndServiceId] != NULL) { // handler was registered
Ret =
EplDlluCalInstance_g.
m_apfnDlluCbAsnd[uiAsndServiceId]
(&FrameInfo);
}
}
}
Exit:
return Ret;
}
static tEplKernel EplPdokPdoEncode(tEplFrame* pFrame_p, unsigned int uiFrameSize_p, BOOL fReadyFlag_p)
{
tEplKernel Ret = kEplSuccessful;
BYTE bFlag1;
unsigned int uiNodeId;
tEplMsgType MsgType;
tEplPdoChannel* pPdoChannel;
unsigned int uiChannelId;
tEplPdoMappObject* pMappObject;
unsigned int uiMappObjectCount;
// set TPDO invalid, so that only fully processed TPDOs are sent as valid
bFlag1 = AmiGetByteFromLe(&pFrame_p->m_Data.m_Pres.m_le_bFlag1);
AmiSetByteToLe(&pFrame_p->m_Data.m_Pres.m_le_bFlag1, (bFlag1 & ~EPL_FRAME_FLAG1_RD));
// retrieve EPL message type
MsgType = AmiGetByteFromLe(&pFrame_p->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_p->m_le_bDstNodeId);
}
// search for appropriate valid TPDO
for (uiChannelId = 0, pPdoChannel = &EplPdokInstance_g.m_pTxPdoChannel[0];
uiChannelId < EplPdokInstance_g.m_Allocation.m_uiTxPdoChannelCount;
uiChannelId++, pPdoChannel++)
{
if (pPdoChannel->m_uiNodeId != uiNodeId)
{
continue;
}
// valid TPDO found
if ((unsigned int)(pPdoChannel->m_wPdoSize + 24) > uiFrameSize_p)
{ // TPDO is too short
// $$$ raise PDO error, set Ret
break;
}
// set PDO version in frame
AmiSetByteToLe(&pFrame_p->m_Data.m_Pres.m_le_bPdoVersion, pPdoChannel->m_bMappingVersion);
// call data copy callback function right before TPDO access
if ((EplPdokInstance_g.m_pfnCbTpdoPreCopy != NULL) && (pPdoChannel->m_uiMappObjectCount != 0))
{
EplPdokInstance_g.m_pfnCbTpdoPreCopy((BYTE) uiChannelId);
}
// process mapping
for (uiMappObjectCount = pPdoChannel->m_uiMappObjectCount, pMappObject = EplPdokInstance_g.m_paTxObject[uiChannelId];
uiMappObjectCount > 0;
uiMappObjectCount--, pMappObject++)
{
BENCHMARK_MOD_08_TOGGLE(7);
// copy object from process/OD variable to TPDO
Ret = EplPdokCopyVarToPdo(&pFrame_p->m_Data.m_Pres.m_le_abPayload[0], pMappObject);
if (Ret != kEplSuccessful)
{ // other fatal error occurred
goto Exit;
}
}
// set PDO size in frame
AmiSetWordToLe(&pFrame_p->m_Data.m_Pres.m_le_wSize, pPdoChannel->m_wPdoSize);
if (fReadyFlag_p != FALSE)
{
// set TPDO valid
AmiSetByteToLe(&pFrame_p->m_Data.m_Pres.m_le_bFlag1, (bFlag1 | EPL_FRAME_FLAG1_RD));
}
// processing finished successfully
goto Exit;
}
// set PDO size in frame to zero, because no TPDO mapped
AmiSetWordToLe(&pFrame_p->m_Data.m_Pres.m_le_wSize, 0);
if (fReadyFlag_p != FALSE)
{
// set TPDO valid even if TPDO size is 0
AmiSetByteToLe(&pFrame_p->m_Data.m_Pres.m_le_bFlag1, (bFlag1 | EPL_FRAME_FLAG1_RD));
}
Exit:
return Ret;
}
tEplKernel EplPdokPdoDecode(tEplFrame* pFrame_p, unsigned int uiFrameSize_p)
{
tEplKernel Ret = kEplSuccessful;
BYTE bFrameData;
unsigned int uiNodeId;
tEplMsgType MsgType;
tEplPdoChannel* pPdoChannel;
unsigned int uiChannelId;
tEplPdoMappObject* pMappObject;
unsigned int uiMappObjectCount;
// check if received RPDO is valid
bFrameData = AmiGetByteFromLe(&pFrame_p->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_p->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_p->m_le_bSrcNodeId);
}
// search for appropriate valid RPDO
for (uiChannelId = 0, pPdoChannel = &EplPdokInstance_g.m_pRxPdoChannel[0];
uiChannelId < EplPdokInstance_g.m_Allocation.m_uiRxPdoChannelCount;
uiChannelId++, pPdoChannel++)
{
if (pPdoChannel->m_uiNodeId != uiNodeId)
{
continue;
}
// retrieve PDO version from frame
bFrameData = AmiGetByteFromLe(&pFrame_p->m_Data.m_Pres.m_le_bPdoVersion);
if ((pPdoChannel->m_bMappingVersion & 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
if ((unsigned int)(pPdoChannel->m_wPdoSize + EPL_FRAME_OFFSET_PDO_PAYLOAD) > uiFrameSize_p)
{ // RPDO is too short
// $$$ raise PDO error, set Ret
goto Exit;
}
// process mapping
for (uiMappObjectCount = pPdoChannel->m_uiMappObjectCount, pMappObject = EplPdokInstance_g.m_paRxObject[uiChannelId];
uiMappObjectCount > 0;
uiMappObjectCount--, pMappObject++)
{
// copy object from process/OD variable to TPDO
Ret = EplPdokCopyVarFromPdo(&pFrame_p->m_Data.m_Pres.m_le_abPayload[0], pMappObject);
if (Ret != kEplSuccessful)
{ // other fatal error occurred
goto Exit;
}
}
// call data copy callback function right after RPDO access
if ((EplPdokInstance_g.m_pfnCbRpdoPostCopy != NULL) && (pPdoChannel->m_uiMappObjectCount != 0))
{
EplPdokInstance_g.m_pfnCbRpdoPostCopy((BYTE) uiChannelId);
}
// processing finished successfully
break;
}
Exit:
#if EPL_DLL_DISABLE_DEFERRED_RXFRAME_RELEASE_ISOCHRONOUS == FALSE
EplDllkReleaseRxFrame(pFrame_p, uiFrameSize_p);
// $$$ return value?
#endif
return Ret;
}
static tEplKernel EplCfmuDownloadObject(
tEplCfmuNodeInfo* pNodeInfo_p)
{
tEplKernel Ret = kEplSuccessful;
static DWORD dw_le_Signature;
// forward data pointer for last transfer
pNodeInfo_p->m_pbDataConciseDcf += pNodeInfo_p->m_uiCurDataSize;
pNodeInfo_p->m_dwBytesRemaining -= pNodeInfo_p->m_uiCurDataSize;
if (pNodeInfo_p->m_dwEntriesRemaining > 0)
{
if (pNodeInfo_p->m_dwBytesRemaining < EPL_CDC_OFFSET_DATA)
{
// not enough bytes left in ConciseDCF
pNodeInfo_p->m_EventCnProgress.m_EplError = kEplCfmInvalidDcf;
Ret = EplCfmuCallCbProgress(pNodeInfo_p);
if (Ret != kEplSuccessful)
{
goto Exit;
}
Ret = EplCfmuFinishConfig(pNodeInfo_p, kEplNmtNodeCommandConfErr);
goto Exit;
}
// fetch next item from ConciseDCF
pNodeInfo_p->m_EventCnProgress.m_uiObjectIndex = AmiGetWordFromLe(&pNodeInfo_p->m_pbDataConciseDcf[EPL_CDC_OFFSET_INDEX]);
pNodeInfo_p->m_EventCnProgress.m_uiObjectSubIndex = AmiGetByteFromLe(&pNodeInfo_p->m_pbDataConciseDcf[EPL_CDC_OFFSET_SUBINDEX]);
pNodeInfo_p->m_uiCurDataSize = (unsigned int) AmiGetDwordFromLe(&pNodeInfo_p->m_pbDataConciseDcf[EPL_CDC_OFFSET_SIZE]);
pNodeInfo_p->m_pbDataConciseDcf += EPL_CDC_OFFSET_DATA;
pNodeInfo_p->m_dwBytesRemaining -= EPL_CDC_OFFSET_DATA;
pNodeInfo_p->m_EventCnProgress.m_dwBytesDownloaded += EPL_CDC_OFFSET_DATA;
if ((pNodeInfo_p->m_dwBytesRemaining < pNodeInfo_p->m_uiCurDataSize)
|| (pNodeInfo_p->m_uiCurDataSize == 0))
{
// not enough bytes left in ConciseDCF
pNodeInfo_p->m_EventCnProgress.m_EplError = kEplCfmInvalidDcf;
Ret = EplCfmuCallCbProgress(pNodeInfo_p);
if (Ret != kEplSuccessful)
{
goto Exit;
}
Ret = EplCfmuFinishConfig(pNodeInfo_p, kEplNmtNodeCommandConfErr);
goto Exit;
}
pNodeInfo_p->m_dwEntriesRemaining--;
Ret = EplCfmuSdoWriteObject(pNodeInfo_p, pNodeInfo_p->m_pbDataConciseDcf, pNodeInfo_p->m_uiCurDataSize);
if (Ret != kEplSuccessful)
{
goto Exit;
}
}
else
{ // download finished
if (pNodeInfo_p->m_fDoStore != FALSE)
{
// store configuration into non-volatile memory
pNodeInfo_p->m_CfmState = kEplCfmuStateWaitStore;
AmiSetDwordToLe(&dw_le_Signature, 0x65766173);
pNodeInfo_p->m_EventCnProgress.m_uiObjectIndex = 0x1010;
pNodeInfo_p->m_EventCnProgress.m_uiObjectSubIndex = 0x01;
Ret = EplCfmuSdoWriteObject(pNodeInfo_p, &dw_le_Signature, sizeof (dw_le_Signature));
if (Ret != kEplSuccessful)
{
goto Exit;
}
}
else
{
Ret = EplCfmuDownloadCycleLength(pNodeInfo_p);
if (Ret == kEplReject)
{
pNodeInfo_p->m_CfmState = kEplCfmuStateUpToDate;
Ret = kEplSuccessful;
}
else
{
Ret = EplCfmuFinishConfig(pNodeInfo_p, kEplNmtNodeCommandConfReset);
}
}
}
Exit:
return Ret;
}
static tEplKernel EplObdCdcProcess(tEplObdCdcInfo* pCdcInfo_p)
{
tEplKernel Ret = kEplSuccessful;
DWORD dwEntriesRemaining;
unsigned int uiObjectIndex;
unsigned int uiObjectSubIndex;
size_t iCurDataSize;
Ret = EplObdCdcLoadNextBuffer(pCdcInfo_p, sizeof (DWORD));
if (Ret != kEplSuccessful)
{
goto Exit;
}
dwEntriesRemaining = AmiGetDwordFromLe(pCdcInfo_p->m_pbCurBuffer);
if (dwEntriesRemaining == 0)
{
Ret = EplEventuPostError(kEplEventSourceObdu, kEplObdNoConfigData, 0, NULL);
goto Exit;
}
for ( ; dwEntriesRemaining != 0; dwEntriesRemaining--)
{
Ret = EplObdCdcLoadNextBuffer(pCdcInfo_p, EPL_CDC_OFFSET_DATA);
if (Ret != kEplSuccessful)
{
goto Exit;
}
uiObjectIndex = AmiGetWordFromLe(&pCdcInfo_p->m_pbCurBuffer[EPL_CDC_OFFSET_INDEX]);
uiObjectSubIndex = AmiGetByteFromLe(&pCdcInfo_p->m_pbCurBuffer[EPL_CDC_OFFSET_SUBINDEX]);
iCurDataSize = (size_t) AmiGetDwordFromLe(&pCdcInfo_p->m_pbCurBuffer[EPL_CDC_OFFSET_SIZE]);
EPL_DBGLVL_OBD_TRACE("%s: Reading object 0x%04X/%u with size %u from CDC\n", __func__, uiObjectIndex, uiObjectSubIndex, iCurDataSize);
Ret = EplObdCdcLoadNextBuffer(pCdcInfo_p, iCurDataSize);
if (Ret != kEplSuccessful)
{
EPL_DBGLVL_OBD_TRACE("%s: Reading the corresponding data from CDC failed with 0x%02X\n", __func__, Ret);
goto Exit;
}
Ret = EplObdWriteEntryFromLe(uiObjectIndex, uiObjectSubIndex, pCdcInfo_p->m_pbCurBuffer, (tEplObdSize) iCurDataSize);
if (Ret != kEplSuccessful)
{
tEplEventObdError ObdError;
ObdError.m_uiIndex = uiObjectIndex;
ObdError.m_uiSubIndex = uiObjectSubIndex;
EPL_DBGLVL_OBD_TRACE("%s: Writing object 0x%04X/%u to local OBD failed with 0x%02X\n", __func__, uiObjectIndex, uiObjectSubIndex, Ret);
Ret = EplEventuPostError(kEplEventSourceObdu, Ret, sizeof (ObdError), &ObdError);
if (Ret != kEplSuccessful)
{
goto Exit;
}
}
}
Exit:
return Ret;
}