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>

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