|
|
|
@ -88,11 +88,11 @@ sem_t _work_lock[NWORKERS];
@@ -88,11 +88,11 @@ sem_t _work_lock[NWORKERS];
|
|
|
|
|
* |
|
|
|
|
****************************************************************************/ |
|
|
|
|
|
|
|
|
|
static void work_process(FAR struct wqueue_s *wqueue, int lock_id) |
|
|
|
|
static void work_process(struct wqueue_s *wqueue, int lock_id) |
|
|
|
|
{ |
|
|
|
|
volatile FAR struct work_s *work; |
|
|
|
|
volatile struct work_s *work; |
|
|
|
|
worker_t worker; |
|
|
|
|
FAR void *arg; |
|
|
|
|
void *arg; |
|
|
|
|
uint64_t elapsed; |
|
|
|
|
uint32_t remaining; |
|
|
|
|
uint32_t next; |
|
|
|
@ -105,9 +105,9 @@ static void work_process(FAR struct wqueue_s *wqueue, int lock_id)
@@ -105,9 +105,9 @@ static void work_process(FAR struct wqueue_s *wqueue, int lock_id)
|
|
|
|
|
|
|
|
|
|
work_lock(lock_id); |
|
|
|
|
|
|
|
|
|
work = (FAR struct work_s *)wqueue->q.head; |
|
|
|
|
while (work) |
|
|
|
|
{ |
|
|
|
|
work = (struct work_s *)wqueue->q.head; |
|
|
|
|
|
|
|
|
|
while (work) { |
|
|
|
|
/* Is this work ready? It is ready if there is no delay or if
|
|
|
|
|
* the delay has elapsed. qtime is the time that the work was added |
|
|
|
|
* to the work queue. It will always be greater than or equal to |
|
|
|
@ -115,9 +115,9 @@ static void work_process(FAR struct wqueue_s *wqueue, int lock_id)
@@ -115,9 +115,9 @@ static void work_process(FAR struct wqueue_s *wqueue, int lock_id)
|
|
|
|
|
*/ |
|
|
|
|
|
|
|
|
|
elapsed = USEC2TICK(clock_systimer() - work->qtime); |
|
|
|
|
|
|
|
|
|
//printf("work_process: in ticks elapsed=%lu delay=%u\n", elapsed, work->delay);
|
|
|
|
|
if (elapsed >= work->delay) |
|
|
|
|
{ |
|
|
|
|
if (elapsed >= work->delay) { |
|
|
|
|
/* Remove the ready-to-execute work from the list */ |
|
|
|
|
|
|
|
|
|
(void)dq_rem((struct dq_entry_s *)work, &wqueue->q); |
|
|
|
@ -138,11 +138,13 @@ static void work_process(FAR struct wqueue_s *wqueue, int lock_id)
@@ -138,11 +138,13 @@ static void work_process(FAR struct wqueue_s *wqueue, int lock_id)
|
|
|
|
|
*/ |
|
|
|
|
|
|
|
|
|
work_unlock(lock_id); |
|
|
|
|
|
|
|
|
|
if (!worker) { |
|
|
|
|
PX4_WARN("MESSED UP: worker = 0\n"); |
|
|
|
|
} |
|
|
|
|
else |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
worker(arg); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/* Now, unfortunately, since we re-enabled interrupts we don't
|
|
|
|
|
* know the state of the work list and we will have to start |
|
|
|
@ -150,18 +152,17 @@ static void work_process(FAR struct wqueue_s *wqueue, int lock_id)
@@ -150,18 +152,17 @@ static void work_process(FAR struct wqueue_s *wqueue, int lock_id)
|
|
|
|
|
*/ |
|
|
|
|
|
|
|
|
|
work_lock(lock_id); |
|
|
|
|
work = (FAR struct work_s *)wqueue->q.head; |
|
|
|
|
} |
|
|
|
|
else |
|
|
|
|
{ |
|
|
|
|
work = (struct work_s *)wqueue->q.head; |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
/* This one is not ready.. will it be ready before the next
|
|
|
|
|
* scheduled wakeup interval? |
|
|
|
|
*/ |
|
|
|
|
|
|
|
|
|
/* Here: elapsed < work->delay */ |
|
|
|
|
remaining = USEC_PER_TICK * (work->delay - elapsed); |
|
|
|
|
if (remaining < next) |
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
if (remaining < next) { |
|
|
|
|
/* Yes.. Then schedule to wake up when the work is ready */ |
|
|
|
|
|
|
|
|
|
next = remaining; |
|
|
|
@ -169,7 +170,7 @@ static void work_process(FAR struct wqueue_s *wqueue, int lock_id)
@@ -169,7 +170,7 @@ static void work_process(FAR struct wqueue_s *wqueue, int lock_id)
|
|
|
|
|
|
|
|
|
|
/* Then try the next in the list. */ |
|
|
|
|
|
|
|
|
|
work = (FAR struct work_s *)work->dq.flink; |
|
|
|
|
work = (struct work_s *)work->dq.flink; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -247,8 +248,7 @@ int work_hpthread(int argc, char *argv[])
@@ -247,8 +248,7 @@ int work_hpthread(int argc, char *argv[])
|
|
|
|
|
{ |
|
|
|
|
/* Loop forever */ |
|
|
|
|
|
|
|
|
|
for (;;) |
|
|
|
|
{ |
|
|
|
|
for (;;) { |
|
|
|
|
/* First, perform garbage collection. This cleans-up memory de-allocations
|
|
|
|
|
* that were queued because they could not be freed in that execution |
|
|
|
|
* context (for example, if the memory was freed from an interrupt handler). |
|
|
|
@ -276,8 +276,7 @@ int work_lpthread(int argc, char *argv[])
@@ -276,8 +276,7 @@ int work_lpthread(int argc, char *argv[])
|
|
|
|
|
{ |
|
|
|
|
/* Loop forever */ |
|
|
|
|
|
|
|
|
|
for (;;) |
|
|
|
|
{ |
|
|
|
|
for (;;) { |
|
|
|
|
/* First, perform garbage collection. This cleans-up memory de-allocations
|
|
|
|
|
* that were queued because they could not be freed in that execution |
|
|
|
|
* context (for example, if the memory was freed from an interrupt handler). |
|
|
|
@ -306,8 +305,7 @@ int work_usrthread(int argc, char *argv[])
@@ -306,8 +305,7 @@ int work_usrthread(int argc, char *argv[])
|
|
|
|
|
{ |
|
|
|
|
/* Loop forever */ |
|
|
|
|
|
|
|
|
|
for (;;) |
|
|
|
|
{ |
|
|
|
|
for (;;) { |
|
|
|
|
/* Then process queued work. We need to keep interrupts disabled while
|
|
|
|
|
* we process items in the work list. |
|
|
|
|
*/ |
|
|
|
|