diff --git a/src/gdbserver.rs b/src/gdbserver.rs index e763d1a..a449fe8 100644 --- a/src/gdbserver.rs +++ b/src/gdbserver.rs @@ -1,5 +1,5 @@ +use anyhow::{bail, Result}; use std::{ - error::Error, sync::{Arc, Mutex}, thread, }; @@ -126,6 +126,7 @@ struct GdbConnection { client: EmulatorClient, stream_in: BufReader, stream_out: BufWriter, + ack_messages: bool, } impl GdbConnection { @@ -136,19 +137,143 @@ impl GdbConnection { client, stream_in: BufReader::new(rx), stream_out: BufWriter::new(tx), + ack_messages: true, } } - async fn run(mut self) -> Result<(), Box> { + async fn run(mut self) -> Result<()> { println!("Connected for {}", self.sim_id); self.client.send_command(EmulatorCommand::Resume); loop { - let byte = self.read_byte().await?; - println!("{byte}"); - self.stream_out.write_u8(byte).await?; + let message = self.read_message().await?; + println!("received {:?}", message); + + let mut res = ResponseWriter::new(&mut self.stream_out); + res.init(self.ack_messages).await?; + + let body = match &message { + Message::String(str) => str.as_str(), + Message::Signal => { + // TODO: handle this + res.send().await?; + continue; + } + }; + + if body == "QStartNoAckMode" { + self.ack_messages = false; + res.send_ok().await?; + } else { + // unrecognized command + res.send().await?; + } } } + async fn read_message(&mut self) -> Result { + let mut char = self.read_byte().await?; + while char == b'+' { + // just ignore positive acks + char = self.read_byte().await?; + } + if char == b'-' { + bail!("no support for negative acks"); + } + if char == 0x03 { + // This is how the client "cancels an in-flight request" + return Ok(Message::Signal); + } + if char != b'$' { + // Messages are supposed to start with a dollar sign + bail!("malformed message"); + } + + // now read the body + let mut checksum = 0u8; + let mut body = vec![]; + char = self.read_byte().await?; + while char != b'#' { + if char == b'}' { + // escape character + checksum = checksum.wrapping_add(char); + char = self.read_byte().await?; + checksum = checksum.wrapping_add(char); + body.push(char ^ 0x20); + } else { + checksum = checksum.wrapping_add(char); + body.push(char); + } + char = self.read_byte().await?; + } + + let mut checksum_bytes = [b'0'; 2]; + self.stream_in.read_exact(&mut checksum_bytes).await?; + let checksum_str = std::str::from_utf8(&checksum_bytes)?; + let real_checksum = u8::from_str_radix(checksum_str, 16)?; + if checksum != real_checksum { + bail!("invalid checksum"); + } + + let string = String::from_utf8(body)?; + Ok(Message::String(string)) + } + async fn read_byte(&mut self) -> std::io::Result { self.stream_in.read_u8().await } } + +struct ResponseWriter<'a> { + inner: &'a mut BufWriter, + checksum: u8, +} +impl<'a> ResponseWriter<'a> { + fn new(inner: &'a mut BufWriter) -> Self { + Self { inner, checksum: 0 } + } + + async fn init(&mut self, ack: bool) -> std::io::Result<()> { + if ack { + self.inner.write_u8(b'+').await?; + } + self.inner.write_u8(b'$').await + } + + async fn write_str(&mut self, str: &str) -> std::io::Result<()> { + for byte in str.bytes() { + self.checksum = self.checksum.wrapping_add(byte); + } + self.inner.write_all(str.as_bytes()).await + } + + async fn write_hex_u8(&mut self, value: u8) -> std::io::Result<()> { + for digit in [(value >> 4), (value & 0xf)] { + let char = if digit > 9 { + b'a' + digit - 10 + } else { + b'0' + digit + }; + self.checksum = self.checksum.wrapping_add(char); + self.inner.write_u8(char).await?; + } + Ok(()) + } + + async fn send_ok(mut self) -> std::io::Result<()> { + self.write_str("OK").await?; + self.send().await + } + + async fn send(mut self) -> std::io::Result<()> { + let final_checksum = self.checksum; + self.inner.write_u8(b'#').await?; + self.write_hex_u8(final_checksum).await?; + println!("{:?}", std::str::from_utf8(self.inner.buffer())); + self.inner.flush().await + } +} + +#[derive(Debug)] +enum Message { + String(String), + Signal, +}