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 or "fmu-v6s" 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    elif "fmu-v6s" in target:
155        device = "STM32H743II"
156        config = "fmu_v6x"
157    else:
158        raise ValueError(f"Unknown device for '{target}'!")
159
160    px4_dir = Path(px4_directory).absolute().resolve()
161    data_dir = Path(__file__).parent.resolve() / "data"
162
163    if backend in ["stlink", "orbtrace"]:
164        config += f"_{backend}.cfg"
165        backend_obj = emdbg.debug.OpenOcdBackend(config=[data_dir / config])
166    elif backend == "jlink":
167        rtos_so = px4_dir / f"platforms/nuttx/NuttX/nuttx/tools/jlink-nuttx.so"
168        if not rtos_so.exists(): rtos_so = None
169        backend_obj = emdbg.debug.JLinkBackend(device, speed, rtos_so)
170    elif isinstance(backend, Path):
171        backend_obj = emdbg.debug.CrashProbeBackend(backend)
172    elif ":" in backend:
173        backend_obj = emdbg.debug.ProbeBackend(backend)
174    else:
175        raise ValueError(f"Unknown backend '{backend}'!")
176
177    boot_elf = None
178    if Path(target).suffix == ".elf":
179        elf = Path(target)
180    else:
181        if (elf := next((px4_dir / "build" / target).glob("*.elf"), None)) is None:
182            raise ValueError(f"Cannot find ELF file in build folder '{px4_dir}/build/{target}'!")
183        if "_default" in target: # try to find the bootloader elf too
184            boot_elf = next((px4_dir / "build" / target.replace("_default", "_bootloader")).glob("*.elf"), None)
185
186    cmds = [f"dir {px4_dir}",
187            f"source {data_dir}/fmu.gdb",
188            f"python px4._TARGET='{target.lower()}'"]
189    if boot_elf is not None:
190        cmds += [f"add-symbol-file {boot_elf}"]
191    if ui is not None and backend_obj.name != "crashdebug":
192        cmds += Fmu._DBGMCU_CONFIG(target)
193    cmds += (commands or [])
194
195    return backend_obj, elf, cmds
196
197
198# -----------------------------------------------------------------------------
199@contextmanager
200def debug(px4_directory: Path, target: str, serial: str = None,
201          digilent: str = None, power: "emdbg.power.base.Base" = None,
202          ui: str = None, commands: list[str] = None, with_rpyc: bool = False,
203          keep_power_on: bool = True, upload: bool = True, backend: str = None,
204          _FmuClass = Fmu) -> Fmu:
205    """
206    Launches and configures the Fmu test bench.
207    1. Switches on the power relay if specified.
208    2. Starts the J-Link GDB server in the background.
209    3. Launches GDB and connects it to the debug probe backend.
210    4. Connects and initializes the NSH if specified.
211    5. Connects and initializes the Digilent Scope if specified.
212    6. Loads the PX4 specific GDB scripts, the `emdbg.debug.px4`
213       modules Python modules, and the `fmu.gdb` script.
214    7. Uploads the latest firmware to the FMU.
215    8. Yields the test bench in halted reset state.
216
217    :param px4_directory: path to the PX4-Autopilot repository you want to debug.
218    :param target: target name as a string, for example, `px4_fmu-v5x`.
219                   Can also be a path to an ELF file.
220    :param serial: optional serial number of the USB-Serial bridge for the NSH.
221                   If `None`, then `Fmu.nsh` is empty and cannot be used.
222    :param digilent: optional serial number of the Digilent. If `None`, then
223                     `Fmu.io` is empty and cannot be used.
224    :param ui: If not `None`, then this launches the interactive debugger via
225               `emdbg.debug.gdb.call` instead of the scripting API.
226    :param commands: list of additional GDB commands to execute during launch.
227    :param with_rpyc: Use the RPyC GDB interface implementation, rather than the
228                      GDB/MI interface. See `emdbg.debug.remote`.
229    :param keep_power_on: Do not shut off the Fmu after the context has closed.
230    :param upload: Automatically upload the firmware after powering on the FMU.
231    :param backend: `openocd`, `jlink`, or `IP:PORT` for a GDB server on another machine.
232
233    :return: A configured test bench with the latest firmware.
234    """
235    backend, elf, cmds = _px4_config(px4_directory, target, commands, ui,
236                                     backend=backend or "openocd")
237
238    with (nullcontext() if power is None else power) as pwr:
239        try:
240            if ui is not None:
241                # Manual mode that only connects the debugger (blocking)
242                if power: pwr.on()
243                yield emdbg.debug.gdb.call(backend, elf, commands=cmds, ui=ui)
244            else:
245                # Turn off, then connect the serial
246                if power: pwr.off()
247                serial = emdbg.serial.nsh(serial) if serial is not None else nullcontext()
248                scope = emdbg.io.analog_discovery(digilent) if digilent is not None else nullcontext()
249                with serial as nsh, scope as io:
250                    # Then power on and connect GDB to get the full boot log
251                    if power: pwr.on()
252                    gdb_call = emdbg.debug.gdb.call_rpyc if with_rpyc else emdbg.debug.gdb.call_mi
253                    debugger = gdb_call(backend, elf, commands=cmds)
254                    with debugger as gdb:
255                        bench = _FmuClass(target, elf, gdb, nsh, pwr, io)
256                        if upload: bench.upload()
257                        yield bench
258                        bench._deinit()
259        finally:
260            if not keep_power_on:
261                pwr.off(delay=0)
262
263
264# -----------------------------------------------------------------------------
265@contextmanager
266def shell(serial: str = None, power = None, keep_power_on: bool = True) -> Fmu:
267    """
268    Launches and configures the Fmu test bench.
269    1. Switches on the power relay if specified.
270    4. Connects and initializes the NSH if specified.
271    8. Yields the NSH after reset state.
272
273    :param serial: optional serial number of the USB-Serial bridge for the NSH.
274                   If `None`, then `Fmu.nsh` is empty and cannot be used.
275    :param keep_power_on: Do not shut off the Fmu after the context has closed.
276
277    :return: A configured test bench with the latest firmware.
278    """
279
280    if power is None: power = nullcontext()
281    with power as pwr:
282        try:
283            # Turn off, then connect the serial
284            pwr.off()
285            serial = emdbg.serial.nsh(serial) if serial is not None else nullcontext()
286            with serial as nsh:
287                # Then power on and connect GDB to get the full boot log
288                pwr.on()
289                yield nsh
290        finally:
291            if not keep_power_on:
292                pwr.off(delay=0)
293
294# -----------------------------------------------------------------------------
295def _arguments(description, modifier=None):
296    import argparse, emdbg
297    parser = argparse.ArgumentParser(description=description)
298    parser.add_argument(
299        "--px4-dir",
300        default=".",
301        type=Path,
302        help="The PX4 root directory you are working on.")
303    parser.add_argument(
304        "--target",
305        default="px4_fmu-v5x",
306        help="The target you want to debug.")
307    group = parser.add_mutually_exclusive_group(required=True)
308    group.add_argument(
309        "--jlink",
310        default=False,
311        action="store_true",
312        help="Use a J-Link debug probe")
313    group.add_argument(
314        "--stlink",
315        default=False,
316        action="store_true",
317        help="Use an STLink debug probe")
318    group.add_argument(
319        "--orbtrace",
320        default=False,
321        action="store_true",
322        help="Use an ORBtrace mini debug probe")
323    group.add_argument(
324        "--remote",
325        help="Connect to a remote GDB server: 'IP:PORT'")
326    group.add_argument(
327        "--coredump",
328        type=Path,
329        help="Inspect a GDB coredump or PX4 hardfault log.")
330    parser.add_argument(
331        "--ui",
332        default="cmd",
333        choices=["tui", "gdbgui", "cmd", "batch"],
334        help="The user interface you want to use.")
335    parser.add_argument(
336        "-v",
337        dest="verbosity",
338        action="count",
339        default=0,
340        help="Verbosity level.")
341    parser.add_argument(
342        "-ex",
343        dest="commands",
344        action="append",
345        help="Extra GDB commands.")
346    if modifier: modifier(parser)
347    args = parser.parse_args()
348    emdbg.logger.configure(args.verbosity)
349    backend = args.remote
350    if args.stlink: backend = "stlink"
351    if args.jlink: backend = "jlink"
352    if args.orbtrace: backend = "orbtrace"
353    if args.coredump: backend = args.coredump
354    return args, backend
355
356
357if __name__ == "__main__":
358    args, backend = _arguments("Debug FMU")
359
360    with debug(args.px4_dir, args.target, ui=args.ui, commands=args.commands,
361               backend=backend) as gdb_call:
362        exit(gdb_call)