From e24405d3747b2732cbfa562b5cc1683bf5ae752a Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 5 May 2015 00:27:14 -0700 Subject: [PATCH] POSIX: fixed hrt call and workqueue implementation The HRT call processing normally happens via HW timer interrupt handler. Since the POSIX port has no ISR handling, the HP work queue is used. Instead of irq_save() and irq_restore() calls to disable/enable interrupts, a mutex is used to protect each queue. Signed-off-by: Mark Charlebois --- src/platforms/posix/main.cpp | 6 +++ src/platforms/posix/px4_layer/drv_hrt.c | 16 +++++- .../posix/px4_layer/px4_posix_impl.cpp | 28 +++------- src/platforms/posix/px4_layer/work_lock.h | 51 +++++++++++++++++++ src/platforms/posix/px4_layer/work_queue.c | 7 ++- src/platforms/posix/px4_layer/work_thread.c | 47 +++++++++++++---- .../posix/tests/hrt_test/hrt_test.cpp | 15 ++++-- .../posix/tests/hrt_test/hrt_test_main.cpp | 6 +-- src/platforms/px4_workqueue.h | 9 ++++ 9 files changed, 144 insertions(+), 41 deletions(-) create mode 100644 src/platforms/posix/px4_layer/work_lock.h diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index b76a12ef9c..b497133b36 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -43,6 +43,10 @@ #include #include +namespace px4 { +void init_once(void); +} + using namespace std; typedef int (*px4_main_t)(int argc, char *argv[]); @@ -94,6 +98,8 @@ int main(int argc, char **argv) string mystr; + px4::init_once(); + px4::init(argc, argv, "mainapp"); while(1) { diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 7094ca7b8d..f5c351b187 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -66,11 +66,13 @@ 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); } @@ -219,11 +221,13 @@ hrt_call_enter(struct hrt_call *entry) static void hrt_tim_isr(void *p) { - hrt_lock(); + //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(); @@ -262,7 +266,7 @@ hrt_call_reschedule() if (next->deadline <= (now + HRT_INTERVAL_MIN)) { //lldbg("pre-expired\n"); /* set a minimal deadline so that we call ASAP */ - ticks = USEC2TICK(HRT_INTERVAL_MIN); + ticks = USEC2TICK(HRT_INTERVAL_MIN*1000); } else if (next->deadline < deadline) { //lldbg("due soon\n"); @@ -272,6 +276,7 @@ hrt_call_reschedule() // 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); } @@ -353,6 +358,7 @@ 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(); @@ -376,8 +382,13 @@ hrt_call_invoke(void) /* 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 */ @@ -392,5 +403,6 @@ hrt_call_invoke(void) hrt_call_enter(call); } } + hrt_unlock(); } diff --git a/src/platforms/posix/px4_layer/px4_posix_impl.cpp b/src/platforms/posix/px4_layer/px4_posix_impl.cpp index d164337687..3f1916a51a 100644 --- a/src/platforms/posix/px4_layer/px4_posix_impl.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_impl.cpp @@ -55,32 +55,20 @@ extern void hrt_init(void); __END_DECLS -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); - - // 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); - - hrt_init(); } } diff --git a/src/platforms/posix/px4_layer/work_lock.h b/src/platforms/posix/px4_layer/work_lock.h new file mode 100644 index 0000000000..89e8b65eca --- /dev/null +++ b/src/platforms/posix/px4_layer/work_lock.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * 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) +{ + //printf("work_lock %d\n", id); + sem_wait(&_work_lock[id]); +} + +inline void work_unlock(int id) +{ + //printf("work_unlock %d\n", id); + sem_post(&_work_lock[id]); +} + diff --git a/src/platforms/posix/px4_layer/work_queue.c b/src/platforms/posix/px4_layer/work_queue.c index cd96aacd2f..46ca359842 100644 --- a/src/platforms/posix/px4_layer/work_queue.c +++ b/src/platforms/posix/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, SIGCONT); /* Wake up the worker thread */ - //irqrestore(flags); + work_unlock(qid); return PX4_OK; } diff --git a/src/platforms/posix/px4_layer/work_thread.c b/src/platforms/posix/px4_layer/work_thread.c index 57e6e327f1..1128a80944 100644 --- a/src/platforms/posix/px4_layer/work_thread.c +++ b/src/platforms/posix/px4_layer/work_thread.c @@ -45,6 +45,7 @@ #include #include #include +#include "work_lock.h" #ifdef CONFIG_SCHED_WORKQUEUE @@ -66,6 +67,7 @@ struct wqueue_s g_work[NWORKERS]; /**************************************************************************** * Private Variables ****************************************************************************/ +sem_t _work_lock[NWORKERS]; /**************************************************************************** * Private Functions @@ -85,11 +87,10 @@ 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; uint64_t elapsed; uint32_t remaining; @@ -100,7 +101,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) { @@ -133,7 +136,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 +148,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 @@ -172,14 +175,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); usleep(next); - //irqrestore(flags); } /**************************************************************************** * 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 * @@ -234,7 +263,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 */ @@ -261,7 +290,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 */ @@ -282,7 +311,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 */ diff --git a/src/platforms/posix/tests/hrt_test/hrt_test.cpp b/src/platforms/posix/tests/hrt_test/hrt_test.cpp index c54e35430a..877462a44b 100644 --- a/src/platforms/posix/tests/hrt_test/hrt_test.cpp +++ b/src/platforms/posix/tests/hrt_test/hrt_test.cpp @@ -46,9 +46,17 @@ px4::AppState HRTTest::appState; +static struct hrt_call t1; +static int update_interval = 1; + static void timer_expired(void *arg) { + static int i = 0; printf("Test\n"); + if (i < 5) { + i++; + hrt_call_after(&t1, update_interval, timer_expired, (void *)0); + } } int HRTTest::main() @@ -67,18 +75,15 @@ int HRTTest::main() printf("Elapsed time %llu in 1 sec (sleep)\n", (unsigned long long)elt); printf("Start time %llu\n", (unsigned long long)t); - struct hrt_call t1; - int update_interval = 1; - memset(&t1, 0, sizeof(t1)); printf("HRT_CALL %d\n", hrt_called(&t1)); hrt_call_after(&t1, update_interval, timer_expired, (void *)0); sleep(2); - printf("HRT_CALL %d\n", hrt_called(&t1)); + printf("HRT_CALL - %d\n", hrt_called(&t1)); hrt_cancel(&t1); - printf("HRT_CALL %d\n", hrt_called(&t1)); + printf("HRT_CALL + %d\n", hrt_called(&t1)); return 0; } diff --git a/src/platforms/posix/tests/hrt_test/hrt_test_main.cpp b/src/platforms/posix/tests/hrt_test/hrt_test_main.cpp index b7c332cd3b..9a01690e38 100644 --- a/src/platforms/posix/tests/hrt_test/hrt_test_main.cpp +++ b/src/platforms/posix/tests/hrt_test/hrt_test_main.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file hello_main.cpp + * @file hrt_test_main.cpp * Example for Linux * * @author Mark Charlebois @@ -44,9 +44,9 @@ int PX4_MAIN(int argc, char **argv) { - px4::init(argc, argv, "hello"); + px4::init(argc, argv, "hrt_test"); - printf("hello\n"); + printf("starting\n"); HRTTest test; test.main(); diff --git a/src/platforms/px4_workqueue.h b/src/platforms/px4_workqueue.h index 67c2fcdfa7..b9ae6b4b6c 100644 --- a/src/platforms/px4_workqueue.h +++ b/src/platforms/px4_workqueue.h @@ -73,6 +73,15 @@ struct work_s uint32_t delay; /* Delay until work performed */ }; +/**************************************************************************** + * Name: work_queues_init() + * + * Description: + * Initialize the work queues. + * + ****************************************************************************/ +void work_queues_init(void); + /**************************************************************************** * Name: work_queue *