GdbServer.cpp

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