// Copyright 2013 Dolphin Emulator Project
// Licensed under GPLv2+
// Refer to the license.txt file included.

// Originally written by Sven Peter <sven@fail0verflow.com> for anergistic.

#include <fcntl.h>
#include <stdarg.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#ifdef _WIN32
#include <ws2tcpip.h>
#include <iphlpapi.h>
#include <iphlpapi.h>
#else
#include <sys/select.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <netinet/in.h>
#endif

#include "Core/Host.h"
#include "Core/PowerPC/GDBStub.h"

#define GDB_BFR_MAX  10000
#define GDB_MAX_BP   10

#define GDB_STUB_START '$'
#define GDB_STUB_END   '#'
#define GDB_STUB_ACK   '+'
#define GDB_STUB_NAK   '-'


static int tmpsock = -1;
static int sock = -1;

static u8 cmd_bfr[GDB_BFR_MAX];
static u32 cmd_len;

static u32 sig = 0;
static u32 send_signal = 0;
static u32 step_break = 0;

typedef struct {
	u32 active;
	u32 addr;
	u32 len;
} gdb_bp_t;

static gdb_bp_t bp_x[GDB_MAX_BP];
static gdb_bp_t bp_r[GDB_MAX_BP];
static gdb_bp_t bp_w[GDB_MAX_BP];
static gdb_bp_t bp_a[GDB_MAX_BP];

// private helpers
static u8 hex2char(u8 hex)
{
	if (hex >= '0' && hex <= '9')
		return hex - '0';
	else if (hex >= 'a' && hex <= 'f')
		return hex - 'a' + 0xa;
	else if (hex >= 'A' && hex <= 'F')
		return hex - 'A' + 0xa;

	ERROR_LOG(GDB_STUB, "Invalid nibble: %c (%02x)\n", hex, hex);
	return 0;
}

static u8 nibble2hex(u8 n)
{
	n &= 0xf;
	if (n < 0xa)
		return '0' + n;
	else
		return 'A' + n - 0xa;
}

static void mem2hex(u8 *dst, u8 *src, u32 len)
{
	u8 tmp;

	while (len-- > 0) {
		tmp = *src++;
		*dst++ = nibble2hex(tmp>>4);
		*dst++ = nibble2hex(tmp);
	}
}

static void hex2mem(u8 *dst, u8 *src, u32 len)
{
	while (len-- > 0) {
		*dst++ = (hex2char(*src) << 4) | hex2char(*(src+1));
		src += 2;
	}
}

static u8 gdb_read_byte()
{
	ssize_t res;
	u8 c = '+';

	res = recv(sock, &c, 1, MSG_WAITALL);
	if (res != 1)
	{
		ERROR_LOG(GDB_STUB, "recv failed : %ld", res);
		gdb_deinit();
	}

	return c;
}

static u8 gdb_calc_chksum()
{
	u32 len = cmd_len;
	u8 *ptr = cmd_bfr;
	u8 c = 0;

	while (len-- > 0)
		c += *ptr++;

	return c;
}

static gdb_bp_t *gdb_bp_ptr(u32 type)
{
	switch (type) {
		case GDB_BP_TYPE_X:
			return bp_x;
		case GDB_BP_TYPE_R:
			return bp_x;
		case GDB_BP_TYPE_W:
			return bp_x;
		case GDB_BP_TYPE_A:
			return bp_x;
		default:
			return nullptr;
	}
}

static gdb_bp_t *gdb_bp_empty_slot(u32 type)
{
	gdb_bp_t *p;
	u32 i;

	p = gdb_bp_ptr(type);
	if (p == nullptr)
		return nullptr;

	for (i = 0; i < GDB_MAX_BP; i++)
	{
		if (p[i].active == 0)
			return &p[i];
	}

	return nullptr;
}

static gdb_bp_t *gdb_bp_find(u32 type, u32 addr, u32 len)
{
	gdb_bp_t *p;
	u32 i;

	p = gdb_bp_ptr(type);
	if (p == nullptr)
		return nullptr;

	for (i = 0; i < GDB_MAX_BP; i++)
	{
		if (p[i].active == 1 &&
			p[i].addr == addr &&
			p[i].len == len)
			return &p[i];
	}

	return nullptr;
}

static void gdb_bp_remove(u32 type, u32 addr, u32 len)
{
	gdb_bp_t *p;

	do
	{
		p = gdb_bp_find(type, addr, len);
		if (p != nullptr)
		{
			DEBUG_LOG(GDB_STUB, "gdb: removed a breakpoint: %08x bytes at %08x\n", len, addr);
			p->active = 0;
			memset(p, 0, sizeof(gdb_bp_t));
		}
	} while (p != nullptr);
}

static int gdb_bp_check(u32 addr, u32 type)
{
	gdb_bp_t *p;
	u32 i;

	p = gdb_bp_ptr(type);
	if (p == nullptr)
		return 0;

	for (i = 0; i < GDB_MAX_BP; i++)
	{
		if (p[i].active == 1 &&
			(addr >= p[i].addr && addr < p[i].addr + p[i].len))
			return 1;
	}

	return 0;
}

static void gdb_nak()
{
	const char nak = GDB_STUB_NAK;
	ssize_t res;

	res = send(sock, &nak, 1, 0);
	if (res != 1)
		ERROR_LOG(GDB_STUB, "send failed");
}

static void gdb_ack()
{
	const char ack = GDB_STUB_ACK;
	ssize_t res;

	res = send(sock, &ack, 1, 0);
	if (res != 1)
		ERROR_LOG(GDB_STUB, "send failed");
}

static void gdb_read_command()
{
	u8 c;
	u8 chk_read, chk_calc;

	cmd_len = 0;
	memset(cmd_bfr, 0, sizeof cmd_bfr);

	c = gdb_read_byte();
	if (c == '+')
	{
		//ignore ack
		return;
	}
	else if (c == 0x03)
	{
		CPU::Break();
		gdb_signal(SIGTRAP);
		return;
	}
	else if (c != GDB_STUB_START)
	{
		DEBUG_LOG(GDB_STUB, "gdb: read invalid byte %02x\n", c);
		return;
	}

	while ((c = gdb_read_byte()) != GDB_STUB_END)
	{
		cmd_bfr[cmd_len++] = c;
		if (cmd_len == sizeof cmd_bfr)
		{
			ERROR_LOG(GDB_STUB, "gdb: cmd_bfr overflow\n");
			gdb_nak();
			return;
		}
	}

	chk_read = hex2char(gdb_read_byte()) << 4;
	chk_read |= hex2char(gdb_read_byte());

	chk_calc = gdb_calc_chksum();

	if (chk_calc != chk_read)
	{
		ERROR_LOG(GDB_STUB, "gdb: invalid checksum: calculated %02x and read %02x for $%s# (length: %d)\n", chk_calc, chk_read, cmd_bfr, cmd_len);
		cmd_len = 0;

		gdb_nak();
		return;
	}

	DEBUG_LOG(GDB_STUB, "gdb: read command %c with a length of %d: %s\n", cmd_bfr[0], cmd_len, cmd_bfr);
	gdb_ack();
}

static int gdb_data_available() {
	struct timeval t;
	fd_set _fds, *fds = &_fds;

	FD_ZERO(fds);
	FD_SET(sock, fds);

	t.tv_sec = 0;
	t.tv_usec = 20;

	if (select(sock + 1, fds, nullptr, nullptr, &t) < 0)
	{
		ERROR_LOG(GDB_STUB, "select failed");
		return 0;
	}

	if (FD_ISSET(sock, fds))
		return 1;
	return 0;
}

static void gdb_reply(const char *reply)
{
	u8 chk;
	u32 left;
	u8 *ptr;
	int n;

	if (!gdb_active())
		return;

	memset(cmd_bfr, 0, sizeof cmd_bfr);

	cmd_len = strlen(reply);
	if (cmd_len + 4 > sizeof cmd_bfr)
		ERROR_LOG(GDB_STUB, "cmd_bfr overflow in gdb_reply");

	memcpy(cmd_bfr + 1, reply, cmd_len);

	cmd_len++;
	chk = gdb_calc_chksum();
	cmd_len--;
	cmd_bfr[0] = GDB_STUB_START;
	cmd_bfr[cmd_len + 1] = GDB_STUB_END;
	cmd_bfr[cmd_len + 2] = nibble2hex(chk >> 4);
	cmd_bfr[cmd_len + 3] = nibble2hex(chk);

	DEBUG_LOG(GDB_STUB, "gdb: reply (len: %d): %s\n", cmd_len, cmd_bfr);

	ptr = cmd_bfr;
	left = cmd_len + 4;
	while (left > 0)
	{
		n = send(sock, ptr, left, 0);
		if (n < 0)
		{
			ERROR_LOG(GDB_STUB, "gdb: send failed");
			return gdb_deinit();
		}
		left -= n;
		ptr += n;
	}
}

static void gdb_handle_query()
{
	DEBUG_LOG(GDB_STUB, "gdb: query '%s'\n", cmd_bfr+1);

	if (!strcmp((const char *)(cmd_bfr+1), "TStatus"))
	{
		return gdb_reply("T0");
	}

	gdb_reply("");
}

static void gdb_handle_set_thread()
{
	if (memcmp(cmd_bfr, "Hg0", 3) == 0 ||
		memcmp(cmd_bfr, "Hc-1", 4) == 0 ||
		memcmp(cmd_bfr, "Hc0", 4) == 0 ||
		memcmp(cmd_bfr, "Hc1", 4) == 0)
		return gdb_reply("OK");
	gdb_reply("E01");
}

static void gdb_handle_signal()
{
	char bfr[128];
	memset(bfr, 0, sizeof bfr);
	sprintf(bfr, "T%02x%02x:%08x;%02x:%08x;", sig, 64, PC, 1, GPR(1));
	gdb_reply(bfr);
}

static void wbe32hex(u8 *p, u32 v)
{
	u32 i;
	for (i = 0; i < 8; i++)
		p[i] =  nibble2hex(v >> (28 - 4*i));
}

static void wbe64hex(u8 *p, u64 v)
{
	u32 i;
	for (i = 0; i < 16; i++)
		p[i] =  nibble2hex(v >> (60 - 4*i));
}

static u32 re32hex(u8 *p)
{
	u32 i;
	u32 res = 0;

	for (i = 0; i < 8; i++)
		res = (res << 4) | hex2char(p[i]);

	return res;
}

static u64 re64hex(u8 *p)
{
	u32 i;
	u64 res = 0;

	for (i = 0; i < 16; i++)
		res = (res << 4) | hex2char(p[i]);

	return res;
}

static void gdb_read_register()
{
	static u8 reply[64];
	u32 id;

	memset(reply, 0, sizeof reply);
	id = hex2char(cmd_bfr[1]);
	if (cmd_bfr[2] != '\0')
	{
		id <<= 4;
		id |= hex2char(cmd_bfr[2]);
	}


	switch (id) {
		case 0 ... 31:
			wbe32hex(reply, GPR(id));
			break;
		case 32 ... 63:
			wbe64hex(reply, riPS0(id-32));
			break;
		case 64:
			wbe32hex(reply, PC);
			break;
		case 65:
			wbe32hex(reply, MSR);
			break;
		case 66:
			wbe32hex(reply, GetCR());
			break;
		case 67:
			wbe32hex(reply, LR);
			break;
		case 68:
			wbe32hex(reply, CTR);
			break;
		case 69:
			wbe32hex(reply, PowerPC::ppcState.spr[SPR_XER]);
			break;
		case 70:
			wbe32hex(reply, 0x0BADC0DE);
			break;
		case 71:
			wbe32hex(reply, FPSCR.Hex);
			break;
		default:
			return gdb_reply("E01");
			break;
	}

	gdb_reply((char *)reply);
}

static void gdb_read_registers()
{
	static u8 bfr[GDB_BFR_MAX - 4];
	u8 * bufptr = bfr;
	u32 i;

	memset(bfr, 0, sizeof bfr);

	for (i = 0; i < 32; i++)
	{
		wbe32hex(bufptr + i*8, GPR(i));
	}
	bufptr += 32 * 8;

	/*
	for (i = 0; i < 32; i++)
	{
		wbe32hex(bufptr + i*8, riPS0(i));
	}
	bufptr += 32 * 8;
	wbe32hex(bufptr, PC);      bufptr += 4;
	wbe32hex(bufptr, MSR);     bufptr += 4;
	wbe32hex(bufptr, GetCR()); bufptr += 4;
	wbe32hex(bufptr, LR);      bufptr += 4;


	wbe32hex(bufptr, CTR);     bufptr += 4;
	wbe32hex(bufptr, PowerPC::ppcState.spr[SPR_XER]); bufptr += 4;
	// MQ register not used.
	wbe32hex(bufptr, 0x0BADC0DE); bufptr += 4;
	*/


	gdb_reply((char *)bfr);
}

static void gdb_write_registers()
{
	u32 i;
	u8 * bufptr = cmd_bfr;

	for (i = 0; i < 32; i++)
	{
		GPR(i) = re32hex(bufptr + i*8);
	}
	bufptr += 32 * 8;

	gdb_reply("OK");
}

static void gdb_write_register()
{
	u32 id;

	u8 * bufptr = cmd_bfr + 3;

	id = hex2char(cmd_bfr[1]);
	if (cmd_bfr[2] != '=')
	{
		++bufptr;
		id <<= 4;
		id |= hex2char(cmd_bfr[2]);
	}

	switch (id) {
		case 0 ... 31:
			GPR(id) = re32hex(bufptr);
			break;
		case 32 ... 63:
			riPS0(id-32) = re64hex(bufptr);
			break;
		case 64:
			PC = re32hex(bufptr);
			break;
		case 65:
			MSR = re32hex(bufptr);
			break;
		case 66:
			SetCR(re32hex(bufptr));
			break;
		case 67:
			LR = re32hex(bufptr);
			break;
		case 68:
			CTR = re32hex(bufptr);
			break;
		case 69:
			PowerPC::ppcState.spr[SPR_XER] = re32hex(bufptr);
			break;
		case 70:
			// do nothing, we dont have MQ
			break;
		case 71:
			FPSCR.Hex = re32hex(bufptr);
			break;
		default:
			return gdb_reply("E01");
			break;
	}

	gdb_reply("OK");
}

static void gdb_read_mem()
{
	static u8 reply[GDB_BFR_MAX - 4];
	u32 addr, len;
	u32 i;

	i = 1;
	addr = 0;
	while (cmd_bfr[i] != ',')
		addr = (addr << 4) | hex2char(cmd_bfr[i++]);
	i++;

	len = 0;
	while (i < cmd_len)
		len = (len << 4) | hex2char(cmd_bfr[i++]);
	DEBUG_LOG(GDB_STUB, "gdb: read memory: %08x bytes from %08x\n", len, addr);

	if (len*2 > sizeof reply)
		gdb_reply("E01");
	u8 * data = Memory::GetPointer(addr);
	if (!data)
		return gdb_reply("E0");
	mem2hex(reply, data, len);
	reply[len*2] = '\0';
	gdb_reply((char *)reply);
}

static void gdb_write_mem()
{
	u32 addr, len;
	u32 i;

	i = 1;
	addr = 0;
	while (cmd_bfr[i] != ',')
		addr = (addr << 4) | hex2char(cmd_bfr[i++]);
	i++;

	len = 0;
	while (cmd_bfr[i] != ':')
		len = (len << 4) | hex2char(cmd_bfr[i++]);
	DEBUG_LOG(GDB_STUB, "gdb: write memory: %08x bytes to %08x\n", len, addr);

	u8 * dst = Memory::GetPointer(addr);
	if (!dst)
		return gdb_reply("E00");
	hex2mem(dst, cmd_bfr + i + 1, len);
	gdb_reply("OK");
}

// forces a break on next instruction check
void gdb_break()
{
	step_break = 1;
	send_signal = 1;
}

static void gdb_step()
{
	gdb_break();
}

static void gdb_continue()
{
	send_signal = 1;
}

bool gdb_add_bp(u32 type, u32 addr, u32 len)
{
	gdb_bp_t *bp;
	bp = gdb_bp_empty_slot(type);
	if (bp == nullptr)
		return false;

	bp->active = 1;
	bp->addr = addr;
	bp->len = len;

	DEBUG_LOG(GDB_STUB, "gdb: added %d breakpoint: %08x bytes at %08x\n", type, bp->len, bp->addr);
	return true;
}

static void _gdb_add_bp()
{
	u32 type;
	u32 i, addr = 0, len = 0;

	type = hex2char(cmd_bfr[1]);
	switch (type)
	{
		case 0:
		case 1:
			type = GDB_BP_TYPE_X;
			break;
		case 2:
			type = GDB_BP_TYPE_W;
			break;
		case 3:
			type = GDB_BP_TYPE_R;
			break;
		case 4:
			type = GDB_BP_TYPE_A;
			break;
		default:
			return gdb_reply("E01");
	}

	i = 3;
	while (cmd_bfr[i] != ',')
		addr = addr << 4 | hex2char(cmd_bfr[i++]);
	i++;

	while (i < cmd_len)
		len = len << 4 | hex2char(cmd_bfr[i++]);

	if (!gdb_add_bp(type, addr, len))
		return gdb_reply("E02");
	gdb_reply("OK");
}

static void gdb_remove_bp()
{
	u32 type, addr, len, i;

	type = hex2char(cmd_bfr[1]);
	switch (type) {
		case 0:
		case 1:
			type = GDB_BP_TYPE_X;
			break;
		case 2:
			type = GDB_BP_TYPE_W;
			break;
		case 3:
			type = GDB_BP_TYPE_R;
			break;
		case 4:
			type = GDB_BP_TYPE_A;
			break;
		default:
			return gdb_reply("E01");
	}

	addr = 0;
	len = 0;

	i = 3;
	while (cmd_bfr[i] != ',')
		addr = (addr << 4) | hex2char(cmd_bfr[i++]);
	i++;

	while (i < cmd_len)
		len = (len << 4) | hex2char(cmd_bfr[i++]);

	gdb_bp_remove(type, addr, len);
	gdb_reply("OK");
}

void gdb_handle_exception()
{
	while (gdb_active())
	{
		if (!gdb_data_available())
			continue;
		gdb_read_command();
		if (cmd_len == 0)
			continue;

		switch (cmd_bfr[0]) {
			case 'q':
				gdb_handle_query();
				break;
			case 'H':
				gdb_handle_set_thread();
				break;
			case '?':
				gdb_handle_signal();
				break;
			case 'k':
				gdb_deinit();
				INFO_LOG(GDB_STUB, "killed by gdb");
				return;
			case 'g':
				gdb_read_registers();
				break;
			case 'G':
				gdb_write_registers();
				break;
			case 'p':
				gdb_read_register();
				break;
			case 'P':
				gdb_write_register();
				break;
			case 'm':
				gdb_read_mem();
				break;
			case 'M':
				gdb_write_mem();
				PowerPC::ppcState.iCache.Reset();
				Host_UpdateDisasmDialog();
				break;
			case 's':
				gdb_step();
				return;
			case 'C':
			case 'c':
				gdb_continue();
				return;
			case 'z':
				gdb_remove_bp();
				break;
			case 'Z':
				_gdb_add_bp();
				break;
			default:
				gdb_reply("");
				break;
		}
	}
}

#ifdef _WIN32
WSADATA InitData;
#endif

// exported functions

static void gdb_init_generic(int domain,
	const sockaddr *server_addr, socklen_t server_addrlen,
	sockaddr *client_addr, socklen_t *client_addrlen);

#ifndef _WIN32
void gdb_init_local(const char *socket)
{
	unlink(socket);

	sockaddr_un addr = {};
	addr.sun_family = AF_UNIX;
	strcpy(addr.sun_path, socket);

	gdb_init_generic(PF_LOCAL, (const sockaddr *)&addr, sizeof(addr),
		NULL, NULL);
}
#endif

void gdb_init(u32 port)
{
	sockaddr_in saddr_server = {};
	sockaddr_in saddr_client;

	saddr_server.sin_family = AF_INET;
	saddr_server.sin_port = htons(port);
	saddr_server.sin_addr.s_addr = INADDR_ANY;

	socklen_t client_addrlen = sizeof(saddr_client);

	gdb_init_generic(PF_INET,
		(const sockaddr *)&saddr_server, sizeof(saddr_server),
		(sockaddr *)&saddr_client, &client_addrlen);

	saddr_client.sin_addr.s_addr = ntohl(saddr_client.sin_addr.s_addr);
	/*if (((saddr_client.sin_addr.s_addr >> 24) & 0xff) != 127 ||
	*      ((saddr_client.sin_addr.s_addr >> 16) & 0xff) !=   0 ||
	*      ((saddr_client.sin_addr.s_addr >>  8) & 0xff) !=   0 ||
	*      ((saddr_client.sin_addr.s_addr >>  0) & 0xff) !=   1)
	*      ERROR_LOG(GDB_STUB, "gdb: incoming connection not from localhost");
	*/
}

static void gdb_init_generic(int domain,
	const sockaddr *server_addr, socklen_t server_addrlen,
	sockaddr *client_addr, socklen_t *client_addrlen)
{
	int on;
	#ifdef _WIN32
	WSAStartup(MAKEWORD(2,2), &InitData);
	#endif

	memset(bp_x, 0, sizeof bp_x);
	memset(bp_r, 0, sizeof bp_r);
	memset(bp_w, 0, sizeof bp_w);
	memset(bp_a, 0, sizeof bp_a);

	tmpsock = socket(domain, SOCK_STREAM, 0);
	if (tmpsock == -1)
		ERROR_LOG(GDB_STUB, "Failed to create gdb socket");

	on = 1;
	if (setsockopt(tmpsock, SOL_SOCKET, SO_REUSEADDR, &on, sizeof on) < 0)
		ERROR_LOG(GDB_STUB, "Failed to setsockopt");

	if (bind(tmpsock, server_addr, server_addrlen) < 0)
		ERROR_LOG(GDB_STUB, "Failed to bind gdb socket");

	if (listen(tmpsock, 1) < 0)
		ERROR_LOG(GDB_STUB, "Failed to listen to gdb socket");

	INFO_LOG(GDB_STUB, "Waiting for gdb to connect...\n");

	sock = accept(tmpsock, client_addr, client_addrlen);
	if (sock < 0)
		ERROR_LOG(GDB_STUB, "Failed to accept gdb client");
	INFO_LOG(GDB_STUB, "Client connected.\n");

	close(tmpsock);
	tmpsock = -1;
}


void gdb_deinit()
{
	if (tmpsock != -1)
	{
		shutdown(tmpsock, SHUT_RDWR);
		tmpsock = -1;
	}
	if (sock != -1)
	{
		shutdown(sock, SHUT_RDWR);
		sock = -1;
	}

	#ifdef _WIN32
	WSACleanup();
	#endif
}

bool gdb_active()
{
	return tmpsock != -1 || sock != -1;
}

int gdb_signal(u32 s)
{
	if (sock == -1)
		return 1;

	sig = s;

	if (send_signal)
	{
		gdb_handle_signal();
		send_signal = 0;
	}

	return 0;
}

int gdb_bp_x(u32 addr)
{
	if (sock == -1)
		return 0;

	if (step_break)
	{
		step_break = 0;

		DEBUG_LOG(GDB_STUB, "Step was successful.");
		return 1;
	}

	return gdb_bp_check(addr, GDB_BP_TYPE_X);
}

int gdb_bp_r(u32 addr)
{
	if (sock == -1)
		return 0;

	return gdb_bp_check(addr, GDB_BP_TYPE_R);
}

int gdb_bp_w(u32 addr)
{
	if (sock == -1)
		return 0;

	return gdb_bp_check(addr, GDB_BP_TYPE_W);
}

int gdb_bp_a(u32 addr)
{
	if (sock == -1)
		return 0;

	return gdb_bp_check(addr, GDB_BP_TYPE_A);
}
