From 84a34577a7ae4a29cf9c7e44bff8ac0f226704ed Mon Sep 17 00:00:00 2001
From: Alexander Lerach <alexander@auterion.com>
Date: Mon, 30 Sep 2024 10:30:16 +0200
Subject: [PATCH] [gdb] Fixed task switching for J-Link

---
 CHANGELOG.md                |  4 ++++
 src/emdbg/debug/px4/base.py | 11 ++++++++---
 src/emdbg/debug/px4/task.py |  1 +
 3 files changed, 13 insertions(+), 3 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index f78be95..d867a45 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -1,5 +1,9 @@
 # Changelog
 
+## 1.5.7
+
+- Fix `px4_switch_task` for J-Link (previously only fixed for ST-LINK).
+
 ## 1.5.6
 
 - Fix `px4_switch_task` for NuttX v11.
diff --git a/src/emdbg/debug/px4/base.py b/src/emdbg/debug/px4/base.py
index 05f23b4..b12a88e 100644
--- a/src/emdbg/debug/px4/base.py
+++ b/src/emdbg/debug/px4/base.py
@@ -56,18 +56,23 @@ def write_register(self, name: str, value: int):
     def write_registers(self, values: dict[str, int]):
         """Writes all named registers into the CPU"""
         for name, value in values.items():
-            #
             if name in ["control", "faultmask", "primask"]: continue
             # GDB does not know SP, only MSP and PSP
             if name in ["sp", "r13"]:
                 name = "msp"
             if name == "msp":
-                # NuttX stores the SP incorrectly (is off by 4 bytes)
-                self.write_register("r13", value + 4)
+                self.write_register("r13", value)
             # Remove double FP registers
             if name.startswith("d"): continue
             self.write_register(name, value)
 
+    def fix_nuttx_sp(self, regs: dict[str, int]):
+        """Fixes stored SP, as NuttX incorrectly stores the SP (is off by 4 bytes)"""
+        regs["msp"] = regs["msp"] + 4
+        regs["sp"] = regs["msp"]
+        regs["r13"] = regs["msp"]
+        return regs
+
     def lookup_static_symbol_in_function(self, symbol_name: str, function_name: str) -> "gdb.Symbol | None":
         """
         Lookup a static symbol inside a function. GDB makes this complicated
diff --git a/src/emdbg/debug/px4/task.py b/src/emdbg/debug/px4/task.py
index 57f5551..d30f4e5 100644
--- a/src/emdbg/debug/px4/task.py
+++ b/src/emdbg/debug/px4/task.py
@@ -215,6 +215,7 @@ def switch_to(self) -> bool:
             return False
         regs = {name: self._tcb["xcp"]["regs"][offset]
                 for name, offset in _XCP_REGS_MAP.items()}
+        regs = self.fix_nuttx_sp(regs)
         self.write_registers(regs)
         self._is_running_switched = True
         return True