@patleeman/pi-boy 0.1.0

package/mgba-bridge/vendor/mgba/src/debugger/gdb-stub.c

@@ -0,0 +1,918 @@
+/* Copyright (c) 2013-2016 Jeffrey Pfau
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */
+#include <mgba/internal/debugger/gdb-stub.h>
+
+#include <mgba/core/core.h>
+#include <mgba/internal/arm/debugger/debugger.h>
+#include <mgba/internal/arm/isa-inlines.h>
+#include <mgba/internal/gba/memory.h>
+#include <mgba-util/string.h>
+
+#include <signal.h>
+
+#ifndef SIGTRAP
+#define SIGTRAP 5 /* Win32 Signals do not include SIGTRAP */
+#endif
+
+enum GDBError {
+	GDB_NO_ERROR = 0x00,
+	GDB_BAD_ARGUMENTS = 0x06,
+	GDB_UNSUPPORTED_COMMAND = 0x07
+};
+
+enum {
+	MACH_O_ARM = 12,
+	MACH_O_ARM_V4T = 5
+};
+
+static const char* TARGET_XML = "<target version=\"1.0\">"
+                                "<architecture>armv4t</architecture>"
+                                "<osabi>none</osabi>"
+                                "<feature name=\"org.gnu.gdb.arm.core\">"
+                                "<reg name=\"r0\" bitsize=\"32\" type=\"uint32\"/>"
+                                "<reg name=\"r1\" bitsize=\"32\" type=\"uint32\"/>"
+                                "<reg name=\"r2\" bitsize=\"32\" type=\"uint32\"/>"
+                                "<reg name=\"r3\" bitsize=\"32\" type=\"uint32\"/>"
+                                "<reg name=\"r4\" bitsize=\"32\" type=\"uint32\"/>"
+                                "<reg name=\"r5\" bitsize=\"32\" type=\"uint32\"/>"
+                                "<reg name=\"r6\" bitsize=\"32\" type=\"uint32\"/>"
+                                "<reg name=\"r7\" bitsize=\"32\" type=\"uint32\"/>"
+                                "<reg name=\"r8\" bitsize=\"32\" type=\"uint32\"/>"
+                                "<reg name=\"r9\" bitsize=\"32\" type=\"uint32\"/>"
+                                "<reg name=\"r10\" bitsize=\"32\" type=\"uint32\"/>"
+                                "<reg name=\"r11\" bitsize=\"32\" type=\"uint32\"/>"
+                                "<reg name=\"r12\" bitsize=\"32\" type=\"uint32\"/>"
+                                "<reg name=\"sp\" bitsize=\"32\" type=\"data_ptr\"/>"
+                                "<reg name=\"lr\" bitsize=\"32\"/>"
+                                "<reg name=\"pc\" bitsize=\"32\" type=\"code_ptr\"/>"
+                                "<flags id=\"cpsr_flags\" size=\"4\">"
+                                "<field name=\"N\" start=\"31\" end=\"31\"/>"
+                                "<field name=\"Z\" start=\"30\" end=\"30\"/>"
+                                "<field name=\"C\" start=\"29\" end=\"29\"/>"
+                                "<field name=\"V\" start=\"28\" end=\"28\"/>"
+                                "<field name=\"I\" start=\"7\" end=\"7\"/>"
+                                "<field name=\"F\" start=\"6\" end=\"6\"/>"
+                                "<field name=\"T\" start=\"5\" end=\"5\"/>"
+                                "<field name=\"M\" start=\"0\" end=\"4\"/>"
+                                "</flags>"
+                                "<reg name=\"cpsr\" bitsize=\"32\" regnum=\"25\" type=\"cpsr_flags\"/>"
+                                "</feature>"
+                                "</target>";
+
+static void _sendMessage(struct GDBStub* stub);
+
+static void _gdbStubDeinit(struct mDebuggerModule* debugger) {
+	struct GDBStub* stub = (struct GDBStub*) debugger;
+	if (!SOCKET_FAILED(stub->socket)) {
+		GDBStubShutdown(stub);
+	}
+}
+
+static void _gdbStubEntered(struct mDebuggerModule* debugger, enum mDebuggerEntryReason reason, struct mDebuggerEntryInfo* info) {
+	struct GDBStub* stub = (struct GDBStub*) debugger;
+	switch (reason) {
+	case DEBUGGER_ENTER_MANUAL:
+		snprintf(stub->outgoing, GDB_STUB_MAX_LINE - 4, "S%02x", SIGINT);
+		break;
+	case DEBUGGER_ENTER_BREAKPOINT:
+		if (stub->supportsHwbreak && stub->supportsSwbreak && info) {
+			snprintf(stub->outgoing, GDB_STUB_MAX_LINE - 4, "T%02x%cwbreak:;", SIGTRAP, info->type.bp.breakType == BREAKPOINT_SOFTWARE ? 's' : 'h');
+		} else {
+			snprintf(stub->outgoing, GDB_STUB_MAX_LINE - 4, "S%02xk", SIGTRAP);
+		}
+		break;
+	case DEBUGGER_ENTER_WATCHPOINT:
+		if (info) {
+			const char* type = 0;
+
+			if (stub->watchpointsBehavior != GDB_WATCHPOINT_STANDARD_LOGIC && info->type.wp.watchType & WATCHPOINT_WRITE) {
+				// We send S05 instead of T05watch because it bypasses GDB's internal logic to check if
+				// the value changed and to bypass a step into by GDB. This allows to control the change
+				// logic even when using savestates which we already handle in the core debugger logic
+				snprintf(stub->outgoing, GDB_STUB_MAX_LINE - 4, "S%02x", SIGTRAP);
+				_sendMessage(stub);
+				return;
+			}
+
+			switch (info->type.wp.watchType) {
+			case WATCHPOINT_WRITE:
+			case WATCHPOINT_WRITE_CHANGE:
+				type = "watch";
+				break;
+			case WATCHPOINT_READ:
+				type = "rwatch";
+				break;
+			case WATCHPOINT_RW:
+				type = "awatch";
+				break;
+			case WATCHPOINT_CHANGE:
+				break;
+			}
+			snprintf(stub->outgoing, GDB_STUB_MAX_LINE - 4, "T%02x%s:%08x;", SIGTRAP, type, info->address);
+		} else {
+			snprintf(stub->outgoing, GDB_STUB_MAX_LINE - 4, "S%02x", SIGTRAP);
+		}
+		break;
+	case DEBUGGER_ENTER_ILLEGAL_OP:
+		snprintf(stub->outgoing, GDB_STUB_MAX_LINE - 4, "S%02x", SIGILL);
+		break;
+	case DEBUGGER_ENTER_ATTACHED:
+	case DEBUGGER_ENTER_STACK:
+		return;
+	}
+	_sendMessage(stub);
+}
+
+static void _gdbStubPoll(struct mDebuggerModule* debugger) {
+	struct GDBStub* stub = (struct GDBStub*) debugger;
+	--stub->untilPoll;
+	if (stub->untilPoll > 0) {
+		return;
+	}
+	stub->untilPoll = GDB_STUB_INTERVAL;
+	GDBStubUpdate(stub, 0);
+}
+
+static void _gdbStubWait(struct mDebuggerModule* debugger, int32_t timeoutMs) {
+	struct GDBStub* stub = (struct GDBStub*) debugger;
+	GDBStubUpdate(stub, timeoutMs);
+}
+
+static void _gdbStubUpdate(struct mDebuggerModule* debugger) {
+	struct GDBStub* stub = (struct GDBStub*) debugger;
+	GDBStubUpdate(stub, 0);
+}
+
+static void _ack(struct GDBStub* stub) {
+	char ack = '+';
+	SocketSend(stub->connection, &ack, 1);
+}
+
+static void _nak(struct GDBStub* stub) {
+	char nak = '-';
+	mLOG(DEBUGGER, WARN, "Packet error");
+	SocketSend(stub->connection, &nak, 1);
+}
+
+static uint32_t _hex2int(const char* hex, int maxDigits) {
+	uint32_t value = 0;
+	uint8_t letter;
+
+	while (maxDigits--) {
+		letter = *hex - '0';
+		if (letter > 9) {
+			letter = *hex - 'a';
+			if (letter > 5) {
+				break;
+			}
+			value *= 0x10;
+			value += letter + 10;
+		} else {
+			value *= 0x10;
+			value += letter;
+		}
+		++hex;
+	}
+	return value;
+}
+
+static void _int2hex8(uint8_t value, char* out) {
+	static const char language[] = "0123456789abcdef";
+	out[0] = language[value >> 4];
+	out[1] = language[value & 0xF];
+}
+
+static void _int2hex32(uint32_t value, char* out) {
+	static const char language[] = "0123456789abcdef";
+	out[6] = language[value >> 28];
+	out[7] = language[(value >> 24) & 0xF];
+	out[4] = language[(value >> 20) & 0xF];
+	out[5] = language[(value >> 16) & 0xF];
+	out[2] = language[(value >> 12) & 0xF];
+	out[3] = language[(value >> 8) & 0xF];
+	out[0] = language[(value >> 4) & 0xF];
+	out[1] = language[value & 0xF];
+}
+
+static uint32_t _readHex(const char* in, unsigned* out) {
+	unsigned i;
+	for (i = 0; i < 8; ++i) {
+		if (in[i] == ',' || in[i] == ':' || in[i] == '=') {
+			break;
+		}
+	}
+	*out += i;
+	return _hex2int(in, i);
+}
+
+static void _sendMessage(struct GDBStub* stub) {
+	if (stub->lineAck != GDB_ACK_OFF) {
+		stub->lineAck = GDB_ACK_PENDING;
+	}
+	uint8_t checksum = 0;
+	int i = 1;
+	char buffer = stub->outgoing[0];
+	char swap;
+	stub->outgoing[0] = '$';
+	if (buffer) {
+		for (; i < GDB_STUB_MAX_LINE - 5; ++i) {
+			checksum += buffer;
+			swap = stub->outgoing[i];
+			stub->outgoing[i] = buffer;
+			buffer = swap;
+			if (!buffer) {
+				++i;
+				break;
+			}
+		}
+	}
+	stub->outgoing[i] = '#';
+	_int2hex8(checksum, &stub->outgoing[i + 1]);
+	stub->outgoing[i + 3] = 0;
+	mLOG(DEBUGGER, DEBUG, "> %s", stub->outgoing);
+	SocketSend(stub->connection, stub->outgoing, i + 3);
+}
+
+static void _error(struct GDBStub* stub, enum GDBError error) {
+	snprintf(stub->outgoing, GDB_STUB_MAX_LINE - 4, "E%02x", error);
+	_sendMessage(stub);
+}
+
+static void _writeHostInfo(struct GDBStub* stub) {
+	snprintf(stub->outgoing, GDB_STUB_MAX_LINE - 4, "cputype:%u;cpusubtype:%u:ostype:none;vendor:none;endian:little;ptrsize:4;", MACH_O_ARM, MACH_O_ARM_V4T);
+	_sendMessage(stub);
+}
+
+static void _continue(struct GDBStub* stub, const char* message) {
+	stub->untilPoll = GDB_STUB_INTERVAL;
+	stub->d.isPaused = false;
+	mDebuggerModuleSetNeedsCallback(&stub->d);
+	// TODO: parse message
+	UNUSED(message);
+}
+
+static void _step(struct GDBStub* stub, const char* message) {
+	const struct ARMCore* cpu = stub->d.p->core->cpu;
+	const int32_t pc = cpu->gprs[ARM_PC];
+
+	stub->d.p->core->step(stub->d.p->core);
+
+	if (pc >= GBA_SIZE_BIOS && cpu->gprs[ARM_PC] < GBA_SIZE_BIOS) {
+		// GDB cannot cope with jumps into BIOS
+		// skip over them by placing a temporary breakpoint at PC (instruction after the jump)
+		// and then continue without sending a GDB SIGTRAP
+		const struct mBreakpoint breakpoint = {
+			.address = pc,
+			.type = BREAKPOINT_HARDWARE,
+			.isTemporary = true
+		};
+		stub->d.p->platform->setBreakpoint(stub->d.p->platform, &stub->d, &breakpoint);
+		_continue(stub, message);
+		return;
+	}
+
+	snprintf(stub->outgoing, GDB_STUB_MAX_LINE - 4, "S%02x", SIGTRAP);
+	_sendMessage(stub);
+	// TODO: parse message
+	UNUSED(message);
+}
+
+static void _writeMemoryBinary(struct GDBStub* stub, const char* message) {
+	const char* readAddress = message;
+	unsigned i = 0;
+	uint32_t address = _readHex(readAddress, &i);
+	readAddress += i + 1;
+
+	i = 0;
+	uint32_t size = _readHex(readAddress, &i);
+	readAddress += i + 1;
+
+	if (size > 512) {
+		_error(stub, GDB_BAD_ARGUMENTS);
+		return;
+	}
+
+	struct ARMCore* cpu = stub->d.p->core->cpu;
+	for (i = 0; i < size; i++) {
+		uint8_t byte = *readAddress;
+		++readAddress;
+
+		// Parse escape char
+		if (byte == 0x7D) {
+			byte = *readAddress ^ 0x20;
+			++readAddress;
+		}
+
+		GBAPatch8(cpu, address + i, byte, 0);
+	}
+
+	strncpy(stub->outgoing, "OK", GDB_STUB_MAX_LINE - 4);
+	_sendMessage(stub);
+}
+
+
+static void _writeMemory(struct GDBStub* stub, const char* message) {
+	const char* readAddress = message;
+	unsigned i = 0;
+	uint32_t address = _readHex(readAddress, &i);
+	readAddress += i + 1;
+
+	i = 0;
+	uint32_t size = _readHex(readAddress, &i);
+	readAddress += i + 1;
+
+	if (size > 512) {
+		_error(stub, GDB_BAD_ARGUMENTS);
+		return;
+	}
+
+	struct ARMCore* cpu = stub->d.p->core->cpu;
+	for (i = 0; i < size; ++i, readAddress += 2) {
+		uint8_t byte = _hex2int(readAddress, 2);
+		GBAPatch8(cpu, address + i, byte, 0);
+	}
+
+	strncpy(stub->outgoing, "OK", GDB_STUB_MAX_LINE - 4);
+	_sendMessage(stub);
+}
+
+static void _readMemory(struct GDBStub* stub, const char* message) {
+	const char* readAddress = message;
+	unsigned i = 0;
+	uint32_t address = _readHex(readAddress, &i);
+	readAddress += i + 1;
+	uint32_t size = _readHex(readAddress, &i);
+	if (size > 512) {
+		_error(stub, GDB_BAD_ARGUMENTS);
+		return;
+	}
+	struct ARMCore* cpu = stub->d.p->core->cpu;
+	int writeAddress = 0;
+	for (i = 0; i < size; ++i, writeAddress += 2) {
+		uint8_t byte = cpu->memory.load8(cpu, address + i, 0);
+		_int2hex8(byte, &stub->outgoing[writeAddress]);
+	}
+	stub->outgoing[writeAddress] = 0;
+	_sendMessage(stub);
+}
+
+static void _writeGPRs(struct GDBStub* stub, const char* message) {
+	struct ARMCore* cpu = stub->d.p->core->cpu;
+	const char* readAddress = message;
+
+	int r;
+	for (r = 0; r <= ARM_PC; ++r) {
+		cpu->gprs[r] = _hex2int(readAddress, 8);
+		readAddress += 8;
+	}
+	if (cpu->executionMode == MODE_ARM) {
+		ARMWritePC(cpu);
+	} else {
+		ThumbWritePC(cpu);
+	}
+
+	strncpy(stub->outgoing, "OK", GDB_STUB_MAX_LINE - 4);
+	_sendMessage(stub);
+}
+
+static int32_t _readPC(struct ARMCore* cpu) {
+	return cpu->gprs[ARM_PC] - (cpu->cpsr.t ? WORD_SIZE_THUMB : WORD_SIZE_ARM);
+}
+
+static void _readGPRs(struct GDBStub* stub, const char* message) {
+	struct ARMCore* cpu = stub->d.p->core->cpu;
+	UNUSED(message);
+	int r;
+	int i = 0;
+
+	// General purpose registers
+	for (r = 0; r < ARM_PC; ++r) {
+		_int2hex32(cpu->gprs[r], &stub->outgoing[i]);
+		i += 8;
+	}
+
+	// Program counter
+	_int2hex32(_readPC(cpu), &stub->outgoing[i]);
+	i += 8;
+
+	// CPU status
+	_int2hex32(cpu->cpsr.packed, &stub->outgoing[i]);
+	i += 8;
+
+	stub->outgoing[i] = 0;
+	_sendMessage(stub);
+}
+
+static void _writeRegister(struct GDBStub* stub, const char* message) {
+	struct ARMCore* cpu = stub->d.p->core->cpu;
+	const char* readAddress = message;
+
+	unsigned i = 0;
+	uint32_t reg = _readHex(readAddress, &i);
+	readAddress += i + 1;
+
+	uint32_t value = _readHex(readAddress, &i);
+
+	LOAD_32BE(value, 0, &value);
+
+	if (reg <= ARM_PC) {
+		cpu->gprs[reg] = value;
+		if (reg == ARM_PC) {
+			if (cpu->executionMode == MODE_ARM) {
+				ARMWritePC(cpu);
+			} else {
+				ThumbWritePC(cpu);
+			}
+		}
+	} else if (reg == 0x19) {
+		cpu->cpsr.packed = value;
+	} else {
+		stub->outgoing[0] = '\0';
+		_sendMessage(stub);
+		return;
+	}
+
+	strncpy(stub->outgoing, "OK", GDB_STUB_MAX_LINE - 4);
+	_sendMessage(stub);
+}
+
+static void _readRegister(struct GDBStub* stub, const char* message) {
+	struct ARMCore* cpu = stub->d.p->core->cpu;
+	const char* readAddress = message;
+	unsigned i = 0;
+	uint32_t reg = _readHex(readAddress, &i);
+	uint32_t value;
+	if (reg < ARM_PC) {
+		value = cpu->gprs[reg];
+	} else if (reg == ARM_PC) {
+		value = _readPC(cpu);
+	} else if (reg == 0x19) {
+		value = cpu->cpsr.packed;
+	} else {
+		stub->outgoing[0] = '\0';
+		_sendMessage(stub);
+		return;
+	}
+	_int2hex32(value, stub->outgoing);
+	stub->outgoing[8] = '\0';
+	_sendMessage(stub);
+}
+
+static void _processQSupportedCommand(struct GDBStub* stub, const char* message) {
+	const char* terminator = strrchr(message, '#');
+	stub->supportsSwbreak = false;
+	stub->supportsHwbreak = false;
+	while (message < terminator) {
+		const char* end = strchr(message, ';');
+		size_t len;
+		if (end && end < terminator) {
+			len = end - message;
+		} else {
+			len = terminator - message;
+		}
+		if (!strncmp(message, "swbreak+", len)) {
+			stub->supportsSwbreak = true;
+		} else if (!strncmp(message, "hwbreak+", len)) {
+			stub->supportsHwbreak = true;
+		} else if (!strncmp(message, "swbreak-", len)) {
+			stub->supportsSwbreak = false;
+		} else if (!strncmp(message, "hwbreak-", len)) {
+			stub->supportsHwbreak = false;
+		}
+		if (!end) {
+			break;
+		}
+		message = end + 1;
+	}
+	strncpy(stub->outgoing, "swbreak+;hwbreak+;qXfer:features:read+;qXfer:memory-map:read+;QStartNoAckMode+", GDB_STUB_MAX_LINE - 4);
+}
+
+static void _processQXferCommand(struct GDBStub* stub, const char* params, const char* data) {
+	unsigned offset = 0;
+	unsigned length = 0;
+
+	unsigned index = 0;
+	for (index = 0; params[index] != ','; ++index) {
+		offset <<= 4;
+		offset |= _hex2int(&params[index], 1);
+	}
+
+	++index;
+	unsigned int paramsLength = strlen(params);
+	for (; index + 3 < paramsLength; ++index) {
+		length <<= 4;
+		length |= _hex2int(&params[index], 1);
+	}
+
+	length += 1;
+
+	if (length + 4 > GDB_STUB_MAX_LINE) {
+		length = GDB_STUB_MAX_LINE - 4;
+	}
+
+	if (strlen(data) < length + offset) {
+		length = strlen(data) - offset + 1;
+		stub->outgoing[0] = 'l';
+	} else {
+		stub->outgoing[0] = 'm';
+	}
+	strlcpy(&stub->outgoing[1], &data[offset], length);
+}
+
+static void _generateMemoryMapXml(struct GDBStub* stub, char* memoryMap) {
+	size_t index = 0;
+	strncpy(memoryMap, "<memory-map version=\"1.0\">", 27);
+	index += strlen(memoryMap);
+	const struct mCoreMemoryBlock* blocks;
+	size_t nBlocks = stub->d.p->core->listMemoryBlocks(stub->d.p->core, &blocks);
+	size_t i;
+	for (i = 0; i < nBlocks; ++i) {
+		if (!(blocks[i].flags & mCORE_MEMORY_MAPPED)) {
+			continue;
+		}
+
+		const char* type = blocks[i].flags & (mCORE_MEMORY_WRITE | mCORE_MEMORY_WORM) ? "ram" : "rom";
+		index += snprintf(&memoryMap[index], GDB_STUB_MAX_LINE - index, "<memory type=\"%s\" start=\"0x%08x\" length=\"0x%08x\"/>", type, blocks[i].start, blocks[i].size);
+	}
+	int amountLeft = GDB_STUB_MAX_LINE - index;
+	strncpy(&memoryMap[index], "</memory-map>", amountLeft);
+}
+
+static void _processQReadCommand(struct GDBStub* stub, const char* message) {
+	stub->outgoing[0] = '\0';
+	if (!strncmp("HostInfo#", message, 9)) {
+		_writeHostInfo(stub);
+		return;
+	}
+	if (!strncmp("Attached#", message, 9)) {
+		strncpy(stub->outgoing, "1", GDB_STUB_MAX_LINE - 4);
+	} else if (!strncmp("VAttachOrWaitSupported#", message, 23)) {
+		strncpy(stub->outgoing, "OK", GDB_STUB_MAX_LINE - 4);
+	} else if (!strncmp("C#", message, 2)) {
+		strncpy(stub->outgoing, "QC1", GDB_STUB_MAX_LINE - 4);
+	} else if (!strncmp("fThreadInfo#", message, 12)) {
+		strncpy(stub->outgoing, "m1", GDB_STUB_MAX_LINE - 4);
+	} else if (!strncmp("sThreadInfo#", message, 12)) {
+		strncpy(stub->outgoing, "l", GDB_STUB_MAX_LINE - 4);
+	} else if (!strncmp("Xfer:features:read:target.xml:", message, 30)) {
+		_processQXferCommand(stub, message + 30, TARGET_XML);
+	} else if (!strncmp("Xfer:memory-map:read::", message, 22)) {
+		if (strlen(stub->memoryMapXml) == 0) {
+			_generateMemoryMapXml(stub, stub->memoryMapXml);
+		}
+		_processQXferCommand(stub, message + 22, stub->memoryMapXml);
+	} else if (!strncmp("Supported:", message, 10)) {
+		_processQSupportedCommand(stub, message + 10);
+	}
+	_sendMessage(stub);
+}
+
+static void _processQWriteCommand(struct GDBStub* stub, const char* message) {
+	stub->outgoing[0] = '\0';
+	if (!strncmp("StartNoAckMode#", message, 15)) {
+		stub->lineAck = GDB_ACK_OFF;
+		strncpy(stub->outgoing, "OK", GDB_STUB_MAX_LINE - 4);
+	}
+	_sendMessage(stub);
+}
+
+static void _processVWriteCommand(struct GDBStub* stub, const char* message) {
+	UNUSED(message);
+	stub->outgoing[0] = '\0';
+	_sendMessage(stub);
+}
+
+static void _processVReadCommand(struct GDBStub* stub, const char* message) {
+	stub->outgoing[0] = '\0';
+	if (!strncmp("Attach", message, 6)) {
+		strncpy(stub->outgoing, "1", GDB_STUB_MAX_LINE - 4);
+		stub->d.isPaused = true;
+		struct mDebuggerEntryInfo info = {
+			.target = &stub->d
+		};
+		mDebuggerEnter(stub->d.p, DEBUGGER_ENTER_MANUAL, &info);
+	}
+	_sendMessage(stub);
+}
+
+static void _setBreakpoint(struct GDBStub* stub, const char* message) {
+	const char* readAddress = &message[2];
+	unsigned i = 0;
+	uint32_t address = _readHex(readAddress, &i);
+	readAddress += i + 1;
+
+	struct mBreakpoint breakpoint = {
+		.address = address,
+		.type = BREAKPOINT_HARDWARE
+	};
+	struct mWatchpoint watchpoint = {
+		.minAddress = address,
+		.maxAddress = address + 1
+	};
+
+	switch (message[0]) {
+	case '0':
+	case '1':
+		stub->d.p->platform->setBreakpoint(stub->d.p->platform, &stub->d, &breakpoint);
+		break;
+	case '2':
+		watchpoint.type = stub->watchpointsBehavior == GDB_WATCHPOINT_OVERRIDE_LOGIC_ANY_WRITE ? WATCHPOINT_WRITE : WATCHPOINT_WRITE_CHANGE;
+		stub->d.p->platform->setWatchpoint(stub->d.p->platform, &stub->d, &watchpoint);
+		break;
+	case '3':
+		watchpoint.type = WATCHPOINT_READ;
+		stub->d.p->platform->setWatchpoint(stub->d.p->platform, &stub->d, &watchpoint);
+		break;
+	case '4':
+		watchpoint.type = WATCHPOINT_RW;
+		stub->d.p->platform->setWatchpoint(stub->d.p->platform, &stub->d, &watchpoint);
+		break;
+	default:
+		stub->outgoing[0] = '\0';
+		_sendMessage(stub);
+		return;
+	}
+	strncpy(stub->outgoing, "OK", GDB_STUB_MAX_LINE - 4);
+	_sendMessage(stub);
+}
+
+static void _clearBreakpoint(struct GDBStub* stub, const char* message) {
+	const char* readAddress = &message[2];
+	unsigned i = 0;
+	uint32_t address = _readHex(readAddress, &i);
+	struct mBreakpointList breakpoints;
+	struct mWatchpointList watchpoints;
+
+	size_t index;
+	switch (message[0]) {
+	case '0':
+	case '1':
+		mBreakpointListInit(&breakpoints, 0);
+		stub->d.p->platform->listBreakpoints(stub->d.p->platform, &stub->d, &breakpoints);
+		for (index = 0; index < mBreakpointListSize(&breakpoints); ++index) {
+			if (mBreakpointListGetPointer(&breakpoints, index)->address != address) {
+				continue;
+			}
+			stub->d.p->platform->clearBreakpoint(stub->d.p->platform, mBreakpointListGetPointer(&breakpoints, index)->id);
+		}
+		mBreakpointListDeinit(&breakpoints);
+		break;
+	case '2':
+	case '3':
+	case '4':
+		mWatchpointListInit(&watchpoints, 0);
+		stub->d.p->platform->listWatchpoints(stub->d.p->platform, &stub->d, &watchpoints);
+		for (index = 0; index < mWatchpointListSize(&watchpoints); ++index) {
+			struct mWatchpoint* watchpoint = mWatchpointListGetPointer(&watchpoints, index);
+			if (address >= watchpoint->minAddress && address < watchpoint->maxAddress) {
+				continue;
+			}
+			stub->d.p->platform->clearBreakpoint(stub->d.p->platform, watchpoint->id);
+		}
+		mWatchpointListDeinit(&watchpoints);
+		break;
+	default:
+		break;
+	}
+	strncpy(stub->outgoing, "OK", GDB_STUB_MAX_LINE - 4);
+	_sendMessage(stub);
+}
+
+size_t _parseGDBMessage(struct GDBStub* stub, const char* message) {
+	uint8_t checksum = 0;
+	int parsed = 1;
+	struct mDebuggerEntryInfo info = {
+		.target = &stub->d
+	};
+	switch (*message) {
+	case '+':
+		stub->lineAck = GDB_ACK_RECEIVED;
+		return parsed;
+	case '-':
+		stub->lineAck = GDB_NAK_RECEIVED;
+		return parsed;
+	case '$':
+		++message;
+		break;
+	case '\x03':
+		stub->d.isPaused = true;
+		mDebuggerEnter(stub->d.p, DEBUGGER_ENTER_MANUAL, &info);
+		return parsed;
+	default:
+		_nak(stub);
+		return parsed;
+	}
+
+	int i;
+	char messageType = message[0];
+	for (i = 0; message[i] != '#'; ++i, ++parsed) {
+		checksum += message[i];
+	}
+	if (!message[i]) {
+		_nak(stub);
+		return parsed;
+	}
+	++i;
+	++parsed;
+	if (!message[i]) {
+		_nak(stub);
+		return parsed;
+	} else if (!message[i + 1]) {
+		++parsed;
+		_nak(stub);
+		return parsed;
+	}
+	parsed += 2;
+	int networkChecksum = _hex2int(&message[i], 2);
+	if (networkChecksum != checksum) {
+		mLOG(DEBUGGER, WARN, "Checksum error: expected %02x, got %02x", checksum, networkChecksum);
+		_nak(stub);
+		return parsed;
+	}
+
+	_ack(stub);
+	++message;
+	switch (messageType) {
+	case '?':
+		snprintf(stub->outgoing, GDB_STUB_MAX_LINE - 4, "S%02x", SIGINT);
+		_sendMessage(stub);
+		break;
+	case 'c':
+		_continue(stub, message);
+		break;
+	case 'G':
+		_writeGPRs(stub, message);
+		break;
+	case 'g':
+		_readGPRs(stub, message);
+		break;
+	case 'H':
+	case 'T':
+		// This is faked because we only have one thread
+		strncpy(stub->outgoing, "OK", GDB_STUB_MAX_LINE - 4);
+		_sendMessage(stub);
+		break;
+	case 'M':
+		_writeMemory(stub, message);
+		break;
+	case 'm':
+		_readMemory(stub, message);
+		break;
+	case 'P':
+		_writeRegister(stub, message);
+		break;
+	case 'p':
+		_readRegister(stub, message);
+		break;
+	case 'Q':
+		_processQWriteCommand(stub, message);
+		break;
+	case 'q':
+		_processQReadCommand(stub, message);
+		break;
+	case 's':
+		_step(stub, message);
+		break;
+	case 'V':
+		_processVWriteCommand(stub, message);
+		break;
+	case 'v':
+		_processVReadCommand(stub, message);
+		break;
+	case 'X':
+		_writeMemoryBinary(stub, message);
+		break;
+	case 'Z':
+		_setBreakpoint(stub, message);
+		break;
+	case 'z':
+		_clearBreakpoint(stub, message);
+		break;
+	default:
+		_error(stub, GDB_UNSUPPORTED_COMMAND);
+		break;
+	}
+	return parsed;
+}
+
+void GDBStubCreate(struct GDBStub* stub) {
+	stub->socket = INVALID_SOCKET;
+	stub->connection = INVALID_SOCKET;
+	stub->d.init = NULL;
+	stub->d.deinit = _gdbStubDeinit;
+	stub->d.paused = _gdbStubWait;
+	stub->d.update = _gdbStubUpdate;
+	stub->d.entered = _gdbStubEntered;
+	stub->d.custom = _gdbStubPoll;
+	stub->d.interrupt = NULL;
+	stub->d.type = DEBUGGER_GDB;
+	stub->untilPoll = GDB_STUB_INTERVAL;
+	stub->lineAck = GDB_ACK_PENDING;
+}
+
+bool GDBStubListen(struct GDBStub* stub, int port, const struct Address* bindAddress, enum GDBWatchpointsBehvaior watchpointsBehavior) {
+	if (!SOCKET_FAILED(stub->socket)) {
+		GDBStubShutdown(stub);
+	}
+	stub->socket = SocketOpenTCP(port, bindAddress);
+	if (SOCKET_FAILED(stub->socket)) {
+		mLOG(DEBUGGER, ERROR, "Couldn't open socket");
+		return false;
+	}
+	if (!SocketSetBlocking(stub->socket, false)) {
+		goto cleanup;
+	}
+	int err = SocketListen(stub->socket, 1);
+	if (err) {
+		goto cleanup;
+	}
+
+	stub->watchpointsBehavior = watchpointsBehavior;
+	memset(stub->memoryMapXml, 0, GDB_STUB_MAX_LINE);
+
+	return true;
+
+cleanup:
+	mLOG(DEBUGGER, ERROR, "Couldn't listen on port");
+	SocketClose(stub->socket);
+	stub->socket = INVALID_SOCKET;
+	return false;
+}
+
+void GDBStubHangup(struct GDBStub* stub) {
+	strncpy(stub->outgoing, "W00", GDB_STUB_MAX_LINE - 4);
+	_sendMessage(stub);
+	if (!SOCKET_FAILED(stub->connection)) {
+		SocketClose(stub->connection);
+		stub->connection = INVALID_SOCKET;
+	}
+	stub->d.needsCallback = false;
+	stub->d.isPaused = false;
+	mDebuggerUpdatePaused(stub->d.p);
+}
+
+void GDBStubShutdown(struct GDBStub* stub) {
+	GDBStubHangup(stub);
+	if (!SOCKET_FAILED(stub->socket)) {
+		SocketClose(stub->socket);
+		stub->socket = INVALID_SOCKET;
+	}
+}
+
+bool GDBStubUpdate(struct GDBStub* stub, int32_t timeoutMs) {
+	if (stub->socket == INVALID_SOCKET) {
+		stub->d.needsCallback = false;
+		stub->d.isPaused = false;
+		mDebuggerUpdatePaused(stub->d.p);
+		return false;
+	}
+	if (stub->connection == INVALID_SOCKET) {
+		if (timeoutMs) {
+			Socket reads = stub->socket;
+			SocketPoll(1, &reads, 0, 0, timeoutMs);
+		}
+		stub->connection = SocketAccept(stub->socket, 0);
+		if (!SOCKET_FAILED(stub->connection)) {
+			if (!SocketSetBlocking(stub->connection, false)) {
+				goto connectionLost;
+			}
+			mDebuggerEnter(stub->d.p, DEBUGGER_ENTER_ATTACHED, 0);
+		} else if (SocketWouldBlock()) {
+			return false;
+		} else {
+			goto connectionLost;
+		}
+		SocketSetTCPPush(stub->connection, 1);
+	}
+
+	if (timeoutMs) {
+		Socket reads = stub->connection;
+		SocketPoll(1, &reads, 0, 0, timeoutMs);
+	}
+	ssize_t messageLen = SocketRecv(stub->connection, stub->line, GDB_STUB_MAX_LINE - 1);
+	if (messageLen == 0) {
+		goto connectionLost;
+	}
+	if (messageLen == -1) {
+		if (SocketWouldBlock()) {
+			return false;
+		}
+		goto connectionLost;
+	}
+
+	stub->line[messageLen] = '\0';
+	mLOG(DEBUGGER, DEBUG, "< %s", stub->line);
+	ssize_t position = 0;
+	while (position < messageLen) {
+		position += _parseGDBMessage(stub, &stub->line[position]);
+	}
+	return true;
+
+connectionLost:
+	mLOG(DEBUGGER, WARN, "Connection lost");
+	GDBStubHangup(stub);
+	return false;
+}