gdbremote.cpp

linux下的jtag调试软件

/*
 * This file is part of Jelie,
 * (c) 2002 Julien Pilet <[e-mail]> and
 * Stephane Magnenat <[e-mail]>
 *
 * Jelie is free software; you can redistribute it
 * and/or modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of the License,
 * or (at your option) any later version.
 *
 * Jelie is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Foobar; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

/* \file
 * \brief gdb protocol talking
 *
 * This file contains the glue between gdb and jelie.
 */

#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include "jtagpxa250.h"
#include "gdbremote.h"
#include "debug.h"
#include "step.h"

GdbRemote::GdbRemote(JTAGpxa250 *pxa) : pxa(pxa), clientSocket(-1), running(false) {
	sSocket = -1;
	pxa->registerTargetReadyCallback(callback, this);
}

GdbRemote::~GdbRemote() {
	//if (sSocket != -1)
	//	close_socket(sSocket);
	if (clientSocket != -1)
		close_socket(clientSocket);
}

bool GdbRemote::openPort(int port) {
	return tcpstartsrv(port, 1) == 0;
}

int GdbRemote::prepareFD_SET(fd_set *set) {
	int max = 0;

	if (sSocket > 0) {
		FD_SET(sSocket, set);
		max = sSocket;
	}

	if (clientSocket > 0) {
		FD_SET(clientSocket, set);
		if (clientSocket > max) max = clientSocket;
	}
	return max;
}

void GdbRemote::processFD_SET(fd_set *set) {

	if (sSocket != -1 && FD_ISSET(sSocket, set)) {
		struct  sockaddr_in addr;
		socklen_t addrLen = sizeof(addr);
		int sock = accept(sSocket, (struct sockaddr *) &addr, &addrLen);
		if (sock == -1) {
			perror("accept()");
			fprintf(stderr, "TCP connection failed.\n");
		} else {
			if (clientSocket == -1) {
				// good. We can accept the connection.
				printf("TCP connection accepted from %s\n", inet_ntoa(addr.sin_addr) );
				clientSocket = sock;
			} else {
				printf("TCP connection rejected from %s: already connected.\n", 
						inet_ntoa(addr.sin_addr) );
				close_socket(sock);
			}
		}
	}

	if (clientSocket != -1 && FD_ISSET(clientSocket, set)) {
		bufferLength = read(clientSocket, buffer, sizeof(buffer) - 1);

		if ( bufferLength <= 0 ) {
			printf("Read error from TCP client. Closing connection.\n");
			close_socket(clientSocket);
			clientSocket = -1;
		} else {
			buffer[bufferLength] = 0;
			debugMessage(GDB_REMOTE_DEBUG, "<- \"%s\"\n", buffer);
			parse();
		}
	}
}

void GdbRemote::printError() {
	geterror();
}

void GdbRemote::parse() {

	reset();

	// check if the target is ready
	pxa->pollForTX();

	do {

		switch (current()) {
		case '+': break;
		case '-': printf("TCP: warning: GDB refused last packet\n"); break;
		case '$': parsePacket(); break;
		case 3: pxa->extBreak(); break;
		default:
			printf("TCP: parse error (%c %d)\n", current(), current());
		}

	} while(next());
}

void GdbRemote::parsePacket() {

	assert( current() == '$' );

	unsigned char sum = 0;

	unsigned char * packet = buffer + ptr;
	unsigned char * eopacket = (unsigned char *) index((char *)packet, '#');

	if (!eopacket) {
		fprintf(stderr, "TCP error: end of packet not found.\n");
		ptr = bufferLength;
		return;
	}

	int len = eopacket - packet - 1;

	for (int i=1; i <= len; ++i) {
		sum += packet[i];
	}
	
	unsigned char packetSum = strtol((char *)packet + len + 2, 0, 16);

	if (packetSum != sum) {
		printf("gdb protocol checksum error (%02X, %02X)\n", sum, packetSum);
		message[0] = '-';
		write(clientSocket, message, 1);
		debugMessage(GDB_REMOTE_DEBUG,"-> '-'\n");

		// eat the invalid packet
		while(current() != '#') next();
		ptr += 2;
	} else {
		// tell gdb we got the packet
		message[0] = '+';
		write(clientSocket, message, 1);
		debugMessage(GDB_REMOTE_DEBUG,"-> '+'\n");

		next(); // eat the '$'
		parseValidPacket();
	}
}

unsigned char GdbRemote::toAsciiHex(unsigned int v) {
	unsigned char hexChars[] = { '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' };
	return hexChars[v&0x0F];
}

void GdbRemote::sendPacket() {
	unsigned char sum = 0;
	int len = strlen(message) + 4;
	unsigned char *sendBuff = (unsigned char *) alloca(len + 1);
	unsigned char *p = sendBuff;

	*(p++) = '$';

	for (int i=0; message[i]; ++i) {
		*(p++) = message[i];
		sum += message[i];
	}
	*(p++) = '#';
	*(p++) = toAsciiHex(sum >> 4);
	*(p++) = toAsciiHex(sum & 0x0F);
	*p = 0;
	write(clientSocket, sendBuff, len);
	debugMessage(GDB_REMOTE_DEBUG,"-> '%s'\n", sendBuff);
}

void GdbRemote::hexPrintWord(unsigned char *str, unsigned int val) {

	for (int i=0; i<4; ++i) {
		str[i*2] = toAsciiHex(val >> ((i*8)+4));
		str[i*2+1] = toAsciiHex(val>>(i*8));
	}
}

unsigned char GdbRemote::asciiToHex(unsigned char c) {
	if (c >= 'a' && c <= 'f')
		return c - 'a' + 0xA;
	if (c >= '0' && c <= '9')
		return c - '0';
	if (c >= 'A' && c <= 'F')
		return c - 'A' + 0xA;
	return 0;
}

unsigned int GdbRemote::parseHexByte() {
	unsigned char h, l, c;

	h = asciiToHex(c=current());

	if (c!='#') 
		next();

	l = asciiToHex(c=current());

	if (c!='#') 
		next();

	return (h << 4) + l;
}
	
//! \TODO rewrite this function 
unsigned int GdbRemote::parseHexWord() {
	unsigned int r=0;

	for (int i=0; i < 4; ++i) {
		r += parseHexByte() << (i*8);
	}
	return r;
}

void GdbRemote::callback(void *p) {
	GdbRemote *t = (GdbRemote *) p;

	if (t->running) {
		jelie_cancel_bpt(); /* delete any "step" breakpoints */
		t->lastSignal();
	}

	t->running = false;
}

void GdbRemote::query() {
	next();

	if (memcmp(buffer + ptr, "Offsets", 7) == 0) {
		sprintf(message, "Text=%x;Data=%x;Bss=%x", 0,0,0);
		sendPacket();
		return;
	}

	// query not understood
	message[0] = 0;
	sendPacket();
}	

void GdbRemote::lastSignal() {
	sprintf(message, "T05");
	sendPacket();
}

void GdbRemote::setThread() {
	sprintf(message, "OK");
	sendPacket();
}

void GdbRemote::getRegisters() {
	message[0] = 0;
	char *p = message;
	unsigned int i;
	for (i =0; i<16; ++i) {
		hexPrintWord((unsigned char *)p, pxa->getSavePlace(i));
		p += 8;
	}
	for (i=0; i< ((96 * 8 + 32) / 8); ++i) {
		sprintf(p, "00");
		p+=2;
	}
	hexPrintWord((unsigned char *)p, pxa->getSavePlace(16));
	p[8]=0;
	sendPacket();
}

void GdbRemote::setRegisters() {
	unsigned i;

	// eat the 'G'
	next(); 

	for (i=0; i<16; ++i) {
		pxa->setSavePlace(i, parseHexWord());
	}

	// skip FPU regs
	for (i=0; i< ((96 * 8 + 32)/8); ++i)
		next();

	pxa->setSavePlace(16, parseHexWord());

	sprintf(message, "OK");
	sendPacket();
}

void GdbRemote::readMem() {

	// eat the 'm'
	next(); 

	unsigned char *nextStr;
	unsigned int addr, len, unaligned;

	addr = strtoul((char *)buffer + ptr, (char **) &nextStr, 16);
	unaligned = addr & 0x3;
	addr = addr & (~0x3);
	len = unaligned + strtoul((char *)nextStr + 1, 0, 16);

	if ((len) & 0x3) len = (len & ~0x3) + 4;
	
	if (2*len >= GDB_REMOTE_BUFFER_SIZE) {
		fprintf(stderr, "gdb remote: send buffer size too small to send %d bytes of memory !\n",
				len);
	}

	unsigned int *data = (unsigned int *) alloca(len);
	pxa->getData(addr, len / 4, data);
	nextStr = (unsigned char *)message;

	// do this mess if GDB tries to read an unaligned address
	for (unsigned int j= unaligned; j<4; j++) {
		unsigned int byte = data[0] >> (j*8);	// little endian
		nextStr[0] = toAsciiHex(byte >> 4);
		nextStr[1] = toAsciiHex(byte);
		nextStr+=2;
	}

	// now it's aligned. Much easier.
	for (unsigned int i=1; i < len/4; ++i) {
		hexPrintWord(nextStr, data[i]);
		nextStr+=8;
	}

	// terminate the string
	nextStr[0] = 0;

	sendPacket();
}

void GdbRemote::writeMem() {

	// eat the 'M'
	next();

	unsigned char *nextStr;
	unsigned int addr, len;

	// get the address
	addr = strtoul((char *)buffer + ptr, (char **)&nextStr, 16);

	// we want to deal with 32 bits aligned address. 
	unsigned int unaligned = addr & 0x3;
	addr = addr & (~0x3);
	len = unaligned + strtoul((char *)nextStr+1, (char **)&nextStr, 16);

	ptr = nextStr - buffer;
	next(); // skip the ':' or whatever separates length from data
	
	unsigned int *data = (unsigned int *) alloca(len + 4);

	// to write a partial word, we have to fetch the word, modify
	// the byte(s), and write it back. Fetch the begining word.
	if (unaligned) {
		pxa->getData(addr, 1, data);
		pxa->pollForTX();
	}

	// same problem at the end of the transmission
	if ((len & 0x3) && len > 4) {
		pxa->getData(addr + (len/4), 1, data + (len/4));
		pxa->pollForTX();
	}

	// now read bytes from the gdb packet
	for (unsigned int i=unaligned; i<len; i++) {
		unsigned int w =  i/4;
		unsigned int b =  i & 0x3;

		// replace the 8 concerned bits
		data[w] = (data[w] & ~(0xFF<<(b*8))) | (parseHexByte() << (b*8));
	}
	
	// HACK: replace undefined instruction by 'bkpt #0'
	if (len == 4 && data[0] == 0xe7ffdefe)
		data[0] = BKPT_0_INSTR;

	pxa->putData(addr, (len / 4) + (len&0x3 ? 1 : 0), data);

	sprintf(message, "OK");
	sendPacket();
}

void GdbRemote::resumeExec() {

	// if we're trying to exec a breakpoint, skip it.
	unsigned int instr;
	pxa->getData(pxa->getSavePlace(15), 1, &instr);
	pxa->pollForTX();

	if ((instr & 0xFFF000F0) == BKPT_0_INSTR)
		pxa->setSavePlace(15, pxa->getSavePlace(15) + 4);

	// now continue execution
	running= true;
	pxa->continueCmd();
}

void GdbRemote::cont() {

	next();

	if (current() != '#') {
		unsigned int addr; 

		// get the address
		addr = strtoul((char *)buffer + ptr, 0, 16);
		pxa->setSavePlace(15, addr);
	}

	// disable the hard break point #1, used for step by step.
	pxa->setCp15DebugRegister(0, JTAGpxa250::IBCR1);
	pxa->pollForTX();

	resumeExec();
}

void GdbRemote::step() {
	next();

	if (current() != '#') {
		unsigned int addr;

		// get the address
		addr = strtoul((char *)buffer + ptr, 0, 16);
		pxa->setSavePlace(15, addr);
	}
	pxa->setCp15DebugRegister(0, JTAGpxa250::IBCR1);
	pxa->pollForTX();

	jelie_set_bpt();
	resumeExec();
}
	

void GdbRemote::insertBreakWatch() {

	// eat the 'Z'
	next();
	if (current() == '1') {  // insert hardware breakpoint
		next(); // the '1'
		next(); // the ','
		unsigned int addr = strtoul((char *)buffer + ptr, 0, 16);
		for (int hb=0; hb<2; hb++) {
			if (hardBreak[hb] == 0) {
				pxa->setCp15DebugRegister(addr | 1, 
						hb==0 ? JTAGpxa250::IBCR0 : JTAGpxa250::IBCR1);
				hardBreak[hb] = addr;
				sprintf(message, "OK");
				sendPacket();
				return;
			}
		}
		// every hard break is taken :/
		sprintf(message, "E01");
		sendPacket();
		return;
	}

	// reply nothing if unsupported
	message[0] = 0;
	sendPacket();
}

void GdbRemote::removeBreakWatch() {
	next();

	if (current() == '1') {
		// remove hard break
		next(); // the '1'
		next(); // the ','
		unsigned int addr = strtoul((char *)buffer + ptr, 0, 16);
		for (int hb=0; hb<2; hb++) {
			if (hardBreak[hb] == addr) {
				hardBreak[hb] = 0;
				pxa->setCp15DebugRegister(0, 
					hb==0 ? JTAGpxa250::IBCR0 : JTAGpxa250::IBCR1);
				sprintf(message, "OK");
				sendPacket();
				return;
			}
			// hum.. trying to remove a non-existent breakpoint ?
			sprintf(message, "E02");
			sendPacket();
			return;
		}
	}
	// reply nothing if unsupported
	message[0] = 0;
	sendPacket();
}

void GdbRemote::parseValidPacket() {
	int startOfPacket = ptr;

	switch (current()) {
	case 'H': setThread(); break;
	case 'q': query(); break;
	case '?': lastSignal(); break;
	case 'g': getRegisters(); break;
	case 'G': setRegisters(); break;
	case 'm': readMem(); break;
	case 'M': writeMem(); break;
	case 'c': cont(); break;
	case 'Z': insertBreakWatch(); break;
	case 'z': removeBreakWatch(); break;
	case 's': step();break;
	default:
		printf("gdb command %c not understood.\n", current());
		message[0] = 0;
		sendPacket();
	}

	// eat the end of the packet
	ptr = startOfPacket;
	while(current() != '#') next();
	next();
	next();
}