---

gdbremote.cpp

/*
 * This file is part of Jelie,
 * (c) 2002 Julien Pilet <julien.pilet@epfl.ch> and
 * Stephane Magnenat <stephane.magnenat@epfl.ch>
 *
 * 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;
}
        
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();
}

---