Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 18:47:21 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

Merge pull request #2627 from mcharleb/posix_whitespace_fixes

Browse Source 
POSIX: code format changes
This commit is contained in:
Lorenz Meier
2015-08-01 16:44:13 +02:00
parent be9dbe90bc c1317ad463
commit ff1c9da390
14 changed files with 410 additions and 429 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/platforms/posix/work_queue/dq_addlast.c
+11 -13
View File
@@ -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->flink = NULL;
node->blink = queue->tail; node->blink = queue->tail;
if (!queue->head) if (!queue->head) {
{ queue->head = node;
queue->head = node; queue->tail = node;
queue->tail = node;
} } else {
else queue->tail->flink = node;
{ queue->tail = node;
queue->tail->flink = node; }
queue->tail = node;
}
} }
src/platforms/posix/work_queue/dq_rem.c
+17 -21
View File
@@ -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; dq_entry_t *prev = node->blink;
FAR dq_entry_t *next = node->flink; dq_entry_t *next = node->flink;
if (!prev) if (!prev) {
{ queue->head = next;
queue->head = next;
}
else
{
prev->flink = next;
}
if (!next) } else {
{ prev->flink = next;
queue->tail = prev; }
}
else
{
next->blink = prev;
}
node->flink = NULL; if (!next) {
node->blink = NULL; queue->tail = prev;
} else {
next->blink = prev;
}
node->flink = NULL;
node->blink = NULL;
} }
src/platforms/posix/work_queue/dq_remfirst.c
+17 -19
View File
@@ -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) if (ret) {
{ dq_entry_t *next = ret->flink;
FAR dq_entry_t *next = ret->flink;
if (!next)
{
queue->head = NULL;
queue->tail = NULL;
}
else
{
queue->head = next;
next->blink = NULL;
}
ret->flink = NULL; if (!next) {
ret->blink = NULL; queue->head = NULL;
} queue->tail = NULL;
return ret; } else {
queue->head = next;
next->blink = NULL;
}
ret->flink = NULL;
ret->blink = NULL;
}
return ret;
} }
src/platforms/posix/work_queue/hrt_queue.c
+17 -17
View File
@@ -103,31 +103,31 @@
int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay) 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->worker = worker; /* Work callback */
work->arg = arg; /* Callback argument */ work->arg = arg; /* Callback argument */
work->delay = delay; /* Delay until work performed */ work->delay = delay; /* Delay until work performed */
/* Now, time-tag that entry and put it in the work queue. This must be /* 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 * done with interrupts disabled. This permits this function to be called
* from with task logic or interrupt handlers. * from with task logic or interrupt handlers.
*/ */
hrt_work_lock(); hrt_work_lock();
work->qtime = hrt_absolute_time(); /* Time work queued */ work->qtime = hrt_absolute_time(); /* Time work queued */
//PX4_INFO("hrt work_queue adding work delay=%u time=%lu", delay, work->qtime); //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 #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 #else
px4_task_kill(wqueue->pid, SIGCONT); /* Wake up the worker thread */ px4_task_kill(wqueue->pid, SIGCONT); /* Wake up the worker thread */
#endif #endif
hrt_work_unlock(); hrt_work_unlock();
return PX4_OK; return PX4_OK;
} }
src/platforms/posix/work_queue/hrt_thread.c
+96 -97
View File
@@ -89,103 +89,103 @@ static void hrt_work_process(void);
static void hrt_work_process() static void hrt_work_process()
{ {
struct wqueue_s *wqueue = &g_hrt_work; struct wqueue_s *wqueue = &g_hrt_work;
volatile FAR struct work_s *work; volatile struct work_s *work;
worker_t worker; worker_t worker;
FAR void *arg; void *arg;
uint64_t elapsed; uint64_t elapsed;
uint32_t remaining; uint32_t remaining;
uint32_t next; uint32_t next;
/* Then process queued work. We need to keep interrupts disabled while /* Then process queued work. We need to keep interrupts disabled while
* we process items in the work list. * we process items in the work list.
*/ */
/* Default to sleeping for 1 sec */ /* Default to sleeping for 1 sec */
next = 1000000; next = 1000000;
hrt_work_lock(); hrt_work_lock();
work = (FAR struct work_s *)wqueue->q.head; 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; while (work) {
//PX4_INFO("hrt work_process: in usec elapsed=%lu delay=%u work=%p", elapsed, work->delay, work); /* Is this work ready? It is ready if there is no delay or if
if (elapsed >= work->delay) * 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
/* Remove the ready-to-execute work from the list */ * zero. Therefore a delay of zero will always execute immediately.
*/
(void)dq_rem((struct dq_entry_s *)work, &wqueue->q); elapsed = hrt_absolute_time() - work->qtime;
//PX4_INFO("Dequeued work=%p", work);
/* Extract the work description from the entry (in case the work //PX4_INFO("hrt work_process: in usec elapsed=%lu delay=%u work=%p", elapsed, work->delay, work);
* instance by the re-used after it has been de-queued). if (elapsed >= work->delay) {
*/ /* Remove the ready-to-execute work from the list */
worker = work->worker; (void)dq_rem((struct dq_entry_s *)work, &wqueue->q);
arg = work->arg; //PX4_INFO("Dequeued work=%p", work);
/* Mark the work as no longer being queued */ /* Extract the work description from the entry (in case the work
* instance by the re-used after it has been de-queued).
*/
work->worker = NULL; worker = work->worker;
arg = work->arg;
/* Do the work. Re-enable interrupts while the work is being /* Mark the work as no longer being queued */
* performed... we don't have any idea how long that will take!
*/
hrt_work_unlock(); work->worker = NULL;
if (!worker) {
PX4_ERR("MESSED UP: worker = 0");
}
else {
worker(arg);
}
/* Now, unfortunately, since we re-enabled interrupts we don't /* Do the work. Re-enable interrupts while the work is being
* know the state of the work list and we will have to start * performed... we don't have any idea how long that will take!
* back at the head of the list. */
*/
hrt_work_lock(); hrt_work_unlock();
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 */ if (!worker) {
remaining = work->delay - elapsed; PX4_ERR("MESSED UP: worker = 0");
//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; } else {
} worker(arg);
}
/* Then try the next in the list. */ /* 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 = (FAR struct work_s *)work->dq.flink; hrt_work_lock();
//PX4_INFO("next %u work %p", next, work); work = (struct work_s *)wqueue->q.head;
}
}
/* Wait awhile to check the work list. We will wait here until either } else {
* the time elapses or until we are awakened by a signal. /* This one is not ready.. will it be ready before the next
*/ * scheduled wakeup interval?
hrt_work_unlock(); */
/* might sleep less if a signal received and new item was queued */ /* Here: elapsed < work->delay */
//PX4_INFO("Sleeping for %u usec", next); remaining = work->delay - elapsed;
usleep(next);
//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[]) static int work_hrtthread(int argc, char *argv[])
{ {
/* Loop forever */ /* Loop forever */
for (;;) for (;;) {
{ /* First, perform garbage collection. This cleans-up memory de-allocations
/* First, perform garbage collection. This cleans-up memory de-allocations * that were queued because they could not be freed in that execution
* that were queued because they could not be freed in that execution * context (for example, if the memory was freed from an interrupt handler).
* 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
* NOTE: If the work thread is disabled, this clean-up is performed by * the IDLE thread (at a very, very low priority).
* the IDLE thread (at a very, very low priority). */
*/
/* Then process queued work. We need to keep interrupts disabled while /* Then process queued work. We need to keep interrupts disabled while
* we process items in the work list. * 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 // Create high priority worker thread
g_hrt_work.pid = px4_task_spawn_cmd("wkr_hrt", g_hrt_work.pid = px4_task_spawn_cmd("wkr_hrt",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX, SCHED_PRIORITY_MAX,
2000, 2000,
work_hrtthread, work_hrtthread,
(char* const*)NULL); (char *const *)NULL);
} }
src/platforms/posix/work_queue/hrt_work_cancel.c
+19 -19
View File
@@ -82,30 +82,30 @@
void hrt_work_cancel(struct work_s *work) 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 /* Cancelling the work is simply a matter of removing the work structure
* from the work queue. This must be done with interrupts disabled because * from the work queue. This must be done with interrupts disabled because
* new work is typically added to the work queue from interrupt handlers. * new work is typically added to the work queue from interrupt handlers.
*/ */
hrt_work_lock(); hrt_work_lock();
if (work->worker != NULL)
{
/* A little test of the integrity of the work queue */
//DEBUGASSERT(work->dq.flink ||(FAR dq_entry_t *)work == wqueue->q.tail); if (work->worker != NULL) {
//DEBUGASSERT(work->dq.blink ||(FAR dq_entry_t *)work == wqueue->q.head); /* A little test of the integrity of the work queue */
/* Remove the entry from the work queue and make sure that it is //DEBUGASSERT(work->dq.flink ||(dq_entry_t *)work == wqueue->q.tail);
* mark as availalbe (i.e., the worker field is nullified). //DEBUGASSERT(work->dq.blink ||(dq_entry_t *)work == wqueue->q.head);
*/
dq_rem((FAR dq_entry_t *)work, &wqueue->q); /* Remove the entry from the work queue and make sure that it is
work->worker = NULL; * 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();
} }
src/platforms/posix/work_queue/queue.c
+39 -43
View File
@@ -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); sq_entry_t *sq_remafter(sq_entry_t *node, sq_queue_t *queue);
void sq_rem(sq_entry_t *node, sq_queue_t *queue) void sq_rem(sq_entry_t *node, sq_queue_t *queue)
{ {
if (queue->head && node) if (queue->head && node) {
{ if (node == queue->head) {
if (node == queue->head) queue->head = node->flink;
{
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) if (node == queue->tail) {
{ queue->tail = NULL;
sq_remafter(prev, queue); }
}
} } 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 *sq_remafter(sq_entry_t *node, sq_queue_t *queue)
{ {
sq_entry_t *ret = node->flink; 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; if (queue->head && ret) {
} if (queue->tail == ret) {
queue->tail = node;
node->flink = NULL;
return ret; } else {
node->flink = ret->flink;
}
ret->flink = NULL;
}
return ret;
} }
__EXPORT void sq_addfirst(sq_entry_t *node, sq_queue_t *queue); __EXPORT void sq_addfirst(sq_entry_t *node, sq_queue_t *queue);
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; node->flink = queue->head;
if (!queue->head)
{ if (!queue->head) {
queue->tail = node; queue->tail = node;
} }
queue->head = node;
queue->head = node;
} }
src/platforms/posix/work_queue/sq_addafter.c
+9 -11
View File
@@ -56,16 +56,14 @@
* *
************************************************************/ ************************************************************/
void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, void sq_addafter(sq_entry_t *prev, sq_entry_t *node,
sq_queue_t *queue) sq_queue_t *queue)
{ {
if (!queue->head || prev == queue->tail) if (!queue->head || prev == queue->tail) {
{ sq_addlast(node, queue);
sq_addlast(node, queue);
} } else {
else node->flink = prev->flink;
{ prev->flink = node;
node->flink = prev->flink; }
prev->flink = node;
}
} }
src/platforms/posix/work_queue/sq_addlast.c
+11 -12
View File
@@ -56,18 +56,17 @@
* the 'queue' * 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; node->flink = NULL;
if (!queue->head)
{ if (!queue->head) {
queue->head = node; queue->head = node;
queue->tail = node; queue->tail = node;
}
else } else {
{ queue->tail->flink = node;
queue->tail->flink = node; queue->tail = node;
queue->tail = node; }
}
} }
src/platforms/posix/work_queue/sq_remfirst.c
+11 -12
View File
@@ -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) if (ret) {
{ queue->head = ret->flink;
queue->head = ret->flink;
if (!queue->head)
{
queue->tail = NULL;
}
ret->flink = NULL; if (!queue->head) {
} queue->tail = NULL;
}
return ret; ret->flink = NULL;
}
return ret;
} }
src/platforms/posix/work_queue/work_cancel.c
+20 -20
View File
@@ -88,33 +88,33 @@
int work_cancel(int qid, struct work_s *work) 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 /* Cancelling the work is simply a matter of removing the work structure
* from the work queue. This must be done with interrupts disabled because * from the work queue. This must be done with interrupts disabled because
* new work is typically added to the work queue from interrupt handlers. * new work is typically added to the work queue from interrupt handlers.
*/ */
work_lock(qid); work_lock(qid);
if (work->worker != NULL)
{
/* A little test of the integrity of the work queue */
//DEBUGASSERT(work->dq.flink ||(FAR dq_entry_t *)work == wqueue->q.tail); if (work->worker != NULL) {
//DEBUGASSERT(work->dq.blink ||(FAR dq_entry_t *)work == wqueue->q.head); /* A little test of the integrity of the work queue */
/* Remove the entry from the work queue and make sure that it is //DEBUGASSERT(work->dq.flink ||(dq_entry_t *)work == wqueue->q.tail);
* mark as availalbe (i.e., the worker field is nullified). //DEBUGASSERT(work->dq.blink ||(dq_entry_t *)work == wqueue->q.head);
*/
dq_rem((FAR dq_entry_t *)work, &wqueue->q); /* Remove the entry from the work queue and make sure that it is
work->worker = NULL; * mark as availalbe (i.e., the worker field is nullified).
} */
work_unlock(qid); dq_rem((dq_entry_t *)work, &wqueue->q);
return PX4_OK; work->worker = NULL;
}
work_unlock(qid);
return PX4_OK;
} }
#endif /* CONFIG_SCHED_WORKQUEUE */ #endif /* CONFIG_SCHED_WORKQUEUE */
src/platforms/posix/work_queue/work_lock.c
+4 -4
View File
@@ -40,12 +40,12 @@ extern sem_t _work_lock[];
void work_lock(int id) void work_lock(int id)
{ {
//PX4_INFO("work_lock %d", id); //PX4_INFO("work_lock %d", id);
sem_wait(&_work_lock[id]); sem_wait(&_work_lock[id]);
} }
void work_unlock(int id) void work_unlock(int id)
{ {
//PX4_INFO("work_unlock %d", id); //PX4_INFO("work_unlock %d", id);
sem_post(&_work_lock[id]); sem_post(&_work_lock[id]);
} }
src/platforms/posix/work_queue/work_queue.c
+17 -17
View File
@@ -105,33 +105,33 @@
int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay) 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->worker = worker; /* Work callback */
work->arg = arg; /* Callback argument */ work->arg = arg; /* Callback argument */
work->delay = delay; /* Delay until work performed */ work->delay = delay; /* Delay until work performed */
/* Now, time-tag that entry and put it in the work queue. This must be /* 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 * done with interrupts disabled. This permits this function to be called
* from with task logic or interrupt handlers. * from with task logic or interrupt handlers.
*/ */
work_lock(qid); work_lock(qid);
work->qtime = clock_systimer(); /* Time work queued */ 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 #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 #else
px4_task_kill(wqueue->pid, SIGCONT); /* Wake up the worker thread */ px4_task_kill(wqueue->pid, SIGCONT); /* Wake up the worker thread */
#endif #endif
work_unlock(qid); work_unlock(qid);
return PX4_OK; return PX4_OK;
} }
#endif /* CONFIG_SCHED_WORKQUEUE */ #endif /* CONFIG_SCHED_WORKQUEUE */
src/platforms/posix/work_queue/work_thread.c
+119 -121
View File
@@ -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; volatile struct work_s *work;
worker_t worker; worker_t worker;
FAR void *arg; void *arg;
uint64_t elapsed; uint64_t elapsed;
uint32_t remaining; uint32_t remaining;
uint32_t next; uint32_t next;
/* Then process queued work. We need to keep interrupts disabled while /* Then process queued work. We need to keep interrupts disabled while
* we process items in the work list. * we process items in the work list.
*/ */
next = CONFIG_SCHED_WORKPERIOD; next = CONFIG_SCHED_WORKPERIOD;
work_lock(lock_id); work_lock(lock_id);
work = (FAR struct work_s *)wqueue->q.head; 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); while (work) {
//printf("work_process: in ticks elapsed=%lu delay=%u\n", elapsed, work->delay); /* Is this work ready? It is ready if there is no delay or if
if (elapsed >= work->delay) * 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
/* Remove the ready-to-execute work from the list */ * zero. Therefore a delay of zero will always execute immediately.
*/
(void)dq_rem((struct dq_entry_s *)work, &wqueue->q); elapsed = USEC2TICK(clock_systimer() - work->qtime);
/* Extract the work description from the entry (in case the work //printf("work_process: in ticks elapsed=%lu delay=%u\n", elapsed, work->delay);
* instance by the re-used after it has been de-queued). if (elapsed >= work->delay) {
*/ /* Remove the ready-to-execute work from the list */
worker = work->worker; (void)dq_rem((struct dq_entry_s *)work, &wqueue->q);
arg = work->arg;
/* Mark the work as no longer being queued */ /* Extract the work description from the entry (in case the work
* instance by the re-used after it has been de-queued).
*/
work->worker = NULL; worker = work->worker;
arg = work->arg;
/* Do the work. Re-enable interrupts while the work is being /* Mark the work as no longer being queued */
* performed... we don't have any idea how long that will take!
*/
work_unlock(lock_id); work->worker = NULL;
if (!worker) {
PX4_WARN("MESSED UP: worker = 0\n");
}
else
worker(arg);
/* Now, unfortunately, since we re-enabled interrupts we don't /* Do the work. Re-enable interrupts while the work is being
* know the state of the work list and we will have to start * performed... we don't have any idea how long that will take!
* back at the head of the list. */
*/
work_lock(lock_id); work_unlock(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 */ if (!worker) {
remaining = USEC_PER_TICK*(work->delay - elapsed); PX4_WARN("MESSED UP: worker = 0\n");
if (remaining < next)
{
/* Yes.. Then schedule to wake up when the work is ready */
next = remaining; } else {
} worker(arg);
}
/* Then try the next in the list. */ /* 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 = (FAR struct work_s *)work->dq.flink; work_lock(lock_id);
} work = (struct work_s *)wqueue->q.head;
}
/* Wait awhile to check the work list. We will wait here until either } else {
* the time elapses or until we are awakened by a signal. /* This one is not ready.. will it be ready before the next
*/ * scheduled wakeup interval?
work_unlock(lock_id); */
usleep(next); /* 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 // Create high priority worker thread
g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high", g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX-1, SCHED_PRIORITY_MAX - 1,
2000, 2000,
work_hpthread, work_hpthread,
(char* const*)NULL); (char *const *)NULL);
// Create low priority worker thread // Create low priority worker thread
g_work[LPWORK].pid = px4_task_spawn_cmd("wkr_low", g_work[LPWORK].pid = px4_task_spawn_cmd("wkr_low",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MIN, SCHED_PRIORITY_MIN,
2000, 2000,
work_lpthread, work_lpthread,
(char* const*)NULL); (char *const *)NULL);
} }
@@ -245,56 +246,54 @@ void work_queues_init(void)
int work_hpthread(int argc, char *argv[]) int work_hpthread(int argc, char *argv[])
{ {
/* Loop forever */ /* Loop forever */
for (;;) for (;;) {
{ /* First, perform garbage collection. This cleans-up memory de-allocations
/* First, perform garbage collection. This cleans-up memory de-allocations * that were queued because they could not be freed in that execution
* that were queued because they could not be freed in that execution * context (for example, if the memory was freed from an interrupt handler).
* 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
* NOTE: If the work thread is disabled, this clean-up is performed by * the IDLE thread (at a very, very low priority).
* the IDLE thread (at a very, very low priority). */
*/
#ifndef CONFIG_SCHED_LPWORK #ifndef CONFIG_SCHED_LPWORK
sched_garbagecollection(); sched_garbagecollection();
#endif #endif
/* Then process queued work. We need to keep interrupts disabled while /* Then process queued work. We need to keep interrupts disabled while
* we process items in the work list. * 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 #ifdef CONFIG_SCHED_LPWORK
int work_lpthread(int argc, char *argv[]) int work_lpthread(int argc, char *argv[])
{ {
/* Loop forever */ /* Loop forever */
for (;;) for (;;) {
{ /* First, perform garbage collection. This cleans-up memory de-allocations
/* First, perform garbage collection. This cleans-up memory de-allocations * that were queued because they could not be freed in that execution
* that were queued because they could not be freed in that execution * context (for example, if the memory was freed from an interrupt handler).
* 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
* NOTE: If the work thread is disabled, this clean-up is performed by * the IDLE thread (at a very, very low priority).
* the IDLE thread (at a very, very low priority). */
*/
//sched_garbagecollection(); //sched_garbagecollection();
/* Then process queued work. We need to keep interrupts disabled while /* Then process queued work. We need to keep interrupts disabled while
* we process items in the work list. * 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 */ #endif /* CONFIG_SCHED_LPWORK */
@@ -304,18 +303,17 @@ int work_lpthread(int argc, char *argv[])
int work_usrthread(int argc, char *argv[]) int work_usrthread(int argc, char *argv[])
{ {
/* Loop forever */ /* Loop forever */
for (;;) for (;;) {
{ /* Then process queued work. We need to keep interrupts disabled while
/* Then process queued work. We need to keep interrupts disabled while * we process items in the work list.
* 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 */ #endif /* CONFIG_SCHED_USRWORK */