trve/src/gdb.rs

// Copyright (c) 2025 taitep
// SPDX-License-Identifier: BSD-2-Clause
//
// 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::{
    io::{self, BufRead, ErrorKind, Write},
    net::{TcpListener, TcpStream},
    sync::mpsc,
};

use crate::{
    core::commands::CoreCmd,
    exceptions::{ExceptionType, MemoryExceptionType},
};

pub(crate) enum DebugCommand {
    GetRegs(oneshot::Sender<RegsResponse>),
    ReadMem {
        addr: u64,
        len: u64,
        responder: oneshot::Sender<Result<Vec<u8>, MemoryExceptionType>>,
    },
    Step(oneshot::Sender<StopReason>),
    Continue(oneshot::Sender<StopReason>, oneshot::Receiver<()>),
    SetBreakpoint(u64),
    RemoveBreakpoint(u64),
    ExitDebugMode,
}

pub struct DebugStream(pub(crate) mpsc::Receiver<DebugCommand>);

#[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: [u64; 32],
    pub pc: u64,
}

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(DebugStream(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(())
}