From 5c86a0748183f82af457164b8d3d799ea157823d Mon Sep 17 00:00:00 2001
From: Romain Goyet <romain.goyet@numworks.com>
Date: Wed, 4 Apr 2018 10:20:36 +0200
Subject: [PATCH] [ion] Add the ability to perform a jump-reset

---
 ion/include/ion.h         |  2 +-
 ion/src/device/Makefile   |  1 +
 ion/src/device/device.cpp | 21 ++++++++++++++++++++-
 ion/src/device/set_msp.h  |  8 ++++++++
 ion/src/device/set_msp.s  | 10 ++++++++++
 5 files changed, 40 insertions(+), 2 deletions(-)
 create mode 100644 ion/src/device/set_msp.h
 create mode 100644 ion/src/device/set_msp.s

diff --git a/ion/include/ion.h b/ion/include/ion.h
index ac38e587d..806e0508b 100644
--- a/ion/include/ion.h
+++ b/ion/include/ion.h
@@ -34,7 +34,7 @@ const char * patchLevel();
 const char * fccId();
 
 /* CAUTION: This is a complete reset! */
-void reset();
+void reset(bool jump = false);
 
 // CRC32 : non xor-ed, non reversed, direct, polynomial 4C11DB7
 // Only accepts whole 32bit values
diff --git a/ion/src/device/Makefile b/ion/src/device/Makefile
index 210b33e54..f22832edc 100644
--- a/ion/src/device/Makefile
+++ b/ion/src/device/Makefile
@@ -23,6 +23,7 @@ objs += $(addprefix ion/src/device/, \
   led.o\
   power.o\
   sd_card.o\
+  set_msp.o \
   swd.o \
   usb.o \
   wakeup.o \
diff --git a/ion/src/device/device.cpp b/ion/src/device/device.cpp
index 1786289c1..e98d0bc8f 100644
--- a/ion/src/device/device.cpp
+++ b/ion/src/device/device.cpp
@@ -14,6 +14,7 @@ extern "C" {
 #include "swd.h"
 #include "usb.h"
 #include "bench/bench.h"
+#include "set_msp.h"
 
 #define USE_SD_CARD 0
 
@@ -65,10 +66,28 @@ uint32_t Ion::random() {
   return result;
 }
 
-void Ion::reset() {
+static void coreReset() {
+  // Perform a full core reset
   CM4.AIRCR()->requestReset();
 }
 
+static void jumpReset() {
+  Ion::Device::shutdown();
+  uint32_t * stackPointerAddress = reinterpret_cast<uint32_t *>(0x08000000);
+  uint32_t * resetHandlerAddress = reinterpret_cast<uint32_t *>(0x08000004);
+  set_msp(*stackPointerAddress);
+  void (*ResetHandler)(void) = (void (*)())(*resetHandlerAddress);
+  ResetHandler();
+}
+
+void Ion::reset(bool jump) {
+  if (jump) {
+    jumpReset();
+  } else {
+    coreReset();
+  }
+}
+
 static inline char hex(uint8_t d) {
   if (d > 9) {
     return 'A'+d-10;
diff --git a/ion/src/device/set_msp.h b/ion/src/device/set_msp.h
new file mode 100644
index 000000000..f1441d3d8
--- /dev/null
+++ b/ion/src/device/set_msp.h
@@ -0,0 +1,8 @@
+#ifndef ION_DEVICE_SETMSP_H
+#define ION_DEVICE_SETMSP_H
+
+#include <stdint.h>
+
+extern "C" void set_msp(uint32_t address);
+
+#endif
diff --git a/ion/src/device/set_msp.s b/ion/src/device/set_msp.s
new file mode 100644
index 000000000..ba568cd04
--- /dev/null
+++ b/ion/src/device/set_msp.s
@@ -0,0 +1,10 @@
+.syntax unified
+
+.section .text
+.align 2
+.thumb
+.global set_msp
+set_msp:
+  msr msp, r0
+  bx lr
+.type set_msp, function