Parse GDB packet format

src/debugger/gdb-stub.c

@@ -4,16 +4,146 @@
 #include <fcntl.h>
 #include <netinet/in.h>
 #include <stdio.h>
+#include <string.h>
 #include <sys/socket.h>
 #include <unistd.h>
 
-void _gdbStubDeinit(struct ARMDebugger* debugger) {
+enum GDBError {
+	GDB_NO_ERROR = 0x00,
+	GDB_UNSUPPORTED_COMMAND = 0x07
+};
+
+static void _gdbStubDeinit(struct ARMDebugger* debugger) {
 	struct GDBStub* stub = (struct GDBStub*) debugger;
 	if (stub->socket >= 0) {
 		GDBStubShutdown(stub);
 	}
 }
 
+static void _ack(struct GDBStub* stub) {
+	char ack = '+';
+	send(stub->connection, &ack, 1, 0);
+}
+
+static void _nak(struct GDBStub* stub) {
+	char nak = '-';
+	printf("Packet error\n");
+	send(stub->connection, &nak, 1, 0);
+}
+
+static int _hex2int(const char* hex) {
+	uint8_t dec = 0;
+	uint8_t letter;
+
+	letter = *hex - '0';
+	if (letter > 9) {
+		letter = *hex - 'a';
+		if  (letter > 5) {
+			return -1;
+		}
+		dec += letter + 10;
+	} else {
+		dec += letter;
+	}
+	++hex;
+	dec *= 0x10;
+
+	letter = *hex - '0';
+	if (letter > 9) {
+		letter = *hex - 'a';
+		if  (letter > 5) {
+			return -1;
+		}
+		dec += letter + 10;
+	} else {
+		dec += letter;
+	}
+	return dec;
+}
+
+static void _int2hex(uint8_t value, char* out) {
+	static const char language[] = "0123456789abcdef";
+	out[0] = language[value >> 4];
+	out[1] = language[value & 0xF];
+}
+
+static void _sendMessage(struct GDBStub* stub) {
+	if (stub->lineAck != GDB_ACK_OFF) {
+		stub->lineAck = GDB_ACK_PENDING;
+	}
+	uint8_t checksum = 0;
+	int i;
+	char buffer = stub->outgoing[0];
+	char swap;
+	stub->outgoing[0] = '$';
+	for (i = 1; 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] = '#';
+	_int2hex(checksum, &stub->outgoing[i + 1]);
+	stub->outgoing[GDB_STUB_MAX_LINE - 1] = 0;
+	printf("> %s\n", stub->outgoing);
+	send(stub->connection, stub->outgoing, i + 3, 0);
+}
+
+static void _error(struct GDBStub* stub, enum GDBError error) {
+	snprintf(stub->outgoing, GDB_STUB_MAX_LINE - 1, "E%02x", error);
+	_sendMessage(stub);
+}
+
+size_t _parseGDBMessage(struct GDBStub* stub, const char* message) {
+	uint8_t checksum = 0;
+	int parsed = 1;
+	printf("< %s\n", stub->line);
+	switch (*message) {
+	case '+':
+		stub->lineAck = GDB_ACK_RECEIVED;
+		return parsed;
+	case '-':
+		stub->lineAck = GDB_NAK_RECEIVED;
+		return parsed;
+	case '$':
+		++message;
+		break;
+	default:
+		_nak(stub);
+	}
+	for (; *message && *message != '#'; ++message, ++parsed) {
+		checksum += *message;
+	}
+	if (!*message) {
+		_nak(stub);
+		return parsed;
+	}
+	++message;
+	++parsed;
+	if (!message[0]) {
+		_nak(stub);
+		return parsed;
+	} else if (!message[1]) {
+		++parsed;
+		_nak(stub);
+		return parsed;
+	}
+	parsed += 2;
+	int networkChecksum = _hex2int(message);
+	if (networkChecksum != checksum) {
+		printf("Checksum error: expected %02x, got %02x\n", checksum, networkChecksum);
+		_nak(stub);
+		return parsed;
+	}
+	_ack(stub);
+	_error(stub, GDB_UNSUPPORTED_COMMAND);
+	return parsed;
+}
+
 void GDBStubCreate(struct GDBStub* stub) {
 	stub->socket = -1;
 	stub->connection = -1;
@@ -107,9 +237,11 @@ void GDBStubUpdate(struct GDBStub* stub) {
 			goto connectionLost;
 		}
 		stub->line[messageLen] = '\0';
-		printf("Received message: %s\n", stub->line);
+		ssize_t position = 0;
+		while (position < messageLen) {
+			position += _parseGDBMessage(stub, &stub->line[position]);
+		}
 	}
-	return;
 
 connectionLost:
 	// TODO: add logging support to the debugging interface

src/debugger/gdb-stub.h

@@ -5,10 +5,19 @@
 
 #define GDB_STUB_MAX_LINE 256
 
+enum GDBStubAckState {
+	GDB_ACK_PENDING = 0,
+	GDB_ACK_RECEIVED,
+	GDB_NAK_RECEIVED,
+	GDB_ACK_OFF
+};
+
 struct GDBStub {
 	struct ARMDebugger d;
 
 	char line[GDB_STUB_MAX_LINE];
+	char outgoing[GDB_STUB_MAX_LINE];
+	enum GDBStubAckState lineAck;
 
 	int socket;
 	int connection;