diff --git a/makefiles/posix/config_posix_sitl.mk b/makefiles/posix/config_posix_sitl.mk index 909c6d74e2..61b25c6212 100644 --- a/makefiles/posix/config_posix_sitl.mk +++ b/makefiles/posix/config_posix_sitl.mk @@ -72,9 +72,10 @@ MODULES += lib/geo_lookup MODULES += lib/conversion # -# Linux port +# POSIX port # MODULES += platforms/posix/px4_layer +MODULES += platforms/posix/work_queue MODULES += platforms/posix/drivers/accelsim MODULES += platforms/posix/drivers/gyrosim MODULES += platforms/posix/drivers/adcsim diff --git a/makefiles/qurt/config_qurt_default.mk b/makefiles/qurt/config_qurt_default.mk index 398fe05194..5f154b110a 100644 --- a/makefiles/qurt/config_qurt_default.mk +++ b/makefiles/qurt/config_qurt_default.mk @@ -63,6 +63,7 @@ MODULES += lib/conversion # QuRT port # MODULES += platforms/qurt/px4_layer +MODULES += platforms/posix/work_queue MODULES += platforms/posix/drivers/accelsim MODULES += platforms/posix/drivers/gyrosim MODULES += platforms/posix/drivers/adcsim diff --git a/makefiles/qurt/config_qurt_hello.mk b/makefiles/qurt/config_qurt_hello.mk index 4b11a79fe3..12d17d9a9f 100644 --- a/makefiles/qurt/config_qurt_hello.mk +++ b/makefiles/qurt/config_qurt_hello.mk @@ -58,6 +58,7 @@ MODULES += modules/uORB # QuRT port # MODULES += platforms/qurt/px4_layer +MODULES += platforms/posix/work_queue #MODULES += platforms/posix/drivers/accelsim #MODULES += platforms/posix/drivers/gyrosim #MODULES += platforms/posix/drivers/adcsim diff --git a/makefiles/qurt/config_qurt_hil.mk b/makefiles/qurt/config_qurt_hil.mk index 9d4a48eeed..02082e8335 100644 --- a/makefiles/qurt/config_qurt_hil.mk +++ b/makefiles/qurt/config_qurt_hil.mk @@ -62,6 +62,7 @@ MODULES += lib/conversion # QuRT port # MODULES += platforms/qurt/px4_layer +MODULES += platforms/posix/work_queue #MODULES += platforms/posix/drivers/accelsim #MODULES += platforms/posix/drivers/gyrosim #MODULES += platforms/posix/drivers/adcsim diff --git a/makefiles/qurt/config_qurt_muorb_test.mk b/makefiles/qurt/config_qurt_muorb_test.mk index b503e44a69..6b30b77d1b 100644 --- a/makefiles/qurt/config_qurt_muorb_test.mk +++ b/makefiles/qurt/config_qurt_muorb_test.mk @@ -60,6 +60,7 @@ MODULES += lib/conversion # QuRT port # MODULES += platforms/qurt/px4_layer +MODULES += platforms/posix/work_queue MODULES += platforms/posix/drivers/accelsim MODULES += platforms/posix/drivers/gyrosim MODULES += platforms/posix/drivers/adcsim diff --git a/src/platforms/posix/px4_layer/module.mk b/src/platforms/posix/px4_layer/module.mk index 73b6ce9e00..7d7e75afee 100644 --- a/src/platforms/posix/px4_layer/module.mk +++ b/src/platforms/posix/px4_layer/module.mk @@ -32,27 +32,13 @@ ############################################################################ # -# NuttX / uORB adapter library +# POSIX platform dependent files # SRCS = \ px4_posix_impl.cpp \ px4_posix_tasks.cpp \ - hrt_thread.c \ - hrt_queue.c \ - hrt_work_cancel.c \ - work_thread.c \ - work_lock.c \ - work_queue.c \ - work_cancel.c \ lib_crc32.c \ drv_hrt.c \ - queue.c \ - dq_addlast.c \ - dq_remfirst.c \ - sq_addlast.c \ - sq_remfirst.c \ - sq_addafter.c \ - dq_rem.c MAXOPTIMIZATION = -Os diff --git a/src/platforms/posix/px4_layer/work_lock.c b/src/platforms/posix/px4_layer/work_lock.c deleted file mode 100644 index b2ad307d7c..0000000000 --- a/src/platforms/posix/px4_layer/work_lock.c +++ /dev/null @@ -1,19 +0,0 @@ -//#pragma once -#include -#include -#include "work_lock.h" - - -extern sem_t _work_lock[]; - -void work_lock(int id) -{ - //printf("work_lock %d\n", id); - sem_wait(&_work_lock[id]); -} - -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_thread.c b/src/platforms/posix/px4_layer/work_thread.c deleted file mode 100644 index 107d233f22..0000000000 --- a/src/platforms/posix/px4_layer/work_thread.c +++ /dev/null @@ -1,328 +0,0 @@ -/**************************************************************************** - * libc/wqueue/work_thread.c - * - * Copyright (C) 2009-2013 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ****************************************************************************/ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include "work_lock.h" - -#ifdef CONFIG_SCHED_WORKQUEUE - -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Type Declarations - ****************************************************************************/ - -/**************************************************************************** - * Public Variables - ****************************************************************************/ - -/* The state of each work queue. */ -struct wqueue_s g_work[NWORKERS]; - -/**************************************************************************** - * Private Variables - ****************************************************************************/ -sem_t _work_lock[NWORKERS]; - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: work_process - * - * Description: - * This is the logic that performs actions placed on any work list. - * - * Input parameters: - * wqueue - Describes the work queue to be processed - * - * Returned Value: - * None - * - ****************************************************************************/ - -static void work_process(FAR 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; - - /* Then process queued work. We need to keep interrupts disabled while - * we process items in the work list. - */ - - next = CONFIG_SCHED_WORKPERIOD; - - 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. - */ - - elapsed = USEC2TICK(clock_systimer() - work->qtime); - //PX4_INFO("work_process: queue=%p in ticks elapsed=%lu delay=%u time=%u", wqueue, elapsed, work->delay,clock_systimer()); - if (elapsed >= work->delay) - { - /* Remove the ready-to-execute work from the list */ - - (void)dq_rem((struct dq_entry_s *)work, &wqueue->q); - - /* Extract the work description from the entry (in case the work - * instance by the re-used after it has been de-queued). - */ - - worker = work->worker; - arg = work->arg; - - /* Mark the work as no longer being queued */ - - work->worker = NULL; - - /* Do the work. Re-enable interrupts while the work is being - * performed... we don't have any idea how long that will take! - */ - - work_unlock(lock_id); - if (!worker) { - PX4_ERR("MESSED UP: worker = 0"); - } - else { - worker(arg); - } - - /* 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_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? - */ - - /* 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 = (FAR 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); -} - -/**************************************************************************** - * 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 - * - * Description: - * These are the worker threads that performs actions placed on the work - * lists. - * - * work_hpthread and work_lpthread: These are the kernel mode work queues - * (also build in the flat build). One of these threads also performs - * periodic garbage collection (that is otherwise performed by the idle - * thread if CONFIG_SCHED_WORKQUEUE is not defined). - * - * These worker threads are started by the OS during normal bringup. - * - * work_usrthread: This is a user mode work queue. It must be built into - * the applicatino blob during the user phase of a kernel build. The - * user work thread will then automatically be started when the system - * boots by calling through the pointer found in the header on the user - * space blob. - * - * All of these entrypoints are referenced by OS internally and should not - * not be accessed by application logic. - * - * Input parameters: - * argc, argv (not used) - * - * Returned Value: - * Does not return - * - ****************************************************************************/ - -#ifdef CONFIG_SCHED_HPWORK - -int work_hpthread(int argc, char *argv[]) -{ - /* 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). - */ - -#ifndef CONFIG_SCHED_LPWORK - sched_garbagecollection(); -#endif - - /* Then process queued work. We need to keep interrupts disabled while - * we process items in the work list. - */ - - work_process(&g_work[HPWORK], HPWORK); - } - - return PX4_OK; /* To keep some compilers happy */ -} - -#ifdef CONFIG_SCHED_LPWORK - -int work_lpthread(int argc, char *argv[]) -{ - /* 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). - */ - - //sched_garbagecollection(); - - /* Then process queued work. We need to keep interrupts disabled while - * we process items in the work list. - */ - - work_process(&g_work[LPWORK], LPWORK); - } - - return PX4_OK; /* To keep some compilers happy */ -} - -#endif /* CONFIG_SCHED_LPWORK */ -#endif /* CONFIG_SCHED_HPWORK */ - -#ifdef CONFIG_SCHED_USRWORK - -int work_usrthread(int argc, char *argv[]) -{ - /* Loop forever */ - - 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); - } - - return PX4_OK; /* To keep some compilers happy */ -} - -#endif /* CONFIG_SCHED_USRWORK */ - -uint32_t clock_systimer() -{ - //printf("clock_systimer: %0lx\n", hrt_absolute_time()); - return (0x00000000ffffffff & hrt_absolute_time()); -} -#endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/posix/px4_layer/dq_addlast.c b/src/platforms/posix/work_queue/dq_addlast.c similarity index 100% rename from src/platforms/posix/px4_layer/dq_addlast.c rename to src/platforms/posix/work_queue/dq_addlast.c diff --git a/src/platforms/posix/px4_layer/dq_rem.c b/src/platforms/posix/work_queue/dq_rem.c similarity index 100% rename from src/platforms/posix/px4_layer/dq_rem.c rename to src/platforms/posix/work_queue/dq_rem.c diff --git a/src/platforms/posix/px4_layer/dq_remfirst.c b/src/platforms/posix/work_queue/dq_remfirst.c similarity index 100% rename from src/platforms/posix/px4_layer/dq_remfirst.c rename to src/platforms/posix/work_queue/dq_remfirst.c diff --git a/src/platforms/posix/px4_layer/hrt_queue.c b/src/platforms/posix/work_queue/hrt_queue.c similarity index 98% rename from src/platforms/posix/px4_layer/hrt_queue.c rename to src/platforms/posix/work_queue/hrt_queue.c index e45132253a..27ac1110fa 100644 --- a/src/platforms/posix/px4_layer/hrt_queue.c +++ b/src/platforms/posix/work_queue/hrt_queue.c @@ -121,7 +121,11 @@ int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t del //PX4_INFO("hrt work_queue adding work delay=%u time=%lu", delay, work->qtime); dq_addlast((dq_entry_t *)work, &wqueue->q); +#ifdef __PX4_QURT + px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */ +#else px4_task_kill(wqueue->pid, SIGCONT); /* Wake up the worker thread */ +#endif hrt_work_unlock(); return PX4_OK; diff --git a/src/platforms/posix/px4_layer/hrt_thread.c b/src/platforms/posix/work_queue/hrt_thread.c similarity index 100% rename from src/platforms/posix/px4_layer/hrt_thread.c rename to src/platforms/posix/work_queue/hrt_thread.c diff --git a/src/platforms/posix/px4_layer/hrt_work_cancel.c b/src/platforms/posix/work_queue/hrt_work_cancel.c similarity index 100% rename from src/platforms/posix/px4_layer/hrt_work_cancel.c rename to src/platforms/posix/work_queue/hrt_work_cancel.c diff --git a/src/platforms/posix/work_queue/module.mk b/src/platforms/posix/work_queue/module.mk new file mode 100644 index 0000000000..2a493ab7b6 --- /dev/null +++ b/src/platforms/posix/work_queue/module.mk @@ -0,0 +1,54 @@ +############################################################################ +# +# Copyright (c) 2014 PX4 Development Team. 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. +# +############################################################################ + +# +# POSIX compatible queue and work_queue implementation +# + +SRCS = \ + hrt_thread.c \ + hrt_queue.c \ + hrt_work_cancel.c \ + work_thread.c \ + work_lock.c \ + work_queue.c \ + work_cancel.c \ + queue.c \ + dq_addlast.c \ + dq_remfirst.c \ + sq_addlast.c \ + sq_remfirst.c \ + sq_addafter.c \ + dq_rem.c + +MAXOPTIMIZATION = -Os diff --git a/src/platforms/posix/px4_layer/queue.c b/src/platforms/posix/work_queue/queue.c similarity index 100% rename from src/platforms/posix/px4_layer/queue.c rename to src/platforms/posix/work_queue/queue.c diff --git a/src/platforms/posix/px4_layer/sq_addafter.c b/src/platforms/posix/work_queue/sq_addafter.c similarity index 100% rename from src/platforms/posix/px4_layer/sq_addafter.c rename to src/platforms/posix/work_queue/sq_addafter.c diff --git a/src/platforms/posix/px4_layer/sq_addlast.c b/src/platforms/posix/work_queue/sq_addlast.c similarity index 100% rename from src/platforms/posix/px4_layer/sq_addlast.c rename to src/platforms/posix/work_queue/sq_addlast.c diff --git a/src/platforms/posix/px4_layer/sq_remfirst.c b/src/platforms/posix/work_queue/sq_remfirst.c similarity index 100% rename from src/platforms/posix/px4_layer/sq_remfirst.c rename to src/platforms/posix/work_queue/sq_remfirst.c diff --git a/src/platforms/posix/px4_layer/work_cancel.c b/src/platforms/posix/work_queue/work_cancel.c similarity index 100% rename from src/platforms/posix/px4_layer/work_cancel.c rename to src/platforms/posix/work_queue/work_cancel.c diff --git a/src/platforms/qurt/px4_layer/work_lock.c b/src/platforms/posix/work_queue/work_lock.c similarity index 100% rename from src/platforms/qurt/px4_layer/work_lock.c rename to src/platforms/posix/work_queue/work_lock.c diff --git a/src/platforms/posix/px4_layer/work_lock.h b/src/platforms/posix/work_queue/work_lock.h similarity index 100% rename from src/platforms/posix/px4_layer/work_lock.h rename to src/platforms/posix/work_queue/work_lock.h diff --git a/src/platforms/posix/px4_layer/work_queue.c b/src/platforms/posix/work_queue/work_queue.c similarity index 98% rename from src/platforms/posix/px4_layer/work_queue.c rename to src/platforms/posix/work_queue/work_queue.c index 46ca359842..23a5c93638 100644 --- a/src/platforms/posix/px4_layer/work_queue.c +++ b/src/platforms/posix/work_queue/work_queue.c @@ -124,7 +124,11 @@ int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_ work->qtime = clock_systimer(); /* Time work queued */ dq_addlast((dq_entry_t *)work, &wqueue->q); +#ifdef __PX4_QURT + px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */ +#else px4_task_kill(wqueue->pid, SIGCONT); /* Wake up the worker thread */ +#endif work_unlock(qid); return PX4_OK; diff --git a/src/platforms/qurt/px4_layer/work_thread.c b/src/platforms/posix/work_queue/work_thread.c similarity index 100% rename from src/platforms/qurt/px4_layer/work_thread.c rename to src/platforms/posix/work_queue/work_thread.c diff --git a/src/platforms/qurt/px4_layer/dq_addlast.c b/src/platforms/qurt/px4_layer/dq_addlast.c deleted file mode 100644 index 3ef08abd05..0000000000 --- a/src/platforms/qurt/px4_layer/dq_addlast.c +++ /dev/null @@ -1,74 +0,0 @@ -/************************************************************ - * libc/queue/dq_addlast.c - * - * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ************************************************************/ - -/************************************************************ - * Compilation Switches - ************************************************************/ - -/************************************************************ - * Included Files - ************************************************************/ - -#include - -/************************************************************ - * Public Functions - ************************************************************/ - -/************************************************************ - * Name: dq_addlast - * - * Description - * dq_addlast adds 'node' to the end of 'queue' - * - ************************************************************/ - -void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue) -{ - node->flink = NULL; - node->blink = queue->tail; - - if (!queue->head) - { - queue->head = node; - queue->tail = node; - } - else - { - queue->tail->flink = node; - queue->tail = node; - } -} - diff --git a/src/platforms/qurt/px4_layer/dq_rem.c b/src/platforms/qurt/px4_layer/dq_rem.c deleted file mode 100644 index db20762c75..0000000000 --- a/src/platforms/qurt/px4_layer/dq_rem.c +++ /dev/null @@ -1,84 +0,0 @@ -/************************************************************ - * libc/queue/dq_rem.c - * - * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ************************************************************/ - -/************************************************************ - * Compilation Switches - ************************************************************/ - -/************************************************************ - * Included Files - ************************************************************/ - -#include - -/************************************************************ - * Public Functions - ************************************************************/ - -/************************************************************ - * Name: dq_rem - * - * Descripton: - * dq_rem removes 'node' from 'queue' - * - ************************************************************/ - -void dq_rem(FAR dq_entry_t *node, dq_queue_t *queue) -{ - FAR dq_entry_t *prev = node->blink; - FAR dq_entry_t *next = node->flink; - - if (!prev) - { - queue->head = next; - } - else - { - prev->flink = next; - } - - if (!next) - { - queue->tail = prev; - } - else - { - next->blink = prev; - } - - node->flink = NULL; - node->blink = NULL; -} - diff --git a/src/platforms/qurt/px4_layer/dq_remfirst.c b/src/platforms/qurt/px4_layer/dq_remfirst.c deleted file mode 100644 index e87acc3382..0000000000 --- a/src/platforms/qurt/px4_layer/dq_remfirst.c +++ /dev/null @@ -1,82 +0,0 @@ -/************************************************************ - * libc/queue/dq_remfirst.c - * - * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ************************************************************/ - -/************************************************************ - * Compilation Switches - ************************************************************/ - -/************************************************************ - * Included Files - ************************************************************/ - -#include - -/************************************************************ - * Public Functions - ************************************************************/ - -/************************************************************ - * Name: dq_remfirst - * - * Description: - * dq_remfirst removes 'node' from the head of 'queue' - * - ************************************************************/ - -FAR dq_entry_t *dq_remfirst(dq_queue_t *queue) -{ - FAR 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; - } - - ret->flink = NULL; - ret->blink = NULL; - } - - return ret; -} - diff --git a/src/platforms/qurt/px4_layer/hrt_queue.c b/src/platforms/qurt/px4_layer/hrt_queue.c deleted file mode 100644 index 04bf8bdd49..0000000000 --- a/src/platforms/qurt/px4_layer/hrt_queue.c +++ /dev/null @@ -1,129 +0,0 @@ -/**************************************************************************** - * libc/wqueue/work_queue.c - * - * Copyright (C) 2009-2011, 2013 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ****************************************************************************/ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include "hrt_work.h" - -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Type Declarations - ****************************************************************************/ - -/**************************************************************************** - * Public Variables - ****************************************************************************/ - -/**************************************************************************** - * Private Variables - ****************************************************************************/ - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: hrt_work_queue - * - * Description: - * Queue work to be performed at a later time. All queued work will be - * performed on the worker thread of of execution (not the caller's). - * - * The work structure is allocated by caller, but completely managed by - * the work queue logic. The caller should never modify the contents of - * the work queue structure; the caller should not call work_queue() - * again until either (1) the previous work has been performed and removed - * from the queue, or (2) work_cancel() has been called to cancel the work - * and remove it from the work queue. - * - * Input parameters: - * work - The work structure to queue - * worker - The worker callback to be invoked. The callback will invoked - * on the worker thread of execution. - * arg - The argument that will be passed to the workder callback when - * int is invoked. - * delay - Delay (in microseconds) from the time queue until the worker - * is invoked. Zero means to perform the work immediately. - * - * Returned Value: - * Zero on success, a negated errno on failure - * - ****************************************************************************/ - -int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay) -{ - struct wqueue_s *wqueue = &g_hrt_work; - - /* First, initialize the work structure */ - - 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. - */ - - 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); - px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */ - - hrt_work_unlock(); - return PX4_OK; -} - diff --git a/src/platforms/qurt/px4_layer/hrt_thread.c b/src/platforms/qurt/px4_layer/hrt_thread.c deleted file mode 100644 index dc0f3baa97..0000000000 --- a/src/platforms/qurt/px4_layer/hrt_thread.c +++ /dev/null @@ -1,256 +0,0 @@ -/**************************************************************************** - * libc/wqueue/work_thread.c - * - * Copyright (C) 2009-2013 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * Modified by: Mark Charlebois to use hrt compatible times - * - * 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 NuttX 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. - * - ****************************************************************************/ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include "hrt_work.h" - -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Type Declarations - ****************************************************************************/ - -/**************************************************************************** - * Public Variables - ****************************************************************************/ - -/* The state of each work queue. */ -struct wqueue_s g_hrt_work; - -/**************************************************************************** - * Private Variables - ****************************************************************************/ -sem_t _hrt_work_lock; - -/**************************************************************************** - * Private Functions - ****************************************************************************/ -static void hrt_work_process(void); - -/**************************************************************************** - * Name: work_process - * - * Description: - * This is the logic that performs actions placed on any work list. - * - * Input parameters: - * wqueue - Describes the work queue to be processed - * - * Returned Value: - * None - * - ****************************************************************************/ - -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; - - /* 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; - - 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. - */ - - 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 */ - - (void)dq_rem((struct dq_entry_s *)work, &wqueue->q); - //PX4_INFO("Dequeued work=%p", work); - - /* Extract the work description from the entry (in case the work - * instance by the re-used after it has been de-queued). - */ - - worker = work->worker; - arg = work->arg; - - /* Mark the work as no longer being queued */ - - work->worker = NULL; - - /* 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_unlock(); - if (!worker) { - PX4_ERR("MESSED UP: worker = 0"); - } - else { - worker(arg); - } - - /* 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. - */ - - 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? - */ - - /* 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 = (FAR 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); -} - -/**************************************************************************** - * Name: work_hrtthread - * - * Description: - * This is the worker threads that performs actions placed on the ISR work - * list. - * - * work_hpthread and work_lpthread: These are the kernel mode work queues - * (also build in the flat build). One of these threads also performs - * periodic garbage collection (that is otherwise performed by the idle - * thread if CONFIG_SCHED_WORKQUEUE is not defined). - * - * These worker threads are started by the OS during normal bringup. - * - * All of these entrypoints are referenced by OS internally and should not - * not be accessed by application logic. - * - * Input parameters: - * argc, argv (not used) - * - * Returned Value: - * Does not return - * - ****************************************************************************/ - -static int work_hrtthread(int argc, char *argv[]) -{ - /* 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). - */ - - /* Then process queued work. We need to keep interrupts disabled while - * we process items in the work list. - */ - - hrt_work_process(); - } - - return PX4_OK; /* To keep some compilers happy */ -} - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -void hrt_work_queue_init(void) -{ - sem_init(&_hrt_work_lock, 0, 1); - - // 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); - -} - diff --git a/src/platforms/qurt/px4_layer/hrt_work_cancel.c b/src/platforms/qurt/px4_layer/hrt_work_cancel.c deleted file mode 100644 index 864f4b695f..0000000000 --- a/src/platforms/qurt/px4_layer/hrt_work_cancel.c +++ /dev/null @@ -1,112 +0,0 @@ -/**************************************************************************** - * libc/wqueue/work_cancel.c - * - * Copyright (C) 2009-2010, 2012-2013 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ****************************************************************************/ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include -#include -#include -#include -#include -#include "hrt_work.h" - -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Type Declarations - ****************************************************************************/ - -/**************************************************************************** - * Public Variables - ****************************************************************************/ - -/**************************************************************************** - * Private Variables - ****************************************************************************/ - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: hrt_work_cancel - * - * Description: - * Cancel previously queued work. This removes work from the work queue. - * After work has been canceled, it may be re-queue by calling - * hrt_work_queue() again. - * - * Input parameters: - * work - The previously queue work structure to cancel - * - ****************************************************************************/ - -void hrt_work_cancel(struct work_s *work) -{ - struct wqueue_s *wqueue = &g_hrt_work; - - //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. - */ - - 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); - //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). - */ - - dq_rem((FAR dq_entry_t *)work, &wqueue->q); - work->worker = NULL; - } - - hrt_work_unlock(); -} diff --git a/src/platforms/qurt/px4_layer/module.mk b/src/platforms/qurt/px4_layer/module.mk index 2747c18b8b..a10df59d36 100644 --- a/src/platforms/qurt/px4_layer/module.mk +++ b/src/platforms/qurt/px4_layer/module.mk @@ -42,25 +42,9 @@ SRCDIR=$(dir $(MODULE_MK)) SRCS = \ px4_qurt_impl.cpp \ px4_qurt_tasks.cpp \ - hrt_thread.c \ - hrt_queue.c \ - hrt_work_cancel.c \ - work_thread.c \ - work_queue.c \ - work_lock.c \ - work_cancel.c \ lib_crc32.c \ drv_hrt.c \ - queue.c \ - dq_addlast.c \ - dq_remfirst.c \ - sq_addlast.c \ - sq_remfirst.c \ - sq_addafter.c \ - dq_rem.c \ - hrt_work.c \ qurt_stubs.c \ - qurt_hacks.c \ main.cpp ifeq ($(CONFIG),qurt_hello) SRCS += commands_hello.c diff --git a/src/platforms/qurt/px4_layer/queue.c b/src/platforms/qurt/px4_layer/queue.c deleted file mode 100644 index eecbd98830..0000000000 --- a/src/platforms/qurt/px4_layer/queue.c +++ /dev/null @@ -1,102 +0,0 @@ -/************************************************************************ - * - * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - * Modified for use in Linux by Mark Charlebois - * - ************************************************************************/ - -// FIXME - need px4_queue -#include -#include - -__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 (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; - } - - 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; -} - - diff --git a/src/platforms/qurt/px4_layer/sq_addafter.c b/src/platforms/qurt/px4_layer/sq_addafter.c deleted file mode 100644 index 5d47feba0f..0000000000 --- a/src/platforms/qurt/px4_layer/sq_addafter.c +++ /dev/null @@ -1,71 +0,0 @@ -/************************************************************ - * libc/queue/sq_addafter.c - * - * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ************************************************************/ - -/************************************************************ - * Compilation Switches - ************************************************************/ - -/************************************************************ - * Included Files - ************************************************************/ - -#include - -/************************************************************ - * Public Functions - ************************************************************/ - -/************************************************************ - * Name: sq_addafter.c - * - * Description: - * The sq_addafter function adds 'node' after 'prev' in the - * 'queue.' - * - ************************************************************/ - -void sq_addafter(FAR sq_entry_t *prev, FAR 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; - } -} diff --git a/src/platforms/qurt/px4_layer/sq_addlast.c b/src/platforms/qurt/px4_layer/sq_addlast.c deleted file mode 100644 index faa07efb5c..0000000000 --- a/src/platforms/qurt/px4_layer/sq_addlast.c +++ /dev/null @@ -1,72 +0,0 @@ -/************************************************************ - * libc/queue/sq_addlast.c - * - * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ************************************************************/ - -/************************************************************ - * Compilation Switches - ************************************************************/ - -/************************************************************ - * Included Files - ************************************************************/ - -#include - -/************************************************************ - * Public Functions - ************************************************************/ - -/************************************************************ - * Name: sq_addlast - * - * Description: - * The sq_addlast function places the 'node' at the tail of - * the 'queue' - ************************************************************/ - -void sq_addlast(FAR 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; - } -} - diff --git a/src/platforms/qurt/px4_layer/sq_remfirst.c b/src/platforms/qurt/px4_layer/sq_remfirst.c deleted file mode 100644 index f81c18dc2e..0000000000 --- a/src/platforms/qurt/px4_layer/sq_remfirst.c +++ /dev/null @@ -1,76 +0,0 @@ -/************************************************************ - * libc/queue/sq_remfirst.c - * - * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ************************************************************/ - -/************************************************************ - * Compilation Switches - ************************************************************/ - -/************************************************************ - * Included Files - ************************************************************/ - -#include - -/************************************************************ - * Public Functions - ************************************************************/ - -/************************************************************ - * Name: sq_remfirst - * - * Description: - * sq_remfirst function removes the first entry from - * 'queue' - * - ************************************************************/ - -FAR sq_entry_t *sq_remfirst(sq_queue_t *queue) -{ - FAR sq_entry_t *ret = queue->head; - - if (ret) - { - queue->head = ret->flink; - if (!queue->head) - { - queue->tail = NULL; - } - - ret->flink = NULL; - } - - return ret; -} - diff --git a/src/platforms/qurt/px4_layer/work_cancel.c b/src/platforms/qurt/px4_layer/work_cancel.c deleted file mode 100644 index 6f737877d9..0000000000 --- a/src/platforms/qurt/px4_layer/work_cancel.c +++ /dev/null @@ -1,120 +0,0 @@ -/**************************************************************************** - * libc/wqueue/work_cancel.c - * - * Copyright (C) 2009-2010, 2012-2013 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ****************************************************************************/ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include -#include -#include -#include - -#ifdef CONFIG_SCHED_WORKQUEUE - -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Type Declarations - ****************************************************************************/ - -/**************************************************************************** - * Public Variables - ****************************************************************************/ - -/**************************************************************************** - * Private Variables - ****************************************************************************/ - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: work_cancel - * - * Description: - * Cancel previously queued work. This removes work from the work queue. - * After work has been canceled, it may be re-queue by calling work_queue() - * again. - * - * Input parameters: - * qid - The work queue ID - * work - The previously queue work structure to cancel - * - * Returned Value: - * Zero on success, a negated errno on failure - * - ****************************************************************************/ - -int work_cancel(int qid, struct work_s *work) -{ - struct wqueue_s *wqueue = &g_work[qid]; - //irqstate_t flags; - - //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(); - 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); - - /* Remove the entry from the work queue and make sure that it is - * mark as availalbe (i.e., the worker field is nullified). - */ - - dq_rem((FAR dq_entry_t *)work, &wqueue->q); - work->worker = NULL; - } - - //irqrestore(flags); - return PX4_OK; -} - -#endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/qurt/px4_layer/work_lock.h b/src/platforms/qurt/px4_layer/work_lock.h deleted file mode 100644 index ad2e5b4a01..0000000000 --- a/src/platforms/qurt/px4_layer/work_lock.h +++ /dev/null @@ -1,43 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -#ifndef _work_lock_h_ -#define _work_lock_h_ - - -//#pragma once - -void work_lock(int id); -void work_unlock(int id); - -#endif // _work_lock_h_ diff --git a/src/platforms/qurt/px4_layer/work_queue.c b/src/platforms/qurt/px4_layer/work_queue.c deleted file mode 100644 index 1746359b63..0000000000 --- a/src/platforms/qurt/px4_layer/work_queue.c +++ /dev/null @@ -1,133 +0,0 @@ -/**************************************************************************** - * libc/wqueue/work_queue.c - * - * Copyright (C) 2009-2011, 2013 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ****************************************************************************/ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include -#include - -#include -#include -#include -#include -#include -#include -#include "work_lock.h" - -#ifdef CONFIG_SCHED_WORKQUEUE - -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Type Declarations - ****************************************************************************/ - -/**************************************************************************** - * Public Variables - ****************************************************************************/ - -/**************************************************************************** - * Private Variables - ****************************************************************************/ - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: work_queue - * - * Description: - * Queue work to be performed at a later time. All queued work will be - * performed on the worker thread of of execution (not the caller's). - * - * The work structure is allocated by caller, but completely managed by - * the work queue logic. The caller should never modify the contents of - * the work queue structure; the caller should not call work_queue() - * again until either (1) the previous work has been performed and removed - * from the queue, or (2) work_cancel() has been called to cancel the work - * and remove it from the work queue. - * - * Input parameters: - * qid - The work queue ID (index) - * work - The work structure to queue - * worker - The worker callback to be invoked. The callback will invoked - * on the worker thread of execution. - * arg - The argument that will be passed to the workder callback when - * int is invoked. - * delay - Delay (in clock ticks) from the time queue until the worker - * is invoked. Zero means to perform the work immediately. - * - * Returned Value: - * Zero on success, a negated errno on failure - * - ****************************************************************************/ - -int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay) -{ - struct wqueue_s *wqueue = &g_work[qid]; - - //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); - - /* First, initialize the work structure */ - - 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. - */ - - 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 */ - - work_unlock(qid); - return PX4_OK; -} - -#endif /* CONFIG_SCHED_WORKQUEUE */