assertion fail発生時にスタックトレースを表示

## 1. はじめに
assertion失敗時のデバッグを効率化する方法を探していませんか？
通常、assert() が失敗すると発生個所を表すラーメッセージは表示されますが、それだけではどのような経緯でassert() が失敗したかがわからず、原因の特定に時間がかかってしまいます。このような場合に、スタックトレースが得られると非常に役立ちます。

この記事では、assertion失敗時にスタックトレースを自動で出力する方法を紹介します。
小規模なコード変更で、assertが発生した際に関数の呼び出し履歴を表示できるようになり、トラブルシューティングが格段に楽になります。
なお、開発環境はEarle Philhower版を使用しています。

## 2. 実装の方針
### 2.1 assertion fail発生時に呼ばれる関数を再定義
assertマクロは以下のように定義されており、```__assert_func()```を呼び出すようになっています。
```c:assert.h
# define assert(__e) ((__e) ? (void)0 : __assert_func (__FILE__, __LINE__, \
						       __ASSERT_FUNC, #__e))
```
```__assert_func()```は以下のように宣言されていますので、これに合わせた実装を行います。
```c:assert.h
void __assert_func (const char *, int, const char *, const char *)
	    _ATTRIBUTE ((__noreturn__));
```

### 2.2 スタックトレースの表示
スタックトレースを表示する方法については、以下の記事をご参照ください。

[rp2040で例外発生時にスタックトレースを表示する](https://qiita.com/yasuhiro-k/items/e4114211a1c4a995768d)

## 3. 実装

```c:StackTrace.h
#pragma once

#include <stdint.h>
#include <stdbool.h>

#ifdef __cplusplus
extern "C" {
#endif

extern uint32_t findReturnAddress(uint32_t **sp, uint32_t *stackLimit, uint32_t lr);
extern uint32_t * getStackTop(bool isMsp, uint8_t coreNum);
extern uint32_t adjustLRValue(uint32_t lr);

#ifdef __cplusplus
}
#endif
```
以前の記事では戻りアドレスを返却するようにしていましたが、処理内でBL命令(4bytes)かBLX命令(2bytes)かを判定していることから、BLあるいはBLX命令のアドレスを返却するように変更しました。
これにより関数呼び出し箇所の行番号が表示されるようになり、わかりやすくなっています。
```c:StackTrace.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 "StackTrace.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) >= (void *)(lower)) && ((void *)(addr) < (void *)(upper)))
#define ADDRESS_IN_RANGE(addr, lower, upper)  (((void *)(addr) >= (void *)(lower)) && ((void *)(addr) < (void *)(upper)))

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;
}

typedef enum { BL_BYTES = 4, BLX_BYTES = 2, OTHER_BYTES = 0, } bl_bytes_t;

static bl_bytes_t
isBLorBLXInstruction(uint32_t returnAddress)
{
  const uint16_t *returnAddress16 = (int16_t *)(returnAddress & ~0x00000001U) - 2;
  const uint16_t b1 = returnAddress16[0];
  const uint16_t b2 = returnAddress16[1];
  if (IS_BL_INSTRUCTION(b1, b2)) {
    return BL_BYTES;
  }
  if (IS_BLX_INSTRUCTION(b2)) {
    return BLX_BYTES;
  }
  return OTHER_BYTES;
}



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, uint32_t lr) 
{
  uint32_t *p = *sp;
  uint32_t returnAddress;
  bl_bytes_t bl_bytes = OTHER_BYTES;
  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__)) {
        bl_bytes = isBLorBLXInstruction(returnAddress);
        if (bl_bytes != OTHER_BYTES) {
          // 最初に見つかった戻り番地候補がLRと同じ値の場合は無視
          // ※LRがスタック上に保存されている場合に２回表示させないため
          if (checkSavedLR) {
            checkSavedLR = false;
            if (returnAddress != 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;
  }
  if (returnAddress != 0U) {
    returnAddress &= ~0x00000001U;
    returnAddress -= (uint32_t)bl_bytes;
  }
  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(bool isMsp, uint8_t coreNum)
{
  uint32_t * stack_top;
  if (isMsp) {
    if (coreNum == 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;
}

uint32_t
adjustLRValue(uint32_t lr)
{
  bl_bytes_t bl_bytes = isBLorBLXInstruction(lr);
  lr &= ~0x00000001U;
  lr -= (uint32_t)bl_bytes;
  return lr;
}

```

```c:AssertionHndler.cpp
#include <Arduino.h>
#include "StackTrace.h"
#if PICO_RP2040
#include <RP2040.h>
#else
#include <RP2350.h>
#endif

extern Stream *con;

extern "C" void
__assert_func(const char *file, int line, const char *func, const char *failedexpr)
{
  uint32_t ipsr = __get_IPSR();
  if (ipsr != 0) {
    for (;;) {
      __WFI();
    }
  }
  con->printf("assertion \"%s\" failed: file \"%s\", line %d%s%s\n",
              failedexpr, file, line,
              (func != NULL) ? ", function: " : "",
              (func != NULL) ? func : "");
  uint32_t lr = reinterpret_cast<uint32_t>(__builtin_return_address(0));
  con->printf("Backtrace: 0x%08x", adjustLRValue(lr));
  uint32_t * sp = reinterpret_cast<uint32_t *>(__builtin_stack_address());
  uint32_t * sp_org = sp;
  uint32_t control = __get_CONTROL();
  bool isMsp = (control & (1U << 1)) == 0;
  uint32_t * stack_top = getStackTop(isMsp, get_core_num());
  uint32_t return_address;
  while ((return_address = findReturnAddress(&sp, stack_top, lr)) != 0) {
    con->printf(" 0x%08x", return_address); 
  }
  con->printf("\n\n");
  for (;;) {
    delay(1000);
  }
}
```

## 4. テスト用プログラム
以下のテストプログラムを作成しました。
```c:main.cpp
#include <Arduino.h>
#include <atomic>
#include "StackTrace.h"
#if PICO_RP2040
#include <RP2040.h>
#else
#include <RP2350.h>
#endif

Stream *con = nullptr;

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

void
setup()
{
  Serial.begin(115200);
  Serial1.begin(115200);
  while (!Serial)
  {
    ;
  }
  con = &Serial;
  delay(1000);
  con->printf("build [%s %s] %08x\n", __DATE__, __TIME__);
  runCore1 = true;
} 

int 
sub2()
{
  assert(false);
  return 0;
}

int 
sub1()
{
  sub2();
  return 0;
}

void
loop()
{
  delay(1000);
}

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

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



```

## 5. 実行結果

以下のようにスタックトレースが取得できます。
```
assertion "false" failed: file "src\main.cpp", line 42, function: int sub2()
Backtrace: 0x100032ce 0x100032e2 0x1000331a 0x10004268 0x100074e2
  #0  0x100032ce in sub2() at src/main.cpp:42
  #1  0x100032e2 in sub1() at src/main.cpp:49
  #2  0x1000331a in loop1 at src/main.cpp:70
  #3  0x10004268 in main1 at C:\Users\％USERNAME%\.platformio\packages\framework-arduinopico\cores\rp2040/main.cpp:63
  #4  0x100074e2 in core1_wrapper at /home/earle/Arduino/hardware/pico/rp2040/pico-sdk/src/rp2_common/pico_multicore/multicore.c:98
```