diff --git a/makefiles/qurt.mk b/makefiles/qurt.mk index 919dcc4aba..f34392a210 100644 --- a/makefiles/qurt.mk +++ b/makefiles/qurt.mk @@ -36,5 +36,5 @@ MODULES += \ platforms/common \ - platforms/linux/px4_layer + platforms/qurt/px4_layer diff --git a/makefiles/qurt/config_qurt_default.mk b/makefiles/qurt/config_qurt_default.mk index 2a856ad3eb..c076cd5841 100644 --- a/makefiles/qurt/config_qurt_default.mk +++ b/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 diff --git a/makefiles/qurt_elf.mk b/makefiles/qurt_elf.mk index 701d731ef3..57c6434e1d 100644 --- a/makefiles/qurt_elf.mk +++ b/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)) # diff --git a/makefiles/toolchain_hexagon.mk b/makefiles/toolchain_hexagon.mk index 6b3027684e..dacb60bfb1 100644 --- a/makefiles/toolchain_hexagon.mk +++ b/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 = v5 # Set for APQ8074 +#V_ARCH = v4 +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) \ - -c \ - -G0 \ - $(_CODE) +ARCHCPUFLAGS = -m$(V_ARCH) # 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 -LIB_HEXAGON =$(HEXAGON_TOOLS_ROOT)/qc/lib/$(V_ARCH)/G0/libhexagon.a +HEXAGON_LIB_PATH = $(HEXAGON_TOOLS_ROOT)/gnu/hexagon/lib/$(V_ARCH)/G0 +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 diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index a40943d3f2..3ef3dbce94 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -39,6 +39,7 @@ #pragma once +#include #include #include #include diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h index 5388f08f1d..caeeb09a8b 100644 --- a/src/platforms/px4_config.h +++ b/src/platforms/px4_config.h @@ -42,7 +42,7 @@ #if defined(__PX4_NUTTX) #include -#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 diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 97f5bc7d6e..f2bd2f969d 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -102,7 +102,7 @@ #include #include -#elif defined(__PX4_LINUX) || defined(__PX4_QURT) +#elif defined(__PX4_LINUX) #include #include #include @@ -132,6 +132,36 @@ #include #include #include +#elif defined(__PX4_QURT) +#include +#include +#include + +#define ASSERT(x) assert(x) + +#ifdef __cplusplus +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#endif +#include +#include +#include #else #error "No target platform defined" #endif diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index d86ddd6645..786614fc9d 100644 --- a/src/platforms/px4_middleware.h +++ b/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 diff --git a/src/platforms/px4_workqueue.h b/src/platforms/px4_workqueue.h index 686bfdf258..a275faebe9 100644 --- a/src/platforms/px4_workqueue.h +++ b/src/platforms/px4_workqueue.h @@ -41,7 +41,7 @@ #include #include #include -#elif defined(__PX4_LINUX) +#elif defined(__PX4_LINUX) || defined(__PX4_QURT) #include #include diff --git a/src/platforms/qurt/include/poll.h b/src/platforms/qurt/include/poll.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/platforms/qurt/main.cpp b/src/platforms/qurt/main.cpp index 8ae5ee58d5..aab84cd016 100644 --- a/src/platforms/qurt/main.cpp +++ b/src/platforms/qurt/main.cpp @@ -42,63 +42,20 @@ #include #include #include +#include -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 &appargs); -void run_cmd(const vector &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 appargs(5); - - stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4]; - run_cmd(appargs); -} +//static command = "list_builtins"; int main(int argc, char **argv) { - // Execute a command list of provided - if (argc == 2) { - ifstream infile(argv[1]); - - 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 = ""; - } + printf("hello\n"); + list_builtins(); + //apps["hello"](i,(char **)arg);; } diff --git a/src/platforms/qurt/px4_layer/dq_addlast.c b/src/platforms/qurt/px4_layer/dq_addlast.c new file mode 100644 index 0000000000..3ef08abd05 --- /dev/null +++ b/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 + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: dq_addlast + * + * Description + * dq_addlast adds 'node' to the end of 'queue' + * + ************************************************************/ + +void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue) +{ + node->flink = NULL; + node->blink = queue->tail; + + if (!queue->head) + { + queue->head = node; + queue->tail = node; + } + else + { + queue->tail->flink = node; + queue->tail = node; + } +} + diff --git a/src/platforms/qurt/px4_layer/dq_rem.c b/src/platforms/qurt/px4_layer/dq_rem.c new file mode 100644 index 0000000000..db20762c75 --- /dev/null +++ b/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 + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: dq_rem + * + * Descripton: + * dq_rem removes 'node' from 'queue' + * + ************************************************************/ + +void dq_rem(FAR dq_entry_t *node, dq_queue_t *queue) +{ + FAR dq_entry_t *prev = node->blink; + FAR dq_entry_t *next = node->flink; + + if (!prev) + { + queue->head = next; + } + else + { + prev->flink = next; + } + + if (!next) + { + queue->tail = prev; + } + else + { + next->blink = prev; + } + + node->flink = NULL; + node->blink = NULL; +} + diff --git a/src/platforms/qurt/px4_layer/dq_remfirst.c b/src/platforms/qurt/px4_layer/dq_remfirst.c new file mode 100644 index 0000000000..e87acc3382 --- /dev/null +++ b/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 + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: dq_remfirst + * + * Description: + * dq_remfirst removes 'node' from the head of 'queue' + * + ************************************************************/ + +FAR dq_entry_t *dq_remfirst(dq_queue_t *queue) +{ + FAR dq_entry_t *ret = queue->head; + + if (ret) + { + FAR dq_entry_t *next = ret->flink; + if (!next) + { + queue->head = NULL; + queue->tail = NULL; + } + else + { + queue->head = next; + next->blink = NULL; + } + + ret->flink = NULL; + ret->blink = NULL; + } + + return ret; +} + diff --git a/src/platforms/qurt/px4_layer/drv_hrt.c b/src/platforms/qurt/px4_layer/drv_hrt.c new file mode 100644 index 0000000000..39e0f73b37 --- /dev/null +++ b/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 +#include +#include + +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 + diff --git a/src/platforms/qurt/px4_layer/lib_crc32.c b/src/platforms/qurt/px4_layer/lib_crc32.c new file mode 100644 index 0000000000..4ba6fbf6df --- /dev/null +++ b/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 +#include +#include + +// 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); +} diff --git a/src/platforms/qurt/px4_layer/module.mk b/src/platforms/qurt/px4_layer/module.mk new file mode 100644 index 0000000000..7d377e8bdd --- /dev/null +++ b/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 diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp new file mode 100644 index 0000000000..36c795a213 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#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); + +} + +} + diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp new file mode 100644 index 0000000000..f402bde549 --- /dev/null +++ b/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 + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#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; iargv[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, ¶m); + 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) { + 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) + 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 + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Modified for use in Linux by Mark Charlebois + * + ************************************************************************/ + +// FIXME - need px4_queue +#include +#include + +__EXPORT void sq_rem(sq_entry_t *node, sq_queue_t *queue); +sq_entry_t *sq_remafter(sq_entry_t *node, sq_queue_t *queue); +void sq_rem(sq_entry_t *node, sq_queue_t *queue) +{ + if (queue->head && node) + { + if (node == queue->head) + { + queue->head = node->flink; + if (node == queue->tail) + { + queue->tail = NULL; + } + } + else + { + sq_entry_t *prev; + for(prev = (sq_entry_t*)queue->head; + prev && prev->flink != node; + prev = prev->flink); + + if (prev) + { + sq_remafter(prev, queue); + } + } + } +} + +sq_entry_t *sq_remafter(sq_entry_t *node, sq_queue_t *queue) +{ + sq_entry_t *ret = node->flink; + if (queue->head && ret) + { + if (queue->tail == ret) + { + queue->tail = node; + node->flink = NULL; + } + else + { + node->flink = ret->flink; + } + + ret->flink = NULL; + } + + return ret; +} + +__EXPORT void sq_addfirst(sq_entry_t *node, sq_queue_t *queue); +void sq_addfirst(sq_entry_t *node, sq_queue_t *queue) +{ + node->flink = queue->head; + if (!queue->head) + { + queue->tail = node; + } + queue->head = node; +} + + diff --git a/src/platforms/qurt/px4_layer/sq_addafter.c b/src/platforms/qurt/px4_layer/sq_addafter.c new file mode 100644 index 0000000000..5d47feba0f --- /dev/null +++ b/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 + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: sq_addafter.c + * + * Description: + * The sq_addafter function adds 'node' after 'prev' in the + * 'queue.' + * + ************************************************************/ + +void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, + sq_queue_t *queue) +{ + if (!queue->head || prev == queue->tail) + { + sq_addlast(node, queue); + } + else + { + node->flink = prev->flink; + prev->flink = node; + } +} diff --git a/src/platforms/qurt/px4_layer/sq_addlast.c b/src/platforms/qurt/px4_layer/sq_addlast.c new file mode 100644 index 0000000000..faa07efb5c --- /dev/null +++ b/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 + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: sq_addlast + * + * Description: + * The sq_addlast function places the 'node' at the tail of + * the 'queue' + ************************************************************/ + +void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue) +{ + node->flink = NULL; + if (!queue->head) + { + queue->head = node; + queue->tail = node; + } + else + { + queue->tail->flink = node; + queue->tail = node; + } +} + diff --git a/src/platforms/qurt/px4_layer/sq_remfirst.c b/src/platforms/qurt/px4_layer/sq_remfirst.c new file mode 100644 index 0000000000..f81c18dc2e --- /dev/null +++ b/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 + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: sq_remfirst + * + * Description: + * sq_remfirst function removes the first entry from + * 'queue' + * + ************************************************************/ + +FAR sq_entry_t *sq_remfirst(sq_queue_t *queue) +{ + FAR sq_entry_t *ret = queue->head; + + if (ret) + { + queue->head = ret->flink; + if (!queue->head) + { + queue->tail = NULL; + } + + ret->flink = NULL; + } + + return ret; +} + diff --git a/src/platforms/qurt/px4_layer/work_cancel.c b/src/platforms/qurt/px4_layer/work_cancel.c new file mode 100644 index 0000000000..6f737877d9 --- /dev/null +++ b/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 + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include + +#ifdef CONFIG_SCHED_WORKQUEUE + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: work_cancel + * + * Description: + * Cancel previously queued work. This removes work from the work queue. + * After work has been canceled, it may be re-queue by calling work_queue() + * again. + * + * Input parameters: + * qid - The work queue ID + * work - The previously queue work structure to cancel + * + * Returned Value: + * Zero on success, a negated errno on failure + * + ****************************************************************************/ + +int work_cancel(int qid, struct work_s *work) +{ + struct wqueue_s *wqueue = &g_work[qid]; + //irqstate_t flags; + + //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); + + /* Cancelling the work is simply a matter of removing the work structure + * from the work queue. This must be done with interrupts disabled because + * new work is typically added to the work queue from interrupt handlers. + */ + + //flags = irqsave(); + if (work->worker != NULL) + { + /* A little test of the integrity of the work queue */ + + //DEBUGASSERT(work->dq.flink ||(FAR dq_entry_t *)work == wqueue->q.tail); + //DEBUGASSERT(work->dq.blink ||(FAR dq_entry_t *)work == wqueue->q.head); + + /* Remove the entry from the work queue and make sure that it is + * mark as availalbe (i.e., the worker field is nullified). + */ + + dq_rem((FAR dq_entry_t *)work, &wqueue->q); + work->worker = NULL; + } + + //irqrestore(flags); + return PX4_OK; +} + +#endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/qurt/px4_layer/work_queue.c b/src/platforms/qurt/px4_layer/work_queue.c new file mode 100644 index 0000000000..cd96aacd2f --- /dev/null +++ b/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 + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include +#include + +#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 */ diff --git a/src/platforms/qurt/px4_layer/work_thread.c b/src/platforms/qurt/px4_layer/work_thread.c new file mode 100644 index 0000000000..cb8cbe9766 --- /dev/null +++ b/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 + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include + +#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 */