static msg_t GTimerThreadHandler(void *arg) {
(void)arg;
GTimer *pt;
systime_t tm;
systime_t nxtTimeout;
systime_t lastTime;
GTimerFunction fn;
void *param;
#if CH_USE_REGISTRY
chRegSetThreadName("GTimer");
#endif
nxtTimeout = TIME_INFINITE;
lastTime = 0;
while(1) {
/* Wait for work to do. */
chThdYield(); // Give someone else a go no matter how busy we are
chBSemWaitTimeout(&waitsem, nxtTimeout);
restartTimerChecks:
// Our reference time
tm = chTimeNow();
nxtTimeout = TIME_INFINITE;
/* We need to obtain the mutex */
chMtxLock(&mutex);
if (pTimerHead) {
pt = pTimerHead;
do {
// Do we have something to do for this timer?
if ((pt->flags & GTIMER_FLG_JABBED) || (!(pt->flags & GTIMER_FLG_INFINITE) && TimeIsWithin(pt->when, lastTime, tm))) {
// Is this timer periodic?
if ((pt->flags & GTIMER_FLG_PERIODIC) && pt->period != TIME_IMMEDIATE) {
// Yes - Update ready for the next period
if (!(pt->flags & GTIMER_FLG_INFINITE)) {
// We may have skipped a period.
// We use this complicated formulae rather than a loop
// because the gcc compiler stuffs up the loop so that it
// either loops forever or doesn't get executed at all.
pt->when += ((tm + pt->period - pt->when) / pt->period) * pt->period;
}
// We are definitely no longer jabbed
pt->flags &= ~GTIMER_FLG_JABBED;
} else {
// No - get us off the timers list
if (pt->next == pt->prev)
pTimerHead = 0;
else {
pt->next->prev = pt->prev;
pt->prev->next = pt->next;
if (pTimerHead == pt)
pTimerHead = pt->next;
}
pt->flags = 0;
}
// Call the callback function
fn = pt->fn;
param = pt->param;
chMtxUnlock();
fn(param);
// We no longer hold the mutex, the callback function may have taken a while
// and our list may have been altered so start again!
goto restartTimerChecks;
}
// Find when we next need to wake up
if (!(pt->flags & GTIMER_FLG_INFINITE) && pt->when - tm < nxtTimeout)
nxtTimeout = pt->when - tm;
pt = pt->next;
} while(pt != pTimerHead);
}
// Ready for the next loop
lastTime = tm;
chMtxUnlock();
}
return 0;
}
static DECLARE_THREAD_FUNCTION(GTimerThreadHandler, arg) {
GTimer *pt;
systemticks_t tm;
systemticks_t nxtTimeout;
systemticks_t lastTime;
GTimerFunction fn;
void *param;
(void) arg;
nxtTimeout = TIME_INFINITE;
lastTime = 0;
while(1) {
/* Wait for work to do. */
gfxYield(); // Give someone else a go no matter how busy we are
gfxSemWait(&waitsem, nxtTimeout);
restartTimerChecks:
// Our reference time
tm = gfxSystemTicks();
nxtTimeout = TIME_INFINITE;
/* We need to obtain the mutex */
gfxMutexEnter(&mutex);
if (pTimerHead) {
pt = pTimerHead;
do {
// Do we have something to do for this timer?
if ((pt->flags & GTIMER_FLG_JABBED) || (!(pt->flags & GTIMER_FLG_INFINITE) && TimeIsWithin(pt->when, lastTime, tm))) {
// Is this timer periodic?
if ((pt->flags & GTIMER_FLG_PERIODIC) && pt->period != TIME_IMMEDIATE) {
// Yes - Update ready for the next period
if (!(pt->flags & GTIMER_FLG_INFINITE)) {
// We may have skipped a period.
// We use this complicated formulae rather than a loop
// because the gcc compiler stuffs up the loop so that it
// either loops forever or doesn't get executed at all.
pt->when += ((tm + pt->period - pt->when) / pt->period) * pt->period;
}
// We are definitely no longer jabbed
pt->flags &= ~GTIMER_FLG_JABBED;
} else {
// No - get us off the timers list
if (pt->next == pt)
pTimerHead = 0;
else {
pt->next->prev = pt->prev;
pt->prev->next = pt->next;
if (pTimerHead == pt)
pTimerHead = pt->next;
}
pt->flags = 0;
}
// Call the callback function
fn = pt->fn;
param = pt->param;
gfxMutexExit(&mutex);
fn(param);
// We no longer hold the mutex, the callback function may have taken a while
// and our list may have been altered so start again!
goto restartTimerChecks;
}
// Find when we next need to wake up
if (!(pt->flags & GTIMER_FLG_INFINITE) && pt->when - tm < nxtTimeout)
nxtTimeout = (pt->when - tm)/ticks2ms;
pt = pt->next;
} while(pt != pTimerHead);
}
// Ready for the next loop
lastTime = tm;
gfxMutexExit(&mutex);
}
THREAD_RETURN(0);
}
