Qurt: Added more support for the QuRT target

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

makefiles/qurt.mk

@@ -36,5 +36,5 @@
 
 MODULES += \
 		platforms/common \
-		platforms/linux/px4_layer 
+		platforms/qurt/px4_layer 
 

makefiles/qurt/config_qurt_default.mk

@@ -59,9 +59,9 @@
 #MODULES		+= lib/conversion
 
 #
-# Linux port
+# QuRT port
 #
-#MODULES		+= platforms/linux/px4_layer
+MODULES		+= platforms/qurt/px4_layer
 #MODULES		+= platforms/linux/drivers/accelsim
 #MODULES		+= platforms/linux/drivers/gyrosim
 #MODULES		+= platforms/linux/drivers/adcsim
@@ -70,7 +70,7 @@
 #
 # Unit tests
 #
-MODULES		+= platforms/linux/tests/hello
+MODULES		+= platforms/qurt/tests/hello
 #MODULES		+= platforms/linux/tests/vcdev_test
 #MODULES		+= platforms/linux/tests/hrt_test
 #MODULES		+= platforms/linux/tests/wqueue

makefiles/qurt_elf.mk

@@ -56,9 +56,9 @@ $(PRODUCT_SHARED_PRELINK):	$(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS) $(GLOBAL_DEPS)
 $(PRODUCT_SHARED_LIB):		$(PRODUCT_SHARED_PRELINK)
 	$(call LINK_A,$@,$(PRODUCT_SHARED_PRELINK))
 
-MAIN = $(PX4_BASE)/src/platforms/linux/main.cpp
+MAIN = $(PX4_BASE)/src/platforms/qurt/main.cpp
 $(WORK_DIR)mainapp: $(PRODUCT_SHARED_LIB)
-	$(PX4_BASE)/Tools/linux_apps.py > apps.h
+	$(PX4_BASE)/Tools/qurt_apps.py > apps.h
 	$(call LINK,$@, -I. $(MAIN) $(PRODUCT_SHARED_LIB))
 
 #

makefiles/toolchain_hexagon.mk

@@ -38,8 +38,8 @@
 # Toolchain commands. Normally only used inside this file.
 #
 HEXAGON_TOOLS_ROOT	 = /opt/6.4.05
-#V_ARCH			 = v4 # Set for APQ8064
+#V_ARCH			 = v4
-V_ARCH			 = v5 # Set for APQ8074
+V_ARCH			 = v5
 CROSSDEV		 = hexagon-
 HEXAGON_BIN		 = $(addsuffix /gnu/bin,$(HEXAGON_TOOLS_ROOT))
 HEXAGON_CLANG_BIN	 = $(addsuffix /qc/bin,$(HEXAGON_TOOLS_ROOT))
@@ -72,11 +72,7 @@ MAXOPTIMIZATION		 ?= -O2
 
 # Base CPU flags for each of the supported architectures.
 #
-_CODE			 = $(addprefix -G,$(V_G_THRESHOLD))
+ARCHCPUFLAGS	 	 = -m$(V_ARCH) 
-ARCHCPUFLAGS	 	 = -m$(V_ARCH) \
-			   -c \
-			   -G0 \
-			   $(_CODE)
 
 
 # Set the board flags
@@ -86,8 +82,13 @@ $(error Board config does not define CONFIG_BOARD)
 endif
 ARCHDEFINES		+= -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \
 			    -D__PX4_QURT \
+			    -D__EXPORT= \
+			    -Dnoreturn_function= \
+			    -Drestrict= \
 			    -I$(PX4_BASE)/src/lib/eigen \
 			    -I$(PX4_BASE)/src/platforms/qurt/include \
+			    -I$(PX4_BASE)/../dspalmc/include \
+			    -I$(PX4_BASE)/../dspalmc/sys \
 			    -Wno-error=shadow
 
 # optimisation flags
@@ -114,6 +115,8 @@ ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics
 ARCHWARNINGS		 = -Wall \
 			   -Wextra \
 			   -Werror \
+			   -Wno-unused-parameter \
+			   -Wno-unused-variable \
 			   -Wno-cast-align \
 			   -Wno-missing-braces \
 			   -Wno-strict-aliasing
@@ -169,8 +172,8 @@ ifeq (1,$(V_dynamic))
 CXX_FLAGS += -fpic -D__V_DYNAMIC__
 endif
 
-HEXAGON_LIB_PATH=$(HEXAGON_TOOLS_ROOT)/gnu/hexagon/lib/$(V_ARCH)/G0
+HEXAGON_LIB_PATH	 = $(HEXAGON_TOOLS_ROOT)/gnu/hexagon/lib/$(V_ARCH)/G0
-LIB_HEXAGON =$(HEXAGON_TOOLS_ROOT)/qc/lib/$(V_ARCH)/G0/libhexagon.a
+LIB_HEXAGON 		 = $(HEXAGON_TOOLS_ROOT)/qc/lib/$(V_ARCH)/G0/libhexagon.a
 
 # Flags we pass to the assembler
 #
@@ -178,14 +181,11 @@ AFLAGS			 = $(CFLAGS) -D__ASSEMBLY__ \
 			   $(EXTRADEFINES) \
 			   $(EXTRAAFLAGS)
 
+LDSCRIPT		 = $(PX4_BASE)/linux-configs/linuxtest/scripts/ld.script
 # Flags we pass to the linker
 #
 LDFLAGS			+=  \
-			   -Wl,--start-group -Wl,--whole-archive -lc -lgcc -lstdc++ \
-			   $(LIB_HEXAGON) -Wl,--no-whole-archive -Wl,--end-group -Wl,--dynamic-linker= \
-			   -Wl,-E -Wl,--force-dynamic \
 			   $(EXTRALDFLAGS) \
-			   $(addprefix -T,$(LDSCRIPT)) \
 			   $(addprefix -L,$(LIB_DIRS))
 
 # Compiler support library
@@ -215,6 +215,7 @@ endef
 define COMPILEXX
 	@$(ECHO) "CXX:     $1"
 	@$(MKDIR) -p $(dir $2)
+	@echo $(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2
 	$(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2
 endef
 
@@ -228,11 +229,25 @@ endef
 
 # Produce partially-linked $1 from files in $2
 #
+#$(Q) $(LD) -Ur -o $1 $2 # -Ur not supported in ld.gold
 define PRELINK
 	@$(ECHO) "PRELINK: $1"
 	@$(MKDIR) -p $(dir $1)
-	$(Q) $(LD) -Ur -Map $1.map -o $1 $2 && $(OBJCOPY) --localize-hidden $1
+	echo $(Q) $(LD) -Ur -o $1 $2
+	$(Q) $(LD) -Ur -o $1 $2
+
 endef
+# Produce partially-linked $1 from files in $2
+#
+#$(Q) $(LD) -Ur -o $1 $2 # -Ur not supported in ld.gold
+define PRELINKF
+	@$(ECHO) "PRELINKF: $1"
+	@$(MKDIR) -p $(dir $1)
+	echo $(Q) $(LD) -Ur -T$(LDSCRIPT) -o $1 $2
+	$(Q) $(LD) -Ur -T$(LDSCRIPT) -o $1 $2
+
+endef
+#	$(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1
 
 # Update the archive $1 with the files in $2
 #
@@ -242,63 +257,33 @@ define ARCHIVE
 	$(Q) $(AR) $1 $2
 endef
 
-# Link the objects in $2 into the binary $1
+# Link the objects in $2 into the shared library $1
+#
+define LINK_A
+	@$(ECHO) "LINK_A:    $1"
+	@$(MKDIR) -p $(dir $1)
+	echo "$(Q) $(AR) $1 $2"
+	$(Q) $(AR) $1 $2
+endef
+
+# Link the objects in $2 into the shared library $1
+#
+define LINK_SO
+	@$(ECHO) "LINK_SO:    $1"
+	@$(MKDIR) -p $(dir $1)
+	echo "$(Q) $(CXX) $(LDFLAGS) -shared -Wl,-soname,`basename $1`.1 -o $1 $2 $(LIBS) $(EXTRA_LIBS)"
+	$(Q) $(CXX) $(LDFLAGS) -shared -Wl,-soname,`basename $1`.1 -o $1 $2 $(LIBS) -pthread -lc
+endef
+
+# Link the objects in $2 into the application $1
 #
 define LINK
 	@$(ECHO) "LINK:    $1"
 	@$(MKDIR) -p $(dir $1)
-	$(Q) $(LD) $(LDFLAGS) -Map $1.map -o $1 --start-group $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC) --end-group
+	echo $(Q) $(CXX) $(CXXFLAGS) $(LDFLAGS) -o $1 $2 $(LIBS) 
+	$(Q) $(CXX) $(CXXFLAGS) $(LDFLAGS) -o $1 $2
+
+#	$(Q) $(CXX) $(CXXFLAGS) $(LDFLAGS) -o $1 $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC)
+
 endef
 
-# Convert $1 from a linked object to a raw binary in $2
-#
-define SYM_TO_BIN
-	@$(ECHO) "BIN:     $2"
-	@$(MKDIR) -p $(dir $2)
-	$(Q) $(OBJCOPY) -O binary $1 $2
-endef
-
-# Take the raw binary $1 and make it into an object file $2.
-# The symbol $3 points to the beginning of the file, and $3_len
-# gives its length.
-#
-# - compile an empty file to generate a suitable object file
-# - relink the object and insert the binary file
-# - extract the length
-# - create const unsigned $3_len with the extracted length as its value and compile it to an object file
-# - link the two generated object files together
-# - edit symbol names to suit
-#
-# NOTE: exercise caution using this with absolute pathnames; it looks
-#       like the MinGW tools insert an extra _ in the binary symbol name; e.g.
-#	the path:
-#
-#	/d/px4/firmware/Build/px4fmu_default.build/romfs.img
-#
-#	is assigned symbols like:
-#
-#	_binary_d__px4_firmware_Build_px4fmu_default_build_romfs_img_size
-#
-#	when we would expect
-#
-#	_binary__d_px4_firmware_Build_px4fmu_default_build_romfs_img_size
-#
-define BIN_SYM_PREFIX
-	_binary_$(subst /,_,$(subst .,_,$1))
-endef
-define BIN_TO_OBJ
-	@$(ECHO) "OBJ:     $2"
-	@$(MKDIR) -p $(dir $2)
-	$(Q) $(ECHO) > $2.c
-	$(call COMPILE,$2.c,$2.c.o)
-	$(Q) $(LD) -r -o $2.bin.o $2.c.o -b binary $1
-	$(Q) $(ECHO) "const unsigned int $3_len = 0x`$(NM) -p --radix=x $2.bin.o | $(GREP) $(call BIN_SYM_PREFIX,$1)_size$$ | $(GREP) -o ^[0-9a-fA-F]*`;" > $2.c
-	$(call COMPILE,$2.c,$2.c.o)
-	$(Q) $(LD) -r -o $2 $2.c.o $2.bin.o
-	$(Q) $(OBJCOPY) $2 \
-		--redefine-sym $(call BIN_SYM_PREFIX,$1)_start=$3 \
-		--strip-symbol $(call BIN_SYM_PREFIX,$1)_size \
-		--strip-symbol $(call BIN_SYM_PREFIX,$1)_end \
-		--rename-section .data=.rodata
-	$(Q) $(REMOVE) $2.c $2.c.o $2.bin.o
-endef

src/drivers/drv_hrt.h

@@ -39,6 +39,7 @@
 
 #pragma once
 
+#include <px4_posix.h>
 #include <sys/types.h>
 #include <stdbool.h>
 #include <inttypes.h>

src/platforms/px4_config.h

@@ -42,7 +42,7 @@
 
 #if defined(__PX4_NUTTX)
 #include <px4_config.h>
-#elif defined (__PX4_LINUX)
+#elif defined (__PX4_LINUX) || defined (__PX4_QURT)
 #define CONFIG_NFILE_STREAMS 1
 #define CONFIG_SCHED_WORKQUEUE 1
 #define CONFIG_SCHED_HPWORK 1

src/platforms/px4_includes.h

@@ -102,7 +102,7 @@
 #include <systemlib/param/param.h>
 #include <systemlib/systemlib.h>
 
-#elif defined(__PX4_LINUX) || defined(__PX4_QURT)
+#elif defined(__PX4_LINUX)
 #include <string.h>
 #include <assert.h>
 #include <uORB/uORB.h>
@@ -132,6 +132,36 @@
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>
 #include <systemlib/systemlib.h>
+#elif defined(__PX4_QURT)
+#include <string.h>
+#include <assert.h>
+#include <uORB/uORB.h>
+
+#define ASSERT(x) assert(x)
+
+#ifdef __cplusplus
+#include <platforms/qurt/px4_messages/px4_rc_channels.h>
+#include <platforms/qurt/px4_messages/px4_vehicle_attitude_setpoint.h>
+#include <platforms/qurt/px4_messages/px4_manual_control_setpoint.h>
+#include <platforms/qurt/px4_messages/px4_actuator_controls.h>
+#include <platforms/qurt/px4_messages/px4_actuator_controls_0.h>
+#include <platforms/qurt/px4_messages/px4_actuator_controls_1.h>
+#include <platforms/qurt/px4_messages/px4_actuator_controls_2.h>
+#include <platforms/qurt/px4_messages/px4_actuator_controls_3.h>
+#include <platforms/qurt/px4_messages/px4_vehicle_rates_setpoint.h>
+#include <platforms/qurt/px4_messages/px4_vehicle_attitude.h>
+#include <platforms/qurt/px4_messages/px4_vehicle_control_mode.h>
+#include <platforms/qurt/px4_messages/px4_actuator_armed.h>
+#include <platforms/qurt/px4_messages/px4_parameter_update.h>
+#include <platforms/qurt/px4_messages/px4_vehicle_status.h>
+#include <platforms/qurt/px4_messages/px4_vehicle_local_position_setpoint.h>
+#include <platforms/qurt/px4_messages/px4_vehicle_global_velocity_setpoint.h>
+#include <platforms/qurt/px4_messages/px4_vehicle_local_position.h>
+#include <platforms/qurt/px4_messages/px4_position_setpoint_triplet.h>
+#endif
+#include <systemlib/err.h>
+#include <systemlib/param/param.h>
+#include <systemlib/systemlib.h>
 #else
 #error "No target platform defined"
 #endif

src/platforms/px4_middleware.h

@@ -60,6 +60,9 @@ extern bool task_should_exit;
  * Returns true if the app/task should continue to run
  */
 __EXPORT inline bool ok() { return !task_should_exit; }
+#elif defined(__PX4_QURT)
+// FIXME - usleep not supported by DSPAL
+inline void usleep(uint64_t sleep_interval) { }
 #else
 /**
  * Linux needs to have globally unique checks for thread/task status

src/platforms/px4_workqueue.h

@@ -41,7 +41,7 @@
 #include <nuttx/arch.h>
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
-#elif defined(__PX4_LINUX)
+#elif defined(__PX4_LINUX) || defined(__PX4_QURT)
 
 #include <stdint.h>
 #include <queue.h>

src/platforms/qurt/main.cpp

@@ -42,63 +42,20 @@
 #include <string>
 #include <sstream>
 #include <vector>
+#include <hexagon_standalone.h>
 
-using namespace std;
+//using namespace std;
 
-typedef int (*px4_main_t)(int argc, char *argv[]);
+//typedef int (*px4_main_t)(int argc, char *argv[]);
 
 #include "apps.h"
-#include "px4_middleware.h"
+//#include "px4_middleware.h"
 
-void run_cmd(const vector<string> &appargs);
+//static command = "list_builtins";
-void run_cmd(const vector<string> &appargs) {
-	// command is appargs[0]
-	string command = appargs[0];
-	if (apps.find(command) != apps.end()) {
-		const char *arg[appargs.size()+2];
-
-		unsigned int i = 0;
-		while (i < appargs.size() && appargs[i] != "") {
-			arg[i] = (char *)appargs[i].c_str();
-			++i;
-		}
-		arg[i] = (char *)0;
-		apps[command](i,(char **)arg);
-	}
-	else
-	{
-		cout << "Invalid command" << endl;
-		list_builtins();
-	}
-}
-
-static void process_line(string &line)
-{
-	vector<string> appargs(5);
-
-	stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4];
-	run_cmd(appargs);
-}
 
 int main(int argc, char **argv)
 {
-	// Execute a command list of provided
+	printf("hello\n");
-	if (argc == 2) {
+	list_builtins();
-		ifstream infile(argv[1]);
+	//apps["hello"](i,(char **)arg);;
-
-		for (string line; getline(infile, line, '\n'); ) {
-			process_line(line);
-		}
-	}
-
-	string mystr;
-	
-	px4::init(argc, argv, "mainapp");
-
-	while(1) {
-		cout << "Enter a command and its args:" << endl;
-		getline (cin,mystr);
-		process_line(mystr);
-		mystr = "";
-	}
 }

src/platforms/qurt/px4_layer/dq_addlast.c

@@ -0,0 +1,74 @@
+/************************************************************
+ * 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

@@ -0,0 +1,84 @@
+/************************************************************
+ * 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

@@ -0,0 +1,82 @@
+/************************************************************
+ * 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/drv_hrt.c

@@ -0,0 +1,294 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file drv_hrt.c
+ *
+ * High-resolution timer with callouts and timekeeping.
+ */
+
+#include <drivers/drv_hrt.h>
+#include <time.h>
+#include <string.h>
+
+static struct sq_queue_s	callout_queue;
+
+/* latency histogram */
+#define LATENCY_BUCKET_COUNT 8
+__EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT;
+__EXPORT const uint16_t	latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
+__EXPORT uint32_t	latency_counters[LATENCY_BUCKET_COUNT + 1];
+
+static void		hrt_call_reschedule(void);
+
+#define HRT_INTERVAL_MIN	50
+#define HRT_INTERVAL_MAX	50000
+
+/*
+ * Get absolute time.
+ */
+hrt_abstime hrt_absolute_time(void)
+{
+	struct timespec ts;
+
+	// FIXME - not supported in QURT
+	//clock_gettime(CLOCK_MONOTONIC, &ts);
+	return ts_to_abstime(&ts);
+}
+
+/*
+ * Convert a timespec to absolute time.
+ */
+hrt_abstime ts_to_abstime(struct timespec *ts)
+{
+	hrt_abstime	result;
+
+	result = (hrt_abstime)(ts->tv_sec) * 1000000;
+	result += ts->tv_nsec / 1000;
+
+	return result;
+}
+
+/*
+ * Compute the delta between a timestamp taken in the past
+ * and now.
+ *
+ * This function is safe to use even if the timestamp is updated
+ * by an interrupt during execution.
+ */
+hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then)
+{
+	hrt_abstime delta = hrt_absolute_time() - *then;
+	return delta;
+}
+
+/*
+ * Store the absolute time in an interrupt-safe fashion.
+ *
+ * This function ensures that the timestamp cannot be seen half-written by an interrupt handler.
+ */
+hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now)
+{
+	hrt_abstime ts = hrt_absolute_time();
+	return ts;
+}
+
+
+/*
+ * If this returns true, the entry has been invoked and removed from the callout list,
+ * or it has never been entered.
+ *
+ * Always returns false for repeating callouts.
+ */
+bool	hrt_called(struct hrt_call *entry)
+{
+	return (entry->deadline == 0);
+}
+
+/*
+ * Remove the entry from the callout list.
+ */
+void	hrt_cancel(struct hrt_call *entry)
+{
+	// FIXME - need a lock
+	sq_rem(&entry->link, &callout_queue);
+	entry->deadline = 0;
+
+	/* if this is a periodic call being removed by the callout, prevent it from
+	 * being re-entered when the callout returns.
+	 */
+	entry->period = 0;
+	// endif
+}
+
+/*
+ * initialise a hrt_call structure
+ */
+void	hrt_call_init(struct hrt_call *entry)
+{
+	memset(entry, 0, sizeof(*entry));
+}
+
+/*
+ * delay a hrt_call_every() periodic call by the given number of
+ * microseconds. This should be called from within the callout to
+ * cause the callout to be re-scheduled for a later time. The periodic
+ * callouts will then continue from that new base time at the
+ * previously specified period.
+ */
+void	hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
+{
+	entry->deadline = hrt_absolute_time() + delay;
+}
+
+/*
+ * Initialise the HRT.
+ */
+void	hrt_init(void)
+{
+	sq_init(&callout_queue);
+}
+
+static void
+hrt_call_enter(struct hrt_call *entry)
+{
+	struct hrt_call	*call, *next;
+
+	call = (struct hrt_call *)sq_peek(&callout_queue);
+
+	if ((call == NULL) || (entry->deadline < call->deadline)) {
+		sq_addfirst(&entry->link, &callout_queue);
+		//lldbg("call enter at head, reschedule\n");
+		/* we changed the next deadline, reschedule the timer event */
+		hrt_call_reschedule();
+
+	} else {
+		do {
+			next = (struct hrt_call *)sq_next(&call->link);
+
+			if ((next == NULL) || (entry->deadline < next->deadline)) {
+				//lldbg("call enter after head\n");
+				sq_addafter(&call->link, &entry->link, &callout_queue);
+				break;
+			}
+		} while ((call = next) != NULL);
+	}
+
+	//lldbg("scheduled\n");
+}
+
+/**
+ * Reschedule the next timer interrupt.
+ *
+ * This routine must be called with interrupts disabled.
+ */
+static void
+hrt_call_reschedule()
+{
+	hrt_abstime	now = hrt_absolute_time();
+	struct hrt_call	*next = (struct hrt_call *)sq_peek(&callout_queue);
+	hrt_abstime	deadline = now + HRT_INTERVAL_MAX;
+
+	/*
+	 * Determine what the next deadline will be.
+	 *
+	 * Note that we ensure that this will be within the counter
+	 * period, so that when we truncate all but the low 16 bits
+	 * the next time the compare matches it will be the deadline
+	 * we want.
+	 *
+	 * It is important for accurate timekeeping that the compare
+	 * interrupt fires sufficiently often that the base_time update in
+	 * hrt_absolute_time runs at least once per timer period.
+	 */
+	if (next != NULL) {
+		//lldbg("entry in queue\n");
+		if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
+			//lldbg("pre-expired\n");
+			/* set a minimal deadline so that we call ASAP */
+			deadline = now + HRT_INTERVAL_MIN;
+
+		} else if (next->deadline < deadline) {
+			//lldbg("due soon\n");
+			deadline = next->deadline;
+		}
+	}
+}
+
+static void
+hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	/* if the entry is currently queued, remove it */
+	/* note that we are using a potentially uninitialised
+	   entry->link here, but it is safe as sq_rem() doesn't
+	   dereference the passed node unless it is found in the
+	   list. So we potentially waste a bit of time searching the
+	   queue for the uninitialised entry->link but we don't do
+	   anything actually unsafe.
+	*/
+	if (entry->deadline != 0)
+		sq_rem(&entry->link, &callout_queue);
+
+	entry->deadline = deadline;
+	entry->period = interval;
+	entry->callout = callout;
+	entry->arg = arg;
+
+	hrt_call_enter(entry);
+}
+
+/*
+ * Call callout(arg) after delay has elapsed.
+ *
+ * If callout is NULL, this can be used to implement a timeout by testing the call
+ * with hrt_called().
+ */
+void	hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry,
+			  hrt_absolute_time() + delay,
+			  0,
+			  callout,
+			  arg);
+}
+
+/*
+ * Call callout(arg) after delay, and then after every interval.
+ *
+ * Note thet the interval is timed between scheduled, not actual, call times, so the call rate may
+ * jitter but should not drift.
+ */
+void	hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry,
+			  hrt_absolute_time() + delay,
+			  interval,
+			  callout,
+			  arg);
+}
+
+/*
+ * Call callout(arg) at absolute time calltime.
+ */
+void	hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry, calltime, 0, callout, arg);
+}
+
+#if 0
+/*
+ * Convert absolute time to a timespec.
+ */
+void	abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
+#endif
+

src/platforms/qurt/px4_layer/lib_crc32.c

@@ -0,0 +1,126 @@
+/************************************************************************************************
+ * libc/misc/lib_crc32.c
+ *
+ * This file is a part of NuttX:
+ *
+ *   Copyright (C) 2010-2011 Gregory Nutt. All rights reserved.
+ *
+ * The logic in this file was developed by Gary S. Brown:
+ *
+ *   COPYRIGHT (C) 1986 Gary S. Brown.  You may use this program, or code or tables
+ *   extracted from it, as desired without restriction.
+ *
+ * First, the polynomial itself and its table of feedback terms.  The polynomial is:
+ *
+ *    X^32+X^26+X^23+X^22+X^16+X^12+X^11+X^10+X^8+X^7+X^5+X^4+X^2+X^1+X^0
+ *
+ * Note that we take it "backwards" and put the highest-order term in the lowest-order bit.
+ * The X^32 term is "implied"; the LSB is the X^31 term, etc.  The X^0 term (usually shown
+ * as "+1") results in the MSB being 1
+ *
+ * Note that the usual hardware shift register implementation, which is what we're using
+ * (we're merely optimizing it by doing eight-bit chunks at a time) shifts bits into the
+ * lowest-order term.  In our implementation, that means shifting towards the right.  Why
+ * do we do it this way?  Because the calculated CRC must be transmitted in order from
+ * highest-order term to lowest-order term.  UARTs transmit characters in order from LSB
+ * to MSB.  By storing the CRC this way we hand it to the UART in the order low-byte to
+ * high-byte; the UART sends each low-bit to hight-bit; and the result is transmission bit
+ * by bit from highest- to lowest-order term without requiring any bit shuffling on our
+ * part.  Reception works similarly
+ *
+ * The feedback terms table consists of 256, 32-bit entries.  Notes
+ *
+ * - The table can be generated at runtime if desired; code to do so is shown later.  It
+ *   might not be obvious, but the feedback terms simply represent the results of eight
+ *   shift/xor operations for all combinations of data and CRC register values
+ *
+ * - The values must be right-shifted by eight bits by the updcrc logic; the shift must
+ *   be u_(bring in zeroes).  On some hardware you could probably optimize the shift in
+ *   assembler by using byte-swap instructions polynomial $edb88320
+  ************************************************************************************************/
+
+/************************************************************************************************
+ * Included Files
+ ************************************************************************************************/
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <crc32.h>
+
+// Needed for Linux
+#define FAR 
+
+/************************************************************************************************
+ * Private Data
+ ************************************************************************************************/
+ 
+static const uint32_t crc32_tab[] =
+{
+  0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
+  0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
+  0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
+  0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
+  0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
+  0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
+  0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
+  0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
+  0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
+  0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
+  0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
+  0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
+  0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
+  0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
+  0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
+  0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
+  0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
+  0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
+  0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
+  0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
+  0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
+  0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
+  0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
+  0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
+  0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
+  0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
+  0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
+  0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
+  0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
+  0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
+  0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
+  0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
+};
+
+/************************************************************************************************
+ * Public Functions
+ ************************************************************************************************/
+/************************************************************************************************
+ * Name: crc32part
+ *
+ * Description:
+ *   Continue CRC calculation on a part of the buffer.
+ *
+ ************************************************************************************************/
+
+uint32_t crc32part(FAR const uint8_t *src, size_t len, uint32_t crc32val)
+{
+  size_t i;
+
+  for (i = 0;  i < len;  i++)
+    {
+      crc32val = crc32_tab[(crc32val ^ src[i]) & 0xff] ^ (crc32val >> 8);
+    }
+  return crc32val;
+}
+
+/************************************************************************************************
+ * Name: crc32
+ *
+ * Description:
+ *   Return a 32-bit CRC of the contents of the 'src' buffer, length 'len'
+ *
+ ************************************************************************************************/
+
+uint32_t crc32(FAR const uint8_t *src, size_t len)
+{
+  return crc32part(src, len, 0);
+}

src/platforms/qurt/px4_layer/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.
+#
+############################################################################
+
+#
+# NuttX / uORB adapter library
+#
+
+SRCS		 = 	\
+			px4_qurt_impl.cpp \
+			px4_qurt_tasks.cpp  \
+			work_thread.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/qurt/px4_layer/px4_qurt_impl.cpp

@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_linux_impl.cpp
+ *
+ * PX4 Middleware Wrapper Linux Implementation
+ */
+
+#include <px4_defines.h>
+#include <px4_middleware.h>
+#include <px4_workqueue.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <signal.h>
+#include <errno.h>
+#include <unistd.h>
+#include "systemlib/param/param.h"
+
+__BEGIN_DECLS
+
+// FIXME - sysconf(_SC_CLK_TCK) not supported
+long PX4_TICKS_PER_SEC = 1000;
+
+__END_DECLS
+
+extern struct wqueue_s gwork[NWORKERS];
+
+namespace px4
+{
+
+void init(int argc, char *argv[], const char *app_name)
+{
+	printf("App name: %s\n", app_name);
+
+	// Create high priority worker thread
+	g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high",
+			       SCHED_DEFAULT,
+			       SCHED_PRIORITY_MAX,
+			       2000,
+			       work_hpthread,
+			       (char* const*)NULL);
+
+	// Create low priority worker thread
+	g_work[LPWORK].pid = px4_task_spawn_cmd("wkr_low",
+			       SCHED_DEFAULT,
+			       SCHED_PRIORITY_MIN,
+			       2000,
+			       work_lpthread,
+			       (char* const*)NULL);
+
+}
+
+}
+

src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp

@@ -0,0 +1,284 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
+ *   Author: @author Mark Charlebois <charlebm#gmail.com>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_linux_tasks.c
+ * Implementation of existing task API for Linux
+ */
+
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <stdbool.h>
+#include <signal.h>
+#include <fcntl.h>
+#include <sched.h>
+#include <unistd.h>
+#include <string.h>
+
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <string>
+
+#include <px4_tasks.h>
+
+#define MAX_CMD_LEN 100
+
+#define PX4_MAX_TASKS 100
+struct task_entry
+{
+	pthread_t pid;
+	std::string name;
+	bool isused;
+	task_entry() : isused(false) {}
+};
+
+static task_entry taskmap[PX4_MAX_TASKS];
+
+typedef struct 
+{
+	px4_main_t entry;
+	int argc;
+	char *argv[];
+	// strings are allocated after the 
+} pthdata_t;
+
+static void *entry_adapter ( void *ptr )
+{
+	pthdata_t *data;            
+	data = (pthdata_t *) ptr;  
+
+	data->entry(data->argc, data->argv);
+	free(ptr);
+	printf("Before px4_task_exit\n");
+	px4_task_exit(0); 
+	printf("After px4_task_exit\n");
+
+	return NULL;
+} 
+
+void
+px4_systemreset(bool to_bootloader)
+{
+	printf("Called px4_system_reset\n");
+}
+
+px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[])
+{
+	int rv;
+	int argc = 0;
+	int i;
+	unsigned int len = 0;
+	unsigned long offset;
+	unsigned long structsize;
+	char * p = (char *)argv;
+
+        pthread_t task;
+	pthread_attr_t attr;
+	struct sched_param param;
+
+	// Calculate argc
+	while (p != (char *)0) {
+		p = argv[argc];
+		if (p == (char *)0)
+			break;
+		++argc;
+		len += strlen(p)+1;
+	}
+        structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *);
+	pthdata_t *taskdata;
+    
+	// not safe to pass stack data to the thread creation
+	taskdata = (pthdata_t *)malloc(structsize+len);
+	offset = ((unsigned long)taskdata)+structsize;
+
+    	taskdata->entry = entry;
+	taskdata->argc = argc;
+
+	for (i=0; i<argc; i++) {
+		printf("arg %d %s\n", i, argv[i]);
+		taskdata->argv[i] = (char *)offset;
+		strcpy((char *)offset, argv[i]);
+		offset+=strlen(argv[i])+1;
+	}
+	// Must add NULL at end of argv
+	taskdata->argv[argc] = (char *)0;
+
+#if 0
+	rv = pthread_attr_init(&attr);
+	if (rv != 0) {
+		printf("px4_task_spawn_cmd: failed to init thread attrs\n");
+		return (rv < 0) ? rv : -rv;
+	}
+	rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
+	if (rv != 0) {
+		printf("px4_task_spawn_cmd: failed to set inherit sched\n");
+		return (rv < 0) ? rv : -rv;
+	}
+	rv = pthread_attr_setschedpolicy(&attr, scheduler);
+	if (rv != 0) {
+		printf("px4_task_spawn_cmd: failed to set sched policy\n");
+		return (rv < 0) ? rv : -rv;
+	}
+
+	param.sched_priority = priority;
+
+	rv = pthread_attr_setschedparam(&attr, &param);
+	if (rv != 0) {
+		printf("px4_task_spawn_cmd: failed to set sched param\n");
+		return (rv < 0) ? rv : -rv;
+	}
+#endif
+
+        //rv = pthread_create (&task, &attr, &entry_adapter, (void *) taskdata);
+        rv = pthread_create (&task, NULL, &entry_adapter, (void *) taskdata);
+	if (rv != 0) {
+
+		if (rv == EPERM) {
+			//printf("WARNING: NOT RUNING AS ROOT, UNABLE TO RUN REALTIME THREADS\n");
+        		rv = pthread_create (&task, NULL, &entry_adapter, (void *) taskdata);
+			if (rv != 0) {
+				printf("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno);
+				return (rv < 0) ? rv : -rv;
+			}
+		}
+		else {
+			return (rv < 0) ? rv : -rv;
+		}
+	}
+
+	for (i=0; i<PX4_MAX_TASKS; ++i) {
+		if (taskmap[i].isused == false) {
+			taskmap[i].pid = task;
+			taskmap[i].name = name;
+			taskmap[i].isused = true;
+			break;
+		}
+	}
+	if (i>=PX4_MAX_TASKS) {
+		return -ENOSPC;
+	}
+        return i;
+}
+
+int px4_task_delete(px4_task_t id)
+{
+	int rv = 0;
+	pthread_t pid;
+	printf("Called px4_task_delete\n");
+
+	if (id < PX4_MAX_TASKS && taskmap[id].isused)
+		pid = taskmap[id].pid;
+	else
+		return -EINVAL;
+
+	// If current thread then exit, otherwise cancel
+        if (pthread_self() == pid) {
+		taskmap[id].isused = false;
+		pthread_exit(0);
+	} else {
+		rv = pthread_cancel(pid);
+	}
+
+	taskmap[id].isused = false;
+
+	return rv;
+}
+
+void px4_task_exit(int ret)
+{
+	int i; 
+	pthread_t pid = pthread_self();
+
+	// Get pthread ID from the opaque ID
+	for (i=0; i<PX4_MAX_TASKS; ++i) {
+		if (taskmap[i].pid == pid) {
+			taskmap[i].isused = false;
+			break;
+		}
+	}
+	if (i>=PX4_MAX_TASKS) 
+		printf("px4_task_exit: self task not found!\n");
+	else
+		printf("px4_task_exit: %s\n", taskmap[i].name.c_str());
+
+	pthread_exit((void *)(unsigned long)ret);
+}
+
+void px4_killall(void)
+{
+	//printf("Called px4_killall\n");
+	for (int i=0; i<PX4_MAX_TASKS; ++i) {
+		// FIXME - precludes pthread task to have an ID of 0
+		if (taskmap[i].isused == true) {
+			px4_task_delete(i);
+		}
+	}
+}
+
+int px4_task_kill(px4_task_t id, int sig)
+{
+	int rv = 0;
+	pthread_t pid;
+	//printf("Called px4_task_delete\n");
+
+	if (id < PX4_MAX_TASKS && taskmap[id].pid != 0)
+		pid = taskmap[id].pid;
+	else
+		return -EINVAL;
+
+	// If current thread then exit, otherwise cancel
+	rv = pthread_kill(pid, sig);
+
+	return rv;
+}
+
+void px4_show_tasks()
+{
+	int idx;
+	int count = 0;
+
+	printf("Active Tasks:\n");
+	for (idx=0; idx < PX4_MAX_TASKS; idx++)
+	{
+		if (taskmap[idx].isused) {
+			printf("   %-10s %p\n", taskmap[idx].name.c_str(), taskmap[idx].pid);
+			count++;
+		}
+	}
+	if (count == 0)
+		printf("   No running tasks\n");
+
+}

src/platforms/qurt/px4_layer/queue.c

@@ -0,0 +1,102 @@
+/************************************************************************
+ *
+ *   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/linux/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

@@ -0,0 +1,71 @@
+/************************************************************
+ * 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

@@ -0,0 +1,72 @@
+/************************************************************
+ * 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

@@ -0,0 +1,76 @@
+/************************************************************
+ * 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

@@ -0,0 +1,120 @@
+/****************************************************************************
+ * 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_queue.c

@@ -0,0 +1,130 @@
+/****************************************************************************
+ * 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 <px4_workqueue.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.
+   */
+
+  //flags        = irqsave();
+  work->qtime  = clock_systimer(); /* Time work queued */
+
+  dq_addlast((dq_entry_t *)work, &wqueue->q);
+  px4_task_kill(wqueue->pid, SIGCONT);      /* Wake up the worker thread */
+
+  //irqrestore(flags);
+  return PX4_OK;
+}
+
+#endif /* CONFIG_SCHED_WORKQUEUE */

src/platforms/qurt/px4_layer/work_thread.c

@@ -0,0 +1,298 @@
+/****************************************************************************
+ * 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>
+
+#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
+ ****************************************************************************/
+
+/****************************************************************************
+ * 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)
+{
+  volatile FAR struct work_s *work;
+  worker_t  worker;
+  //irqstate_t flags;
+  FAR void *arg;
+  uint32_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;
+  //flags = irqsave();
+  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 = clock_systimer() - work->qtime;
+      //printf("work_process: elapsed=%d delay=%d\n", elapsed, work->delay);
+      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!
+           */
+
+          //irqrestore(flags);
+	  if (!worker) {
+             printf("MESSED UP: worker = 0\n");
+          }
+          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.
+           */
+
+          //flags = irqsave();
+          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;
+          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.
+   */
+
+  //FIXME - DSPAL doesn't support usleep
+  //usleep(next);
+  //irqrestore(flags);
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+/****************************************************************************
+ * 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]);
+    }
+
+  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]);
+    }
+
+  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]);
+    }
+
+  return PX4_OK; /* To keep some compilers happy */
+}
+
+#endif /* CONFIG_SCHED_USRWORK */
+
+uint32_t clock_systimer()
+{
+	return (0x00000000ffffffff & hrt_absolute_time());
+}
+#endif /* CONFIG_SCHED_WORKQUEUE */