From b7b2e6be3e3356f1935eb8994ae830d43ad2a40e Mon Sep 17 00:00:00 2001
From: Peter van der Perk <peter.vanderperk@nxp.com>
Date: Tue, 2 Jan 2024 10:16:15 +0100
Subject: [PATCH] v6x-rt: Add reboot into isp bootloader mode

---
 .../nuttx/src/px4/nxp/imxrt/board_reset/CMakeLists.txt   | 2 ++
 .../nuttx/src/px4/nxp/imxrt/board_reset/board_reset.cpp  | 9 +++++++++
 src/systemcmds/reboot/reboot.cpp                         | 7 ++++++-
 3 files changed, 17 insertions(+), 1 deletion(-)

diff --git a/platforms/nuttx/src/px4/nxp/imxrt/board_reset/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/imxrt/board_reset/CMakeLists.txt
index 06ccd082037c..cba22cb32025 100644
--- a/platforms/nuttx/src/px4/nxp/imxrt/board_reset/CMakeLists.txt
+++ b/platforms/nuttx/src/px4/nxp/imxrt/board_reset/CMakeLists.txt
@@ -35,6 +35,8 @@ px4_add_library(arch_board_reset
 	board_reset.cpp
 )
 
+target_link_libraries(arch_board_reset PRIVATE arch_board_romapi)
+
 # up_systemreset
 if (NOT DEFINED CONFIG_BUILD_FLAT)
 	target_link_libraries(arch_board_reset PRIVATE nuttx_karch)
diff --git a/platforms/nuttx/src/px4/nxp/imxrt/board_reset/board_reset.cpp b/platforms/nuttx/src/px4/nxp/imxrt/board_reset/board_reset.cpp
index aa54650834f0..54dbda48e60b 100644
--- a/platforms/nuttx/src/px4/nxp/imxrt/board_reset/board_reset.cpp
+++ b/platforms/nuttx/src/px4/nxp/imxrt/board_reset/board_reset.cpp
@@ -43,6 +43,10 @@
 #include <arm_internal.h>
 #include <hardware/rt117x/imxrt117x_snvs.h>
 
+
+#include <px4_arch/imxrt_flexspi_nor_flash.h>
+#include <px4_arch/imxrt_romapi.h>
+
 #define BOOT_RTC_SIGNATURE                0xb007b007
 #define PX4_IMXRT_RTC_REBOOT_REG          3
 #define PX4_IMXRT_RTC_REBOOT_REG_ADDRESS  IMXRT_SNVS_LPGPR3
@@ -63,6 +67,11 @@ int board_reset(int status)
 {
 	if (status == 1) {
 		board_reset_enter_bootloader();
+
+	} else if (status == 2) {
+		uint32_t arg = 0xeb100000;
+		ROM_API_Init();
+		ROM_RunBootloader(&arg);
 	}
 
 #if defined(BOARD_HAS_ON_RESET)
diff --git a/src/systemcmds/reboot/reboot.cpp b/src/systemcmds/reboot/reboot.cpp
index 8d0b3419e81b..3ece3a3d2135 100644
--- a/src/systemcmds/reboot/reboot.cpp
+++ b/src/systemcmds/reboot/reboot.cpp
@@ -51,6 +51,7 @@ static void print_usage()
 
 	PRINT_MODULE_USAGE_NAME_SIMPLE("reboot", "command");
 	PRINT_MODULE_USAGE_PARAM_FLAG('b', "Reboot into bootloader", true);
+	PRINT_MODULE_USAGE_PARAM_FLAG('i', "Reboot into ISP (1st stage bootloader)", true);
 
 	PRINT_MODULE_USAGE_ARG("lock|unlock", "Take/release the shutdown lock (for testing)", true);
 }
@@ -63,12 +64,16 @@ extern "C" __EXPORT int reboot_main(int argc, char *argv[])
 	int myoptind = 1;
 	const char *myoptarg = nullptr;
 
-	while ((ch = px4_getopt(argc, argv, "b", &myoptind, &myoptarg)) != -1) {
+	while ((ch = px4_getopt(argc, argv, "bi", &myoptind, &myoptarg)) != -1) {
 		switch (ch) {
 		case 'b':
 			to_bootloader = true;
 			break;
 
+		case 'i':
+			board_reset(2);
+			break;
+
 		default:
 			print_usage();
 			return 1;