//------------------------------------------------------------------------------
tCircBufError circbuf_allocBuffer(tCircBufInstance* pInstance_p, size_t* pSize_p)
{
// Check parameter validity
ASSERT(pInstance_p != NULL);
ASSERT(pSize_p != NULL);
pInstance_p->pCircBufHeader = (tCircBufHeader*)OPLK_MALLOC(sizeof(tCircBufHeader));
if (pInstance_p->pCircBufHeader == NULL)
return kCircBufNoResource;
pInstance_p->pCircBuf = OPLK_MALLOC(*pSize_p);
if (pInstance_p->pCircBuf == NULL)
{
OPLK_FREE(pInstance_p->pCircBufHeader);
return kCircBufNoResource;
}
return kCircBufOk;
}
//------------------------------------------------------------------------------
tOplkError timeru_deleteTimer(tTimerHdl* pTimerHdl_p)
{
tTimeruData* pData;
if (pTimerHdl_p == NULL)
return kErrorTimerInvalidHandle;
// check handle itself, i.e. was the handle initialized before
if (*pTimerHdl_p == 0)
{
return kErrorOk;
}
pData = (tTimeruData*)*pTimerHdl_p;
timer_delete(pData->timer);
removeTimer(pData);
OPLK_FREE(pData);
// uninitialize handle
*pTimerHdl_p = 0;
return kErrorOk;
}
//------------------------------------------------------------------------------
tCircBufInstance* circbuf_createInstance(UINT8 id_p, BOOL fNew_p)
{
tCircBufInstance* pInstance;
tCircBufArchInstance* pArch;
char semName[16];
pInstance = (tCircBufInstance*)OPLK_MALLOC(sizeof(tCircBufInstance) +
sizeof(tCircBufArchInstance));
if (pInstance == NULL)
{
DEBUG_LVL_ERROR_TRACE("%s() malloc failed!\n", __func__);
return NULL;
}
OPLK_MEMSET(pInstance, 0, sizeof(tCircBufInstance) + sizeof(tCircBufArchInstance));
pInstance->pCircBufArchInstance = (BYTE*)pInstance + sizeof(tCircBufInstance);
pInstance->bufferId = id_p;
pArch = (tCircBufArchInstance*)pInstance->pCircBufArchInstance;
sprintf(semName, "/semCircbuf-%d", id_p);
if (fNew_p)
{
sem_unlink(semName);
}
if ((pArch->lockSem = sem_open(semName, O_CREAT, S_IRWXG, 1)) == SEM_FAILED)
{
DEBUG_LVL_ERROR_TRACE("%s() open sem failed!\n", __func__);
OPLK_FREE(pInstance);
return NULL;
}
return pInstance;
}
//------------------------------------------------------------------------------
tOplkError timeru_delInstance(void)
{
#if (TARGET_SYSTEM == _WIN32_ || TARGET_SYSTEM == _WINCE_ )
SetEvent(timeruInstance_l.ahEvents[TIMERU_EVENT_SHUTDOWN]);
WaitForSingleObject(timeruInstance_l.hProcessThread, INFINITE);
CloseHandle(timeruInstance_l.hProcessThread);
CloseHandle(timeruInstance_l.ahEvents[TIMERU_EVENT_SHUTDOWN]);
CloseHandle(timeruInstance_l.ahEvents[TIMERU_EVENT_WAKEUP]);
DeleteCriticalSection(&timeruInstance_l.aCriticalSections[TIMERU_TIMER_LIST]);
DeleteCriticalSection(&timeruInstance_l.aCriticalSections[TIMERU_FREE_LIST]);
#endif
OPLK_FREE(timeruInstance_l.pEntries);
timeruInstance_l.pEntries = NULL;
timeruInstance_l.pFreeListFirst = NULL;
timeruInstance_l.pTimerListFirst = NULL;
timeruInstance_l.freeEntries = 0;
return kErrorOk;
}
//------------------------------------------------------------------------------
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;
}
//------------------------------------------------------------------------------
void circbuf_freeBuffer(tCircBufInstance* pInstance_p)
{
OPLK_FREE(pInstance_p->pCircBuf);
OPLK_FREE(pInstance_p->pCircBufHeader);
}
//------------------------------------------------------------------------------
void circbuf_freeInstance(tCircBufInstance* pInstance_p)
{
OPLK_FREE(pInstance_p);
}
//------------------------------------------------------------------------------
static tOplkError loadNextBuffer(tObdCdcInfo* pCdcInfo_p, size_t bufferSize)
{
tOplkError ret = kErrorOk;
int readSize;
UINT8* pBuffer;
switch (pCdcInfo_p->type)
{
case kObdCdcTypeFile:
if (pCdcInfo_p->bufferSize < bufferSize)
{
if (pCdcInfo_p->pCurBuffer != NULL)
{
OPLK_FREE(pCdcInfo_p->pCurBuffer);
pCdcInfo_p->pCurBuffer = NULL;
pCdcInfo_p->bufferSize = 0;
}
pCdcInfo_p->pCurBuffer = OPLK_MALLOC(bufferSize);
if (pCdcInfo_p->pCurBuffer == NULL)
{
ret = eventu_postError(kEventSourceObdu, kErrorObdOutOfMemory, 0, NULL);
if (ret != kErrorOk)
return ret;
return kErrorReject;
}
pCdcInfo_p->bufferSize = bufferSize;
}
pBuffer = pCdcInfo_p->pCurBuffer;
do
{
readSize = read(pCdcInfo_p->handle.fdCdcFile, pBuffer, bufferSize);
if (readSize <= 0)
{
ret = eventu_postError(kEventSourceObdu, kErrorObdInvalidDcf, 0, NULL);
if (ret != kErrorOk)
return ret;
return kErrorReject;
}
pBuffer += readSize;
bufferSize -= readSize;
pCdcInfo_p->cdcSize -= readSize;
}
while (bufferSize > 0);
break;
case kObdCdcTypeBuffer:
if (pCdcInfo_p->bufferSize < bufferSize)
{
ret = eventu_postError(kEventSourceObdu, kErrorObdInvalidDcf, 0, NULL);
if (ret != kErrorOk)
return ret;
return kErrorReject;
}
pCdcInfo_p->pCurBuffer = pCdcInfo_p->handle.pNextBuffer;
pCdcInfo_p->handle.pNextBuffer += bufferSize;
pCdcInfo_p->bufferSize -= bufferSize;
break;
}
return ret;
}
//------------------------------------------------------------------------------
tOplkError pdok_allocChannelMem(tPdoAllocationParam* pAllocationParam_p)
{
tOplkError ret = kErrorOk;
#if NMT_MAX_NODE_ID > 0
tDllNodeOpParam nodeOpParam;
nodeOpParam.opNodeType = kDllNodeOpTypeFilterPdo;
nodeOpParam.nodeId = C_ADR_BROADCAST;
ret = dllk_deleteNode(&nodeOpParam);
if (ret != kErrorOk)
{
goto Exit;
}
#endif // NMT_MAX_NODE_ID > 0
if (pdokInstance_g.pdoChannels.allocation.rxPdoChannelCount != pAllocationParam_p->rxPdoChannelCount)
{ // allocation should be changed
pdokInstance_g.pdoChannels.allocation.rxPdoChannelCount = pAllocationParam_p->rxPdoChannelCount;
if (pdokInstance_g.pdoChannels.pRxPdoChannel != NULL)
{
OPLK_FREE(pdokInstance_g.pdoChannels.pRxPdoChannel);
pdokInstance_g.pdoChannels.pRxPdoChannel = NULL;
}
if (pAllocationParam_p->rxPdoChannelCount > 0)
{
pdokInstance_g.pdoChannels.pRxPdoChannel =
(tPdoChannel*)OPLK_MALLOC(sizeof(*pdokInstance_g.pdoChannels.pRxPdoChannel) *
pAllocationParam_p->rxPdoChannelCount);
if (pdokInstance_g.pdoChannels.pRxPdoChannel == NULL)
{
ret = kErrorPdoInitError;
goto Exit;
}
}
}
disablePdoChannels(pdokInstance_g.pdoChannels.pRxPdoChannel,
pdokInstance_g.pdoChannels.allocation.rxPdoChannelCount);
if (pdokInstance_g.pdoChannels.allocation.txPdoChannelCount != pAllocationParam_p->txPdoChannelCount)
{ // allocation should be changed
pdokInstance_g.pdoChannels.allocation.txPdoChannelCount = pAllocationParam_p->txPdoChannelCount;
if (pdokInstance_g.pdoChannels.pTxPdoChannel != NULL)
{
OPLK_FREE(pdokInstance_g.pdoChannels.pTxPdoChannel);
pdokInstance_g.pdoChannels.pTxPdoChannel = NULL;
}
if (pAllocationParam_p->txPdoChannelCount > 0)
{
pdokInstance_g.pdoChannels.pTxPdoChannel =
(tPdoChannel*)OPLK_MALLOC(sizeof(*pdokInstance_g.pdoChannels.pTxPdoChannel) *
pAllocationParam_p->txPdoChannelCount);
if (pdokInstance_g.pdoChannels.pTxPdoChannel == NULL)
{
ret = kErrorPdoInitError;
goto Exit;
}
}
}
disablePdoChannels(pdokInstance_g.pdoChannels.pTxPdoChannel,
pdokInstance_g.pdoChannels.allocation.txPdoChannelCount);
Exit:
return ret;
}
//------------------------------------------------------------------------------
tOplkError sdotestcom_cbEvent(tEvent* pOplkEvent_p)
{
tOplkError ret;
tOplkError Sequret;
tSdoTestComCon* pCmdCon;
tPlkFrame* pFrame;
tCircBufError Cbret;
ULONG ulDataSize;
size_t size_p;
size_t FrameSize;
ret = kErrorOk;
pFrame = NULL;
FrameSize = C_DLL_MAX_ASYNC_MTU;
size_p = 0x00;
ulDataSize = 0x00;
switch (pOplkEvent_p->eventType)
{
case kEventTypeSdoAsySend:
// Check parameter
if (sizeof(*pCmdCon) == pOplkEvent_p->eventArgSize)
{
pCmdCon = (tSdoTestComCon*)pOplkEvent_p->pEventArg;
}
else
{
ret = kErrorSdoComInvalidParam;
goto Exit;
}
pFrame = (tPlkFrame*)OPLK_MALLOC(FrameSize);
if (pFrame == NULL)
{
ret = kErrorNoResource;
goto Exit;
}
// Send all frames in that are currently in the shared buffer
do
{
OPLK_MEMSET(pFrame, 0, FrameSize);
Cbret = circbuf_readData(pCmdCon->pCbBufInst,
pFrame, size_p,
&FrameSize);
if ((kCircBufOk == Cbret) &&
(ulDataSize > PLK_FRAME_OFFSET_SDO_COMU))
{
Sequret = sdoseq_sendData(pCmdCon->tSeqHdl,
ulDataSize - PLK_FRAME_OFFSET_SDO_COMU,
pFrame);
// Send all frames, but return error code if any frame fails
if (Sequret != kErrorOk)
{
ret = Sequret;
}
}
}
while (Cbret == kCircBufOk);
// kShbNoReadableData would be the only error free condition
// for the preceding loop to end
if (kCircBufNoReadableData != Cbret)
{
ret = kErrorInvalidOperation;
}
break;
default:
// Reject unknown events
ret = kErrorInvalidOperation;
break;
}
Exit:
if (pFrame != NULL)
{
OPLK_FREE(pFrame);
}
return ret;
}
//------------------------------------------------------------------------------
tOplkError sdotestcom_sendFrame(UINT nodeId_p, tSdoType sdoType_p,
tAsySdoCom* pSdoCom_p, size_t sdoSize_p)
{
tOplkError ret;
tSdoTestComCon* pCmdCon;
tEvent Event;
tCircBufError CbError;
tPlkFrame* pFrame;
tAsySdoCom* pSdoCom_Dst;
size_t FrameSize;
ret = kErrorOk;
pCmdCon = &sdoTestComInst.tCmdCon;
FrameSize = PLK_FRAME_OFFSET_SDO_COMU + sdoSize_p;
// Check if parameters are valid
if (FrameSize > C_DLL_MAX_ASYNC_MTU)
{
return kErrorInvalidOperation;
}
if (kOplkTestSdoComStateIdle != pCmdCon->tState)
{
// If the connection is already in use, node ID and SDO type have to match
if ((pCmdCon->nodeId != nodeId_p) ||
(pCmdCon->tSdoType != sdoType_p))
{
return kErrorInvalidOperation;
}
}
// Get frame buffer
pFrame = (tPlkFrame *)OPLK_MALLOC(FrameSize);
if (pFrame == NULL)
{
ret = kErrorNoResource;
}
// Generate frame
pSdoCom_Dst = &pFrame->data.asnd.payload.sdoSequenceFrame.sdoSeqPayload;
OPLK_MEMSET(pFrame, 0, FrameSize);
OPLK_MEMCPY(pSdoCom_Dst, pSdoCom_p, sdoSize_p);
// Save frame in shared buffer
CbError = circbuf_writeData(pCmdCon->pCbBufInst,
pFrame, FrameSize);
OPLK_FREE(pFrame);
if (kCircBufOk != CbError)
{
ret = kErrorInvalidOperation;
}
else
{
// Sequence layer handling
// Either create a new connection, or reuse existing one
switch (pCmdCon->tState)
{
case kOplkTestSdoComStateIdle:
// Get new sequence layer connection
// When the connection is ready, the callback will trigger sending
ret = sdoseq_initCon(&pCmdCon->tSeqHdl, nodeId_p, sdoType_p);
pCmdCon->tState = kOplkTestSdoComStateWaitInit;
pCmdCon->tSdoType = sdoType_p;
pCmdCon->nodeId = nodeId_p;
break;
case kOplkTestSdoComStateWaitInit:
// Connection setup is already in progress
// Nothing left to do
break;
case kOplkTestSdoComStateConnected:
// Connection is already up and running,
// just trigger frame send event
OPLK_MEMSET(&Event.netTime, 0x00, sizeof(Event.netTime));
Event.eventType = kEventTypeSdoAsySend;
Event.pEventArg = pCmdCon;
Event.eventArgSize = sizeof(*pCmdCon);
Event.eventSink = kEventSinkSdoTest;
ret = eventu_postEvent(&Event);
break;
default:
// Reject unknown states
ret = kErrorInvalidOperation;
break;
}
}
return ret;
}
//------------------------------------------------------------------------------
tOplkError dllknode_cleanupLocalNode(tNmtState oldNmtState_p)
{
tOplkError ret = kErrorOk;
BYTE aMulticastMac[6];
#if NMT_MAX_NODE_ID > 0
UINT index;
#endif
#if defined(CONFIG_INCLUDE_NMT_MN)
UINT handle;
#else
UNUSED_PARAMETER(oldNmtState_p);
#endif
// remove all filters from Edrv
ret = edrv_changeRxFilter(NULL, 0, 0, 0);
// delete timer
#if CONFIG_TIMER_USE_HIGHRES != FALSE
if ((ret = hrestimer_deleteTimer(&dllkInstance_g.timerHdlCycle)) != kErrorOk)
return ret;
#if defined(CONFIG_INCLUDE_NMT_RMN)
if ((ret = hrestimer_deleteTimer(&dllkInstance_g.timerHdlSwitchOver)) != kErrorOk)
return ret;
#endif
#endif
#if defined(CONFIG_INCLUDE_NMT_MN)
if ((ret = edrvcyclic_stopCycle(FALSE)) != kErrorOk)
return ret;
if ((ret = edrvcyclic_regSyncHandler(NULL)) != kErrorOk)
return ret;
#endif
#if (CONFIG_DLL_PROCESS_SYNC == DLL_PROCESS_SYNC_ON_TIMER)
if ((ret = synctimer_stopSync()) != kErrorOk)
return ret;
#endif
#if defined(CONFIG_INCLUDE_NMT_MN)
// destroy all data structures
OPLK_FREE(dllkInstance_g.ppTxBufferList);
dllkInstance_g.ppTxBufferList = NULL;
#endif
// delete Tx frames
if ((ret = dllkframe_deleteTxFrame(DLLK_TXFRAME_IDENTRES)) != kErrorOk)
return ret;
if ((ret = dllkframe_deleteTxFrame(DLLK_TXFRAME_STATUSRES)) != kErrorOk)
return ret;
if ((ret = dllkframe_deleteTxFrame(DLLK_TXFRAME_PRES)) != kErrorOk)
return ret;
#if defined(CONFIG_INCLUDE_NMT_RMN)
if (dllkInstance_g.fRedundancy)
{
if ((ret = dllkframe_deleteTxFrame(DLLK_TXFRAME_AMNI)) != kErrorOk)
return ret;
}
#endif
dllkInstance_g.aTxBufferStateNmtReq[0] = kDllkTxBufEmpty;
dllkInstance_g.aTxBufferStateNmtReq[1] = kDllkTxBufEmpty;
if ((ret = dllkframe_deleteTxFrame(DLLK_TXFRAME_NMTREQ)) != kErrorOk)
return ret;
#if CONFIG_DLL_PRES_CHAINING_CN != FALSE
if ((ret = dllkframe_deleteTxFrame(DLLK_TXFRAME_SYNCRES)) != kErrorOk)
return ret;
#endif
dllkInstance_g.aTxBufferStateNonPlk[0] = kDllkTxBufEmpty;
dllkInstance_g.aTxBufferStateNonPlk[1] = kDllkTxBufEmpty;
if ((ret = dllkframe_deleteTxFrame(DLLK_TXFRAME_NONPLK)) != kErrorOk)
return ret;
#if defined(CONFIG_INCLUDE_NMT_MN)
#if !defined(CONFIG_INCLUDE_NMT_RMN)
if (NMT_IF_MN_OR_RMN(oldNmtState_p))
#else
if (NMT_IF_MN_OR_RMN(oldNmtState_p) || (dllkInstance_g.fRedundancy))
#endif
{ // local node was MN
if ((ret = dllkframe_deleteTxFrame(DLLK_TXFRAME_SOC)) != kErrorOk)
return ret;
if ((ret = dllkframe_deleteTxFrame(DLLK_TXFRAME_SOA)) != kErrorOk)
return ret;
for (index = 0; index < tabentries (dllkInstance_g.aNodeInfo); index++)
{
if (dllkInstance_g.aNodeInfo[index].pPreqTxBuffer != NULL)
{
handle = (UINT)(dllkInstance_g.aNodeInfo[index].pPreqTxBuffer - dllkInstance_g.pTxBuffer);
dllkInstance_g.aNodeInfo[index].pPreqTxBuffer = NULL;
if (handle != DLLK_TXFRAME_PRES)
{
if ((ret = dllkframe_deleteTxFrame(handle)) != kErrorOk)
return ret;
}
}
// disable PReq and PRes for this node
dllkInstance_g.aNodeInfo[index].preqPayloadLimit = 0;
dllkInstance_g.aNodeInfo[index].presPayloadLimit = 0;
}
}
else
{ // local node was CN
for (index = 0; index < tabentries(dllkInstance_g.aNodeInfo); index++)
{
// disable PReq and PRes for this node
dllkInstance_g.aNodeInfo[index].presPayloadLimit = 0;
dllkInstance_g.aNodeInfo[index].preqPayloadLimit = 0;
}
}
#else
// must be CN, because MN part is not compiled!
#if NMT_MAX_NODE_ID > 0
for (index = 0; index < tabentries(dllkInstance_g.aNodeInfo); index++)
{
// disable PRes for this node
dllkInstance_g.aNodeInfo[index].presPayloadLimit = 0;
}
#endif
#endif
// de-register multicast MACs in Ethernet driver
ami_setUint48Be(&aMulticastMac[0], C_DLL_MULTICAST_SOC);
ret = edrv_clearRxMulticastMacAddr(aMulticastMac);
ami_setUint48Be(&aMulticastMac[0], C_DLL_MULTICAST_SOA);
ret = edrv_clearRxMulticastMacAddr(aMulticastMac);
ami_setUint48Be(&aMulticastMac[0], C_DLL_MULTICAST_PRES);
ret = edrv_clearRxMulticastMacAddr(aMulticastMac);
ami_setUint48Be(&aMulticastMac[0], C_DLL_MULTICAST_ASND);
ret = edrv_clearRxMulticastMacAddr(aMulticastMac);
return ret;
}
//------------------------------------------------------------------------------
void circbuf_freeBuffer(tCircBufInstance* pInstance_p)
{
if (pInstance_p->pCircBufArchInstance == NULL)
OPLK_FREE(pInstance_p->pCircBufHeader);
}
