From b18b1be0fe8c1cea1605e5f99e2dba2a87fd0843 Mon Sep 17 00:00:00 2001
From: Derek Christ <christ.derek@gmail.com>
Date: Thu, 2 Nov 2023 18:47:34 +0100
Subject: [PATCH] Working memory map

---
 aarch64-gem5.ld | 17 ++++++++-----
 src/main.rs     |  8 +++++++
 src/start.s     | 63 +++++++++++++++++++++++++++++++++++++++++++++++--
 3 files changed, 80 insertions(+), 8 deletions(-)

diff --git a/aarch64-gem5.ld b/aarch64-gem5.ld
index cf7a578..b20f756 100644
--- a/aarch64-gem5.ld
+++ b/aarch64-gem5.ld
@@ -1,12 +1,17 @@
+MEMORY
+{
+    bootmem : ORIGIN = 0x0, LENGTH = 0x10000
+    dram : ORIGIN = 0x80000000, LENGTH = 0x100000000
+}
+
 ENTRY(_start)
 SECTIONS
 {
-    . = 0x80000000;
-    .text.boot : { *(.text.boot) }
-    .text : { *(.text) }
-    .data : { *(.data) }
-    .rodata : { *(.rodata) }
-    .bss : { *(.bss) }
+    .init : { *(.init) } > bootmem
+    .text : { KEEP(*(.text)) } > dram
+    .data : { *(.data) } > dram
+    .rodata : { *(.rodata) } > dram
+    .bss : { *(.bss) } > dram
 
     . = ALIGN(8);
     . = . + 0x8000;
diff --git a/src/main.rs b/src/main.rs
index 3595426..752badf 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -2,6 +2,7 @@
 #![no_main]
 
 use crate::uart::Uart0;
+use core::arch::asm;
 use core::sync::atomic::{self, Ordering};
 use core::{arch::global_asm, fmt::Write, panic::PanicInfo};
 
@@ -17,6 +18,13 @@ pub extern "C" fn entry() -> ! {
         writeln!(&mut uart, "Hello from Rust {i}!").unwrap();
     }
 
+    for val in 0..64 {
+        unsafe {
+            asm!("dc cvac, {val}", val = in(reg) &val);
+            asm!("dsb sy");
+        }
+    }
+
     loop {
         atomic::compiler_fence(Ordering::SeqCst);
     }
diff --git a/src/start.s b/src/start.s
index 283a554..bd8845f 100644
--- a/src/start.s
+++ b/src/start.s
@@ -1,12 +1,71 @@
 .globl _start
 .extern LD_STACK_PTR
 
-.section ".text.boot"
+// Put a 64-bit value with little endianness.
+.macro PUT_64B high, low
+    .word \low
+    .word \high
+.endm
+
+// Create an entry pointing to a next-level table.
+.macro TABLE_ENTRY PA, ATTR
+    PUT_64B \ATTR, (\PA) + 0x3
+.endm
+
+// Create an entry for a 1GB block.
+.macro BLOCK_1GB PA, ATTR_HI, ATTR_LO
+    PUT_64B \ATTR_HI | ((\PA) >> 32), ((\PA) & 0xC0000000) | \ATTR_LO | 0x1
+.endm
+
+// Create an entry for a 2MB block.
+.macro BLOCK_2MB PA, ATTR_HI, ATTR_LO
+    PUT_64B \ATTR_HI, ((\PA) & 0xFFE00000) | \ATTR_LO | 0x1
+.endm
+
+.section .init
+
+.align 12
+ttb0_base:
+.set ADDR, 0x000
+.rept 0x02
+BLOCK_1GB (ADDR << 29), 0, 0x740
+.set ADDR, ADDR+2
+.endr
+.rept 0x3E
+BLOCK_1GB (ADDR << 29), 0, 0x74C
+.set ADDR, ADDR+2
+.endr
 
 _start:
     ldr     x30, =LD_STACK_PTR
     mov     sp, x30
-    bl      entry
+
+    // Initialize translation table control registers
+    LDR X1, =0x13520 // 64GB space 4KB granularity Inner-shareable. Normal Inner and Outer Cacheable.
+    MSR TCR_EL3, X1
+
+    LDR X1, =0xFF440400
+    MSR MAIR_EL3, X1 // ATTR0 Device-nGnRnE ATTR1 Device. ATTR2 Normal Non-Cacheable. ATTR3 Normal Cacheable.
+
+    ADR X0, ttb0_base
+    MSR TTBR0_EL3, X0
+
+    // Enable MMU and caches
+    // It is implemented in the CPUECTLR register.
+    MRS X0, S3_1_C15_C2_1
+    ORR X0, X0, #(0x1 << 6) // The SMP bit.
+    MSR S3_1_C15_C2_1, X0
+
+    // Enable caches and the MMU.
+    MRS X0, SCTLR_EL3
+    ORR X0, X0, #(0x1 << 2) // The C bit (data cache).
+    ORR X0, X0, #(0x1 << 12) // The I bit (instruction cache).
+    ORR X0, X0, #0x1 // The M bit (MMU).
+    MSR SCTLR_EL3, X0
+    DSB SY
+    ISB
+
+    bl entry
 
 .equ PSCI_SYSTEM_OFF, 0x84000008
 .globl system_off