fix memory issues and add basic device discovery

discuss: pci/dtb device discovery? now that the x0 issue is fixed it is
entirely possible.

3 files changed, 49 insertions, 16 deletions

diff --git a/src/core.rs b/src/core.rs
index 60d720b..6317c9e 100644
--- a/src/core.rs
+++ b/src/core.rs
@@ -168,7 +168,7 @@ pub fn lh(state: &mut CpuState, rd: usize, rs1: usize, imm: u32) {
 }
 
 pub fn lw(state: &mut CpuState, rd: usize, rs1: usize, imm: u32) {
-    let addr = (state.registers[rs1] + imm) as usize;
+    let addr = (state.registers[rs1].wrapping_add(imm)) as usize;
     let slice = state.memory.read_multiple_bytes(addr, 4, 99).unwrap();
     let number = u32::from_le_bytes(slice[0..4].try_into().unwrap());
 
@@ -224,42 +224,54 @@ pub fn sw(state: &mut CpuState, rs1: usize, rs2: usize, imm: u32) {
 
 pub fn beq(state: &mut CpuState, rs1: usize, rs2: usize, imm: u32) {
     if state.registers[rs1] == state.registers[rs2] {
-        state.pc += imm;
+        state.pc = state.pc.wrapping_add(imm);
+    } else {
+        state.pc += 4;
     }
 }
 
 pub fn bne(state: &mut CpuState, rs1: usize, rs2: usize, imm: u32) {
     if state.registers[rs1] != state.registers[rs2] {
-        state.pc += imm;
+        state.pc = state.pc.wrapping_add(imm);
+    } else {
+        state.pc += 4;
     }
 }
 
 pub fn blt(state: &mut CpuState, rs1: usize, rs2: usize, imm: u32) {
     if (state.registers[rs1] as i32) < (state.registers[rs2] as i32) {
-        state.pc += imm;
+        state.pc = state.pc.wrapping_add(imm);
+    } else {
+        state.pc += 4;
     }
 }
 
 pub fn bge(state: &mut CpuState, rs1: usize, rs2: usize, imm: u32) {
     if (state.registers[rs1] as i32) >= (state.registers[rs2] as i32) {
-        state.pc += imm;
+        state.pc = state.pc.wrapping_add(imm);
+    } else {
+        state.pc += 4;
     }
 }
 
 pub fn bltu(state: &mut CpuState, rs1: usize, rs2: usize, imm: u32) {
     if state.registers[rs1] < state.registers[rs2] {
-        state.pc += imm;
+        state.pc = state.pc.wrapping_add(imm);
+    } else {
+        state.pc += 4;
     }
 }
 
 pub fn bgeu(state: &mut CpuState, rs1: usize, rs2: usize, imm: u32) {
     if state.registers[rs1] >= state.registers[rs2] {
-        state.pc += imm;
+        state.pc = state.pc.wrapping_add(imm);
+    } else {
+        state.pc += 4;
     }
 }
 pub fn jal(state: &mut CpuState, rd: usize, imm: u32) {
     state.registers[rd] = state.pc + 4;
-    state.pc += imm;
+    state.pc = state.pc.wrapping_add(imm);
 }
 
 pub fn jalr(state: &mut CpuState, rd: usize, rs1: usize, imm: u32) {
diff --git a/src/main.rs b/src/main.rs
index 20dde55..bb1d735 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -5,10 +5,12 @@ pub mod state;
 use crate::core::*;
 use crate::mem::Memory;
 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 WDT_LOCATION: u32 = 0x90000000;
 const UART_ADDR: u32 = 0x10000000;
 
 fn main() {
@@ -23,9 +25,23 @@ fn main() {
         .memory
         .write_multiple_bytes(RO_TARGET as usize, f_dat.as_slice(), 99)
         .unwrap();
+    state
+        .memory
+        .write_multiple_bytes(WDT_LOCATION as usize, &[
+            65, 65, 103, 103, //0x67674141
+            1, 0, 0, 0, // 1 (n_devices)
+            0, 0, 0, 0, // 0 (device type, free RAM)
+            0, 0, 0, 0, // 0 (free RAM, no driver)
+            0, 0, 0, 16, // 0x10000000 (start addr)
+            0, 0, 0, 3 // 48 MB (size)
+        ], 99)
+        .unwrap();
+
     state.memory.write(UART_ADDR as usize, 99, 0).unwrap();
     state.pc = RO_TARGET;
 
+    state.registers[10] = WDT_LOCATION;
+
     loop {
         {
             handle_instruction(&mut state);
@@ -36,6 +52,9 @@ fn main() {
                 print!("{}", uart_byte as char);
             }
             state.memory.write(UART_ADDR as usize, 99, 0).unwrap();
+            std::io::stdout().flush().unwrap();
+
+            state.registers[0] = 0;
         }
     }
 }
@@ -221,7 +240,6 @@ fn handle_instruction(state: &mut CpuState) {
 
                 _ => panic!("Illegal instruction: {:x}", word),
             }
-
         } // ecall (ebreak not implemented)
         0b0101111 => {
             let f5 = word >> 27;
diff --git a/src/mem.rs b/src/mem.rs
index 7458c67..2043aa1 100644
--- a/src/mem.rs
+++ b/src/mem.rs
@@ -38,10 +38,11 @@ impl Memory {
                 return Some(i);
             }
         }
+        eprintln!("tried to allocate {:X} but there are no more pages!", virtual_addr);
         None
     }
 
-    pub fn lookup_page(&mut self, virtual_addr: usize, permission: usize) -> Option<usize> {
+    fn lookup_page(&mut self, virtual_addr: usize, permission: usize) -> Option<usize> {
         for (i, page) in self.pages.iter().enumerate() {
             if page.virtual_addr == virtual_addr
                 && permission >= page.permission
@@ -50,7 +51,7 @@ impl Memory {
                 return Some(i);
             }
         }
-        return self.create_page(virtual_addr, permission);
+        self.create_page(virtual_addr, permission)
     }
 
     pub fn ro_lookup(&self, virtual_addr: usize, permission: usize) -> Option<usize> {
@@ -62,6 +63,7 @@ impl Memory {
                 return Some(i);
             }
         }
+        println!("virtual_addr {:X} is not allocated", virtual_addr);
         None
     }
 
@@ -69,21 +71,22 @@ impl Memory {
         let page = address & PAGE_MASK;
         let offset = address - page;
 
-        if let Some(page) = self.lookup_page(address, permission) {
+        if let Some(page) = self.lookup_page(page, permission) {
             self.memory[self.pages[page].real_addr + offset] = value;
-            Ok(())
-        } else {
-            Err(())
+            return Ok(());
         }
+        Err(())
     }
 
     pub fn read(&self, address: usize, permission: usize) -> Result<u8, ()> {
         let page = address & PAGE_MASK;
         let offset = address - page;
 
-        if let Some(page) = self.ro_lookup(address, permission) {
+        if let Some(page) = self.ro_lookup(page, permission) {
             Ok(self.memory[self.pages[page].real_addr + offset])
         } else {
+            println!("address {:X} is not allocated", address);
+
             Err(())
         }
     }