px4_reboot_request supports continue_boot option

tiiuae · Nov 6, 2023 · 4865d4e · 4865d4e
1 parent 8ee56b0
commit 4865d4e
Show file tree

Hide file tree

Showing 3 changed files with 35 additions and 3 deletions.
diff --git 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
@@ -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
@@ -40,6 +40,7 @@
 #include <px4_platform_common/px4_config.h>
 #include <errno.h>
 #include <nuttx/board.h>
+#include <hardware/mpfs_wdog.h>
 
 #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 */