Fix watchpoints, add PC backtrace

3 files changed, 169 insertions, 121 deletions

diff --git a/bus.cpp b/bus.cpp
index df44164..085275a 100644
--- a/bus.cpp
+++ b/bus.cpp
@@ -16,12 +16,13 @@ namespace Adr {
 static constexpr uint32_t kZ80BusReq = 0x00a11100;
 }
 
+extern void m68k_breakpoint_callback(void);
+
 unsigned char g_rom[ROM_SIZE] = {};
 unsigned char g_ram[RAM_SIZE] = {};
 unsigned char g_sound_ram[SOUND_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, ...)
@@ -60,6 +61,7 @@ static void report_error(const char* fmt, ...)
     char buff[100]{};
     m68k_disassemble(buff, pc, M68K_CPU_TYPE_68000);
     fprintf(stderr, "%08x: %s\n", pc, buff);
+    in_error = false;
 }
 
 static inline bool is_in_range(uint32_t value, uint32_t begin, uint32_t length)
@@ -68,9 +70,9 @@ static inline bool is_in_range(uint32_t value, uint32_t begin, uint32_t length)
 }
 
 enum bitness {
-    BITNESS_8,
-    BITNESS_16,
-    BITNESS_32,
+    BITNESS_8 = 1,
+    BITNESS_16 = 2,
+    BITNESS_32 = 4,
 };
 
 struct read_result {
@@ -79,9 +81,9 @@ struct read_result {
 };
 
 static inline unsigned int memory_read_concrete(
-        enum bitness bitness,
+        const enum bitness bitness,
         unsigned char const * base,
-        unsigned int address)
+        const uint32_t address)
 {
     switch (bitness) {
     case BITNESS_8:
@@ -97,10 +99,40 @@ static inline unsigned int memory_read_concrete(
     UNREACHABLE;
 }
 
+static inline bool ranges_overlap(
+        const uint32_t a_start,
+        const uint32_t a_len,
+        const uint32_t b_start,
+        const uint32_t b_len)
+{
+    return (a_start < b_start + b_len && b_start < a_start + a_len);
+}
+
+static inline void m68k_read_callback(const uint32_t address, const uint32_t size)
+{
+    const auto access_it = std::find_if(
+            access_bkpts.begin(),
+            access_bkpts.end(),
+            [&](const Breakpoint& b) { return ranges_overlap(address, size, b.offset, b.length); });
+    if (access_it != access_bkpts.end()) {
+        printf("Access watchpoint @ 0x%08x\n", address);
+        m68k_breakpoint_callback();
+    }
+    const auto it = std::find_if(
+            read_bkpts.begin(),
+            read_bkpts.end(),
+            [&](const Breakpoint& b) { return ranges_overlap(address, size, b.offset, b.length); });
+    if (it != read_bkpts.end()) {
+        printf("Read watchpoint @ 0x%08x\n", address);
+        m68k_breakpoint_callback();
+    }
+}
+
 static inline struct read_result memory_read(
-        enum bitness bitness,
-        unsigned int address)
+        const enum bitness bitness,
+        const uint32_t address)
 {
+    m68k_read_callback(address, bitness);
     if (is_in_range(address, ROM_START, ROM_SIZE)) {
         return read_result{
             memory_read_concrete(bitness, g_rom, address - ROM_START),
@@ -132,9 +164,9 @@ static inline struct read_result memory_read(
 
 static inline void memory_write_concrete(
         enum bitness bitness,
-        unsigned char * base,
-        unsigned int address,
-        unsigned int value)
+        unsigned char * const base,
+        const uint32_t address,
+        const unsigned int value)
 {
     switch (bitness) {
     case BITNESS_8:
@@ -153,11 +185,32 @@ static inline void memory_write_concrete(
     }
 }
 
+static inline void m68k_write_callback(const uint32_t address, const uint32_t size)
+{
+    const auto access_it = std::find_if(
+            access_bkpts.begin(),
+            access_bkpts.end(),
+            [&](const Breakpoint& b) { return ranges_overlap(address, size, b.offset, b.length); });
+    if (access_it != access_bkpts.end()) {
+        printf("Access watchpoint @ 0x%08x\n", address);
+        m68k_breakpoint_callback();
+    }
+    const auto it = std::find_if(
+            write_bkpts.begin(),
+            write_bkpts.end(),
+            [&](const Breakpoint& b) { return ranges_overlap(address, size, b.offset, b.length); });
+    if (it != write_bkpts.end()) {
+        printf("Write watchpoint @ 0x%08x\n", address);
+        m68k_breakpoint_callback();
+    }
+}
+
 static inline bool memory_write(
-        enum bitness bitness,
-        unsigned int address,
-        unsigned int value)
+        const enum bitness bitness,
+        const uint32_t address,
+        const unsigned int value)
 {
+    m68k_write_callback(address, bitness);
     if (is_in_range(address, ROM_START, ROM_SIZE)) {
         memory_write_concrete(bitness, g_rom, address - ROM_START, value);
         return true;
@@ -184,142 +237,85 @@ static inline bool memory_write(
     return false;
 }
 
-static inline bool ranges_overlap(
-        const uint32_t a_start,
-        const uint32_t a_len,
-        const uint32_t b_start,
-        const uint32_t b_len)
-{
-    return (a_start < b_start + b_len && b_start < a_start + a_len);
-}
-
-extern void m68k_breakpoint_callback(void);
-
-#define MASK_24(X) ((X) & (0xFF << 24))
+#define MASK_24(X) ((X) & 0x00ffffff)
 
-static void m68k_read_callback(const uint32_t address, const uint32_t size)
+unsigned int m68k_read_memory_8(const unsigned int address_a)
 {
-    const auto access_it = std::find_if(
-            access_bkpts.begin(),
-            access_bkpts.end(),
-            [&](const Breakpoint& b) { return ranges_overlap(address, size, b.offset, b.length); });
-    if (access_it != access_bkpts.end()) {
-        m68k_breakpoint_callback();
-        printf("Access watchpoint @ 0x%08x\n", address);
-    }
-    const auto it = std::find_if(
-            read_bkpts.begin(),
-            read_bkpts.end(),
-            [&](const Breakpoint& b) { return ranges_overlap(address, size, b.offset, b.length); });
-    if (it != read_bkpts.end()) {
-        m68k_breakpoint_callback();
-        printf("Read watchpoint @ 0x%08x\n", address);
-    }
-}
-
-static void m68k_write_callback(const uint32_t address, const uint32_t size)
-{
-    const auto access_it = std::find_if(
-            access_bkpts.begin(),
-            access_bkpts.end(),
-            [&](const Breakpoint& b) { return ranges_overlap(address, size, b.offset, b.length); });
-    if (access_it != access_bkpts.end()) {
-        m68k_breakpoint_callback();
-        printf("Access watchpoint @ 0x%08x\n", address);
-    }
-    const auto it = std::find_if(
-            write_bkpts.begin(),
-            write_bkpts.end(),
-            [&](const Breakpoint& b) { return ranges_overlap(address, size, b.offset, b.length); });
-    if (it != write_bkpts.end()) {
-        m68k_breakpoint_callback();
-        printf("Write watchpoint @ 0x%08x\n", address);
-    }
-}
-
-unsigned int m68k_read_memory_8(unsigned int address)
-{
-    assert(MASK_24(address) == 0); // Just curious
-    m68k_read_callback(address, 4);
+    const uint32_t address = MASK_24(address_a);
     const struct read_result ret = memory_read(BITNESS_8, address);
     if (!ret.successful) {
-        m68k_breakpoint_callback();
         report_error("Read error u8 @%08x", address);
+        m68k_breakpoint_callback();
     }
     return ret.result;
 }
 
-unsigned int m68k_read_memory_16(unsigned int address)
+unsigned int m68k_read_memory_16(const unsigned int address_a)
 {
-    m68k_read_callback(address, 2);
-    assert(MASK_24(address) == 0); // Just curious
+    const uint32_t address = MASK_24(address_a);
     const struct read_result ret = memory_read(BITNESS_16, address);
     if (!ret.successful) {
-        m68k_breakpoint_callback();
         report_error("Read error u16 @%08x", address);
+        m68k_breakpoint_callback();
     }
     return ret.result;
 }
 
-unsigned int m68k_read_memory_32(unsigned int address)
+unsigned int m68k_read_memory_32(const unsigned int address_a)
 {
-    m68k_read_callback(address, 1);
-    assert(MASK_24(address) == 0); // Just curious
+    const uint32_t address = MASK_24(address_a);
     const struct read_result ret = memory_read(BITNESS_32, address);
     if (!ret.successful) {
-        m68k_breakpoint_callback();
         report_error("Read error u32 @%08x", address);
+        m68k_breakpoint_callback();
     }
     return ret.result;
 }
 
-unsigned int m68k_read_disassembler_16(unsigned int address)
+unsigned int m68k_read_disassembler_16(const unsigned int address_a)
 {
-    assert(MASK_24(address) == 0); // Just curious
+    const uint32_t address = MASK_24(address_a);
     const struct read_result ret = memory_read(BITNESS_16, address);
     if (0 && !ret.successful)
         exit_error("Disasm read error u16 @0x%08x", address);
     return ret.result;
 }
 
-unsigned int m68k_read_disassembler_32(unsigned int address)
+unsigned int m68k_read_disassembler_32(const unsigned int address_a)
 {
-    assert(MASK_24(address) == 0); // Just curious
+    const uint32_t address = MASK_24(address_a);
     const struct read_result ret = memory_read(BITNESS_32, address);
     if (0 && !ret.successful)
         exit_error("Disasm read error u32 @0x%08x", address);
     return ret.result;
 }
 
-void m68k_write_memory_8(unsigned int address, unsigned int value)
+void m68k_write_memory_8(const unsigned int address_a, const unsigned int value)
 {
-    assert(MASK_24(address) == 0); // Just curious
-    m68k_write_callback(address, 1);
+    const uint32_t address = MASK_24(address_a);
     const bool successful = memory_write(BITNESS_8, address, value);
     if (!successful) {
-        m68k_breakpoint_callback();
         report_error("Attempted to write 0x%02x (u8) to address %08x", value&0xff, address);
+        m68k_breakpoint_callback();
     }
 }
 
-void m68k_write_memory_16(unsigned int address, unsigned int value)
+void m68k_write_memory_16(const unsigned int address_a, const unsigned int value)
 {
-    assert(MASK_24(address) == 0); // Just curious
-    m68k_write_callback(address, 2);
+    const uint32_t address = MASK_24(address_a);
     const bool successful = memory_write(BITNESS_16, address, value);
     if (!successful) {
-        m68k_breakpoint_callback();
         report_error("Attempted to write 0x%04x (u16) to address %08x", value&0xff, address);
+        m68k_breakpoint_callback();
     }
 }
 
-void m68k_write_memory_32(unsigned int address, unsigned int value)
+void m68k_write_memory_32(const unsigned int address_a, const unsigned int value)
 {
-    assert(MASK_24(address) == 0); // Just curious
-    m68k_write_callback(address, 4);
+    const uint32_t address = MASK_24(address_a);
     const bool successful = memory_write(BITNESS_16, address, value);
     if (!successful) {
-        m68k_breakpoint_callback();
         report_error("Attempted to write 0x%08x (u32) to address %08x", value&0xff, address);
+        m68k_breakpoint_callback();
     }
 }
diff --git a/emulator.cpp b/emulator.cpp
index 6557120..67566b9 100644
--- a/emulator.cpp
+++ b/emulator.cpp
@@ -29,10 +29,35 @@
 
 #define MESSAGE_BUFFER_SIZE 1024
 
+template<typename T, size_t S>
+struct Backtrace {
+    static_assert(S > 0, "Backtrace size cannot be zero");
+    void Push(T value)
+    {
+        head = (head < S) ? head + 1 : 0;
+        buffer[head] = value;
+    }
+    void Normalize(void)
+    {
+        constexpr auto last_offset = S - 1;
+        for (; head < last_offset; head++) {
+            const T tmp = buffer[last_offset];
+            memmove(&buffer[1], &buffer[0], (S - 1) * sizeof(buffer[0]));
+            buffer[0] = tmp;
+        }
+    }
+    static constexpr size_t Size(void) {
+        return S;
+    }
+    T buffer[S]{};
+    size_t head{};
+};
+
 static constexpr struct timespec kOneMillisecond{0, 1000000};
-char msg_buf[MESSAGE_BUFFER_SIZE];
-M68KDebuggingControl g_m68k_debug{};
-bool g_no_ack_mode{};
+static char msg_buf[MESSAGE_BUFFER_SIZE];
+static M68KDebuggingControl g_m68k_debug{};
+static bool g_no_ack_mode{};
+static Backtrace<uint32_t, 10> g_pc_backtrace{};
 
 static int set_socket_reuseaddr(const int socket_fd)
 {
@@ -164,7 +189,7 @@ static std::string CreateResponse(
     case PacketType::kEnableExtendedMode:
         break;
     case PacketType::kInterrupt:
-        m68k_debug.SetRunning(false);
+        g_m68k_debug.RaiseBreakpoint();
         return "S05"; // TODO real reason
     case PacketType::kContinue:
         m68k_debug.SetRunning(true);
@@ -270,36 +295,52 @@ static void make_hex(char* buff, unsigned int pc, unsigned int length)
     }
 }
 
+static void PrintInstructionTrace(const uint32_t pc)
+{
+    char buff[100];
+    const 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);
+}
+
 // TODO m68k_set_illg_instr_callback for true software breakpoint "4e4f"
 
 int m68k_instr_callback(const int pc)
 {
+    g_pc_backtrace.Push(pc);
     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 @ 0x%08x\n", pc);
+        g_pc_backtrace.Normalize();
+        printf("PC backtrace (size=%zu):\n", g_pc_backtrace.Size());
+        for (size_t i = 0; i < g_pc_backtrace.Size(); i++)
+        {
+            PrintInstructionTrace(g_pc_backtrace.buffer[i]);
+        }
         return 1;
     }
     if (DEBUG_TRACE_INSTRUCTIONS) {
-        char buff[100];
-        const 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);
+        PrintInstructionTrace(pc);
     }
     return 0;
 }
 
 void m68k_breakpoint_callback(void)
 {
-    g_m68k_debug.SetRunning(false);
+    g_pc_backtrace.Normalize();
+    printf("PC backtrace (size=%zu):\n", g_pc_backtrace.Size());
+    for (size_t i = 0; i < g_pc_backtrace.Size(); i++)
+    {
+        PrintInstructionTrace(g_pc_backtrace.buffer[i]);
+    }
     g_m68k_debug.RaiseBreakpoint();
     m68k_end_timeslice();
 }
@@ -389,6 +430,7 @@ int emulator(M68KDebuggingControl& m68k_debug)
                 m68k_execute(10000);
             }
             if (m68k_debug.HasBreakpoint()) {
+                m68k_debug.SetRunning(false);
                 m68k_debug.ResetPendingBreakpoint();
                     const auto response =
                         exchange_ctx.WrapDataToSend("S05");
diff --git a/m68k_debugging.cpp b/m68k_debugging.cpp
index d02e463..7b7c84c 100644
--- a/m68k_debugging.cpp
+++ b/m68k_debugging.cpp
@@ -9,7 +9,7 @@
 #include <cstdlib>
 #include <cassert>
 
-static inline m68k_register_t ConvertRegisterId(M68KRegister register_id)
+static inline m68k_register_t ConvertRegisterId(const M68KRegister register_id)
 {
     switch (register_id) {
     case M68KRegister::kD0:
@@ -60,7 +60,7 @@ static inline m68k_register_t ConvertRegisterId(M68KRegister register_id)
     UNREACHABLE;
 }
 
-uint32_t M68KDebuggingControl::GetRegister(M68KRegister register_id) const
+uint32_t M68KDebuggingControl::GetRegister(const M68KRegister register_id) const
 {
     return m68k_get_reg(nullptr, ConvertRegisterId(register_id));
 }
@@ -92,7 +92,7 @@ M68KCPUState M68KDebuggingControl::GetCPUState() const
     };
 }
 
-void M68KDebuggingControl::SetRegister(M68KRegister register_id, uint32_t value)
+void M68KDebuggingControl::SetRegister(const M68KRegister register_id, const uint32_t value)
 {
     return m68k_set_reg(ConvertRegisterId(register_id), value);
 }
@@ -114,38 +114,44 @@ void M68KDebuggingControl::Pause()
     abort();
 }
 
-uint8_t M68KDebuggingControl::Read8(uint32_t address) const
+#define MASK_24(X) ((X) & 0x00ffffff)
+
+uint8_t M68KDebuggingControl::Read8(const uint32_t address) const
 {
-    return static_cast<uint8_t>(m68k_read_memory_8(address & 0x00FFFFFF));
+    return static_cast<uint8_t>(m68k_read_memory_8(MASK_24(address)));
 }
 
-uint16_t M68KDebuggingControl::Read16(uint32_t address) const
+uint16_t M68KDebuggingControl::Read16(const uint32_t address) const
 {
-    return static_cast<uint16_t>(m68k_read_memory_16(address & 0x00FFFFFF));
+    return static_cast<uint16_t>(m68k_read_memory_16(MASK_24(address)));
 }
 
-uint32_t M68KDebuggingControl::Read32(uint32_t address) const
+uint32_t M68KDebuggingControl::Read32(const uint32_t address) const
 {
-    return static_cast<uint32_t>(m68k_read_memory_32(address & 0x00FFFFFF));
+    return static_cast<uint32_t>(m68k_read_memory_32(MASK_24(address)));
 }
 
-void M68KDebuggingControl::Write8(uint32_t address, uint8_t value)
+void M68KDebuggingControl::Write8(const uint32_t address, const uint8_t value)
 {
     m68k_write_memory_8(address, value);
 }
 
-void M68KDebuggingControl::Write16(uint32_t address, uint16_t value)
+void M68KDebuggingControl::Write16(const uint32_t address, const uint16_t value)
 {
     m68k_write_memory_16(address, value);
 }
 
-void M68KDebuggingControl::Write32(uint32_t address, uint32_t value)
+void M68KDebuggingControl::Write32(const uint32_t address, const uint32_t value)
 {
     m68k_write_memory_32(address, value);
 }
 
-void M68KDebuggingControl::SetBreakpoint(BreakpointType type, uint32_t address, uint32_t length)
+void M68KDebuggingControl::SetBreakpoint(
+        const BreakpointType type,
+        const uint32_t address_a,
+        const uint32_t length)
 {
+    const uint32_t address = MASK_24(address_a);
     switch (type) {
     case BreakpointType::kSoftwareBreakpoint:
     case BreakpointType::kHardwareBreakpoint: {
@@ -187,8 +193,12 @@ void M68KDebuggingControl::SetBreakpoint(BreakpointType type, uint32_t address,
     }
 }
 
-void M68KDebuggingControl::RemoveBreakpoint(BreakpointType type, uint32_t address, uint32_t length)
+void M68KDebuggingControl::RemoveBreakpoint(
+        const BreakpointType type,
+        const uint32_t address_a,
+        const uint32_t length)
 {
+    const uint32_t address = MASK_24(address_a);
     switch (type) {
     case BreakpointType::kSoftwareBreakpoint:
     case BreakpointType::kHardwareBreakpoint: {