1 files changed, 75 insertions, 0 deletions
diff --git a/arduino/libraries/Bluefruit52Lib/src/utility/bootloader_util.c b/arduino/libraries/Bluefruit52Lib/src/utility/bootloader_util.c
new file mode 100755
index 0000000..470666c
--- /dev/null
+++ b/arduino/libraries/Bluefruit52Lib/src/utility/bootloader_util.c
@@ -0,0 +1,75 @@
+/* Copyright (c) 2013 Nordic Semiconductor. All Rights Reserved.
+ *
+ * The information contained herein is property of Nordic Semiconductor ASA.
+ * Terms and conditions of usage are described in detail in NORDIC
+ * SEMICONDUCTOR STANDARD SOFTWARE LICENSE AGREEMENT.
+ *
+ * Licensees are granted free, non-transferable use of the information. NO
+ * WARRANTY of ANY KIND is provided. This heading must NOT be removed from
+ * the file.
+ *
+ */
+
+#include <stdint.h>
+#include <string.h>
+
+
+/**
+ * @brief Function for aborting current application/bootloader jump to to other app/bootloader.
+ *
+ * @details This functions will use the address provide to swap the stack pointer and then load 
+ *          the address of the reset handler to be executed. It will check current system mode 
+ *          (thread/handler) and if in thread mode it will reset into other application.
+ *          If in handler mode \ref isr_abort will be executed to ensure correct exit of handler 
+ *          mode and jump into reset handler of other application.
+ *
+ * @param[in]  start_addr  Start address of other application. This address must point to the 
+               initial stack pointer of the application.
+ *
+ * @note This function will never return but issue a reset into provided application.
+ */
+static inline void bootloader_util_reset (uint32_t start_addr) __attribute__ ((optimize("-fomit-frame-pointer")));
+static inline void bootloader_util_reset(uint32_t start_addr)
+{
+    __asm volatile(
+        "ldr   r0, [%0]\t\n"            // Get App initial MSP for bootloader.
+        "msr   msp, r0\t\n"             // Set the main stack pointer to the applications MSP.
+        "ldr   r0, [%0, #0x04]\t\n"     // Load Reset handler into R0.
+
+        "movs  r4, #0xFF\t\n"           // Move ones to R4.
+        "sxtb  r4, r4\t\n"              // Sign extend R4 to obtain 0xFFFFFFFF instead of 0xFF.
+
+        "mrs   r5, IPSR\t\n"            // Load IPSR to R5 to check for handler or thread mode.
+        "cmp   r5, #0x00\t\n"           // Compare, if 0 then we are in thread mode and can continue to reset handler of bootloader.
+        "bne   isr_abort\t\n"           // If not zero we need to exit current ISR and jump to reset handler of bootloader.
+
+        "mov   lr, r4\t\n"              // Clear the link register and set to ones to ensure no return.
+        "bx    r0\t\n"                  // Branch to reset handler of bootloader.
+
+        "isr_abort:  \t\n"
+
+        "mov   r5, r4\t\n"              // Fill with ones before jumping to reset handling. Will be popped as LR when exiting ISR. Ensures no return to application.
+        "mov   r6, r0\t\n"              // Move address of reset handler to R6. Will be popped as PC when exiting ISR. Ensures the reset handler will be executed when exist ISR.
+        "movs  r7, #0x21\t\n"           // Move MSB reset value of xPSR to R7. Will be popped as xPSR when exiting ISR. xPSR is 0x21000000 thus MSB is 0x21.
+        "rev   r7, r7\t\n"              // Reverse byte order to put 0x21 as MSB.
+        "push  {r4-r7}\t\n"             // Push everything to new stack to allow interrupt handler to fetch it on exiting the ISR.
+
+        "movs  r4, #0x00\t\n"           // Fill with zeros before jumping to reset handling. We be popped as R0 when exiting ISR (Cleaning up of the registers).
+        "movs  r5, #0x00\t\n"           // Fill with zeros before jumping to reset handling. We be popped as R1 when exiting ISR (Cleaning up of the registers).
+        "movs  r6, #0x00\t\n"           // Fill with zeros before jumping to reset handling. We be popped as R2 when exiting ISR (Cleaning up of the registers).
+        "movs  r7, #0x00\t\n"           // Fill with zeros before jumping to reset handling. We be popped as R3 when exiting ISR (Cleaning up of the registers).
+        "push  {r4-r7}\t\n"             // Push zeros (R4-R7) to stack to prepare for exiting the interrupt routine.
+
+        "movs  r0, #0xF9\t\n"           // Move the execution return command into register, 0xFFFFFFF9.
+        "sxtb  r0, r0\t\n"              // Sign extend R0 to obtain 0xFFFFFFF9 instead of 0xF9.
+        "bx    r0\t\n"                  // No return - Handler mode will be exited. Stack will be popped and execution will continue in reset handler initializing other application.
+        ".align\t\n"
+        :: "r" (start_addr)             // Argument list for the gcc assembly. start_addr is %0.
+        :  "r0", "r4", "r5", "r6", "r7" // List of register maintained manually.
+    );
+}
+
+void bootloader_util_app_start(uint32_t start_addr)
+{
+    bootloader_util_reset(start_addr);
+}