Debugging works now in a very restricted way

src/cpu.rs

@@ -373,11 +373,11 @@ impl CPU {
             Instruction::LDI(ref r, v) => self.set_register(r, v),
             Instruction::SER(ref r) => self.set_register(r, 0xFF),
             Instruction::RJMP(v) => {
+                self.pc = self.pc.wrapping_add(v as _);
                 if v == -1 && self.test_flag(StatusFlag::GlobalInterruptEnable) == false {
                     info!(self.logger, "HALTED ");
                     return Err(CPUError::Exit);
                 }
-                self.pc = self.pc.wrapping_add(v as _);
             },
             Instruction::CLR_FLAG(v) => self.clear_flag(v),
             Instruction::SET_FLAG(v) => self.set_flag(v),

src/gdbstub.rs

@@ -1,4 +1,10 @@
 use std;
+use std::net::TcpListener;
+use std::io::{Read, Write};
+
+use slog;
+
+use cpu;
 
 #[derive(Debug)]
 pub struct GDBPacket {
@@ -87,6 +93,136 @@ impl GDBPacket {
     }
 }
 
+fn step(log: &slog::Logger, cpu: &mut cpu::CPU, rom: &mut [u8], ram: &mut [u8]) -> bool{
+    let r = cpu.step(rom, ram);
+    match r {
+        Ok(_) => {true}
+        Err(ref e) => {
+            warn!(log, "Error occured: {:?}", e);
+            false
+        }
+    }
+}
+
+fn stop_reply(cpu: &cpu::CPU) -> Vec<u8> {
+    format!("T0520:{:02X};21:{:04X};22:{:08X};",
+            cpu.sreg, 1337u16.swap_bytes(), (2 * cpu.pc).swap_bytes()
+    ).as_bytes().to_vec()
+}
+
+pub fn run(log: slog::Logger, cpu: &mut cpu::CPU, rom: &mut [u8], ram: &mut [u8]) {
+    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 GDBPacket::from_packet(&buf) {
+                Ok(_) => break 'a,
+                Err(_) => {}
+            }
+        }
+        let pkg = 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]).as_bytes().to_vec());
+            }
+            response.extend(format!("{:02X}", cpu.sreg).as_bytes().to_vec());
+            response.extend(format!("{:04X}", 0x1337u16.swap_bytes()).as_bytes().to_vec()); // TODO: SP
+            response.extend(format!("{:08X}", (2*cpu.pc).swap_bytes()).as_bytes().to_vec());
+        } else if pkg.raw_data[0] == b's' {
+            // TODO: Optional PC argument?
+            if !step(&log, cpu, rom, ram) {
+                // ERR
+            }
+            response = stop_reply(cpu);
+        } else if pkg.raw_data[0] == b'c' {
+            while step(&log, cpu, rom, ram) {}
+            response = stop_reply(cpu);
+        } else if pkg.raw_data[0] == b'm' {
+            // Read memory
+            let full_cmd = std::str::from_utf8(&pkg.raw_data).unwrap().chars().skip(1).collect::<String>();
+            let parts = full_cmd.split(",").collect::<Vec<_>>();
+            let mem_addr = usize::from_str_radix(parts[0], 16).unwrap();
+            let len = usize::from_str_radix(parts[1], 16).unwrap();
+            for i in 0..len {
+                // Copied from megumi again <3
+                // GDB uses a special addressing to allow addressing of flash, SRAM, etc.
+                //  - flash starts at 0x00000000
+                //  - SRAM starts at 0x00800000
+                //  - mask for memory space is 0x00f00000
+                // Constants used below are retrieved from GDB sources, in avr-tdep.c.
+                let addr = mem_addr.wrapping_add(i);
+                // TODO: Overflow checks.
+                if addr & 0x00f00000 > 0 {
+                    // RAM
+                    response.extend(format!("{:02X}", ram[addr & 0xFFFFF]).as_bytes().to_vec());
+                } else {
+                    // ROM
+                    response.extend(format!("{:02X}", rom[addr]).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 = 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;
+            }
+        }
+        */
+    }
+}
+
 /*
 fn main() {
     println!("Sample pkg: {:?}", String::from_utf8_lossy(&GDBPacket { raw_data: vec![]}.to_packet_format()));

src/main.rs

@@ -3,8 +3,6 @@ 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;
@@ -36,83 +34,7 @@ fn main() {
     let mut cpu = cpu::CPU::new(log.clone());
 
     info!(log, "Enabling GDB backend");
-    let listener = TcpListener::bind("0.0.0.0:1234").unwrap();
+    gdbstub::run(log.clone(), &mut cpu, &mut rom, &mut ram);
-    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();
 }