Impl breakpoints (pretty much draft)

Breakpoints work somehow, but overstep 1 instruction. Needs to be fixed.

8 files changed, 181 insertions, 34 deletions

diff --git a/bus.cpp b/bus.cpp
index aef20a4..444749c 100644
--- a/bus.cpp
+++ b/bus.cpp
@@ -17,6 +17,8 @@ unsigned char g_ram[RAM_SIZE] = {};
 unsigned char g_io1[IO1_SIZE] = {};
 unsigned char g_io2[IO2_SIZE] = {};
 
+std::vector<Breakpoint> code_bkpts{}, read_bkpts{}, write_bkpts{}, access_bkpts{};
+
 static void exit_error(const char* fmt, ...)
 {
     va_list args;
@@ -63,7 +65,7 @@ static inline unsigned int memory_read_concrete(
             (base[address + 2] << 8) |
             base[address + 3];
     }
-    UNREACHABLE();
+    UNREACHABLE;
 }
 
 static inline struct read_result memory_read(
diff --git a/bus.hpp b/bus.hpp
index baa65be..1b4503d 100644
--- a/bus.hpp
+++ b/bus.hpp
@@ -3,6 +3,10 @@
 
 #pragma once
 
+#include <cstdint>
+#include <vector>
+#include <algorithm>
+
 #define ROM_START       (0)
 #define ROM_SIZE        (0x400000)
 #define RAM_START       (0xFF0000)
@@ -12,7 +16,12 @@
 #define IO2_START       (0xC00000)
 #define IO2_SIZE        (0x20)
 
+struct Breakpoint {
+    uint32_t offset, length;
+};
+
 extern unsigned char g_rom[ROM_SIZE];
 extern unsigned char g_ram[RAM_SIZE];
 extern unsigned char g_io1[IO1_SIZE];
 extern unsigned char g_io2[IO2_SIZE];
+extern std::vector<Breakpoint> code_bkpts, read_bkpts, write_bkpts, access_bkpts;
diff --git a/emulator.cpp b/emulator.cpp
index 1c8c6f9..5228231 100644
--- a/emulator.cpp
+++ b/emulator.cpp
@@ -31,6 +31,7 @@
 
 static constexpr struct timespec kOneMillisecond{0, 1000000};
 char msg_buf[MESSAGE_BUFFER_SIZE];
+M68KDebuggingControl g_m68k_debug{};
 
 static int set_socket_reuseaddr(int socket_fd)
 {
@@ -116,9 +117,19 @@ static std::string CreateResponse(
         return "1";
     } else if (packet.type == PacketType::kInterrupt) {
         return "OK";
+    } else if (packet.type == PacketType::kSetBreakpoint) {
+        const auto * const bkpt_data =
+            static_cast<const PacketDataBreakpoint*>(packet.data.get());
+        m68k_debug.SetBreakpoint(bkpt_data->type, bkpt_data->offset, bkpt_data->length);
+        return "OK";
+    } else if (packet.type == PacketType::kDeleteBreakpoint) {
+        const auto * const bkpt_data =
+            static_cast<const PacketDataBreakpoint*>(packet.data.get());
+        m68k_debug.RemoveBreakpoint(bkpt_data->type, bkpt_data->offset, bkpt_data->length);
+        return "OK";
     } else if (packet.type == PacketType::kReadMemory) {
         const auto * const data =
-            reinterpret_cast<const PacketDataReadMemory*>(packet.data.get());
+            static_cast<const PacketDataReadMemory*>(packet.data.get());
         const uint32_t offset = data->offset;
         const uint32_t length = data->length;
         auto ret_data = std::string(length * 2, '\0');
@@ -129,7 +140,7 @@ static std::string CreateResponse(
         return ret_data;
     } else if (packet.type == PacketType::kWriteMemory) {
         const auto * const packet_data =
-            reinterpret_cast<const PacketDataWriteMemory*>(packet.data.get());
+            static_cast<const PacketDataWriteMemory*>(packet.data.get());
         const uint32_t offset = packet_data->offset;
         const uint32_t length = packet_data->length;
         const auto write_data = packet_data->data;
@@ -194,17 +205,30 @@ static void make_hex(char* buff, unsigned int pc, unsigned int length)
     }
 }
 
+// TODO m68k_set_illg_instr_callback for true software breakpoint "4e4f"
+
 void m68k_instr_callback(int pc)
 {
-    if (!DEBUG_TRACE_INSTRUCTIONS)
-        return;
-    char buff[100];
-    unsigned int instr_size =
-        m68k_disassemble(buff, pc, M68K_CPU_TYPE_68000);
-    char buff2[100];
-    make_hex(buff2, pc, instr_size);
-    printf("  %08X: %-20s: %s\n", pc, buff2, buff);
-    fflush(stdout);
+    const auto it = std::find_if(
+            code_bkpts.begin(),
+            code_bkpts.end(),
+            [&](const Breakpoint& b) { return b.offset == static_cast<uint32_t>(pc); });
+    if (it != code_bkpts.end()) {
+        g_m68k_debug.SetRunning(false);
+        g_m68k_debug.RaiseBreakpoint();
+        m68k_end_timeslice();
+        printf("Breakpoint!\n");
+        // TODO notify GDB somehow that breakpoint has been hit
+    }
+    if (DEBUG_TRACE_INSTRUCTIONS) {
+        char buff[100];
+        unsigned int instr_size =
+            m68k_disassemble(buff, pc, M68K_CPU_TYPE_68000);
+        char buff2[100];
+        make_hex(buff2, pc, instr_size);
+        printf("  %08X: %-20s: %s\n", pc, buff2, buff);
+        fflush(stdout);
+    }
 }
 
 void ParseAndReact(
@@ -291,6 +315,14 @@ int emulator(M68KDebuggingControl& m68k_debug)
             if (m68k_debug.IsRunning()) {
                 m68k_execute(1000);
             }
+            if (m68k_debug.HasBreakpoint()) {
+                m68k_debug.ResetPendingBreakpoint();
+                    const auto response =
+                        exchange_ctx.WrapDataToSend("S05");
+                    printf("-> \"%s\"\n", response.c_str());
+                    if (send(conn_fd, &response[0], response.length(), 0) == -1)
+                        perror("Send failed (response)");
+            }
             // TODO turn off O_NONBLOCK and poll instead of sleep, only use
             // nonlock when freerunning
             clock_nanosleep(CLOCK_MONOTONIC, 0, &kOneMillisecond, nullptr);
@@ -323,8 +355,7 @@ int main(int argc, char* argv[])
     m68k_set_cpu_type(M68K_CPU_TYPE_68000);
     m68k_pulse_reset();
 
-    M68KDebuggingControl m68k_debug{};
-    emulator(m68k_debug);
+    emulator(g_m68k_debug);
     while (0)
     {
         // Values to execute determine the interleave rate.
diff --git a/gdbremote_parser.cpp b/gdbremote_parser.cpp
index 6e61802..b9115be 100644
--- a/gdbremote_parser.cpp
+++ b/gdbremote_parser.cpp
@@ -168,8 +168,8 @@ struct Command {
             return Packet::None();
         if (tokens[3].type != TokenType::kHexNumeric)
             return Packet::None();
-        const std::string first = tokens[1].data;
-        const std::string second = tokens[3].data;
+        const std::string& first = tokens[1].data;
+        const std::string& second = tokens[3].data;
         // TODO handle number parsing error (ERANGE)
         const uint32_t offset = strtol(first.c_str(), nullptr, 16);
         const uint32_t lenght = strtol(second.c_str(), nullptr, 16);
@@ -188,23 +188,23 @@ struct Command {
             return Packet::None();
         if (tokens[3].type != TokenType::kHexNumeric)
             return Packet::None();
-        // XXX Is data parameter always present?
+        // XXX Is data parameter always present or is it optional?
         if (tokens[4].type != TokenType::kSeparatorColon)
             return Packet::None();
         if (tokens[5].type != TokenType::kHexNumeric)
             return Packet::None();
         // TODO handle number parsing error (ERANGE)
-        const std::string first = tokens[1].data;
-        const std::string second = tokens[3].data;
-        const std::string data_raw = tokens[5].data;
+        const std::string& first = tokens[1].data;
+        const std::string& second = tokens[3].data;
+        const std::string& data_raw = tokens[5].data;
         const size_t data_len = data_raw.length()/2;
         std::vector<uint8_t> data(data_len);
         assert(data.size() == data_len);
-        const uint32_t offset = strtol(first.c_str(), nullptr, 16);
-        const uint32_t lenght = strtol(second.c_str(), nullptr, 16);
         for (size_t i = 0; i < data_len; i++) {
             data[i] = Utils::ParseByteFromHexChars(data_raw[i*2], data_raw[i*2+1]);
         }
+        const uint32_t offset = strtol(first.c_str(), nullptr, 16);
+        const uint32_t lenght = strtol(second.c_str(), nullptr, 16);
         return Packet{
             PacketType::kWriteMemory,
             std::make_unique<const PacketDataWriteMemory>(offset, lenght, std::move(data)),
@@ -218,17 +218,37 @@ struct Command {
     {
         return Packet{PacketType::kStep};
     }
-    static Packet parseSetBreakpoint(const std::vector<Token>&&)
+    static Packet parseSetOrDeleteBreakpoint(const std::vector<Token>&& tokens)
     {
+        if (tokens.size() < 6)
+            return Packet::None();
+        if (tokens[1].type != TokenType::kHexNumeric)
+            return Packet::None();
+        if (tokens[2].type != TokenType::kSeparatorComma)
+            return Packet::None();
+        if (tokens[3].type != TokenType::kHexNumeric)
+            return Packet::None();
+        // XXX Is third parameter always present or is it optional?
+        if (tokens[4].type != TokenType::kSeparatorComma)
+            return Packet::None();
+        if (tokens[5].type != TokenType::kHexNumeric)
+            return Packet::None();
+        const std::string& first = tokens[1].data;
+        const std::string& second = tokens[3].data;
+        const std::string& third = tokens[5].data;
+        const uint32_t type_num = strtol(first.c_str(), nullptr, 16);
+        const auto type =
+            (type_num >= static_cast<uint32_t>(BreakpointType::kMin) &&
+             type_num < static_cast<uint32_t>(BreakpointType::kUnsupported))
+                ? static_cast<BreakpointType>(type_num)
+                : BreakpointType::kMin;
+        const uint32_t offset = strtol(second.c_str(), nullptr, 16);
+        const uint32_t length = strtol(third.c_str(), nullptr, 16);
         return Packet{
-            PacketType::kSetBreakpoint,
+            tokens[0].data[0] == 'Z' ? PacketType::kSetBreakpoint : PacketType::kDeleteBreakpoint,
+            std::make_unique<const PacketDataBreakpoint>(type, offset, length),
         };
     }
-    static Packet parseDeleteBreakpoint(const std::vector<Token>&&)
-    {
-        // TODO arguments
-        return Packet{PacketType::kDeleteBreakpoint};
-    }
 };
 
 static const Command commands[] = {
@@ -252,8 +272,8 @@ static const Command commands[] = {
     { "M",                  Command::parseWriteMemory },
     { "g",                  Command::parseReadGeneralRegisters },
     { "s",                  Command::parseStep },
-    { "Z",                  Command::parseSetBreakpoint },
-    { "z",                  Command::parseDeleteBreakpoint },
+    { "Z",                  Command::parseSetOrDeleteBreakpoint },
+    { "z",                  Command::parseSetOrDeleteBreakpoint },
 };
 
 Packet Packet::Parse(std::string packet_data)
diff --git a/gdbremote_parser.hpp b/gdbremote_parser.hpp
index e060daa..370e8cc 100644
--- a/gdbremote_parser.hpp
+++ b/gdbremote_parser.hpp
@@ -94,6 +94,24 @@ struct PacketDataWriteMemory: public PacketData {
     virtual ~PacketDataWriteMemory() {}
 };
 
+enum class BreakpointType: uint32_t {
+    kMin = 0,
+    kSoftwareBreakpoint = 0,
+    kHardwareBreakpoint = 1,
+    kWriteWatchpoint = 2,
+    kReadWatchpoint = 3,
+    kAccessWatchpoint = 4,
+    kUnsupported // Keep it last and unassigned
+};
+
+struct PacketDataBreakpoint: public PacketData {
+    PacketDataBreakpoint(BreakpointType a_type, uint32_t a_offset, uint32_t a_length)
+        : type(a_type), offset(a_offset), length(a_length) {}
+    BreakpointType type{};
+    uint32_t offset{}, length{};
+    virtual ~PacketDataBreakpoint() {}
+};
+
 struct Packet {
     const PacketType type{};
     const std::unique_ptr<const PacketData> data{nullptr};
diff --git a/m68k_debugging.cpp b/m68k_debugging.cpp
index f9b0b38..509299d 100644
--- a/m68k_debugging.cpp
+++ b/m68k_debugging.cpp
@@ -3,6 +3,7 @@
 
 #include "m68k_debugging.hpp"
 #include "musashi-m68k/m68k.h"
+#include "bus.hpp"
 #include "utils.hpp"
 
 #include <cstdlib>
@@ -56,7 +57,7 @@ static inline m68k_register_t ConvertRegisterId(M68KRegister register_id)
         break;
     }
     assert(!"Unknown register_id");
-    UNREACHABLE();
+    UNREACHABLE;
 }
 
 uint32_t M68KDebuggingControl::GetRegister(M68KRegister register_id) const
@@ -142,3 +143,53 @@ void M68KDebuggingControl::Write32(uint32_t address, uint32_t value)
 {
     m68k_write_memory_32(address, value);
 }
+
+void M68KDebuggingControl::SetBreakpoint(BreakpointType type, uint32_t address, uint32_t length)
+{
+    switch (type) {
+    case BreakpointType::kSoftwareBreakpoint:
+    case BreakpointType::kHardwareBreakpoint: {
+        const auto it = std::find_if(
+                code_bkpts.begin(),
+                code_bkpts.end(),
+                [&](const Breakpoint& b) { return b.offset == address && b.length == length; });
+        if (it == code_bkpts.end())
+           code_bkpts.push_back(Breakpoint{address, length});
+    } break;
+    case BreakpointType::kWriteWatchpoint: {
+    } break;
+    case BreakpointType::kReadWatchpoint: {
+    } break;
+    case BreakpointType::kAccessWatchpoint: {
+    } break;
+        break;
+    case BreakpointType::kUnsupported: {
+        UNREACHABLE;
+    } break;
+    }
+}
+
+void M68KDebuggingControl::RemoveBreakpoint(BreakpointType type, uint32_t address, uint32_t length)
+{
+    switch (type) {
+    case BreakpointType::kSoftwareBreakpoint:
+    case BreakpointType::kHardwareBreakpoint: {
+        const auto it = std::find_if(
+                code_bkpts.begin(),
+                code_bkpts.end(),
+                [&](const Breakpoint& b) { return b.offset == address && b.length == length; });
+        if (it != code_bkpts.end())
+           code_bkpts.erase(it);
+    } break;
+    case BreakpointType::kWriteWatchpoint: {
+    } break;
+    case BreakpointType::kReadWatchpoint: {
+    } break;
+    case BreakpointType::kAccessWatchpoint: {
+    } break;
+        break;
+    case BreakpointType::kUnsupported: {
+        UNREACHABLE;
+    } break;
+    }
+}
diff --git a/m68k_debugging.hpp b/m68k_debugging.hpp
index d1acce8..1aa3bfc 100644
--- a/m68k_debugging.hpp
+++ b/m68k_debugging.hpp
@@ -3,6 +3,8 @@
 
 #pragma once
 
+#include "gdbremote_parser.hpp"
+
 #include <cstdint>
 #include <cstddef>
 
@@ -41,6 +43,8 @@ struct M68KCPUState {
 
 class M68KDebuggingControl {
 public:
+    using BreakpointType = GDBRemote::BreakpointType;
+
     M68KDebuggingControl() = default;
     M68KDebuggingControl(M68KDebuggingControl&&) = delete;
     M68KDebuggingControl(const M68KDebuggingControl&) = delete;
@@ -59,7 +63,13 @@ public:
     void Write8(uint32_t address, uint8_t value);
     void Write16(uint32_t address, uint16_t value);
     void Write32(uint32_t address, uint32_t value);
+    void SetBreakpoint(BreakpointType type, uint32_t address, uint32_t length);
+    void RemoveBreakpoint(BreakpointType type, uint32_t address, uint32_t length);
+    void RaiseBreakpoint() { _have_break_risen = true; }
+    bool HasBreakpoint() const { return _have_break_risen; }
+    void ResetPendingBreakpoint() { _have_break_risen = false; }
 
 private:
     bool _is_running{};
+    bool _have_break_risen{};
 };
diff --git a/utils.hpp b/utils.hpp
index 4a83a8d..0c49739 100644
--- a/utils.hpp
+++ b/utils.hpp
@@ -3,13 +3,19 @@
 
 #pragma once
 
+#if defined(NDEBUG)
 #ifdef __clang__
-#   define UNREACHABLE __builtin_unreachable
+#   define UNREACHABLE (__builtin_unreachable())
 #elif __GNUC__
-#   define UNREACHABLE __builtin_unreachable
+#   define UNREACHABLE (__builtin_unreachable())
 #else
 #   define UNREACHABLE
 #endif
+#else
+#include <cassert>
+#define UNREACHABLE (assert((void*)0 == "Unreachable code reached"), abort(), 1)
+#endif
+
 
 #include <stdint.h>