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