Qurt tasks implementation (#20499)

platforms/qurt/CMakeLists.txt

@@ -38,4 +38,5 @@ QURT_LIB(LIB_NAME px4
 		${PX4_SOURCE_DIR}/platforms/qurt/unresolved_symbols.c
 	LINK_LIBS
 		${module_libraries}
+		px4_layer
 	)

platforms/qurt/src/px4/CMakeLists.txt

@@ -32,3 +32,10 @@
 ############################################################################
 
 # Placeholder
+set(QURT_LAYER_SRCS
+	tasks.cpp
+	)
+
+add_library(px4_layer
+        ${QURT_LAYER_SRCS}
+)

platforms/qurt/src/px4/tasks.cpp

@@ -0,0 +1,376 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2022 ModalAI, Inc. 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.
+ *
+ ****************************************************************************/
+
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/tasks.h>
+
+#include <pthread.h>
+
+#define PX4_TASK_STACK_SIZE 8192
+#define PX4_TASK_MAX_NAME_LENGTH 32
+#define PX4_TASK_MAX_ARGC 32
+#define PX4_TASK_MAX_ARGV_LENGTH 32
+#define PX4_MAX_TASKS 24
+
+typedef struct task_entry {
+	pthread_t tid;
+	char name[PX4_TASK_MAX_NAME_LENGTH + 4];
+	char stack[PX4_TASK_STACK_SIZE];
+	pthread_attr_t attr;
+	px4_main_t main_entry;
+	int argc;
+	char argv_storage[PX4_TASK_MAX_ARGC][PX4_TASK_MAX_ARGV_LENGTH];
+	char *argv[PX4_TASK_MAX_ARGC];
+	bool isused;
+
+	task_entry() : isused(false) {}
+} task_entry_t;
+
+static task_entry_t taskmap[PX4_MAX_TASKS];
+
+static bool task_mutex_initialized = false;
+static pthread_mutex_t task_mutex;
+
+static void *entry_adapter(void *ptr)
+{
+	task_entry_t *data;
+	data = (task_entry_t *) ptr;
+
+	if (data->main_entry) { data->main_entry(data->argc, data->argv); }
+
+	else { PX4_ERR("No valid task entry points"); }
+
+	pthread_exit(nullptr);
+	return nullptr;
+
+}
+
+static px4_task_t px4_task_spawn_internal(const char *name, int priority, px4_main_t main_entry, char *const argv[])
+{
+	int retcode = 0;
+	int i = 0;
+	int task_index = 0;
+	char *p = (char *)argv;
+
+	PX4_INFO("Creating pthread %s\n", name);
+
+	if (task_mutex_initialized == false) {
+		task_mutex_initialized = true;
+		pthread_mutex_init(&task_mutex, nullptr);
+	}
+
+	pthread_mutex_lock(&task_mutex);
+
+	for (task_index = 0; task_index < PX4_MAX_TASKS; task_index++) {
+		if (taskmap[task_index].isused == false) { break; }
+	}
+
+	if (task_index == PX4_MAX_TASKS) {
+		pthread_mutex_unlock(&task_mutex);
+		PX4_ERR("Hit maximum number of threads");
+		return -1;
+	}
+
+	taskmap[task_index].argc = 0;
+
+	while (p) {
+		taskmap[task_index].argc++;
+		p = argv[taskmap[task_index].argc];
+	}
+
+	if (taskmap[task_index].argc >= PX4_TASK_MAX_ARGC) {
+		pthread_mutex_unlock(&task_mutex);
+		PX4_ERR("Too many arguments for thread %d", taskmap[task_index].argc);
+		return -1;
+	}
+
+	for (i = 0; i < PX4_TASK_MAX_ARGC; i++) {
+		if (i < taskmap[task_index].argc) {
+			int argument_length = strlen(argv[i]);
+
+			if (argument_length >= PX4_TASK_MAX_ARGV_LENGTH) {
+				pthread_mutex_unlock(&task_mutex);
+				PX4_ERR("Argument %d is too long %d", i, argument_length);
+				return -1;
+
+			} else {
+				//px4_clock_gettimemap[task_index].argv_storage[i], argv[i]);
+				taskmap[task_index].argv[i] = taskmap[task_index].argv_storage[i];
+			}
+
+		} else {
+			taskmap[task_index].argv[i] = nullptr;
+			break;
+		}
+	}
+
+	taskmap[task_index].main_entry = main_entry;
+
+	if ((priority > 255) || (priority < 0)) {
+		pthread_mutex_unlock(&task_mutex);
+		PX4_ERR("Invalid priority %d", priority);
+		return -1;
+	}
+
+	priority = 255 - priority;
+
+	if (priority < 5) { priority = 5; }
+
+	if (priority > 250) { priority = 250; }
+
+	if (strlen(name) >= PX4_TASK_MAX_NAME_LENGTH) {
+		pthread_mutex_unlock(&task_mutex);
+		PX4_ERR("Task name is too long %s", name);
+		return -1;
+	}
+
+	strcpy(taskmap[task_index].name, "PX4_");
+	strcpy(&taskmap[task_index].name[4], name);
+
+	struct sched_param param;
+	param.sched_priority = priority;
+
+	pthread_attr_init(&taskmap[task_index].attr);
+	//pthread_attr_setthreadname(&taskmap[task_index].attr, taskmap[task_index].name);
+	//pthread_attr_setstackaddr(&taskmap[task_index].attr, taskmap[task_index].stack);
+	pthread_attr_setstacksize(&taskmap[task_index].attr, PX4_TASK_STACK_SIZE);
+	pthread_attr_setschedparam(&taskmap[task_index].attr, &param);
+
+	retcode = pthread_create(&taskmap[task_index].tid, &taskmap[task_index].attr, entry_adapter,
+				 (void *) &taskmap[task_index]);
+
+	if (retcode != PX4_OK) {
+		pthread_mutex_unlock(&task_mutex);
+		PX4_ERR("Couldn't create pthread %s", name);
+		return -1;
+
+	} else {
+		PX4_INFO("Successfully created px4 task %s with tid %u",
+			 taskmap[task_index].name,
+			 (unsigned int) taskmap[task_index].tid);
+	}
+
+	taskmap[task_index].isused = true;
+
+	pthread_mutex_unlock(&task_mutex);
+
+	return i;
+}
+
+px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry,
+			      char *const argv[])
+{
+	if (entry == nullptr) {
+		PX4_ERR("Entry function pointer is null");
+		return -1;
+	}
+
+	return px4_task_spawn_internal(name, priority, entry, argv);
+}
+
+int px4_task_delete(px4_task_t id)
+{
+	int rv = 0;
+
+	PX4_ERR("Ignoring px4_task_delete for task %d", id);
+
+	pthread_t pid;
+	PX4_WARN("Called px4_task_delete");
+
+	if (id < PX4_MAX_TASKS && taskmap[id].isused) {
+		pid = taskmap[id].tid;
+
+	} else {
+		return -EINVAL;
+	}
+
+	pthread_mutex_lock(&task_mutex);
+
+	if (pthread_self() == pid) {
+		pthread_join(pid, nullptr);
+		taskmap[id].isused = false;
+		pthread_mutex_unlock(&task_mutex);
+		pthread_exit(nullptr);
+
+	} else {
+		rv = pthread_cancel(pid);
+	}
+
+	taskmap[id].isused = false;
+
+	return rv;
+}
+
+void px4_task_exit(int ret)
+{
+	PX4_ERR("Ignoring px4_task_exit with return value %d", ret);
+
+	int i;
+	pthread_t pid = pthread_self();
+
+	for (i = 0; i < PX4_MAX_TASKS; ++i) {
+		if (taskmap[i].tid == pid) {
+			pthread_mutex_lock(&task_mutex);
+			taskmap[i].isused = false;
+			break;
+		}
+	}
+
+	if (i >= PX4_MAX_TASKS)  {
+		PX4_ERR("px4_task_exit: self task not found!");
+
+	} else {
+		PX4_DEBUG("px4_task_exit: %s", taskmap[i].name);
+	}
+
+	pthread_mutex_unlock(&task_mutex);
+
+	pthread_exit((void *)(unsigned long)ret);
+}
+
+int px4_task_kill(px4_task_t id, int sig)
+{
+	int rv = 0;
+	pthread_t pid;
+	PX4_DEBUG("Called px4_task_kill %d, taskname %s", sig, taskmap[id].name);
+
+	if (id < PX4_MAX_TASKS && taskmap[id].tid != 0) {
+		pthread_mutex_lock(&task_mutex);
+		pid = taskmap[id].tid;
+		pthread_mutex_unlock(&task_mutex);
+
+	} else {
+		return -EINVAL;
+	}
+
+	rv = pthread_kill(pid, sig);
+
+	return rv;
+}
+
+void px4_show_tasks()
+{
+	int idx = 0;
+	int count = 0;
+
+	PX4_INFO("Active Tasks:");
+
+	for (; idx < PX4_MAX_TASKS; idx++) {
+		if (taskmap[idx].isused) {
+			PX4_INFO("   %-10s %u", taskmap[idx].name,
+				 (unsigned int) taskmap[idx].tid);
+			count++;
+		}
+	}
+
+	if (count == 0) {
+		PX4_INFO("No running tasks");
+	}
+}
+
+px4_task_t px4_getpid()
+{
+	pthread_t pid = pthread_self();
+	px4_task_t ret = -1;
+
+	pthread_mutex_lock(&task_mutex);
+
+	for (int i = 0; i < PX4_MAX_TASKS; i++) {
+		if (taskmap[i].isused && taskmap[i].tid == pid) {
+			ret = i;
+		}
+	}
+
+	pthread_mutex_unlock(&task_mutex);
+	return ret;
+
+}
+
+
+const char *px4_get_taskname()
+{
+	pthread_t pid = pthread_self();
+	const char *prog_name = "UnknownApp";
+
+	pthread_mutex_lock(&task_mutex);
+
+	for (int i = 0; i < PX4_MAX_TASKS; i++) {
+		if (taskmap[i].isused && taskmap[i].tid == pid) {
+			prog_name = taskmap[i].name;
+		}
+	}
+
+	pthread_mutex_unlock(&task_mutex);
+
+	return prog_name;
+
+}
+
+// static void timer_cb(void *data)
+// {
+// 	px4_sem_t *sem = reinterpret_cast<px4_sem_t *>(data);
+
+// 	sem_post(sem);
+// }
+
+int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *ts)
+{
+	// work_s _hpwork = {};
+
+	// struct timespec ts_now;
+	// px4_clock_gettime(CLOCK_MONOTONIC, &ts_now);
+
+	// hrt_abstime timeout_us = ts_to_abstime((struct timespec *)ts) - ts_to_abstime(&ts_now);
+
+	// hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)sem, timeout_us);
+	// sem_wait(sem);
+	// hrt_work_cancel(&_hpwork);
+	return 0;
+}
+
+int px4_prctl(int option, const char *arg2, pthread_t pid)
+{
+	int rv = -1;
+	pthread_mutex_lock(&task_mutex);
+
+	for (int i = 0; i < PX4_MAX_TASKS; i++) {
+		if (taskmap[i].isused && taskmap[i].tid == pid) {
+			rv = pthread_attr_setthreadname(&taskmap[i].attr, arg2);
+			return rv;
+		}
+	}
+
+	return rv;
+}

src/modules/muorb/slpi/uORBProtobufChannel.cpp

@@ -30,14 +30,13 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
+
 #include "uORBProtobufChannel.hpp"
 #include "MUORBTest.hpp"
 #include <string>
 
-#include <qurt.h>
-#include <qurt_thread.h>
 #include <pthread.h>
-
+#include <px4_platform_common/tasks.h>
 #include <px4_platform_common/log.h>
 
 // Definition of test to run when in muorb test mode
@@ -45,11 +44,11 @@ static MUORBTestType test_to_run;
 
 fc_func_ptrs muorb_func_ptrs;
 
-static void *test_runner(void *test)
+static void *test_runner(void *)
 {
 	PX4_INFO("test_runner called");
 
-	switch (*((MUORBTestType *) test)) {
+	switch (test_to_run) {
 	case ADVERTISE_TEST_TYPE:
 		(void) muorb_func_ptrs.advertise_func_ptr(muorb_test_topic_name);
 		break;
@@ -93,13 +92,13 @@ char stack[TEST_STACK_SIZE];
 
 void run_test(MUORBTestType test)
 {
-	pthread_t tid;
-	pthread_attr_t attr;
-	pthread_attr_init(&attr);
-	pthread_attr_setstacksize(&attr, TEST_STACK_SIZE);
 	test_to_run = test;
-	pthread_create(&tid, &attr, &test_runner, (void *) &test_to_run);
+	(void)px4_task_spawn_cmd("test_MUORB",
-	pthread_attr_destroy(&attr);
+				 SCHED_DEFAULT,
+				 SCHED_PRIORITY_MAX - 2,
+				 2000,
+				 (px4_main_t)&test_runner,
+				 nullptr);
 }
 
 int px4muorb_topic_advertised(const char *topic_name)