From beabe56253fcb5408d592a009c45e90810e51b8f Mon Sep 17 00:00:00 2001
From: Peter van der Perk <peter.vanderperk@nxp.com>
Date: Fri, 23 Dec 2022 15:03:23 +0100
Subject: [PATCH] S32K3XX call functional reset on board_reset

---
 .../px4/nxp/s32k3xx/board_reset/board_reset.cpp   | 15 ++++++++++-----
 1 file changed, 10 insertions(+), 5 deletions(-)

diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/board_reset/board_reset.cpp b/platforms/nuttx/src/px4/nxp/s32k3xx/board_reset/board_reset.cpp
index 3de295f7de..0607e174c9 100644
--- a/platforms/nuttx/src/px4/nxp/s32k3xx/board_reset/board_reset.cpp
+++ b/platforms/nuttx/src/px4/nxp/s32k3xx/board_reset/board_reset.cpp
@@ -46,7 +46,7 @@
 
 #ifdef CONFIG_BOARDCTL_RESET
 
-static int board_reset_enter_bootloader()
+static int board_functional_reset()
 {
 	putreg32(MC_ME_MODE_CONF_FUNC_RST, S32K3XX_MC_ME_MODE_CONF);
 	putreg32(MC_ME_MODE_UPD, S32K3XX_MC_ME_MODE_UPD);
@@ -80,15 +80,20 @@ static int board_reset_enter_bootloader()
 
 int board_reset(int status)
 {
-	if (status == 1) {
-		board_reset_enter_bootloader();
-	}
+	board_functional_reset();
 
 #if defined(BOARD_HAS_ON_RESET)
 	board_on_reset(status);
 #endif
 
-	up_systemreset();
+	/* Ensure completion of memory accesses */
+
+	__asm volatile("dsb");
+
+	/* Wait for the reset */
+
+	for (; ;);
+
 	return 0;
 }