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). */