Add an ioctl handler to launch built-in modules in kernel side

Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>

platforms/nuttx/src/px4/common/board_ctrl.c

@@ -42,6 +42,15 @@
 #include <px4_platform/board_ctrl.h>
 #include "board_config.h"
 
+#include <nuttx/lib/builtin.h>
+#include <NuttX/kernel_builtin/kernel_builtin_proto.h>
+
+FAR const struct builtin_s g_kernel_builtins[] = {
+#include <NuttX/kernel_builtin/kernel_builtin_list.h>
+};
+
+const int g_n_kernel_builtins = sizeof(g_kernel_builtins) / sizeof(struct builtin_s);
+
 static struct {
 	ioctl_ptr_t ioctl_func;
 } ioctl_ptrs[MAX_IOCTL_PTRS];
@@ -88,6 +97,39 @@ int board_ioctl(unsigned int cmd, uintptr_t arg)
 	return ret;
 }
 
+/************************************************************************************
+ * Name: launch_kernel_builtin
+ *
+ * Description:
+ *   launches a kernel-side build-in module
+ *
+ ************************************************************************************/
+
+int launch_kernel_builtin(unsigned int cmd, unsigned long arg)
+{
+	builtinioclaunch_t *kcmd = (builtinioclaunch_t *)arg;
+	int argc = kcmd->argc;
+	char **argv = kcmd->argv;
+	int i;
+	FAR const struct builtin_s *builtin = NULL;
+
+	for (i = 0; i < g_n_kernel_builtins; i++) {
+		if (!strcmp(g_kernel_builtins[i].name, argv[0])) {
+			builtin = &g_kernel_builtins[i];
+			break;
+		}
+	}
+
+	if (builtin) {
+		/* This is running in the userspace thread, created by nsh, and
+		   called via boardctl. Call the main directly */
+		kcmd->ret = builtin->main(argc, argv);
+		return OK;
+	}
+
+	return ENOENT;
+}
+
 /************************************************************************************
  * Name: kernel_ioctl_initialize
  *
@@ -101,4 +143,6 @@ void kernel_ioctl_initialize(void)
 	for (int i = 0; i < MAX_IOCTL_PTRS; i++) {
 		ioctl_ptrs[i].ioctl_func = NULL;
 	}
+
+	px4_register_boardct_ioctl(_BUILTINIOCBASE, launch_kernel_builtin);
 }

platforms/nuttx/src/px4/common/include/px4_platform/board_ctrl.h

@@ -50,6 +50,18 @@
 #define _BUILTINIOCBASE			IOCTL_IDX_TO_BASE(3)
 #define MAX_IOCTL_PTRS 4
 
+/* The BUILTINIOCLAUNCH IOCTL is used to launch kernel side modules
+ * from the user side code
+ */
+
+#define BUILTINIOCLAUNCH               (_PX4_IOC(_BUILTINIOCBASE, 1))
+
+typedef struct builtinioclaunch {
+	int argc;
+	char **argv;
+	int ret;
+} builtinioclaunch_t;
+
 typedef int (*ioctl_ptr_t)(unsigned int, unsigned long);
 
 __BEGIN_DECLS