From 4865d4e168e9a4c4c6e875fbf62281d842e599f2 Mon Sep 17 00:00:00 2001 From: Jari Nippula Date: Mon, 6 Nov 2023 09:56:52 +0200 Subject: [PATCH] px4_reboot_request supports continue_boot option --- .../include/px4_platform_common/shutdown.h | 2 +- platforms/common/shutdown.cpp | 21 +++++++++++++++++-- .../mpfs/board_reset/board_reset.cpp | 15 +++++++++++++ 3 files changed, 35 insertions(+), 3 deletions(-) diff --git a/platforms/common/include/px4_platform_common/shutdown.h b/platforms/common/include/px4_platform_common/shutdown.h index f4bd2e9cf6b3..6f26e5d900ae 100644 --- a/platforms/common/include/px4_platform_common/shutdown.h +++ b/platforms/common/include/px4_platform_common/shutdown.h @@ -83,7 +83,7 @@ __EXPORT int px4_unregister_shutdown_hook(shutdown_hook_t hook); * @return 0 on success, <0 on error */ #if defined(CONFIG_BOARDCTL_RESET) -__EXPORT int px4_reboot_request(bool to_bootloader = false, uint32_t delay_us = 0); +__EXPORT int px4_reboot_request(bool to_bootloader = false, uint32_t delay_us = 0, bool continue_boot = false); #endif // CONFIG_BOARDCTL_RESET diff --git a/platforms/common/shutdown.cpp b/platforms/common/shutdown.cpp index fafd9b32db4a..5c0be97a009c 100644 --- a/platforms/common/shutdown.cpp +++ b/platforms/common/shutdown.cpp @@ -108,6 +108,8 @@ static uint16_t shutdown_counter = 0; ///< count how many times the shutdown wor #define SHUTDOWN_ARG_IN_PROGRESS (1<<0) #define SHUTDOWN_ARG_REBOOT (1<<1) #define SHUTDOWN_ARG_TO_BOOTLOADER (1<<2) +#define SHUTDOWN_ARG_BL_CONTINUE_BOOT (1<<3) + static uint8_t shutdown_args = 0; static constexpr int max_shutdown_hooks = 1; @@ -175,7 +177,18 @@ static void shutdown_worker(void *arg) if (shutdown_args & SHUTDOWN_ARG_REBOOT) { #if defined(CONFIG_BOARDCTL_RESET) PX4_INFO_RAW("Reboot NOW."); - boardctl(BOARDIOC_RESET, (shutdown_args & SHUTDOWN_ARG_TO_BOOTLOADER) ? 1 : 0); + uintptr_t reboot_arg = 0; + + if (shutdown_args & SHUTDOWN_ARG_TO_BOOTLOADER) { + if (shutdown_args & SHUTDOWN_ARG_BL_CONTINUE_BOOT) { + reboot_arg = 2; + + } else { + reboot_arg = 1; + } + } + + boardctl(BOARDIOC_RESET, reboot_arg); #else PX4_PANIC("board reset not available"); #endif @@ -206,7 +219,7 @@ static void shutdown_worker(void *arg) } #if defined(CONFIG_BOARDCTL_RESET) -int px4_reboot_request(bool to_bootloader, uint32_t delay_us) +int px4_reboot_request(bool to_bootloader, uint32_t delay_us, bool continue_boot) { pthread_mutex_lock(&shutdown_mutex); @@ -224,6 +237,10 @@ int px4_reboot_request(bool to_bootloader, uint32_t delay_us) if (to_bootloader) { shutdown_args |= SHUTDOWN_ARG_TO_BOOTLOADER; + + if (continue_boot) { + shutdown_args |= SHUTDOWN_ARG_BL_CONTINUE_BOOT; + } } shutdown_time_us = hrt_absolute_time(); diff --git a/platforms/nuttx/src/px4/microchip/mpfs/board_reset/board_reset.cpp b/platforms/nuttx/src/px4/microchip/mpfs/board_reset/board_reset.cpp index f83c259195f6..a532977f9556 100644 --- a/platforms/nuttx/src/px4/microchip/mpfs/board_reset/board_reset.cpp +++ b/platforms/nuttx/src/px4/microchip/mpfs/board_reset/board_reset.cpp @@ -40,6 +40,7 @@ #include #include #include +#include #include "riscv_internal.h" @@ -52,6 +53,17 @@ static void board_reset_enter_bootloader() up_systemreset(); } +static void board_reset_enter_bootloader_and_continue_boot() +{ + /* Reset by triggering WDOG */ + + putreg32(WDOG_FORCE_IMMEDIATE_RESET, MPFS_WDOG0_LO_BASE + MPFS_WDOG_FORCE_OFFSET); + + /* Wait for the reset */ + + for (; ;); +} + static void board_reset_enter_app(uintptr_t hartid) { register long r0 asm("a0") = (long)(hartid); @@ -71,6 +83,9 @@ int board_reset(int status) if (status == 1) { board_reset_enter_bootloader(); + + } else if (status == 2) { + board_reset_enter_bootloader_and_continue_boot(); } /* Just reboot via reset vector */