1 files changed, 105 insertions, 35 deletions

diff --git a/src/main.rs b/src/main.rs
index 45f567f..c276280 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -3,53 +3,106 @@ pub mod mem;
 pub mod state;
 
 use crate::core::*;
-use crate::mem::Memory;
+use crate::mem::{CpuMemory, LogicalMemory};
 use crate::state::CpuState;
-use std::io::Write;
 
 const MEMORY_SIZE: usize = 64 * 1024 * 1024;
 const REGISTER_NAME_WIDTH: u32 = 0b11111;
 const RO_TARGET: u32 = 0x80000000;
-const FDT_LOCATION: u32 = 0x90000000;
-const UART_ADDR: u32 = 0x10000000;
+const FDT_LOCATION: u32 = 0x83000000;
+
+const RINGBUF_ADDR: usize = 0x82000000;
+const RINGBUF_SIZE: usize = 0x100000;
+
+fn write_all(memory: &mut CpuMemory, value: &[u8], addr: u32, size: u32) {
+    let mut remaining_size = size;
+    let mut curr_addr = addr;
+    let mut idx = 0;
+    while remaining_size >= 4 {
+        memory
+            .write_word(
+                curr_addr,
+                u32::from_le_bytes(value[idx..(idx + 4)].try_into().unwrap()),
+            )
+            .unwrap();
+        remaining_size -= 4;
+        curr_addr += 4;
+        idx += 4;
+    }
+    while remaining_size >= 2 {
+        memory
+            .write_halfword(
+                curr_addr,
+                u16::from_le_bytes(value[idx..(idx + 2)].try_into().unwrap()),
+            )
+            .unwrap();
+        remaining_size -= 2;
+        curr_addr += 2;
+        idx += 2;
+    }
+    while remaining_size >= 1 {
+        memory.write(curr_addr, value[idx]).unwrap();
+        remaining_size -= 1;
+        curr_addr += 1;
+        idx += 1;
+    }
+}
+
+fn read_all(memory: &CpuMemory, addr: u32, size: u32) -> Vec<u8> {
+    let mut ret = Vec::new();
+    for a in addr..addr + size {
+        ret.push(memory.read(a).unwrap());
+    }
+    ret
+}
 
 fn main() {
     let path = std::path::PathBuf::from("wkern.bin");
     let f_dat = std::fs::read(path).expect("Unable to read file");
-
     let dtb_dat = std::fs::read(env!("DTB_PATH")).expect("Unable to read file");
 
-    let memory = Memory::new(MEMORY_SIZE);
+    let mut memory = CpuMemory {
+        memory: vec![0u8; MEMORY_SIZE].into_boxed_slice(),
+        devices: Vec::new(),
+    };
 
-    let mut state = CpuState::new(memory);
+    let ram_device = LogicalMemory {
+        mode: false,
+        asid: 0,
+        ppn: 0,
+        base: 0x80000000,
+    };
 
-    state
-        .memory
-        .write_multiple_bytes(RO_TARGET as usize, f_dat.as_slice(), 99)
-        .unwrap();
-    state
-        .memory
-        .write_multiple_bytes(FDT_LOCATION as usize, dtb_dat.as_slice(), 99)
-        .unwrap();
+    memory.register_device(
+        0x80000000..0x80000000 + MEMORY_SIZE as u32,
+        Box::new(ram_device),
+    );
 
-    state.memory.write(UART_ADDR as usize, 99, 0).unwrap();
-    state.pc = RO_TARGET;
+    let mut state = CpuState::new(memory);
+
+    write_all(
+        &mut state.memory,
+        f_dat.as_slice(),
+        RO_TARGET,
+        f_dat.len() as u32,
+    );
+    write_all(
+        &mut state.memory,
+        dtb_dat.as_slice(),
+        FDT_LOCATION,
+        dtb_dat.len() as u32,
+    );
+
+    state.pc = 0x80000000;
 
     state.registers[10] = 0;
     state.registers[11] = FDT_LOCATION;
 
+    println!("loaded!");
+
     loop {
         {
             handle_instruction(&mut state);
-
-            // simple polling uart
-            let uart_byte = state.memory.read(UART_ADDR as usize, 99).unwrap();
-            if uart_byte != 0 {
-                print!("{}", uart_byte as char);
-            }
-            state.memory.write(UART_ADDR as usize, 99, 0).unwrap();
-            std::io::stdout().flush().unwrap();
-
             state.registers[0] = 0;
         }
     }
@@ -57,8 +110,7 @@ fn main() {
 
 fn handle_instruction(state: &mut CpuState) {
     let pc = state.pc;
-    let instruction = state.memory.read_multiple_bytes(pc as usize, 4, 99);
-    let word = u32::from_le_bytes(instruction.unwrap()[0..4].try_into().unwrap());
+    let word = state.memory.read_word(pc).unwrap();
     let opcode = word & 0x7f;
     match opcode {
         0b0110011 => {
@@ -220,12 +272,15 @@ fn handle_instruction(state: &mut CpuState) {
 
             match f3 {
                 0 => {
-                    eprintln!(
-                        "TRAP! syscall {} with gp state {:?}",
-                        state.registers[17], pc
-                    );
-
-                    // todo: trap to CSR 0x305
+                    match imm {
+                        0 => trap_to_kernel(state),        // ecall
+                        258 => return_to_userspace(state), // sret
+                        261 => {
+                            println!("wfi");
+                            exit(state);
+                        }
+                        _ => panic!("Illegal instruction: {:x}", word),
+                    }
                 } // ecall
                 1 => csrrw(state, rd, rs1, imm as usize),
                 2 => csrrs(state, rd, rs1, imm as usize),
@@ -349,8 +404,23 @@ fn handle_instruction(state: &mut CpuState) {
             let rs1 = ((word >> 15) & REGISTER_NAME_WIDTH) as usize;
             fcvt_w_s(state, rd, rs1);
         }
-        _ => panic!("Illegal instruction or todo: {:x}", word),
+        _ => panic!("Illegal instruction or todo: {:032b}", word),
     }
 
     state.pc += 4;
 }
+
+fn trap_to_kernel(state: &mut CpuState) {
+    state.csr[0x141 /* sepc */] = state.pc;
+    state.pc = state.csr[0x105 /* stvec */];
+}
+
+fn return_to_userspace(state: &mut CpuState) {
+    state.pc = state.csr[0x141 /* sepc */] + 4;
+}
+
+fn exit(state: &mut CpuState) {
+    let data = read_all(&state.memory, RINGBUF_ADDR as u32, RINGBUF_SIZE as u32);
+    std::fs::write("core_dump.txt", data).unwrap();
+    std::process::exit(0);
+}