From 884f62878d5222cf235b94b8b4792aa6c26031c8 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 13 May 2015 16:21:52 -0700 Subject: [PATCH] QuRT: pthread API now working The use of std::map and static initialization was an issue. The code was refactored to not use static initialization. Signed-off-by: Mark Charlebois --- Tools/posix_apps.py | 2 +- Tools/qurt_apps.py | 33 +++- makefiles/qurt/config_qurt_default.mk | 6 +- makefiles/qurt_elf.mk | 10 +- makefiles/toolchain_hexagon.mk | 5 - src/platforms/px4_log.h | 1 + src/platforms/qurt/{ => px4_layer}/main.cpp | 37 ++-- src/platforms/qurt/px4_layer/module.mk | 9 +- .../qurt/px4_layer/px4_qurt_impl.cpp | 49 +++-- .../qurt/px4_layer/px4_qurt_tasks.cpp | 174 ++++++++++++------ 10 files changed, 208 insertions(+), 118 deletions(-) rename src/platforms/qurt/{ => px4_layer}/main.cpp (85%) diff --git a/Tools/posix_apps.py b/Tools/posix_apps.py index 375e71fc2d..00e11fcbbb 100755 --- a/Tools/posix_apps.py +++ b/Tools/posix_apps.py @@ -71,7 +71,7 @@ static map app_map(void); static map app_map(void) { - map apps; + static map apps; """ for app in apps: print '\tapps["'+app+'"] = '+app+'_main;' diff --git a/Tools/qurt_apps.py b/Tools/qurt_apps.py index 540a564aa5..e5d75344be 100755 --- a/Tools/qurt_apps.py +++ b/Tools/qurt_apps.py @@ -61,27 +61,28 @@ for app in apps: print """ static int shutdown_main(int argc, char *argv[]); static int list_tasks_main(int argc, char *argv[]); +static int list_files_main(int argc, char *argv[]); +static int list_devices_main(int argc, char *argv[]); +static int list_topics_main(int argc, char *argv[]); } -static map app_map(void); - -static map app_map(void) +void init_app_map(map &apps) { - map apps; """ for app in apps: print '\tapps["'+app+'"] = '+app+'_main;' print '\tapps["shutdown"] = shutdown_main;' print '\tapps["list_tasks"] = list_tasks_main;' +print '\tapps["list_files"] = list_files_main;' +print '\tapps["list_devices"] = list_devices_main;' +print '\tapps["list_topics"] = list_topics_main;' + print """ - return apps; } -map apps = app_map(); - -static void list_builtins(void) +void list_builtins(map &apps) { printf("Builtin Commands:\\n"); for (map::iterator it=apps.begin(); it!=apps.end(); ++it) @@ -100,5 +101,21 @@ static int list_tasks_main(int argc, char *argv[]) return 0; } +static int list_devices_main(int argc, char *argv[]) +{ + px4_show_devices(); + return 0; +} + +static int list_topics_main(int argc, char *argv[]) +{ + px4_show_topics(); + return 0; +} +static int list_files_main(int argc, char *argv[]) +{ + px4_show_files(); + return 0; +} """ diff --git a/makefiles/qurt/config_qurt_default.mk b/makefiles/qurt/config_qurt_default.mk index 7fab719582..5e30743576 100644 --- a/makefiles/qurt/config_qurt_default.mk +++ b/makefiles/qurt/config_qurt_default.mk @@ -10,7 +10,7 @@ # # Board support modules # -#MODULES += drivers/device +MODULES += drivers/device #MODULES += drivers/blinkm #MODULES += drivers/hil #MODULES += drivers/led @@ -42,9 +42,9 @@ # # Library modules # -#MODULES += modules/systemlib +MODULES += modules/systemlib #MODULES += modules/systemlib/mixer -#MODULES += modules/uORB +MODULES += modules/uORB #MODULES += modules/dataman #MODULES += modules/sdlog2 #MODULES += modules/simulator diff --git a/makefiles/qurt_elf.mk b/makefiles/qurt_elf.mk index 0634013451..95a3fc315a 100644 --- a/makefiles/qurt_elf.mk +++ b/makefiles/qurt_elf.mk @@ -56,8 +56,14 @@ $(PRODUCT_SHARED_PRELINK): $(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS) $(GLOBAL_DEPS) $(PRODUCT_SHARED_LIB): $(PRODUCT_SHARED_PRELINK) $(call LINK_A,$@,$(PRODUCT_SHARED_PRELINK)) -$(WORK_DIR)mainapp: $(PRODUCT_SHARED_LIB) - $(call LINK,$@, $(PRODUCT_SHARED_LIB)) +$(WORK_DIR)apps.cpp: $(PX4_BASE)/Tools/qurt_apps.py + $(PX4_BASE)/Tools/qurt_apps.py > $@ + +$(WORK_DIR)apps.o: $(WORK_DIR)apps.cpp + $(call COMPILEXX,$<, $@) + +$(WORK_DIR)mainapp: $(WORK_DIR)apps.o $(PRODUCT_SHARED_LIB) + $(call LINK,$@, $^) # # Utility rules diff --git a/makefiles/toolchain_hexagon.mk b/makefiles/toolchain_hexagon.mk index 11564918c8..42144e9177 100644 --- a/makefiles/toolchain_hexagon.mk +++ b/makefiles/toolchain_hexagon.mk @@ -199,11 +199,6 @@ 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 - -#EXTRA_LIBS += $(PX4_BASE)../dspal_libs/libdspal.a - # Flags we pass to the assembler # AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \ diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 6fd8387326..7d6f602083 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -44,6 +44,7 @@ __BEGIN_DECLS extern void qurt_log(const char *fmt, ...); __END_DECLS +#define qurt_log(...) {printf(__VA_ARGS__); printf("\n");} #define PX4_DBG(...) qurt_log(__VA_ARGS__) #define PX4_DEBUG(...) qurt_log(__VA_ARGS__) #define PX4_INFO(...) qurt_log(__VA_ARGS__) diff --git a/src/platforms/qurt/main.cpp b/src/platforms/qurt/px4_layer/main.cpp similarity index 85% rename from src/platforms/qurt/main.cpp rename to src/platforms/qurt/px4_layer/main.cpp index ae90d13e51..6a4493a292 100644 --- a/src/platforms/qurt/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -37,16 +37,18 @@ * @author Mark Charlebois */ -#include +#include +#include +#include #include -//#include +#include +#include +#include -//using namespace std; +using namespace std; -//typedef int (*px4_main_t)(int argc, char *argv[]); - -#include "apps.h" -#include "px4_middleware.h" +extern void init_app_map(map &apps); +extern void list_builtins(map &apps); static const char *commands = "hello start" @@ -71,30 +73,24 @@ static const char *commands = #endif ; - -static void run_cmd(const vector &appargs) { +static void run_cmd(map &apps, const vector &appargs) { // command is appargs[0] string command = appargs[0]; - //printf("Looking for %s\n", command.c_str()); if (apps.find(command) != apps.end()) { const char *arg[2+1]; unsigned int i = 0; - //printf("size = %d\n", appargs.size()); while (i < appargs.size() && appargs[i].c_str()[0] != '\0') { arg[i] = (char *)appargs[i].c_str(); - //printf(" arg = '%s'\n", arg[i]); + printf(" arg = '%s'\n", arg[i]); ++i; } arg[i] = (char *)0; - //printf("BEFORE argc = %d %s %s %p\n", i, arg[0], arg[1], arg[2]); apps[command](i,(char **)arg); - //printf("AFTER argc = %d %s %s %p\n", i, arg[0], arg[1], arg[2]); } else { - //cout << "Invalid command" << endl; - list_builtins(); + list_builtins(apps); } } @@ -106,7 +102,7 @@ void eat_whitespace(const char *&b, int &i) i=0; } -static void process_commands(const char *cmds) +static void process_commands(map &apps, const char *cmds) { vector appargs; int i=0; @@ -126,7 +122,7 @@ static void process_commands(const char *cmds) // If we have a command to run if (appargs.size() > 0) { - run_cmd(appargs); + run_cmd(apps, appargs); } appargs.clear(); if (b[i] == '\n') { @@ -145,6 +141,7 @@ static void process_commands(const char *cmds) eat_whitespace(b, ++i); continue; } + printf("ch %c\n", b[i]); ++i; } } @@ -156,9 +153,11 @@ extern void init_once(void); int main(int argc, char **argv) { printf("In main\n"); + map apps; + init_app_map(apps); px4::init_once(); px4::init(argc, argv, "mainapp"); - process_commands(commands); + process_commands(apps, commands); for (;;) {} } diff --git a/src/platforms/qurt/px4_layer/module.mk b/src/platforms/qurt/px4_layer/module.mk index 5ee11aa2ad..92f1c6abfb 100644 --- a/src/platforms/qurt/px4_layer/module.mk +++ b/src/platforms/qurt/px4_layer/module.mk @@ -37,13 +37,6 @@ SRCDIR=$(dir $(MODULE_MK)) -apps.h: $(PX4_BASE)/Tools/qurt_apps.py - $(PX4_BASE)/Tools/qurt_apps.py > $@ - -# Force creation of apps.h -main_.cpp: $(SRCDIR)/../main.cpp apps.h - cp $(SRCDIR)/../main.cpp $@ - SRCS = \ px4_qurt_impl.cpp \ px4_qurt_tasks.cpp \ @@ -59,6 +52,6 @@ SRCS = \ sq_remfirst.c \ sq_addafter.c \ dq_rem.c \ - main_.cpp + main.cpp MAXOPTIMIZATION = -Os diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp index ad971c08be..3ff09b3d4f 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -38,6 +38,7 @@ */ #include +#include #include #include #include @@ -47,19 +48,28 @@ #include #include #include +#include #include "systemlib/param/param.h" -#include __BEGIN_DECLS // FIXME - sysconf(_SC_CLK_TCK) not supported long PX4_TICKS_PER_SEC = 1000; -void usleep(useconds_t usec) {} -unsigned int sleep(unsigned int sec) { return 0; } +void usleep(useconds_t usec) { + qurt_timer_sleep(usec); +} + +unsigned int sleep(unsigned int sec) +{ + for (unsigned int i=0; i< sec; i++) + qurt_timer_sleep(1000000); + return 0; +} extern void hrt_init(void); +#if 0 void qurt_log(const char *fmt, ...) { va_list args; @@ -67,11 +77,15 @@ void qurt_log(const char *fmt, ...) printf(fmt, args); printf("n"); } +#endif + +extern int _posix_init(void); __END_DECLS extern struct wqueue_s gwork[NWORKERS]; + namespace px4 { @@ -79,25 +93,11 @@ void init_once(void); void init_once(void) { + // Required for QuRT + _posix_init(); + work_queues_init(); hrt_init(); - - // 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); - } void init(int argc, char *argv[], const char *app_name) @@ -141,3 +141,12 @@ size_t strnlen(const char *s, size_t maxlen) return i; } +int ioctl(int a, int b, unsigned long c) +{ + return -1; +} + +int write(int a, char const* b, int c) +{ + return -1; +} diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index e5d2297e9a..95eea3191b 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -33,10 +33,11 @@ ****************************************************************************/ /** - * @file px4_linux_tasks.c + * @file px4_posix_tasks.c * Implementation of existing task API for Linux */ +#include #include #include #include @@ -53,20 +54,16 @@ #include #include -#include -#include #define MAX_CMD_LEN 100 -#define PX4_MAX_TASKS 5 - +#define PX4_MAX_TASKS 100 struct task_entry { - int pid; + pthread_t pid; std::string name; bool isused; task_entry() : isused(false) {} - void *sp; }; static task_entry taskmap[PX4_MAX_TASKS]; @@ -75,31 +72,32 @@ typedef struct { px4_main_t entry; int argc; - char * argv[]; - // strings are allocated after + char *argv[]; + // strings are allocated after the } pthdata_t; -static void entry_adapter ( void *ptr ) +static void *entry_adapter ( void *ptr ) { - pthdata_t *data = (pthdata_t *) ptr; - PX4_DEBUG("entry_adapter %p %p entry %p %d %p\n", ptr, data, data->entry, data->argc, data->argv[0]); + pthdata_t *data; + data = (pthdata_t *) ptr; - PX4_DEBUG("data->entry = %p\n", data->entry); + PX4_DBG("entry_adapter"); data->entry(data->argc, data->argv); free(ptr); - PX4_DEBUG("after entry\n"); - PX4_DEBUG("Before px4_task_exit\n"); + PX4_DBG("Before px4_task_exit"); px4_task_exit(0); - PX4_DEBUG("After px4_task_exit\n"); + PX4_DBG("After px4_task_exit"); + + return NULL; } void px4_systemreset(bool to_bootloader) { - PX4_DEBUG("Called px4_system_reset\n"); + PX4_WARN("Called px4_system_reset"); } -px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const *argv) +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; @@ -109,7 +107,11 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned long structsize; char * p = (char *)argv; - PX4_DEBUG("px4_task_spawn_cmd entry = %p %p %s\n", entry, argv, argv[0]); + printf("Creating %s\n", name); + pthread_t task; + pthread_attr_t attr; + struct sched_param param; + // Calculate argc while (p != (char *)0) { p = argv[argc]; @@ -118,24 +120,18 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int ++argc; len += strlen(p)+1; } - PX4_DEBUG("arg %d %p\n", argc, argv); structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *); pthdata_t *taskdata; - PX4_DEBUG("arg %d %p\n", argc, argv); // not safe to pass stack data to the thread creation taskdata = (pthdata_t *)malloc(structsize+len); - PX4_DEBUG("arg %d %p\n", argc, argv); offset = ((unsigned long)taskdata)+structsize; - PX4_DEBUG("arg %d %p\n", argc, argv); taskdata->entry = entry; taskdata->argc = argc; - PX4_DEBUG("arg %d %p\n", argc, argv); for (i=0; iargv[i] = (char *)offset; strcpy((char *)offset, argv[i]); offset+=strlen(argv[i])+1; @@ -143,38 +139,123 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int // Must add NULL at end of argv taskdata->argv[argc] = (char *)0; + rv = pthread_attr_init(&attr); + if (rv != 0) { + PX4_WARN("px4_task_spawn_cmd: failed to init thread attrs"); + return (rv < 0) ? rv : -rv; + } +#if 0 + rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); + if (rv != 0) { + PX4_WARN("px4_task_spawn_cmd: failed to set inherit sched"); + return (rv < 0) ? rv : -rv; + } + rv = pthread_attr_setschedpolicy(&attr, scheduler); + if (rv != 0) { + PX4_WARN("px4_task_spawn_cmd: failed to set sched policy"); + return (rv < 0) ? rv : -rv; + } +#endif + + param.sched_priority = priority; + + rv = pthread_attr_setschedparam(&attr, ¶m); + if (rv != 0) { + PX4_WARN("px4_task_spawn_cmd: failed to set sched param"); + return (rv < 0) ? rv : -rv; + } + + rv = pthread_create (&task, &attr, &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) { + PX4_ERR("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; ientry, taskdata->argc, taskdata->argv[0]); - thread_create(entry_adapter, taskmap[i].sp, i+1, (void *) taskdata); - - return i+1; + if (i>=PX4_MAX_TASKS) { + return -ENOSPC; + } + return i; } int px4_task_delete(px4_task_t id) { - PX4_DEBUG("Called px4_task_delete\n"); - return -EINVAL; + int rv = 0; + pthread_t pid; + PX4_WARN("Called px4_task_delete"); + + 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) { - thread_stop(); + int i; + pthread_t pid = pthread_self(); - // Free stack + // Get pthread ID from the opaque ID + for (i=0; i=PX4_MAX_TASKS) { + PX4_ERR("px4_task_exit: self task not found!"); + } + else { + PX4_DBG("px4_task_exit: %s", taskmap[i].name.c_str()); + } + + //pthread_exit((void *)(unsigned long)ret); } int px4_task_kill(px4_task_t id, int sig) { - PX4_DEBUG("Called px4_task_kill\n"); - return -EINVAL; + int rv = 0; + pthread_t pid; + PX4_DBG("Called px4_task_kill %d", sig); + + 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() @@ -182,26 +263,15 @@ void px4_show_tasks() int idx; int count = 0; - PX4_DEBUG("Active Tasks:\n"); + PX4_INFO("Active Tasks:"); for (idx=0; idx < PX4_MAX_TASKS; idx++) { if (taskmap[idx].isused) { - PX4_DEBUG(" %-10s %d\n", taskmap[idx].name.c_str(), taskmap[idx].pid); + PX4_INFO(" %-10s %u", taskmap[idx].name.c_str(), taskmap[idx].pid); count++; } } if (count == 0) - PX4_DEBUG(" No running tasks\n"); + PX4_INFO(" No running tasks"); } - -// STUBS - -extern "C" { -void hrt_sleep(unsigned long) -{ -} - -} -int ioctl(int d, int request, unsigned long foo) { return 0; } -int write(int a, char const*b, int c) { return c; }