From 77f71e61d2b848f934cdf2d9639b0d773046d6a4 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Fri, 9 Oct 2020 14:14:21 +0300 Subject: [PATCH] Add a generic hrt driver userspace interface This adds a nuttx userspace interface for hrt driver, communicating with the actual px4 hrt driver via BOARDCTL IOCTLs This is be used when running PX4 in NuttX protected or kernel builds Signed-off-by: Jukka Laitinen --- .../src/px4/common/px4_protected_layers.cmake | 1 + platforms/nuttx/src/px4/common/usr_hrt.cpp | 221 ++++++++++++++++++ .../nuttx/src/px4/stm/stm32_common/hrt/hrt.c | 114 +++++++++ src/drivers/drv_hrt.h | 37 +++ 4 files changed, 373 insertions(+) create mode 100644 platforms/nuttx/src/px4/common/usr_hrt.cpp diff --git a/platforms/nuttx/src/px4/common/px4_protected_layers.cmake b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake index 95599eba82..898fe3aeac 100644 --- a/platforms/nuttx/src/px4/common/px4_protected_layers.cmake +++ b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake @@ -8,6 +8,7 @@ add_library(px4_layer cdc_acm_check.cpp ${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/print_load.cpp ${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/cpuload.cpp + usr_hrt.cpp ) target_link_libraries(px4_layer 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..d57712a32d --- /dev/null +++ b/platforms/nuttx/src/px4/common/usr_hrt.cpp @@ -0,0 +1,221 @@ +/**************************************************************************** + * + * 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 +#include + +static px4_task_t g_usr_hrt_task = -1; + +/** + * 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; +} + +/** + * 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; +} + +/** + * Request stop. + */ +bool hrt_request_stop() +{ + px4_task_delete(g_usr_hrt_task); + return true; +} + +/** + * Initialise the high-resolution timing module. + */ +void +hrt_init(void) +{ + px4_register_shutdown_hook(hrt_request_stop); + g_usr_hrt_task = 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 ce63a41c2b..025e31a003 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c @@ -72,6 +72,33 @@ # define hrtinfo(x...) #endif +#if !defined(CONFIG_BUILD_FLAT) +#include +#include +#include + +#define HRT_ENTRY_QUEUE_MAX_SIZE 3 +static px4_sem_t g_wait_sem; +static struct hrt_call *next_hrt_entry[HRT_ENTRY_QUEUE_MAX_SIZE]; +static int hrt_entry_queued = 0; +static bool suppress_entry_queue_error = false; +static bool hrt_entry_queue_error = false; + +void hrt_usr_call(void *arg) +{ + // This is called from hrt interrupt + if (hrt_entry_queued < HRT_ENTRY_QUEUE_MAX_SIZE) { + next_hrt_entry[hrt_entry_queued++] = (struct hrt_call *)arg; + + } else { + hrt_entry_queue_error = true; + } + + px4_sem_post(&g_wait_sem); +} + +#endif + #ifdef HRT_TIMER /* HRT configuration */ @@ -275,6 +302,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 */ @@ -718,6 +747,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 } /** @@ -978,6 +1017,81 @@ 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: { + irqstate_t flags; + px4_sem_wait(&g_wait_sem); + /* Atomically update the pointer to user side hrt entry */ + flags = px4_enter_critical_section(); + + /* This should be always true, but check it anyway */ + if (hrt_entry_queued > 0) { + *(struct hrt_call **)arg = next_hrt_entry[--hrt_entry_queued]; + next_hrt_entry[hrt_entry_queued] = NULL; + + } else { + hrt_entry_queue_error = true; + } + + px4_leave_critical_section(flags); + + /* Warn once for entry queue being full */ + if (hrt_entry_queue_error && !suppress_entry_queue_error) { + PX4_ERR("HRT entry error, queue size now %d", hrt_entry_queued); + suppress_entry_queue_error = true; + } + } + 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 c4d3c12d32..fb1cc0722f 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -47,6 +47,10 @@ #include #include +#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) +#include +#endif + __BEGIN_DECLS /** @@ -76,6 +80,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; @@ -89,6 +97,35 @@ typedef struct latency_info { uint32_t counter; } latency_info_t; +#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) + +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_boardctl { + uint16_t bucket_idx; + uint16_t counter_idx; + latency_info_t latency; +} latency_boardctl_t; + +#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) + +#endif + /** * Get absolute time in [us] (does not wrap). */