[v4,1/4] pdbg/gdbserver: Add in basic skeleton for a gdbserver on p8

diff --git a/.gitignore b/.gitignore
index c25b053..79f1ef9 100644
--- a/.gitignore
+++ b/.gitignore
@@ -39,3 +39,4 @@  optcmd_test
 *.trs
 *.log
 test-driver
+src/gdb_parser.c
diff --git a/Makefile.am b/Makefile.am
index 0d451d3..fb2b812 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -83,7 +83,9 @@  pdbg_SOURCES = \
 	src/optcmd.h \
 	src/options.h \
 	src/parsers.h \
-	src/progress.h
+	src/progress.h \
+	src/pdbgproxy.c \
+	src/gdb_parser.c
 
 src/main.c: $(DT_headers)
 
@@ -194,6 +196,10 @@  GEN_V = $(GEN_V_$(V))
 GEN_V_ = $(GEN_V_$(AM_DEFAULT_VERBOSITY))
 GEN_V_0 = @echo "  GEN     " $@;
 
+RAGEL_V = $(RAGEL_V_$(V))
+RAGEL_V_ = $(RAGEL_V_$(AM_DEFAULT_VERBOSITY))
+RAGEL_V_0 = @echo "  RAGEL   " $@;
+
 %.dts: %.dts.m4
 	$(M4_V)$(M4) -I$(dir $<) $< | $(DTC) -I dts -O dts > $@
 
@@ -214,4 +220,7 @@  p9z-fsi.dts: p9z-fsi.dts.m4 p9-fsi.dtsi
 %.dtb.o: %.dtb
 	$(AM_V_CC)$(CC) -c $(srcdir)/template.S -DSYMBOL_PREFIX=$(shell echo $@ | tr '.-' '_') -DFILENAME=\"$<\" -o $@
 
-MOSTLYCLEANFILES = *.dtb *.dts *.dt.h p9-fsi.dtsi
+%.c: %.rl
+	$(RAGEL_V)$(RAGEL) -o $@ $<
+
+MOSTLYCLEANFILES = *.dtb *.dts *.dt.h p9-fsi.dtsi src/gdb_parser.c
diff --git a/configure.ac b/configure.ac
index e6939e0..05c002e 100644
--- a/configure.ac
+++ b/configure.ac
@@ -17,6 +17,9 @@  if test x"$ac_cv_path_DTC" = x ; then
 fi
 AC_SUBST([DTC])
 
+AC_PATH_PROG([RAGEL], [ragel])
+AC_SUBST([RAGEL])
+
 AC_CONFIG_MACRO_DIR([m4])
 AC_CONFIG_HEADERS([config.h])
 AC_CONFIG_FILES([Makefile])
diff --git a/libpdbg/libpdbg.h b/libpdbg/libpdbg.h
index c54e852..1977c75 100644
--- a/libpdbg/libpdbg.h
+++ b/libpdbg/libpdbg.h
@@ -139,6 +139,7 @@  struct thread_regs {
 };
 
 int ram_putmsr(struct pdbg_target *target, uint64_t val);
+int ram_getmem(struct pdbg_target *thread, uint64_t addr, uint64_t *value);
 int ram_putnia(struct pdbg_target *target, uint64_t val);
 int ram_putspr(struct pdbg_target *target, int spr, uint64_t val);
 int ram_putgpr(struct pdbg_target *target, int spr, uint64_t val);
diff --git a/libpdbg/operations.h b/libpdbg/operations.h
index 4a6db2f..dc5042a 100644
--- a/libpdbg/operations.h
+++ b/libpdbg/operations.h
@@ -70,9 +70,6 @@ 
 
 #define MXSPR_SPR(opcode) (((opcode >> 16) & 0x1f) | ((opcode >> 6) & 0x3e0))
 
-/* GDB server functionality */
-int gdbserver_start(uint16_t port);
-
 enum fsi_system_type {FSI_SYSTEM_P8, FSI_SYSTEM_P9W, FSI_SYSTEM_P9R, FSI_SYSTEM_P9Z};
 enum chip_type get_chip_type(uint64_t chip_id);
 
diff --git a/libpdbg/target.h b/libpdbg/target.h
index c8da048..d1a6aec 100644
--- a/libpdbg/target.h
+++ b/libpdbg/target.h
@@ -165,4 +165,5 @@  struct chiplet {
 	int (*getring)(struct chiplet *, uint64_t, int64_t, uint32_t[]);
 };
 #define target_to_chiplet(x) container_of(x, struct chiplet, target)
+
 #endif
diff --git a/src/gdb_parser.rl b/src/gdb_parser.rl
new file mode 100644
index 0000000..6259b96
--- /dev/null
+++ b/src/gdb_parser.rl
@@ -0,0 +1,146 @@ 
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <assert.h>
+
+#include "src/pdbgproxy.h"
+
+%%{
+	machine gdb;
+
+	action reset {
+		cmd = 0;
+		rsp = NULL;
+		data = stack;
+		memset(stack, 0, sizeof(stack));
+		crc = 0;
+	}
+
+	action crc {
+		crc += *p;
+	}
+
+	action push {
+		data++;
+		assert(data < &stack[10]);
+	}
+
+	action hex_digit {
+		*data *= 16;
+
+		if (*p >= '0' && *p <= '9')
+			*data += *p - '0';
+		else if (*p >= 'a' && *p <= 'f')
+			*data += *p - 'a' + 10;
+		else if (*p >= 'A' && *p <= 'F')
+			*data += *p - 'A' + 10;
+	}
+
+	action end {
+		/* *data should point to the CRC */
+		if (crc != *data) {
+			printf("CRC error\n");
+			send_nack(priv);
+		} else {
+			printf("Cmd %d\n", cmd);
+			send_ack(priv);
+
+			/* Push the response onto the stack */
+			if (rsp)
+				*data = (uintptr_t)rsp;
+			else
+				*data = 0;
+
+			command_callbacks[cmd](stack, priv);
+		}
+	}
+
+	get_mem = ('m' @{cmd = GET_MEM;}
+		   xdigit+ $hex_digit %push
+		   ','
+		   xdigit+ $hex_digit %push);
+
+	put_mem = ('M' any* @{cmd = PUT_MEM;}
+		   xdigit+ $hex_digit %push
+		   ','
+		   xdigit+ $hex_digit %push
+		   ':'
+		   xdigit+ $hex_digit %push);
+
+	get_gprs = ('g' @{cmd = GET_GPRS;});
+
+	get_spr = ('p' @{cmd = GET_SPR;}
+		   xdigit+ $hex_digit %push);
+
+	stop_reason = ('?' @{cmd = STOP_REASON;});
+
+	set_thread = ('H' any* @{cmd = SET_THREAD;});
+
+	disconnect = ('D' @{cmd = DISCONNECT;}
+		     xdigit+ $hex_digit %push);
+
+	# TODO: We don't actually listen to what's supported
+	q_attached = ('qAttached:' xdigit* @{rsp = "1";});
+	q_C = ('qC' @{rsp = "QC1";});
+	q_supported = ('qSupported:' any* @{rsp = "multiprocess+;vContSupported+";});
+	qf_threadinfo = ('qfThreadInfo' @{rsp = "m1l";});
+
+	# vCont packet parsing
+	v_contq = ('vCont?' @{rsp = "vCont;c;C;s;S";});
+	v_contc = ('vCont;c' any* @{cmd = V_CONTC;});
+	v_conts = ('vCont;s' any* @{cmd = V_CONTS;});
+
+	interrupt = (3 @{command_callbacks[INTERRUPT](stack, priv);});
+
+	commands = (get_mem | get_gprs | get_spr | stop_reason | set_thread |
+		    q_attached | q_C | q_supported | qf_threadinfo | q_C |
+		    v_contq | v_contc | v_conts | put_mem | disconnect );
+
+	cmd = ((commands & ^'#'*) | ^'#'*) $crc
+	      ('#' xdigit{2} $hex_digit @end);
+
+	# We ignore ACK/NACK for the moment
+	ack = ('+');
+	nack = ('-');
+
+	main := (( ^('$' | interrupt)*('$' | interrupt) @reset) (cmd | ack | nack))*;
+
+}%%
+
+static enum gdb_command cmd = NONE;
+static uint64_t stack[10], *data = stack;
+static char *rsp;
+static uint8_t crc;
+static int cs;
+
+command_cb *command_callbacks;
+
+%%write data;
+
+void parser_init(command_cb *callbacks)
+{
+	%%write init;
+
+	command_callbacks = callbacks;
+}
+
+int parse_buffer(char *buf, size_t len, void *priv)
+{
+	char *p = buf;
+	char *pe = p + len + 1;
+
+	%%write exec;
+
+	return 0;
+}
+
+#if 0
+int main(int argc, char **argv)
+{
+	parser_init(NULL);
+
+	if (argc > 1)
+		parse_buffer(argv[1], strlen(argv[1]), NULL);
+	return 0;
+}
+#endif
diff --git a/src/main.c b/src/main.c
index 4966d51..8ee0e38 100644
--- a/src/main.c
+++ b/src/main.c
@@ -38,6 +38,7 @@ 
 #include "options.h"
 #include "optcmd.h"
 #include "progress.h"
+#include "pdbgproxy.h"
 
 #define PR_ERROR(x, args...) \
 	pdbg_log(PDBG_ERROR, x, ##args)
@@ -88,7 +89,7 @@  extern struct optcmd_cmd
 	optcmd_getring, optcmd_start, optcmd_stop, optcmd_step,
 	optcmd_threadstatus, optcmd_sreset, optcmd_regs, optcmd_probe,
 	optcmd_getmem, optcmd_putmem, optcmd_getxer, optcmd_putxer,
-	optcmd_getcr, optcmd_putcr;
+	optcmd_getcr, optcmd_putcr, optcmd_gdbserver;
 
 static struct optcmd_cmd *cmds[] = {
 	&optcmd_getscom, &optcmd_putscom, &optcmd_getcfam, &optcmd_putcfam,
@@ -97,7 +98,7 @@  static struct optcmd_cmd *cmds[] = {
 	&optcmd_getring, &optcmd_start, &optcmd_stop, &optcmd_step,
 	&optcmd_threadstatus, &optcmd_sreset, &optcmd_regs, &optcmd_probe,
 	&optcmd_getmem, &optcmd_putmem, &optcmd_getxer, &optcmd_putxer,
-	&optcmd_getcr, &optcmd_putcr,
+	&optcmd_getcr, &optcmd_putcr, &optcmd_gdbserver,
 };
 
 /* Purely for printing usage text. We could integrate printing argument and flag
@@ -136,6 +137,7 @@  static struct action actions[] = {
 	{ "threadstatus", "", "Print the status of a thread" },
 	{ "sreset",  "", "Reset" },
 	{ "regs",  "[--backtrace]", "State (optionally display backtrace)" },
+	{ "gdbserver", "", "Start a gdb server" },
 };
 
 static void print_usage(char *pname)
diff --git a/src/pdbgproxy.c b/src/pdbgproxy.c
new file mode 100644
index 0000000..5ccadfc
--- /dev/null
+++ b/src/pdbgproxy.c
@@ -0,0 +1,547 @@ 
+#define _GNU_SOURCE
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netdb.h>
+#include <inttypes.h>
+#include <assert.h>
+#include <getopt.h>
+#include <errno.h>
+#include <ccan/array_size/array_size.h>
+
+#include <backend.h>
+#include <operations.h>
+
+#include "pdbgproxy.h"
+#include "main.h"
+#include "optcmd.h"
+#include "debug.h"
+#include "chip.h"
+
+/* Maximum packet size */
+#define BUFFER_SIZE    	8192
+
+/* GDB packets */
+#define STR(e) "e"
+#define ACK "+"
+#define NACK "-"
+#define OK "OK"
+#define TRAP "S05"
+#define ERROR(e) "E"STR(e)
+
+#define TEST_SKIBOOT_ADDR 0x40000000
+
+static struct pdbg_target *thread_target = NULL;
+static struct timeval timeout;
+static int poll_interval = 100;
+static int fd = -1;
+static int littleendian = 1;
+enum client_state {IDLE, SIGNAL_WAIT};
+static enum client_state state = IDLE;
+
+static void destroy_client(int dead_fd);
+
+static uint8_t gdbcrc(char *data)
+{
+	uint8_t crc = 0;
+	int i;
+
+	for (i = 0; i < strlen(data); i++)
+		crc += data[i];
+
+	return crc;
+}
+
+static void send_response(int fd, char *response)
+{
+	int len;
+	char *result;
+
+	len = asprintf(&result, "$%s#%02x", response, gdbcrc(response));
+	PR_INFO("Send: %s\n", result);
+	send(fd, result, len, 0);
+	free(result);
+}
+
+void send_nack(void *priv)
+{
+	PR_INFO("Send: -\n");
+	send(fd, NACK, 1, 0);
+}
+
+void send_ack(void *priv)
+{
+	PR_INFO("Send: +\n");
+	send(fd, ACK, 1, 0);
+}
+
+static void set_thread(uint64_t *stack, void *priv)
+{
+	send_response(fd, OK);
+}
+
+static void stop_reason(uint64_t *stack, void *priv)
+{
+	send_response(fd, TRAP);
+}
+
+static void disconnect(uint64_t *stack, void *priv)
+{
+	PR_INFO("Terminating connection with client. pid %16" PRIi64 "\n", stack[0]);
+	send_response(fd, OK);
+}
+
+/* 32 registers represented as 16 char hex numbers with null-termination */
+#define REG_DATA_SIZE (32*16+1)
+static void get_gprs(uint64_t *stack, void *priv)
+{
+	char data[REG_DATA_SIZE] = "";
+	struct thread_regs regs;
+	int i;
+
+	if(ram_state_thread(thread_target, &regs))
+		PR_ERROR("Error reading gprs\n");
+
+	for (i = 0; i < 32; i++) {
+		PR_INFO("r%d = 0x%016" PRIx64 "\n", i, regs.gprs[i]);
+		snprintf(data + i*16, 17, "%016" PRIx64 , __builtin_bswap64(regs.gprs[i]));
+	}
+
+	send_response(fd, data);
+}
+
+static void get_spr(uint64_t *stack, void *priv)
+{
+	char data[REG_DATA_SIZE];
+	uint64_t value;
+
+	switch (stack[0]) {
+	case 0x40:
+		/* Get PC/NIA */
+		if (ram_getnia(thread_target, &value))
+			PR_ERROR("Error reading NIA\n");
+		snprintf(data, REG_DATA_SIZE, "%016" PRIx64 , __builtin_bswap64(value));
+		send_response(fd, data);
+		break;
+
+	case 0x41:
+		/* Get MSR */
+		if (ram_getmsr(thread_target, &value))
+			PR_ERROR("Error reading MSR\n");
+		snprintf(data, REG_DATA_SIZE, "%016" PRIx64 , __builtin_bswap64(value));
+		send_response(fd, data);
+		break;
+
+	case 0x42:
+		/* Get CR */
+		if (ram_getcr(thread_target, (uint32_t *)&value))
+			PR_ERROR("Error reading CR \n");
+		snprintf(data, REG_DATA_SIZE, "%016" PRIx64 , __builtin_bswap64(value));
+		send_response(fd, data);
+		break;
+
+	case 0x43:
+		/* Get LR */
+		if (ram_getspr(thread_target, 8, &value))
+			PR_ERROR("Error reading LR\n");
+		snprintf(data, REG_DATA_SIZE, "%016" PRIx64 , __builtin_bswap64(value));
+		send_response(fd, data);
+		break;
+
+	case 0x44:
+		/* Get CTR */
+		if (ram_getspr(thread_target, 9, &value))
+			PR_ERROR("Error reading CTR\n");
+		snprintf(data, REG_DATA_SIZE, "%016" PRIx64 , __builtin_bswap64(value));
+		send_response(fd, data);
+		break;
+
+	case 0x45:
+		/* We can't get the whole XER register in RAM mode as part of it
+ 		 * is in latches that we need to stop the clocks to get. Probably
+ 		 * not helpful to only return part of a register in a debugger so
+ 		 * return unavailable. */
+		send_response(fd, "xxxxxxxxxxxxxxxx");
+		break;
+
+	default:
+		send_response(fd, "xxxxxxxxxxxxxxxx");
+		break;
+	}
+}
+
+#define MAX_DATA 0x1000
+
+/* Returns a real address to use with adu_getmem or -1UL if we
+ * couldn't determine a real address. At the moment we only deal with
+ * kernel linear mapping but in future we could walk that page
+ * tables. */
+static uint64_t get_real_addr(uint64_t addr)
+{
+	if (GETFIELD(PPC_BITMASK(0, 3), addr) == 0xc)
+		/* Assume all 0xc... addresses are part of the linux linear map */
+		addr &= ~PPC_BITMASK(0, 1);
+	else if (addr < TEST_SKIBOOT_ADDR)
+		return addr;
+	else
+		addr = -1UL;
+
+	return addr;
+}
+
+static void get_mem(uint64_t *stack, void *priv)
+{
+	struct pdbg_target *adu;
+	uint64_t addr, len, linear_map;
+	int i, err = 0;
+	uint64_t data[MAX_DATA/sizeof(uint64_t)];
+	char result[2*MAX_DATA];
+
+	/* stack[0] is the address and stack[1] is the length */
+	addr = stack[0];
+	len = stack[1];
+
+	pdbg_for_each_class_target("adu", adu) {
+		if (pdbg_target_probe(adu) == PDBG_TARGET_ENABLED)
+			break;
+	}
+
+	if (adu == NULL) {
+		PR_ERROR("ADU NOT FOUND\n");
+		err=3;
+		goto out;
+	}
+
+	if (len > MAX_DATA) {
+		PR_INFO("Too much memory requested, truncating\n");
+		len = MAX_DATA;
+	}
+
+	if (!addr) {
+		err = 2;
+		goto out;
+	}
+
+	linear_map = get_real_addr(addr);
+	if (linear_map != -1UL) {
+		if (adu_getmem(adu, linear_map, (uint8_t *) data, len)) {
+			PR_ERROR("Unable to read memory\n");
+			err = 1;
+		}
+	} else {
+		/* Virtual address */
+		for (i = 0; i < len; i += sizeof(uint64_t)) {
+			if (ram_getmem(thread_target, addr, &data[i/sizeof(uint64_t)])) {
+				PR_ERROR("Fault reading memory\n");
+				err = 2;
+				break;
+			}
+		}
+	}
+
+out:
+	if (!err)
+		for (i = 0; i < len; i ++) {
+			sprintf(&result[i*2], "%02x", *(((uint8_t *) data) + i));
+		}
+	else
+		sprintf(result, "E%02x", err);
+
+	send_response(fd, result);
+}
+
+static void put_mem(uint64_t *stack, void *priv)
+{
+	struct pdbg_target *adu;
+	uint64_t addr, len;
+	uint8_t *data;
+	uint8_t attn_opcode[] = {0x00, 0x00, 0x02, 0x00};
+	int err = 0;
+	struct thread *thread = target_to_thread(thread_target);
+
+	if (littleendian) {
+		attn_opcode[1] = 0x02;
+		attn_opcode[2] = 0x00;
+	}
+
+	addr = stack[0];
+	len = stack[1];
+	data = (uint8_t *) &stack[2];
+
+	pdbg_for_each_class_target("adu", adu) {
+		if (pdbg_target_probe(adu) == PDBG_TARGET_ENABLED)
+			break;
+	}
+
+	if (adu == NULL) {
+		PR_ERROR("ADU NOT FOUND\n");
+		err=3;
+		goto out;
+	}
+
+	addr = get_real_addr(addr);
+	if (addr == -1UL) {
+		PR_ERROR("TODO: No virtual address support for putmem\n");
+		err = 1;
+		goto out;
+	}
+
+
+	if (len == 4 && stack[2] == 0x0810827d) {
+		/* According to linux-ppc-low.c gdb only uses this
+		 * op-code for sw break points so we replace it with
+		 * the correct attn opcode which is what we need for
+		 * breakpoints.
+		 *
+		 * TODO: Upstream a patch to gdb so that it uses the
+		 * right opcode for baremetal debug. */
+		PR_INFO("Breakpoint opcode detected, replacing with attn\n");
+		data = attn_opcode;
+
+		/* Need to enable the attn instruction in HID0 */
+		if (thread->enable_attn(thread_target))
+			goto out;
+	} else
+		stack[2] = __builtin_bswap64(stack[2]) >> 32;
+
+	PR_INFO("put_mem 0x%016" PRIx64 " = 0x%016" PRIx64 "\n", addr, stack[2]);
+
+	if (adu_putmem(adu, addr, data, len)) {
+		PR_ERROR("Unable to write memory\n");
+		err = 3;
+	}
+
+out:
+	if (err)
+		send_response(fd, ERROR(EPERM));
+	else
+		send_response(fd, OK);
+}
+
+static void v_conts(uint64_t *stack, void *priv)
+{
+	ram_step_thread(thread_target, 1);
+	send_response(fd, TRAP);
+}
+
+#define VCONT_POLL_DELAY 100000
+static void v_contc(uint64_t *stack, void *priv)
+{
+	ram_start_thread(thread_target);
+	state = SIGNAL_WAIT;
+	poll_interval = 1;
+}
+
+static void interrupt(uint64_t *stack, void *priv)
+{
+	PR_INFO("Interrupt\n");
+	ram_stop_thread(thread_target);
+	send_response(fd, TRAP);
+
+	return;
+}
+
+static void poll(void)
+{
+	uint64_t nia;
+	struct thread_state status;
+
+	thread_target->probe(thread_target);
+	status = thread_status(thread_target);
+
+	switch (state) {
+	case IDLE:
+		break;
+
+	case SIGNAL_WAIT:
+		if (!(status.quiesced))
+			break;
+
+		state = IDLE;
+		poll_interval = VCONT_POLL_DELAY;
+		if (!(status.active)) {
+			PR_ERROR("Thread inactive after trap\n");
+			send_response(fd, ERROR(EPERM));
+			return;
+		}
+
+		/* Restore NIA */
+		if (ram_getnia(thread_target, &nia))
+			PR_ERROR("Error during getnia\n");
+		if (ram_putnia(thread_target, nia - 4))
+			PR_ERROR("Error during putnia\n");
+		send_response(fd, TRAP);
+		break;
+	}
+}
+
+static void cmd_default(uint64_t *stack, void *priv)
+{
+	uintptr_t tmp = stack[0];
+	if (stack[0]) {
+		send_response(fd, (char *) tmp);
+	} else
+		send_response(fd, "");
+}
+
+static void create_client(int new_fd)
+{
+	PR_INFO("Client connected\n");
+	fd = new_fd;
+}
+
+static void destroy_client(int dead_fd)
+{
+	PR_INFO("Client disconnected\n");
+	close(dead_fd);
+	fd = -1;
+}
+
+static int read_from_client(int fd)
+{
+	char buffer[BUFFER_SIZE + 1];
+	int nbytes;
+
+	nbytes = read(fd, buffer, sizeof(buffer));
+	if (nbytes < 0) {
+		perror(__FUNCTION__);
+		return -1;
+	} else if (nbytes == 0) {
+		PR_INFO("0 bytes\n");
+		return -1;
+	} else {
+		buffer[nbytes] = '\0';
+		PR_INFO("%x\n", buffer[0]);
+		PR_INFO("Recv: %s\n", buffer);
+		parse_buffer(buffer, nbytes, &fd);
+	}
+
+	return 0;
+}
+
+command_cb callbacks[LAST_CMD + 1] = {
+	cmd_default,
+	get_gprs,
+	get_spr,
+	get_mem,
+	stop_reason,
+	set_thread,
+	v_contc,
+	v_conts,
+	put_mem,
+	interrupt,
+	disconnect,
+	NULL};
+
+int gdbserver_start(struct pdbg_target *target, uint16_t port)
+{
+	int sock, i;
+	struct sockaddr_in name;
+	fd_set active_fd_set, read_fd_set;
+
+	parser_init(callbacks);
+	thread_target = target;
+
+	sock = socket(PF_INET, SOCK_STREAM, 0);
+	if (sock < 0) {
+		perror(__FUNCTION__);
+		return -1;
+	}
+
+	name.sin_family = AF_INET;
+	name.sin_port = htons(port);
+	name.sin_addr.s_addr = htonl(INADDR_ANY);
+	if (bind(sock, (struct sockaddr *) &name, sizeof(name)) < 0) {
+		perror(__FUNCTION__);
+		return -1;
+	}
+
+	if (listen(sock, 1) < 0) {
+		perror(__FUNCTION__);
+		return -1;
+	}
+
+	FD_ZERO(&active_fd_set);
+	FD_SET(sock, &active_fd_set);
+
+	while (1) {
+		read_fd_set = active_fd_set;
+		timeout.tv_sec = 0;
+		timeout.tv_usec = poll_interval;
+		if (select(FD_SETSIZE, &read_fd_set, NULL, NULL, &timeout) < 0) {
+			perror(__FUNCTION__);
+			return -1;
+		}
+
+		for (i = 0; i < FD_SETSIZE; i++) {
+			if (FD_ISSET(i, &read_fd_set)) {
+				if (i == sock) {
+					int new;
+					new = accept(sock, NULL, NULL);
+					if (new < 0) {
+						perror(__FUNCTION__);
+						return -1;
+					}
+
+					if (fd > 0)
+						/* It only makes sense to accept a single client */
+						close(new);
+					else {
+						create_client(new);
+						FD_SET(new, &active_fd_set);
+					}
+				} else {
+					if (read_from_client(i) < 0) {
+						destroy_client(i);
+						FD_CLR(i, &active_fd_set);
+					}
+				}
+			}
+		}
+
+		poll();
+	}
+
+	return 1;
+}
+
+static int gdbserver(uint16_t port)
+{
+	struct pdbg_target *target = NULL;
+	uint64_t msr;
+	int rc;
+
+	for_each_class_target("thread", target) {
+		if (!target_selected(target))
+			continue;
+		if (pdbg_target_probe(target) == PDBG_TARGET_ENABLED)
+			break;
+	}
+	if (!target->class)
+		return -1;
+	assert(!strcmp(target->class, "thread"));
+	// Temporary until I can get this working a bit smoother on p9
+	if (strcmp(target->compatible, "ibm,power8-thread")) {
+		PR_ERROR("GDBSERVER is only tested on POWER8\n");
+		return -1;
+	}
+
+	/* Check endianess in MSR */
+	rc = ram_getmsr(target, &msr);
+	if (rc) {
+		PR_ERROR("Couldn't read the MSR. Are all threads on this chiplet quiesced?\n");
+		return 1;
+	}
+	littleendian = 0x01 & msr;
+
+	gdbserver_start(target, port);
+	return 0;
+}
+OPTCMD_DEFINE_CMD_WITH_ARGS(gdbserver, gdbserver, (DATA16));
diff --git a/src/pdbgproxy.h b/src/pdbgproxy.h
new file mode 100644
index 0000000..1fe67f2
--- /dev/null
+++ b/src/pdbgproxy.h
@@ -0,0 +1,13 @@ 
+#ifndef __PDBGPROXY_H
+#define __PDBGPROXY_H
+
+enum gdb_command {NONE, GET_GPRS, GET_SPR, GET_MEM,
+                 STOP_REASON, SET_THREAD, V_CONTC, V_CONTS,
+                 PUT_MEM, INTERRUPT, DISCONNECT, LAST_CMD};
+typedef void (*command_cb)(uint64_t *stack, void *priv);
+
+void parser_init(command_cb *callbacks);
+int parse_buffer(char *buf, size_t len, void *priv);
+void send_nack(void *priv);
+void send_ack(void *priv);
+#endif