Handle one message

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<OwnedReadHalf>,
     stream_out: BufWriter<OwnedWriteHalf>,
+    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<dyn Error>> {
+    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?;
+            let message = self.read_message().await?;
-            println!("{byte}");
+            println!("received {:?}", message);
-            self.stream_out.write_u8(byte).await?;
+
+            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<Message> {
+        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<u8> {
         self.stream_in.read_u8().await
     }
 }
+
+struct ResponseWriter<'a> {
+    inner: &'a mut BufWriter<OwnedWriteHalf>,
+    checksum: u8,
+}
+impl<'a> ResponseWriter<'a> {
+    fn new(inner: &'a mut BufWriter<OwnedWriteHalf>) -> 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,
+}