First quick-and-dirty stub that allows gdb to attach

src/gdbstub.rs

@@ -0,0 +1,108 @@
+use std;
+
+#[derive(Debug)]
+pub struct GDBPacket {
+    pub raw_data: Vec<u8>,
+}
+
+/*
+At a minimum, a stub is required to support the ‘g’ and ‘G’ commands for register access, and the ‘m’ and ‘M’ commands for memory access.
+Stubs that only control single-threaded targets can implement run control with the ‘c’ (continue), and ‘s’ (step) commands.
+Stubs that support multi-threading targets should support the ‘vCont’ command. All other commands are optional. 
+*/
+
+#[derive(Debug)]
+pub enum GDBPacketError {
+    IncorrectChecksum,
+    InvalidFormat,
+}
+
+impl GDBPacket {
+    pub fn from_packet(data: &Vec<u8>) -> Result<GDBPacket, GDBPacketError> {
+        let mut is_escaped = false;
+        let mut in_checksum = false;
+        let mut decoded_data = Vec::new();
+        let mut checksum = [0u8; 2];
+        let mut checksum_idx = 0;
+        for (i, &v) in data.iter().enumerate() {
+            if i == 0 {
+                if v != b'$' {
+                    return Err(GDBPacketError::InvalidFormat);
+                }
+            } else if !in_checksum {
+                if v == b'#' && !is_escaped {
+                    in_checksum = true;
+                } else if is_escaped {
+                    decoded_data.push(v ^ 0x20);
+                    is_escaped = false;
+                } else if v == b'{' {
+                    is_escaped = true;
+                } else {
+                    decoded_data.push(v);
+                }
+            } else if checksum_idx < 2 {
+                checksum[checksum_idx] = v;
+                checksum_idx += 1;
+            } else {
+                return Err(GDBPacketError::IncorrectChecksum);
+            }
+        }
+
+        let checksum = u8::from_str_radix(
+            std::str::from_utf8(&checksum).map_err(|_| (GDBPacketError::InvalidFormat))?, 16)
+        .map_err(|_| (GDBPacketError::InvalidFormat))?;
+        let p = GDBPacket {
+            raw_data: decoded_data.clone()
+        };
+
+        if p.gen_checksum() == checksum {
+            Ok(p)
+        } else {
+            Err(GDBPacketError::IncorrectChecksum)
+        }
+    }
+
+    fn gen_checksum(&self) -> u8 {
+        self.raw_data.iter().fold(0u8, |r, v| r.wrapping_add(*v))
+    }
+
+    fn escaped_data(&self) -> Vec<u8> {
+        let mut r = Vec::new();
+        for e in self.raw_data.iter() {
+            match *e {
+                b'$' | b'#' | b'}' | b'*' => { r.push(b'}'); r.push(*e ^ 0x20); }
+                _ => r.push(*e)
+            }
+        }
+        r
+    }
+
+    pub fn to_packet_format(&self) -> Vec<u8> {
+        let mut r = Vec::new();
+        r.push(b'$');
+        r.extend(self.escaped_data());
+        r.push(b'#');
+        r.extend(format!("{:02X}", self.gen_checksum()).as_bytes());
+        r
+    }
+}
+
+/*
+fn main() {
+    println!("Sample pkg: {:?}", String::from_utf8_lossy(&GDBPacket { raw_data: vec![]}.to_packet_format()));
+    let pkg = GDBPacket::from_packet(&"$#00".as_bytes().iter().map(|v| *v).collect::<Vec<_>>()).expect("Could not parse pkg");
+
+    // Handler for g: Read general registers. 
+    // Each byte of register data is described by two hex digits. The bytes with the register are transmitted in target byte order. The size of each register and their position within the ‘g’ packet are determined by the GDB internal gdbarch functions DEPRECATED_REGISTER_RAW_SIZE and gdbarch_register_name. 
+    // From megumi: 35 value,s total 39 bytes
+    // 0-31: Regular regs
+    // 33 SREG
+    // 34-35 SP
+    // 36-39 PC
+    let get_reg_response = GDBPacket { raw_data: [0u8; 39].to_vec() };
+    // G: same but set
+    // m M
+    // m: addr:hex,len:hex
+    // s c
+}
+*/

src/main.rs

@@ -3,20 +3,22 @@ use std::path::Path;
 use std::io;
 use std::io::{Read, Write};
 use std::fs;
+use std::net::{TcpListener, TcpStream};
+use std::io::prelude::*;
 
 #[macro_use]
 extern crate slog;
-extern crate slog_term;
 extern crate slog_async;
+extern crate slog_term;
 
 use slog::Drain;
 
-
 mod cpu;
 mod regs;
 mod decoder;
 mod chip;
 mod chip_definitions;
+mod gdbstub;
 
 fn main() {
     let decorator = slog_term::PlainDecorator::new(std::io::stdout());
@@ -25,24 +27,91 @@ fn main() {
 
     let log = slog::Logger::root(
         slog::LevelFilter::new(drain, slog::Level::Info).fuse(),
-        o!("version" => "0.1")
+        o!("version" => "0.1"),
     );
     info!(log, "AVREmu starting up");
 
-    let mut rom = read_file(
-        std::env::args().nth(1).unwrap_or("rom.bin".to_string())).unwrap();
+    let mut rom = read_file(std::env::args().nth(1).unwrap_or("rom.bin".to_string())).unwrap();
     let mut ram = [0u8; 8 * 1024 + 8 * 1024];
     let mut cpu = cpu::CPU::new(log.clone());
 
+    info!(log, "Enabling GDB backend");
+    let listener = TcpListener::bind("0.0.0.0:1234").unwrap();
+    let mut conn = listener.incoming().nth(0).unwrap().unwrap();
+    info!(log, "Debugger? attached.");
+
     loop {
+        let mut buf = Vec::new();
+        'a: loop {
+            let mut s = [0u8];
+            if conn.read(&mut s).unwrap() != 1 {
+                panic!("Connection closed?")
+            }
+            if buf.len() == 0 && s[0] != b'$' {
+                // Wait for beginning of pkg.
+                continue;
+            }
+            buf.push(s[0]);
+            // warn!(log, "{:?}", std::str::from_utf8(&buf));
+            match gdbstub::GDBPacket::from_packet(&buf) {
+                Ok(_) => break 'a,
+                Err(_) => {}
+            }
+        }
+        let pkg = gdbstub::GDBPacket::from_packet(&buf).unwrap();
+        // warn!(log, "Got pkg: {:?}", pkg);
+        warn!(log, "<- {:?}", std::str::from_utf8(&pkg.raw_data));
+        // Send ACK
+        {
+            let s = [b'+'];
+            if conn.write(&s).unwrap() != 1 {
+                panic!("Connection closed?");
+            }
+        }
+
+        let mut response = Vec::new();
+
+        if pkg.raw_data[0] == b'q' {
+            let query = std::str::from_utf8(&pkg.raw_data).unwrap();
+            info!(log, "Got query: {}", query);
+            // TODO: Let's do this right :(
+            if query.chars().skip(1).take(9).collect::<String>() == "Supported" {
+                response = "PacketSize=1024".as_bytes().to_vec();
+            } else if pkg.raw_data[1] == b'C' {
+                response = "0".as_bytes().to_vec();
+            } else {
+                // panic!("Unknown query");
+            }
+        } else if pkg.raw_data[0] == b'H' {
+            // TODO: Make sure we have the right stuff here.
+            response = "OK".as_bytes().to_vec();
+        } else if pkg.raw_data[0] == b'?' {
+            response = "S05".as_bytes().to_vec();
+        } else if pkg.raw_data[0] == b'g' {
+            for i in 0..32 {
+                response.extend(format!("{:02X}", cpu.registers[i] ^ (i as u8)).as_bytes().to_vec());
+            }
+            response.extend(format!("{:02X}", cpu.sreg).as_bytes().to_vec());
+            response.extend(format!("{:04X}", 0x1337).as_bytes().to_vec()); // TODO: SP
+            response.extend(format!("{:08X}", cpu.pc).as_bytes().to_vec());
+        } else {
+            info!(log, "Unknown cmd: {}", pkg.raw_data[0]);
+            // Unknown
+        }
+
+        info!(log, "Send our response: {:?}", std::str::from_utf8(&response).unwrap());
+        let response = gdbstub::GDBPacket { raw_data: response };
+        conn.write_all(&response.to_packet_format()).unwrap();
+        /*
         let r = cpu.step(&mut rom, &mut ram);
         match r {
             Ok(_) => {}
             Err(ref e) => {
                 warn!(log, "Error occured: {:?}", e);
                 break;
-            },
+            }
         }
+        */
     }
     warn!(log, "{}", cpu);
     write_file("ram.dmp", &ram).unwrap();