From f25bddce355a94a11e03c911bce29755583a91d4 Mon Sep 17 00:00:00 2001
From: Alex Voinea <voinea.dragos.alexandru@gmail.com>
Date: Wed, 2 Feb 2022 21:06:58 +0100
Subject: [PATCH] Simplify GETPC()

---
 Firmware/Dcodes.cpp      |  2 +-
 Firmware/asm.h           | 21 +++++++++------------
 Firmware/xflash_dump.cpp |  2 +-
 3 files changed, 11 insertions(+), 14 deletions(-)

diff --git a/Firmware/Dcodes.cpp b/Firmware/Dcodes.cpp
index d99a2c89..4a51b4d7 100644
--- a/Firmware/Dcodes.cpp
+++ b/Firmware/Dcodes.cpp
@@ -990,7 +990,7 @@ void __attribute__((noinline)) serial_dump_and_reset(dump_crash_reason reason)
 
     // sample SP/PC
     sp = SP;
-    GETPC(&pc);
+    pc = GETPC();
 
     // extend WDT long enough to allow writing the entire stream
     wdt_enable(WDTO_8S);
diff --git a/Firmware/asm.h b/Firmware/asm.h
index 2ffb88ad..0c3d7457 100644
--- a/Firmware/asm.h
+++ b/Firmware/asm.h
@@ -1,24 +1,21 @@
 #pragma once
 #include <stdint.h>
+#include "macros.h"
 
 #ifdef __AVR_ATmega2560__
 
 // return the current PC (on AVRs with 22bit PC)
-static inline void GETPC(uint32_t* v)
+FORCE_INLINE __uint24 GETPC(void)
 {
-  uint8_t a, b, c;
-  asm
-  (
+  __uint24 ret;
+  asm (
       "rcall .\n"
-      "pop %2\n"
-      "pop %1\n"
-      "pop %0\n"
-      : "=r" (a), "=r" (b), "=r" (c)
+      "pop %A0\n"
+      "pop %B0\n"
+      "pop %C0\n"
+      : "=&r" (ret)
   );
-  ((uint8_t*)v)[0] = a;
-  ((uint8_t*)v)[1] = b;
-  ((uint8_t*)v)[2] = c;
-  ((uint8_t*)v)[3] = 0;
+  return ret;
 }
 
 #endif
diff --git a/Firmware/xflash_dump.cpp b/Firmware/xflash_dump.cpp
index e69c1d3a..4caba11b 100644
--- a/Firmware/xflash_dump.cpp
+++ b/Firmware/xflash_dump.cpp
@@ -59,7 +59,7 @@ static void __attribute__((noinline)) xfdump_dump_core(dump_header_t& hdr, uint3
 
     // sample SP/PC
     hdr.sp = SP;
-    GETPC(&hdr.pc);
+    hdr.pc = GETPC();
 
     // write header
     static_assert(sizeof(hdr) <= 256, "header is larger than a single page write");