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 <charlebm@gmail.com>

Tools/posix_apps.py

@@ -71,7 +71,7 @@ static map<string,px4_main_t> app_map(void);
 
 static map<string,px4_main_t> app_map(void)
 {
-	map<string,px4_main_t> apps;
+	static map<string,px4_main_t> apps;
 """
 for app in apps:
 	print '\tapps["'+app+'"] = '+app+'_main;'

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<string,px4_main_t> app_map(void);
+void init_app_map(map<string,px4_main_t> &apps)
-
-static map<string,px4_main_t> app_map(void)
 {
-	map<string,px4_main_t> 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<string,px4_main_t> apps = app_map();
+void list_builtins(map<string,px4_main_t> &apps)
-
-static void list_builtins(void)
 {
 	printf("Builtin Commands:\\n");
 	for (map<string,px4_main_t>::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;
+}
 """
 

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

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)
+$(WORK_DIR)apps.cpp: $(PX4_BASE)/Tools/qurt_apps.py
-	$(call LINK,$@, $(PRODUCT_SHARED_LIB))
+	$(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

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__ \

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__)

src/platforms/qurt/px4_layer/main.cpp

@@ -37,16 +37,18 @@
  * @author Mark Charlebois <charlebm@gmail.com>
  */
 
-#include <string>
+#include <px4_middleware.h>
+#include <px4_tasks.h>
+#include <px4_posix.h>
 #include <vector>
-//#include <hexagon_standalone.h>
+#include <string>
+#include <map>
+#include <stdio.h>
 
-//using namespace std;
+using namespace std;
 
-//typedef int (*px4_main_t)(int argc, char *argv[]);
+extern void init_app_map(map<string,px4_main_t> &apps);
-
+extern void list_builtins(map<string,px4_main_t> &apps);
-#include "apps.h"
-#include "px4_middleware.h"
 
 static const char *commands = 
 "hello start"
@@ -71,30 +73,24 @@ static const char *commands =
 #endif
 ;
 
-
+static void run_cmd(map<string,px4_main_t> &apps, const vector<string> &appargs) {
-static void run_cmd(const vector<string> &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(apps);
-		list_builtins();
 	}
 }
 
@@ -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<string,px4_main_t> &apps, const char *cmds)
 {
 	vector<string> 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<string,px4_main_t> apps;
+	init_app_map(apps);
 	px4::init_once();
 	px4::init(argc, argv, "mainapp");
-	process_commands(commands);
+	process_commands(apps, commands);
 	for (;;) {}
 }
 

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

src/platforms/qurt/px4_layer/px4_qurt_impl.cpp

@@ -38,6 +38,7 @@
  */
 
 #include <px4_defines.h>
+#include <px4_tasks.h>
 #include <px4_middleware.h>
 #include <px4_workqueue.h>
 #include <dataman/dataman.h>
@@ -47,19 +48,28 @@
 #include <errno.h>
 #include <unistd.h>
 #include <semaphore.h>
+#include <qurt.h>
 #include "systemlib/param/param.h"
-#include <hexagon_standalone.h>
 
 __BEGIN_DECLS
 
 // FIXME - sysconf(_SC_CLK_TCK) not supported
 long PX4_TICKS_PER_SEC = 1000;
 
-void usleep(useconds_t usec) {}
+void usleep(useconds_t usec) {
-unsigned int sleep(unsigned int sec) { return 0; }
+	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;
+}

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 <px4_log.h>
 #include <unistd.h>
 #include <stdio.h>
 #include <stdlib.h>
@@ -53,20 +54,16 @@
 #include <string>
 
 #include <px4_tasks.h>
-#include <px4_log.h>
-#include <hexagon_standalone.h>
 
 #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[];
+	char *argv[];
-	// strings are allocated after  
+	// 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;
+	pthdata_t *data;            
-	PX4_DEBUG("entry_adapter %p %p entry %p %d %p\n", ptr, data, data->entry, data->argc, data->argv[0]);
+	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_DBG("Before px4_task_exit");
-	PX4_DEBUG("Before px4_task_exit\n");
 	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; i<argc; i++) {
-		PX4_DEBUG("TEST\n");
+		printf("arg %d %s\n", i, argv[i]);
-		PX4_DEBUG("arg %d %s\n", i, argv[i]);
 		taskdata->argv[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, &param);
+	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; i<PX4_MAX_TASKS; ++i) {
 		if (taskmap[i].isused == false) {
-			taskmap[i].pid = i+1;
+			taskmap[i].pid = task;
 			taskmap[i].name = name;
 			taskmap[i].isused = true;
-			taskmap[i].sp = malloc(2048);
 			break;
 		}
 	}
-	PX4_DEBUG("taskdata %p entry %p %d %p\n", taskdata, taskdata->entry, taskdata->argc, taskdata->argv[0]);
+	if (i>=PX4_MAX_TASKS) {
-	thread_create(entry_adapter, taskmap[i].sp, i+1, (void *) taskdata);
+		return -ENOSPC;
-
+	}
-        return i+1;
+        return i;
 }
 
 int px4_task_delete(px4_task_t id)
 {
-	PX4_DEBUG("Called px4_task_delete\n");
+	int rv = 0;
-	return -EINVAL;
+	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; ++i) {
+		if (taskmap[i].pid == pid) {
+			taskmap[i].isused = false;
+			break;
+		}
+	}
+	if (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");
+	int rv = 0;
-	return -EINVAL;
+	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; }