fix: return E22 if hwbreak arr full

Also, ensure that the above case is the only time we return an errno
err. All the others are actually problems when parsing an invalid packet
so we should nack instead

src/Packet.zig

@@ -2,6 +2,7 @@ const std = @import("std");
 
 const Allocator = std.mem.Allocator;
 const Emulator = @import("lib.zig").Emulator;
+const State = @import("State.zig");
 
 const target = @import("Server.zig").target;
 const memory_map = @import("Server.zig").memory_map;
@@ -45,7 +46,7 @@ const String = union(enum) {
     }
 };
 
-pub fn parse(self: *Self, allocator: Allocator, emu: Emulator) !String {
+pub fn parse(self: *Self, allocator: Allocator, state: *State, emu: Emulator) !String {
     switch (self.contents[0]) {
         // Required
         '?' => {
@@ -76,8 +77,8 @@ pub fn parse(self: *Self, allocator: Allocator, emu: Emulator) !String {
         'G' => @panic("TODO: Register Write"),
         'm' => {
             var tokens = std.mem.tokenize(u8, self.contents[1..], ",");
-            const addr_str = tokens.next() orelse return .{ .static = "E9999" }; // EUNKNOWN
-            const length_str = tokens.next() orelse return .{ .static = "E9999" }; // EUNKNOWN
+            const addr_str = tokens.next() orelse return error.InvalidPacket;
+            const length_str = tokens.next() orelse return error.InvalidPacket;
 
             const addr = try std.fmt.parseInt(u32, addr_str, 16);
             const len = try std.fmt.parseInt(u32, length_str, 16);
@@ -87,9 +88,6 @@ pub fn parse(self: *Self, allocator: Allocator, emu: Emulator) !String {
             {
                 var i: u32 = 0;
                 while (i < len) : (i += 1) {
-                    const value = emu.read(addr + i);
-                    log.debug("read 0x{X:0>2} from 0x{X:0>8}", .{ value, addr + i });
-
                     // writes the formatted integer to the buffer, returns a slice to the buffer but we ignore that
                     _ = std.fmt.bufPrintIntToSlice(ret[i * 2 ..][0..2], emu.read(addr + i), 16, .lower, .{ .fill = '0', .width = 2 });
                 }
@@ -98,15 +96,67 @@ pub fn parse(self: *Self, allocator: Allocator, emu: Emulator) !String {
             return .{ .alloc = ret };
         },
         'M' => @panic("TODO: Memory Write"),
-        'c' => @panic("TODO: Continue"),
+        'c' => {
+            while (true) {
+                emu.step();
+
+                const r = emu.registers();
+                const is_thumb = emu.cpsr() >> 5 & 1 == 1;
+                const r15 = r[15] -| if (is_thumb) @as(u32, 4) else 8;
+
+                if (state.hw_bkpt.isHit(r15)) {
+                    return .{ .static = "T05 hwbreak;" };
+                }
+            }
+        },
         's' => {
             // var tokens = std.mem.tokenize(u8, self.contents[1..], " ");
             // const addr = if (tokens.next()) |s| try std.fmt.parseInt(u32, s, 16) else null;
 
             emu.step();
+            return .{ .static = "S05" }; // Signal.Trap
+        },
 
-            const ret = try std.fmt.allocPrint(allocator, "S{x:0>2}", .{@enumToInt(Signal.Trap)});
-            return .{ .alloc = ret };
+        // Breakpoints
+        'z' => switch (self.contents[1]) {
+            '0' => return .{ .static = "" }, //TODO: Remove Software Breakpoint
+            '1' => {
+                var tokens = std.mem.tokenize(u8, self.contents[2..], ",");
+
+                const addr_str = tokens.next() orelse return error.InvalidPacket;
+                const addr = try std.fmt.parseInt(u32, addr_str, 16);
+
+                state.hw_bkpt.remove(addr);
+                return .{ .static = "OK" };
+            },
+            '2' => return .{ .static = "" }, // TODO: Remove Write Watchpoint
+            '3' => return .{ .static = "" }, // TODO: Remove Read Watchpoint
+            '4' => return .{ .static = "" }, // TODO: Remove Access Watchpoint
+            else => return .{ .static = "" },
+        },
+        'Z' => switch (self.contents[1]) {
+            '0' => return .{ .static = "" }, //TODO: Insert Software Breakpoint
+            '1' => {
+                var tokens = std.mem.tokenize(u8, self.contents[2..], ",");
+                const addr_str = tokens.next() orelse return error.InvalidPacket;
+                const kind_str = tokens.next() orelse return error.InvalidPacket;
+
+                const addr = try std.fmt.parseInt(u32, addr_str, 16);
+                const kind = try std.fmt.parseInt(u32, kind_str, 16);
+
+                state.hw_bkpt.add(addr, kind) catch |e| {
+                    switch (e) {
+                        error.OutOfSpace => return .{ .static = "E22" }, // FIXME: Which errno?
+                        else => return e,
+                    }
+                };
+
+                return .{ .static = "OK" };
+            },
+            '2' => return .{ .static = "" }, // TODO: Insert Write Watchpoint
+            '3' => return .{ .static = "" }, // TODO: Insert Read Watchpoint
+            '4' => return .{ .static = "" }, // TODO: Insert Access Watchpoint
+            else => return .{ .static = "" },
         },
 
         // Optional
@@ -125,6 +175,7 @@ pub fn parse(self: *Self, allocator: Allocator, emu: Emulator) !String {
         },
         'q' => {
             if (self.contents[1] == 'C' and self.contents.len == 2) return .{ .static = "QC1" };
+
             if (substr(self.contents[1..], "fThreadInfo")) return .{ .static = "m1" };
             if (substr(self.contents[1..], "sThreadInfo")) return .{ .static = "l" };
             if (substr(self.contents[1..], "Attached")) return .{ .static = "1" }; // Tell GDB we're attached to a process
@@ -142,9 +193,10 @@ pub fn parse(self: *Self, allocator: Allocator, emu: Emulator) !String {
                 _ = tokens.next(); // Xfer
                 _ = tokens.next(); // features
                 _ = tokens.next(); // read
-                const annex = tokens.next() orelse return .{ .static = "E9999" };
-                const offset_str = tokens.next() orelse return .{ .static = "E99999" };
-                const length_str = tokens.next() orelse return .{ .static = "E9999" };
+
+                const annex = tokens.next() orelse return error.InvalidPacket;
+                const offset_str = tokens.next() orelse return error.InvalidPacket;
+                const length_str = tokens.next() orelse return error.InvalidPacket;
 
                 if (std.mem.eql(u8, annex, "target.xml")) {
                     const offset = try std.fmt.parseInt(usize, offset_str, 16);
@@ -173,8 +225,8 @@ pub fn parse(self: *Self, allocator: Allocator, emu: Emulator) !String {
                 _ = tokens.next(); // Xfer
                 _ = tokens.next(); // memory-map
                 _ = tokens.next(); // read
-                const offset_str = tokens.next() orelse return .{ .static = "E9999" };
-                const length_str = tokens.next() orelse return .{ .static = "E9999" };
+                const offset_str = tokens.next() orelse return error.InvalidPacket;
+                const length_str = tokens.next() orelse return error.InvalidPacket;
 
                 const offset = try std.fmt.parseInt(usize, offset_str, 16);
                 const length = try std.fmt.parseInt(usize, length_str, 16);

src/Server.zig

@@ -1,6 +1,7 @@
 const std = @import("std");
 const network = @import("network");
 const Packet = @import("Packet.zig");
+const State = @import("State.zig");
 
 const Socket = network.Socket;
 const Allocator = std.mem.Allocator;
@@ -59,6 +60,7 @@ pkt_cache: ?[]const u8 = null,
 client: Socket,
 _socket: Socket,
 
+state: State = .{},
 emu: Emulator,
 
 pub fn init(emulator: Emulator) !Self {
@@ -129,7 +131,7 @@ fn handlePacket(self: *Self, allocator: Allocator, input: []const u8) !Action {
     var packet = Packet.from(allocator, input) catch return .nack;
     defer packet.deinit(allocator);
 
-    var string = packet.parse(allocator, self.emu) catch return .nack;
+    var string = packet.parse(allocator, &self.state, self.emu) catch return .nack;
     defer string.deinit(allocator);
 
     const reply = string.inner();

src/State.zig

@@ -0,0 +1,68 @@
+const std = @import("std");
+
+hw_bkpt: HwBkpt = .{},
+
+const HwBkpt = struct {
+    const log = std.log.scoped(.HwBkpt);
+
+    list: [2]?Bkpt = .{ null, null },
+
+    pub fn isHit(self: *const @This(), addr: u32) bool {
+        for (self.list) |bkpt_opt| {
+            if (bkpt_opt == null) continue;
+            const bkpt = bkpt_opt.?;
+
+            if (bkpt.addr == addr) return true;
+        }
+
+        return false;
+    }
+
+    pub fn add(self: *@This(), addr: u32, kind: u32) !void {
+        for (self.list) |*bkpt_opt| {
+            if (bkpt_opt.* != null) {
+                const bkpt = bkpt_opt.*.?;
+                if (bkpt.addr == addr) return; // makes this fn indempotent
+
+                continue;
+            }
+
+            bkpt_opt.* = .{ .addr = addr, .kind = try Bkpt.Kind.from(u32, kind) };
+            log.debug("Added Breakpoint at 0x{X:0>8}", .{addr});
+
+            return;
+        }
+
+        return error.OutOfSpace;
+    }
+
+    pub fn remove(self: *@This(), addr: u32) void {
+        for (self.list) |*bkpt_opt| {
+            if (bkpt_opt.* == null) continue;
+            const bkpt = bkpt_opt.*.?; // FIXME: bkpt_opt.?.addr works though?
+
+            log.debug("Removed Breakpoint at 0x{X:0>8}", .{addr});
+            if (bkpt.addr == addr) bkpt_opt.* = null;
+        }
+    }
+};
+
+const Bkpt = struct {
+    addr: u32,
+    kind: Kind,
+
+    const Kind = enum(u3) {
+        Arm = 2,
+        Thumb = 4,
+
+        pub fn from(comptime T: type, num: T) !@This() {
+            comptime std.debug.assert(@typeInfo(T) == .Int);
+
+            return switch (num) {
+                2 => .Arm,
+                4 => .Thumb,
+                else => error.UnknownBkptKind,
+            };
+        }
+    };
+};