From cc5bb8a81f83e15035388f7936e7fd713b3d0408 Mon Sep 17 00:00:00 2001
From: Yuhao Zhou <miskcoo@gmail.com>
Date: Wed, 8 May 2019 00:31:44 +0800
Subject: [PATCH] Fix alignment issue in TrapFrame.

---
 kernel/src/arch/mipsel/board/malta/consts.rs |  4 ++--
 kernel/src/arch/mipsel/boot/linker.ld        |  2 +-
 kernel/src/arch/mipsel/boot/trap.S           | 18 ++++++++++++++++--
 kernel/src/arch/mipsel/consts.rs             |  2 +-
 kernel/src/arch/mipsel/context.rs            |  6 ++++--
 kernel/src/arch/mipsel/interrupt.rs          |  5 +++++
 6 files changed, 29 insertions(+), 8 deletions(-)

diff --git a/kernel/src/arch/mipsel/board/malta/consts.rs b/kernel/src/arch/mipsel/board/malta/consts.rs
index 73ff9665..98f0a5ec 100644
--- a/kernel/src/arch/mipsel/board/malta/consts.rs
+++ b/kernel/src/arch/mipsel/board/malta/consts.rs
@@ -1,3 +1,3 @@
 /// board specific constants
-pub const MEMORY_END: usize = 0x8800_0000;
-pub const KERNEL_HEAP_SIZE: usize = 0x00a0_0000;
+pub const MEMORY_END: usize = 0x8080_0000;
+pub const KERNEL_HEAP_SIZE: usize = 0x0044_0000;
diff --git a/kernel/src/arch/mipsel/boot/linker.ld b/kernel/src/arch/mipsel/boot/linker.ld
index 0e4d0bb5..420430da 100644
--- a/kernel/src/arch/mipsel/boot/linker.ld
+++ b/kernel/src/arch/mipsel/boot/linker.ld
@@ -4,7 +4,7 @@
 OUTPUT_ARCH(riscv)
 ENTRY(_start)
 
-BASE_ADDRESS = 0x80000000;
+BASE_ADDRESS = 0x80100000;
 
 SECTIONS
 {
diff --git a/kernel/src/arch/mipsel/boot/trap.S b/kernel/src/arch/mipsel/boot/trap.S
index 35d30965..07b7ffaa 100644
--- a/kernel/src/arch/mipsel/boot/trap.S
+++ b/kernel/src/arch/mipsel/boot/trap.S
@@ -31,7 +31,18 @@ trap_from_kernel:
 	 *  k0 is damaged
 	 *  k1 = old stack pointer
 	 *  sp = kernel stack */
+
+	# align stack pointer
+	andi k0, sp, 0xf
+	beqz k0, sp_aligned
+	nop
+
+	la k0, 0xfffffff0
+	and k0, sp, k0
+	sw sp, -176(k0)
+	move sp, k0
 	
+sp_aligned:
 	# allocate 38 words for trapframe + 6 extra words
 	addiu sp, sp, -176
 
@@ -147,9 +158,12 @@ trap_return:
 	lw fp, 156(sp)
 	lw ra, 160(sp)
 
-	// save kernel stack
-	la k0, _cur_kstack_ptr
+	# save kernel stack
+	lw k0, 0(sp)
 	addiu k1, sp, 176
+	movn k1, k0, k0
+
+	la k0, _cur_kstack_ptr
 	sw k1, 0(k0)
 	nop
 
diff --git a/kernel/src/arch/mipsel/consts.rs b/kernel/src/arch/mipsel/consts.rs
index b3dcc5be..84ae103e 100644
--- a/kernel/src/arch/mipsel/consts.rs
+++ b/kernel/src/arch/mipsel/consts.rs
@@ -2,7 +2,7 @@
 ///
 pub use super::board::consts::*;
 
-pub const KERNEL_OFFSET: usize = 0x80000000;
+pub const KERNEL_OFFSET: usize = 0x80100000;
 
 pub const MEMORY_OFFSET: usize = 0x8000_0000;
 
diff --git a/kernel/src/arch/mipsel/context.rs b/kernel/src/arch/mipsel/context.rs
index 1f63f3ae..116e9f05 100644
--- a/kernel/src/arch/mipsel/context.rs
+++ b/kernel/src/arch/mipsel/context.rs
@@ -5,8 +5,10 @@ use mips::tlb;
 #[derive(Clone)]
 #[repr(C)]
 pub struct TrapFrame {
-    /// unused 16 bytes
-    pub unused: [usize; 4],
+    /// Non-zero if the kernel stack is not 16-byte-aligned
+    pub unaligned_kstack: usize,
+    /// unused 12 bytes
+    pub unused: [usize; 3],
     /// CP0 status register
     pub status: cp0::status::Status,
     /// CP0 cause register
diff --git a/kernel/src/arch/mipsel/interrupt.rs b/kernel/src/arch/mipsel/interrupt.rs
index 60abde3a..2909d289 100644
--- a/kernel/src/arch/mipsel/interrupt.rs
+++ b/kernel/src/arch/mipsel/interrupt.rs
@@ -60,6 +60,11 @@ pub unsafe fn restore(flags: usize) {
     }
 }
 
+#[no_mangle]
+pub extern "C" fn stack_pointer_not_aligned(sp: usize) {
+    panic!("Stack pointer not aligned: sp = 0x{:x?}", sp);
+}
+
 /// Dispatch and handle interrupt.
 ///
 /// This function is called from `trap.asm`.