From d85d824bfd63d959111188abce1f37c01d629f18 Mon Sep 17 00:00:00 2001
From: Berkus Decker <berkus+github@metta.systems>
Date: Fri, 20 Nov 2020 03:09:43 +0200
Subject: [PATCH] Switch on MMU immediately to avoid unaligned access

Since enabling armv6 unaligned access bit U in CP15 C1
requires AArch32 assembly and I don't want to introduce
yet another boot stub, I'll just initialize MMU right away
- this causes CPU to treat SCTLR_EL1.A differently and not
cause any alignment faults.

In the future, a small AArch32 boot stub that uses
now commented out fn enable_armv6_unaligned_access()
should be used to set U=1 and A=0 for full unaligned
access even when MMU is off. See ARM documentation
linked from that fn.
---
 nucleus/src/arch/aarch64/boot.rs | 20 ++++++++++++++++++++
 nucleus/src/main.rs              |  7 +++----
 2 files changed, 23 insertions(+), 4 deletions(-)

diff --git a/nucleus/src/arch/aarch64/boot.rs b/nucleus/src/arch/aarch64/boot.rs
index 447c04e..ba1c523 100644
--- a/nucleus/src/arch/aarch64/boot.rs
+++ b/nucleus/src/arch/aarch64/boot.rs
@@ -57,6 +57,24 @@ unsafe fn reset() -> ! {
     main()
 }
 
+// [ARMv6 unaligned data access restrictions](https://developer.arm.com/documentation/ddi0333/h/unaligned-and-mixed-endian-data-access-support/unaligned-access-support/armv6-unaligned-data-access-restrictions?lang=en)
+// dictates that compatibility bit U in CP15 must be set to 1 to allow Unaligned accesses while MMU is off.
+// (In addition to SCTLR_EL1.A being 0)
+// See also [CP15 C1 docs](https://developer.arm.com/documentation/ddi0290/g/system-control-coprocessor/system-control-processor-registers/c1--control-register).
+// #[link_section = ".text.boot"]
+// #[inline]
+// fn enable_armv6_unaligned_access() {
+//     unsafe {
+//         asm!(
+//             "mrc p15, 0, {u}, c1, c0, 0",
+//             "or {u}, {u}, {CR_U}",
+//             "mcr p15, 0, {u}, c1, c0, 0",
+//             u = out(reg) _,
+//             CR_U = const 1 << 22
+//         );
+//     }
+// }
+
 #[link_section = ".text.boot"]
 #[inline]
 fn shared_setup_and_enter_pre() {
@@ -79,6 +97,8 @@ fn shared_setup_and_enter_pre() {
             + SCTLR_EL1::SA0::Disable,
     );
 
+    // enable_armv6_unaligned_access();
+
     // Set Hypervisor Configuration Register (EL2)
     // Set EL1 execution state to AArch64
     // @todo Explain the SWIO bit (SWIO hardwired on Pi3)
diff --git a/nucleus/src/main.rs b/nucleus/src/main.rs
index 84be6fc..37373ba 100644
--- a/nucleus/src/main.rs
+++ b/nucleus/src/main.rs
@@ -75,7 +75,6 @@ fn print_mmu_state_and_features() {
 }
 
 fn init_mmu() {
-    print_mmu_state_and_features();
     unsafe {
         memory::mmu::init().unwrap();
     }
@@ -150,12 +149,12 @@ pub fn kmain() -> ! {
     #[cfg(feature = "jtag")]
     jtag::wait_debugger();
 
+    init_mmu();
+    init_exception_traps();
+
     #[cfg(not(feature = "noserial"))]
     init_uart_serial();
 
-    init_exception_traps();
-    init_mmu();
-
     #[cfg(test)]
     test_main();