rp2040で例外発生時にスタックトレースを表示する

## 1. はじめに
先月以下の記事を投稿しました。
[RP2040でHard Fault発生時の情報を表示する](https://qiita.com/yasuhiro-k/items/f57eb12510000d1da32d)
この記事では、PC (例外発生箇所) と LR (直前の呼び出し元) のみの表示を実現しました。しかし、今回さらに機能を拡張し、スタックトレースを表示する機能を追加しました。これにより、より詳細なデバッグ情報の取得が可能になります。

## 2. スタックトレースについて
以前Cortex-M系でスタックトレースを取得する方法について、以下の記事を投稿しました。
[CmBacktraceにおけるcall stack情報取得方法](https://qiita.com/yasuhiro-k/items/ed1f1615df7f87f02e7b)
この方法を応用し、RP2040向けにカスタマイズを行いました。
主な変更点：
・コード領域判定方法を変更
・スタック領域取得方法を変更(特にPSPの場合)

## 3. 作成したコード

```c:HardFaultHanler.h
#pragma once

#ifdef __cplusplus
extern "C" {
#endif
typedef struct {
  uint32_t r0;
  uint32_t r1;
  uint32_t r2;
  uint32_t r3;
  uint32_t r12;
  uint32_t lr;
  uint32_t pc;
  uint32_t psr;
  uint32_t sp;
  uint8_t core_num;
  bool is_msp;
} fault_regs_t;

extern void registerHardfaultHandler(void (*func)(const fault_regs_t *regs));
extern uint32_t findReturnAddress(uint32_t **sp, uint32_t *stackLimit, const fault_regs_t *regs);
extern uint32_t * getStackTop(const fault_regs_t *regs);

#ifdef __cplusplus
}
#endif
```

```c:HardFaultHanler.c
#include <stdio.h>
#include <stdint.h>
#include <stdbool.h>
#include <hardware/exception.h>
#include <pico/platform/panic.h>
#if PICO_RP2040
#include <RP2040.h>
#else
#include <RP2350.h>
#endif
#include "HardFaultHandler.h"

#define BL_INSTRUCTION1       (0xf000)
#define BL_INSTRUCTION1_MASK  (0x07ff)
#define BL_INSTRUCTION2       (0xd000)
#define BL_INSTRUCTION2_MASK  (0x2fff)
#define BLX_INSTRUCTION       (0x4780)
#define BLX_INSTRUCTION_MASK  (0x007f)
#define PUSH_INSTRUCTION      (0xb400)
#define PUSH_INSTRUCTION_MASK (0x01ff)

#define IS_BL_INSTRUCTION(b1, b2) ((((b1) & ~BL_INSTRUCTION1_MASK) == BL_INSTRUCTION1) && \
                                   (((b2) & ~BL_INSTRUCTION2_MASK) == BL_INSTRUCTION2))
#define IS_BLX_INSTRUCTION(b)     (((b) & ~BLX_INSTRUCTION_MASK) == BLX_INSTRUCTION)
#define IS_PUSH_INSTRUCTION(b)    (((b) & ~PUSH_INSTRUCTION_MASK) == PUSH_INSTRUCTION)

#define ADDRESS_IN_RANGE(addr, lower, upper)  (((void *)(addr) >= lower) && ((void *)(addr) < upper))

uint32_t sp_offset = 0;
static fault_regs_t saved_regs;
static void (*handler)(const fault_regs_t *regs) = NULL;

static void
exceptionHandlerEntry(void)
{
  uint32_t *sp;
  uint32_t lr = (uint32_t)__builtin_return_address(0);
  if ((lr & 4) == 0) {
    sp = (uint32_t *)(__get_MSP() + sp_offset);
    saved_regs.is_msp = true;
  } else {
    sp = (uint32_t *)__get_PSP();
    saved_regs.is_msp = false;
  }
  saved_regs.core_num = get_core_num();
  saved_regs.r0  = sp[0];
  saved_regs.r1  = sp[1];
  saved_regs.r2  = sp[2];
  saved_regs.r3  = sp[3];
  saved_regs.r12 = sp[4];
  saved_regs.lr  = sp[5];
  saved_regs.pc  = sp[6];
  saved_regs.psr = sp[7];
  saved_regs.sp  = ((uint32_t)sp);

  if ((saved_regs.psr &0x1f) != 0) {
    // 例外処理中にfaultが発生」した場合は、
    // 表示ができない可能性があるため、ここで停止させる。
    for (;;) {
      ;
    }
  }
  // 例外からの戻りアドレスを変更
  sp[6] = ((uint32_t)handler) & 0xfffffffeU;
  // レジスタの値を格納した構造体のアドレスを第一引数として渡す
  sp[0] = (uint32_t)&saved_regs;
}

void
registerHardfaultHandler(void (*func)(const fault_regs_t *regs))
{
  uint16_t first_instruction = *((uint16_t *)((uint32_t)exceptionHandlerEntry & ~0x00000001));
  if (IS_PUSH_INSTRUCTION(first_instruction)) {
    uint16_t saved_registers = first_instruction & PUSH_INSTRUCTION_MASK;
    while (saved_registers != 0) {
      if ((saved_registers & 0x0001) != 0) {
        sp_offset += sizeof(uint32_t);
      }
      saved_registers >>= 1;
    }
  }
  handler = func;
  exception_set_exclusive_handler(HARDFAULT_EXCEPTION, exceptionHandlerEntry);
}

static uint32_t
getBlOffset(uint16_t b1, uint16_t b2)
{
  const uint32_t s = (b1 >> 10) & 0x0001;
  uint32_t j1 = (b2 >> 13) & 0x0001;
  uint32_t j2 = (b2 >> 11) & 0x0001;
  if (s == 0) {
    j1 = (~j1) & 0x0001;
    j2 = (~j2) & 0x0001;
  }
  const uint32_t blOffset = ((s == 1) ? 0xff000000 : 0 ) | 
                            (j1 << 23) | (j2 << 22) | 
                            ((b1 & 0x3ff) << 12) | ((b2 & 0x7ff)) << 1;
  return blOffset;
}

extern uint16_t __flash_binary_start[];
extern uint16_t __etext[];
extern uint16_t __data_start__[];
extern uint16_t __data_end__[];

static bool checkSavedLR = true;

uint32_t
findReturnAddress(uint32_t **sp, uint32_t *stackLimit, const fault_regs_t *regs) 
{
  uint32_t *p = *sp;
  uint32_t returnAddress;
  while (p < stackLimit) {
    returnAddress = *p++;
    if ((returnAddress & 0x00000001U) != 0) {
      const uint16_t *returnAddress16 = (int16_t *)(returnAddress & ~0x00000001U) - 2;
      // 戻りアドレスがコード領域にあるか
      if (ADDRESS_IN_RANGE(returnAddress16, __flash_binary_start, __etext) ||
          ADDRESS_IN_RANGE(returnAddress16, __data_start__, __data_end__)) {
        uint16_t b1 = returnAddress16[0];
        uint16_t b2 = returnAddress16[1];
        if (IS_BL_INSTRUCTION(b1, b2) || IS_BLX_INSTRUCTION(b2)) {
          // 最初に見つかった戻り番地候補がLRと同じ値の場合は無視
          // ※LRがスタック上に保存されている場合に２回表示させないため
          if (checkSavedLR) {
            checkSavedLR = false;
            if (returnAddress != regs->lr) {
              break;
            }
          } else {
            break;
          }
        } else {
          // タスクの戻りアドレスはprvTaskExitErrorとなっており、
          // 戻り先が関数prvTaskExitErrorとなっていたらそこで終了とする。
          // なおこの関数はstatic宣言されているので、命令パターンで判定
          // <prvTaskExitError>:
          //   	push	{r4, lr}
          //  	bl	10008538 <panic_unsupported>
          returnAddress16 += 2;
          uint16_t b3 = returnAddress16[0];
          uint16_t b4 = returnAddress16[1];
          uint16_t b5 = returnAddress16[2];
          if (IS_PUSH_INSTRUCTION(b3) && IS_BL_INSTRUCTION(b4, b5)) {
            // とび先アドレスが一致しているかをチェック
            const uint32_t baseAddr = (uint32_t)&returnAddress16[3];
            const uint32_t panicUnsupportedAddr = ((uint32_t)panic_unsupported) & ~0x00000001;
            const uint32_t expectedOffset = panicUnsupportedAddr - baseAddr;
            const uint32_t blOffset = getBlOffset(b4, b5);  // B+命令のoffset部分を抽出
            if (expectedOffset == blOffset) {
              returnAddress = 0U;
              break;
            }
          }
        }
      }
    }
  }
  *sp = p;
  if (p >= stackLimit) {
    returnAddress = 0U;
  }
  returnAddress &= ~0x00000001U;
  return returnAddress;
}

// 
extern uint32_t __HeapLimit[];
extern uint32_t __StackOneTop[];
extern uint32_t __StackTop[];

extern bool core1_separate_stack;
extern uint32_t* core1_separate_stack_address;

uint32_t *
getStackTop(const fault_regs_t *regs)
{
  uint32_t * stack_top;
  if (regs->is_msp) {
    if (regs->core_num == 0) {
      stack_top = __StackTop;
    } else if (core1_separate_stack) {
      stack_top = core1_separate_stack_address + (0x2000 / sizeof(uint32_t));
    } else {
      stack_top = __StackOneTop;
    }
  } else {
    stack_top = __HeapLimit;
  }
  return stack_top;
}

```
テストプログラム（FreeRTOSを使用しない場合）：

```cpp:main.cpp

#include <Arduino.h>
#include <stdlib.h>
#include <atomic>
#include <hardware/exception.h>
#include "HardFaultHandler.h"

static void __attribute__((noreturn))
showFaultStatus(const fault_regs_t *regs)
{
  Serial.printf("******** Hard fault (Core %d) ********\n", regs->core_num);
  Serial.printf("r0: %08x r1:%08x r2:%08x r3: %08x\n", regs->r0, regs->r1, regs->r2, regs->r3);
  Serial.printf("r12:%08x lr:%08x pc:%08x psr:%08x\n", regs->r12, regs->lr, regs->pc, regs->psr);
  Serial.printf("sp: %08x\n", regs->sp);
  Serial.printf("******** Halted ********\n\n");

  Serial.printf("Backtrace: 0x%08x 0x%08x", regs->pc, (regs->lr) & ~0x00000001U);
  uint32_t * stack_top = getStackTop(regs);
  uint32_t *sp = reinterpret_cast<uint32_t *>(regs->sp);
  uint32_t return_address;
  while ((return_address = findReturnAddress(&sp, stack_top, regs)) != 0) {
    Serial.printf(" 0x%08x", return_address); 
  }
  Serial.printf("\n\n");
  while (true) {
    delay(5000);
  }
}

static std::atomic<bool> runCore1 = false; 

void
setup()
{
  while (!Serial) {
    ;
  }
  delay(200);
  Serial.printf("build [%s %s]\n", __DATE__, __TIME__);
  registerHardfaultHandler(showFaultStatus);
  runCore1 = true;
}

void 
sub2()
{
  __breakpoint();
}

void 
sub1()
{
  sub2();
}

void
loop()
{
  // sub1();
  delay(1000);
}

void
setup1()
{
  while (!runCore1) {
    delay(1);
  }
}

void
loop1()
{
  sub1();
  delay(1000);
}

```
:::note info
FreeRTOSを使うようにするには、FreeRTOS.hをインクルードしてください。
:::
esp32_exception_decoderを使えるようにする方法については、下記リンク先を参照してください。
## 4. esp32_exception_decoderを使えるようにする
esp32_exception_decoderを使えるようにする方法については、下記リンク先を参照してください。
[esp32_exception_decoderを使えるようにする](https://qiita.com/yasuhiro-k/items/f57eb12510000d1da32d#5-esp32_exception_decoder%E3%82%92%E4%BD%BF%E3%81%88%E3%82%8B%E3%82%88%E3%81%86%E3%81%AB%E3%81%99%E3%82%8B)

## 5. 実行結果
FreeRTOSを使用しない場合の出力です。
```
******** Hard fault (Core 1) ********
r0: 40054000 r1:0021f208 r2:0021f208 r3: 10003305
r12:00000000 lr:100032d3 pc:100032c8 psr:21000000
sp: 20040fc8
******** Halted ********

Backtrace: 0x100032c8 0x100032d2 0x1000330a 0x10003c24 0x10006c60
  #0  0x100032c8 in sub2() at C:\Users\%USERNAME%\.platformio\packages\framework-arduinopico/pico-sdk/src/rp2040/pico_platform/include/pico/platform.h:126
      (inlined by) sub2() at src/main.cpp:47
  #1  0x100032d2 in sub1() at src/main.cpp:54
  #2  0x1000330a in loop1 at src/main.cpp:75
  #3  0x10003c24 in main1 at C:\Users\%USERNAME%\.platformio\packages\framework-arduinopico\cores\rp2040/main.cpp:58
  #4  0x10006c60 in core1_wrapper at /home/earle/Arduino/hardware/pico/rp2040/pico-sdk/src/rp2_common/pico_multicore/multicore.c:100    

```
FreeRTOS使用時の出力です。
```
******** Hard fault (Core 1) ********
r0: 00000000 r1:00000004 r2:10000000 r3: 10003305
r12:a5a5a5a5 lr:100032d3 pc:100032c8 psr:21000000
sp: 20007230
******** Halted ********

Backtrace: 0x100032c8 0x100032d2 0x1000330a 0x100036ea
  #0  0x100032c8 in sub2() at C:\Users\%USERNAME%\.platformio\packages\framework-arduinopico/pico-sdk/src/rp2040/pico_platform/include/pico/platform.h:126
      (inlined by) sub2() at src/main.cpp:48
  #1  0x100032d2 in sub1() at src/main.cpp:55
  #2  0x1000330a in loop1 at src/main.cpp:76
  #3  0x100036ea in __core1(void*) at C:\Users\%USERNAME%\.platformio\packages\framework-arduinopico\libraries\FreeRTOS\src/variantHooks.cpp:162
```
スタック領域は、FreeRRTOSを使う場合と使わない場合で確保領域が異なりますが、どちらの場合でもスタックトレースが取得できることが確認できました。