From 84ca66dcf7a40b98d2eab64d5deb778f3f73a8a0 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 7 May 2015 22:58:51 -0700 Subject: [PATCH] QuRT: updated task support based on posix fixes The posix layer implementations should work on QuRT. QuRT needs to provide a way for getting the current time. Signed-off-by: Mark Charlebois --- makefiles/qurt/config_qurt_default.mk | 4 +- src/platforms/px4_time.h | 4 + src/platforms/qurt/main.cpp | 12 +- src/platforms/qurt/px4_layer/drv_hrt.c | 123 +++++++++++++++++- .../qurt/px4_layer/px4_qurt_impl.cpp | 10 ++ .../qurt/px4_layer/px4_qurt_tasks.cpp | 30 ++--- src/platforms/qurt/px4_layer/work_lock.h | 53 ++++++++ src/platforms/qurt/px4_layer/work_queue.c | 7 +- src/platforms/qurt/px4_layer/work_thread.c | 60 ++++++--- 9 files changed, 261 insertions(+), 42 deletions(-) create mode 100644 src/platforms/qurt/px4_layer/work_lock.h diff --git a/makefiles/qurt/config_qurt_default.mk b/makefiles/qurt/config_qurt_default.mk index 48fcefffe5..2e2323f4f0 100644 --- a/makefiles/qurt/config_qurt_default.mk +++ b/makefiles/qurt/config_qurt_default.mk @@ -72,7 +72,7 @@ MODULES += platforms/posix/drivers/barosim # Unit tests # MODULES += platforms/qurt/tests/hello -#MODULES += platforms/posix/tests/vcdev_test +MODULES += platforms/posix/tests/vcdev_test #MODULES += platforms/posix/tests/hrt_test -#MODULES += platforms/posix/tests/wqueue +MODULES += platforms/posix/tests/wqueue diff --git a/src/platforms/px4_time.h b/src/platforms/px4_time.h index d72cdbb8b1..0c9b7f24c9 100644 --- a/src/platforms/px4_time.h +++ b/src/platforms/px4_time.h @@ -10,10 +10,13 @@ #elif defined(__PX4_QURT) +#include + #define CLOCK_REALTIME 1 __BEGIN_DECLS +#if 0 #if !defined(__cplusplus) struct timespec { @@ -21,6 +24,7 @@ struct timespec long tv_nsec; }; #endif +#endif int px4_clock_gettime(clockid_t clk_id, struct timespec *tp); int px4_clock_settime(clockid_t clk_id, struct timespec *tp); diff --git a/src/platforms/qurt/main.cpp b/src/platforms/qurt/main.cpp index 217fa68d0c..58d37da24b 100644 --- a/src/platforms/qurt/main.cpp +++ b/src/platforms/qurt/main.cpp @@ -52,6 +52,9 @@ #include "px4_middleware.h" static const char *commands = +"x\n" +"hello start" +#if 0 "uorb start\n" "simulator start -s\n" "barosim start\n" @@ -68,7 +71,9 @@ static const char *commands = "hil mode_pwm\n" "commander start\n" "list_devices\n" -"list_topics\n"; +"list_topics\n" +#endif +; static void run_cmd(const vector &appargs) { @@ -148,8 +153,13 @@ static void process_commands(const char *cmds) } } +namespace px4 { +extern void init_once(void); +}; + int main(int argc, char **argv) { + px4::init_once(); px4::init(argc, argv, "mainapp"); process_commands(commands); for (;;) {} diff --git a/src/platforms/qurt/px4_layer/drv_hrt.c b/src/platforms/qurt/px4_layer/drv_hrt.c index 39e0f73b37..3edf34f64c 100644 --- a/src/platforms/qurt/px4_layer/drv_hrt.c +++ b/src/platforms/qurt/px4_layer/drv_hrt.c @@ -37,9 +37,12 @@ * High-resolution timer with callouts and timekeeping. */ +#include #include +#include #include #include +#include static struct sq_queue_s callout_queue; @@ -51,9 +54,28 @@ __EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; static void hrt_call_reschedule(void); +// Intervals in ms #define HRT_INTERVAL_MIN 50 #define HRT_INTERVAL_MAX 50000 +static sem_t _hrt_lock; +static struct work_s _hrt_work; + +static void +hrt_call_invoke(void); + +static void hrt_lock(void) +{ + //printf("hrt_lock\n"); + sem_wait(&_hrt_lock); +} + +static void hrt_unlock(void) +{ + //printf("hrt_unlock\n"); + sem_post(&_hrt_lock); +} + /* * Get absolute time. */ @@ -61,7 +83,7 @@ hrt_abstime hrt_absolute_time(void) { struct timespec ts; - // FIXME - not supported in QURT + // FIXME - clock_gettime unsupported in QuRT //clock_gettime(CLOCK_MONOTONIC, &ts); return ts_to_abstime(&ts); } @@ -120,7 +142,7 @@ bool hrt_called(struct hrt_call *entry) */ void hrt_cancel(struct hrt_call *entry) { - // FIXME - need a lock + hrt_lock(); sq_rem(&entry->link, &callout_queue); entry->deadline = 0; @@ -128,6 +150,7 @@ void hrt_cancel(struct hrt_call *entry) * being re-entered when the callout returns. */ entry->period = 0; + hrt_unlock(); // endif } @@ -156,7 +179,10 @@ void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) */ void hrt_init(void) { + //printf("hrt_init\n"); sq_init(&callout_queue); + sem_init(&_hrt_lock, 0, 1); + memset(&_hrt_work, 0, sizeof(_hrt_work)); } static void @@ -164,6 +190,7 @@ hrt_call_enter(struct hrt_call *entry) { struct hrt_call *call, *next; + //printf("hrt_call_enter\n"); call = (struct hrt_call *)sq_peek(&callout_queue); if ((call == NULL) || (entry->deadline < call->deadline)) { @@ -187,6 +214,27 @@ hrt_call_enter(struct hrt_call *entry) //lldbg("scheduled\n"); } +/** + * Timer interrupt handler + * + * This routine simulates a timer interrupt handler + */ +static void +hrt_tim_isr(void *p) +{ + + //printf("hrt_tim_isr\n"); + /* run any callouts that have met their deadline */ + hrt_call_invoke(); + + hrt_lock(); + + /* and schedule the next interrupt */ + hrt_call_reschedule(); + + hrt_unlock(); +} + /** * Reschedule the next timer interrupt. * @@ -198,7 +246,10 @@ hrt_call_reschedule() hrt_abstime now = hrt_absolute_time(); struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); hrt_abstime deadline = now + HRT_INTERVAL_MAX; + uint32_t ticks = USEC2TICK(HRT_INTERVAL_MAX*1000); + //printf("hrt_call_reschedule\n"); + /* * Determine what the next deadline will be. * @@ -216,18 +267,26 @@ hrt_call_reschedule() if (next->deadline <= (now + HRT_INTERVAL_MIN)) { //lldbg("pre-expired\n"); /* set a minimal deadline so that we call ASAP */ - deadline = now + HRT_INTERVAL_MIN; + ticks = USEC2TICK(HRT_INTERVAL_MIN*1000); } else if (next->deadline < deadline) { //lldbg("due soon\n"); - deadline = next->deadline; + ticks = USEC2TICK((next->deadline - now)*1000); } } + + // There is no timer ISR, so simulate one by putting an event on the + // high priority work queue + //printf("ticks = %u\n", ticks); + work_queue(HPWORK, &_hrt_work, (worker_t)&hrt_tim_isr, NULL, ticks); } static void hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg) { + //printf("hrt_call_internal\n"); + hrt_lock(); + //printf("hrt_call_internal after lock\n"); /* if the entry is currently queued, remove it */ /* note that we are using a potentially uninitialised entry->link here, but it is safe as sq_rem() doesn't @@ -245,6 +304,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte entry->arg = arg; hrt_call_enter(entry); + hrt_unlock(); } /* @@ -255,6 +315,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte */ void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) { + //printf("hrt_call_after\n"); hrt_call_internal(entry, hrt_absolute_time() + delay, 0, @@ -292,3 +353,57 @@ void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callo void abstime_to_ts(struct timespec *ts, hrt_abstime abstime); #endif +static void +hrt_call_invoke(void) +{ + struct hrt_call *call; + hrt_abstime deadline; + + hrt_lock(); + while (true) { + /* get the current time */ + hrt_abstime now = hrt_absolute_time(); + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if (call == NULL) + break; + + if (call->deadline > now) + break; + + sq_rem(&call->link, &callout_queue); + //lldbg("call pop\n"); + + /* save the intended deadline for periodic calls */ + deadline = call->deadline; + + /* zero the deadline, as the call has occurred */ + call->deadline = 0; + + /* invoke the callout (if there is one) */ + if (call->callout) { + // Unlock so we don't deadlock in callback + hrt_unlock(); + + //lldbg("call %p: %p(%p)\n", call, call->callout, call->arg); + call->callout(call->arg); + + hrt_lock(); + } + + /* if the callout has a non-zero period, it has to be re-entered */ + if (call->period != 0) { + // re-check call->deadline to allow for + // callouts to re-schedule themselves + // using hrt_call_delay() + if (call->deadline <= now) { + call->deadline = deadline + call->period; + } + + hrt_call_enter(call); + } + } + hrt_unlock(); +} + diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp index 5b7df7d6df..f878a8d140 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -56,6 +56,8 @@ long PX4_TICKS_PER_SEC = 1000; void usleep(useconds_t usec) {} unsigned int sleep(unsigned int sec) { return 0; } +extern void hrt_init(void); + __END_DECLS extern struct wqueue_s gwork[NWORKERS]; @@ -63,6 +65,14 @@ extern struct wqueue_s gwork[NWORKERS]; namespace px4 { +void init_once(void); + +void init_once(void) +{ + work_queues_init(); + hrt_init(); +} + void init(int argc, char *argv[], const char *app_name) { printf("App name: %s\n", app_name); diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index b4554bf4fe..6431536e75 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -79,17 +79,15 @@ typedef struct static void entry_adapter ( void *ptr ) { + printf("entry_adapter\n"); pthdata_t *data = (pthdata_t *) ptr; - printf("TEST3\n"); -#if 0 - //data->entry(data->argc, data->argv); - printf("TEST4\n"); + data->entry(data->argc, data->argv); + free(ptr); + printf("after entry\n"); printf("Before px4_task_exit\n"); px4_task_exit(0); - //free(ptr); printf("After px4_task_exit\n"); -#endif } void @@ -106,21 +104,17 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned int len = 0; unsigned long offset; unsigned long structsize; - char * p; + char * p = (char *)argv; - //printf("arg %d %s %s %p\n", argc, argv[0], argv[1], argv[2]); - printf("arg %d %p\n", argc, argv); // Calculate argc - if (argv) { - for(;;) { - p = argv[argc]; - if (p == (char *)0) - break; - printf("arg %d %s\n", argc, argv[argc]); - ++argc; - len += strlen(p)+1; - } + while (p != (char *)0) { + p = argv[argc]; + if (p == (char *)0) + break; + ++argc; + len += strlen(p)+1; } + printf("arg %d %p\n", argc, argv); structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *); pthdata_t *taskdata; diff --git a/src/platforms/qurt/px4_layer/work_lock.h b/src/platforms/qurt/px4_layer/work_lock.h new file mode 100644 index 0000000000..f77f228b36 --- /dev/null +++ b/src/platforms/qurt/px4_layer/work_lock.h @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include + +#pragma once +extern sem_t _work_lock[]; + +inline void work_lock(int id); +inline void work_lock(int id) +{ + //printf("work_lock %d\n", id); + sem_wait(&_work_lock[id]); +} + +inline void work_unlock(int id); +inline void work_unlock(int id) +{ + //printf("work_unlock %d\n", id); + sem_post(&_work_lock[id]); +} + diff --git a/src/platforms/qurt/px4_layer/work_queue.c b/src/platforms/qurt/px4_layer/work_queue.c index 644b25e54b..1746359b63 100644 --- a/src/platforms/qurt/px4_layer/work_queue.c +++ b/src/platforms/qurt/px4_layer/work_queue.c @@ -43,7 +43,10 @@ #include #include #include +#include +#include #include +#include "work_lock.h" #ifdef CONFIG_SCHED_WORKQUEUE @@ -117,13 +120,13 @@ int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_ * from with task logic or interrupt handlers. */ - //flags = irqsave(); + work_lock(qid); work->qtime = clock_systimer(); /* Time work queued */ dq_addlast((dq_entry_t *)work, &wqueue->q); px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */ - //irqrestore(flags); + work_unlock(qid); return PX4_OK; } diff --git a/src/platforms/qurt/px4_layer/work_thread.c b/src/platforms/qurt/px4_layer/work_thread.c index cb8cbe9766..a57cf4b10a 100644 --- a/src/platforms/qurt/px4_layer/work_thread.c +++ b/src/platforms/qurt/px4_layer/work_thread.c @@ -39,12 +39,14 @@ #include #include +#include #include #include #include #include #include #include +#include "work_lock.h" #ifdef CONFIG_SCHED_WORKQUEUE @@ -66,6 +68,7 @@ struct wqueue_s g_work[NWORKERS]; /**************************************************************************** * Private Variables ****************************************************************************/ +sem_t _work_lock[NWORKERS]; /**************************************************************************** * Private Functions @@ -85,13 +88,12 @@ struct wqueue_s g_work[NWORKERS]; * ****************************************************************************/ -static void work_process(FAR struct wqueue_s *wqueue) +static void work_process(FAR struct wqueue_s *wqueue, int lock_id) { volatile FAR struct work_s *work; worker_t worker; - //irqstate_t flags; FAR void *arg; - uint32_t elapsed; + uint64_t elapsed; uint32_t remaining; uint32_t next; @@ -100,7 +102,9 @@ static void work_process(FAR struct wqueue_s *wqueue) */ next = CONFIG_SCHED_WORKPERIOD; - //flags = irqsave(); + + work_lock(lock_id); + work = (FAR struct work_s *)wqueue->q.head; while (work) { @@ -110,8 +114,8 @@ static void work_process(FAR struct wqueue_s *wqueue) * zero. Therefore a delay of zero will always execute immediately. */ - elapsed = clock_systimer() - work->qtime; - //printf("work_process: elapsed=%d delay=%d\n", elapsed, work->delay); + 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 */ @@ -133,7 +137,7 @@ static void work_process(FAR struct wqueue_s *wqueue) * performed... we don't have any idea how long that will take! */ - //irqrestore(flags); + work_unlock(lock_id); if (!worker) { printf("MESSED UP: worker = 0\n"); } @@ -145,7 +149,7 @@ static void work_process(FAR struct wqueue_s *wqueue) * back at the head of the list. */ - //flags = irqsave(); + work_lock(lock_id); work = (FAR struct work_s *)wqueue->q.head; } else @@ -155,7 +159,7 @@ static void work_process(FAR struct wqueue_s *wqueue) */ /* Here: elapsed < work->delay */ - remaining = work->delay - elapsed; + remaining = USEC_PER_TICK*(work->delay - elapsed); if (remaining < next) { /* Yes.. Then schedule to wake up when the work is ready */ @@ -172,15 +176,40 @@ static void work_process(FAR struct wqueue_s *wqueue) /* 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); - //FIXME - DSPAL doesn't support usleep - //usleep(next); - //irqrestore(flags); + usleep(next); } /**************************************************************************** * Public Functions ****************************************************************************/ +void work_queues_init(void) +{ + sem_init(&_work_lock[HPWORK], 0, 1); + sem_init(&_work_lock[LPWORK], 0, 1); +#ifdef CONFIG_SCHED_USRWORK + sem_init(&_work_lock[USRWORK], 0, 1); +#endif + + // 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); + + // 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); + +} + /**************************************************************************** * Name: work_hpthread, work_lpthread, and work_usrthread * @@ -235,7 +264,7 @@ int work_hpthread(int argc, char *argv[]) * we process items in the work list. */ - work_process(&g_work[HPWORK]); + work_process(&g_work[HPWORK], HPWORK); } return PX4_OK; /* To keep some compilers happy */ @@ -262,7 +291,7 @@ int work_lpthread(int argc, char *argv[]) * we process items in the work list. */ - work_process(&g_work[LPWORK]); + work_process(&g_work[LPWORK], LPWORK); } return PX4_OK; /* To keep some compilers happy */ @@ -283,7 +312,7 @@ int work_usrthread(int argc, char *argv[]) * we process items in the work list. */ - work_process(&g_work[USRWORK]); + work_process(&g_work[USRWORK], USRWORK); } return PX4_OK; /* To keep some compilers happy */ @@ -293,6 +322,7 @@ int work_usrthread(int argc, char *argv[]) uint32_t clock_systimer() { + //printf("clock_systimer: %0lx\n", hrt_absolute_time()); return (0x00000000ffffffff & hrt_absolute_time()); } #endif /* CONFIG_SCHED_WORKQUEUE */