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

7 files changed, 478 insertions, 33 deletions

diff --git a/src/core.rs b/src/core.rs
index acc52f2..dde8fd7 100644
--- a/src/core.rs
+++ b/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);
+            if let Ok(cmd) = self.command_stream.try_recv() {
+                match cmd {
+                    CoreCmd::EnterDbgMode(dbg_stream) => {
+                        let _ = self.debug_loop(dbg_stream);
+                    }
+                };
+            }
+
+            if let Err(e) = self.step() {
+                self.throw_exception(e);
                 break;
             }
+        }
+    }
 
-            let instr = match self.mem.read_word(self.pc) {
-                Ok(i) => i,
-                Err(e) => {
-                    self.throw_exception(e.to_exception_instr());
-                    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");
+        }
 
-            if instr == 0 {
-                self.throw_exception(ExceptionType::IllegalInstruction);
-                break;
-            }
+        eprintln!("Command processed");
 
-            if instr & 3 != 3 {
-                // Compressed instruction - (currently) unsupported
-                self.throw_exception(ExceptionType::IllegalInstruction);
-                break;
+        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);
             }
 
-            let instr = Instruction(instr);
+            if let Ok(_) = stopper.try_recv() {
+                return StopReason::Interrupted;
+            }
 
-            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;
+                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);
diff --git a/src/core/commands.rs b/src/core/commands.rs
new file mode 100644
index 0000000..93bdd1e
--- /dev/null
+++ b/src/core/commands.rs
@@ -0,0 +1,7 @@
+use std::sync::mpsc;
+
+use crate::gdb;
+
+pub enum CoreCmd {
+    EnterDbgMode(mpsc::Receiver<gdb::DebugCommand>),
+}
diff --git a/src/decode.rs b/src/decode.rs
index 4258a6f..d2e8a80 100644
--- a/src/decode.rs
+++ b/src/decode.rs
@@ -57,7 +57,9 @@ impl Instruction {
 
     #[inline]
     pub fn imm_s(self) -> DWord {
-        (self.0 as i32 as i64 >> (25 - 5) & (0x7f << 5)) as DWord | (self.0 >> 7 & 0b11111) as DWord
+        let imm_11_5 = (self.0 as i32 as i64 >> 25 << 5) as DWord;
+        let imm_4_0 = (self.0 >> 7 & 0x1f) as DWord;
+        imm_11_5 | imm_4_0
     }
 
     #[inline]
diff --git a/src/exceptions.rs b/src/exceptions.rs
index 7f00e5c..3c1d4bd 100644
--- a/src/exceptions.rs
+++ b/src/exceptions.rs
@@ -29,6 +29,7 @@ pub enum ExceptionType {
     HardwareError = 19,
 }
 
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
 pub enum MemoryExceptionType {
     AddressMisaligned,
     AccessFault,
diff --git a/src/gdb.rs b/src/gdb.rs
new file mode 100644
index 0000000..17e2f71
--- /dev/null
+++ b/src/gdb.rs
@@ -0,0 +1,315 @@
+use std::{
+    io::{self, BufRead, ErrorKind, Write},
+    net::{TcpListener, TcpStream},
+    sync::mpsc,
+};
+
+use crate::{
+    consts::{Addr, RegValue},
+    core::commands::CoreCmd,
+    exceptions::{ExceptionType, MemoryExceptionType},
+};
+
+pub(crate) enum DebugCommand {
+    GetRegs(oneshot::Sender<RegsResponse>),
+    ReadMem {
+        addr: Addr,
+        len: u64,
+        responder: oneshot::Sender<Result<Vec<u8>, MemoryExceptionType>>,
+    },
+    Step(oneshot::Sender<StopReason>),
+    Continue(oneshot::Sender<StopReason>, oneshot::Receiver<()>),
+    SetBreakpoint(Addr),
+    RemoveBreakpoint(Addr),
+    ExitDebugMode,
+}
+
+#[derive(Clone, Copy, Debug)]
+pub(crate) enum StopReason {
+    Exception(ExceptionType),
+    Step,
+    Interrupted,
+}
+
+impl StopReason {
+    fn to_rsp(self) -> &'static str {
+        match self {
+            StopReason::Step => "S05",
+            StopReason::Exception(e) => {
+                use ExceptionType::*;
+                match e {
+                    IllegalInstruction => "S04",
+                    InstructionAddressMisaligned
+                    | InstructionAccessFault
+                    | InstructionPageFault
+                    | LoadAddressMisaligned
+                    | LoadAccessFault
+                    | LoadPageFault
+                    | StoreAmoAddressMisaligned
+                    | StoreAmoAccessFault
+                    | StoreAmoPageFault => "S0b",
+                    _ => "S05",
+                }
+            }
+            StopReason::Interrupted => "S02",
+        }
+    }
+}
+
+pub(crate) struct RegsResponse {
+    pub x_regs: [RegValue; 32],
+    pub pc: Addr,
+}
+
+pub fn run_stub(cmd_sender: mpsc::Sender<CoreCmd>) {
+    std::thread::spawn(move || {
+        let listener = TcpListener::bind("127.0.0.1:1234").expect("couldnt start tcp listener");
+
+        for stream_res in listener.incoming() {
+            if let Ok(stream) = stream_res {
+                let (dbg_tx, dbg_rx) = mpsc::channel();
+
+                stream
+                    .set_nonblocking(true)
+                    .expect("Couldnt set TCP stream to nonblocking");
+
+                cmd_sender
+                    .send(CoreCmd::EnterDbgMode(dbg_rx))
+                    .expect("couldnt ask core to enter debug mode");
+
+                handle_gdb_connection(stream, dbg_tx).expect("failure during connection");
+            }
+        }
+    });
+}
+
+fn handle_gdb_connection(
+    gdb_stream: TcpStream,
+    dbg_tx: mpsc::Sender<DebugCommand>,
+) -> io::Result<()> {
+    eprintln!("gdb connected");
+    let mut reader = io::BufReader::new(gdb_stream.try_clone()?);
+    let mut writer = gdb_stream;
+
+    loop {
+        match read_rsp_packet(&mut reader) {
+            Ok(packet) => match handle_packet(
+                &packet[..packet.len() - 1],
+                &mut writer,
+                &dbg_tx,
+                &mut reader,
+            ) {
+                Err(_) => {
+                    let _ = dbg_tx.send(DebugCommand::ExitDebugMode);
+                    break;
+                }
+                _ => {}
+            },
+            Err(ref e) if e.kind() == ErrorKind::WouldBlock => {
+                std::thread::yield_now();
+            }
+            Err(_) => {
+                let _ = dbg_tx.send(DebugCommand::ExitDebugMode);
+                break;
+            }
+        }
+    }
+
+    Ok(())
+}
+
+fn read_rsp_packet<R: BufRead>(reader: &mut R) -> io::Result<String> {
+    let mut buf = Vec::new();
+
+    // Wait for leading '$'
+    loop {
+        let mut byte = [0u8];
+        let n = reader.read(&mut byte)?;
+        if n == 0 {
+            return Err(io::Error::new(ErrorKind::UnexpectedEof, "Disconnected"));
+        }
+        if byte[0] == b'$' {
+            break;
+        }
+    }
+
+    // Read until '#'
+    reader.read_until(b'#', &mut buf)?;
+
+    let mut checksum = [0u8; 2];
+    reader.read_exact(&mut checksum)?;
+
+    String::from_utf8(buf).map_err(|e| io::Error::new(ErrorKind::InvalidData, e))
+}
+
+fn handle_packet<W: Write, R: BufRead>(
+    packet: &str,
+    writer: &mut W,
+    dbg_tx: &mpsc::Sender<DebugCommand>,
+    reader: &mut R,
+) -> io::Result<()> {
+    writer.write_all(b"+")?;
+    if packet.is_empty() {
+        send_packet("", writer)?;
+        return Ok(());
+    }
+
+    match &packet[0..1] {
+        "?" => {
+            send_packet("S05", writer)?;
+        }
+
+        "g" => {
+            let (regs_tx, regs_rx) = oneshot::channel();
+            dbg_tx.send(DebugCommand::GetRegs(regs_tx)).unwrap();
+            let regs = regs_rx.recv().unwrap();
+            let mut hex = String::with_capacity(32 * 8 * 2 + 8 * 2);
+            for &reg in &regs.x_regs {
+                hex.push_str(
+                    &reg.to_le_bytes()
+                        .iter()
+                        .map(|b| format!("{b:02x}"))
+                        .collect::<String>(),
+                );
+            }
+            hex.push_str(
+                &regs
+                    .pc
+                    .to_le_bytes()
+                    .iter()
+                    .map(|b| format!("{b:02x}"))
+                    .collect::<String>(),
+            );
+            send_packet(&hex, writer)?;
+        }
+
+        "m" => {
+            if let Some((addr_str, len_str)) = packet[1..].split_once(',') {
+                if let (Ok(addr), Ok(len)) = (
+                    u64::from_str_radix(addr_str, 16),
+                    u64::from_str_radix(len_str, 16),
+                ) {
+                    let (responder, response) = oneshot::channel();
+                    dbg_tx
+                        .send(DebugCommand::ReadMem {
+                            addr,
+                            len,
+                            responder,
+                        })
+                        .unwrap();
+                    let response = response.recv().unwrap();
+
+                    match response {
+                        Ok(data) => {
+                            let hex: String = data.iter().map(|b| format!("{b:02x}")).collect();
+                            send_packet(&hex, writer)?;
+                        }
+                        Err(e) => send_packet(&format!("E.{e:?}"), writer)?,
+                    };
+                } else {
+                    send_packet("", writer)?;
+                }
+            } else {
+                send_packet("", writer)?;
+            }
+        }
+
+        "s" => {
+            let (responder, stop_reason_rx) = oneshot::channel();
+            dbg_tx.send(DebugCommand::Step(responder)).unwrap();
+            let stop_reason = stop_reason_rx.recv().unwrap();
+            send_packet(&stop_reason.to_rsp(), writer)?;
+        }
+
+        "c" => {
+            let (responder, stop_reason_rx) = oneshot::channel();
+            let (stopper, stop_listener) = oneshot::channel();
+            dbg_tx
+                .send(DebugCommand::Continue(responder, stop_listener))
+                .unwrap();
+
+            loop {
+                let mut byte = [0u8];
+                match reader.read(&mut byte) {
+                    Ok(0) => {
+                        stopper.send(()).unwrap();
+                        break;
+                    }
+                    Ok(1) if byte[0] == 0x03 => {
+                        stopper.send(()).unwrap();
+                        break;
+                    }
+                    Ok(_) => {}
+                    Err(ref e) if e.kind() == io::ErrorKind::WouldBlock => {}
+                    Err(e) => {
+                        return Err(e);
+                    }
+                }
+
+                if let Ok(stop_reason) = stop_reason_rx.try_recv() {
+                    send_packet(&stop_reason.to_rsp(), writer)?;
+                    return Ok(());
+                }
+
+                std::thread::yield_now();
+            }
+
+            let stop_reason = stop_reason_rx.recv().unwrap();
+            send_packet(stop_reason.to_rsp(), writer)?;
+        }
+
+        "Z" if packet.chars().nth(1) == Some('0') => {
+            if let Some((addr_str, size_str)) = packet[3..].split_once(',') {
+                if let (Ok(addr), Ok(size)) = (
+                    u64::from_str_radix(addr_str, 16),
+                    u64::from_str_radix(size_str, 16),
+                ) {
+                    if size != 4 {
+                        send_packet("", writer)?;
+                        return Ok(());
+                    }
+
+                    dbg_tx.send(DebugCommand::SetBreakpoint(addr)).unwrap();
+                    send_packet("OK", writer)?;
+                    return Ok(());
+                }
+            }
+            send_packet("", writer)?;
+        }
+
+        "z" if packet.chars().nth(1) == Some('0') => {
+            if let Some((addr_str, size_str)) = packet[3..].split_once(',') {
+                if let (Ok(addr), Ok(size)) = (
+                    u64::from_str_radix(addr_str, 16),
+                    u64::from_str_radix(size_str, 16),
+                ) {
+                    if size != 4 {
+                        send_packet("", writer)?;
+                        return Ok(());
+                    }
+
+                    dbg_tx.send(DebugCommand::RemoveBreakpoint(addr)).unwrap();
+                    send_packet("OK", writer)?;
+                    return Ok(());
+                }
+            }
+            send_packet("", writer)?;
+        }
+
+        _ => {
+            send_packet("", writer)?;
+        }
+    }
+
+    Ok(())
+}
+
+fn send_packet<W: Write>(packet: &str, writer: &mut W) -> io::Result<()> {
+    writer.write_all(b"$")?;
+    writer.write_all(packet.as_bytes())?;
+    writer.write_all(b"#")?;
+    let checksum = packet.bytes().fold(0u8, |acc, b| acc.wrapping_add(b));
+    write!(writer, "{checksum:02x}")?;
+    // eprintln!("successfully sent packet {packet:?}");
+    Ok(())
+}
diff --git a/src/lib.rs b/src/lib.rs
index 7a9ac37..6eda620 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -2,5 +2,6 @@ pub mod consts;
 pub mod core;
 mod decode;
 pub mod exceptions;
+pub mod gdb;
 mod instructions;
 pub mod mem;
diff --git a/src/main.rs b/src/main.rs
index 03072f6..274ef02 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -4,33 +4,38 @@
 // 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::{env, sync::Arc, time::Duration};
+use std::{env, path::PathBuf, sync::Arc, time::Duration};
+
+use clap::Parser;
 
 use trve::{
     consts::{Addr, Byte, DWord, HWord, Word},
     core::Core,
     exceptions::MemoryExceptionType,
+    gdb,
     mem::{MemConfig, MemDeviceInterface, MmioRoot, Ram},
 };
 
-use anyhow::{Result, bail};
+use anyhow::Result;
 
 use crate::basic_uart::BasicUart;
 
 mod execload;
 
+#[derive(Parser)]
+struct Args {
+    executable: PathBuf,
+    #[arg(long)]
+    wait: bool,
+}
+
 fn main() -> Result<()> {
+    let args = Args::parse();
+
     let mut ram = Ram::try_new(16 * 1024 * 1024)?;
     let buf = ram.buf_mut();
 
-    let args: Vec<String> = env::args().collect();
-
-    if args.len() != 2 {
-        eprintln!("USAGE: trve <ram_image>");
-        bail!("Wrong number of arguments");
-    }
-
-    let entry_point = execload::load(&args[1], buf)?;
+    let entry_point = execload::load(args.executable, buf)?;
 
     let mut mmio_root = MmioRoot::default();
     mmio_root.insert(0, Arc::new(DbgOut));
@@ -44,9 +49,18 @@ fn main() -> Result<()> {
         mmio_root,
     };
 
-    let mut core = Core::new(mem_cfg);
+    let (cmd_sender, cmd_reciever) = std::sync::mpsc::channel();
+
+    gdb::run_stub(cmd_sender.clone());
+
+    let mut core = Core::new(mem_cfg, cmd_reciever);
     core.reset(entry_point);
-    core.run();
+
+    if args.wait {
+        core.run_waiting_for_cmd();
+    } else {
+        core.run();
+    }
 
     Ok(())
 }