//------------------------------------------------------------------------------
static void* timerThread(void* pParm_p)
{
INT iRet;
tHresTimerInfo* pTimerInfo;
sigset_t awaitedSignal;
siginfo_t signalInfo;
UNUSED_PARAMETER(pParm_p);
DEBUG_LVL_TIMERH_TRACE("%s(): ThreadId:%ld\n", __func__, syscall(SYS_gettid));
sigemptyset(&awaitedSignal);
sigaddset(&awaitedSignal, SIGHIGHRES);
pthread_sigmask(SIG_BLOCK, &awaitedSignal, NULL);
/* loop forever until thread will be canceled */
while (1)
{
if ((iRet = sigwaitinfo(&awaitedSignal, &signalInfo)) > 0)
{
pTimerInfo = (tHresTimerInfo*)signalInfo.si_value.sival_ptr;
/* call callback function */
if (pTimerInfo->pfnCallback != NULL)
{
pTimerInfo->pfnCallback(&pTimerInfo->eventArg);
}
}
}
DEBUG_LVL_TIMERH_TRACE("%s() Exiting!\n", __func__);
return NULL;
}
//------------------------------------------------------------------------------
static void restoreTimerResolution(void)
{
LONG winRet = 0;
ULONG current = ~0UL;
// Reset timer resolution to old value
winRet = NtSetTimerResolution(0, FALSE, ¤t);
DEBUG_LVL_TIMERH_TRACE("NtSetTimerResolution returned %ld, current resolution = %lu\n", winRet, current);
// Free library NTDLL.DLL
FreeLibrary(hInstLibNtDll_l);
hInstLibNtDll_l = NULL;
}
//------------------------------------------------------------------------------
tOplkError hrestimer_delInstance(void)
{
tHresTimerInfo* pTimerInfo;
tOplkError ret = kErrorOk;
UINT index;
for (index = 0; index < TIMER_COUNT; index++)
{
pTimerInfo = &hresTimerInstance_l.aTimerInfo[index];
timer_delete(pTimerInfo->timer);
pTimerInfo->eventArg.timerHdl = 0;
pTimerInfo->pfnCallback = NULL;
}
/* send exit signal to thread */
pthread_cancel(hresTimerInstance_l.threadId);
/* wait until thread terminates */
DEBUG_LVL_TIMERH_TRACE("%s() Waiting for thread to exit...\n", __func__);
pthread_join(hresTimerInstance_l.threadId, NULL);
DEBUG_LVL_TIMERH_TRACE("%s() Thread exited!\n", __func__);
return ret;
}
//------------------------------------------------------------------------------
tOplkError hrestimer_deleteTimer(tTimerHdl* pTimerHdl_p)
{
tOplkError ret = kErrorOk;
UINT index;
tHresTimerInfo* pTimerInfo;
struct itimerspec relTime;
DEBUG_LVL_TIMERH_TRACE("%s() Deleting timer:%lx\n", __func__, *pTimerHdl_p);
if (pTimerHdl_p == NULL)
return kErrorTimerInvalidHandle;
if (*pTimerHdl_p == 0)
{ // no timer created yet
return ret;
}
else
{
index = HDL_TO_IDX(*pTimerHdl_p);
if (index >= TIMER_COUNT)
{ // invalid handle
return kErrorTimerInvalidHandle;
}
pTimerInfo = &hresTimerInstance_l.aTimerInfo[index];
if (pTimerInfo->eventArg.timerHdl != *pTimerHdl_p)
{ // invalid handle
return ret;
}
}
// values of 0 disarms the timer
relTime.it_value.tv_sec = 0;
relTime.it_value.tv_nsec = 0;
timer_settime(pTimerInfo->timer, 0, &relTime, NULL);
*pTimerHdl_p = 0;
pTimerInfo->eventArg.timerHdl = 0;
pTimerInfo->pfnCallback = NULL;
return ret;
}
//------------------------------------------------------------------------------
tOplkError hrestimer_deleteTimer(tTimerHdl* pTimerHdl_p)
{
tOplkError ret = kErrorOk;
UINT index;
tHresTimerInfo* pTimerInfo;
HANDLE hTimer;
DEBUG_LVL_TIMERH_TRACE("%s() Deleting timer: %lx\n", __func__, *pTimerHdl_p);
if (pTimerHdl_p == NULL)
return kErrorTimerInvalidHandle;
if (*pTimerHdl_p == 0)
{ // no timer created yet
return ret;
}
else
{
index = (UINT)HDL_TO_IDX(*pTimerHdl_p);
if (index >= TIMER_COUNT)
{ // invalid handle
return kErrorTimerInvalidHandle;
}
pTimerInfo = &hresTimerInstance_l.aTimerInfo[index];
if (pTimerInfo->eventArg.timerHdl.handle != *pTimerHdl_p)
{ // invalid handle
return ret;
}
}
pTimerInfo->pfnCallback = NULL;
*pTimerHdl_p = 0;
// Cancel timer
hTimer = hresTimerInstance_l.aHandle[index + HRTIMER_HDL_TIMER0];
CancelWaitableTimer(hTimer);
return ret;
}
//------------------------------------------------------------------------------
tOplkError hrestimer_modifyTimer(tTimerHdl* pTimerHdl_p, ULONGLONG time_p,
tTimerkCallback pfnCallback_p, ULONG argument_p,
BOOL fContinue_p)
{
tOplkError ret = kErrorOk;
UINT index;
tHresTimerInfo* pTimerInfo;
struct itimerspec relTime;
if (pTimerHdl_p == NULL)
{
DEBUG_LVL_ERROR_TRACE("%s() Invalid timer handle\n", __func__);
return kErrorTimerInvalidHandle;
}
if (*pTimerHdl_p == 0)
{ // no timer created yet, search free timer info structure
pTimerInfo = &hresTimerInstance_l.aTimerInfo[0];
for (index = 0; index < TIMER_COUNT; index++, pTimerInfo++)
{
if (pTimerInfo->eventArg.timerHdl == 0)
break; // free structure found
}
if (index >= TIMER_COUNT)
{ // no free structure found
DEBUG_LVL_ERROR_TRACE("%s() Invalid timer index:%d\n", __func__, index);
return kErrorTimerNoTimerCreated;
}
pTimerInfo->eventArg.timerHdl = HDL_INIT(uiIndex);
}
else
{
index = HDL_TO_IDX(*pTimerHdl_p);
if (index >= TIMER_COUNT)
{ // invalid handle
DEBUG_LVL_ERROR_TRACE("%s() Invalid timer index:%d\n", __func__, index);
return kErrorTimerInvalidHandle;
}
pTimerInfo = &hresTimerInstance_l.aTimerInfo[index];
}
// increase too small time values
if (fContinue_p != FALSE)
{
if (time_p < TIMER_MIN_VAL_CYCLE)
time_p = TIMER_MIN_VAL_CYCLE;
}
else
{
if (time_p < TIMER_MIN_VAL_SINGLE)
time_p = TIMER_MIN_VAL_SINGLE;
}
/* Increment timer handle (if timer expires right after this statement, the user
* would detect an unknown timer handle and discard it) */
pTimerInfo->eventArg.timerHdl = HDL_INC(pTimerInfo->eventArg.timerHdl);
*pTimerHdl_p = pTimerInfo->eventArg.timerHdl;
/* initialize timer info */
pTimerInfo->eventArg.argument.value = argument_p;
pTimerInfo->pfnCallback = pfnCallback_p;
hrtimer_setCallback(pTimerInfo->timer, (void*)pTimerInfo->pfnCallback,
(void*)&pTimerInfo->eventArg);
/*logMsg("set TCB: %p(%p)\n", (int)pTimerInfo->pfnCallback, (int)pTimerInfo->eventArg.argument.value, 0, 0, 0, 0);*/
if (time_p >= 1000000000L)
{
relTime.it_value.tv_sec = (time_p / 1000000000L);
relTime.it_value.tv_nsec = (time_p % 1000000000);
}
else
{
relTime.it_value.tv_sec = 0;
relTime.it_value.tv_nsec = time_p;
}
if (fContinue_p)
{
relTime.it_interval.tv_nsec = relTime.it_value.tv_nsec;
relTime.it_interval.tv_sec = relTime.it_value.tv_sec;
}
else
{
relTime.it_interval.tv_nsec = 0;
relTime.it_interval.tv_sec = 0;
}
#if 0
DEBUG_LVL_TIMERH_TRACE("hrestimer_modifyTimer() timer=%lx ",
pTimerInfo->eventArg.timerHdl);
DEBUG_LVL_TIMERH_TRACE(" timeout=%ld:%ld/%ld:%ld\n",
relTime.it_value.tv_sec, relTime.it_value.tv_nsec,
relTime.it_interval.tv_sec, relTime.it_interval.tv_nsec);
#endif
hrtimer_settime(pTimerInfo->timer, 0, &relTime, NULL);
return ret;
}
//------------------------------------------------------------------------------
tOplkError hrestimer_modifyTimer(tTimerHdl* pTimerHdl_p, ULONGLONG time_p,
tTimerkCallback pfnCallback_p, ULONG argument_p,
BOOL fContinue_p)
{
tOplkError ret = kErrorOk;
UINT index;
tHresTimerInfo* pTimerInfo;
struct itimerspec RelTime;
// check pointer to handle
if (pTimerHdl_p == NULL)
{
DEBUG_LVL_ERROR_TRACE("%s() Invalid timer handle\n", __func__);
return kErrorTimerInvalidHandle;
}
if (*pTimerHdl_p == 0)
{ // no timer created yet -> search free timer info structure
pTimerInfo = &hresTimerInstance_l.aTimerInfo[0];
for (index = 0; index < TIMER_COUNT; index++, pTimerInfo++)
{
if (pTimerInfo->eventArg.timerHdl == 0)
{ // free structure found
break;
}
}
if (index >= TIMER_COUNT)
{ // no free structure found
DEBUG_LVL_ERROR_TRACE("%s() Invalid timer index:%d\n", __func__, index);
return kErrorTimerNoTimerCreated;
}
pTimerInfo->eventArg.timerHdl = HDL_INIT(index);
}
else
{
index = HDL_TO_IDX(*pTimerHdl_p);
if (index >= TIMER_COUNT)
{ // invalid handle
DEBUG_LVL_ERROR_TRACE("%s() Invalid timer index:%d\n", __func__, index);
return kErrorTimerInvalidHandle;
}
pTimerInfo = &hresTimerInstance_l.aTimerInfo[index];
}
// increase too small time values
if (fContinue_p != FALSE)
{
if (time_p < TIMER_MIN_VAL_CYCLE)
time_p = TIMER_MIN_VAL_CYCLE;
}
else
{
if (time_p < TIMER_MIN_VAL_SINGLE)
time_p = TIMER_MIN_VAL_SINGLE;
}
/* increment timer handle
* (if timer expires right after this statement, the user
* would detect an unknown timer handle and discard it) */
pTimerInfo->eventArg.timerHdl = HDL_INC(pTimerInfo->eventArg.timerHdl);
*pTimerHdl_p = pTimerInfo->eventArg.timerHdl;
/* initialize timer info */
pTimerInfo->eventArg.argument.value = argument_p;
pTimerInfo->pfnCallback = pfnCallback_p;
if (time_p >= 1000000000L)
{
RelTime.it_value.tv_sec = (time_p / 1000000000L);
RelTime.it_value.tv_nsec = (time_p % 1000000000);
}
else
{
RelTime.it_value.tv_sec = 0;
RelTime.it_value.tv_nsec = time_p;
}
if (fContinue_p)
{
RelTime.it_interval.tv_nsec = RelTime.it_value.tv_nsec;
RelTime.it_interval.tv_sec = RelTime.it_value.tv_sec;
}
else
{
RelTime.it_interval.tv_nsec = 0;
RelTime.it_interval.tv_sec = 0;
}
DEBUG_LVL_TIMERH_TRACE("%s() timer:%lx timeout=%ld:%ld\n", __func__,
pTimerInfo->eventArg.timerHdl,
RelTime.it_value.tv_sec, RelTime.it_value.tv_nsec);
timer_settime(pTimerInfo->timer, 0, &RelTime, NULL);
return ret;
}
//------------------------------------------------------------------------------
static void* timerThread(void* pArgument_p)
{
INT iRet;
tHresTimerInfo* pTimerInfo;
struct timespec startTime, timeout;
ULONGLONG period;
tTimerHdl timerHdl;
#ifdef HIGH_RESK_TIMER_LATENCY_DEBUG
struct timespec debugtime, curTime;
#endif
DEBUG_LVL_TIMERH_TRACE("%s(): ThreadId:%ld\n", __func__, syscall(SYS_gettid));
DEBUG_LVL_TIMERH_TRACE("%s(): timer:%lx\n", __func__, (unsigned long)pArgument_p);
/* thread parameter contains the address of the timer information structure */
pTimerInfo = (tHresTimerInfo*)pArgument_p;
/* loop forever until thread will be canceled */
while (1)
{
/* wait for semaphore which signals a timer start */
sem_wait(&pTimerInfo->syncSem);
/* check if thread should terminate */
if (pTimerInfo->fTerminate)
{
DEBUG_LVL_TIMERH_TRACE("%s() Exiting signal received!\n", __func__);
break;
}
else
{
/* save timer information into local variables */
startTime = pTimerInfo->startTime;
timerHdl = pTimerInfo->eventArg.timerHdl;
period = pTimerInfo->time;
/* calculate the timeout value for the timer cycle */
timespec_add(&startTime, period, &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)
{
DEBUG_LVL_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, period);
#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->maxLatency)
{
DEBUG_LVL_TIMERH_TRACE("%s() Timer elapsed: max latency=%ld ns\n",
__func__, debugtime.tv_nsec);
pTimerInfo->maxLatency = debugtime.tv_nsec;
}
if (timeout.tv_nsec < pTimerInfo->minLatency)
{
DEBUG_LVL_TIMERH_TRACE("%s() Timer elapsed: min latency=%ld ns\n",
__func__, debugtime.tv_nsec);
pTimerInfo->minLatency = debugtime.tv_nsec;
}
#endif
/* check if timer handle is valid */
if (timerHdl == pTimerInfo->eventArg.timerHdl)
{
/* call callback function */
if (pTimerInfo->pfnCallback != NULL)
{
pTimerInfo->pfnCallback(&pTimerInfo->eventArg);
}
}
/* check if timer handle is still valid. Could be modified in callback! */
if (timerHdl == pTimerInfo->eventArg.timerHdl)
{
if (pTimerInfo->fContinue)
{
/* calculate timeout value for next timer cycle */
timespec_add(&timeout, period, &timeout);
}
}
} while ((pTimerInfo->fContinue) &&
(timerHdl == pTimerInfo->eventArg.timerHdl));
}
}
return NULL;
}
//------------------------------------------------------------------------------
tOplkError hrestimer_modifyTimer(tTimerHdl* pTimerHdl_p, ULONGLONG time_p,
tTimerkCallback pfnCallback_p, ULONG argument_p,
BOOL fContinue_p)
{
tOplkError ret = kErrorOk;
UINT index;
tHresTimerInfo* pTimerInfo;
DEBUG_LVL_TIMERH_TRACE("%s() pTimerHdl_p=%08x/%08x\n",
__func__, (unsigned int)pTimerHdl_p,(unsigned int)*pTimerHdl_p);
if(pTimerHdl_p == NULL)
return kErrorTimerInvalidHandle;
if (*pTimerHdl_p == 0)
{ // no timer created yet
// search free timer info structure
pTimerInfo = &hresTimerInstance_l.aTimerInfo[0];
for (index = 0; index < TIMER_COUNT; index++, pTimerInfo++)
{
if (pTimerInfo->eventArg.timerHdl == 0)
{ // free structure found
break;
}
}
if (index >= TIMER_COUNT)
{ // no free structure found
return kErrorTimerNoTimerCreated;
}
pTimerInfo->eventArg.timerHdl = HDL_INIT(index);
}
else
{
index = HDL_TO_IDX(*pTimerHdl_p);
if (index >= TIMER_COUNT)
{ // invalid handle
return kErrorTimerInvalidHandle;
}
pTimerInfo = &hresTimerInstance_l.aTimerInfo[index];
}
// increase too small time values
if (fContinue_p != FALSE)
{
if (time_p < TIMER_MIN_VAL_CYCLE)
time_p = TIMER_MIN_VAL_CYCLE;
}
else
{
if (time_p < TIMER_MIN_VAL_SINGLE)
time_p = TIMER_MIN_VAL_SINGLE;
}
/* increment timer handle
* (if timer expires right after this statement, the user
* would detect an unknown timer handle and discard it) */
pTimerInfo->eventArg.timerHdl = HDL_INC(pTimerInfo->eventArg.timerHdl);
*pTimerHdl_p = pTimerInfo->eventArg.timerHdl;
/* initialize timer info */
pTimerInfo->eventArg.argument.value = argument_p;
pTimerInfo->pfnCallback = pfnCallback_p;
pTimerInfo->fContinue = fContinue_p;
pTimerInfo->time = time_p;
clock_gettime(CLOCK_MONOTONIC, &pTimerInfo->startTime); // get current time
sem_post(&pTimerInfo->syncSem); /* signal timer start to thread */
return ret;
}