From 3b9936edd2644e75b9d58585459e9d11d2f197f1 Mon Sep 17 00:00:00 2001 From: Ville Juven Date: Wed, 29 Jun 2022 09:54:24 +0300 Subject: [PATCH] hrt_ioctl: Move common code under /common folder The HRT ioctl is common code, so move it out of chip specific code --- platforms/nuttx/src/px4/common/hrt_ioctl.c | 172 ++++++++++++++++++ platforms/nuttx/src/px4/common/px4_init.cpp | 4 + .../src/px4/common/px4_protected_layers.cmake | 2 + .../nuttx/src/px4/stm/stm32_common/hrt/hrt.c | 129 ------------- src/drivers/drv_hrt.h | 5 + 5 files changed, 183 insertions(+), 129 deletions(-) create mode 100644 platforms/nuttx/src/px4/common/hrt_ioctl.c diff --git a/platforms/nuttx/src/px4/common/hrt_ioctl.c b/platforms/nuttx/src/px4/common/hrt_ioctl.c new file mode 100644 index 0000000000..e9b6e82efd --- /dev/null +++ b/platforms/nuttx/src/px4/common/hrt_ioctl.c @@ -0,0 +1,172 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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. + * + ****************************************************************************/ + +#include + +#include + +#include +#include +#include +#include +#include + +#include + +#ifndef MODULE_NAME +# define MODULE_NAME "hrt_ioctl" +#endif + +#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); +} + +int hrt_ioctl(unsigned int cmd, unsigned long arg); + +void hrt_ioctl_init(void) +{ + /* 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); +} + +/* These functions are inlined in all but NuttX protected/kernel builds */ + +latency_info_t get_latency(uint16_t bucket_idx, uint16_t counter_idx) +{ + latency_info_t ret = {latency_buckets[bucket_idx], latency_counters[counter_idx]}; + return ret; +} + +void reset_latency_counters(void) +{ + for (int i = 0; i <= get_latency_bucket_count(); i++) { + 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; +} diff --git a/platforms/nuttx/src/px4/common/px4_init.cpp b/platforms/nuttx/src/px4/common/px4_init.cpp index 08e2841869..599f071955 100644 --- a/platforms/nuttx/src/px4/common/px4_init.cpp +++ b/platforms/nuttx/src/px4/common/px4_init.cpp @@ -122,6 +122,10 @@ int px4_platform_init() hrt_init(); +#if !defined(CONFIG_BUILD_FLAT) + hrt_ioctl_init(); +#endif + param_init(); /* configure CPU load estimation */ diff --git a/platforms/nuttx/src/px4/common/px4_protected_layers.cmake b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake index 7dbdce767d..7f76502e68 100644 --- a/platforms/nuttx/src/px4/common/px4_protected_layers.cmake +++ b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake @@ -29,6 +29,7 @@ target_link_libraries(px4_layer add_library(px4_board_ctrl board_ctrl.c board_ioctl.c + hrt_ioctl.c ) add_dependencies(px4_board_ctrl nuttx_context px4_kernel_builtin_list_target) @@ -65,3 +66,4 @@ endif() add_dependencies(px4_kernel_layer prebuild_targets) target_compile_options(px4_kernel_layer PRIVATE -D__KERNEL__) +target_link_libraries(px4_kernel_layer PUBLIC px4_board_ctrl) 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 025e31a003..f31c240fed 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c @@ -72,33 +72,6 @@ # 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 */ @@ -747,16 +720,6 @@ 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 } /** @@ -1002,96 +965,4 @@ hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) entry->deadline = hrt_absolute_time() + delay; } -#if !defined(CONFIG_BUILD_FLAT) -/* These functions are inlined in all but NuttX protected/kernel builds */ - -latency_info_t get_latency(uint16_t bucket_idx, uint16_t counter_idx) -{ - latency_info_t ret = {latency_buckets[bucket_idx], latency_counters[counter_idx]}; - return ret; -} - -void reset_latency_counters(void) -{ - for (int i = 0; i <= get_latency_bucket_count(); i++) { - 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 242d2a2ee8..c0fa973251 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -235,6 +235,11 @@ __EXPORT extern void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay); */ __EXPORT extern void hrt_init(void); +/* + * Initialise the HRT ioctl (user mode access to HRT). + */ +__EXPORT extern void hrt_ioctl_init(void); + #if defined(ENABLE_LOCKSTEP_SCHEDULER) __EXPORT extern int px4_lockstep_register_component(void);