gdbserver.cpp

Source Code of jlinkserver for Segger JLink

				return -1;
		}
	}
	while (1);
}

/** Clear a breakpoint
*/
void GdbServer::ClearHardBreakpoint() {
	unsigned int address = 0;
	char* pointer = buffer + 3;

	while (*pointer != ',') {
		address <<= 4;
		address |= ReadHex(*pointer++) & 0x0f;
	}


	int index = -1;
	for (index = 0; index < 2; index++) {
		if (hardBreakpoints[index] == address) break;
	}
	if (index == 2) {
		cerr << "Failed to clear breakpoint" << endl;
		MakePacket("E02");
	}
	else {
		cerr.setf(ios_base::hex);
		cerr << "Clear breakpoint at 0x" << address << endl;

		JLINKARM_ClrBP(index);
		hardBreakpoints[index] = 0xffffffff;
		numHardBreakpoints--;
		MakePacket("OK");
	}

}


/** Set a breakpoint
*/
void GdbServer::SetHardBreakpoint() {
	unsigned int address = 0;
	char* pointer = buffer + 3;

	while (*pointer != ',') {
		address <<= 4;
		address |= ReadHex(*pointer++) & 0x0f;
	}


	int index = -1;
	for (index = 0; index < 2; index++) {
		if (hardBreakpoints[index] == 0xffffffff) break;
	}
	if (index == 2) {
		cerr << "Failed to set breakpoint" << endl;
		MakePacket("E02");
	}
	else {
		cerr.setf(ios_base::hex);
		cerr << "Set breakpoint at 0x" << address << endl;

		JLINKARM_SetBP(index, address);
		hardBreakpoints[index] = address;
		numHardBreakpoints++;
		MakePacket("OK");
	}

}

/** Read the register number
*/
int* GdbServer::RegisterMap() {
	unsigned int psr = JLINKARM_ReadReg(ARM_REG_CPSR);
	int* registerMap = NULL;

	switch (psr & 0x1F) {
		case 0x00: 
		case 0x10:
		default:
			registerMap = USRRegisterMap; 
			break;

		case 0x01:
		case 0x11:
			registerMap = FIQRegisterMap; 
			break;

		case 0x02:
		case 0x12:
			registerMap = IRQRegisterMap; 
			break;

		case 0x03:
		case 0x13:
			registerMap = SVCRegisterMap; 
			break;

		case 0x17: 
			registerMap = ABTRegisterMap; 
			break;

		case 0x1B: 
			registerMap = UNDRegisterMap; 
			break;

		case 0x1F: 
			registerMap = USRRegisterMap; 
			break;
	}

	return registerMap;
}



/** Make a packet full of registers
*/
void GdbServer::ReadRegisters() {
	char* pointer = buffer + 1;

	int* registerMap = RegisterMap();


	for (int reg = 0; reg < NUM_REGS; reg++) {
		int regnum = registerMap[reg];
		if (regnum < 0) {
			// Cope with zero spacing
			for (int i = 0; i < -regnum; i++) {
				*pointer++ = '0';
			}
		}
		else {
			unsigned int value = JLINKARM_ReadReg((ARM_REG)regnum);
			if ((ARM_REG)regnum == ARM_REG_R15) {
				fprintf(stderr, "PC = 0x%08X\n", value);
			}

			pointer = WriteHex(value, 4, pointer);
		}
	}
	*pointer++ = 0;
}

void GdbServer::Step() {
	if (flashing) {
		printf("Resetting\n");
		Reset();
		flashing = 0;
	}

	JLINKARM_Step();
}

void GdbServer::Reset() {
	jtag->Initialise();
	jtag->MapRam(0);
}

void GdbServer::Go() {
	JLINKARM_Go();
}

void GdbServer::Halt() {
	JLINKARM_Halt();
}


void GdbServer::RetrieveMemory() {
	unsigned int address = 0;
	unsigned int length = 0;
	char* pointer = buffer + 1;

	while (*pointer != ',') {
		address <<= 4;
		address |= ReadHex(*pointer++) & 0x0f;
	}

	pointer++;

	while (*pointer) {
		length <<= 4;
		length |= ReadHex(*pointer++) & 0x0f;
	}

//	printf("Read from 0x%08X for 0x%08X bytes\n", address, length);

	if (length > mlen) {
		delete[] memory;
		memory = new unsigned char[length];
		mlen = length;
	}

	JLINKARM_ReadMem(address, length, memory);

	// Update any memory blocks with breakpoints
	for (unsigned int pos = 0; pos < length; pos++) {
		for (int bp = 0; bp < NUM_SOFT_BREAKPOINTS; bp++) {
			if (softBreakpoints[bp] == 0xffffffff) continue;

			if ((address + pos) == softBreakpoints[bp]) {
				*((int*)(memory + pos)) = 0xfedeffe7;
				pos += 3;
				break;
			}
		}

	}

	pointer = buffer + 1;

	for (unsigned int i = 0; i < length; i++) {
		*pointer++ = WriteHex((memory[i] & 0xf0) >> 4);
		*pointer++ = WriteHex(memory[i] & 0x0f);
	}
	*pointer++ = 0;
}


/** Clear a soft breakpoint
*/
void GdbServer::ClearSoftBreakpoint() {
	unsigned int address = 0;
	char* pointer = buffer + 3;

	while (*pointer != ',') {
		address <<= 4;
		address |= ReadHex(*pointer++) & 0x0f;
	}


	int index = -1;
	for (index = 0; index < 2; index++) {
		if (softBreakpoints[index] == address) break;
	}
	if (index == 2) {
		cerr << "Failed to clear breakpoint" << endl;
		MakePacket("E02");
	}
	else {
		cerr.setf(ios_base::hex);
		cerr << "Clear breakpoint at 0x" << address << endl;

		softBreakpoints[index] = 0xffffffff;
		numSoftBreakpoints--;
		MakePacket("OK");
	}

}


/** Set a breakpoint
*/
void GdbServer::SetSoftBreakpoint() {
	unsigned int address = 0;
	char* pointer = buffer + 3;

	while (*pointer != ',') {
		address <<= 4;
		address |= ReadHex(*pointer++) & 0x0f;
	}


	int index = -1;
	for (index = 0; index < 2; index++) {
		if (softBreakpoints[index] == 0xffffffff) break;
	}
	if (index == 2) {
		cerr << "Failed to set breakpoint" << endl;
		MakePacket("E02");
	}
	else {
		cerr.setf(ios_base::hex);
		cerr << "Set breakpoint at 0x" << address << endl;

		softBreakpoints[index] = address;
		numSoftBreakpoints++;
		MakePacket("OK");
	}
}

void GdbServer::ModifyMemory(unsigned int address, unsigned int length, char* pointer) {
	unsigned char* memory;
	unsigned int leadingBytes = 0;
	unsigned int trailingBytes = 0;
	unsigned int total;

	total = length;

	if (address & 0x03) {
		leadingBytes = address & 0x03;
		address -= leadingBytes;
		total += leadingBytes;
	}

	if (total & 0x03) {
		trailingBytes = 4 - (total & 0x03);
		total += trailingBytes;
	}

	memory = new unsigned char[total];

	unsigned int pos = 0;
	for (unsigned int i = 0; i < leadingBytes; i++) {
		memory[pos++] = 0xff;
	}

	for (unsigned int i = 0; i < length; i++) {
		memory[pos++] = (ReadHex(*pointer++) << 4) | ReadHex(*pointer++);
	}

	for (unsigned int i = 0; i < trailingBytes; i++) {
		memory[pos++] = 0xff;
	}

	// Erase the flash if required
	if (!flashing && ((address & 0x0FFFFF) < 128)) {
		printf("Initialising for flashing\n");
		jtag->InitialiseForFlashing();
		flashing = 1;
	}

	jtag->FlashWrite(address, total, memory);

	delete[] memory;

	MakePacket("OK");
}


void GdbServer::ModifyMemory() {
	unsigned int address = 0;
	unsigned int bytes = 0;
	unsigned int value = 0;

	char* pointer = buffer + 1;

	while (*pointer != ',') {
		address <<= 4;
		address |= ReadHex(*pointer++) & 0x0f;
	}

	pointer++;

	while (*pointer != ':') {
		bytes <<= 4;
		bytes |= ReadHex(*pointer++) & 0x0f;
	}

	pointer++;

	if (flashing || (bytes != 4)) {
		ModifyMemory(address, bytes, pointer);
		return;
	}


	for (int i = 0; i < 8; i++) {
		value <<= 4;
		value |= ReadHex(*pointer++) & 0x0f;
	}

	if (value == 0xFEDEFFE7) {
		for (int index = 0; index < NUM_SOFT_BREAKPOINTS; index++) {
			if (softBreakpoints[index] == 0xffffffff) {
				softBreakpoints[index] = address;
				numSoftBreakpoints++;
				printf("Set breakpoint at 0x%08X\n", address);
				MakePacket("OK");
				return;
			}
		}
	}
	else {
		if (address == 0xffffffff) {
			MakePacket("E00");
			return;
		}

		for (int index = 0; index < NUM_SOFT_BREAKPOINTS; index++) {
			if (softBreakpoints[index] == address) {
				softBreakpoints[index] = 0xffffffff;
				numSoftBreakpoints--;
				printf("Clear breakpoint at 0x%08X\n", address);
				MakePacket("OK");
				return;
			}
		}
	}
	MakePacket("");

}

/** Run using hardware breakpoints only
*/
void GdbServer::RunUsingHardBreakpoints() {
	bool used[NUM_HARD_BREAKPOINTS];

	printf("Running using hardware breakpoints: Hard = %d, Soft = %d\n", 
		numHardBreakpoints, numSoftBreakpoints);

	for (int i = 0; i < NUM_HARD_BREAKPOINTS; i++) {
		used[i] = false;
	}



	if (numSoftBreakpoints > 0) {
		for (int i = 0; i < NUM_SOFT_BREAKPOINTS; i++) {
			if (softBreakpoints[i] != 0xffffffff) {
				for (int j = 0; j < NUM_HARD_BREAKPOINTS; j++) {
					if (!used[j]) {
						used[j] = true;
						JLINKARM_SetBP(j, softBreakpoints[i]);
						break;
					}
				}
			}
		}
	}

#if 0
	if (numHardBreakpoints + numSoftBreakpoints < 2) {
		int hard = 0;
		for (int i = 0; i < NUM_HARD_BREAKPOINTS; i++) {
			if (used[i]) {
				hard = i;
				break;
			}
		}

		// Allow breakpoints on writing/reading to low memory
		hard = 1 - i;
		JLINKARM_WriteICEReg(8 + (hard * 8), 0x00000000, 0);
		JLINKARM_WriteICEReg(9 + (hard * 8), 0x000000FF, 0);
		JLINKARM_WriteICEReg(10 + (hard * 8), 0x00000000, 0);
		JLINKARM_WriteICEReg(11 + (hard * 8), 0xFFFFFFFF, 0);
		JLINKARM_WriteICEReg(12 + (hard * 8), 0x00000100, 0);
		JLINKARM_WriteICEReg(13 + (hard * 8), 0x000000FF, 0);
	}

	for (int i = 0; i < 6; i++) {
		printf("ICR Reg[%d] = { %08X, %08X }\n", i,
			JLINKARM_ReadICEReg(8 + i),
			JLINKARM_ReadICEReg(16 + i));
	}
#endif

	JLINKARM_Go();

	while (!JLINKARM_IsHalted()) {
		if (DataAvailable()) {
			JLINKARM_Halt();
			break;
		}
		// spin
	}
	printf("Run to: 0x%08X\n", JLINKARM_ReadReg(ARM_REG_R15));


	if (numSoftBreakpoints > 0) {
		for (int j = 0; j < NUM_HARD_BREAKPOINTS; j++) {
			if (used[j]) {
				JLINKARM_ClrBP(j);
			}
		}
	}
}

/** Continue running
*/
void GdbServer::Continue() {
	if (flashing) {
		Reset();
		flashing = 0;
	}

	if (1 && ((numSoftBreakpoints + numHardBreakpoints) <= NUM_HARD_BREAKPOINTS)) {
		RunUsingHardBreakpoints();
	}
	else {
		bool hit = false;
		int max = 0;
		int count = 0;
		for (int i = 0; i < NUM_SOFT_BREAKPOINTS; i++) {
			if (softBreakpoints[i] != 0xffffffff) max = i;
		}

		printf("Continue using stepping: %d breakpoints\n", max);


		do {
			JLINKARM_Step();

			unsigned int pc = JLINKARM_ReadReg(ARM_REG_R15);
//			printf("Continuing, pc = 0x%08X\r", pc);



			for (int index = 0; index <= max; index++) {
				if (pc == softBreakpoints[index]) {
					hit = true;
					break;
				}
			}

//			if ((count == 0) || !hit) {
//				JLINKARM_Step();
//			}

			count++;

		}
		while (!hit);
		printf("Finished at %08X\n", JLINKARM_ReadReg(ARM_REG_R15));
	}
}

/** Read a register
*/
void GdbServer::ReadRegister() {
	int regnum = 0;
	char* pointer = buffer + 1;

	while (*pointer) {
		regnum <<= 4;
		regnum |= ReadHex(*pointer++) & 0x0f;
	}

	int index = RegisterMap()[regnum];

	pointer = buffer + 1;
	if (index < 0) {
		// Cope with zero spacing
		for (int i = 0; i < -index; i++) {
			*pointer++ = '0';
		}
	}
	else {
		unsigned int value = JLINKARM_ReadReg((ARM_REG)index);
		pointer = WriteHex(value, 4, pointer);
	}
	*pointer++ = '\0';
}

void GdbServer::WriteRegister() {
	int regnum = 0;
	unsigned int value = 0;
	char* pointer = buffer + 1;

	while (*pointer != '=') {
		regnum <<= 4;
		regnum |= ReadHex(*pointer++) & 0x0f;
	}
	pointer++;

	while (*pointer) {
		value <<= 4;
		value |= ReadHex(*pointer++) & 0x0f;
	}


	int index = RegisterMap()[regnum];

	if (index >= 0) {
		JLINKARM_WriteReg((ARM_REG)index, value);
	}
	MakePacket("OK");
}