From 3d5b90f5832974a23e176fa490024bb5688bff79 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?L=C3=A1szl=C3=B3=20Monda?= <laci@monda.hu>
Date: Fri, 21 Apr 2017 09:50:20 +0200
Subject: [PATCH] Add wormhole_t, getSP() and tweak SP before touched by the C
 runtime.

---
 right/src/main.c | 20 ++++++++++++++++++++
 1 file changed, 20 insertions(+)

diff --git a/right/src/main.c b/right/src/main.c
index 71cc9e9..22e6daf 100644
--- a/right/src/main.c
+++ b/right/src/main.c
@@ -86,6 +86,26 @@ 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();