From 8497dbb2b584c2fbeb30b22ccf2c08dcd91fd6ac Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Fri, 9 Oct 2020 14:14:21 +0300 Subject: [PATCH] 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 --- platforms/nuttx/src/px4/common/CMakeLists.txt | 1 - platforms/nuttx/src/px4/common/board_ctrl.c | 114 ++++++++ .../common/include/px4_platform/board_ctrl.h | 61 +++++ platforms/nuttx/src/px4/common/usr_hrt.cpp | 255 ++++++++++++++++++ .../nuttx/src/px4/stm/stm32_common/hrt/hrt.c | 88 +++++- src/drivers/drv_hrt.h | 36 +++ 6 files changed, 553 insertions(+), 2 deletions(-) create mode 100644 platforms/nuttx/src/px4/common/board_ctrl.c create mode 100644 platforms/nuttx/src/px4/common/include/px4_platform/board_ctrl.h create mode 100644 platforms/nuttx/src/px4/common/usr_hrt.cpp diff --git a/platforms/nuttx/src/px4/common/CMakeLists.txt b/platforms/nuttx/src/px4/common/CMakeLists.txt index ca5de70b72..45cb4afba6 100644 --- a/platforms/nuttx/src/px4/common/CMakeLists.txt +++ b/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) diff --git a/platforms/nuttx/src/px4/common/board_ctrl.c b/platforms/nuttx/src/px4/common/board_ctrl.c new file mode 100644 index 0000000000..48d7565e08 --- /dev/null +++ b/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 +#include +#include +#include +#include "board_config.h" + +#include +#include + +#include +#include + +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; +} diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/board_ctrl.h b/platforms/nuttx/src/px4/common/include/px4_platform/board_ctrl.h new file mode 100644 index 0000000000..2e5cd1a449 --- /dev/null +++ b/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 + * + * 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 diff --git a/platforms/nuttx/src/px4/common/usr_hrt.cpp b/platforms/nuttx/src/px4/common/usr_hrt.cpp new file mode 100644 index 0000000000..b820dc79d4 --- /dev/null +++ b/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 +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#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); +} diff --git a/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c b/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c index 92e503fb97..9927682b92 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c @@ -62,7 +62,6 @@ #include #include - #include "stm32_gpio.h" #include "stm32_tim.h" @@ -72,6 +71,23 @@ # define hrtinfo(x...) #endif +#ifdef __PX4_NUTTX +#include +#include +#include + +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 */ diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index 6def3f85ee..b102d2c253 100644 --- a/src/drivers/drv_hrt.h +++ b/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). */