Implement a GDB stub and fix another huge issue in S-type immediate decoding

src/core.rs

@@ -4,10 +4,14 @@
 // This file is part of TRVE (https://gitea.taitep.se/taitep/trve)
 // See LICENSE file in the project root for full license text.
 
+use std::{collections::HashSet, sync::mpsc};
+
 use crate::{
     consts::{Addr, RegId, RegValue},
+    core::commands::CoreCmd,
     decode::Instruction,
     exceptions::ExceptionType,
+    gdb::{self, DebugCommand, StopReason},
     instructions::find_and_exec,
     mem::MemConfig,
 };
@@ -16,53 +20,154 @@ pub struct Core {
     pub(crate) x_regs: [RegValue; 32],
     pub(crate) pc: Addr,
     pub(crate) mem: MemConfig,
+    command_stream: mpsc::Receiver<CoreCmd>,
 }
 
+pub mod commands;
+
 impl Core {
-    pub fn new(mem: MemConfig) -> Self {
+    pub fn new(mem: MemConfig, command_stream: mpsc::Receiver<CoreCmd>) -> Self {
         Self {
             x_regs: [0; 32],
             pc: 0,
             mem,
+            command_stream,
         }
     }
 
     pub fn run(&mut self) {
         loop {
-            if !self.pc.is_multiple_of(4) {
-                self.throw_exception(ExceptionType::InstructionAddressMisaligned);
-                break;
+            if let Ok(cmd) = self.command_stream.try_recv() {
+                match cmd {
+                    CoreCmd::EnterDbgMode(dbg_stream) => {
+                        let _ = self.debug_loop(dbg_stream);
+                    }
+                };
             }
 
-            let instr = match self.mem.read_word(self.pc) {
-                Ok(i) => i,
-                Err(e) => {
-                    self.throw_exception(e.to_exception_instr());
-                    break;
-                }
-            };
-
-            if instr == 0 {
-                self.throw_exception(ExceptionType::IllegalInstruction);
-                break;
-            }
-
-            if instr & 3 != 3 {
-                // Compressed instruction - (currently) unsupported
-                self.throw_exception(ExceptionType::IllegalInstruction);
-                break;
-            }
-
-            let instr = Instruction(instr);
-
-            if let Err(e) = find_and_exec(instr, self) {
+            if let Err(e) = self.step() {
                 self.throw_exception(e);
-                eprintln!("instr: {:08x}", instr.0);
                 break;
             }
         }
     }
 
+    pub fn run_waiting_for_cmd(&mut self) {
+        eprintln!("Waiting for any core command...");
+        if let Ok(cmd) = self.command_stream.recv() {
+            eprintln!("Recieved a command");
+            match cmd {
+                CoreCmd::EnterDbgMode(dbg_stream) => {
+                    let _ = self.debug_loop(dbg_stream);
+                }
+            };
+        } else {
+            eprintln!("Error recieving command, starting anyway");
+        }
+
+        eprintln!("Command processed");
+
+        self.run();
+    }
+
+    fn debug_loop(&mut self, dbg_stream: mpsc::Receiver<gdb::DebugCommand>) -> anyhow::Result<()> {
+        let mut breakpoints = HashSet::new();
+
+        loop {
+            match dbg_stream.recv()? {
+                DebugCommand::GetRegs(sender) => sender.send(gdb::RegsResponse {
+                    x_regs: self.x_regs.clone(),
+                    pc: self.pc,
+                })?,
+                DebugCommand::ReadMem {
+                    addr,
+                    len,
+                    responder,
+                } => {
+                    let data = (0..len)
+                        .map(|offset| self.mem.read_byte(addr + offset))
+                        .collect();
+
+                    responder.send(data)?;
+                }
+                DebugCommand::SetBreakpoint(addr) => {
+                    breakpoints.insert(addr);
+                }
+                DebugCommand::RemoveBreakpoint(addr) => {
+                    breakpoints.remove(&addr);
+                }
+                DebugCommand::Step(responder) => {
+                    responder.send(match self.step() {
+                        Ok(_) => gdb::StopReason::Step,
+                        Err(e) => {
+                            self.throw_exception(e);
+                            gdb::StopReason::Exception(e)
+                        }
+                    })?;
+                }
+                DebugCommand::Continue(responder, stopper) => {
+                    responder.send(self.continue_loop(&breakpoints, stopper))?;
+                }
+                DebugCommand::ExitDebugMode => {
+                    eprintln!("exitdbgmode");
+                    break Ok(());
+                }
+            };
+        }
+    }
+
+    fn continue_loop(
+        &mut self,
+        breakpoints: &HashSet<Addr>,
+        stopper: oneshot::Receiver<()>,
+    ) -> StopReason {
+        loop {
+            if breakpoints.contains(&self.pc) {
+                return StopReason::Exception(ExceptionType::Breakpoint);
+            }
+
+            if let Ok(_) = stopper.try_recv() {
+                return StopReason::Interrupted;
+            }
+
+            if let Err(e) = self.step() {
+                self.throw_exception(e);
+                return StopReason::Exception(e);
+            }
+        }
+    }
+
+    pub(crate) fn step(&mut self) -> Result<(), ExceptionType> {
+        if !self.pc.is_multiple_of(4) {
+            self.throw_exception(ExceptionType::InstructionAddressMisaligned);
+        }
+
+        let instr = match self.mem.read_word(self.pc) {
+            Ok(i) => i,
+            Err(e) => {
+                return Err(e.to_exception_instr());
+            }
+        };
+
+        if instr == 0 {
+            return Err(ExceptionType::IllegalInstruction);
+        }
+
+        if instr & 3 != 3 {
+            // Compressed instruction - (currently) unsupported
+            return Err(ExceptionType::IllegalInstruction);
+        }
+
+        let instr = Instruction(instr);
+
+        if let Err(e) = find_and_exec(instr, self) {
+            eprintln!("instr: {:08x}", instr.0);
+            return Err(e);
+        }
+
+        Ok(())
+    }
+
     fn throw_exception(&mut self, exception_type: ExceptionType) {
         eprintln!("Exception: {exception_type:?}");
         dbg!(self.pc, self.x_regs);