From 70872d94c80b7869ed1e7334ce4a745a7f38a9af Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Tue, 6 Oct 2020 15:43:27 +0300 Subject: [PATCH] Split px4_layer into kernel and userspace parts for nuttx protected build Split the px4_layer into user and kernel space libraries. Add some stubs for user-space (e.g. version) where an interface to the kernel needs to be added Use posix-versions for cpuload.cpp and print_load.cpp for userspace; these link to nuttx internals. This functinality could be built on top of posix (e.g. procfs) interfaces Signed-off-by: Jukka Laitinen --- platforms/nuttx/src/px4/common/CMakeLists.txt | 40 +++--- .../src/px4/common/console_buffer_usr.cpp | 65 ++++++++++ .../nuttx/src/px4/common/px4_layer.cmake | 22 ++++ .../src/px4/common/px4_protected_layers.cmake | 41 +++++++ .../nuttx/src/px4/common/usr_mcu_version.cpp | 114 ++++++++++++++++++ 5 files changed, 264 insertions(+), 18 deletions(-) create mode 100644 platforms/nuttx/src/px4/common/console_buffer_usr.cpp create mode 100644 platforms/nuttx/src/px4/common/px4_layer.cmake create mode 100644 platforms/nuttx/src/px4/common/px4_protected_layers.cmake create mode 100644 platforms/nuttx/src/px4/common/usr_mcu_version.cpp diff --git a/platforms/nuttx/src/px4/common/CMakeLists.txt b/platforms/nuttx/src/px4/common/CMakeLists.txt index 8a2d7ebdf1..fbccd77a0f 100644 --- a/platforms/nuttx/src/px4/common/CMakeLists.txt +++ b/platforms/nuttx/src/px4/common/CMakeLists.txt @@ -33,8 +33,8 @@ # skip for px4_layer support on an IO board if(NOT PX4_BOARD MATCHES "io-v2") - - add_library(px4_layer + # Kernel side & nuttx flat build common sources + set(KERNEL_SRCS board_crashdump.c board_dma_alloc.c board_fat_dma_alloc.c @@ -51,32 +51,36 @@ if(NOT PX4_BOARD MATCHES "io-v2") px4_24xxxx_mtd.c px4_crypto.cpp ) - target_link_libraries(px4_layer - PRIVATE - arch_board_reset - arch_board_critmon - arch_version - nuttx_apps - nuttx_sched - nuttx_mm - px4_work_queue - uORB + + # Kernel side & nuttx flat build common libraries + set(KERNEL_LIBS + arch_board_reset + arch_board_critmon + arch_version + nuttx_sched ) + +if (NOT DEFINED CONFIG_BUILD_FLAT AND "${PX4_PLATFORM}" MATCHES "nuttx") + # Build the NuttX user and kernel space px4 layers + include(${CMAKE_CURRENT_SOURCE_DIR}/px4_protected_layers.cmake) + +else() + # Build the flat build px4_layer + include(${CMAKE_CURRENT_SOURCE_DIR}/px4_layer.cmake) +endif() + else() + # Build the px4 layer for io_v2 add_library(px4_layer ${PX4_SOURCE_DIR}/platforms/common/empty.c) endif() + add_dependencies(px4_layer prebuild_targets) add_subdirectory(gpio) add_subdirectory(srgbled) +# Build px4_random if (DEFINED PX4_CRYPTO) add_library(px4_random nuttx_random.c) - add_dependencies(px4_random nuttx_context) target_link_libraries(px4_random PRIVATE nuttx_crypto) - - target_link_libraries(px4_layer - PUBLIC - crypto_backend - ) endif() diff --git a/platforms/nuttx/src/px4/common/console_buffer_usr.cpp b/platforms/nuttx/src/px4/common/console_buffer_usr.cpp new file mode 100644 index 0000000000..83023f934d --- /dev/null +++ b/platforms/nuttx/src/px4/common/console_buffer_usr.cpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 + +#ifdef BOARD_ENABLE_CONSOLE_BUFFER +#ifndef BOARD_CONSOLE_BUFFER_SIZE +# define BOARD_CONSOLE_BUFFER_SIZE (1024*4) // default buffer size +#endif + + +// TODO: User side implementation of px4_console_buffer + +int px4_console_buffer_init() +{ + return 0; +} + +int px4_console_buffer_size() +{ + return 0; +} + +int px4_console_buffer_read(char *buffer, int buffer_length, int *offset) +{ + return 0; +} + +#endif /* BOARD_ENABLE_CONSOLE_BUFFER */ diff --git a/platforms/nuttx/src/px4/common/px4_layer.cmake b/platforms/nuttx/src/px4/common/px4_layer.cmake new file mode 100644 index 0000000000..b138f05ddc --- /dev/null +++ b/platforms/nuttx/src/px4/common/px4_layer.cmake @@ -0,0 +1,22 @@ +# Build the px4 layer for nuttx flat build + +add_library(px4_layer + ${KERNEL_SRCS} + cdc_acm_check.cpp + ) + +target_link_libraries(px4_layer + PRIVATE + ${KERNEL_LIBS} + nuttx_c + nuttx_arch + nuttx_mm + ) + + +if (DEFINED PX4_CRYPTO) + target_link_libraries(px4_layer + PUBLIC + crypto_backend + ) +endif() diff --git a/platforms/nuttx/src/px4/common/px4_protected_layers.cmake b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake new file mode 100644 index 0000000000..91edc74073 --- /dev/null +++ b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake @@ -0,0 +1,41 @@ + +# Build the user side px4_layer + +add_library(px4_layer + tasks.cpp + console_buffer_usr.cpp + usr_mcu_version.cpp + 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 +) + +target_link_libraries(px4_layer + PRIVATE + m + nuttx_c + nuttx_xx + nuttx_mm +) + +# Build the kernel side px4_kernel_layer + +add_library(px4_kernel_layer + ${KERNEL_SRCS} +) + +target_link_libraries(px4_kernel_layer + PRIVATE + ${KERNEL_LIBS} + nuttx_kc + nuttx_karch + nuttx_kmm +) + +if (DEFINED PX4_CRYPTO) + target_link_libraries(px4_kernel_layer PUBLIC crypto_backend) +endif() + +target_compile_options(px4_kernel_layer PRIVATE -D__KERNEL__) + +add_dependencies(px4_kernel_layer prebuild_targets) diff --git a/platforms/nuttx/src/px4/common/usr_mcu_version.cpp b/platforms/nuttx/src/px4/common/usr_mcu_version.cpp new file mode 100644 index 0000000000..0928bb3a10 --- /dev/null +++ b/platforms/nuttx/src/px4/common/usr_mcu_version.cpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (C) 2020 Technology Innovation Institute. All rights reserved. + * Author: @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. + * + ****************************************************************************/ + +/** + * @file usr_mcu_version.c + * Implementation of generic user-space version API + */ + +#include +#include + +#include "board_config.h" + +static int hw_version = 0; +static int hw_revision = 0; +static char hw_info[] = HW_INFO_INIT; + +__EXPORT const char *board_get_hw_type_name(void) +{ + return (const char *) hw_info; +} + +__EXPORT int board_get_hw_version(void) +{ + return hw_version; +} + +__EXPORT int board_get_hw_revision() +{ + return hw_revision; +} + +__EXPORT void board_get_uuid32(uuid_uint32_t uuid_words) +{ + /* TODO: This is a stub for userspace build. Use some proper interface + * to fetch the uuid32 from the kernel + */ + uint32_t chip_uuid[PX4_CPU_UUID_WORD32_LENGTH]; + memset((uint8_t *)chip_uuid, 0, PX4_CPU_UUID_WORD32_LENGTH * 4); + + for (unsigned i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) { + uuid_words[i] = chip_uuid[i]; + } +} + +int board_mcu_version(char *rev, const char **revstr, const char **errata) +{ + /* TODO: This is a stub for userspace build. Use some proper interface + * to fetch the version from the kernel + */ + return -1; +} + +int board_get_px4_guid(px4_guid_t px4_guid) +{ + /* TODO: This is a stub for userspace build. Use some proper interface + * to fetch the guid from the kernel + */ + uint8_t *pb = (uint8_t *) &px4_guid[0]; + memset(pb, 0, PX4_GUID_BYTE_LENGTH); + + return PX4_GUID_BYTE_LENGTH; +} + +int board_get_px4_guid_formated(char *format_buffer, int size) +{ + px4_guid_t px4_guid; + board_get_px4_guid(px4_guid); + int offset = 0; + + /* size should be 2 per byte + 1 for termination + * So it needs to be odd + */ + size = size & 1 ? size : size - 1; + + /* Discard from MSD */ + for (unsigned i = PX4_GUID_BYTE_LENGTH - size / 2; offset < size && i < PX4_GUID_BYTE_LENGTH; i++) { + offset += snprintf(&format_buffer[offset], size - offset, "%02x", px4_guid[i]); + } + + return offset; +} +