From 8675effabc7bf656fbc9ba742efcf05823d28e9d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?L=C3=A1szl=C3=B3=20Monda?= <laci@monda.hu>
Date: Mon, 1 May 2017 17:02:18 +0200
Subject: [PATCH] Add wormhole.[ch]

---
 right/build/kds/.cproject        |  2 +-
 right/src/main.c                 | 52 +++++++++++---------------------
 right/src/usb_protocol_handler.c |  2 ++
 right/src/wormhole.c             | 17 +++++++++++
 right/src/wormhole.h             | 24 +++++++++++++++
 5 files changed, 61 insertions(+), 36 deletions(-)
 create mode 100644 right/src/wormhole.c
 create mode 100644 right/src/wormhole.h

diff --git a/right/build/kds/.cproject b/right/build/kds/.cproject
index e15dc10..68d4c88 100644
--- a/right/build/kds/.cproject
+++ b/right/build/kds/.cproject
@@ -112,7 +112,7 @@
 								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.c.linker.cref.742351963" name="Cross reference (-Xlinker --cref)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.c.linker.cref" value="false" valueType="boolean"/>
 								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.c.linker.printgcsections.192927503" name="Print removed sections (-Xlinker --print-gc-sections)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.c.linker.printgcsections" value="false" valueType="boolean"/>
 								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.c.linker.strip.1437487702" name="Omit all symbol information (-s)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.c.linker.strip" value="false" valueType="boolean"/>
-								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.c.linker.other.237939626" name="Other linker flags" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.c.linker.other" value=" -Wall  --specs=nano.specs          -fno-common  -ffunction-sections  -fdata-sections  -ffreestanding  -fno-builtin  -mapcs  -Xlinker -static  -Xlinker -z  -Xlinker muldefs " valueType="string"/>
+								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.c.linker.other.237939626" name="Other linker flags" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.c.linker.other" value=" -Wall  --specs=nano.specs          -fno-common  -ffunction-sections  -fdata-sections  -ffreestanding  -fno-builtin  -mapcs  -Xlinker -static  -Xlinker -z  -Xlinker muldefs" valueType="string"/>
 								<inputType id="ilg.gnuarmeclipse.managedbuild.cross.tool.c.linker.input.1959593877" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.c.linker.input">
 									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
 									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
diff --git a/right/src/main.c b/right/src/main.c
index 934cef3..5a52314 100644
--- a/right/src/main.c
+++ b/right/src/main.c
@@ -13,6 +13,7 @@
 #include "bootloader_config.h"
 #include "command.h"
 #include "test_states.h"
+#include "wormhole.h"
 
 key_matrix_t KeyMatrix = {
     .colNum = KEYBOARD_MATRIX_COLS_NUM,
@@ -90,44 +91,25 @@ void UpdateUsbReports()
     IsUsbBasicKeyboardReportSent = false;
 }
 
-typedef struct {
-  uint32_t magicNumber;
-  uint8_t enumerationMode;
-} wormhole_t;
-
-wormhole_t *message_ptr __attribute__ ((__section__ (".noinit")));
-
-void* getSP(void)
-{
-    void *sp;
-    __asm__ __volatile__ ("mov %0, sp" : "=r"(sp));
-    return sp;
-}
-
-void __attribute__ ((__section__ (".init3"), __naked__)) move_sp(void) {
-    void* SP = getSP();
-    SP -= sizeof(wormhole_t);
-    message_ptr = (wormhole_t*)(SP + 1);
-}
-
 void main() {
     InitPeripherials();
     InitClock();
-#ifdef ENABLE_BUSPAL
-    init_hardware();
-    handleUsbBusPalCommand();
-#else
-    LedDriver_InitAllLeds(1);
-    InitBridgeProtocolScheduler();
-    KeyMatrix_Init(&KeyMatrix);
-    UpdateUsbReports();
-    InitUsb();
-
-    // deserialize_Layer(testData, 0);
-
-    while (1) {
+    if (false/* || Wormhole->magicNumber == WORMHOLE_MAGIC_NUMBER*/) {
+        Wormhole->magicNumber = 0;
+        init_hardware();
+        handleUsbBusPalCommand();
+    } else {
+        LedDriver_InitAllLeds(1);
+        InitBridgeProtocolScheduler();
+        KeyMatrix_Init(&KeyMatrix);
         UpdateUsbReports();
-        asm("wfi");
+        InitUsb();
+
+        // deserialize_Layer(testData, 0);
+
+        while (1) {
+            UpdateUsbReports();
+            asm("wfi");
+        }
     }
-#endif
 }
diff --git a/right/src/usb_protocol_handler.c b/right/src/usb_protocol_handler.c
index c36aaa7..10a7e86 100644
--- a/right/src/usb_protocol_handler.c
+++ b/right/src/usb_protocol_handler.c
@@ -9,6 +9,7 @@
 #include "led_pwm.h"
 #include "bridge_protocol_scheduler.h"
 #include "bridge_slaves/bridge_slave_uhk_module.h"
+#include "wormhole.h"
 
 void setError(uint8_t error);
 void setGenericError();
@@ -109,6 +110,7 @@ void getSystemProperty() {
 }
 
 void jumpToBootloader() {
+//    Wormhole->magicNumber = WORMHOLE_MAGIC_NUMBER;
     SCB->AIRCR = 0x5FA<<SCB_AIRCR_VECTKEY_Pos | SCB_AIRCR_SYSRESETREQ_Msk; // Reset the MCU.
     for (;;);
 }
diff --git a/right/src/wormhole.c b/right/src/wormhole.c
new file mode 100644
index 0000000..0191b85
--- /dev/null
+++ b/right/src/wormhole.c
@@ -0,0 +1,17 @@
+#include "wormhole.h"
+
+wormhole_t *Wormhole NO_INIT_GCC;
+
+void* getSP(void)
+{
+    void *sp;
+//    __asm__ __volatile__ ("mov %0, sp" : "=r"(sp));
+    __asm__ __volatile__ ("mrs %0, msp" : "=r"(sp));
+    return sp;
+}
+
+void __attribute__ ((__section__ (".init3"), __naked__)) move_sp(void) {
+    void* SP = getSP();
+//    SP -= sizeof(wormhole_t);
+    Wormhole = (wormhole_t*)(SP + 1);
+}
diff --git a/right/src/wormhole.h b/right/src/wormhole.h
new file mode 100644
index 0000000..be481ae
--- /dev/null
+++ b/right/src/wormhole.h
@@ -0,0 +1,24 @@
+#ifndef __WORMHOLE_H__
+#define __WORMHOLE_H__
+
+// Includes:
+
+    #include <stdint.h>
+
+// Macros:
+
+    #define WORMHOLE_MAGIC_NUMBER 0x3b04cd9e94521f9a
+    #define NO_INIT_GCC __attribute__ ((section (".noinit")))
+
+// Typedefs:
+
+    typedef struct {
+      uint64_t magicNumber;
+      uint8_t enumerationMode;
+    } wormhole_t;
+
+// Variables:
+
+    extern wormhole_t *Wormhole NO_INIT_GCC;
+
+#endif