From 0fff4d427f7551bf14dafe948b552a94b385495a Mon Sep 17 00:00:00 2001
From: Simon Gellis <simongellis@gmail.com>
Date: Sat, 18 Jan 2025 00:20:57 -0500
Subject: [PATCH] Implement writing registers

---
 src/emulator.rs                 |  7 +++++++
 src/emulator/shrooms_vb_core.rs | 20 ++++++++++++++++++++
 src/gdbserver.rs                | 26 ++++++++++++++++++++++++++
 src/gdbserver/request.rs        | 14 ++++++++++++++
 4 files changed, 67 insertions(+)

diff --git a/src/emulator.rs b/src/emulator.rs
index aa065b0..b0d3d61 100644
--- a/src/emulator.rs
+++ b/src/emulator.rs
@@ -537,6 +537,12 @@ impl Emulator {
                 let value = sim.read_register(register);
                 let _ = done.send(value);
             }
+            EmulatorCommand::WriteRegister(sim_id, register, value) => {
+                let Some(sim) = self.sims.get_mut(sim_id.to_index()) else {
+                    return;
+                };
+                sim.write_register(register, value);
+            }
             EmulatorCommand::ReadMemory(sim_id, start, length, mut buffer, done) => {
                 let Some(sim) = self.sims.get_mut(sim_id.to_index()) else {
                     return;
@@ -631,6 +637,7 @@ pub enum EmulatorCommand {
     DebugContinue(SimId),
     DebugStep(SimId),
     ReadRegister(SimId, VBRegister, oneshot::Sender<u32>),
+    WriteRegister(SimId, VBRegister, u32),
     ReadMemory(SimId, u32, usize, Vec<u8>, oneshot::Sender<Vec<u8>>),
     AddBreakpoint(SimId, u32),
     RemoveBreakpoint(SimId, u32),
diff --git a/src/emulator/shrooms_vb_core.rs b/src/emulator/shrooms_vb_core.rs
index 208f86a..773114f 100644
--- a/src/emulator/shrooms_vb_core.rs
+++ b/src/emulator/shrooms_vb_core.rs
@@ -145,6 +145,10 @@ extern "C" {
     fn vb_set_option(sim: *mut VB, key: VBOption, value: c_int);
     #[link_name = "vbSetPeer"]
     fn vb_set_peer(sim: *mut VB, peer: *mut VB);
+    #[link_name = "vbSetProgramCounter"]
+    fn vb_set_program_counter(sim: *mut VB, value: u32) -> u32;
+    #[link_name = "vbSetProgramRegister"]
+    fn vb_set_program_register(sim: *mut VB, index: c_uint, value: i32) -> i32;
     #[link_name = "vbSetReadCallback"]
     fn vb_set_read_callback(sim: *mut VB, callback: Option<OnRead>) -> Option<OnRead>;
     #[link_name = "vbSetSamples"]
@@ -154,6 +158,8 @@ extern "C" {
         typ_: VBDataType,
         capacity: c_uint,
     ) -> c_int;
+    #[link_name = "vbSetSystemRegister"]
+    fn vb_set_system_register(sim: *mut VB, index: c_uint, value: u32) -> u32;
     #[link_name = "vbSetUserData"]
     fn vb_set_user_data(sim: *mut VB, tag: *mut c_void);
     #[link_name = "vbSetWriteCallback"]
@@ -447,6 +453,20 @@ impl Sim {
         }
     }
 
+    pub fn write_register(&mut self, register: VBRegister, value: u32) {
+        match register {
+            VBRegister::Program(index) => unsafe {
+                vb_set_program_register(self.sim, index, value as i32);
+            },
+            VBRegister::System(index) => unsafe {
+                vb_set_system_register(self.sim, index, value);
+            },
+            VBRegister::PC => unsafe {
+                vb_set_program_counter(self.sim, value);
+            },
+        }
+    }
+
     pub fn read_memory(&mut self, start: u32, length: usize, into: &mut Vec<u8>) {
         let mut address = start;
         for _ in 0..length {
diff --git a/src/gdbserver.rs b/src/gdbserver.rs
index e5a6231..35ca275 100644
--- a/src/gdbserver.rs
+++ b/src/gdbserver.rs
@@ -318,6 +318,32 @@ impl GdbConnection {
             } else {
                 self.response()
             }
+        } else if req.match_str("P") {
+            let mut write_register = || {
+                let register_index = req.match_hex::<usize>()?;
+                let register = REGISTERS.get(register_index)?.to_vb_register();
+                if !req.match_str("=") {
+                    return None;
+                }
+                let value = {
+                    let mut buffer = [0; 4];
+                    if !req.match_hex_bytes(&mut buffer) {
+                        return None;
+                    }
+                    u32::from_le_bytes(buffer)
+                };
+                self.client.send_command(EmulatorCommand::WriteRegister(
+                    self.sim_id,
+                    register,
+                    value,
+                ));
+                Some(())
+            };
+            if let Some(()) = write_register() {
+                self.response().write_str("OK")
+            } else {
+                self.response()
+            }
         } else if let Some(op) = req.match_some_str(["m", "x"]) {
             let mut read_memory = || {
                 let start = req.match_hex::<u64>()?;
diff --git a/src/gdbserver/request.rs b/src/gdbserver/request.rs
index b3be216..d0459b3 100644
--- a/src/gdbserver/request.rs
+++ b/src/gdbserver/request.rs
@@ -60,6 +60,20 @@ impl Request<'_> {
             }
         }
     }
+
+    pub fn match_hex_bytes(&mut self, buffer: &mut [u8]) -> bool {
+        if self.buffer.len() < buffer.len() * 2 {
+            return false;
+        }
+        for (i, item) in buffer.iter_mut().enumerate() {
+            match u8::from_radix_16(&self.buffer[(i * 2)..(i * 2) + 2]) {
+                (byte, 2) => *item = byte,
+                _ => return false,
+            };
+        }
+        self.buffer = self.buffer.split_at(buffer.len()).1;
+        true
+    }
 }
 
 pub struct RequestSource<R> {