gdbserver.cpp

Source Code of jlinkserver for Segger JLink

	#include "stdafx.h"

using namespace std;

#define NUM_REGS 26

static int USRRegisterMap[] = {
	ARM_REG_R0,
	ARM_REG_R1,
	ARM_REG_R2,
	ARM_REG_R3,
	ARM_REG_R4,
	ARM_REG_R5,
	ARM_REG_R6,
	ARM_REG_R7,
	ARM_REG_R8_USR,
	ARM_REG_R9_USR,
	ARM_REG_R10_USR,
	ARM_REG_R11_USR,
	ARM_REG_R12_USR,
	ARM_REG_R13_USR,
	ARM_REG_R14_USR,
	ARM_REG_R15,
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(32 / 4),
	ARM_REG_CPSR
};

static int FIQRegisterMap[] = {
	ARM_REG_R0,
	ARM_REG_R1,
	ARM_REG_R2,
	ARM_REG_R3,
	ARM_REG_R4,
	ARM_REG_R5,
	ARM_REG_R6,
	ARM_REG_R7,
	ARM_REG_R8_FIQ,
	ARM_REG_R9_FIQ,
	ARM_REG_R10_FIQ,
	ARM_REG_R11_FIQ,
	ARM_REG_R12_FIQ,
	ARM_REG_R13_FIQ,
	ARM_REG_R14_FIQ,
	ARM_REG_R15,
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(32 / 4),
	ARM_REG_CPSR
};

static int IRQRegisterMap[] = {
	ARM_REG_R0,
	ARM_REG_R1,
	ARM_REG_R2,
	ARM_REG_R3,
	ARM_REG_R4,
	ARM_REG_R5,
	ARM_REG_R6,
	ARM_REG_R7,
	ARM_REG_R8_USR,
	ARM_REG_R9_USR,
	ARM_REG_R10_USR,
	ARM_REG_R11_USR,
	ARM_REG_R12_USR,
	ARM_REG_R13_IRQ,
	ARM_REG_R14_IRQ,
	ARM_REG_R15,
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(32 / 4),
	ARM_REG_CPSR
};

static int SVCRegisterMap[] = {
	ARM_REG_R0,
	ARM_REG_R1,
	ARM_REG_R2,
	ARM_REG_R3,
	ARM_REG_R4,
	ARM_REG_R5,
	ARM_REG_R6,
	ARM_REG_R7,
	ARM_REG_R8_USR,
	ARM_REG_R9_USR,
	ARM_REG_R10_USR,
	ARM_REG_R11_USR,
	ARM_REG_R12_USR,
	ARM_REG_R13_SVC,
	ARM_REG_R14_SVC,
	ARM_REG_R15,
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(32 / 4),
	ARM_REG_CPSR
};

static int ABTRegisterMap[] = {
	ARM_REG_R0,
	ARM_REG_R1,
	ARM_REG_R2,
	ARM_REG_R3,
	ARM_REG_R4,
	ARM_REG_R5,
	ARM_REG_R6,
	ARM_REG_R7,
	ARM_REG_R8_USR,
	ARM_REG_R9_USR,
	ARM_REG_R10_USR,
	ARM_REG_R11_USR,
	ARM_REG_R12_USR,
	ARM_REG_R13_ABT,
	ARM_REG_R14_ABT,
	ARM_REG_R15,
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(32 / 4),
	ARM_REG_CPSR
};

static int UNDRegisterMap[] = {
	ARM_REG_R0,
	ARM_REG_R1,
	ARM_REG_R2,
	ARM_REG_R3,
	ARM_REG_R4,
	ARM_REG_R5,
	ARM_REG_R6,
	ARM_REG_R7,
	ARM_REG_R8_USR,
	ARM_REG_R9_USR,
	ARM_REG_R10_USR,
	ARM_REG_R11_USR,
	ARM_REG_R12_USR,
	ARM_REG_R13_UND,
	ARM_REG_R14_UND,
	ARM_REG_R15,
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(32 / 4),
	ARM_REG_CPSR
};

static int SYSRegisterMap[] = {
	ARM_REG_R0,
	ARM_REG_R1,
	ARM_REG_R2,
	ARM_REG_R3,
	ARM_REG_R4,
	ARM_REG_R5,
	ARM_REG_R6,
	ARM_REG_R7,
	ARM_REG_R8_USR,
	ARM_REG_R9_USR,
	ARM_REG_R10_USR,
	ARM_REG_R11_USR,
	ARM_REG_R12_USR,
	ARM_REG_R13_SYS,
	ARM_REG_R14_SYS,
	ARM_REG_R15,
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(96 / 4),
	-(32 / 4),
	ARM_REG_CPSR
};


void DumpLinkRegisters(void) {
	printf("LR: USR=%08X, FIQ=%08X IRQ=%08X SVC=%08X ABT=%08X UND=%08X\n",
		JLINKARM_ReadReg(ARM_REG_R14_USR),
		JLINKARM_ReadReg(ARM_REG_R14_FIQ),
		JLINKARM_ReadReg(ARM_REG_R14_IRQ),
		JLINKARM_ReadReg(ARM_REG_R14_SVC),
		JLINKARM_ReadReg(ARM_REG_R14_ABT),
		JLINKARM_ReadReg(ARM_REG_R14_UND));
}



/** Write a hex value into a string
*/
char* GdbServer::WriteHex(unsigned int value, int bytes, char* pointer) {
	for (int i = 0; i < bytes; i++) {
		*pointer++ = WriteHex((value >> 4) & 0x0f);
		*pointer++ = WriteHex(value & 0x0f);
		value >>= 8;
	}
	return pointer;
}


/** Create a server attached to the socket
*/
GdbServer::GdbServer(SOCKET socket, JTAG* jtag) {
	this->socket = socket;
	this->jtag = jtag;
	npos = 0;
	nlen = 0;
	mlen = 0;
	memory = NULL;
	packetid = 0;
	flashing = 0;
	for (int i = 0; i < (sizeof(hardBreakpoints) / sizeof(unsigned int)); i++) {
		hardBreakpoints[i] = 0xffffffff;
	}
	numHardBreakpoints = 0;

	for (int i = 0; i < (sizeof(softBreakpoints) / sizeof(unsigned int)); i++) {
		softBreakpoints[i] = 0xffffffff;
	}
	numSoftBreakpoints = 0;


}

/** Run the debugger
*/
void GdbServer::Run() {
	bool running = true;

	while (running) {
//		printf("Read packet\n");
		if (ReadPacket() < 0) {
			break;
		}


//		printf("Packet %d -> '%-10.10s'\n", packetid++, buffer);

		switch (buffer[0]) {
			default:
				MakePacket("");
				break;

			case '?':
				MakeStopReply('T', 5);
				break;

			case 'g':
				ReadRegisters();
				break;

			case 's':
				Step();
//				printf("Step\n");
				MakeStopReply('T', 5);
//				DumpLinkRegisters();
				break;

			case 'm':
				RetrieveMemory();
				break;

			case 'M':
				ModifyMemory();
				break;

			case 'p':
				ReadRegister();
				break;

			case 'P':
				WriteRegister();
				break;

			case 'G':
				MakePacket("E01");
				break;

			case 'c':
				Continue();
				MakeStopReply('T', 5);
//				DumpLinkRegisters();
				break;

			case 'z':
				switch (buffer[1]) {
			case '0':
				ClearSoftBreakpoint();
				break;

			case '1':
				ClearHardBreakpoint();
				break;

			default:
				MakePacket("");
				break;
				};
				break;

			case 'Z':
				switch (buffer[1]) {
			case '0':
				SetSoftBreakpoint();
				break;

			case '1':
				SetHardBreakpoint();
				break;

			default:
				MakePacket("");
				break;
				}
				break;

			case 'q':
				switch (buffer[1]) {
			case 'f':
				MakePacket("m0,l");
				break;

			case 's':
				MakePacket("l");
				break;

			case 'O':
				MakePacket("Text=0;Data=0;Bss=0");
				break;

			case 'C':
				MakePacket("QC0");
				break;

			default:
				MakePacket("");
				break;

				}

				break;

		}


		WritePacket();


	}
}

int GdbServer::DataAvailable() {
	fd_set readfds;
	fd_set exceptfds;
	struct timeval timeout;

	FD_ZERO(&readfds);
	FD_ZERO(&exceptfds);

	FD_SET(socket, &readfds);
	FD_SET(socket, &exceptfds);

	timeout.tv_sec = 1;
	timeout.tv_usec = 0;

	int status = select((int)socket, &readfds, NULL, &exceptfds, &timeout);
	printf("Status = %d\n", status);
	return (status > 0);
}

int GdbServer::ReadCharacter() {
	if (npos >= nlen) {
		npos = 0;
		nlen = recv(socket, nbuf, sizeof(nbuf), 0);

		if (nlen <= 0) return -1;
	}

	return nbuf[npos++];
}

/** Read the packet, returning -1 on error
*/
int GdbServer::ReadPacket() {

	// Attempt to read a valid packet
	while (true) {

		// Read to the start of a packet
		while (true) {
			int ch;

			ch = ReadCharacter();
			if (ch == '$') break;

			if (ch < 1) return -1;
		}

		// Now read the packet
		char* position = buffer;
		unsigned char checksum = 0;

		while (true) {
			int ch = ReadCharacter();

			if (ch < 0) return -1;

			if (ch == '#') break;

			*position++ = ch;
			checksum += ch;
		}

		*position = 0;

		// Read and compare the checksums
		unsigned char readcsum = (ReadHex(ReadCharacter()) << 4) + (ReadHex(ReadCharacter()));
		if (checksum == readcsum) {
			break;
		}

		cerr.setf(ios_base::hex);
		cerr << "Invalid checksum. Calculated 0x" << checksum << ", read 0x" 
					<< readcsum << ", '" << buffer << "'" << endl;

		WriteCharacters("-", 1);
    }

	// Successful packet
	WriteCharacters("+", 1);

	return (int)strlen(buffer);

}

/** Send a response to the debugger
	@param msg The message to send
*/
int GdbServer::WriteCharacters(const char* msg, int length) {
//	cerr << "WriteCharacters '" << msg << "'" << endl;

	return send(socket, msg, length, 0);
}


int GdbServer::ReadHex(int ch) {
	if ((ch >= '0') && (ch <= '9')) {
		return ch - '0';
	}
	if ((ch >= 'a') && (ch <= 'f')) {
		return ch - 'a' + 10;
	}
	return 0;
}

char GdbServer::WriteHex(int value) {
	if (value < 10) {
		return '0' + value;
	}
	return 'a' + value - 10;
}

void GdbServer::MakeStopReply(char state, int signal) {
	char* pointer = buffer + 1;

	*pointer++ = state;
	*pointer++ = WriteHex((signal & 0xF0) >> 4);
	*pointer++ = WriteHex(signal & 0x0F);

	int* registerMap = RegisterMap();


	// The expedited registers
	pointer = WriteHex(11, 1, pointer);
	*pointer++ = ':';
	pointer = WriteHex(JLINKARM_ReadReg((ARM_REG)registerMap[11]), 4, pointer);
	*pointer++ = ';';

	pointer = WriteHex(13, 1, pointer);
	*pointer++ = ':';
	pointer = WriteHex(JLINKARM_ReadReg((ARM_REG)registerMap[13]), 4, pointer);
	*pointer++ = ';';

	pointer = WriteHex(15, 1, pointer);
	*pointer++ = ':';
	pointer = WriteHex(JLINKARM_ReadReg((ARM_REG)registerMap[15]), 4, pointer);
	*pointer++ = ';';

	*pointer++ = 0;
}

void GdbServer::MakePacket(const char* packet) {
	strcpy(buffer + 1, packet);
}


int GdbServer::WritePacket() {
	// Checksum the packet in the buffer
	char* pointer = buffer;
	*pointer++ = '$';

	unsigned char checksum = 0;
	while (*pointer) {
		checksum += *pointer++;
	}

	// Finish the packet
	*pointer++ = '#';
	*pointer++ = WriteHex((checksum & 0xf0) >> 4);
	*pointer++ = WriteHex(checksum & 0x0f);
	*pointer = '\0';

	// Transmit the packet until a positive ack is received
	do {
//		cerr << "Writing '" << buffer << "'" << endl;

		if (WriteCharacters(buffer, (int)(pointer - buffer)) != (int)(pointer - buffer)) {
			return -1;
		}

		// Wait for the valid response
		int ch = ReadCharacter();
		if (ch == -1) {
			cerr << "Read -1" << endl;
		}
		else {
//			cerr << "Read '" << (char)ch << "'" << endl;
		}

		switch (ch) {
			case -1: 
				return -1;

			case '+':
				return 0;

			case '\003':