Platform cleanup - remove duplicate files

The POSIX and QURT platforms contain several duplicate files.
These files have been factored out into platforms/posix/work_queue.
The config files have been updated to include the
platforms/posix/work_queue module.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

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

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

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

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

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

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

src/platforms/posix/px4_layer/work_lock.c

@@ -1,19 +0,0 @@
-//#pragma once
-#include <semaphore.h>
-#include <stdio.h>
-#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]);
-}

src/platforms/posix/px4_layer/work_thread.c

@@ -1,328 +0,0 @@
-/****************************************************************************
- * libc/wqueue/work_thread.c
- *
- *   Copyright (C) 2009-2013 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
- *
- * 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 <px4_config.h>
-#include <px4_defines.h>
-#include <stdint.h>
-#include <stdio.h>
-#include <unistd.h>
-#include <queue.h>
-#include <px4_workqueue.h>
-#include <drivers/drv_hrt.h>
-#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 */

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;

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

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;

src/platforms/qurt/px4_layer/dq_addlast.c

@@ -1,74 +0,0 @@
-/************************************************************
- * libc/queue/dq_addlast.c
- *
- *   Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
- *
- * 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 <queue.h>
-
-/************************************************************
- * 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;
-    }
-}
-

src/platforms/qurt/px4_layer/dq_rem.c

@@ -1,84 +0,0 @@
-/************************************************************
- * libc/queue/dq_rem.c
- *
- *   Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
- *
- * 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 <queue.h>
-
-/************************************************************
- * 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;
-}
-

src/platforms/qurt/px4_layer/dq_remfirst.c

@@ -1,82 +0,0 @@
-/************************************************************
- * libc/queue/dq_remfirst.c
- *
- *   Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
- *
- * 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 <queue.h>
-
-/************************************************************
- * 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;
-}
-

src/platforms/qurt/px4_layer/hrt_queue.c

@@ -1,129 +0,0 @@
-/****************************************************************************
- * libc/wqueue/work_queue.c
- *
- *   Copyright (C) 2009-2011, 2013 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
- *
- * 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 <px4_config.h>
-#include <px4_defines.h>
-
-#include <signal.h>
-#include <stdint.h>
-#include <queue.h>
-#include <stdio.h>
-#include <semaphore.h>
-#include <drivers/drv_hrt.h>
-#include <px4_workqueue.h>
-#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;
-}
-

src/platforms/qurt/px4_layer/hrt_thread.c

@@ -1,256 +0,0 @@
-/****************************************************************************
- * libc/wqueue/work_thread.c
- *
- *   Copyright (C) 2009-2013 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
- *   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 <px4_config.h>
-#include <px4_defines.h>
-#include <stdint.h>
-#include <stdio.h>
-#include <unistd.h>
-#include <queue.h>
-#include <px4_workqueue.h>
-#include <drivers/drv_hrt.h>
-#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);
-
-}
-

src/platforms/qurt/px4_layer/hrt_work_cancel.c

@@ -1,112 +0,0 @@
-/****************************************************************************
- * libc/wqueue/work_cancel.c
- *
- *   Copyright (C) 2009-2010, 2012-2013 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
- *
- * 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 <px4_config.h>
-#include <px4_defines.h>
-#include <queue.h>
-#include <semaphore.h>
-#include <px4_workqueue.h>
-#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();
-}

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

src/platforms/qurt/px4_layer/queue.c

@@ -1,102 +0,0 @@
-/************************************************************************
- *
- *   Copyright (C) 2007-2009 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
- *
- * 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 <platforms/qurt/include/queue.h>
-#include <stddef.h>
-
-__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;
-}
-
-

src/platforms/qurt/px4_layer/sq_addafter.c

@@ -1,71 +0,0 @@
-/************************************************************
- * libc/queue/sq_addafter.c
- *
- *   Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
- *
- * 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 <queue.h>
-
-/************************************************************
- * 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;
-    }
-}

src/platforms/qurt/px4_layer/sq_addlast.c

@@ -1,72 +0,0 @@
-/************************************************************
- * libc/queue/sq_addlast.c
- *
- *   Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
- *
- * 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 <queue.h>
-
-/************************************************************
- * 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;
-    }
-}
-

src/platforms/qurt/px4_layer/sq_remfirst.c

@@ -1,76 +0,0 @@
-/************************************************************
- * libc/queue/sq_remfirst.c
- *
- *   Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
- *
- * 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 <queue.h>
-
-/************************************************************
- * 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;
-}
-

src/platforms/qurt/px4_layer/work_cancel.c

@@ -1,120 +0,0 @@
-/****************************************************************************
- * libc/wqueue/work_cancel.c
- *
- *   Copyright (C) 2009-2010, 2012-2013 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
- *
- * 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 <px4_config.h>
-#include <px4_defines.h>
-#include <queue.h>
-#include <px4_workqueue.h>
-
-#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 */

src/platforms/qurt/px4_layer/work_lock.h

@@ -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_

src/platforms/qurt/px4_layer/work_queue.c

@@ -1,133 +0,0 @@
-/****************************************************************************
- * libc/wqueue/work_queue.c
- *
- *   Copyright (C) 2009-2011, 2013 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
- *
- * 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 <px4_config.h>
-#include <px4_defines.h>
-
-#include <signal.h>
-#include <stdint.h>
-#include <queue.h>
-#include <stdio.h>
-#include <semaphore.h>
-#include <px4_workqueue.h>
-#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 */