//------------------------------------------------------------------------------ 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]); } } }