From aedf6fe6284ca19597ca9b9c0b5cfeaf8ee85995 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 25 Mar 2015 10:53:08 -0700 Subject: [PATCH] Linux: Added preliminary work queue support Based on NuttX work queue code. Not yet functional. Signed-off-by: Mark Charlebois --- src/platforms/linux/px4_layer/module.mk | 6 +- .../linux/px4_layer/px4_linux_impl.cpp | 66 ++++++------------- src/platforms/linux/px4_layer/work_cancel.c | 29 ++++---- src/platforms/linux/px4_layer/work_queue.c | 30 ++++----- src/platforms/linux/px4_layer/work_thread.c | 59 +++++------------ src/platforms/px4_config.h | 3 + src/platforms/px4_defines.h | 3 +- src/platforms/px4_tasks.h | 1 + src/platforms/px4_workqueue.h | 27 ++++++-- 9 files changed, 95 insertions(+), 129 deletions(-) diff --git a/src/platforms/linux/px4_layer/module.mk b/src/platforms/linux/px4_layer/module.mk index 1d987418ac..aeb707b5c3 100644 --- a/src/platforms/linux/px4_layer/module.mk +++ b/src/platforms/linux/px4_layer/module.mk @@ -38,6 +38,9 @@ SRCS = \ px4_linux_impl.cpp \ px4_linux_tasks.c \ + work_thread.c \ + work_queue.c \ + work_cancel.c \ lib_crc32.c \ drv_hrt.c \ queue.c \ @@ -45,6 +48,7 @@ SRCS = \ dq_remfirst.c \ sq_addlast.c \ sq_remfirst.c \ - sq_addafter.c + sq_addafter.c \ + dq_rem.c MAXOPTIMIZATION = -Os diff --git a/src/platforms/linux/px4_layer/px4_linux_impl.cpp b/src/platforms/linux/px4_layer/px4_linux_impl.cpp index 059e1c9176..7f06b7a916 100644 --- a/src/platforms/linux/px4_layer/px4_linux_impl.cpp +++ b/src/platforms/linux/px4_layer/px4_linux_impl.cpp @@ -56,60 +56,32 @@ long PX4_TICKS_PER_SEC = sysconf(_SC_CLK_TCK); __END_DECLS +extern struct wqueue_s gwork[NWORKERS]; + namespace px4 { void init(int argc, char *argv[], const char *app_name) { - struct param_info_s test_1 = { - "TEST_1", - PARAM_TYPE_INT32 - }; - test_1.val.i = 2; - - struct param_info_s test_2 = { - "TEST_2", - PARAM_TYPE_INT32 - }; - test_2.val.i = 4; - - struct param_info_s rc_x = { - "RC_X", - PARAM_TYPE_INT32 - }; - rc_x.val.i = 8; - - struct param_info_s rc2_x = { - "RC2_X", - PARAM_TYPE_INT32 - }; - rc2_x.val.i = 16; - - param_array[0] = test_1; - param_array[1] = test_2; - param_array[2] = rc_x; - param_array[3] = rc2_x; - param_info_base = (struct param_info_s *) ¶m_array[0]; - param_info_limit = (struct param_info_s *) ¶m_array[4]; // needs to point at the end of the data, - // therefore number of params + 1 - printf("App name: %s\n", app_name); + + // Create high priority worker thread + g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX, + 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); + } } - - - -int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay) -{ - printf("work_queue: UNIMPLEMENTED\n"); - return PX4_OK; -} - -int work_cancel(int qid, struct work_s *work) -{ - printf("work_cancel: UNIMPLEMENTED\n"); - return PX4_OK; -} - diff --git a/src/platforms/linux/px4_layer/work_cancel.c b/src/platforms/linux/px4_layer/work_cancel.c index 2671bf2530..6f737877d9 100644 --- a/src/platforms/linux/px4_layer/work_cancel.c +++ b/src/platforms/linux/px4_layer/work_cancel.c @@ -37,15 +37,10 @@ * Included Files ****************************************************************************/ -#include - +#include +#include #include -#include -#include -#include - -#include -#include +#include #ifdef CONFIG_SCHED_WORKQUEUE @@ -90,25 +85,25 @@ * ****************************************************************************/ -int work_cancel(int qid, FAR struct work_s *work) +int work_cancel(int qid, struct work_s *work) { - FAR struct wqueue_s *wqueue = &g_work[qid]; - irqstate_t flags; + struct wqueue_s *wqueue = &g_work[qid]; + //irqstate_t flags; - 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. */ - flags = irqsave(); + //flags = irqsave(); 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); - DEBUGASSERT(work->dq.blink ||(FAR dq_entry_t *)work == wqueue->q.head); + //DEBUGASSERT(work->dq.flink ||(FAR dq_entry_t *)work == wqueue->q.tail); + //DEBUGASSERT(work->dq.blink ||(FAR dq_entry_t *)work == wqueue->q.head); /* Remove the entry from the work queue and make sure that it is * mark as availalbe (i.e., the worker field is nullified). @@ -118,8 +113,8 @@ int work_cancel(int qid, FAR struct work_s *work) work->worker = NULL; } - irqrestore(flags); - return OK; + //irqrestore(flags); + return PX4_OK; } #endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/linux/px4_layer/work_queue.c b/src/platforms/linux/px4_layer/work_queue.c index c85f564e54..b67cfdc75b 100644 --- a/src/platforms/linux/px4_layer/work_queue.c +++ b/src/platforms/linux/px4_layer/work_queue.c @@ -37,17 +37,13 @@ * Included Files ****************************************************************************/ -#include +#include +#include +#include #include #include -#include -#include -#include - -#include -#include -#include +#include #ifdef CONFIG_SCHED_WORKQUEUE @@ -104,13 +100,11 @@ * ****************************************************************************/ -int work_queue(int qid, FAR struct work_s *work, worker_t worker, - FAR void *arg, uint32_t delay) +int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay) { - FAR struct wqueue_s *wqueue = &g_work[qid]; - irqstate_t flags; + struct wqueue_s *wqueue = &g_work[qid]; - DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); + //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); /* First, initialize the work structure */ @@ -123,14 +117,14 @@ int work_queue(int qid, FAR struct work_s *work, worker_t worker, * from with task logic or interrupt handlers. */ - flags = irqsave(); + //flags = irqsave(); work->qtime = clock_systimer(); /* Time work queued */ - dq_addlast((FAR dq_entry_t *)work, &wqueue->q); - kill(wqueue->pid, SIGWORK); /* Wake up the worker thread */ + dq_addlast((dq_entry_t *)work, &wqueue->q); + pthread_kill(wqueue->pid, SIGUSR1); /* Wake up the worker thread */ - irqrestore(flags); - return OK; + //irqrestore(flags); + return PX4_OK; } #endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/linux/px4_layer/work_thread.c b/src/platforms/linux/px4_layer/work_thread.c index 6ef8a874b0..b238979ed4 100644 --- a/src/platforms/linux/px4_layer/work_thread.c +++ b/src/platforms/linux/px4_layer/work_thread.c @@ -37,19 +37,12 @@ * Included Files ****************************************************************************/ -#include - +#include +#include #include #include #include -#include -#include -#include - -#include -#include -#include -#include +#include #ifdef CONFIG_SCHED_WORKQUEUE @@ -66,29 +59,8 @@ ****************************************************************************/ /* The state of each work queue. */ - -#ifdef CONFIG_NUTTX_KERNEL - - /* Play some games in the kernel mode build to assure that different - * naming is used for the global work queue data structures. This may - * not be necessary but it safer. - * - * In this case g_work is #define'd to be either g_kernelwork or - * g_usrwork in include/nuttx/wqueue.h - */ - -# ifdef __KERNEL__ -struct wqueue_s g_kernelwork[NWORKERS]; -# else -struct wqueue_s g_usrwork[NWORKERS]; -# endif - -#else /* CONFIG_NUTTX_KERNEL */ - struct wqueue_s g_work[NWORKERS]; -#endif /* CONFIG_NUTTX_KERNEL */ - /**************************************************************************** * Private Variables ****************************************************************************/ @@ -115,7 +87,7 @@ static void work_process(FAR struct wqueue_s *wqueue) { volatile FAR struct work_s *work; worker_t worker; - irqstate_t flags; + //irqstate_t flags; FAR void *arg; uint32_t elapsed; uint32_t remaining; @@ -125,8 +97,9 @@ static void work_process(FAR struct wqueue_s *wqueue) * we process items in the work list. */ - next = CONFIG_SCHED_WORKPERIOD / USEC_PER_TICK; - flags = irqsave(); + //next = CONFIG_SCHED_WORKPERIOD / USEC_PER_TICK; + next = 100; + //flags = irqsave(); work = (FAR struct work_s *)wqueue->q.head; while (work) { @@ -158,7 +131,7 @@ static void work_process(FAR struct wqueue_s *wqueue) * performed... we don't have any idea how long that will take! */ - irqrestore(flags); + //irqrestore(flags); worker(arg); /* Now, unfortunately, since we re-enabled interrupts we don't @@ -166,7 +139,7 @@ static void work_process(FAR struct wqueue_s *wqueue) * back at the head of the list. */ - flags = irqsave(); + //flags = irqsave(); work = (FAR struct work_s *)wqueue->q.head; } else @@ -195,7 +168,7 @@ static void work_process(FAR struct wqueue_s *wqueue) */ usleep(next * USEC_PER_TICK); - irqrestore(flags); + //irqrestore(flags); } /**************************************************************************** @@ -258,7 +231,7 @@ int work_hpthread(int argc, char *argv[]) work_process(&g_work[HPWORK]); } - return OK; /* To keep some compilers happy */ + return PX4_OK; /* To keep some compilers happy */ } #ifdef CONFIG_SCHED_LPWORK @@ -276,7 +249,7 @@ int work_lpthread(int argc, char *argv[]) * 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. @@ -285,7 +258,7 @@ int work_lpthread(int argc, char *argv[]) work_process(&g_work[LPWORK]); } - return OK; /* To keep some compilers happy */ + return PX4_OK; /* To keep some compilers happy */ } #endif /* CONFIG_SCHED_LPWORK */ @@ -306,9 +279,13 @@ int work_usrthread(int argc, char *argv[]) work_process(&g_work[USRWORK]); } - return OK; /* To keep some compilers happy */ + return PX4_OK; /* To keep some compilers happy */ } #endif /* CONFIG_SCHED_USRWORK */ +int clock_systimer() +{ + return 1; +} #endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h index d2d1708eb6..5b1db531d6 100644 --- a/src/platforms/px4_config.h +++ b/src/platforms/px4_config.h @@ -44,6 +44,9 @@ #include #elif defined (__PX4_LINUX) #define CONFIG_NFILE_STREAMS 1 +#define CONFIG_SCHED_WORKQUEUE 1 +#define CONFIG_SCHED_HPWORK 1 +#define CONFIG_SCHED_LPWORK 1 #define CONFIG_ARCH_BOARD_LINUXTEST 1 #define px4_errx(x, ...) errx(x, __VA_ARGS__) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 26c9a85951..2777367568 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -123,7 +123,8 @@ __BEGIN_DECLS extern long PX4_TICKS_PER_SEC; __END_DECLS -#define USEC2TICK(x) (PX4_TICKS_PER_SEC*(long)x/1000000L) +#define USEC2TICK(x) (PX4_TICKS_PER_SEC*(long)(x)/1000000L) +#define USEC_PER_TICK (1000000L/PX4_TICKS_PER_SEC) #define px4_statfs_buf_f_bavail_t unsigned long diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index 3a9adfd37e..02080e7133 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -55,6 +55,7 @@ typedef int px4_task_t; #define SCHED_DEFAULT SCHED_FIFO #define SCHED_PRIORITY_MAX sched_get_priority_max(SCHED_FIFO) +#define SCHED_PRIORITY_MIN sched_get_priority_min(SCHED_FIFO) #define SCHED_PRIORITY_DEFAULT sched_get_priority_max(SCHED_FIFO) typedef pthread_t px4_task_t; diff --git a/src/platforms/px4_workqueue.h b/src/platforms/px4_workqueue.h index b6552c204f..d5575b282e 100644 --- a/src/platforms/px4_workqueue.h +++ b/src/platforms/px4_workqueue.h @@ -43,10 +43,22 @@ #include #elif defined(__PX4_LINUX) +#include #include +__BEGIN_DECLS + +#define HPWORK 0 #define LPWORK 1 -#define HPWORK 2 +#define NWORKERS 2 + +struct wqueue_s +{ + pid_t pid; /* The task ID of the worker thread */ + struct dq_queue_s q; /* The queue of pending work */ +}; + +extern struct wqueue_s g_work[NWORKERS]; /* Defines the work callback */ @@ -90,8 +102,7 @@ struct work_s * ****************************************************************************/ -int work_queue(int qid, FAR struct work_s *work, worker_t worker, - FAR void *arg, uint32_t delay); +int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay); /**************************************************************************** * Name: work_cancel @@ -110,7 +121,15 @@ int work_queue(int qid, FAR struct work_s *work, worker_t worker, * ****************************************************************************/ -int work_cancel(int qid, FAR struct work_s *work); +int work_cancel(int qid, struct work_s *work); + +int clock_systimer(void); + +int work_hpthread(int argc, char *argv[]); +int work_lpthread(int argc, char *argv[]); + +__END_DECLS + #else #error "Unknown target OS" #endif