From a59a7fbe9a3e4878bcd8abbc913e87b46c6c717d Mon Sep 17 00:00:00 2001
From: equation314 <equation618@gmail.com>
Date: Sat, 1 Dec 2018 20:00:35 +0800
Subject: [PATCH] aarch64/mmu: simply handle page fault

---
 crate/aarch64/src/regs/far_el1.rs            | 31 ++++++++++++++++++++
 crate/aarch64/src/regs/mod.rs                |  2 ++
 kernel/src/arch/aarch64/interrupt/handler.rs | 28 +++++++++++++++---
 3 files changed, 57 insertions(+), 4 deletions(-)
 create mode 100644 crate/aarch64/src/regs/far_el1.rs

diff --git a/crate/aarch64/src/regs/far_el1.rs b/crate/aarch64/src/regs/far_el1.rs
new file mode 100644
index 0000000..fc809fa
--- /dev/null
+++ b/crate/aarch64/src/regs/far_el1.rs
@@ -0,0 +1,31 @@
+/*
+ * Copyright (c) 2018 by the author(s)
+ *
+ * =============================================================================
+ *
+ * Licensed under either of
+ *   - Apache License, Version 2.0 (http://www.apache.org/licenses/LICENSE-2.0)
+ *   - MIT License (http://opensource.org/licenses/MIT)
+ * at your option.
+ *
+ * =============================================================================
+ *
+ * Author(s):
+ *   - Andre Richter <andre.o.richter@gmail.com>
+ */
+
+//! Fault Address Register - EL1
+//!
+//! Holds the faulting Virtual Address for all synchronous Instruction or Data
+//! Abort, PC alignment fault and Watchpoint exceptions that are taken to EL1.
+
+use register::cpu::RegisterReadWrite;
+
+pub struct Reg;
+
+impl RegisterReadWrite<u64, ()> for Reg {
+    sys_coproc_read_raw!(u64, "FAR_EL1");
+    sys_coproc_write_raw!(u64, "FAR_EL1");
+}
+
+pub static FAR_EL1: Reg = Reg {};
diff --git a/crate/aarch64/src/regs/mod.rs b/crate/aarch64/src/regs/mod.rs
index f69f0c4..11a6d3f 100644
--- a/crate/aarch64/src/regs/mod.rs
+++ b/crate/aarch64/src/regs/mod.rs
@@ -12,6 +12,7 @@ mod cntvoff_el2;
 mod currentel;
 mod daif;
 mod elr_el2;
+mod far_el1;
 mod hcr_el2;
 mod id_aa64mmfr0_el1;
 mod mair_el1;
@@ -38,6 +39,7 @@ pub use self::cntvoff_el2::CNTVOFF_EL2;
 pub use self::currentel::CurrentEL;
 pub use self::daif::DAIF;
 pub use self::elr_el2::ELR_EL2;
+pub use self::far_el1::FAR_EL1;
 pub use self::hcr_el2::HCR_EL2;
 pub use self::id_aa64mmfr0_el1::ID_AA64MMFR0_EL1;
 pub use self::mair_el1::{MAIR_EL1, MAIR_ATTR};
diff --git a/kernel/src/arch/aarch64/interrupt/handler.rs b/kernel/src/arch/aarch64/interrupt/handler.rs
index 787cbf5..21142f9 100644
--- a/kernel/src/arch/aarch64/interrupt/handler.rs
+++ b/kernel/src/arch/aarch64/interrupt/handler.rs
@@ -2,7 +2,9 @@
 
 use arch::board::irq::handle_irq;
 use super::context::TrapFrame;
-use super::syndrome::Syndrome;
+use super::syndrome::{Fault, Syndrome};
+
+use aarch64::regs::*;
 
 global_asm!(include_str!("trap.S"));
 global_asm!(include_str!("vector.S"));
@@ -46,7 +48,14 @@ pub extern "C" fn rust_trap(info: Info, esr: u32, tf: &mut TrapFrame) {
             // syndrome is only valid with sync
             match syndrome {
                 Syndrome::Brk(brk) => handle_break(brk, tf),
-                Syndrome::Svc(_) => handle_syscall(tf),
+                Syndrome::Svc(svc) => handle_syscall(svc, tf),
+                Syndrome::DataAbort { kind, level: _ }
+                | Syndrome::InstructionAbort { kind, level: _ } => match kind {
+                    Fault::Translation | Fault::AccessFlag | Fault::Permission => {
+                        handle_page_fault(tf)
+                    }
+                    _ => ::trap::error(tf),
+                },
                 _ => ::trap::error(tf),
             }
         }
@@ -57,12 +66,16 @@ pub extern "C" fn rust_trap(info: Info, esr: u32, tf: &mut TrapFrame) {
     trace!("Interrupt end");
 }
 
-fn handle_break(num: u16, tf: &mut TrapFrame) {
+fn handle_break(_num: u16, tf: &mut TrapFrame) {
     // Skip the current brk instruction (ref: J1.1.2, page 6147)
     tf.elr += 4;
 }
 
-fn handle_syscall(tf: &mut TrapFrame) {
+fn handle_syscall(num: u16, tf: &mut TrapFrame) {
+    if num != 0 {
+        ::trap::error(tf);
+    }
+
     // svc instruction has been skipped in syscall (ref: J1.1.2, page 6152)
     let ret = ::syscall::syscall(
         tf.x1to29[7] as usize,
@@ -78,3 +91,10 @@ fn handle_syscall(tf: &mut TrapFrame) {
     );
     tf.x0 = ret as usize;
 }
+
+fn handle_page_fault(tf: &mut TrapFrame) {
+    let addr = FAR_EL1.get();
+    trace!("\nEXCEPTION: Page Fault @ {:#x}", addr);
+
+    ::trap::error(tf);
+}