From 2784f527e5c003dddc02555f8db027f5027b6243 Mon Sep 17 00:00:00 2001
From: Peter van der Perk <peter.vanderperk@nxp.com>
Date: Tue, 2 Jan 2024 14:48:49 +0100
Subject: [PATCH] v6x-rt: Add reboot to isp support

---
 boards/px4/fmu-v6xrt/src/board_config.h                  | 2 ++
 .../nuttx/src/px4/nxp/imxrt/board_reset/CMakeLists.txt   | 2 ++
 .../nuttx/src/px4/nxp/imxrt/board_reset/board_reset.cpp  | 9 +++++++++
 3 files changed, 13 insertions(+)

diff --git a/boards/px4/fmu-v6xrt/src/board_config.h b/boards/px4/fmu-v6xrt/src/board_config.h
index 82b7e4b2e09e..788fb60c588c 100644
--- a/boards/px4/fmu-v6xrt/src/board_config.h
+++ b/boards/px4/fmu-v6xrt/src/board_config.h
@@ -539,6 +539,8 @@
 
 /* This board provides the board_on_reset interface */
 
+#define BOARD_HAS_ISP_BOOTLOADER 1
+
 #define BOARD_HAS_ON_RESET 1
 
 #define PX4_GPIO_INIT_LIST { \
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)