diff --git a/src/cpu.rs b/src/cpu.rs index c041bad..ce0515d 100644 --- a/src/cpu.rs +++ b/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), diff --git a/src/gdbstub.rs b/src/gdbstub.rs index a0e1a00..b903198 100644 --- a/src/gdbstub.rs +++ b/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 { + 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::() == "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::(); + let parts = full_cmd.split(",").collect::>(); + 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())); diff --git a/src/main.rs b/src/main.rs index c661e71..3261498 100644 --- a/src/main.rs +++ b/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(); - 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::() == "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; - } - } - */ - } + gdbstub::run(log.clone(), &mut cpu, &mut rom, &mut ram); warn!(log, "{}", cpu); write_file("ram.dmp", &ram).unwrap(); }