From b75e388aa1fdfde33a3b356bc3c3b958b2014461 Mon Sep 17 00:00:00 2001
From: Harry Chen <i@harrychen.xyz>
Date: Sat, 6 Apr 2019 14:15:55 +0800
Subject: [PATCH] Fix backtrace for mipsel

Signed-off-by: Harry Chen <i@harrychen.xyz>
---
 kernel/src/arch/mipsel/boot/entry.S |  2 +-
 kernel/src/backtrace.rs             | 76 ++++++++++++++++++++++++++---
 2 files changed, 69 insertions(+), 9 deletions(-)

diff --git a/kernel/src/arch/mipsel/boot/entry.S b/kernel/src/arch/mipsel/boot/entry.S
index 7be682d..1a2a44b 100644
--- a/kernel/src/arch/mipsel/boot/entry.S
+++ b/kernel/src/arch/mipsel/boot/entry.S
@@ -22,7 +22,7 @@ _start:
     mtc0  t0, $12
 
     # directly jump to main function
-    j rust_main
+    jal   rust_main
 	nop
 
     .section .bss.stack
diff --git a/kernel/src/backtrace.rs b/kernel/src/backtrace.rs
index af1b045..337cfc8 100644
--- a/kernel/src/backtrace.rs
+++ b/kernel/src/backtrace.rs
@@ -24,8 +24,8 @@ pub fn fp() -> usize {
     }
     #[cfg(any(target_arch = "mips"))]
     unsafe {
-        // fp = $30
-        asm!("ori $0, $$30, 0" : "=r"(ptr));
+        // read $sp
+        asm!("ori $0, $$29, 0" : "=r"(ptr));
     }
 
     ptr
@@ -57,22 +57,46 @@ pub fn lr() -> usize {
     ptr
 }
 
+
 // Print the backtrace starting from the caller
 pub fn backtrace() {
     unsafe {
         let mut current_pc = lr();
         let mut current_fp = fp();
         let mut stack_num = 0;
+
+        // adjust sp to the top address of backtrace() function
+        #[cfg(target_arch = "mips")]
+        {
+            let func_base = backtrace as *const isize;
+            let sp_offset = (*func_base << 16) >> 16;
+            current_fp = ((current_fp as isize) - sp_offset) as usize;
+        }
+
         while current_pc >= stext as usize
             && current_pc <= etext as usize
             && current_fp as usize != 0
         {
-            println!(
-                "#{} {:#018X} fp {:#018X}",
-                stack_num,
-                current_pc - size_of::<usize>(),
-                current_fp
-            );
+            // print current backtrace
+            match size_of::<usize>() {
+                4 => {
+                    println!(
+                        "Stack #{:02} PC: {:#010X} FP: {:#010X}",
+                        stack_num,
+                        current_pc - size_of::<usize>(),
+                        current_fp
+                    );
+                },
+                _ => {
+                    println!(
+                        "Stack #{:02} PC: {:#018X} FP: {:#018X}",
+                        stack_num,
+                        current_pc - size_of::<usize>(),
+                        current_fp
+                    );
+                }
+            }
+
             stack_num = stack_num + 1;
             #[cfg(any(target_arch = "riscv32", target_arch = "riscv64"))]
             {
@@ -86,6 +110,42 @@ pub fn backtrace() {
                     current_pc = *(current_fp as *const usize).offset(1);
                 }
             }
+            #[cfg(target_arch = "mips")]
+            {
+                // the prologue of function is always like:
+                // main+0: 27bd____ addiu sp, sp, -____
+                // main+4: afbf____ sw    ra, ____(sp)
+                let mut code_ptr = current_pc as *const isize;
+                code_ptr = code_ptr.offset(-1);
+
+                // get the stack size of last function
+                while (*code_ptr as usize >> 16) != 0x27bd {
+                    code_ptr = code_ptr.offset(-1);
+                }
+                let sp_offset = (*code_ptr << 16) >> 16;
+                trace!("Found addiu sp @ {:08X}({:08x}) with sp offset {}", code_ptr as usize, *code_ptr, sp_offset);
+
+                // get the return address offset of last function
+                let mut last_fun_found = false;
+                while (code_ptr as usize) < current_pc {
+                    if (*code_ptr as usize >> 16) == 0xafbf {
+                        last_fun_found = true;
+                        break;
+                    }
+                    code_ptr = code_ptr.offset(1);
+                }
+                if last_fun_found {
+                    // unwind stack
+                    let ra_offset = (*code_ptr << 16) >> 16;
+                    trace!("Found sw ra @ {:08X}({:08x}) with ra offset {}", code_ptr as usize, *code_ptr, ra_offset);
+                    current_pc = *(((current_fp as isize) + ra_offset) as *const usize);
+                    current_fp = ((current_fp as isize) - sp_offset) as usize;
+                    trace!("New PC {:08X} FP {:08X}", current_pc, current_fp);
+                    continue;
+                } else {
+                    break;
+                }
+            }
             #[cfg(target_arch = "x86_64")]
             {
                 // Kernel stack at 0x0000_57ac_0000_0000 (defined in bootloader crate)