自学教程：C++ DEBUG_LVL_ERROR_TRACE函数代码示例

//------------------------------------------------------------------------------static void getMacAdrs(const char* pIfName_p, UINT8* pMacAddr_p){    ULONG                 outBufLen;    PIP_ADAPTER_ADDRESSES pAdapterAddresses;    PIP_ADAPTER_ADDRESSES pAdapter = NULL;    UINT32                retVal = 0;    // search for the corresponding MAC address via IPHLPAPI    outBufLen = sizeof(IP_ADAPTER_ADDRESSES);    pAdapterAddresses = (IP_ADAPTER_ADDRESSES*)OPLK_MALLOC(sizeof(IP_ADAPTER_ADDRESSES));    if (pAdapterAddresses == NULL)    {        DEBUG_LVL_ERROR_TRACE("Error allocating memory needed to call GetAdaptersAdresses/n");        goto Exit;    }    // Make an initial call to GetAdaptersAdresses to get    // the necessary size into the outBufLen variable    retVal = GetAdaptersAddresses(AF_UNSPEC, 0, NULL, pAdapterAddresses, &outBufLen);    if (retVal == ERROR_BUFFER_OVERFLOW)    {        OPLK_FREE(pAdapterAddresses);        pAdapterAddresses = (IP_ADAPTER_ADDRESSES*)OPLK_MALLOC(outBufLen);        if (pAdapterAddresses == NULL)        {            DEBUG_LVL_ERROR_TRACE("Error allocating memory needed to call GetAdaptersAdresses/n");            goto Exit;        }    }    // Get the real values    retVal = GetAdaptersAddresses(AF_UNSPEC, 0, NULL, pAdapterAddresses, &outBufLen);    if (retVal == NO_ERROR)    {        pAdapter = pAdapterAddresses;        while (pAdapter)        {            if (pAdapter->IfType == IF_TYPE_ETHERNET_CSMACD)            {                if (strstr(pIfName_p, pAdapter->AdapterName) != NULL)                {   // corresponding adapter found                    OPLK_MEMCPY(pMacAddr_p, pAdapter->PhysicalAddress, min(pAdapter->PhysicalAddressLength, 6));                    break;                }            }            pAdapter = pAdapter->Next;        }    }    else    {        DEBUG_LVL_ERROR_TRACE("GetAdaptersAddresses failed with error: %d/n", retVal);    }    if (pAdapterAddresses)        OPLK_FREE(pAdapterAddresses);Exit:    return;}

//------------------------------------------------------------------------------tOplkError edrv_allocTxBuffer(tEdrvTxBuffer* pBuffer_p){    tNdisErrorStatus    ndisStatus;    void*               pTxBuffer = NULL;    if (pBuffer_p->maxBufferSize > EDRV_MAX_BUFFER_SIZE)        return kErrorEdrvNoFreeBufEntry;    ndisStatus = ndis_getTxBuff(&pTxBuffer, pBuffer_p->maxBufferSize,                                &pBuffer_p->txBufferNumber.pArg);    if (ndisStatus != kNdisStatusSuccess)    {        DEBUG_LVL_ERROR_TRACE("%s Tx buffers currently not allocated %x/n", __func__, ndisStatus);        return kErrorEdrvNoFreeBufEntry;    }    if ((pTxBuffer != NULL) && (pBuffer_p->txBufferNumber.pArg != NULL))    {        pBuffer_p->pBuffer = (UINT8*)pTxBuffer;    }    else    {        DEBUG_LVL_ERROR_TRACE("%s Error Allocating buffer/n", __func__);        return kErrorEdrvNoFreeBufEntry;    }    return kErrorOk;}

//------------------------------------------------------------------------------static DWORD WINAPI edrvWorkerThread(LPVOID pArgument_p){    tEdrvInstance*  pInstance = (tEdrvInstance*)pArgument_p;    int             pcapRet;    // Increase thread priority    SetThreadPriority(GetCurrentThread(), THREAD_PRIORITY_TIME_CRITICAL);    // Start the pcap capturing loop    // This function is blocking and returns only, if the loop is broken,    // e.g. because of calling pcap_breakloop()    pcapRet = pcap_loop(pInstance->pPcap, -1, packetHandler, (u_char*)pInstance);    // Evaluate the reason of a loop break    switch (pcapRet)    {        case 0:            DEBUG_LVL_ERROR_TRACE("%s(): pcap_loop ended because 'cnt' is exhausted./n", __func__);            break;        case -1:            DEBUG_LVL_ERROR_TRACE("%s(): pcap_loop ended because of an error!/n", __func__);            break;        case -2:            DEBUG_LVL_ERROR_TRACE("%s(): pcap_loop ended normally./n", __func__);            break;        default:            DEBUG_LVL_ERROR_TRACE("%s(): pcap_loop ended (unknown return value)./n", __func__);            break;    }    return 0;}

//------------------------------------------------------------------------------tOplkError ctrlucal_init(void){    tHostifReturn   hifRet;    tHostifConfig   hifConfig;    OPLK_MEMSET(&instance_l, 0, sizeof(instance_l));    OPLK_MEMSET(&hifConfig, 0, sizeof(hifConfig));    hifConfig.instanceNum = 0;    hifConfig.pBase = (UINT8*)HOSTIF_BASE; //FIXME: Get it from somewhere else?    hifConfig.version.revision = HOSTIF_VERSION_REVISION;    hifConfig.version.minor = HOSTIF_VERSION_MINOR;    hifConfig.version.major = HOSTIF_VERSION_MAJOR;    hifRet = hostif_create(&hifConfig, &instance_l.hifInstance);    if (hifRet != kHostifSuccessful)    {        DEBUG_LVL_ERROR_TRACE("Could not initialize host interface (0x%X)/n", hifRet);        return kErrorNoResource;    }    //disable master IRQ    instance_l.fIrqMasterEnable = FALSE;    hifRet = hostif_irqMasterEnable(instance_l.hifInstance, instance_l.fIrqMasterEnable);    if (hifRet != kHostifSuccessful)    {        DEBUG_LVL_ERROR_TRACE("Could not disable master IRQ (0x%X)/n", hifRet);        return kErrorNoResource;    }    return kErrorOk;}

//------------------------------------------------------------------------------static void* getSharedMemory(tHostifInstance pHifInstance_p){    tHostifReturn   hifRet;    void*           pBase;    size_t          span;    hifRet = hostif_getBuf(pHifInstance_p, kHostifInstIdTimesync, &pBase, &span);    if (hifRet != kHostifSuccessful)    {        DEBUG_LVL_ERROR_TRACE("%s() Could not get buffer from host interface (%d)/n",                              __func__,                              hifRet);        return NULL;    }    if (span < sizeof(tTimesyncSharedMemory))    {        DEBUG_LVL_ERROR_TRACE("%s() Time Synchronization Buffer too small (shall be: %lu)/n",                              __func__,                              (ULONG)sizeof(tTimesyncSharedMemory));        return NULL;    }    return pBase;}

//------------------------------------------------------------------------------tCircBufInstance* circbuf_createInstance(UINT8 id_p, BOOL fNew_p){    tCircBufInstance*           pInstance;    tCircBufArchInstance*       pArch;    TCHAR                       mutexName[MAX_PATH];    UNUSED_PARAMETER(fNew_p);    if ((pInstance = OPLK_MALLOC(sizeof(tCircBufInstance) +                                 sizeof(tCircBufArchInstance))) == 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(mutexName, "Local//circbufMutex%d", id_p);    if ((pArch->lockMutex = CreateMutex(NULL, FALSE, mutexName)) == NULL)    {        DEBUG_LVL_ERROR_TRACE("%s() creating mutex failed!/n", __func__);        OPLK_FREE(pInstance);        return NULL;    }    return pInstance;}

//------------------------------------------------------------------------------tOplkError timesyncucal_init(tSyncCb pfnSyncCb_p){    tHostifReturn   hifRet;    OPLK_MEMSET(&instance_l, 0, sizeof(tTimesyncucalInstance));    instance_l.pHifInstance = hostif_getInstance(0);    if (instance_l.pHifInstance == NULL)    {        DEBUG_LVL_ERROR_TRACE("%s: Could not find hostif instance!/n", __func__);        return kErrorNoResource;    }    hifRet = hostif_irqRegHdl(instance_l.pHifInstance, kHostifIrqSrcSync, hostifIrqSyncCb);    if (hifRet != kHostifSuccessful)    {        DEBUG_LVL_ERROR_TRACE("%s: Enable irq not possible!/n", __func__);        return kErrorNoResource;    }    instance_l.pfnSyncCb = pfnSyncCb_p;#if defined(CONFIG_INCLUDE_SOC_TIME_FORWARD)    instance_l.pSharedMemory = (tTimesyncSharedMemory*)getSharedMemory(instance_l.pHifInstance);#endif    return kErrorOk;}

//------------------------------------------------------------------------------tOplkError eventucal_init(void){    tOplkError          ret = kErrorOk;    struct sched_param  schedParam;    OPLK_MEMSET(&instance_l, 0, sizeof(tEventuCalInstance));    instance_l.fd = ctrlucal_getFd();    instance_l.fStopKernelThread = FALSE;    instance_l.fStopProcessThread = FALSE;    sem_unlink("/semUserEvent");    // Deinitialize any existing instance of the semaphore    if ((instance_l.semUserData = sem_open("/semUserEvent", O_CREAT | O_RDWR, S_IRWXG, 0)) == SEM_FAILED)        goto Exit;    if (eventucal_initQueueCircbuf(kEventQueueUInt) != kErrorOk)        goto Exit;    if (eventucal_setSignalingCircbuf(kEventQueueUInt, signalUIntEvent) != kErrorOk)        goto Exit;    // Create thread for fetching new user data from kernel    if (pthread_create(&instance_l.kernelEventThreadId, NULL, k2uEventFetchThread, NULL) != 0)        goto Exit;    schedParam.sched_priority = KERNEL_EVENT_FETCH_THREAD_PRIORITY;    if (pthread_setschedparam(instance_l.kernelEventThreadId, SCHED_FIFO, &schedParam) != 0)    {        DEBUG_LVL_ERROR_TRACE("%s(): couldn't set K2U thread scheduling parameters! %d/n",                              __func__,                              schedParam.sched_priority);    }#if (defined(__GLIBC__) && (__GLIBC__ >= 2) && (__GLIBC_MINOR__ >= 12))    pthread_setname_np(instance_l.kernelEventThreadId, "oplk-eventufetch");#endif    // Create thread for processing pending user data    if (pthread_create(&instance_l.processEventThreadId, NULL, eventProcessThread, NULL) != 0)        goto Exit;    schedParam.sched_priority = EVENT_PROCESS_THREAD_PRIORITY;    if (pthread_setschedparam(instance_l.processEventThreadId, SCHED_FIFO, &schedParam) != 0)    {        DEBUG_LVL_ERROR_TRACE("%s(): couldn't set event process thread scheduling parameters! %d/n",                              __func__,                              schedParam.sched_priority);    }#if (defined(__GLIBC__) && (__GLIBC__ >= 2) && (__GLIBC_MINOR__ >= 12))    pthread_setname_np(instance_l.processEventThreadId, "oplk-eventuprocess");#endif    instance_l.fInitialized = TRUE;    return kErrorOk;Exit:    eventucal_exit();    return ret;}

//------------------------------------------------------------------------------tCircBufInstance* circbuf_createInstance(UINT8 id_p, BOOL fNew_p){    tCircBufInstance*           pInstance;    tCircBufArchInstance*       pArch;    char                        semName[16];    if ((pInstance = OPLK_MALLOC(sizeof(tCircBufInstance) +                                 sizeof(tCircBufArchInstance))) == 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 pdoucal_allocateMem(size_t memSize_p, BYTE** ppPdoMem_p){    INT     ret = 0;    *ppPdoMem_p = mmap(NULL, memSize_p + ATOMIC_MEM_OFFSET + getpagesize(), PROT_READ | PROT_WRITE, MAP_SHARED,                       fd_l, 0);    if (*ppPdoMem_p == MAP_FAILED)    {        DEBUG_LVL_ERROR_TRACE("%s() mmap failed!/n", __func__);        *ppPdoMem_p = NULL;        return kErrorNoResource;    }    if ((ret = ioctl(fd_l, PLK_CMD_PDO_MAP_OFFSET, &pdoMemOffset)) != 0)    {        DEBUG_LVL_ERROR_TRACE("%s() error %d/n", __func__, ret);        return kErrorNoResource;    }    else    {        *ppPdoMem_p = (UINT8*)((size_t)(*ppPdoMem_p) + (size_t)pdoMemOffset);    }    return kErrorOk;}

//------------------------------------------------------------------------------static tOplkError createTimestampShm(void){    instance_l.memSize = sizeof(tTimesyncSharedMemory);    // Initialize shared memory for SOC timestamp    instance_l.fd = shm_open(TIMESYNC_TIMESTAMP_SHM, O_RDWR, 0);    if (instance_l.fd < 0)    {        DEBUG_LVL_ERROR_TRACE("%s() Initialization of shared memory failed!/n",                              __func__);        return kErrorNoResource;    }    instance_l.pSharedMemory = (void*)mmap(NULL,                                           instance_l.memSize,     // Memory size                                           PROT_READ | PROT_WRITE, // Map as read and write memory                                           MAP_SHARED,             // Map as shared memory                                           instance_l.fd,          // File descriptor for POWERLINK device                                           0);    // Check for valid memory mapping    if (instance_l.pSharedMemory == MAP_FAILED)    {        DEBUG_LVL_ERROR_TRACE("%s() timesync shared memory mmap failed!/n",                              __func__);        instance_l.pSharedMemory = NULL;        //Unlink timesync shared memory        shm_unlink(TIMESYNC_TIMESTAMP_SHM);        return kErrorNoResource;    }    return kErrorOk;}

//------------------------------------------------------------------------------tOplkError drv_mapPdoMem(UINT8** ppKernelMem_p, UINT8** ppUserMem_p,                         size_t* pMemSize_p){    tOplkError      ret;    // Get PDO memory    ret = pdokcal_getPdoMemRegion((UINT8**)&pdoMemInfo_l.pKernelVa,                                  &pdoMemInfo_l.memSize);    if (ret != kErrorOk || pdoMemInfo_l.pKernelVa == NULL)        return kErrorNoResource;    if (*pMemSize_p > pdoMemInfo_l.memSize)    {        DEBUG_LVL_ERROR_TRACE("%s() Higher Memory requested (Kernel-%d User-%d) !/n",                              __func__, pdoMemInfo_l.memSize, *pMemSize_p);        *pMemSize_p = 0;        return kErrorNoResource;    }    // Allocate new MDL pointing to PDO memory    pdoMemInfo_l.pMdl = IoAllocateMdl(pdoMemInfo_l.pKernelVa, pdoMemInfo_l.memSize, FALSE, FALSE,                                      NULL);    if (pdoMemInfo_l.pMdl == NULL)    {        DEBUG_LVL_ERROR_TRACE("%s() Error allocating MDL !/n", __func__);        return kErrorNoResource;    }    // Update the MDL with physical addresses    MmBuildMdlForNonPagedPool(pdoMemInfo_l.pMdl);    // Map the memory in user space and get the address    pdoMemInfo_l.pUserVa = MmMapLockedPagesSpecifyCache(pdoMemInfo_l.pMdl,    // MDL                                                        UserMode,             // Mode                                                        MmCached,             // Caching                                                        NULL,                 // Address                                                        FALSE,                // Bug-check?                                                        NormalPagePriority);  // Priority    if (pdoMemInfo_l.pUserVa == NULL)    {        MmUnmapLockedPages(pdoMemInfo_l.pUserVa, pdoMemInfo_l.pMdl);        IoFreeMdl(pdoMemInfo_l.pMdl);        DEBUG_LVL_ERROR_TRACE("%s() Error mapping MDL !/n", __func__);        return kErrorNoResource;    }    *ppKernelMem_p = pdoMemInfo_l.pKernelVa;    *ppUserMem_p = pdoMemInfo_l.pUserVa;    *pMemSize_p = pdoMemInfo_l.memSize;    TRACE("Mapped memory info U:%p K:%p size %x", pdoMemInfo_l.pUserVa,                                                 (UINT8*)pdoMemInfo_l.pKernelVa,                                                 pdoMemInfo_l.memSize);    return kErrorOk;}

//------------------------------------------------------------------------------tOplkError timeru_setTimer(tTimerHdl* pTimerHdl_p, ULONG timeInMs_p, tTimerArg argument_p){    tTimeruData*        pData;    struct itimerspec   relTime;    tHrtimerSig         sig;    if (pTimerHdl_p == NULL)        return kErrorTimerInvalidHandle;    pData = (tTimeruData*)OPLK_MALLOC(sizeof(tTimeruData));    if (pData == NULL)    {        DEBUG_LVL_ERROR_TRACE("error allocating user timer memory!/n");        return kErrorNoResource;    }    OPLK_MEMCPY(&pData->timerArg, &argument_p, sizeof(tTimerArg));    addTimer(pData);    sig.sigType = kHrtimerSigMsgQueue;    sig.sigParam.m_signalMq.msgQueue = timeruInstance_l.msgQueue;    sig.sigParam.m_signalMq.m_sigData = (ULONG)pData;    if (hrtimer_create(CLOCK_MONOTONIC, &sig, &pData->timer) != 0)    {        DEBUG_LVL_ERROR_TRACE("%s() Error hrtimer_create!/n", __func__);        return kErrorNoResource;    }    if (timeInMs_p >= 1000)    {        relTime.it_value.tv_sec = (timeInMs_p / 1000);        relTime.it_value.tv_nsec = (timeInMs_p % 1000) * 1000000;    }    else    {        relTime.it_value.tv_sec = 0;        relTime.it_value.tv_nsec = timeInMs_p * 1000000;    }    relTime.it_interval.tv_sec = 0;    relTime.it_interval.tv_nsec = 0;    DEBUG_LVL_TIMERU_TRACE("%s() Set timer:%08x timeInMs_p=%ld/n",                           __func__, *pData, timeInMs_p);    if (hrtimer_settime(pData->timer, 0, &relTime, NULL) < 0)    {        DEBUG_LVL_ERROR_TRACE("%s() Error hrtimer_settime!/n", __func__);        return kErrorTimerNoTimerCreated;    }    *pTimerHdl_p = (tTimerHdl)pData;    return kErrorOk;}

//------------------------------------------------------------------------------static tOplkError addInstance(tDllCalQueueInstance* ppDllCalQueue_p,                              tDllCalQueue dllCalQueue_p){    tOplkError                  ret = kErrorOk;    tCircBufError               error = kCircBufOk;    tDllCalCircBufInstance*     pDllCalCircBufInstance;    pDllCalCircBufInstance = (tDllCalCircBufInstance*)OPLK_MALLOC(sizeof(tDllCalCircBufInstance));    if (pDllCalCircBufInstance == NULL)    {        DEBUG_LVL_ERROR_TRACE("%s() malloc error!/n", __func__);        ret = kErrorNoResource;        goto Exit;    }    //store parameters in instance    pDllCalCircBufInstance->dllCalQueue = dllCalQueue_p;    //initialize shared buffer    switch (pDllCalCircBufInstance->dllCalQueue)    {        case kDllCalQueueTxGen:            error = circbuf_alloc(CIRCBUF_DLLCAL_TXGEN, CONFIG_DLLCAL_BUFFER_SIZE_TX_GEN,                                  &pDllCalCircBufInstance->pCircBufInstance);            break;        case kDllCalQueueTxNmt:            error = circbuf_alloc(CIRCBUF_DLLCAL_TXNMT, CONFIG_DLLCAL_BUFFER_SIZE_TX_NMT,                                  &pDllCalCircBufInstance->pCircBufInstance);            break;        case kDllCalQueueTxSync:            error = circbuf_alloc(CIRCBUF_DLLCAL_TXSYNC, CONFIG_DLLCAL_BUFFER_SIZE_TX_SYNC,                                  &pDllCalCircBufInstance->pCircBufInstance);            break;        case kDllCalQueueTxVeth:            error = circbuf_alloc(CIRCBUF_DLLCAL_TXVETH, CONFIG_DLLCAL_BUFFER_SIZE_TX_VETH,                                  &pDllCalCircBufInstance->pCircBufInstance);            break;        default:            DEBUG_LVL_ERROR_TRACE("%s() Invalid Queue!/n", __func__);            ret = kErrorInvalidInstanceParam;            break;    }    if (error != kCircBufOk)    {        DEBUG_LVL_ERROR_TRACE("%s() circbuf_alloc error!/n", __func__);        ret = kErrorNoResource;        goto Exit;    }    *ppDllCalQueue_p = (tDllCalQueueInstance*)pDllCalCircBufInstance;Exit:    return ret;}

//------------------------------------------------------------------------------tCircBufError circbuf_allocBuffer(tCircBufInstance* pInstance_p, size_t* pSize_p){    char                        shmName[16];    size_t                      size;    tCircBufArchInstance*       pArch;    size_t                      pageSize;    unsigned int i;    pArch = (tCircBufArchInstance*)pInstance_p->pCircBufArchInstance;    sprintf(shmName, "/shmCircbuf-%d", pInstance_p->bufferId);    pageSize = (sizeof(tCircBufHeader) + sysconf(_SC_PAGE_SIZE) - 1) & (~(sysconf(_SC_PAGE_SIZE) - 1));    size = *pSize_p + pageSize;    if ((pArch->fd = shm_open(shmName, O_RDWR | O_CREAT, 0)) < 0)    {        DEBUG_LVL_ERROR_TRACE("%s() shm_open failed!/n", __func__);        return kCircBufNoResource;    }    if (ftruncate(pArch->fd, size) == -1)    {        DEBUG_LVL_ERROR_TRACE("%s() ftruncate failed!/n", __func__);        close(pArch->fd);        shm_unlink(shmName);        return kCircBufNoResource;    }    pInstance_p->pCircBufHeader = mmap(NULL, sizeof(tCircBufHeader),                                       PROT_READ | PROT_WRITE, MAP_SHARED, pArch->fd, 0);    if (pInstance_p->pCircBufHeader == MAP_FAILED)    {        DEBUG_LVL_ERROR_TRACE("%s() mmap header failed!/n", __func__);        close(pArch->fd);        shm_unlink(shmName);        return kCircBufNoResource;    }    pInstance_p->pCircBuf = mmap(NULL, *pSize_p, PROT_READ | PROT_WRITE, MAP_SHARED,                                 pArch->fd, pageSize);    if (pInstance_p->pCircBuf == MAP_FAILED)    {        DEBUG_LVL_ERROR_TRACE("%s() mmap buffer failed!/n", __func__);        munmap(pInstance_p->pCircBufHeader, sizeof(tCircBufHeader));        close(pArch->fd);        shm_unlink(shmName);        return kCircBufNoResource;    }    //pre-fault pages    for(i = 0; i < *pSize_p; i+= pageSize)	*(UINT32*)(pInstance_p->pCircBuf + i) = 0;    return kCircBufOk;}

//------------------------------------------------------------------------------tOplkError timeru_setTimer(tTimerHdl* pTimerHdl_p, ULONG timeInMs_p, tTimerArg argument_p){    tTimeruData*        pData;    struct itimerspec   relTime;    struct itimerspec   curTime;    struct sigevent     sev;    if (pTimerHdl_p == NULL)        return kErrorTimerInvalidHandle;    pData = (tTimeruData*)OPLK_MALLOC(sizeof(tTimeruData));    if (pData == NULL)        return kErrorNoResource;    OPLK_MEMCPY(&pData->timerArgument, &argument_p, sizeof(tTimerArg));    addTimer(pData);    sev.sigev_notify = SIGEV_SIGNAL;    sev.sigev_signo = SIGRTMIN;    sev.sigev_value.sival_ptr = pData;    if (timer_create(CLOCK_MONOTONIC, &sev, &pData->timer) == -1)    {        DEBUG_LVL_ERROR_TRACE("%s() Error creating timer!/n", __func__);        OPLK_FREE(pData);        return kErrorNoResource;    }    if (timeInMs_p >= 1000)    {        relTime.it_value.tv_sec = (timeInMs_p / 1000);        relTime.it_value.tv_nsec = (timeInMs_p % 1000) * 1000000;    }    else    {        relTime.it_value.tv_sec = 0;        relTime.it_value.tv_nsec = timeInMs_p * 1000000;    }    /*DEBUG_LVL_TIMERU_TRACE("%s() Set timer: %p, timeInMs_p=%ld/n",                             __func__, (void *)pData, timeInMs_p); */    relTime.it_interval.tv_sec = 0;    relTime.it_interval.tv_nsec = 0;    if (timer_settime(pData->timer, 0, &relTime, &curTime) < 0)    {        DEBUG_LVL_ERROR_TRACE("%s() Error timer_settime!/n", __func__);        return kErrorTimerNoTimerCreated;    }    *pTimerHdl_p = (tTimerHdl)pData;    return kErrorOk;}

//------------------------------------------------------------------------------tCircBufError circbuf_allocBuffer(tCircBufInstance* pInstance_p, size_t* pSize_p){    char                        shmName[16];    size_t                      size;    tCircBufArchInstance*       pArch;    size_t                      pageSize;    // Check parameter validity    ASSERT(pInstance_p != NULL);    ASSERT(pSize_p != NULL);    pArch = (tCircBufArchInstance*)pInstance_p->pCircBufArchInstance;    sprintf(shmName, "/shmCircbuf-%d", pInstance_p->bufferId);    pageSize = (sizeof(tCircBufHeader) + sysconf(_SC_PAGE_SIZE) - 1) & (~(sysconf(_SC_PAGE_SIZE) - 1));    size = *pSize_p + pageSize;    if ((pArch->fd = shm_open(shmName, O_RDWR | O_CREAT, 0)) < 0)    {        DEBUG_LVL_ERROR_TRACE("%s() shm_open failed!/n", __func__);        return kCircBufNoResource;    }    if (ftruncate(pArch->fd, size) == -1)    {        DEBUG_LVL_ERROR_TRACE("%s() ftruncate failed!/n", __func__);        close(pArch->fd);        shm_unlink(shmName);        return kCircBufNoResource;    }    pInstance_p->pCircBufHeader = mmap(NULL, sizeof(tCircBufHeader),                                       PROT_READ | PROT_WRITE, MAP_SHARED, pArch->fd, 0);    if (pInstance_p->pCircBufHeader == MAP_FAILED)    {        DEBUG_LVL_ERROR_TRACE("%s() mmap header failed!/n", __func__);        close(pArch->fd);        shm_unlink(shmName);        return kCircBufNoResource;    }    pInstance_p->pCircBuf = mmap(NULL, *pSize_p, PROT_READ | PROT_WRITE, MAP_SHARED,                                 pArch->fd, pageSize);    if (pInstance_p->pCircBuf == MAP_FAILED)    {        DEBUG_LVL_ERROR_TRACE("%s() mmap buffer failed! (%s)/n", __func__, strerror(errno));        munmap(pInstance_p->pCircBufHeader, sizeof(tCircBufHeader));        close(pArch->fd);        shm_unlink(shmName);        return kCircBufNoResource;    }    return kCircBufOk;}

//------------------------------------------------------------------------------void ctrlucal_exit(void){    tHostifReturn hifRet;    //disable master irq    instance_l.fIrqMasterEnable = FALSE;    hifRet = hostif_irqMasterEnable(instance_l.hifInstance, instance_l.fIrqMasterEnable);    if (hifRet != kHostifSuccessful)        DEBUG_LVL_ERROR_TRACE("Could not disable Master Irq (0x%X)/n", hifRet);    hifRet = hostif_delete(instance_l.hifInstance);    if (hifRet != kHostifSuccessful)        DEBUG_LVL_ERROR_TRACE("Could not delete Host Inetrface (0x%X)/n", hifRet);}

//------------------------------------------------------------------------------tOplkError hrestimer_init(void){    tOplkError                  ret = kErrorOk;    UINT                        index;    struct sched_param          schedParam;    tHresTimerInfo*             pTimerInfo;    OPLK_MEMSET(&hresTimerInstance_l, 0, sizeof(hresTimerInstance_l));    /* Initialize timer threads for all usable timers. */    for (index = 0; index < TIMER_COUNT; index++)    {        pTimerInfo = &hresTimerInstance_l.aTimerInfo[index];        pTimerInfo->fTerminate = FALSE;#ifdef HIGH_RESK_TIMER_LATENCY_DEBUG        pTimerInfo->maxLatency = 0;        pTimerInfo->minLatency = 999999999;#endif        if (sem_init(&pTimerInfo->syncSem, 0, 0) != 0)        {            DEBUG_LVL_ERROR_TRACE("%s() Couldn't init semaphore!/n", __func__);            return kErrorNoResource;        }        if (pthread_create(&pTimerInfo->timerThreadId, NULL, timerThread, pTimerInfo) != 0)        {            sem_destroy(&pTimerInfo->syncSem);            return kErrorNoResource;        }        schedParam.sched_priority = CONFIG_THREAD_PRIORITY_HIGH;        if (pthread_setschedparam(pTimerInfo->timerThreadId, SCHED_FIFO, &schedParam) != 0)        {            DEBUG_LVL_ERROR_TRACE("%s() Couldn't set thread scheduling parameters!/n", __func__);            sem_destroy(&pTimerInfo->syncSem);            pthread_cancel(pTimerInfo->timerThreadId);            return kErrorNoResource;        }#if (defined(__GLIBC__) && __GLIBC__ >= 2 && __GLIBC_MINOR__ >= 12)        pthread_setname_np(pTimerInfo->timerThreadId, "oplk-hrtimer");#endif    }    return ret;}

//------------------------------------------------------------------------------void eventkcal_getEventForUser(void* pEvent_p, size_t* pSize_p){    tOplkError    error;    BOOL          fRet;    UINT32        timeout = 500;    // Check parameter validity    ASSERT(pEvent_p != NULL);    ASSERT(pSize_p != NULL);    if (!instance_l.fInitialized)        return;    fRet = NdisWaitEvent(&instance_l.userWaitEvent, timeout);    if (fRet && (instance_l.userEventCount == 0))    {        NdisResetEvent(&instance_l.userWaitEvent);        return;    }    NdisResetEvent(&instance_l.userWaitEvent);    // Kernel-to-user queue is processed with higher priority.    if (eventkcal_getEventCountCircbuf(kEventQueueK2U) > 0)    {        NdisInterlockedDecrement(&instance_l.userEventCount);        error = eventkcal_getEventCircbuf(kEventQueueK2U, pEvent_p, pSize_p);        if ((error != kErrorOk) || (pEvent_p == NULL))        {            DEBUG_LVL_ERROR_TRACE("%s() Error reading K2U events %d!/n", __func__, error);        }        return;    }    else if (eventkcal_getEventCountCircbuf(kEventQueueUInt) > 0)    {        NdisInterlockedDecrement(&instance_l.userEventCount);        error = eventkcal_getEventCircbuf(kEventQueueUInt, pEvent_p, pSize_p);        if (error != kErrorOk)        {            DEBUG_LVL_ERROR_TRACE("%s() Error reading UINT events %d!/n", __func__, error);            return;        }    }}

//------------------------------------------------------------------------------tOplkError target_init(void){    tOplkError          oplkRet = kErrorOk;    ALT_STATUS_CODE     halRet = ALT_E_SUCCESS;#ifdef ALTARM_CACHE_ENABLE    // Enable Cache    halRet = alt_cache_system_enable();#else    halRet = alt_cache_system_disable();#endif    if (halRet != ALT_E_SUCCESS)    {        oplkRet = kErrorGeneralError;        goto Exit;    }    // Initialize the global interrupt controller    halRet = alt_int_global_init();    if (halRet != ALT_E_SUCCESS)    {        DEBUG_LVL_ERROR_TRACE("global IRQ controller initialization Failed!!/n");        oplkRet = kErrorGeneralError;        goto Exit;    }    // Initialize the cpu interrupt interface    halRet = alt_int_cpu_init();    if (halRet != ALT_E_SUCCESS)    {        DEBUG_LVL_ERROR_TRACE("CPU IRQ interface initialization Failed!!/n");        oplkRet = kErrorGeneralError;        goto Exit;    }    // Enable global interrupt master    halRet = alt_int_global_enable();    if (halRet != ALT_E_SUCCESS)    {        DEBUG_LVL_ERROR_TRACE("enabling global interrupt receiver failed/n");        oplkRet = kErrorGeneralError;        goto Exit;    }Exit:    return oplkRet;}

//------------------------------------------------------------------------------static void* workerThread(void* pArgument_p){    tEdrvInstance*  pInstance = (tEdrvInstance*)pArgument_p;    int             pcapRet;    int             oldCancelType;    DEBUG_LVL_EDRV_TRACE("%s(): ThreadId:%ld/n", __func__, syscall(SYS_gettid));    pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, &oldCancelType);    // Set up and activate the pcap live capture handle    pInstance->pPcapThread = startPcap();    if (pInstance->pPcapThread == NULL)    {        return NULL;    }    if (pcap_setdirection(pInstance->pPcapThread, PCAP_D_INOUT) < 0)    {        DEBUG_LVL_ERROR_TRACE("%s() couldn't set PCAP direction!/n", __func__);    }   // signal that thread is successfully started   sem_post(&pInstance->syncSem);   pcapRet = pcap_loop(pInstance->pPcapThread, -1, packetHandler, (u_char*)pInstance);   switch (pcapRet)   {       case 0:           DEBUG_LVL_ERROR_TRACE("%s(): pcap_loop ended because 'cnt' is exhausted./n", __func__);           break;       case -1:           DEBUG_LVL_ERROR_TRACE("%s(): pcap_loop ended because of an error!/n", __func__);           break;       case -2:           DEBUG_LVL_ERROR_TRACE("%s(): pcap_loop ended normally./n", __func__);           break;       default:           DEBUG_LVL_ERROR_TRACE("%s(): pcap_loop ended (unknown return value)./n", __func__);           break;   }   return NULL;}

//------------------------------------------------------------------------------NTSTATUS DriverEntry(PDRIVER_OBJECT driverObject_p,                     PUNICODE_STRING registryPath_p){    NDIS_STATUS ndisStatus = NDIS_STATUS_SUCCESS;    DEBUG_LVL_ALWAYS_TRACE("PLK: + %s()/n", __func__);    ndisStatus = ndis_initDriver(driverObject_p, registryPath_p);    if (ndisStatus != NDIS_STATUS_SUCCESS)    {        DEBUG_LVL_ERROR_TRACE("%s() Failed to initialize driver 0x%X/n",                              __func__,                              ndisStatus);        return ndisStatus;    }    // register driver interface handlers    ndis_registerDrvIntf(registerDrvIntf, deregisterDrvIntf);    plkDriverInstance_l.fInitialized = FALSE;    plkDriverInstance_l.instanceCount = 0;    DEBUG_LVL_ALWAYS_TRACE("PLK: + %s() - OK/n", __func__);    return ndisStatus;}

//------------------------------------------------------------------------------tOplkError timesyncucal_init(tSyncCb pfnSyncCb_p){#if defined(CONFIG_INCLUDE_SOC_TIME_FORWARD)    tOplkError  ret;#endif    UNUSED_PARAMETER(pfnSyncCb_p);    OPLK_MEMSET(&instance_l, 0, sizeof(tTimesyncucalInstance));    instance_l.syncSem = sem_open(TIMESYNC_SYNC_BSDSEM, O_CREAT, S_IRWXG, 1);    if (instance_l.syncSem == SEM_FAILED)    {        DEBUG_LVL_ERROR_TRACE("%s() creating sem failed!/n", __func__);        return kErrorNoResource;    }#if defined(CONFIG_INCLUDE_SOC_TIME_FORWARD)    ret = createTimestampShm();    if (ret != kErrorOk)    {        // Close the semaphore        sem_close(instance_l.syncSem);        // Unlink the semaphore        sem_unlink(TIMESYNC_SYNC_BSDSEM);        return ret;    }#endif    return kErrorOk;}

//------------------------------------------------------------------------------tMemMapReturn memmap_shutdown(void){    tMemStruc*  pMemStruc = &memMapInstance_l.memStruc;    ULONG       bytesReturned;    if (!DeviceIoControl(memMapInstance_l.hFileHandle,                         PLK_CMD_UNMAP_MEM,                         pMemStruc,                         sizeof(tMemStruc),                         NULL,                         0,                         &bytesReturned,                         NULL))    {        DEBUG_LVL_ERROR_TRACE("%s() Unable to free mem %d/n", __func__, GetLastError());        return kErrorGeneralError;    }    pMemStruc->pKernelAddr = NULL;    pMemStruc->pUserAddr = NULL;    pMemStruc->size = 0;    memMapInstance_l.hFileHandle = NULL;    return kMemMapOk;}

//------------------------------------------------------------------------------tOplkError ctrlucal_readInitParam(tCtrlInitParam* pInitParam_p){    tOplkError      ret = kErrorOk;    tHostifReturn   hifret;    void*           pInitBase;    void*           pSrc;    // Check parameter validity    ASSERT(pInitParam_p != NULL);    hifret = hostif_getInitParam(instance_l.hifInstance, &pInitBase);    if (hifret != kHostifSuccessful)    {        DEBUG_LVL_ERROR_TRACE("%s() Getting init base failed (0x%X)!/n", __func__, hifret);        ret = kErrorNoResource;        goto Exit;    }    pSrc = getDynBuff((UINT32)pInitBase);    if (pSrc == NULL)        return kErrorNoResource;    OPLK_MEMCPY(pInitParam_p, pSrc, sizeof(tCtrlInitParam));    freeDynBuff(pSrc);Exit:    return ret;}

//------------------------------------------------------------------------------tOplkError timesyncu_getSocTime(tOplkApiSocTimeInfo* pSocTime_p){    tTimesyncSocTime*   pSocTime;    if (pSocTime_p == NULL)        return kErrorNoResource;    // Check if the shared memory is available    if (pSharedMemory_l == NULL)    {        DEBUG_LVL_ERROR_TRACE("%s Pointer to shared memory is invalid!/n",                              __func__);        return kErrorNoResource;    }    pSocTime = getSocTime();    if (pSocTime != NULL)    {        // Assign timesync to api structure members        pSocTime_p->fValidRelTime = (pSocTime->fRelTimeValid != 0);        pSocTime_p->relTime = pSocTime->relTime;        pSocTime_p->netTime = pSocTime->netTime;    }    return kErrorOk;}

//------------------------------------------------------------------------------static tOplkError sendGenericAsyncFrame(const tFrameInfo* pFrameInfo_p){    tOplkError  ret = kErrorOk;    UINT16      etherType = ami_getUint16Be(&pFrameInfo_p->frame.pBuffer->etherType);    if ((etherType == 0x0000) || (etherType == C_DLL_ETHERTYPE_EPL))    {        ret = instance_l.pTxGenFuncs->pfnInsertDataBlock(                                          instance_l.dllCalQueueTxGen,                                          (const UINT8*)pFrameInfo_p->frame.pBuffer,                                          pFrameInfo_p->frameSize);    }    else    {#if defined(CONFIG_INCLUDE_VETH)        ret = instance_l.pTxVethFuncs->pfnInsertDataBlock(                                           instance_l.dllCalQueueTxVeth,                                           (const UINT8*)pFrameInfo_p->frame.pBuffer,                                           pFrameInfo_p->frameSize);#else    // Return error since virtual Ethernet is not existing!    ret = kErrorIllegalInstance;    DEBUG_LVL_ERROR_TRACE("%s() frame cannot be sent, "                          "because virtual Ethernet is inactive!/n",                          __func__);#endif    }    return ret;}

//------------------------------------------------------------------------------static void* processThread(void* pArgument_p){    tTimeruData*    pTimer;    sigset_t        awaitedSignal;    siginfo_t       signalInfo;    UNUSED_PARAMETER(pArgument_p);    // Uncomment to show the thread ID on Linux (include must also be uncommented)!    // DEBUG_LVL_TIMERU_TRACE("%s() ThreadId:%d/n", __func__, syscall(SYS_gettid));    sigemptyset(&awaitedSignal);    sigaddset(&awaitedSignal, SIGRTMIN);    pthread_sigmask(SIG_BLOCK, &awaitedSignal, NULL);    /* loop forever until thread will be canceled */    while (1)    {        if (sigwaitinfo(&awaitedSignal, &signalInfo) > 0)        {            pTimer = (tTimeruData*)signalInfo.si_value.sival_ptr;            if (pTimer != NULL)                /* call callback function of timer */                cbTimer((ULONG)pTimer);            else            {                DEBUG_LVL_ERROR_TRACE("%s() sival_ptr==NULL code=%d/n", __func__,                                      signalInfo.si_code);            }        }    }    DEBUG_LVL_TIMERU_TRACE("%s() Exiting!/n", __func__);    return NULL;}

//------------------------------------------------------------------------------static void callTimerCb(UINT index_p){    tHresTimerInfo* pTimerInfo;    // Get the timer info according to the index    pTimerInfo = &hresTimerInstance_l.aTimerInfo[index_p];    // Check if the timer is a periodic timer    if (pTimerInfo->dueTime.QuadPart != 0)    {        HANDLE  hTimer;        BOOL    fRet;        // Set up the timer again        hTimer = hresTimerInstance_l.aHandle[index_p + HRTIMER_HDL_TIMER0];        fRet = SetWaitableTimer(hTimer, &pTimerInfo->dueTime, 0L, NULL, NULL, 0);        if (!fRet)        {            DEBUG_LVL_ERROR_TRACE("SetWaitableTimer failed (%d)/n", GetLastError());            return;        }    }    // If a callback function is given, call it    if (pTimerInfo->pfnCallback != NULL)    {        pTimerInfo->pfnCallback(&pTimerInfo->eventArg);    }}