1 files changed, 66 insertions, 69 deletions
diff --git a/pk/entry.S b/pk/entry.S
index e5faa24..aced3b8 100644
--- a/pk/entry.S
+++ b/pk/entry.S
@@ -14,6 +14,7 @@
 
   .macro save_tf
   # save gprs
+  STORE  x1,1*REGBYTES(x2)
   STORE  x3,3*REGBYTES(x2)
   STORE  x4,4*REGBYTES(x2)
   STORE  x5,5*REGBYTES(x2)
@@ -45,16 +46,15 @@
   STORE  x31,31*REGBYTES(x2)
 
   # get sr, epc, badvaddr, cause
-  csrr   x3,sup0    # x1
-  csrr   x4,sup1    # x2
-  csrr   x5,status
-  csrr   x6,epc
-  csrr   x8,cause
-  STORE  x3,1*REGBYTES(x2)
-  STORE  x4,2*REGBYTES(x2)
-  STORE  x5,32*REGBYTES(x2)
-  STORE  x6,33*REGBYTES(x2)
-  STORE  x8,35*REGBYTES(x2)
+  addi   t0,sp,320
+  csrrw  t0,mscratch,t0
+  csrr   t1,mstatus
+  csrr   t2,mepc
+  csrr   t3,mcause
+  STORE  t0,2*REGBYTES(x2)
+  STORE  t1,32*REGBYTES(x2)
+  STORE  t2,33*REGBYTES(x2)
+  STORE  t3,35*REGBYTES(x2)
 
   la gp, _gp
 
@@ -64,71 +64,68 @@
 1:
   .endm
 
-  .text
-  .globl  pop_tf
-pop_tf:  # write the trap frame onto the stack
-  # restore gprs
-  LOAD  a1,32*REGBYTES(a0)
-  LOAD  a2,1*REGBYTES(a0)
-  LOAD  a3,2*REGBYTES(a0)
-
-  csrw   status, a1 # restore sr (disable interrupts)
-  csrw   sup0, a2
-  csrw   sup1, a3
-
-  move  x1,a0
-  LOAD  x3,3*REGBYTES(x1)
-  LOAD  x4,4*REGBYTES(x1)
-  LOAD  x5,5*REGBYTES(x1)
-  LOAD  x6,6*REGBYTES(x1)
-  LOAD  x7,7*REGBYTES(x1)
-  LOAD  x8,8*REGBYTES(x1)
-  LOAD  x9,9*REGBYTES(x1)
-  LOAD  x10,10*REGBYTES(x1)
-  LOAD  x11,11*REGBYTES(x1)
-  LOAD  x12,12*REGBYTES(x1)
-  LOAD  x13,13*REGBYTES(x1)
-  LOAD  x14,14*REGBYTES(x1)
-  LOAD  x15,15*REGBYTES(x1)
-  LOAD  x16,16*REGBYTES(x1)
-  LOAD  x17,17*REGBYTES(x1)
-  LOAD  x18,18*REGBYTES(x1)
-  LOAD  x19,19*REGBYTES(x1)
-  LOAD  x20,20*REGBYTES(x1)
-  LOAD  x21,21*REGBYTES(x1)
-  LOAD  x22,22*REGBYTES(x1)
-  LOAD  x23,23*REGBYTES(x1)
-  LOAD  x24,24*REGBYTES(x1)
-  LOAD  x25,25*REGBYTES(x1)
-  LOAD  x26,26*REGBYTES(x1)
-  LOAD  x27,27*REGBYTES(x1)
-  LOAD  x28,28*REGBYTES(x1)
-  LOAD  x29,29*REGBYTES(x1)
-  LOAD  x30,30*REGBYTES(x1)
-  LOAD  x31,31*REGBYTES(x1)
-
-  # gtfo!
-  LOAD  x2,33*REGBYTES(x1)
-  csrr  x1, sup0
-  csrw  epc, x2
-  csrr  x2, sup1
-  sret
-
+  .section .text.init,"ax",@progbits
   .global  trap_entry
 trap_entry:
-  csrw  sup0, x1
-  csrw  sup1, sp
+  # entry point for reset
+  j _start
 
-  # when coming from kernel, continue below its stack
-  csrr  x1, status
-  and   x1, x1, SR_PS
-  add   sp, sp, -320
-  bnez  x1, 1f
-  la    sp, stack_top-320
+  # entry point when coming from machine mode
+  j 1f
 
-1:save_tf
+  # entry point when coming from other modes
+  csrrw sp, mscratch, sp
+
+1:addi sp,sp,-320
+  save_tf
   move  a0,sp
   j     handle_trap
+  
+  .globl  pop_tf
+pop_tf:  # write the trap frame onto the stack
+  # restore sr (disable interrupts) and epc
+  LOAD  a1,32*REGBYTES(a0)
+  LOAD  a2,33*REGBYTES(a0)
+  csrw  mstatus, a1
+  csrw  mepc, a2
+
+  # restore x registers
+  LOAD  x1,1*REGBYTES(a0)
+  LOAD  x2,2*REGBYTES(a0)
+  LOAD  x3,3*REGBYTES(a0)
+  LOAD  x4,4*REGBYTES(a0)
+  LOAD  x5,5*REGBYTES(a0)
+  LOAD  x6,6*REGBYTES(a0)
+  LOAD  x7,7*REGBYTES(a0)
+  LOAD  x8,8*REGBYTES(a0)
+  LOAD  x9,9*REGBYTES(a0)
+  LOAD  x11,11*REGBYTES(a0)
+  LOAD  x12,12*REGBYTES(a0)
+  LOAD  x13,13*REGBYTES(a0)
+  LOAD  x14,14*REGBYTES(a0)
+  LOAD  x15,15*REGBYTES(a0)
+  LOAD  x16,16*REGBYTES(a0)
+  LOAD  x17,17*REGBYTES(a0)
+  LOAD  x18,18*REGBYTES(a0)
+  LOAD  x19,19*REGBYTES(a0)
+  LOAD  x20,20*REGBYTES(a0)
+  LOAD  x21,21*REGBYTES(a0)
+  LOAD  x22,22*REGBYTES(a0)
+  LOAD  x23,23*REGBYTES(a0)
+  LOAD  x24,24*REGBYTES(a0)
+  LOAD  x25,25*REGBYTES(a0)
+  LOAD  x26,26*REGBYTES(a0)
+  LOAD  x27,27*REGBYTES(a0)
+  LOAD  x28,28*REGBYTES(a0)
+  LOAD  x29,29*REGBYTES(a0)
+  LOAD  x30,30*REGBYTES(a0)
+  LOAD  x31,31*REGBYTES(a0)
+  # restore a0 last
+  LOAD  x10,10*REGBYTES(a0)
+
+  # gtfo
+  mret
+
 
   .bss
   .align 4