src/GdbServer.cpp

   1 /*
   2  *      avr-sim: An atmel AVR simulator
   3  *  Copyright (C) 2008  Tom Haber
   4  *
   5  *  This program is free software: you can redistribute it and/or modify
   6  *  it under the terms of the GNU General Public License as published by
   7  *  the Free Software Foundation, either version 3 of the License, or
   8  *  (at your option) any later version.
   9
  10  *  This program is distributed in the hope that it will be useful,
  11  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13  *  GNU General Public License for more details.
  14  *
  15  *  You should have received a copy of the GNU General Public License
  16  *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
  17  */
  18
  19 #include "GdbServer.h"
  20 #include "Format.h"
  21 #include "Device.h"
  22 #include "DebugInterface.h"
  23 #include "AccessViolation.h"
  24 #include "Instruction.h"
  25
  26 #include <iostream>
  27 #include <stdio.h>
  28 #include <string.h>
  29
  30 #ifdef WINDOWS
  31 #   include <winsock.h>
  32 #   pragma comment(lib, "wsock32.lib")
  33 #else
  34 #       include <errno.h>
  35 #       include <signal.h>
  36 #   include <sys/socket.h>
  37 #   include <netinet/in.h>
  38 #   include <fcntl.h>
  39 #   include <netinet/tcp.h>
  40 #       include <netdb.h>
  41 #       include <arpa/inet.h>
  42 #       define closesocket(a) close( (a) )
  43 #endif
  44
  45 enum {
  46         BUF_SIZ            = 800,                 /* Buffering size for read operations. */
  47     MAX_BUF        = 400,         /* Maximum size of read/write buffers. */
  48     MAX_READ_RETRY = 50,          /* Maximum number of retries if a read is incomplete. */
  49
  50     MEM_SPACE_MASK = 0x00ff0000,  /* mask to get bits which determine memory space */
  51     FLASH_OFFSET   = 0x00000000,  /* Data in flash has this offset from gdb */
  52     SRAM_OFFSET    = 0x00800000,  /* Data in sram has this offset from gdb */
  53     EEPROM_OFFSET  = 0x00810000,  /* Data in eeprom has this offset from gdb */
  54
  55     GDB_BLOCKING_OFF = 0,         /* Signify that a read is non-blocking. */
  56     GDB_BLOCKING_ON  = 1,         /* Signify that a read will block. */
  57
  58     GDB_RET_NOTHING_RECEIVED = 5, /* if the read in non blocking receives nothing, we have nothing to do */
  59     GDB_RET_SINGLE_STEP = 4,     /* do one single step in gdb loop */
  60     GDB_RET_CONTINUE    = 3,     /* step until another command from gdb is received */
  61     GDB_RET_CTRL_C       = 2,    /* gdb has sent Ctrl-C to interrupt what is doing */
  62     GDB_RET_KILL_REQUEST = 1,    /* gdb has requested that sim be killed */
  63     GDB_RET_OK           =  0     /* continue normal processing of gdb requests */
  64         /* means that we should NOT execute any step!!! */
  65 };
  66
  67 namespace avr {
  68
  69         GdbServer::GdbServer(sim::SimulationClock & clock, int port)
  70                         : clock(clock), current(0), sock(-1), conn(-1) {
  71
  72                 lastReply = 0;
  73                 openSocket(port);
  74                 runMode = GDB_RET_OK;
  75         }
  76
  77         GdbServer::~GdbServer() {
  78                 closeSocket();
  79         }
  80
  81         void GdbServer::add(Device *dev) {
  82                 DebugInterface *dbgi = dev->debugInterface();
  83                 devices.push_back( dbgi );
  84         }
  85
  86         void GdbServer::openSocket(int port) {
  87                 if( (sock = socket(PF_INET, SOCK_STREAM, 0)) < 0 )
  88                         throw GdbException( "Can't create socket" );
  89
  90                 int i = 1;
  91                 setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &i, sizeof(i));
  92                 //fcntl(sock, F_SETFL, fcntl(sock, F_GETFL, 0) | O_NONBLOCK);
  93
  94                 struct sockaddr_in address;
  95                 address.sin_family = AF_INET;
  96                 address.sin_port = htons(port);
  97                 memset( &address.sin_addr, 0, sizeof(address.sin_addr) );
  98
  99                 if( bind( sock, (struct sockaddr *)&address, sizeof(address) ) < 0 )
 100                         throw GdbException( "Can't bind socket" );
 101
 102                 if( listen(sock, 1) < 0 )
 103                         throw GdbException( "Can't listen on socket" );
 104
 105                 std::cout << "Waiting on port " << port
 106                                 << " for gdb client to connect..." << std::endl;
 107
 108                 blockingOn = true;
 109         }
 110
 111         void GdbServer::closeSocket() {
 112                 if( conn != -1 ) {
 113                         closesocket(conn);
 114                         conn = -1;
 115                 }
 116
 117                 if( sock != -1 ) {
 118                         closesocket(sock);
 119                         sock = -1;
 120                 }
 121
 122         }
 123
 124         void GdbServer::acceptClient() {
 125                 struct sockaddr_in address;
 126             socklen_t addrLength = sizeof(struct sockaddr *);
 127
 128             conn = accept( sock, (struct sockaddr *)&address, &addrLength );
 129                 if( conn > 0 ) {
 130                         /* Tell TCP not to delay small packets.  This greatly speeds up
 131                          interactive response. WARNING: If TCP_NODELAY is set on, then gdb
 132                          may timeout in mid-packet if the (gdb)packet is not sent within a
 133                          single (tcp)packet, thus all outgoing (gdb)packets _must_ be sent
 134                          with a single call to write. (see Stevens "Unix Network
 135                          Programming", Vol 1, 2nd Ed, page 202 for more info) */
 136
 137                         int i = 1;
 138                         setsockopt(conn, IPPROTO_TCP, TCP_NODELAY, &i, sizeof (i));
 139
 140                         std::cout << "Connection opened by host "
 141                                                 << inet_ntoa( address.sin_addr )
 142                                                 << ", port " << ntohs( address.sin_port )
 143                                                 << std::endl;
 144                 }
 145         }
 146
 147         int GdbServer::readByte() {
 148                 static char buf[BUF_SIZ];
 149                 static int bufcnt = 0;
 150                 static char *bufp;
 151
 152                 if (bufcnt-- > 0)
 153                         return *bufp++ & 0x7f;
 154
 155                 bufcnt = read( conn, buf, sizeof(buf) );
 156
 157                 if( bufcnt <= 0 ) {
 158                         if( bufcnt == 0 )
 159                                 throw GdbException( "Read failed: Got EOF" );
 160
 161             if( errno == EAGAIN )
 162                 return -1;
 163
 164             throw GdbException( "Read failed" );
 165                 }
 166
 167                 bufp = buf;
 168                 bufcnt--;
 169                 return *bufp++ & 0x7f;
 170         }
 171
 172         void GdbServer::write( const void *buf, size_t count ) {
 173             int res = ::write( conn, buf, count );
 174
 175             if( res < 0 )
 176                 throw GdbException( util::format("write failed: %s") % strerror(errno) );
 177
 178             if( (size_t)res != count )
 179                 throw GdbException( util::format("wrote only wrote %d of %d bytes") % res % count );
 180         }
 181
 182         /* Acknowledge a packet from GDB */
 183         void GdbServer::sendAck() {
 184             write( "+", 1 );
 185         }
 186
 187         void GdbServer::saveLastReply( const char *reply ) {
 188                 if( lastReply != 0 )
 189                         delete [] lastReply;
 190
 191                 int len = strlen( reply );
 192             lastReply = new char[len];
 193             strcpy( lastReply, reply );
 194         }
 195
 196         static char HEX_DIGIT[] = "0123456789abcdef";
 197
 198         /* Send a reply to GDB. */
 199         void GdbServer::sendReply( const char *reply ) {
 200             int cksum = 0;
 201             int bytes;
 202
 203             /* Save the reply to last reply so we can resend if need be. */
 204             if( reply != lastReply )
 205                 saveLastReply( reply );
 206
 207             if( *reply == '\0' ) {
 208                 write( "$#00", 4 );
 209             } else {
 210                 char buf[MAX_BUF];
 211                 memset( buf, '\0', sizeof(buf) );
 212
 213                 buf[0] = '$';
 214                 bytes = 1;
 215
 216                 while( *reply ) {
 217                     cksum += (unsigned char)*reply;
 218                     buf[bytes] = *reply;
 219                     bytes++;
 220                     reply++;
 221
 222                     /* must account for "#cc" to be added */
 223                     if( bytes == (MAX_BUF-3) ) {
 224                         /* FIXME: TRoth 2002/02/18 - splitting reply would be better */
 225                         throw GdbException( "buffer overflow" );
 226                     }
 227                 }
 228
 229                 buf[bytes++] = '#';
 230                 buf[bytes++] = HEX_DIGIT[(cksum >> 4) & 0xf];
 231                 buf[bytes++] = HEX_DIGIT[cksum & 0xf];
 232
 233 #ifdef DEBUG
 234                 std::cout << util::format("Send: \"%s\"") % buf << std::endl;
 235 #endif
 236                 write( buf, bytes );
 237             }
 238         }
 239
 240         void GdbServer::setBlockingMode( bool blocking ) {
 241                 if( blocking == blockingOn )
 242                         return;
 243
 244                 int flags = fcntl(conn, F_GETFL, 0);
 245             if( blocking ) {
 246                 /* turn non-blocking mode off */
 247                 if( fcntl( conn, F_SETFL, flags & ~O_NONBLOCK) < 0 )
 248                         throw GdbException( "fcntl failed" );
 249             } else {
 250                 /* turn non-blocking mode on */
 251                 if (fcntl( conn, F_SETFL, flags | O_NONBLOCK) < 0)
 252                         throw GdbException( "fcntl failed" );
 253             }
 254
 255             blockingOn = blocking;
 256         }
 257
 258         /* Convert a hexidecimal digit to a 4 bit nibble. */
 259         static int hex2nib( char hex ) {
 260             if( (hex >= 'A') && (hex <= 'F') )
 261                 return (10 + (hex - 'A'));
 262
 263             else if( (hex >= 'a') && (hex <= 'f') )
 264                 return (10 + (hex - 'a'));
 265
 266             else if( (hex >= '0') && (hex <= '9') )
 267                 return (hex - '0');
 268
 269             throw GdbException( "Invalid hexidecimal digit" );
 270         }
 271
 272         static byte hex2byte( char **pkt ) {
 273                 char *p = *pkt;
 274                 byte val = hex2nib(*p++);
 275                 val = (val << 4) | hex2nib(*p++);
 276                 *pkt = p;
 277                 return val;
 278         }
 279
 280         static void putHex( char **pkt, byte val ) {
 281                 char *p = *pkt;
 282         *p++ = HEX_DIGIT[(val >> 4) & 0xf];
 283         *p++ = HEX_DIGIT[val & 0xf];
 284                 *pkt = p;
 285         }
 286
 287         static int extractHexNum( char **pkt, char stop = '\0' ) {
 288             int num = 0;
 289             char *p = *pkt;
 290             int max_shifts = sizeof(int)*2-1; /* max number of nibbles to shift through */
 291
 292             int i = 0;
 293             while ( (*p != stop) && (*p != '\0') ) {
 294                 if( i > max_shifts )
 295                          throw GdbException( "number too large" );
 296
 297                 num = (num << 4) | hex2nib(*p);
 298                 i++;
 299                 p++;
 300             }
 301
 302             *pkt = p;
 303             return num;
 304         }
 305
 306         static void getAddrLen( char **pkt, char a_end, char l_end,
 307                                         unsigned int & addr, int & len ) {
 308                 char *p = *pkt;
 309
 310                 addr = 0;
 311             len  = 0;
 312
 313             /* Get the addr from the packet */
 314             while( *p != a_end )
 315                 addr = (addr << 4) + hex2nib(*p++);
 316             p++;                      /* skip over a_end */
 317
 318             /* Get the length from the packet */
 319             while( *p != l_end )
 320                 len = (len << 4) + hex2nib(*p++);
 321             p++;                      /* skip over l_end */
 322
 323             *pkt = p;
 324         }
 325
 326         void GdbServer::queryPacket( char *pkt ) {
 327             char reply[MAX_BUF];
 328             memset(reply, '\0', sizeof(reply));
 329
 330                 if( strcmp(pkt, "C") == 0 ) {
 331                         /*
 332                          * `qC'
 333                          * Return the current thread id.
 334                          * Reply:
 335                          *      `QC pid'
 336                          */
 337                     snprintf( reply, sizeof(reply)-1, "%02x", current );
 338                 } else if( strcmp(pkt, "qfThreadInfo") == 0 ) {
 339                         /*
 340                          * `qfThreadInfo'
 341                          * `qsThreadInfo'
 342                          *  Obtain a list of all active thread ids from the target (OS).
 343                          * Since there may be too many active threads to fit into one
 344                          * reply packet, this query works iteratively: it may require more
 345                          * than one query/reply sequence to obtain the entire list of threads.
 346                          * The first query of the sequence will be the `qfThreadInfo' query;
 347                          * subsequent queries in the sequence will be the `qsThreadInfo' query.
 348                          */
 349
 350                         int n;
 351                         n = snprintf( reply, sizeof(reply)-1, "m %02x", 0 );
 352                         for(size_t i = 1; i < devices.size(); ++i)
 353                                 snprintf( reply + n, sizeof(reply)-n-1, ",%02x", 0 );
 354                 } else if( strcmp(pkt, "qsThreadInfo") == 0 ) {
 355                         reply[0] = 'l';
 356                 } else if( strcmp(pkt, "qSupported") == 0) {
 357                         sendReply( "PacketSize=800;qXfer:features:read+" );
 358                 } else if( strncmp(pkt, "qXfer:features:read:target.xml:", 31) == 0 ) {
 359                         // GDB XML target descriptions, since GDB 6.7 (2007-10-10)
 360                         // see http://sources.redhat.com/gdb/current/onlinedocs/gdb/Target-Descriptions.html
 361                         sendReply( "l"
 362                                         "<?xml version=\"1.0\"?>\n"
 363                                         "<!DOCTYPE target SYSTEM \"gdb-target.dtd\">\n"
 364                                         "<target version=\"1.0\">\n"
 365                                         "    <architecture>avr</architecture>\n"
 366                                         "</target>\n" );
 367                         return;
 368                 }
 369
 370                 sendReply( reply );
 371         }
 372
 373         void GdbServer::readRegisters() {
 374             /* (32 gpwr, SREG, SP, PC) * 2 hex bytes + terminator */
 375             size_t  buf_sz = (32 + 1 + 2 + 4)*2 + 1;
 376             char buf[ buf_sz ];
 377
 378             char *p = buf;
 379             /* 32 gen purpose working registers */
 380             for( int i = 0; i<32; i++ ) {
 381                 byte val = devices[current]->reg(i);
 382                 putHex( &p, val );
 383             }
 384
 385             {
 386                     /* GDB thinks SREG is register number 32 */
 387                 byte val = devices[current]->status();
 388                 putHex( &p, val );
 389             }
 390
 391             {
 392                 /* GDB thinks SP is register number 33 */
 393                 word sp = devices[current]->stackPointer();
 394                 putHex( &p, sp & 0xff );
 395                 putHex( &p, (sp >> 8) & 0xff );
 396             }
 397
 398             {
 399                 dword pc = devices[current]->programCounter();
 400                 /* GDB thinks PC is register number 34.
 401                  * GDB stores PC in a 32 bit value (only uses 23 bits though).
 402                  */
 403
 404                 putHex( &p, pc & 0xff );
 405                 putHex( &p, (pc >> 8) & 0xff );
 406                 putHex( &p, (pc >> 16) & 0xff );
 407                 putHex( &p, (pc >> 24) & 0xff );
 408             }
 409
 410             *p = '\0';
 411             sendReply(  buf );
 412         }
 413
 414         void GdbServer::writeRegisters( char *pkt ) {
 415             /* 32 gen purpose working registers */
 416             for(int i = 0; i < 32; i++) {
 417                 byte val = hex2byte( &pkt );
 418                 devices[current]->setReg( i, val );
 419             }
 420
 421             {
 422                 /* GDB thinks SREG is register number 32 */
 423                 byte val = hex2byte( &pkt );
 424                     devices[current]->setStatus( val );
 425             }
 426
 427             {
 428                 /* GDB thinks SP is register number 33 */
 429                 word sp = hex2byte( &pkt ) | (hex2byte( &pkt ) << 8);
 430                 devices[current]->setStackPointer( sp );
 431             }
 432
 433             {
 434                 /*
 435                  * GDB thinks PC is register number 34.
 436                  * GDB stores PC in a 32 bit value (only uses 23 bits though).
 437                  */
 438                 dword pc = hex2byte( &pkt ) |
 439                                         hex2byte( &pkt ) << 8 |
 440                                         hex2byte( &pkt ) << 16 |
 441                                         hex2byte( &pkt ) << 24;
 442                 devices[current]->setProgramCounter( pc );
 443             }
 444
 445             sendReply( "OK" );
 446         }
 447
 448         void GdbServer::readRegister( char *pkt ) {
 449             char reply[MAX_BUF];
 450             memset(reply, '\0', sizeof(reply));
 451
 452             int reg = extractHexNum(&pkt, '\0');
 453             if( reg < 32 ) {
 454                 byte val = devices[current]->reg(reg);
 455                  snprintf( reply, sizeof(reply)-1, "%02x", val );
 456             } else if( reg == 32 ) {
 457                 byte val = devices[current]->status();
 458                  snprintf( reply, sizeof(reply)-1, "%02x", val );
 459             } else if( reg == 33 ) {
 460                    /* SP */
 461                 word sp = devices[current]->stackPointer();
 462                 snprintf( reply, sizeof(reply)-1, "%02x%02x",
 463                                 sp & 0xff, (sp >> 8) & 0xff );
 464             } else if (reg == 34) {
 465                 /* PC */
 466                 dword val = devices[current]->programCounter();
 467                 snprintf( reply, sizeof(reply)-1,
 468                         "%02x%02x" "%02x%02x",
 469                         val & 0xff, (val >> 8) & 0xff,
 470                         (val >> 16) & 0xff, (val >> 24) & 0xff );
 471             } else {
 472                 sendReply( "E00" );
 473                 return;
 474             }
 475
 476             sendReply( reply );
 477         }
 478
 479         void GdbServer::writeRegister( char *pkt ) {
 480             int reg = extractHexNum(&pkt, '=');
 481             pkt++;                      /* skip over '=' character */
 482
 483             /* extract the low byte of value from pkt */
 484             byte val = hex2byte( &pkt );
 485             if( (reg >= 0) && (reg < 33) ) {
 486                 /* r0 to r31  */
 487                 devices[current]->setReg( reg, val );
 488             } else if( reg == 32 ) {
 489                 /* SREG is register 32 */
 490                 devices[current]->setStatus( val );
 491             } else if( reg == 33 ) {
 492                 /* SP is 2 bytes long so extract upper byte */
 493                 byte hval  = hex2byte( &pkt );
 494                 word sp = (hval << 8) | val;
 495                 devices[current]->setStackPointer( sp );
 496             } else if( reg == 34 ) {
 497                 /* GDB thinks PC is register number 34.
 498                 GDB stores PC in a 32 bit value (only uses 23 bits though).
 499                 GDB thinks PC is bytes into flash, not words like in simulavr.
 500
 501                 Must cast to dword so as not to get mysterious truncation. */
 502
 503                 dword pc = val |
 504                                         hex2byte( &pkt ) << 8 |
 505                                         hex2byte( &pkt ) << 16 |
 506                                         hex2byte( &pkt ) << 24;
 507                 devices[current]->setProgramCounter( pc );
 508             } else {
 509                 sendReply(  "E00" );
 510                 return;
 511             }
 512
 513             sendReply( "OK" );
 514         }
 515
 516         void GdbServer::readMemory( char *pkt ) {
 517                 unsigned int addr;
 518                 int len;
 519             getAddrLen( &pkt, ',', '\0', addr, len );
 520
 521             char *buf = 0;
 522             try {
 523                 /* Treat registers differently from continuous memory */
 524                 if( ((addr & MEM_SPACE_MASK) == SRAM_OFFSET) &&
 525                                 devices[current]->isRegister(addr & ~MEM_SPACE_MASK) ) {
 526                         unsigned int raddr = addr & ~MEM_SPACE_MASK; /* remove the offset bits */
 527                                 buf = new char[len*2+1];
 528                                 int i;
 529                                 for(i = 0; i < len; i++) {
 530                                         byte bval = devices[current]->readRegister(raddr + i);
 531                                         buf[i*2]   = HEX_DIGIT[bval >> 4];
 532                                         buf[i*2+1] = HEX_DIGIT[bval & 0xf];
 533                                 }
 534                                 buf[i*2] = '\0';
 535                 } else {
 536                                 const unsigned char *data;
 537                                 if( (addr & MEM_SPACE_MASK) == EEPROM_OFFSET ) {
 538                                         /* addressing eeprom */
 539                                         unsigned int eaddr = addr & ~MEM_SPACE_MASK; /* remove the offset bits */
 540                                         data = devices[current]->readEeprom( eaddr, len );
 541                                 } else if ( (addr & MEM_SPACE_MASK) == SRAM_OFFSET ) {
 542                                         /* addressing sram */
 543                                         unsigned int saddr = addr & ~MEM_SPACE_MASK; /* remove the offset bits */
 544                                         data = devices[current]->readRam( saddr, len );
 545                                 } else if ( (addr & MEM_SPACE_MASK) == FLASH_OFFSET ) {
 546                                         /* addressing flash */
 547                                         unsigned int faddr = addr & ~MEM_SPACE_MASK; /* remove the offset bits */
 548                                         data = devices[current]->readFlash( faddr, len );
 549                                 } else {
 550                                         throw AccessViolation( "memory space does not exist " );
 551                                 }
 552
 553                                 buf = new char[len*2+1];
 554                                 int i;
 555                                 for(i = 0; i < len; i++) {
 556                                         byte bval = data[i];
 557                                         buf[i*2]   = HEX_DIGIT[bval >> 4];
 558                                         buf[i*2+1] = HEX_DIGIT[bval & 0xf];
 559                                 }
 560                                 buf[i*2] = '\0';
 561                 }
 562             } catch( util::Exception & ex ) {
 563                 std::ios_base::fmtflags f = std::cerr.flags();
 564                 std::cerr << ex.message() << " reading address "
 565                                 << std::hex << addr << " length " << std::dec << len << std::endl;
 566                 std::cerr.flags(f);
 567
 568                 /*
 569                  * If gdb requested a read from invalid memory,
 570                  * always reply with 0's
 571                  * TODO find out why gdb requests these addresses
 572                  */
 573                 if ( (addr & MEM_SPACE_MASK) == SRAM_OFFSET ) {
 574                         buf = new char[len*2+1];
 575                         int i;
 576                                 for(i = 0; i < 2*len; i++)
 577                                         buf[i] = '0';
 578                                 buf[i] = '\0';
 579                 } else {
 580                                 char reply[10];
 581                                 snprintf( reply, sizeof(reply), "E%02x", EIO );
 582                                 sendReply( reply );
 583                                 return;
 584                 }
 585             } catch( std::exception & ex ) {
 586                 std::cerr << ex.what() << std::endl;
 587
 588                 char reply[10];
 589                 snprintf( reply, sizeof(reply), "E%02x", EIO );
 590                 sendReply( reply );
 591                 return;
 592             }
 593
 594             sendReply( buf );
 595
 596         if( buf != 0 )
 597                 delete [] buf;
 598         }
 599
 600         void GdbServer::writeMemory( char *pkt ) {
 601             unsigned int addr;
 602                 int len;
 603             getAddrLen( &pkt, ',', ':', addr, len );
 604
 605             try {
 606                 unsigned char *data = new unsigned char[len];
 607                 for(int i = 0; i < len; ++i) {
 608                         byte val = hex2byte( &pkt );
 609                     data[i] = val;
 610                 }
 611
 612                     if( (addr & MEM_SPACE_MASK) == EEPROM_OFFSET ) {
 613                         /* addressing eeprom */
 614                         addr = addr & ~MEM_SPACE_MASK; /* remove the offset bits */
 615                         devices[current]->writeEeprom(data, addr, len);
 616                     } else if ( (addr & MEM_SPACE_MASK) == SRAM_OFFSET ) {
 617                         /* addressing sram */
 618                         addr = addr & ~MEM_SPACE_MASK; /* remove the offset bits */
 619                         devices[current]->writeRam(data, addr, len);
 620                     } else if ( (addr & MEM_SPACE_MASK) == FLASH_OFFSET ) {
 621                         /* addressing flash */
 622                         addr = addr & ~MEM_SPACE_MASK; /* remove the offset bits */
 623                         devices[current]->writeFlash(data, addr, len);
 624                     } else {
 625                         throw AccessViolation( "memory space does not exist " );
 626                     }
 627             } catch( util::Exception & ex ) {
 628                 std::cerr << ex.message() << std::endl;
 629
 630                 char reply[10];
 631                 snprintf( reply, sizeof(reply), "E%02x", EIO );
 632                 sendReply( reply );
 633                 return;
 634             } catch( std::exception & ex ) {
 635                 std::cerr << ex.what() << std::endl;
 636
 637                 char reply[10];
 638                 snprintf( reply, sizeof(reply), "E%02x", EIO );
 639                 sendReply( reply );
 640                 return;
 641             }
 642
 643             sendReply( "OK" );
 644         }
 645
 646         int GdbServer::getSignal( char *pkt ) {
 647             int signo = hex2byte( &pkt );
 648
 649             /*
 650              * Process signals send via remote protocol from gdb.
 651              * Signals really don't make sense to the program running
 652              * in the simulator, so we are using them sort of as
 653              * an 'out of band' data.
 654              */
 655
 656             switch (signo) {
 657                 case SIGHUP:
 658                     /* Gdb user issuing the 'signal SIGHUP' command tells sim to reset
 659                     itself. We reply with a SIGTRAP the same as we do when gdb
 660                     makes first connection with simulator. */
 661                     devices[current]->reset();
 662                     sendReply( "S05" );
 663             }
 664
 665             return signo;
 666
 667         }
 668
 669         /* Format of breakpoint commands (both insert and remove):
 670
 671         "z<t>,<addr>,<length>"  -  remove break/watch point
 672         "Z<t>,<add>r,<length>"  -  insert break/watch point
 673
 674         In both cases t can be the following:
 675         t = '0'  -  software breakpoint
 676         t = '1'  -  hardware breakpoint
 677         t = '2'  -  write watch point
 678         t = '3'  -  read watch point
 679         t = '4'  -  access watch point
 680
 681         addr is address.
 682         length is in bytes
 683
 684         For a software breakpoint, length specifies the size of the instruction to
 685         be patched. For hardware breakpoints and watchpoints, length specifies the
 686         memory region to be monitored. To avoid potential problems, the operations
 687         should be implemented in an idempotent way. -- GDB 5.0 manual.
 688         */
 689         void GdbServer::breakPoint( char *pkt ) {
 690             char z = *(pkt-1);          /* get char parser already looked at */
 691             char t = *pkt++;
 692             pkt++;                      /* skip over first ',' */
 693
 694             unsigned int addr;
 695                 int len;
 696             getAddrLen( &pkt, ',', '\0', addr, len );
 697
 698             try {
 699                     switch( t ) {
 700                         case '0': /* software breakpoint */
 701                         case '1': /* hardware breakpoint */
 702                             if( z == 'z' )
 703                                 devices[current]->removeBreak( addr );
 704                             else
 705                                 devices[current]->insertBreak( addr );
 706                             break;
 707
 708                         case '2':               /* write watchpoint */
 709                                 sendReply( "" );
 710                             return;
 711
 712                         case '3':               /* read watchpoint */
 713                                 sendReply( "" );
 714                             return;
 715
 716                         case '4':               /* access watchpoint */
 717                                 sendReply( "" );
 718                             return;             /* unsupported yet */
 719                     }
 720
 721                     sendReply( "OK" );
 722             } catch( util::Exception & ex ) {
 723                 std::cerr << ex.message() << std::endl;
 724                 sendReply( "E01" );
 725             }
 726         }
 727
 728         int GdbServer::parsePacket( char *pkt ) {
 729             switch( *pkt++ ) {
 730                 case '?':               /* last signal */
 731                     sendReply( "S05" ); /* signal # 5 is SIGTRAP */
 732                     break;
 733
 734                 case 'g':               /* read registers */
 735                     readRegisters(  );
 736                     break;
 737
 738                 case 'G':               /* write registers */
 739                     writeRegisters(  pkt );
 740                     break;
 741
 742                 case 'p':               /* read a single register */
 743                     readRegister(  pkt );
 744                     break;
 745
 746                 case 'P':               /* write single register */
 747                     writeRegister(  pkt );
 748                     break;
 749
 750                 case 'm':               /* read memory */
 751                     readMemory(  pkt );
 752                     break;
 753
 754                 case 'M':               /* write memory */
 755                     writeMemory(  pkt );
 756                     break;
 757
 758                 case 'D':               /* detach the debugger */
 759                 case 'k':               /* kill request */
 760                     sendReply( "OK" );
 761                     return GDB_RET_KILL_REQUEST;
 762
 763                 case 'c':               /* continue */
 764                     return GDB_RET_CONTINUE;
 765
 766                 case 'C':               /* continue with signal */
 767                     return GDB_RET_CONTINUE;
 768
 769                 case 's':               /* step */
 770                     return GDB_RET_SINGLE_STEP;
 771
 772                 case 'S':               /* step with signal */
 773                     getSignal(pkt);
 774                     return GDB_RET_SINGLE_STEP;
 775
 776                 case 'z':               /* remove break/watch point */
 777                 case 'Z':               /* insert break/watch point */
 778                     breakPoint( pkt );
 779                     break;
 780
 781                 case 'H':                               /* Set thread for subsequent operations */
 782                         /*
 783                          * `H c t'
 784                          *      Set thread for subsequent operations (`m', `M', `g', `G', et.al.).
 785                          * c depends on the operation to be performed: it should be `c'
 786                          * for step and continue operations, `g' for other operations.
 787                          * The thread designator t may be `-1', meaning all the threads,
 788                          * a thread number, or `0' which means pick any thread.
 789                          */
 790                         pkt++;
 791                         current = strtoul(pkt, 0, 16);
 792                         sendReply("OK");
 793                         break;
 794
 795                 case 'q':               /* query requests */
 796                         queryPacket(pkt);
 797                     break;
 798
 799                 default:
 800                         sendReply( "" );
 801             }
 802
 803             return GDB_RET_OK;
 804         }
 805
 806         int GdbServer::readPacket() {
 807             char pkt_buf[MAX_BUF];
 808
 809         memset( pkt_buf, 0, sizeof(pkt_buf) );
 810         setBlockingMode( true );
 811
 812         int pkt_cksum = 0, i = 0;
 813         int c = readByte();
 814         while ( (c != '#') && (i < MAX_BUF) ) {
 815             pkt_buf[i++] = c;
 816             pkt_cksum += (unsigned char)c;
 817             c = readByte();
 818         }
 819
 820         int cksum;
 821         cksum  = hex2nib( readByte() ) << 4;
 822         cksum |= hex2nib( readByte() );
 823
 824         /* FIXME: Should send "-" (Nak) instead of aborting when we get
 825         checksum errors. Leave this as an error until it is actually
 826         seen (I've yet to see a bad checksum - TRoth). It's not a simple
 827         matter of sending (Nak) since you don't want to get into an
 828         infinite loop of (bad cksum, nak, resend, repeat).*/
 829
 830         if( (pkt_cksum & 0xff) != cksum )
 831                 throw GdbException(
 832                                 util::format("Bad checksum: sent 0x%x <--> computed 0x%x") % cksum % pkt_cksum );
 833
 834 #ifdef DEBUG
 835         std::cout << util::format("Recv: \"$%s#%02x\"") % pkt_buf % cksum << std::endl;
 836 #endif
 837
 838         /* always acknowledge a well formed packet immediately */
 839         sendAck();
 840
 841         return parsePacket( pkt_buf );
 842         }
 843
 844         int GdbServer::preParsePacket(bool blocking) {
 845                 setBlockingMode(blocking);
 846
 847             int c = readByte( );
 848             switch (c) {
 849                 case '$':           /* read a packet */
 850                         return readPacket();
 851
 852                 case '-': // Nack
 853                     sendReply( lastReply );
 854                     break;
 855
 856                 case '+': // Ack
 857                     break;
 858
 859                 case 0x03:
 860                     /* Gdb sends this when the user hits C-c. This is gdb's way of
 861                     telling the simulator to interrupt what it is doing and return
 862                     control back to gdb. */
 863                     return GDB_RET_CTRL_C;
 864
 865                 case -1:
 866                     /* fd is non-blocking and no data to read */
 867                     return GDB_RET_NOTHING_RECEIVED;
 868
 869                 default:
 870                         std::cout << "Unknown request from gdb: "
 871                                                 << std::hex << c << std::dec << std::endl;
 872             }
 873
 874             return GDB_RET_OK;
 875         }
 876
 877         void GdbServer::sendPosition(int signo) {
 878                 /* status */
 879                 byte status = devices[current]->status();
 880             /* SP */
 881         word sp = devices[current]->stackPointer();
 882         /* PC */
 883         dword val = devices[current]->programCounter();
 884
 885         char reply[MAX_BUF];
 886             snprintf( reply, sizeof(reply),
 887                                 "T%02x20:%02x;21:%02x%02x;22:%02x%02x%02x%02x;",
 888                                 signo,
 889                                 status,
 890                                 sp & 0xff, (sp >> 8) & 0xff,
 891                                 val & 0xff, (val >> 8) & 0xff,
 892                         (val >> 16) & 0xff, (val >> 24) & 0xff);
 893
 894             sendReply(reply);
 895         }
 896
 897         void GdbServer::exec() {
 898                 while( true ) {
 899                         if( conn < 0 )
 900                                 acceptClient();
 901
 902                         try {
 903                                 if( lastReply != 0 ) {
 904                                         delete [] lastReply;
 905                                         lastReply = 0;
 906                                 }
 907
 908                                 handleClient();
 909                         } catch( GdbException & ex ) {
 910                                 std::cerr << "GdbServer " << ex.message() << std::endl;
 911
 912                                 devices[current]->deleteAllBreakpoints();
 913                 devices[current]->reset();
 914
 915                 if( conn != -1 ) {
 916                         closesocket(conn);
 917                         conn = -1;
 918                 }
 919                         }
 920                 }
 921         }
 922
 923         void GdbServer::handleClient() {
 924                 do {
 925                 int gdbRet = preParsePacket( runMode != GDB_RET_CONTINUE );
 926                 switch( gdbRet ) {
 927                     case GDB_RET_NOTHING_RECEIVED:
 928                         break;
 929
 930                     case GDB_RET_OK:
 931                         runMode = GDB_RET_OK;
 932                         break;
 933
 934                     case GDB_RET_CONTINUE:
 935                         runMode = GDB_RET_CONTINUE;
 936                         break;
 937
 938                     case GDB_RET_SINGLE_STEP:
 939                         runMode = GDB_RET_SINGLE_STEP;
 940                         break;
 941
 942                     case GDB_RET_CTRL_C:
 943                         runMode = GDB_RET_CTRL_C;
 944                         sendPosition(SIGINT);
 945                         break;
 946
 947                     case GDB_RET_KILL_REQUEST:
 948                         devices[current]->deleteAllBreakpoints();
 949                         devices[current]->reset();
 950
 951                         if( conn != -1 ) {
 952                                 closesocket(conn);
 953                                 conn = -1;
 954                         }
 955                         return;
 956                 }
 957
 958                 } while( (runMode != GDB_RET_SINGLE_STEP ) &&
 959                                 ( runMode != GDB_RET_CONTINUE) );
 960
 961                 try {
 962                         do {
 963                                 devices[current]->step();
 964                         } while( (runMode == GDB_RET_SINGLE_STEP )
 965                                         && ! devices[current]->stepDone() );
 966
 967                     if( devices[current]->hasBreaked() ) {
 968                         runMode = GDB_RET_OK;
 969                         sendPosition(SIGTRAP);
 970                     }
 971             } catch( IllegalInstruction & ex ) {
 972                 char reply[10];
 973                 snprintf( reply, sizeof(reply), "S%02x", SIGILL );
 974                         sendReply( reply );
 975                         runMode = GDB_RET_OK;
 976                         sendPosition(SIGILL);
 977             }
 978
 979             if( runMode == GDB_RET_SINGLE_STEP ) {
 980                 runMode = GDB_RET_OK;
 981                 sendPosition(SIGTRAP);
 982             }
 983         }
 984
 985 }