static void
hrt_call_invoke(void)
{
struct hrt_call *call;
hrt_abstime deadline;
hrt_lock();
while (true) {
/* get the current time */
hrt_abstime now = hrt_absolute_time();
call = (struct hrt_call *)sq_peek(&callout_queue);
if (call == NULL) {
break;
}
if (call->deadline > now) {
break;
}
sq_rem(&call->link, &callout_queue);
//PX4_INFO("call pop");
/* save the intended deadline for periodic calls */
deadline = call->deadline;
/* zero the deadline, as the call has occurred */
call->deadline = 0;
/* invoke the callout (if there is one) */
if (call->callout) {
// Unlock so we don't deadlock in callback
hrt_unlock();
//PX4_INFO("call %p: %p(%p)", call, call->callout, call->arg);
call->callout(call->arg);
hrt_lock();
}
/* if the callout has a non-zero period, it has to be re-entered */
if (call->period != 0) {
// re-check call->deadline to allow for
// callouts to re-schedule themselves
// using hrt_call_delay()
if (call->deadline <= now) {
call->deadline = deadline + call->period;
//PX4_INFO("call deadline set to %lu now=%lu", call->deadline, now);
}
hrt_call_enter(call);
}
}
hrt_unlock();
}
static void
hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
{
//printf("hrt_call_internal\n");
hrt_lock();
//printf("hrt_call_internal after lock\n");
/* if the entry is currently queued, remove it */
/* note that we are using a potentially uninitialised
entry->link here, but it is safe as sq_rem() doesn't
dereference the passed node unless it is found in the
list. So we potentially waste a bit of time searching the
queue for the uninitialised entry->link but we don't do
anything actually unsafe.
*/
if (entry->deadline != 0)
sq_rem(&entry->link, &callout_queue);
entry->deadline = deadline;
entry->period = interval;
entry->callout = callout;
entry->arg = arg;
hrt_call_enter(entry);
hrt_unlock();
}
static void
hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
{
//PX4_INFO("hrt_call_internal deadline=%lu interval = %lu", deadline, interval);
hrt_lock();
//PX4_INFO("hrt_call_internal after lock");
/* if the entry is currently queued, remove it */
/* note that we are using a potentially uninitialised
entry->link here, but it is safe as sq_rem() doesn't
dereference the passed node unless it is found in the
list. So we potentially waste a bit of time searching the
queue for the uninitialised entry->link but we don't do
anything actually unsafe.
*/
if (entry->deadline != 0)
sq_rem(&entry->link, &callout_queue);
#if 0
// Use this to debug busy CPU that keeps rescheduling with 0 period time
if (interval < HRT_INTERVAL_MIN) {
PX4_ERR("hrt_call_internal interval too short: %" PRIu64, interval);
}
#endif
entry->deadline = deadline;
entry->period = interval;
entry->callout = callout;
entry->arg = arg;
hrt_call_enter(entry);
hrt_unlock();
}
/*
* Remove the entry from the callout list.
*/
void hrt_cancel(struct hrt_call *entry)
{
hrt_lock();
sq_rem(&entry->link, &callout_queue);
entry->deadline = 0;
/* if this is a periodic call being removed by the callout, prevent it from
* being re-entered when the callout returns.
*/
entry->period = 0;
hrt_unlock();
// endif
}
/**
* Timer interrupt handler
*
* This routine simulates a timer interrupt handler
*/
static void
hrt_tim_isr(void *p)
{
//printf("hrt_tim_isr\n");
/* run any callouts that have met their deadline */
hrt_call_invoke();
hrt_lock();
/* and schedule the next interrupt */
hrt_call_reschedule();
hrt_unlock();
}