diff --git a/src/platforms/posix/work_queue/dq_addlast.c b/src/platforms/posix/work_queue/dq_addlast.c index 3a2ec3dbea..35e42d46d3 100644 --- a/src/platforms/posix/work_queue/dq_addlast.c +++ b/src/platforms/posix/work_queue/dq_addlast.c @@ -56,20 +56,18 @@ * ************************************************************/ -void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue) +void dq_addlast(dq_entry_t *node, dq_queue_t *queue) { - node->flink = NULL; - node->blink = queue->tail; + node->flink = NULL; + node->blink = queue->tail; - if (!queue->head) - { - queue->head = node; - queue->tail = node; - } - else - { - queue->tail->flink = node; - queue->tail = node; - } + if (!queue->head) { + queue->head = node; + queue->tail = node; + + } else { + queue->tail->flink = node; + queue->tail = node; + } } diff --git a/src/platforms/posix/work_queue/dq_rem.c b/src/platforms/posix/work_queue/dq_rem.c index 21247add10..e6c9bbed37 100644 --- a/src/platforms/posix/work_queue/dq_rem.c +++ b/src/platforms/posix/work_queue/dq_rem.c @@ -56,30 +56,26 @@ * ************************************************************/ -void dq_rem(FAR dq_entry_t *node, dq_queue_t *queue) +void dq_rem(dq_entry_t *node, dq_queue_t *queue) { - FAR dq_entry_t *prev = node->blink; - FAR dq_entry_t *next = node->flink; + dq_entry_t *prev = node->blink; + dq_entry_t *next = node->flink; - if (!prev) - { - queue->head = next; - } - else - { - prev->flink = next; - } + if (!prev) { + queue->head = next; - if (!next) - { - queue->tail = prev; - } - else - { - next->blink = prev; - } + } else { + prev->flink = next; + } - node->flink = NULL; - node->blink = NULL; + if (!next) { + queue->tail = prev; + + } else { + next->blink = prev; + } + + node->flink = NULL; + node->blink = NULL; } diff --git a/src/platforms/posix/work_queue/dq_remfirst.c b/src/platforms/posix/work_queue/dq_remfirst.c index fc80923cb2..10394d1063 100644 --- a/src/platforms/posix/work_queue/dq_remfirst.c +++ b/src/platforms/posix/work_queue/dq_remfirst.c @@ -56,28 +56,26 @@ * ************************************************************/ -FAR dq_entry_t *dq_remfirst(dq_queue_t *queue) +dq_entry_t *dq_remfirst(dq_queue_t *queue) { - FAR dq_entry_t *ret = queue->head; + dq_entry_t *ret = queue->head; - if (ret) - { - FAR dq_entry_t *next = ret->flink; - if (!next) - { - queue->head = NULL; - queue->tail = NULL; - } - else - { - queue->head = next; - next->blink = NULL; - } + if (ret) { + dq_entry_t *next = ret->flink; - ret->flink = NULL; - ret->blink = NULL; - } + if (!next) { + queue->head = NULL; + queue->tail = NULL; - return ret; + } else { + queue->head = next; + next->blink = NULL; + } + + ret->flink = NULL; + ret->blink = NULL; + } + + return ret; } diff --git a/src/platforms/posix/work_queue/hrt_queue.c b/src/platforms/posix/work_queue/hrt_queue.c index 27ac1110fa..4238f0733e 100644 --- a/src/platforms/posix/work_queue/hrt_queue.c +++ b/src/platforms/posix/work_queue/hrt_queue.c @@ -103,31 +103,31 @@ int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay) { - struct wqueue_s *wqueue = &g_hrt_work; + struct wqueue_s *wqueue = &g_hrt_work; - /* First, initialize the work structure */ + /* First, initialize the work structure */ - work->worker = worker; /* Work callback */ - work->arg = arg; /* Callback argument */ - work->delay = delay; /* Delay until work performed */ + work->worker = worker; /* Work callback */ + work->arg = arg; /* Callback argument */ + work->delay = delay; /* Delay until work performed */ - /* Now, time-tag that entry and put it in the work queue. This must be - * done with interrupts disabled. This permits this function to be called - * from with task logic or interrupt handlers. - */ + /* Now, time-tag that entry and put it in the work queue. This must be + * done with interrupts disabled. This permits this function to be called + * from with task logic or interrupt handlers. + */ - hrt_work_lock(); - work->qtime = hrt_absolute_time(); /* Time work queued */ - //PX4_INFO("hrt work_queue adding work delay=%u time=%lu", delay, work->qtime); + hrt_work_lock(); + work->qtime = hrt_absolute_time(); /* Time work queued */ + //PX4_INFO("hrt work_queue adding work delay=%u time=%lu", delay, work->qtime); - dq_addlast((dq_entry_t *)work, &wqueue->q); + dq_addlast((dq_entry_t *)work, &wqueue->q); #ifdef __PX4_QURT - px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */ + px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */ #else - px4_task_kill(wqueue->pid, SIGCONT); /* Wake up the worker thread */ + px4_task_kill(wqueue->pid, SIGCONT); /* Wake up the worker thread */ #endif - hrt_work_unlock(); - return PX4_OK; + hrt_work_unlock(); + return PX4_OK; } diff --git a/src/platforms/posix/work_queue/hrt_thread.c b/src/platforms/posix/work_queue/hrt_thread.c index dc0f3baa97..49a0a17a6a 100644 --- a/src/platforms/posix/work_queue/hrt_thread.c +++ b/src/platforms/posix/work_queue/hrt_thread.c @@ -89,103 +89,103 @@ static void hrt_work_process(void); static void hrt_work_process() { - struct wqueue_s *wqueue = &g_hrt_work; - volatile FAR struct work_s *work; - worker_t worker; - FAR void *arg; - uint64_t elapsed; - uint32_t remaining; - uint32_t next; - - /* Then process queued work. We need to keep interrupts disabled while - * we process items in the work list. - */ - - /* Default to sleeping for 1 sec */ - next = 1000000; - - hrt_work_lock(); - - work = (FAR 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 - * zero. Therefore a delay of zero will always execute immediately. - */ - - elapsed = hrt_absolute_time() - work->qtime; - //PX4_INFO("hrt work_process: in usec elapsed=%lu delay=%u work=%p", elapsed, work->delay, work); - if (elapsed >= work->delay) - { - /* Remove the ready-to-execute work from the list */ - - (void)dq_rem((struct dq_entry_s *)work, &wqueue->q); - //PX4_INFO("Dequeued work=%p", work); - - /* Extract the work description from the entry (in case the work - * instance by the re-used after it has been de-queued). - */ - - worker = work->worker; - arg = work->arg; - - /* Mark the work as no longer being queued */ - - work->worker = NULL; - - /* Do the work. Re-enable interrupts while the work is being - * performed... we don't have any idea how long that will take! - */ - - hrt_work_unlock(); - if (!worker) { - PX4_ERR("MESSED UP: worker = 0"); - } - 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 - * back at the head of the list. - */ - - hrt_work_lock(); - work = (FAR 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 = work->delay - elapsed; - //PX4_INFO("remaining=%u delay=%u elapsed=%lu", remaining, work->delay, elapsed); - if (remaining < next) - { - /* Yes.. Then schedule to wake up when the work is ready */ - - next = remaining; - } - - /* Then try the next in the list. */ - - work = (FAR struct work_s *)work->dq.flink; - //PX4_INFO("next %u work %p", next, work); - } - } - - /* Wait awhile to check the work list. We will wait here until either - * the time elapses or until we are awakened by a signal. - */ - hrt_work_unlock(); - - /* might sleep less if a signal received and new item was queued */ - //PX4_INFO("Sleeping for %u usec", next); - usleep(next); + struct wqueue_s *wqueue = &g_hrt_work; + volatile struct work_s *work; + worker_t worker; + void *arg; + uint64_t elapsed; + uint32_t remaining; + uint32_t next; + + /* Then process queued work. We need to keep interrupts disabled while + * we process items in the work list. + */ + + /* Default to sleeping for 1 sec */ + next = 1000000; + + hrt_work_lock(); + + 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 + * zero. Therefore a delay of zero will always execute immediately. + */ + + elapsed = hrt_absolute_time() - work->qtime; + + //PX4_INFO("hrt work_process: in usec elapsed=%lu delay=%u work=%p", elapsed, work->delay, work); + if (elapsed >= work->delay) { + /* Remove the ready-to-execute work from the list */ + + (void)dq_rem((struct dq_entry_s *)work, &wqueue->q); + //PX4_INFO("Dequeued work=%p", work); + + /* Extract the work description from the entry (in case the work + * instance by the re-used after it has been de-queued). + */ + + worker = work->worker; + arg = work->arg; + + /* Mark the work as no longer being queued */ + + work->worker = NULL; + + /* Do the work. Re-enable interrupts while the work is being + * performed... we don't have any idea how long that will take! + */ + + hrt_work_unlock(); + + if (!worker) { + PX4_ERR("MESSED UP: worker = 0"); + + } 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 + * back at the head of the list. + */ + + hrt_work_lock(); + 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 = work->delay - elapsed; + + //PX4_INFO("remaining=%u delay=%u elapsed=%lu", remaining, work->delay, elapsed); + if (remaining < next) { + /* Yes.. Then schedule to wake up when the work is ready */ + + next = remaining; + } + + /* Then try the next in the list. */ + + work = (struct work_s *)work->dq.flink; + //PX4_INFO("next %u work %p", next, work); + } + } + + /* Wait awhile to check the work list. We will wait here until either + * the time elapses or until we are awakened by a signal. + */ + hrt_work_unlock(); + + /* might sleep less if a signal received and new item was queued */ + //PX4_INFO("Sleeping for %u usec", next); + usleep(next); } /**************************************************************************** @@ -215,25 +215,24 @@ static void hrt_work_process() static int work_hrtthread(int argc, char *argv[]) { - /* Loop forever */ + /* Loop forever */ - 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). - * NOTE: If the work thread is disabled, this clean-up is performed by - * the IDLE thread (at a very, very low priority). - */ + 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). + * NOTE: If the work thread is disabled, this clean-up is performed by + * the IDLE thread (at a very, very low priority). + */ - /* Then process queued work. We need to keep interrupts disabled while - * we process items in the work list. - */ + /* Then process queued work. We need to keep interrupts disabled while + * we process items in the work list. + */ - hrt_work_process(); - } + hrt_work_process(); + } - return PX4_OK; /* To keep some compilers happy */ + return PX4_OK; /* To keep some compilers happy */ } /**************************************************************************** @@ -246,11 +245,11 @@ void hrt_work_queue_init(void) // Create high priority worker thread g_hrt_work.pid = px4_task_spawn_cmd("wkr_hrt", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX, - 2000, - work_hrtthread, - (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX, + 2000, + work_hrtthread, + (char *const *)NULL); } diff --git a/src/platforms/posix/work_queue/hrt_work_cancel.c b/src/platforms/posix/work_queue/hrt_work_cancel.c index c1c2c3bef7..d8c5957c0b 100644 --- a/src/platforms/posix/work_queue/hrt_work_cancel.c +++ b/src/platforms/posix/work_queue/hrt_work_cancel.c @@ -72,7 +72,7 @@ * * Description: * Cancel previously queued work. This removes work from the work queue. - * After work has been canceled, it may be re-queue by calling + * After work has been canceled, it may be re-queue by calling * hrt_work_queue() again. * * Input parameters: @@ -82,30 +82,30 @@ void hrt_work_cancel(struct work_s *work) { - struct wqueue_s *wqueue = &g_hrt_work; + struct wqueue_s *wqueue = &g_hrt_work; - //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); + //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); - /* Cancelling the work is simply a matter of removing the work structure - * from the work queue. This must be done with interrupts disabled because - * new work is typically added to the work queue from interrupt handlers. - */ + /* Cancelling the work is simply a matter of removing the work structure + * from the work queue. This must be done with interrupts disabled because + * new work is typically added to the work queue from interrupt handlers. + */ - hrt_work_lock(); - if (work->worker != NULL) - { - /* A little test of the integrity of the work queue */ + hrt_work_lock(); - //DEBUGASSERT(work->dq.flink ||(FAR dq_entry_t *)work == wqueue->q.tail); - //DEBUGASSERT(work->dq.blink ||(FAR dq_entry_t *)work == wqueue->q.head); + if (work->worker != NULL) { + /* A little test of the integrity of the work queue */ - /* Remove the entry from the work queue and make sure that it is - * mark as availalbe (i.e., the worker field is nullified). - */ + //DEBUGASSERT(work->dq.flink ||(dq_entry_t *)work == wqueue->q.tail); + //DEBUGASSERT(work->dq.blink ||(dq_entry_t *)work == wqueue->q.head); - dq_rem((FAR dq_entry_t *)work, &wqueue->q); - work->worker = NULL; - } + /* Remove the entry from the work queue and make sure that it is + * mark as availalbe (i.e., the worker field is nullified). + */ - hrt_work_unlock(); + dq_rem((dq_entry_t *)work, &wqueue->q); + work->worker = NULL; + } + + hrt_work_unlock(); } diff --git a/src/platforms/posix/work_queue/queue.c b/src/platforms/posix/work_queue/queue.c index 826cc2d163..6f9d5b7bde 100644 --- a/src/platforms/posix/work_queue/queue.c +++ b/src/platforms/posix/work_queue/queue.c @@ -42,61 +42,57 @@ __EXPORT void sq_rem(sq_entry_t *node, sq_queue_t *queue); sq_entry_t *sq_remafter(sq_entry_t *node, sq_queue_t *queue); void sq_rem(sq_entry_t *node, sq_queue_t *queue) { - if (queue->head && node) - { - if (node == queue->head) - { - queue->head = node->flink; - if (node == queue->tail) - { - queue->tail = NULL; - } - } - else - { - sq_entry_t *prev; - for(prev = (sq_entry_t*)queue->head; - prev && prev->flink != node; - prev = prev->flink); - - if (prev) - { - sq_remafter(prev, queue); - } - } - } + if (queue->head && node) { + if (node == queue->head) { + queue->head = node->flink; + + if (node == queue->tail) { + queue->tail = NULL; + } + + } else { + sq_entry_t *prev; + + for (prev = (sq_entry_t *)queue->head; + prev && prev->flink != node; + prev = prev->flink); + + if (prev) { + sq_remafter(prev, queue); + } + } + } } sq_entry_t *sq_remafter(sq_entry_t *node, sq_queue_t *queue) { - sq_entry_t *ret = node->flink; - if (queue->head && ret) - { - if (queue->tail == ret) - { - queue->tail = node; - node->flink = NULL; - } - else - { - node->flink = ret->flink; - } - - ret->flink = NULL; - } - - return ret; + sq_entry_t *ret = node->flink; + + if (queue->head && ret) { + if (queue->tail == ret) { + queue->tail = node; + node->flink = NULL; + + } else { + node->flink = ret->flink; + } + + ret->flink = NULL; + } + + return ret; } __EXPORT void sq_addfirst(sq_entry_t *node, sq_queue_t *queue); void sq_addfirst(sq_entry_t *node, sq_queue_t *queue) { - node->flink = queue->head; - if (!queue->head) - { - queue->tail = node; - } - queue->head = node; + node->flink = queue->head; + + if (!queue->head) { + queue->tail = node; + } + + queue->head = node; } diff --git a/src/platforms/posix/work_queue/sq_addafter.c b/src/platforms/posix/work_queue/sq_addafter.c index 5d47feba0f..5200275253 100644 --- a/src/platforms/posix/work_queue/sq_addafter.c +++ b/src/platforms/posix/work_queue/sq_addafter.c @@ -56,16 +56,14 @@ * ************************************************************/ -void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, - sq_queue_t *queue) +void sq_addafter(sq_entry_t *prev, sq_entry_t *node, + sq_queue_t *queue) { - if (!queue->head || prev == queue->tail) - { - sq_addlast(node, queue); - } - else - { - node->flink = prev->flink; - prev->flink = node; - } + if (!queue->head || prev == queue->tail) { + sq_addlast(node, queue); + + } else { + node->flink = prev->flink; + prev->flink = node; + } } diff --git a/src/platforms/posix/work_queue/sq_addlast.c b/src/platforms/posix/work_queue/sq_addlast.c index 25910bc9d7..1a0ff88a44 100644 --- a/src/platforms/posix/work_queue/sq_addlast.c +++ b/src/platforms/posix/work_queue/sq_addlast.c @@ -56,18 +56,17 @@ * the 'queue' ************************************************************/ -void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue) +void sq_addlast(sq_entry_t *node, sq_queue_t *queue) { - node->flink = NULL; - if (!queue->head) - { - queue->head = node; - queue->tail = node; - } - else - { - queue->tail->flink = node; - queue->tail = node; - } + node->flink = NULL; + + if (!queue->head) { + queue->head = node; + queue->tail = node; + + } else { + queue->tail->flink = node; + queue->tail = node; + } } diff --git a/src/platforms/posix/work_queue/sq_remfirst.c b/src/platforms/posix/work_queue/sq_remfirst.c index 81751e8579..42b0b6599d 100644 --- a/src/platforms/posix/work_queue/sq_remfirst.c +++ b/src/platforms/posix/work_queue/sq_remfirst.c @@ -57,21 +57,20 @@ * ************************************************************/ -FAR sq_entry_t *sq_remfirst(sq_queue_t *queue) +sq_entry_t *sq_remfirst(sq_queue_t *queue) { - FAR sq_entry_t *ret = queue->head; + sq_entry_t *ret = queue->head; - if (ret) - { - queue->head = ret->flink; - if (!queue->head) - { - queue->tail = NULL; - } + if (ret) { + queue->head = ret->flink; - ret->flink = NULL; - } + if (!queue->head) { + queue->tail = NULL; + } - return ret; + ret->flink = NULL; + } + + return ret; } diff --git a/src/platforms/posix/work_queue/work_cancel.c b/src/platforms/posix/work_queue/work_cancel.c index 827c1ce4c8..de6a4c3bdc 100644 --- a/src/platforms/posix/work_queue/work_cancel.c +++ b/src/platforms/posix/work_queue/work_cancel.c @@ -88,33 +88,33 @@ int work_cancel(int qid, struct work_s *work) { - struct wqueue_s *wqueue = &g_work[qid]; + struct wqueue_s *wqueue = &g_work[qid]; - //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); + //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); - /* Cancelling the work is simply a matter of removing the work structure - * from the work queue. This must be done with interrupts disabled because - * new work is typically added to the work queue from interrupt handlers. - */ + /* Cancelling the work is simply a matter of removing the work structure + * from the work queue. This must be done with interrupts disabled because + * new work is typically added to the work queue from interrupt handlers. + */ - work_lock(qid); - if (work->worker != NULL) - { - /* A little test of the integrity of the work queue */ + work_lock(qid); - //DEBUGASSERT(work->dq.flink ||(FAR dq_entry_t *)work == wqueue->q.tail); - //DEBUGASSERT(work->dq.blink ||(FAR dq_entry_t *)work == wqueue->q.head); + if (work->worker != NULL) { + /* A little test of the integrity of the work queue */ - /* Remove the entry from the work queue and make sure that it is - * mark as availalbe (i.e., the worker field is nullified). - */ + //DEBUGASSERT(work->dq.flink ||(dq_entry_t *)work == wqueue->q.tail); + //DEBUGASSERT(work->dq.blink ||(dq_entry_t *)work == wqueue->q.head); - dq_rem((FAR dq_entry_t *)work, &wqueue->q); - work->worker = NULL; - } + /* Remove the entry from the work queue and make sure that it is + * mark as availalbe (i.e., the worker field is nullified). + */ - work_unlock(qid); - return PX4_OK; + dq_rem((dq_entry_t *)work, &wqueue->q); + work->worker = NULL; + } + + work_unlock(qid); + return PX4_OK; } #endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/posix/work_queue/work_lock.c b/src/platforms/posix/work_queue/work_lock.c index 99464a6a12..3547362816 100644 --- a/src/platforms/posix/work_queue/work_lock.c +++ b/src/platforms/posix/work_queue/work_lock.c @@ -40,12 +40,12 @@ extern sem_t _work_lock[]; void work_lock(int id) { - //PX4_INFO("work_lock %d", id); - sem_wait(&_work_lock[id]); + //PX4_INFO("work_lock %d", id); + sem_wait(&_work_lock[id]); } void work_unlock(int id) { - //PX4_INFO("work_unlock %d", id); - sem_post(&_work_lock[id]); + //PX4_INFO("work_unlock %d", id); + sem_post(&_work_lock[id]); } diff --git a/src/platforms/posix/work_queue/work_queue.c b/src/platforms/posix/work_queue/work_queue.c index 23a5c93638..36d07ca3d9 100644 --- a/src/platforms/posix/work_queue/work_queue.c +++ b/src/platforms/posix/work_queue/work_queue.c @@ -105,33 +105,33 @@ int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay) { - struct wqueue_s *wqueue = &g_work[qid]; + struct wqueue_s *wqueue = &g_work[qid]; - //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); + //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); - /* First, initialize the work structure */ + /* First, initialize the work structure */ - work->worker = worker; /* Work callback */ - work->arg = arg; /* Callback argument */ - work->delay = delay; /* Delay until work performed */ + work->worker = worker; /* Work callback */ + work->arg = arg; /* Callback argument */ + work->delay = delay; /* Delay until work performed */ - /* Now, time-tag that entry and put it in the work queue. This must be - * done with interrupts disabled. This permits this function to be called - * from with task logic or interrupt handlers. - */ + /* Now, time-tag that entry and put it in the work queue. This must be + * done with interrupts disabled. This permits this function to be called + * from with task logic or interrupt handlers. + */ - work_lock(qid); - work->qtime = clock_systimer(); /* Time work queued */ + work_lock(qid); + work->qtime = clock_systimer(); /* Time work queued */ - dq_addlast((dq_entry_t *)work, &wqueue->q); + dq_addlast((dq_entry_t *)work, &wqueue->q); #ifdef __PX4_QURT - px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */ + px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */ #else - px4_task_kill(wqueue->pid, SIGCONT); /* Wake up the worker thread */ + px4_task_kill(wqueue->pid, SIGCONT); /* Wake up the worker thread */ #endif - work_unlock(qid); - return PX4_OK; + work_unlock(qid); + return PX4_OK; } #endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/posix/work_queue/work_thread.c b/src/platforms/posix/work_queue/work_thread.c index 3873034189..9e3a507a4d 100644 --- a/src/platforms/posix/work_queue/work_thread.c +++ b/src/platforms/posix/work_queue/work_thread.c @@ -88,97 +88,98 @@ 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; - worker_t worker; - FAR void *arg; - uint64_t elapsed; - uint32_t remaining; - uint32_t next; - - /* Then process queued work. We need to keep interrupts disabled while - * we process items in the work list. - */ - - next = CONFIG_SCHED_WORKPERIOD; - - work_lock(lock_id); - - work = (FAR 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 - * zero. Therefore a delay of zero will always execute immediately. - */ - - elapsed = USEC2TICK(clock_systimer() - work->qtime); - //printf("work_process: in ticks elapsed=%lu delay=%u\n", 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); - - /* Extract the work description from the entry (in case the work - * instance by the re-used after it has been de-queued). - */ - - worker = work->worker; - arg = work->arg; - - /* Mark the work as no longer being queued */ - - work->worker = NULL; - - /* Do the work. Re-enable interrupts while the work is being - * performed... we don't have any idea how long that will take! - */ - - work_unlock(lock_id); - if (!worker) { - PX4_WARN("MESSED UP: worker = 0\n"); - } - 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 - * back at the head of the list. - */ - - work_lock(lock_id); - work = (FAR 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) - { - /* Yes.. Then schedule to wake up when the work is ready */ - - next = remaining; - } - - /* Then try the next in the list. */ - - work = (FAR struct work_s *)work->dq.flink; - } - } - - /* Wait awhile to check the work list. We will wait here until either - * the time elapses or until we are awakened by a signal. - */ - work_unlock(lock_id); - - usleep(next); + volatile struct work_s *work; + worker_t worker; + void *arg; + uint64_t elapsed; + uint32_t remaining; + uint32_t next; + + /* Then process queued work. We need to keep interrupts disabled while + * we process items in the work list. + */ + + next = CONFIG_SCHED_WORKPERIOD; + + work_lock(lock_id); + + 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 + * zero. Therefore a delay of zero will always execute immediately. + */ + + elapsed = USEC2TICK(clock_systimer() - work->qtime); + + //printf("work_process: in ticks elapsed=%lu delay=%u\n", 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); + + /* Extract the work description from the entry (in case the work + * instance by the re-used after it has been de-queued). + */ + + worker = work->worker; + arg = work->arg; + + /* Mark the work as no longer being queued */ + + work->worker = NULL; + + /* Do the work. Re-enable interrupts while the work is being + * performed... we don't have any idea how long that will take! + */ + + work_unlock(lock_id); + + if (!worker) { + PX4_WARN("MESSED UP: worker = 0\n"); + + } 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 + * back at the head of the list. + */ + + work_lock(lock_id); + 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) { + /* Yes.. Then schedule to wake up when the work is ready */ + + next = remaining; + } + + /* Then try the next in the list. */ + + work = (struct work_s *)work->dq.flink; + } + } + + /* Wait awhile to check the work list. We will wait here until either + * the time elapses or until we are awakened by a signal. + */ + work_unlock(lock_id); + + usleep(next); } /**************************************************************************** @@ -194,19 +195,19 @@ void work_queues_init(void) // Create high priority worker thread g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX-1, - 2000, - work_hpthread, - (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 1, + 2000, + work_hpthread, + (char *const *)NULL); // Create low priority worker thread g_work[LPWORK].pid = px4_task_spawn_cmd("wkr_low", - SCHED_DEFAULT, - SCHED_PRIORITY_MIN, - 2000, - work_lpthread, - (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MIN, + 2000, + work_lpthread, + (char *const *)NULL); } @@ -245,56 +246,54 @@ void work_queues_init(void) int work_hpthread(int argc, char *argv[]) { - /* Loop forever */ + /* Loop forever */ - 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). - * NOTE: If the work thread is disabled, this clean-up is performed by - * the IDLE thread (at a very, very low priority). - */ + 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). + * NOTE: If the work thread is disabled, this clean-up is performed by + * the IDLE thread (at a very, very low priority). + */ #ifndef CONFIG_SCHED_LPWORK - sched_garbagecollection(); + sched_garbagecollection(); #endif - /* Then process queued work. We need to keep interrupts disabled while - * we process items in the work list. - */ + /* Then process queued work. We need to keep interrupts disabled while + * we process items in the work list. + */ - work_process(&g_work[HPWORK], HPWORK); - } + work_process(&g_work[HPWORK], HPWORK); + } - return PX4_OK; /* To keep some compilers happy */ + return PX4_OK; /* To keep some compilers happy */ } #ifdef CONFIG_SCHED_LPWORK int work_lpthread(int argc, char *argv[]) { - /* Loop forever */ + /* Loop forever */ - 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). - * NOTE: If the work thread is disabled, this clean-up is performed by - * the IDLE thread (at a very, very low priority). - */ + 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). + * NOTE: If the work thread is disabled, this clean-up is performed by + * the IDLE thread (at a very, very low priority). + */ - //sched_garbagecollection(); + //sched_garbagecollection(); - /* Then process queued work. We need to keep interrupts disabled while - * we process items in the work list. - */ + /* Then process queued work. We need to keep interrupts disabled while + * we process items in the work list. + */ - work_process(&g_work[LPWORK], LPWORK); - } + work_process(&g_work[LPWORK], LPWORK); + } - return PX4_OK; /* To keep some compilers happy */ + return PX4_OK; /* To keep some compilers happy */ } #endif /* CONFIG_SCHED_LPWORK */ @@ -304,18 +303,17 @@ int work_lpthread(int argc, char *argv[]) int work_usrthread(int argc, char *argv[]) { - /* Loop forever */ + /* Loop forever */ - for (;;) - { - /* Then process queued work. We need to keep interrupts disabled while - * we process items in the work list. - */ + for (;;) { + /* Then process queued work. We need to keep interrupts disabled while + * we process items in the work list. + */ - work_process(&g_work[USRWORK], USRWORK); - } + work_process(&g_work[USRWORK], USRWORK); + } - return PX4_OK; /* To keep some compilers happy */ + return PX4_OK; /* To keep some compilers happy */ } #endif /* CONFIG_SCHED_USRWORK */