fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues 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
commit ff1c9da390
14 changed files with 410 additions and 429 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/platforms/posix/work_queue/dq_addlast.c
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->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;
}
}

38
src/platforms/posix/work_queue/dq_rem.c
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;
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;
}

36
src/platforms/posix/work_queue/dq_remfirst.c
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)
{
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;
}

34
src/platforms/posix/work_queue/hrt_queue.c
View File

@ -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;
}

195
src/platforms/posix/work_queue/hrt_thread.c
View File

@ -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;
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.
*/
/* 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;
/* Default to sleeping for 1 sec */
next = 1000000;
hrt_work_lock();
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.
*/
work = (struct work_s *)wqueue->q.head;
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 */
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.
*/
(void)dq_rem((struct dq_entry_s *)work, &wqueue->q);
//PX4_INFO("Dequeued work=%p", work);
elapsed = hrt_absolute_time() - work->qtime;
/* Extract the work description from the entry (in case the work
* instance by the re-used after it has been de-queued).
*/
//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 */
worker = work->worker;
arg = work->arg;
(void)dq_rem((struct dq_entry_s *)work, &wqueue->q);
//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
* performed... we don't have any idea how long that will take!
*/
/* Mark the work as no longer being queued */
hrt_work_unlock();
if (!worker) {
PX4_ERR("MESSED UP: worker = 0");
}
else {
worker(arg);
}
work->worker = NULL;
/* 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.
*/
/* 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_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?
*/
hrt_work_unlock();
/* 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 */
if (!worker) {
PX4_ERR("MESSED UP: worker = 0");
next = remaining;
}
/* Then try the next in the list. */
} else {
worker(arg);
}
work = (FAR struct work_s *)work->dq.flink;
//PX4_INFO("next %u work %p", next, work);
}
}
/* 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.
*/
/* 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();
hrt_work_lock();
work = (struct work_s *)wqueue->q.head;
/* might sleep less if a signal received and new item was queued */
//PX4_INFO("Sleeping for %u usec", next);
usleep(next);
} 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);
}

40
src/platforms/posix/work_queue/hrt_work_cancel.c
View File

@ -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();
}

82
src/platforms/posix/work_queue/queue.c
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);
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 (queue->head && node) {
if (node == queue->head) {
queue->head = node->flink;
if (prev)
{
sq_remafter(prev, queue);
}
}
}
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;
}
sq_entry_t *ret = node->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);
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;
}

20
src/platforms/posix/work_queue/sq_addafter.c
View File

@ -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;
}
}

23
src/platforms/posix/work_queue/sq_addlast.c
View File

@ -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;
}
}

23
src/platforms/posix/work_queue/sq_remfirst.c
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)
{
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;
}

40
src/platforms/posix/work_queue/work_cancel.c
View File

@ -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 */

8
src/platforms/posix/work_queue/work_lock.c
View File

@ -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]);
}

34
src/platforms/posix/work_queue/work_queue.c
View File

@ -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 */

242
src/platforms/posix/work_queue/work_thread.c
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;
worker_t worker;
FAR void *arg;
uint64_t elapsed;
uint32_t remaining;
uint32_t 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.
*/
/* Then process queued work. We need to keep interrupts disabled while
* 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;
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.
*/
work = (struct work_s *)wqueue->q.head;
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 */
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.
*/
(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
* instance by the re-used after it has been de-queued).
*/
//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 */
worker = work->worker;
arg = work->arg;
(void)dq_rem((struct dq_entry_s *)work, &wqueue->q);
/* 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
* performed... we don't have any idea how long that will take!
*/
/* Mark the work as no longer being queued */
work_unlock(lock_id);
if (!worker) {
PX4_WARN("MESSED UP: worker = 0\n");
}
else
worker(arg);
work->worker = NULL;
/* 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.
*/
/* Do the work. Re-enable interrupts while the work is being
* performed... we don't have any idea how long that will take!
*/
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?
*/
work_unlock(lock_id);
/* 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 */
if (!worker) {
PX4_WARN("MESSED UP: worker = 0\n");
next = remaining;
}
/* Then try the next in the list. */
} else {
worker(arg);
}
work = (FAR struct work_s *)work->dq.flink;
}
}
/* 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.
*/
/* 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);
work_lock(lock_id);
work = (struct work_s *)wqueue->q.head;
usleep(next);
} 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 */