dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Qurt: Added more support for the QuRT target 

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois 2015-04-17 18:15:18 -07:00
parent 47beddc88f
commit 5d60437164
26 changed files with 2056 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
makefiles/qurt.mk
View File

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

6
makefiles/qurt/config_qurt_default.mk
View File

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

4
makefiles/qurt_elf.mk
View File

@ -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))
#

121
makefiles/toolchain_hexagon.mk
View File

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

1
src/drivers/drv_hrt.h
View File

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

2
src/platforms/px4_config.h
View File

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

32
src/platforms/px4_includes.h
View File

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

3
src/platforms/px4_middleware.h
View File

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

2
src/platforms/px4_workqueue.h
View File

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

0
src/platforms/qurt/include/poll.h Normal file
View File

59
src/platforms/qurt/main.cpp
View File

@ -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);
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);
}
//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);;
}

74
src/platforms/qurt/px4_layer/dq_addlast.c Normal file
View File

@ -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;
}
}

84
src/platforms/qurt/px4_layer/dq_rem.c Normal file
View File

@ -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;
}

82
src/platforms/qurt/px4_layer/dq_remfirst.c Normal file
View File

@ -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;
}

294
src/platforms/qurt/px4_layer/drv_hrt.c Normal file
View File

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

126
src/platforms/qurt/px4_layer/lib_crc32.c Normal file
View File

@ -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);
}

54
src/platforms/qurt/px4_layer/module.mk Normal file
View File

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

85
src/platforms/qurt/px4_layer/px4_qurt_impl.cpp Normal file
View File

@ -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);
}
}

284
src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp Normal file
View File

@ -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");
}

102
src/platforms/qurt/px4_layer/queue.c Normal file
View File

@ -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;
}

71
src/platforms/qurt/px4_layer/sq_addafter.c Normal file
View File

@ -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;
}
}

72
src/platforms/qurt/px4_layer/sq_addlast.c Normal file
View File

@ -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;
}
}

76
src/platforms/qurt/px4_layer/sq_remfirst.c Normal file
View File

@ -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;
}

120
src/platforms/qurt/px4_layer/work_cancel.c Normal file
View File

@ -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 */

130
src/platforms/qurt/px4_layer/work_queue.c Normal file
View File

@ -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 */

298
src/platforms/qurt/px4_layer/work_thread.c Normal file
View File

@ -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 */