From 77af102cab14d4a6627f7357ca48c9dd3a7e7637 Mon Sep 17 00:00:00 2001
From: Jukka Laitinen <jukkax@ssrc.tii.ae>
Date: Mon, 25 Jan 2021 09:16:23 +0200
Subject: [PATCH] px4_work_queue: Use px4_task_spawn_cmd for WorkQueueRunner in
 NuttX protected

In NuttX protected build there are separate work queues in kernel and user sides.

pthreads are only available in user side, so use tasks and kthreads for
memory protected builds.

Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
---
 .../px4_work_queue/WorkQueueManager.cpp       | 71 +++++++++++++++----
 1 file changed, 56 insertions(+), 15 deletions(-)

diff --git a/platforms/common/px4_work_queue/WorkQueueManager.cpp b/platforms/common/px4_work_queue/WorkQueueManager.cpp
index b4efae40af..9263cbd926 100644
--- a/platforms/common/px4_work_queue/WorkQueueManager.cpp
+++ b/platforms/common/px4_work_queue/WorkQueueManager.cpp
@@ -235,6 +235,17 @@ WorkQueueRunner(void *context)
 	return nullptr;
 }
 
+#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT)
+// Wrapper for px4_task_spawn_cmd interface
+inline static int
+WorkQueueRunner(int argc, char *argv[])
+{
+	wq_config_t *context = (wq_config_t *)strtoul(argv[argc - 1], nullptr, 16);
+	WorkQueueRunner(context);
+	return 0;
+}
+#endif
+
 static int
 WorkQueueManagerRun(int, char **)
 {
@@ -248,20 +259,6 @@ WorkQueueManagerRun(int, char **)
 		if (wq != nullptr) {
 			// create new work queue
 
-			pthread_attr_t attr;
-			int ret_attr_init = pthread_attr_init(&attr);
-
-			if (ret_attr_init != 0) {
-				PX4_ERR("attr init for %s failed (%i)", wq->name, ret_attr_init);
-			}
-
-			sched_param param;
-			int ret_getschedparam = pthread_attr_getschedparam(&attr, &param);
-
-			if (ret_getschedparam != 0) {
-				PX4_ERR("getting sched param for %s failed (%i)", wq->name, ret_getschedparam);
-			}
-
 			// stack size
 #if defined(__PX4_QURT)
 			const size_t stacksize = math::max(8 * 1024, PX4_STACK_ADJUSTED(wq->stacksize));
@@ -274,12 +271,32 @@ WorkQueueManagerRun(int, char **)
 			const size_t stacksize_adj = math::max(PTHREAD_STACK_MIN, PX4_STACK_ADJUSTED(wq->stacksize));
 			const size_t stacksize = (stacksize_adj + page_size - (stacksize_adj % page_size));
 #endif
+
+			// priority
+			int sched_priority = sched_get_priority_max(SCHED_FIFO) + wq->relative_priority;
+
+			// use pthreads for NuttX flat and posix builds. For NuttX protected build, use tasks or kernel threads
+#if !defined(__PX4_NUTTX) || defined(CONFIG_BUILD_FLAT)
+			pthread_attr_t attr;
+			int ret_attr_init = pthread_attr_init(&attr);
+
 			int ret_setstacksize = pthread_attr_setstacksize(&attr, stacksize);
 
 			if (ret_setstacksize != 0) {
 				PX4_ERR("setting stack size for %s failed (%i)", wq->name, ret_setstacksize);
 			}
 
+			if (ret_attr_init != 0) {
+				PX4_ERR("attr init for %s failed (%i)", wq->name, ret_attr_init);
+			}
+
+			sched_param param;
+			int ret_getschedparam = pthread_attr_getschedparam(&attr, &param);
+
+			if (ret_getschedparam != 0) {
+				PX4_ERR("getting sched param for %s failed (%i)", wq->name, ret_getschedparam);
+			}
+
 #ifndef __PX4_QURT
 
 			// schedule policy FIFO
@@ -292,7 +309,7 @@ WorkQueueManagerRun(int, char **)
 #endif // ! QuRT
 
 			// priority
-			param.sched_priority = sched_get_priority_max(SCHED_FIFO) + wq->relative_priority;
+			param.sched_priority = sched_priority;
 			int ret_setschedparam = pthread_attr_setschedparam(&attr, &param);
 
 			if (ret_setschedparam != 0) {
@@ -316,6 +333,30 @@ WorkQueueManagerRun(int, char **)
 			if (ret_destroy != 0) {
 				PX4_ERR("failed to destroy thread attributes for %s (%i)", wq->name, ret_create);
 			}
+
+#else
+			// create thread
+
+			// pack wq struct pointer into string, this is compatible with px4_task_spawn_cmd
+			char arg1[sizeof(void *) * 3];
+			sprintf(arg1, "%lx", (long unsigned)wq);
+			const char *arg[2] = {arg1, nullptr};
+
+			int pid = px4_task_spawn_cmd(wq->name,
+						     SCHED_FIFO,
+						     sched_priority,
+						     stacksize,
+						     WorkQueueRunner,
+						     (char *const *)arg);
+
+			if (pid > 0) {
+				PX4_DEBUG("starting: %s, priority: %d, stack: %zu bytes", wq->name, sched_priority, stacksize);
+
+			} else {
+				PX4_ERR("failed to create thread for %s (%i): %s", wq->name, pid, strerror(pid));
+			}
+
+#endif
 		}
 	}