Add a generic nuttx hrt driver userspace interface

This adds a nuttx userspace interface for hrt driver, communicating with
the actual hrt driver upper half via BOARDCTL IOCTLs

This is be used when running PX4 in NuttX protected or kernel builds

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

platforms/nuttx/src/px4/common/CMakeLists.txt

@@ -107,7 +107,6 @@ if (NOT DEFINED CONFIG_BUILD_FLAT AND "${PX4_PLATFORM}" MATCHES "nuttx")
 
 	add_library(px4_board_ctrl
 		board_ctrl.c
-		kernel_modules.c
 	)
 
 	add_dependencies(px4_board_ctrl nuttx_context nuttx_kernel_builtin_list_target)

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

@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020 Technology Innovation Institute. 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file board_ctrl.c
+ *
+ * Provide a kernel-userspace boardctl_ioctl interface
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/sem.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform/board_ctrl.h>
+#include "board_config.h"
+
+#include <stdint.h>
+#include <errno.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_orb_dev.h>
+
+struct {
+	unsigned base;
+	ioctl_ptr_t ioctl_func;
+} ioctl_ptrs[] = {
+	{0, NULL},
+	{0, NULL},
+	{0, NULL},
+	{0, NULL}
+};
+#define MAX_IOCTL_PTRS (sizeof(ioctl_ptrs)/sizeof(ioctl_ptrs[0]))
+
+int launch_builtin_module(int argc, char *argv[]);
+
+/* internal functions */
+
+int px4_register_boardct_ioctl(unsigned base, ioctl_ptr_t func)
+{
+	unsigned i;
+	int ret = PX4_ERROR;
+
+	for (i = 0; i < MAX_IOCTL_PTRS; i++) {
+		if (ioctl_ptrs[i].base == 0) {
+			ioctl_ptrs[i].base = base;
+			ioctl_ptrs[i].ioctl_func = func;
+			ret = PX4_OK;
+			break;
+		}
+	}
+
+	return ret;
+}
+
+/************************************************************************************
+ * Name: board_ioctl
+ *
+ * Description:
+ *   px4 platform layer kernel-userspace interfaces
+ *
+ ************************************************************************************/
+
+int
+board_ioctl(unsigned int cmd, uintptr_t arg)
+{
+	px4_boardctl_t *kcmd = (px4_boardctl_t *)arg;
+	unsigned i;
+
+	if (cmd == PX4_KERNEL_CMD) {
+		/* Launch module on kernel side */
+		kcmd->ret = launch_builtin_module(kcmd->argc, kcmd->argv);
+
+	} else {
+		/* Run some other registered ioctl */
+		for (i = 0; i < MAX_IOCTL_PTRS; i++) {
+			if ((cmd & 0xFF00) == ioctl_ptrs[i].base) {
+				return ioctl_ptrs[i].ioctl_func(cmd, arg);
+			}
+		}
+
+		return -EINVAL;
+	}
+
+	return OK;
+}

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

@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020 Technology Innovation Institute. All rights reserved.
+ *                 Author: Jukka Laitinen <jukkax@ssrc.tii.ae>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+#ifndef PX4_BOARD_CTRL_H_
+#define PX4_BOARD_CTRL_H_
+
+#define _PLATFORMIOCBASE		(0x4000)
+#define _PLATFORMIOC(_n)		(_PX4_IOC(_PLATFORMIOCBASE, _n))
+#define PX4_KERNEL_CMD			_PLATFORMIOC(1)
+
+typedef struct px4_boardctl {
+	int argc;
+	char **argv;
+	int ret;
+} px4_boardctl_t;
+
+typedef int (*ioctl_ptr_t)(unsigned int, unsigned long);
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Function to register a px4 boardctl handler */
+int px4_register_boardct_ioctl(unsigned base, ioctl_ptr_t func);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

platforms/nuttx/src/px4/common/usr_hrt.cpp

@@ -0,0 +1,255 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file usr_hrt.c
+ *
+ * Userspace High-resolution timer callouts and timekeeping.
+ *
+ * This can be used with nuttx userspace
+ *
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/posix.h>
+
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <sched.h>
+#include <errno.h>
+
+#include <assert.h>
+#include <debug.h>
+
+#include <board_config.h>
+#include <drivers/drv_hrt.h>
+#include <sys/boardctl.h>
+
+#ifdef CONFIG_DEBUG_HRT
+#  define hrtinfo _info
+#else
+#  define hrtinfo(x...)
+#endif
+
+
+/**
+ * Fetch a never-wrapping absolute time value in microseconds from
+ * some arbitrary epoch shortly after system start.
+ */
+hrt_abstime
+hrt_absolute_time(void)
+{
+	hrt_abstime abstime = 0;
+	boardctl(HRT_ABSOLUTE_TIME, (uintptr_t)&abstime);
+	return abstime;
+}
+
+/**
+ * Convert a timespec to absolute time
+ */
+hrt_abstime
+ts_to_abstime(const struct timespec *ts)
+{
+	hrt_abstime	result;
+
+	result = (hrt_abstime)(ts->tv_sec) * 1000000;
+	result += ts->tv_nsec / 1000;
+
+	return result;
+}
+
+/**
+ * Convert absolute time to a timespec.
+ */
+void
+abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
+{
+	ts->tv_sec = abstime / 1000000;
+	abstime -= ts->tv_sec * 1000000;
+	ts->tv_nsec = abstime * 1000;
+}
+
+/**
+ * Compare a time value with the current time as atomic operation
+ */
+hrt_abstime
+hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
+{
+	irqstate_t flags = px4_enter_critical_section();
+
+	hrt_abstime delta = hrt_absolute_time() - *then;
+
+	px4_leave_critical_section(flags);
+
+	return delta;
+}
+
+/**
+ * Store the absolute time in an interrupt-safe fashion
+ */
+void
+hrt_store_absolute_time(volatile hrt_abstime *t)
+{
+	irqstate_t flags = px4_enter_critical_section();
+	*t = hrt_absolute_time();
+	px4_leave_critical_section(flags);
+}
+
+/**
+ * Event dispatcher thread
+ */
+int
+event_thread(int argc, char *argv[])
+{
+	struct hrt_call *entry = NULL;
+
+	while (1) {
+		/* Wait for hrt tick */
+		boardctl(HRT_WAITEVENT, (uintptr_t)&entry);
+
+		/* HRT event received, dispatch */
+		if (entry) {
+			entry->usr_callout(entry->usr_arg);
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * Initialise the high-resolution timing module.
+ */
+void
+hrt_init(void)
+{
+	px4_task_spawn_cmd("usr_hrt", SCHED_DEFAULT, SCHED_PRIORITY_MAX, 1000, event_thread, NULL);
+}
+
+/**
+ * Call callout(arg) after interval has elapsed.
+ */
+void
+hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
+{
+	hrt_boardctl_t ioc_parm;
+	ioc_parm.entry = entry;
+	ioc_parm.time = delay;
+	ioc_parm.callout = callout;
+	ioc_parm.arg = arg;
+	entry->usr_callout = callout;
+	entry->usr_arg = arg;
+
+	boardctl(HRT_CALL_AFTER, (uintptr_t)&ioc_parm);
+}
+
+/**
+ * Call callout(arg) at calltime.
+ */
+void
+hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
+{
+	hrt_boardctl_t ioc_parm;
+	ioc_parm.entry = entry;
+	ioc_parm.time = calltime;
+	ioc_parm.interval = 0;
+	ioc_parm.callout = callout;
+	ioc_parm.arg = arg;
+	entry->usr_callout = callout;
+	entry->usr_arg = arg;
+
+	boardctl(HRT_CALL_AT, (uintptr_t)&ioc_parm);
+}
+
+/**
+ * Call callout(arg) every period.
+ */
+void
+hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	hrt_boardctl_t ioc_parm;
+	ioc_parm.entry = entry;
+	ioc_parm.time = delay;
+	ioc_parm.interval = interval;
+	ioc_parm.callout = callout;
+	ioc_parm.arg = arg;
+	entry->usr_callout = callout;
+	entry->usr_arg = arg;
+
+	boardctl(HRT_CALL_EVERY, (uintptr_t)&ioc_parm);
+}
+
+/**
+ * Remove the entry from the callout list.
+ */
+void
+hrt_cancel(struct hrt_call *entry)
+{
+	boardctl(HRT_CANCEL, (uintptr_t)entry);
+}
+
+void
+hrt_call_init(struct hrt_call *entry)
+{
+	memset(entry, 0, sizeof(*entry));
+}
+
+/**
+ * If this returns true, the call has been invoked and removed from the callout list.
+ *
+ * Always returns false for repeating callouts.
+ */
+bool
+hrt_called(struct hrt_call *entry)
+{
+	return (entry->deadline == 0);
+}
+
+latency_info_t
+get_latency(uint16_t bucket_idx, uint16_t counter_idx)
+{
+	latency_boardctl_t latency_ioc;
+	latency_ioc.bucket_idx = bucket_idx;
+	latency_ioc.counter_idx = counter_idx;
+	latency_ioc.latency = {0, 0};
+	boardctl(HRT_GET_LATENCY, (uintptr_t)&latency_ioc);
+	return latency_ioc.latency;
+}
+
+void reset_latency_counters()
+{
+	boardctl(HRT_RESET_LATENCY, NULL);
+}

platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c

@@ -62,7 +62,6 @@
 #include <board_config.h>
 #include <drivers/drv_hrt.h>
 
-
 #include "stm32_gpio.h"
 #include "stm32_tim.h"
 
@@ -72,6 +71,23 @@
 #  define hrtinfo(x...)
 #endif
 
+#ifdef __PX4_NUTTX
+#include <px4_platform_common/defines.h>
+#include <px4_platform/board_ctrl.h>
+#include <px4_platform_common/sem.h>
+
+static px4_sem_t g_wait_sem;
+static struct hrt_call *next_hrt_entry;
+
+void hrt_usr_call(void *arg)
+{
+	// These calls will be made one by one by hrt, there is no race in here
+	next_hrt_entry = (struct hrt_call *)arg;
+	px4_sem_post(&g_wait_sem);
+}
+
+#endif
+
 #ifdef HRT_TIMER
 
 /* HRT configuration */
@@ -275,6 +291,8 @@ static void		hrt_call_enter(struct hrt_call *entry);
 static void		hrt_call_reschedule(void);
 static void		hrt_call_invoke(void);
 
+
+int hrt_ioctl(unsigned int cmd, unsigned long arg);
 /*
  * Specific registers and bits used by PPM sub-functions
  */
@@ -758,6 +776,16 @@ hrt_init(void)
 	/* configure the PPM input pin */
 	px4_arch_configgpio(GPIO_PPM_IN);
 #endif
+
+#if !defined(CONFIG_BUILD_FLAT)
+	/* Create a semaphore for handling hrt driver callbacks */
+	px4_sem_init(&g_wait_sem, 0, 0);
+	/* this is a signalling semaphore */
+	px4_sem_setprotocol(&g_wait_sem, SEM_PRIO_NONE);
+
+	/* register ioctl callbacks */
+	px4_register_boardct_ioctl(_HRTIOCBASE, hrt_ioctl);
+#endif
 }
 
 /**
@@ -1018,6 +1046,64 @@ void reset_latency_counters(void)
 		latency_counters[i] = 0;
 	}
 }
+
+/* board_ioctl interface for user-space hrt driver */
+int
+hrt_ioctl(unsigned int cmd, unsigned long arg)
+{
+	hrt_boardctl_t *h = (hrt_boardctl_t *)arg;
+
+	switch (cmd) {
+	case HRT_WAITEVENT:
+		/* The user side thread calling this is at highest priority, there
+		 * is no race in here
+		 */
+		px4_sem_wait(&g_wait_sem);
+		*(struct hrt_call **)arg = next_hrt_entry;
+		break;
+
+	case HRT_ABSOLUTE_TIME:
+		*(hrt_abstime *)arg = hrt_absolute_time();
+		break;
+
+	case HRT_CALL_AFTER:
+		hrt_call_after(h->entry, h->time, (hrt_callout)hrt_usr_call, h->entry);
+		break;
+
+	case HRT_CALL_AT:
+		hrt_call_at(h->entry, h->time, (hrt_callout)hrt_usr_call, h->entry);
+		break;
+
+	case HRT_CALL_EVERY:
+		hrt_call_every(h->entry, h->time, h->interval, (hrt_callout)hrt_usr_call, h->entry);
+		break;
+
+	case HRT_CANCEL:
+		if (h && h->entry) {
+			hrt_cancel(h->entry);
+
+		} else {
+			PX4_ERR("HRT_CANCEL called with NULL entry");
+		}
+
+		break;
+
+	case HRT_GET_LATENCY: {
+			latency_boardctl_t *latency = (latency_boardctl_t *)arg;
+			latency->latency = get_latency(latency->bucket_idx, latency->counter_idx);
+		}
+		break;
+
+	case HRT_RESET_LATENCY:
+		reset_latency_counters();
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return OK;
+}
 #endif
 
 #endif /* HRT_TIMER */

src/drivers/drv_hrt.h

@@ -76,6 +76,10 @@ typedef struct hrt_call {
 	hrt_abstime		period;
 	hrt_callout		callout;
 	void			*arg;
+#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT)
+	hrt_callout		usr_callout;
+	void			*usr_arg;
+#endif
 } *hrt_call_t;
 
 
@@ -84,6 +88,38 @@ extern const uint16_t latency_bucket_count;
 extern const uint16_t latency_buckets[LATENCY_BUCKET_COUNT];
 extern uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1];
 
+
+typedef struct hrt_boardctl {
+	hrt_call_t		entry;
+	hrt_abstime		time; /* delay or calltime */
+	hrt_abstime		interval;
+	hrt_callout		callout;
+	void			*arg;
+} hrt_boardctl_t;
+
+typedef struct latency_info {
+	uint16_t		bucket;
+	uint32_t		counter;
+} latency_info_t;
+
+typedef struct latency_boardctl {
+	uint16_t		bucket_idx;
+	uint16_t		counter_idx;
+	latency_info_t	latency;
+} latency_boardctl_t;
+
+#define _HRTIOCBASE		(0x1000)
+#define _HRTIOC(_n)		(_PX4_IOC(_HRTIOCBASE, _n))
+
+#define HRT_WAITEVENT		_HRTIOC(1)
+#define HRT_ABSOLUTE_TIME	_HRTIOC(2)
+#define HRT_CALL_AFTER		_HRTIOC(3)
+#define HRT_CALL_AT		_HRTIOC(4)
+#define HRT_CALL_EVERY		_HRTIOC(5)
+#define HRT_CANCEL		_HRTIOC(6)
+#define HRT_GET_LATENCY		_HRTIOC(7)
+#define HRT_RESET_LATENCY	_HRTIOC(8)
+
 /**
  * Get absolute time in [us] (does not wrap).
  */