//------------------------------------------------------------------------------
tOplkError edrv_sendTxBuffer(tEdrvTxBuffer* pBuffer_p)
{
int pcapRet;
// Check parameter validity
ASSERT(pBuffer_p != NULL);
FTRACE_MARKER("%s", __func__);
if (pBuffer_p->txBufferNumber.pArg != NULL)
return kErrorInvalidOperation;
if (getLinkStatus(edrvInstance_l.initParam.pDevName) == FALSE)
{
/* there's no link! We pretend that packet is sent and immediately call
* tx handler! Otherwise the stack would hang! */
if (pBuffer_p->pfnTxHandler != NULL)
{
pBuffer_p->pfnTxHandler(pBuffer_p);
}
}
else
{
pthread_mutex_lock(&edrvInstance_l.mutex);
if (edrvInstance_l.pTransmittedTxBufferLastEntry == NULL)
{
edrvInstance_l.pTransmittedTxBufferLastEntry =
edrvInstance_l.pTransmittedTxBufferFirstEntry = pBuffer_p;
}
else
{
edrvInstance_l.pTransmittedTxBufferLastEntry->txBufferNumber.pArg = pBuffer_p;
edrvInstance_l.pTransmittedTxBufferLastEntry = pBuffer_p;
}
pthread_mutex_unlock(&edrvInstance_l.mutex);
pcapRet = pcap_sendpacket(edrvInstance_l.pPcap, pBuffer_p->pBuffer,
(int)pBuffer_p->txFrameSize);
if (pcapRet != 0)
{
DEBUG_LVL_EDRV_TRACE("%s() pcap_sendpacket returned %d (%s)\n",
__func__, pcapRet, pcap_geterr(edrvInstance_l.pPcap));
return kErrorInvalidOperation;
}
}
return kErrorOk;
}
//---------------------------------------------------------------------------
// Function: EplTimerHighReskProcessThread()
//
// Description: Main function of the high-resolution timer thread.
//
// EplTimerHighReskProcessThread() waits for a timer start
// signal. When it is received it reads the high-resolution
// timer information structure and calculates the timeout value.
// It sleeps by calling clock_nanosleep() until the timeout is
// reached. When the timeout is reached the callback function
// registered in the timer info structure is called. If the
// flag m_fContinuously is set the thread loops until the
// timer is deleted.
//
// Parameters: pArgument_p * = pointer to timer info structure
//
// Return: void * = return value is specified by the pthread
// interface but is not used!
//---------------------------------------------------------------------------
static void * EplTimerHighReskProcessThread(void *pArgument_p)
{
INT iRet;
tEplTimerHighReskTimerInfo *pTimerInfo;
struct timespec startTime, curTime, timeout;
ULONGLONG ullPeriod;
tEplTimerHdl timerHdl;
#ifdef HIGH_RESK_TIMER_LATENCY_DEBUG
struct timespec debugtime;
#endif
EPL_DBGLVL_TIMERH_TRACE("%s(): ThreadId:%ld\n", __func__, syscall(SYS_gettid));
EPL_DBGLVL_TIMERH_TRACE("%s(): timer:%lx\n", __func__, (unsigned long)pArgument_p);
/* thread parameter contains the address of the timer information structure */
pTimerInfo = &EplTimerHighReskInstance_l.m_aTimerInfo[(int)pArgument_p];
/* loop forever until thread will be canceled */
while (1)
{
/* wait for semaphore which signals a timer start */
sem_wait(&pTimerInfo->m_syncSem);
/* check if thread should terminate */
if (pTimerInfo->m_fTerminate)
{
EPL_DBGLVL_TIMERH_TRACE("%s() Exiting signal received!\n", __func__);
break;
}
else
{
/* save timer information into local variables */
startTime = pTimerInfo->m_startTime;
timerHdl = pTimerInfo->m_EventArg.m_TimerHdl;
ullPeriod = pTimerInfo->m_ullTime;
/* calculate the timeout value for the timer cycle */
timespec_add(&startTime, ullPeriod, &timeout);
#ifdef HIGH_RESK_TIMER_LATENCY_DEBUG
clock_gettime(CLOCK_MONOTONIC, &curTime);
#endif
do
{
/* sleep until timeout */
iRet = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &timeout, NULL);
if (iRet < 0)
{
EPL_DBGLVL_ERROR_TRACE("%s(): Error in clock_nanosleep!\n",
__func__);
/* todo how to signal that timeout wasn't correct? */
}
FTRACE_MARKER("HighReskTimer(%d) expired (%d ns)",
(int)pArgument_p, ullPeriod);
#ifdef HIGH_RESK_TIMER_LATENCY_DEBUG
clock_gettime(CLOCK_MONOTONIC, &curTime);
timespec_sub(&timeout, &curTime, &debugtime);
FTRACE_MARKER("%s latency=%ld:%ld", __func__, debugtime.tv_sec,
debugtime.tv_nsec);
if (debugtime.tv_nsec > pTimerInfo->m_maxLatency)
{
EPL_DBGLVL_TIMERH_TRACE("%s() Timer elapsed: max latency=%ld ns\n",
__func__, debugtime.tv_nsec);
pTimerInfo->m_maxLatency = debugtime.tv_nsec;
}
if (timeout.tv_nsec < pTimerInfo->m_minLatency)
{
EPL_DBGLVL_TIMERH_TRACE("%s() Timer elapsed: min latency=%ld ns\n",
__func__, debugtime.tv_nsec);
pTimerInfo->m_minLatency = debugtime.tv_nsec;
}
#endif
/* check if timer handle is valid */
if (timerHdl == pTimerInfo->m_EventArg.m_TimerHdl)
{
/* call callback function */
if (pTimerInfo->m_pfnCallback != NULL)
{
pTimerInfo->m_pfnCallback(&pTimerInfo->m_EventArg);
}
}
/* check if timer handle is still valid. Could be modified in callback! */
if (timerHdl == pTimerInfo->m_EventArg.m_TimerHdl)
{
if (pTimerInfo->m_fContinuously)
{
/* calculate timeout value for next timer cycle */
timespec_add(&timeout, ullPeriod, &timeout);
}
}
} while ((pTimerInfo->m_fContinuously) &&
(timerHdl == pTimerInfo->m_EventArg.m_TimerHdl));
}
}
return NULL;
}
tEplKernel PUBLIC AppCbEvent
(
tEplApiEventType EventType_p, // IN: event type (enum)
tEplApiEventArg* pEventArg_p, // IN: event argument (union)
void GENERIC* pUserArg_p //__attribute((unused))
)
{
tEplKernel EplRet = kEplSuccessful;
UINT uiVarLen;
UNUSED_PARAMETER(pUserArg_p);
// check if NMT_GS_OFF is reached
switch (EventType_p)
{
case kEplApiEventNmtStateChange:
{
switch (pEventArg_p->m_NmtStateChange.m_NewNmtState)
{
case kEplNmtGsOff:
{ // NMT state machine was shut down,
// because of user signal (CTRL-C) or critical EPL stack error
// -> also shut down EplApiProcess() and main()
EplRet = kEplShutdown;
printlog("Event:kEplNmtGsOff originating event = 0x%X (%s)\n", pEventArg_p->m_NmtStateChange.m_NmtEvent,
EplGetNmtEventStr(pEventArg_p->m_NmtStateChange.m_NmtEvent));
break;
}
case kEplNmtGsResetCommunication:
{
// continue
}
case kEplNmtGsResetConfiguration:
{
if (uiCycleLen_g != 0)
{
EplRet = EplApiWriteLocalObject(0x1006, 0x00, &uiCycleLen_g, sizeof (uiCycleLen_g));
uiCurCycleLen_g = uiCycleLen_g;
}
else
{
uiVarLen = sizeof(uiCurCycleLen_g);
EplApiReadLocalObject(0x1006, 0x00, &uiCurCycleLen_g, &uiVarLen);
}
// continue
}
case kEplNmtMsPreOperational1:
{
printlog("AppCbEvent(0x%X) originating event = 0x%X (%s)\n",
pEventArg_p->m_NmtStateChange.m_NewNmtState,
pEventArg_p->m_NmtStateChange.m_NmtEvent,
EplGetNmtEventStr(pEventArg_p->m_NmtStateChange.m_NmtEvent));
// continue
}
case kEplNmtGsInitialising:
case kEplNmtGsResetApplication:
case kEplNmtMsNotActive:
case kEplNmtCsNotActive:
case kEplNmtCsPreOperational1:
{
break;
}
case kEplNmtCsOperational:
case kEplNmtMsOperational:
{
break;
}
default:
{
break;
}
}
break;
}
case kEplApiEventCriticalError:
case kEplApiEventWarning:
{ // error or warning occurred within the stack or the application
// on error the API layer stops the NMT state machine
printlog( "%s(Err/Warn): Source = %s (%02X) EplError = %s (0x%03X)\n",
__func__,
EplGetEventSourceStr(pEventArg_p->m_InternalError.m_EventSource),
pEventArg_p->m_InternalError.m_EventSource,
EplGetEplKernelStr(pEventArg_p->m_InternalError.m_EplError),
pEventArg_p->m_InternalError.m_EplError
);
FTRACE_MARKER("%s(Err/Warn): Source = %s (%02X) EplError = %s (0x%03X)\n",
__func__,
EplGetEventSourceStr(pEventArg_p->m_InternalError.m_EventSource),
pEventArg_p->m_InternalError.m_EventSource,
EplGetEplKernelStr(pEventArg_p->m_InternalError.m_EplError),
pEventArg_p->m_InternalError.m_EplError
);
// check additional argument
switch (pEventArg_p->m_InternalError.m_EventSource)
{
case kEplEventSourceEventk:
case kEplEventSourceEventu:
{ // error occurred within event processing
// either in kernel or in user part
printlog(" OrgSource = %s %02X\n", EplGetEventSourceStr(pEventArg_p->m_InternalError.m_Arg.m_EventSource),
pEventArg_p->m_InternalError.m_Arg.m_EventSource);
FTRACE_MARKER(" OrgSource = %s %02X\n", EplGetEventSourceStr(pEventArg_p->m_InternalError.m_Arg.m_EventSource),
pEventArg_p->m_InternalError.m_Arg.m_EventSource);
break;
}
case kEplEventSourceDllk:
{ // error occurred within the data link layer (e.g. interrupt processing)
// the DWORD argument contains the DLL state and the NMT event
printlog(" val = %X\n", pEventArg_p->m_InternalError.m_Arg.m_dwArg);
FTRACE_MARKER(" val = %X\n", pEventArg_p->m_InternalError.m_Arg.m_dwArg);
break;
}
default:
{
printlog("\n");
break;
}
}
break;
}
case kEplApiEventHistoryEntry:
{ // new history entry
printlog("%s(HistoryEntry): Type=0x%04X Code=0x%04X (0x%02X %02X %02X %02X %02X %02X %02X %02X)\n",
__func__,
pEventArg_p->m_ErrHistoryEntry.m_wEntryType,
pEventArg_p->m_ErrHistoryEntry.m_wErrorCode,
(WORD) pEventArg_p->m_ErrHistoryEntry.m_abAddInfo[0],
(WORD) pEventArg_p->m_ErrHistoryEntry.m_abAddInfo[1],
(WORD) pEventArg_p->m_ErrHistoryEntry.m_abAddInfo[2],
(WORD) pEventArg_p->m_ErrHistoryEntry.m_abAddInfo[3],
(WORD) pEventArg_p->m_ErrHistoryEntry.m_abAddInfo[4],
(WORD) pEventArg_p->m_ErrHistoryEntry.m_abAddInfo[5],
(WORD) pEventArg_p->m_ErrHistoryEntry.m_abAddInfo[6],
(WORD) pEventArg_p->m_ErrHistoryEntry.m_abAddInfo[7]);
FTRACE_MARKER("%s(HistoryEntry): Type=0x%04X Code=0x%04X (0x%02X %02X %02X %02X %02X %02X %02X %02X)\n",
__func__,
pEventArg_p->m_ErrHistoryEntry.m_wEntryType,
pEventArg_p->m_ErrHistoryEntry.m_wErrorCode,
(WORD) pEventArg_p->m_ErrHistoryEntry.m_abAddInfo[0],
(WORD) pEventArg_p->m_ErrHistoryEntry.m_abAddInfo[1],
(WORD) pEventArg_p->m_ErrHistoryEntry.m_abAddInfo[2],
(WORD) pEventArg_p->m_ErrHistoryEntry.m_abAddInfo[3],
(WORD) pEventArg_p->m_ErrHistoryEntry.m_abAddInfo[4],
(WORD) pEventArg_p->m_ErrHistoryEntry.m_abAddInfo[5],
(WORD) pEventArg_p->m_ErrHistoryEntry.m_abAddInfo[6],
(WORD) pEventArg_p->m_ErrHistoryEntry.m_abAddInfo[7]);
break;
}
case kEplApiEventPdoChange:
{
unsigned int uiSubIndex;
QWORD qwMappObject;
printlog("%s(%sPDO=0x%X to node 0x%X with %d objects %s)\n",
__func__,
(pEventArg_p->m_PdoChange.m_fTx ? "T" : "R"),
pEventArg_p->m_PdoChange.m_uiPdoMappIndex,
pEventArg_p->m_PdoChange.m_uiNodeId,
pEventArg_p->m_PdoChange.m_uiMappObjectCount,
(pEventArg_p->m_PdoChange.m_fActivated ? "activated" : "deleted"));
for (uiSubIndex = 1; uiSubIndex <= pEventArg_p->m_PdoChange.m_uiMappObjectCount; uiSubIndex++)
{
uiVarLen = sizeof(qwMappObject);
EplRet = EplApiReadLocalObject(
pEventArg_p->m_PdoChange.m_uiPdoMappIndex,
uiSubIndex, &qwMappObject, &uiVarLen);
if (EplRet != kEplSuccessful)
{
printlog(" Reading 0x%X/%d failed with 0x%X\n",
pEventArg_p->m_PdoChange.m_uiPdoMappIndex,
uiSubIndex, EplRet);
continue;
}
printlog(" %d. mapped object 0x%X/%d\n",
uiSubIndex,
qwMappObject & 0x00FFFFULL,
(qwMappObject & 0xFF0000ULL) >> 16);
}
break;
}
#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
case kEplApiEventNode:
{
// check additional argument
switch (pEventArg_p->m_Node.m_NodeEvent)
{
case kEplNmtNodeEventCheckConf:
{
printlog("%s(Node=0x%X, CheckConf)\n", __func__, pEventArg_p->m_Node.m_uiNodeId);
break;
}
case kEplNmtNodeEventUpdateConf:
{
printlog("%s(Node=0x%X, UpdateConf)\n", __func__, pEventArg_p->m_Node.m_uiNodeId);
break;
}
case kEplNmtNodeEventNmtState:
{
printlog("%s(Node=0x%X, NmtState=%s)\n", __func__, pEventArg_p->m_Node.m_uiNodeId, EplGetNmtStateStr(pEventArg_p->m_Node.m_NmtState));
break;
}
case kEplNmtNodeEventError:
{
printlog("%s (Node=0x%X): Error = %s (0x%.4X)\n", __func__,
pEventArg_p->m_Node.m_uiNodeId,
EplGetEmergErrCodeStr(pEventArg_p->m_Node.m_wErrorCode),
pEventArg_p->m_Node.m_wErrorCode);
break;
}
case kEplNmtNodeEventFound:
{
printlog("%s(Node=0x%X, Found)\n", __func__, pEventArg_p->m_Node.m_uiNodeId);
break;
}
default:
{
break;
}
}
break;
}
#endif
#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_CFM)) != 0)
case kEplApiEventCfmProgress:
{
printlog("%s(Node=0x%X, CFM-Progress: Object 0x%X/%u, ", __func__, pEventArg_p->m_CfmProgress.m_uiNodeId, pEventArg_p->m_CfmProgress.m_uiObjectIndex, pEventArg_p->m_CfmProgress.m_uiObjectSubIndex);
printlog("%lu/%lu Bytes", (ULONG) pEventArg_p->m_CfmProgress.m_dwBytesDownloaded, (ULONG) pEventArg_p->m_CfmProgress.m_dwTotalNumberOfBytes);
if ((pEventArg_p->m_CfmProgress.m_dwSdoAbortCode != 0)
|| (pEventArg_p->m_CfmProgress.m_EplError != kEplSuccessful))
{
printlog(" -> SDO Abort=0x%lX, Error=0x%X)\n", (unsigned long) pEventArg_p->m_CfmProgress.m_dwSdoAbortCode,
pEventArg_p->m_CfmProgress.m_EplError);
}
else
{
printlog(")\n");
}
break;
}
case kEplApiEventCfmResult:
{
switch (pEventArg_p->m_CfmResult.m_NodeCommand)
{
case kEplNmtNodeCommandConfOk:
{
printlog("%s(Node=0x%X, ConfOk)\n", __func__, pEventArg_p->m_CfmResult.m_uiNodeId);
break;
}
case kEplNmtNodeCommandConfErr:
{
printlog("%s(Node=0x%X, ConfErr)\n", __func__, pEventArg_p->m_CfmResult.m_uiNodeId);
break;
}
case kEplNmtNodeCommandConfReset:
{
printlog("%s(Node=0x%X, ConfReset)\n", __func__, pEventArg_p->m_CfmResult.m_uiNodeId);
break;
}
case kEplNmtNodeCommandConfRestored:
{
printlog("%s(Node=0x%X, ConfRestored)\n", __func__, pEventArg_p->m_CfmResult.m_uiNodeId);
break;
}
default:
{
printlog("%s(Node=0x%X, CfmResult=0x%X)\n", __func__, pEventArg_p->m_CfmResult.m_uiNodeId, pEventArg_p->m_CfmResult.m_NodeCommand);
break;
}
}
break;
}
#endif
default:
break;
}
return EplRet;
}
//------------------------------------------------------------------------------
static void packetHandler(u_char* pParam_p,
const struct pcap_pkthdr* pHeader_p,
const u_char* pPktData_p)
{
tEdrvInstance* pInstance = (tEdrvInstance*)pParam_p;
tEdrvRxBuffer rxBuffer;
if (OPLK_MEMCMP(pPktData_p + 6, pInstance->initParam.aMacAddr, 6) != 0)
{ // filter out self generated traffic
rxBuffer.bufferInFrame = kEdrvBufferLastInFrame;
rxBuffer.rxFrameSize = pHeader_p->caplen;
rxBuffer.pBuffer = (void*)pPktData_p;
FTRACE_MARKER("%s RX", __func__);
pInstance->initParam.pfnRxHandler(&rxBuffer);
}
else
{ // self generated traffic
FTRACE_MARKER("%s TX-receive", __func__);
if (pInstance->pTransmittedTxBufferFirstEntry != NULL)
{
tEdrvTxBuffer* pTxBuffer = pInstance->pTransmittedTxBufferFirstEntry;
if (pTxBuffer->pBuffer != NULL)
{
if (OPLK_MEMCMP(pPktData_p, pTxBuffer->pBuffer, 6) == 0)
{
pthread_mutex_lock(&pInstance->mutex);
pInstance->pTransmittedTxBufferFirstEntry = (tEdrvTxBuffer*)pInstance->pTransmittedTxBufferFirstEntry->txBufferNumber.pArg;
if (pInstance->pTransmittedTxBufferFirstEntry == NULL)
{
pInstance->pTransmittedTxBufferLastEntry = NULL;
}
pthread_mutex_unlock(&pInstance->mutex);
pTxBuffer->txBufferNumber.pArg = NULL;
if (pTxBuffer->pfnTxHandler != NULL)
{
pTxBuffer->pfnTxHandler(pTxBuffer);
}
}
else
{
TRACE("%s: no matching TxB: DstMAC=%02X%02X%02X%02X%02X%02X\n",
__func__,
(UINT)((UINT8*)pPktData_p)[0],
(UINT)((UINT8*)pPktData_p)[1],
(UINT)((UINT8*)pPktData_p)[2],
(UINT)((UINT8*)pPktData_p)[3],
(UINT)((UINT8*)pPktData_p)[4],
(UINT)((UINT8*)pPktData_p)[5]);
TRACE(" current TxB %p: DstMAC=%02X%02X%02X%02X%02X%02X\n",
(void*)pTxBuffer,
(UINT)((UINT8*)(pTxBuffer->pBuffer))[0],
(UINT)((UINT8*)(pTxBuffer->pBuffer))[1],
(UINT)((UINT8*)(pTxBuffer->pBuffer))[2],
(UINT)((UINT8*)(pTxBuffer->pBuffer))[3],
(UINT)((UINT8*)(pTxBuffer->pBuffer))[4],
(UINT)((UINT8*)(pTxBuffer->pBuffer))[5]);
}
}
}
else
{
TRACE("%s: no TxB: DstMAC=%02X%02X%02X%02X%02X%02X\n", __func__,
((UINT8*)pPktData_p)[0],
((UINT8*)pPktData_p)[1],
((UINT8*)pPktData_p)[2],
((UINT8*)pPktData_p)[3],
((UINT8*)pPktData_p)[4],
((UINT8*)pPktData_p)[5]);
}
}
}
