aarch64: implement context switch, but not tested

kernel/src/arch/aarch64/board/raspi3/irq.rs

@@ -1,7 +1,8 @@
+use arch::interrupt::TrapFrame;
 use super::bcm2837::timer::Timer;
 use super::bcm2837::interrupt::{Controller, Interrupt};
 
-pub fn handle_irq() {
+pub fn handle_irq(tf: &mut TrapFrame) {
     let controller = Timer::new();
     if controller.is_pending() {
         println!("Timer tick {}...", super::timer::get_cycle());

kernel/src/arch/aarch64/interrupt/context.rs

@@ -14,47 +14,136 @@ pub struct TrapFrame {
     pub x0: u64,
 }
 
-///TODO
+/// 用于在内核栈中构造新线程的中断帧
+impl TrapFrame {
+    fn new_kernel_thread(entry: extern fn(usize) -> !, arg: usize, sp: usize) -> Self {
+        use core::mem::zeroed;
+        let mut tf: Self = unsafe { zeroed() };
+        tf.x0 = arg as u64;
+        tf.sp = sp as u64;
+        tf.elr = entry as u64;
+        tf.spsr = 0b1101_00_0101; // To EL 1, enable IRQ
+        tf
+    }
+    fn new_user_thread(entry_addr: usize, sp: usize) -> Self {
+        use core::mem::zeroed;
+        let mut tf: Self = unsafe { zeroed() };
+        tf.sp = sp as u64;
+        tf.elr = entry_addr as u64;
+        tf.spsr = 0b1101_00_0000; // To EL 0, enable IRQ
+        tf
+    }
+    pub fn is_user(&self) -> bool {
+        unimplemented!()
+    }
+}
+
+/// 新线程的内核栈初始内容
 #[derive(Debug)]
-pub struct Context {
-    // TODO
+#[repr(C)]
+pub struct InitStack {
+    context: ContextData,
+    tf: TrapFrame,
+}
+
+impl InitStack {
+    unsafe fn push_at(self, stack_top: usize) -> Context {
+        let ptr = (stack_top as *mut Self).offset(-1);
+        *ptr = self;
+        Context(ptr as usize)
+    }
+}
+
+extern {
+    fn __trapret();
+}
+
+#[derive(Debug, Default)]
+#[repr(C)]
+struct ContextData {
+    x19to29: [usize; 11],
+    lr: usize,
+    ttbr0: usize,
 }
 
+impl ContextData {
+    fn new(ttbr0: usize) -> Self {
+        ContextData { lr: __trapret as usize, ttbr0, ..ContextData::default() }
+    }
+}
+
+
+#[derive(Debug)]
+pub struct Context(usize);
+
 impl Context {
-    /// TODO
+    /// Switch to another kernel thread.
+    ///
+    /// Defined in `trap.S`.
+    ///
+    /// Push all callee-saved registers at the current kernel stack.
+    /// Store current sp, switch to target.
+    /// Pop all callee-saved registers, then return to the target.
+    #[naked]
     #[inline(never)]
-    pub unsafe extern "C" fn switch(&mut self, target: &mut Self) {
-        unimplemented!()
+    pub unsafe extern fn switch(&mut self, target: &mut Self) {
+        asm!(
+        "
+        mov x10, #-(13 * 8)
+        add x8, sp, x10
+        str x8, [x0]
+        stp x19, x20, [x8], #16     // store callee-saved registers
+        stp x21, x22, [x8], #16
+        stp x23, x24, [x8], #16
+        stp x25, x26, [x8], #16
+        stp x27, x28, [x8], #16
+        stp x29, lr, [x8], #16
+        mrs x9, ttbr0_el1
+        str x9, [x8], #8
+
+        ldr x8, [x1]
+        ldp x19, x20, [x8], #16     // restore callee-saved registers
+        ldp x21, x22, [x8], #16
+        ldp x23, x24, [x8], #16
+        ldp x25, x26, [x8], #16
+        ldp x27, x28, [x8], #16
+        ldp x29, lr, [x8], #16
+        ldr x9, [x8], #8
+        mov sp, x8
+
+        msr ttbr0_el1, x9           // set new page directory
+        dsb ishst                   // ensure write has completed
+        tlbi vmalle1is              // invalidate the TLB entry for the entry that changes
+        dsb ish                     // ensure TLB invalidation is complete
+        isb                         // synchronize context on this processor
+        ret"
+        : : : : "volatile" );
     }
 
-    /// TODO
     pub unsafe fn null() -> Self {
-        unimplemented!()
+        Context(0)
     }
 
-    /// TODO
-    pub unsafe fn new_kernel_thread(
-        entry: extern "C" fn(usize) -> !,
-        arg: usize,
-        kstack_top: usize,
-        cr3: usize,
-    ) -> Self {
-        unimplemented!()
+    pub unsafe fn new_kernel_thread(entry: extern fn(usize) -> !, arg: usize, kstack_top: usize, ttbr0: usize) -> Self {
+        InitStack {
+            context: ContextData::new(ttbr0),
+            tf: TrapFrame::new_kernel_thread(entry, arg, kstack_top),
+        }.push_at(kstack_top)
     }
-
-    /// TODO
-    pub unsafe fn new_user_thread(
-        entry_addr: usize,
-        ustack_top: usize,
-        kstack_top: usize,
-        is32: bool,
-        cr3: usize,
-    ) -> Self {
-        unimplemented!()
+    pub unsafe fn new_user_thread(entry_addr: usize, ustack_top: usize, kstack_top: usize, is32: bool, ttbr0: usize) -> Self {
+        InitStack {
+            context: ContextData::new(ttbr0),
+            tf: TrapFrame::new_user_thread(entry_addr, ustack_top),
+        }.push_at(kstack_top)
     }
-
-    /// TODO
-    pub unsafe fn new_fork(tf: &TrapFrame, kstack_top: usize, cr3: usize) -> Self {
-        unimplemented!()
+    pub unsafe fn new_fork(tf: &TrapFrame, kstack_top: usize, ttbr0: usize) -> Self {
+        InitStack {
+            context: ContextData::new(ttbr0),
+            tf: {
+                let mut tf = tf.clone();
+                tf.x0 = 0;
+                tf
+            },
+        }.push_at(kstack_top)
     }
 }

kernel/src/arch/aarch64/interrupt/handler.rs

@@ -49,7 +49,7 @@ pub extern "C" fn rust_trap(info: Info, esr: u32, tf: &mut TrapFrame) {
                 _ => ::trap::error(tf),
             }
         }
-        Kind::Irq => handle_irq(),
+        Kind::Irq => handle_irq(tf),
         _ => ::trap::error(tf),
     }
     ::trap::before_return();

kernel/src/arch/aarch64/interrupt/mod.rs

@@ -17,13 +17,13 @@ pub fn init() {
     }
 }
 
-/// Enable the interrupt.
+/// Enable the interrupt (only IRQ).
 #[inline(always)]
 pub unsafe fn enable() {
     asm!("msr daifclr, #2");
 }
 
-/// Disable the interrupt.
+/// Disable the interrupt (only IRQ).
 #[inline(always)]
 pub unsafe fn disable() {
     asm!("msr daifset, #2");

kernel/src/arch/aarch64/interrupt/trap.S

@@ -97,7 +97,7 @@ __alltraps:
     mov x2, sp
     bl rust_trap
 
-.global trap_ret
-trap_ret:
+.global __trapret
+__trapret:
     RESTORE_ALL
     eret

kernel/src/arch/aarch64/mod.rs

@@ -24,6 +24,8 @@ pub extern "C" fn rust_main() -> ! {
     interrupt::init();
     timer::init();
 
+    // ::process::init();
+
     unsafe { interrupt::enable(); }
 
     super::fs::show_logo();

kernel/src/process/mod.rs

@@ -5,6 +5,8 @@ pub use ucore_process::processor::{*, Context as _whatever};
 pub use ucore_process::scheduler::*;
 pub use ucore_process::thread::*;
 
+use arch::timer;
+
 mod context;
 
 type Processor = Processor_<Context, StrideScheduler>;
@@ -17,10 +19,18 @@ pub fn init() {
                 // NOTE: max_time_slice <= 5 to ensure 'priority' test pass
                 StrideScheduler::new(5),
             );
-            extern fn idle(arg: usize) -> ! {
-                loop {}
+            extern fn idle1(arg: usize) -> ! {
+                loop {
+                    println!("idle 1 {}", timer::get_cycle());
+                }
+            }
+            extern fn idle2(arg: usize) -> ! {
+                loop {
+                    println!("idle 2 {}", timer::get_cycle());
+                }
             }
-            processor.add(Context::new_kernel(idle, 0));
+            processor.add(Context::new_kernel(idle1, 0));
+            processor.add(Context::new_kernel(idle2, 0));
             processor
         })
     );
@@ -50,4 +60,4 @@ impl ThreadSupport for ThreadSupportImpl {
     fn processor() -> Self::ProcessorGuard {
         processor()
     }
-}
+}