1# Copyright (c) 2023, Auterion AG
  2# SPDX-License-Identifier: BSD-3-Clause
  3
  4"""
  5.. include:: fmu.md
  6"""
  7
  8from __future__ import annotations
  9import time
 10from functools import cached_property
 11from pathlib import Path
 12from contextlib import contextmanager, nullcontext
 13import emdbg
 14
 15
 16class Fmu:
 17    """
 18    FMU test bench with optional nsh, power, and logic analyzer attachments.
 19    """
 20    def _DBGMCU_CONFIG(target):
 21        # Vector catch all exceptions, but not reset
 22        common = ["px4_enable_vector_catch"]
 23        # Halt all timers and peripherals while debugging
 24        if "fmu-v5x" in target:
 25            return ["set *0xE0042008 = 0xffffffff",
 26                    "set *0xE004200C = 0xffffffff"] + common
 27        if "fmu-v6x" in target:
 28            return ["set *0xE00E1034 = 0xffffffff",
 29                    "set *0xE00E103C = 0xffffffff",
 30                    "set *0xE00E104C = 0xffffffff",
 31                    "set *0xE00E1054 = 0xffffffff"] + common
 32        return []
 33
 34    def __init__(self, target: str, elf: Path,
 35                 gdb: "emdbg.debug.remote.gdb.Interface",
 36                 nsh: "emdbg.serial.protocol.Nsh" = None,
 37                 power: "emdbg.power.base.Base" = None,
 38                 io: "emdbg.io.digilent.Digilent" = None):
 39        self.elf: Path = elf
 40        """The ELF file under debug"""
 41        self.gdb: "emdbg.debug.remote.gdb.Interface" = gdb
 42        """The remote GDB access interface object"""
 43        self.nsh: "emdbg.serial.protocol.Nsh" = nsh
 44        """The NSH protocol controller"""
 45        self.power: "emdbg.power.base.Base" = power
 46        """The power relay controlling the FMU"""
 47        self.io: "emdbg.io.digilent.Digilent" = io
 48        """The Digilent Scope"""
 49        self._target = target
 50
 51    def _init(self):
 52        self.gdb.interrupt_and_wait()
 53        for cmd in self._DBGMCU_CONFIG(self._target):
 54            self.gdb.execute(cmd)
 55        self.restart_system_load_monitor()
 56
 57    def _deinit(self):
 58        self.gdb.interrupt_and_wait()
 59        self.gdb.execute("disconnect")
 60        self.gdb.interrupted = False
 61        # Give JLink some time to react to the disconnect before shut down
 62        time.sleep(1)
 63
 64    def restart_system_load_monitor(self):
 65        """See `emdbg.debug.px4.system_load.restart_system_load_monitor`"""
 66        with self.gdb.interrupt_continue():
 67            if self.gdb.type == "mi":
 68                self.gdb.execute("python px4.restart_system_load_monitor(gdb)")
 69            else:
 70                emdbg.debug.px4.restart_system_load_monitor(self.gdb)
 71
 72    def coredump(self, filename: Path = None):
 73        """
 74        Dumps the FMU core for later analysis with CrashDebug (see
 75        `emdbg.debug.crashdebug`).
 76
 77        :param filename: Default `coredump_{datetime}.txt`.
 78        """
 79        with self.gdb.interrupt_continue():
 80            if False:
 81                # Connection is remote, therefore we must use slower RPyC interface
 82                emdbg.debug.px4.coredump(self.gdb, filename=filename)
 83            else:
 84                # Executing directly on the GDB process is *significantly* faster!
 85                if filename: filename = f"--file '{filename}'"
 86                self.gdb.execute(f"px4_coredump {filename or ''}")
 87
 88    def upload(self, source: Path = None):
 89        """
 90        Uploads the ELF file to the FMU, resets the device, clears the NSH
 91        :param source: optional path to ELF file, default is the ELF file passed
 92                       to the constructor.
 93        """
 94        with self.gdb.interrupt_continue():
 95            self.gdb.execute("monitor reset")
 96            self.gdb.execute(f"load {source or self.elf}", timeout=60)
 97            self.reset()
 98            if self.nsh is not None:
 99                self.nsh.clear()
100
101    def power_on_reset(self, delay_off: float = 1, delay_on: float = 2):
102        """
103        Disconnects GDB, stops the backend, power cycles the entire bench,
104        restarts the backend, reconnects and reinitializes GDB.
105        **Only works if initialized with a power relay!**
106        """
107        if self.power is None:
108            LOGGER.warning("Power Relay not configured!")
109            return
110        # Disconnect and stop the backend
111        self._deinit()
112        self.gdb.backend.stop()
113        # Wait for JLink to shut down, os.waitpid is not enough
114        time.sleep(1)
115
116        # Have you tried turning it off and on again?
117        self.power.cycle(delay_off, delay_on)
118
119        # Restart and reattach the backend
120        self.gdb.backend.start()
121        # Wait for JLink to start up again, since it starts non-blocking
122        time.sleep(2)
123        # Reinitialize GDB to attach back to JLink
124        for cmd in self.gdb.backend.init(self.elf):
125            self.gdb.execute(cmd)
126        self._init()
127
128    def reset(self):
129        """Resets the FMU"""
130        with self.gdb.interrupt_continue():
131            self.gdb.execute("monitor reset")
132            self._init()
133
134    def sleep(self, seconds: float, func=None):
135        """
136        Sleeps and calls `func` every second. This can be useful for polling if
137        a condition has happened.
138        """
139        while(seconds > 0):
140            seconds -= 1
141            time.sleep(1)
142            if func is not None:
143                func(seconds)
144
145
146def _px4_config(px4_directory: Path, target: Path, commands: list[str] = None,
147                ui: str = None, speed: int = 16000, backend: str = None) -> tuple:
148    if "fmu-v5x" in target:
149        device = "STM32F765II"
150        config = "fmu_v5x"
151    elif "fmu-v6x" in target:
152        device = "STM32H753II"
153        config = "fmu_v6x"
154    else:
155        raise ValueError(f"Unknown device for '{target}'!")
156
157    px4_dir = Path(px4_directory).absolute().resolve()
158    data_dir = Path(__file__).parent.resolve() / "data"
159
160    if backend in ["stlink", "orbtrace"]:
161        config += f"_{backend}.cfg"
162        backend_obj = emdbg.debug.OpenOcdBackend(config=[data_dir / config])
163    elif backend == "jlink":
164        rtos_so = px4_dir / f"platforms/nuttx/NuttX/nuttx/tools/jlink-nuttx.so"
165        if not rtos_so.exists(): rtos_so = None
166        backend_obj = emdbg.debug.JLinkBackend(device, speed, rtos_so)
167    elif isinstance(backend, Path):
168        backend_obj = emdbg.debug.CrashProbeBackend(backend)
169    elif ":" in backend:
170        backend_obj = emdbg.debug.ProbeBackend(backend)
171    else:
172        raise ValueError(f"Unknown backend '{backend}'!")
173
174    boot_elf = None
175    if Path(target).suffix == ".elf":
176        elf = Path(target)
177    else:
178        if (elf := next((px4_dir / "build" / target).glob("*.elf"), None)) is None:
179            raise ValueError(f"Cannot find ELF file in build folder '{px4_dir}/build/{target}'!")
180        if "_default" in target: # try to find the bootloader elf too
181            boot_elf = next((px4_dir / "build" / target.replace("_default", "_bootloader")).glob("*.elf"), None)
182
183    cmds = [f"dir {px4_dir}",
184            f"source {data_dir}/fmu.gdb",
185            f"python px4._TARGET='{target.lower()}'"]
186    if boot_elf is not None:
187        cmds += [f"add-symbol-file {boot_elf}"]
188    if ui is not None and backend_obj.name != "crashdebug":
189        cmds += Fmu._DBGMCU_CONFIG(target)
190    cmds += (commands or [])
191
192    return backend_obj, elf, cmds
193
194
195# -----------------------------------------------------------------------------
196@contextmanager
197def debug(px4_directory: Path, target: str, serial: str = None,
198          digilent: str = None, power: "emdbg.power.base.Base" = None,
199          ui: str = None, commands: list[str] = None, with_rpyc: bool = False,
200          keep_power_on: bool = True, upload: bool = True, backend: str = None,
201          _FmuClass = Fmu) -> Fmu:
202    """
203    Launches and configures the Fmu test bench.
204    1. Switches on the power relay if specified.
205    2. Starts the J-Link GDB server in the background.
206    3. Launches GDB and connects it to the debug probe backend.
207    4. Connects and initializes the NSH if specified.
208    5. Connects and initializes the Digilent Scope if specified.
209    6. Loads the PX4 specific GDB scripts, the `emdbg.debug.px4`
210       modules Python modules, and the `fmu.gdb` script.
211    7. Uploads the latest firmware to the FMU.
212    8. Yields the test bench in halted reset state.
213
214    :param px4_directory: path to the PX4-Autopilot repository you want to debug.
215    :param target: target name as a string, for example, `px4_fmu-v5x`.
216                   Can also be a path to an ELF file.
217    :param serial: optional serial number of the USB-Serial bridge for the NSH.
218                   If `None`, then `Fmu.nsh` is empty and cannot be used.
219    :param digilent: optional serial number of the Digilent. If `None`, then
220                     `Fmu.io` is empty and cannot be used.
221    :param ui: If not `None`, then this launches the interactive debugger via
222               `emdbg.debug.gdb.call` instead of the scripting API.
223    :param commands: list of additional GDB commands to execute during launch.
224    :param with_rpyc: Use the RPyC GDB interface implementation, rather than the
225                      GDB/MI interface. See `emdbg.debug.remote`.
226    :param keep_power_on: Do not shut off the Fmu after the context has closed.
227    :param upload: Automatically upload the firmware after powering on the FMU.
228    :param backend: `openocd`, `jlink`, or `IP:PORT` for a GDB server on another machine.
229
230    :return: A configured test bench with the latest firmware.
231    """
232    backend, elf, cmds = _px4_config(px4_directory, target, commands, ui,
233                                     backend=backend or "openocd")
234
235    with (nullcontext() if power is None else power) as pwr:
236        try:
237            if ui is not None:
238                # Manual mode that only connects the debugger (blocking)
239                if power: pwr.on()
240                yield emdbg.debug.gdb.call(backend, elf, commands=cmds, ui=ui)
241            else:
242                # Turn off, then connect the serial
243                if power: pwr.off()
244                serial = emdbg.serial.nsh(serial) if serial is not None else nullcontext()
245                scope = emdbg.io.analog_discovery(digilent) if digilent is not None else nullcontext()
246                with serial as nsh, scope as io:
247                    # Then power on and connect GDB to get the full boot log
248                    if power: pwr.on()
249                    gdb_call = emdbg.debug.gdb.call_rpyc if with_rpyc else emdbg.debug.gdb.call_mi
250                    debugger = gdb_call(backend, elf, commands=cmds)
251                    with debugger as gdb:
252                        bench = _FmuClass(target, elf, gdb, nsh, pwr, io)
253                        if upload: bench.upload()
254                        yield bench
255                        bench._deinit()
256        finally:
257            if not keep_power_on:
258                pwr.off(delay=0)
259
260
261# -----------------------------------------------------------------------------
262@contextmanager
263def shell(serial: str = None, power = None, keep_power_on: bool = True) -> Fmu:
264    """
265    Launches and configures the Fmu test bench.
266    1. Switches on the power relay if specified.
267    4. Connects and initializes the NSH if specified.
268    8. Yields the NSH after reset state.
269
270    :param serial: optional serial number of the USB-Serial bridge for the NSH.
271                   If `None`, then `Fmu.nsh` is empty and cannot be used.
272    :param keep_power_on: Do not shut off the Fmu after the context has closed.
273
274    :return: A configured test bench with the latest firmware.
275    """
276
277    if power is None: power = nullcontext()
278    with power as pwr:
279        try:
280            # Turn off, then connect the serial
281            pwr.off()
282            serial = emdbg.serial.nsh(serial) if serial is not None else nullcontext()
283            with serial as nsh:
284                # Then power on and connect GDB to get the full boot log
285                pwr.on()
286                yield nsh
287        finally:
288            if not keep_power_on:
289                pwr.off(delay=0)
290
291# -----------------------------------------------------------------------------
292def _arguments(description, modifier=None):
293    import argparse, emdbg
294    parser = argparse.ArgumentParser(description=description)
295    parser.add_argument(
296        "--px4-dir",
297        default=".",
298        type=Path,
299        help="The PX4 root directory you are working on.")
300    parser.add_argument(
301        "--target",
302        default="px4_fmu-v5x",
303        help="The target you want to debug.")
304    group = parser.add_mutually_exclusive_group(required=True)
305    group.add_argument(
306        "--jlink",
307        default=False,
308        action="store_true",
309        help="Use a J-Link debug probe")
310    group.add_argument(
311        "--stlink",
312        default=False,
313        action="store_true",
314        help="Use an STLink debug probe")
315    group.add_argument(
316        "--orbtrace",
317        default=False,
318        action="store_true",
319        help="Use an ORBtrace mini debug probe")
320    group.add_argument(
321        "--remote",
322        help="Connect to a remote GDB server: 'IP:PORT'")
323    group.add_argument(
324        "--coredump",
325        type=Path,
326        help="Inspect a GDB coredump or PX4 hardfault log.")
327    parser.add_argument(
328        "--ui",
329        default="cmd",
330        choices=["tui", "gdbgui", "cmd", "batch"],
331        help="The user interface you want to use.")
332    parser.add_argument(
333        "-v",
334        dest="verbosity",
335        action="count",
336        default=0,
337        help="Verbosity level.")
338    parser.add_argument(
339        "-ex",
340        dest="commands",
341        action="append",
342        help="Extra GDB commands.")
343    if modifier: modifier(parser)
344    args = parser.parse_args()
345    emdbg.logger.configure(args.verbosity)
346    backend = args.remote
347    if args.stlink: backend = "stlink"
348    if args.jlink: backend = "jlink"
349    if args.orbtrace: backend = "orbtrace"
350    if args.coredump: backend = args.coredump
351    return args, backend
352
353
354if __name__ == "__main__":
355    args, backend = _arguments("Debug FMU")
356
357    with debug(args.px4_dir, args.target, ui=args.ui, commands=args.commands,
358               backend=backend) as gdb_call:
359        exit(gdb_call)