From f60c1c48e0ff2b23a9e9830bb3a2bf4b212479a6 Mon Sep 17 00:00:00 2001
From: armink <armink.ztl@gmail.com>
Date: Thu, 12 Mar 2020 15:23:25 +0800
Subject: [PATCH] =?UTF-8?q?=E3=80=90=E5=AE=8C=E5=96=84=E3=80=91=E5=87=BD?=
 =?UTF-8?q?=E6=95=B0=E8=B0=83=E7=94=A8=E6=A0=88=E8=AF=86=E5=88=AB=E7=AD=96?=
 =?UTF-8?q?=E7=95=A5=EF=BC=8C=E5=A2=9E=E5=8A=A0=20LR=20=E5=9C=B0=E5=9D=80?=
 =?UTF-8?q?=E6=9B=B4=E5=A4=9A=E6=9C=89=E6=95=88=E6=80=A7=E6=A3=80=E6=B5=8B?=
 =?UTF-8?q?=EF=BC=8C=E6=8F=90=E5=8D=87=E8=AF=86=E5=88=AB=E5=87=86=E7=A1=AE?=
 =?UTF-8?q?=E6=80=A7=E3=80=82?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: armink <armink.ztl@gmail.com>
---
 cm_backtrace/cm_backtrace.c | 29 ++++++++++++++++++++++++++---
 cm_backtrace/cmb_def.h      |  2 +-
 2 files changed, 27 insertions(+), 4 deletions(-)

diff --git a/cm_backtrace/cm_backtrace.c b/cm_backtrace/cm_backtrace.c
index 1bd2d8a..056244a 100644
--- a/cm_backtrace/cm_backtrace.c
+++ b/cm_backtrace/cm_backtrace.c
@@ -329,6 +329,26 @@ static void dump_stack(uint32_t stack_start_addr, size_t stack_size, uint32_t *s
 }
 #endif /* CMB_USING_DUMP_STACK_INFO */
 
+/* check the disassembly instruction is 'BL' or 'BLX' */
+static bool disassembly_ins_is_bl_blx(uint32_t addr) {
+    uint16_t ins1 = *((uint16_t *)addr);
+    uint16_t ins2 = *((uint16_t *)(addr + 2));
+
+#define BL_INS_MASK         0xF800
+#define BL_INS_HIGH         0xF800
+#define BL_INS_LOW          0xF000
+#define BLX_INX_MASK        0xFF00
+#define BLX_INX             0x4700
+
+    if ((ins2 & BL_INS_MASK) == BL_INS_HIGH && (ins1 & BL_INS_MASK) == BL_INS_LOW) {
+        return true;
+    } else if ((ins2 & BLX_INX_MASK) == BLX_INX) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
 /**
  * backtrace function call stack
  *
@@ -347,8 +367,8 @@ size_t cm_backtrace_call_stack(uint32_t *buffer, size_t size, uint32_t sp) {
         if (!stack_is_overflow) {
             /* first depth is PC */
             buffer[depth++] = regs.saved.pc;
-            /* second depth is from LR, so need decrease a word to PC */
-            pc = regs.saved.lr - sizeof(size_t);
+            /* fix the LR address in thumb mode */
+            pc = regs.saved.lr - 1;
             if ((pc >= code_start_addr) && (pc <= code_start_addr + code_size) && (depth < CMB_CALL_STACK_MAX_DEPTH)
                     && (depth < size)) {
                 buffer[depth++] = pc;
@@ -386,8 +406,11 @@ size_t cm_backtrace_call_stack(uint32_t *buffer, size_t size, uint32_t sp) {
         if (pc % 2 == 0) {
             continue;
         }
+        /* fix the PC address in thumb mode */
+        pc = *((uint32_t *) sp) - 1;
         if ((pc >= code_start_addr) && (pc <= code_start_addr + code_size) && (depth < CMB_CALL_STACK_MAX_DEPTH)
-                && (depth < size)) {
+                /* check the the instruction before PC address is 'BL' or 'BLX' */
+                && disassembly_ins_is_bl_blx(pc - sizeof(size_t)) && (depth < size)) {
             /* the second depth function may be already saved, so need ignore repeat */
             if ((depth == 2) && regs_saved_lr_is_valid && (pc == buffer[1])) {
                 continue;
diff --git a/cm_backtrace/cmb_def.h b/cm_backtrace/cmb_def.h
index 91aa4a2..513de68 100644
--- a/cm_backtrace/cmb_def.h
+++ b/cm_backtrace/cmb_def.h
@@ -34,7 +34,7 @@
 #include <stdlib.h>
 
 /* library software version number */
-#define CMB_SW_VERSION                "1.3.0"
+#define CMB_SW_VERSION                "1.3.99"
 
 #define CMB_CPU_ARM_CORTEX_M0          0
 #define CMB_CPU_ARM_CORTEX_M3          1