src/server/gdb_server.c

   1 /***************************************************************************
   2  *   Copyright (C) 2005 by Dominic Rath                                    *
   3  *   Dominic.Rath@gmx.de                                                   *
   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 2 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, write to the                         *
  17  *   Free Software Foundation, Inc.,                                       *
  18  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  19  ***************************************************************************/
  20 #include "config.h"
  21
  22 #include "gdb_server.h"
  23
  24 #include "server.h"
  25 #include "log.h"
  26 #include "binarybuffer.h"
  27 #include "breakpoints.h"
  28
  29 #define __USE_GNU
  30 #include <string.h>
  31 #include <errno.h>
  32 #include <unistd.h>
  33 #include <stdlib.h>
  34
  35 #ifndef HAVE_STRNDUP
  36 #include <stdio.h>
  37 char* strndup(const char *s, size_t n)
  38 {
  39         size_t len = strnlen (s, n);
  40         char *new = (char *) malloc (len + 1);
  41
  42         if (new == NULL)
  43                 return NULL;
  44
  45         new[len] = '\0';
  46         return (char *) memcpy (new, s, len);
  47 }
  48 #endif
  49
  50 #if 0
  51 #define _DEBUG_GDB_IO_
  52 #endif
  53
  54 static unsigned short gdb_port;
  55
  56 int gdb_last_signal(target_t *target)
  57 {
  58         switch (target->debug_reason)
  59         {
  60                 case DBG_REASON_DBGRQ:
  61                         return 0x2; /* SIGINT */
  62                 case DBG_REASON_BREAKPOINT:
  63                 case DBG_REASON_WATCHPOINT:
  64                 case DBG_REASON_WPTANDBKPT:
  65                         return 0x05; /* SIGTRAP */
  66                 case DBG_REASON_SINGLESTEP:
  67                         return 0x05; /* SIGTRAP */
  68                 case DBG_REASON_NOTHALTED:
  69                         return 0x0; /* no signal... shouldn't happen */
  70                 default:
  71                         ERROR("BUG: undefined debug reason");
  72                         exit(-1);
  73         }
  74 }
  75
  76 int gdb_get_char(connection_t *connection, int* next_char)
  77 {
  78         gdb_connection_t *gdb_con = connection->priv;
  79         char *debug_buffer;
  80
  81         if (gdb_con->buf_cnt-- > 0)
  82         {
  83                 *next_char = *(gdb_con->buf_p++);
  84                 if (gdb_con->buf_cnt > 0)
  85                         connection->input_pending = 1;
  86                 else
  87                         connection->input_pending = 0;
  88
  89 #ifdef _DEBUG_GDB_IO_
  90                 DEBUG("returned char '%c' (0x%2.2x)", *next_char, *next_char);
  91 #endif
  92
  93                 return ERROR_OK;
  94         }
  95
  96         while ((gdb_con->buf_cnt = read(connection->fd, gdb_con->buffer, GDB_BUFFER_SIZE)) <= 0)
  97         {
  98                 if (gdb_con->buf_cnt == 0)
  99                         return ERROR_SERVER_REMOTE_CLOSED;
 100
 101                 switch(errno)
 102                 {
 103                         case EAGAIN:
 104                                 usleep(1000);
 105                                 break;
 106                         case ECONNABORTED:
 107                                 return ERROR_SERVER_REMOTE_CLOSED;
 108                         case ECONNRESET:
 109                                 return ERROR_SERVER_REMOTE_CLOSED;
 110                         default:
 111                                 ERROR("read: %s", strerror(errno));
 112                                 exit(-1);
 113                 }
 114         }
 115
 116         debug_buffer = malloc(gdb_con->buf_cnt + 1);
 117         memcpy(debug_buffer, gdb_con->buffer, gdb_con->buf_cnt);
 118         debug_buffer[gdb_con->buf_cnt] = 0;
 119         DEBUG("received '%s'", debug_buffer);
 120         free(debug_buffer);
 121
 122         gdb_con->buf_p = gdb_con->buffer;
 123         gdb_con->buf_cnt--;
 124         *next_char = *(gdb_con->buf_p++);
 125         if (gdb_con->buf_cnt > 0)
 126                 connection->input_pending = 1;
 127         else
 128                 connection->input_pending = 0;
 129 #ifdef _DEBUG_GDB_IO_
 130                 DEBUG("returned char '%c' (0x%2.2x)", *next_char, *next_char);
 131 #endif
 132
 133         return ERROR_OK;
 134 }
 135
 136 int gdb_put_packet(connection_t *connection, char *buffer, int len)
 137 {
 138         int i;
 139         unsigned char my_checksum = 0;
 140         char checksum[3];
 141         char *debug_buffer;
 142         int reply;
 143         int retval;
 144         gdb_connection_t *gdb_con = connection->priv;
 145
 146         for (i = 0; i < len; i++)
 147                 my_checksum += buffer[i];
 148
 149         while (1)
 150         {
 151
 152                 debug_buffer = malloc(len + 1);
 153                 memcpy(debug_buffer, buffer, len);
 154                 debug_buffer[len] = 0;
 155                 DEBUG("sending packet '$%s#%2.2x'", debug_buffer, my_checksum);
 156                 free(debug_buffer);
 157
 158                 write(connection->fd, "$", 1);
 159                 if (len > 0)
 160                         write(connection->fd, buffer, len);
 161                 write(connection->fd, "#", 1);
 162
 163                 snprintf(checksum, 3, "%2.2x", my_checksum);
 164
 165                 write(connection->fd, checksum, 2);
 166
 167                 if ((retval = gdb_get_char(connection, &reply)) != ERROR_OK)
 168                         return retval;
 169
 170                 if (reply == '+')
 171                         break;
 172                 else if (reply == '-')
 173                         WARNING("negative reply, retrying");
 174                 else if (reply == 0x3)
 175                 {
 176                         gdb_con->ctrl_c = 1;
 177                         if ((retval = gdb_get_char(connection, &reply)) != ERROR_OK)
 178                                 return retval;
 179                         if (reply == '+')
 180                                 break;
 181                         else if (reply == '-')
 182                                 WARNING("negative reply, retrying");
 183                         else
 184                         {
 185                                 ERROR("unknown character 0x%2.2x in reply, dropping connection", reply);
 186                                 return ERROR_SERVER_REMOTE_CLOSED;
 187                         }
 188                 }
 189                 else
 190                 {
 191                         ERROR("unknown character 0x%2.2x in reply, dropping connection", reply);
 192                         return ERROR_SERVER_REMOTE_CLOSED;
 193                 }
 194         }
 195
 196         return ERROR_OK;
 197 }
 198
 199 int gdb_get_packet(connection_t *connection, char *buffer, int *len)
 200 {
 201         int character;
 202         int count = 0;
 203         int retval;
 204         int first_char = 0;
 205         int packet_type;
 206         char checksum[3];
 207         unsigned char my_checksum = 0;
 208         gdb_connection_t *gdb_con = connection->priv;
 209
 210         while (1)
 211         {
 212                 do
 213                 {
 214                         if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
 215                                 return retval;
 216
 217                         switch (character)
 218                         {
 219                                 case '$':
 220                                         break;
 221                                 case '+':
 222                                         WARNING("acknowledgment received, but no packet pending");
 223                                         break;
 224                                 case '-':
 225                                         WARNING("negative acknowledgment, but no packet pending");
 226                                         break;
 227                                 case 0x3:
 228                                         gdb_con->ctrl_c = 1;
 229                                         *len = 0;
 230                                         return ERROR_OK;
 231                                 default:
 232                                         WARNING("ignoring character 0x%x", character);
 233                                         break;
 234                         }
 235                 } while (character != '$');
 236
 237                 my_checksum = 0;
 238
 239                 do
 240                 {
 241                         if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
 242                                 return retval;
 243
 244                         if( !first_char ) {
 245                                 packet_type = character;
 246                                 first_char = 1;
 247                         }
 248
 249                         if( packet_type == 'X' )
 250                         {
 251                                 switch (character)
 252                                 {
 253                                         case '#':
 254                                                 break;
 255                                         case 0x7d:
 256                                                 /* data transmitted in binary mode (X packet)
 257                                                 * uses 0x7d as escape character */
 258                                                 my_checksum += character & 0xff;
 259                                                 gdb_get_char(connection, &character);
 260                                                 my_checksum += character & 0xff;
 261                                                 buffer[count++] = (character ^ 0x20) & 0xff;
 262                                                 if (count > *len)
 263                                                 {
 264                                                         ERROR("packet buffer too small");
 265                                                         return ERROR_GDB_BUFFER_TOO_SMALL;
 266                                                 }
 267                                                 break;
 268                                         default:
 269                                                 buffer[count++] = character & 0xff;
 270                                                 my_checksum += character & 0xff;
 271                                                 if (count > *len)
 272                                                 {
 273                                                         ERROR("packet buffer too small");
 274                                                         return ERROR_GDB_BUFFER_TOO_SMALL;
 275                                                 }
 276                                                 break;
 277                                 }
 278                         }
 279                         else
 280                         {
 281                                 switch (character)
 282                                 {
 283                                         case '#':
 284                                                 break;
 285                                         case 0x3:
 286                                                 gdb_con->ctrl_c = 1;
 287                                                 break;
 288                                         default:
 289                                                 buffer[count++] = character & 0xff;
 290                                                 my_checksum += character & 0xff;
 291                                                 if (count > *len)
 292                                                 {
 293                                                         ERROR("packet buffer too small");
 294                                                         return ERROR_GDB_BUFFER_TOO_SMALL;
 295                                                 }
 296                                                 break;
 297                                 }
 298                         }
 299                 } while (character != '#');
 300
 301                 *len = count;
 302
 303                 if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
 304                         return retval;
 305                 checksum[0] = character;
 306                 if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
 307                         return retval;
 308                 checksum[1] = character;
 309                 checksum[2] = 0;
 310
 311                 if (my_checksum == strtoul(checksum, NULL, 16))
 312                 {
 313                         write (connection->fd, "+", 1);
 314                         break;
 315                 }
 316
 317                 WARNING("checksum error, requesting retransmission");
 318                 write(connection->fd, "-", 1);
 319         }
 320
 321         return ERROR_OK;
 322 }
 323
 324 int gdb_output(struct command_context_s *context, char* line)
 325 {
 326         connection_t *connection = context->output_handler_priv;
 327         char *hex_buffer;
 328         int i, bin_size;
 329
 330         bin_size = strlen(line);
 331
 332         hex_buffer = malloc(bin_size*2 + 4);
 333
 334         hex_buffer[0] = 'O';
 335         for (i=0; i<bin_size; i++)
 336                 snprintf(hex_buffer + 1 + i*2, 3, "%2.2x", line[i]);
 337         hex_buffer[bin_size*2+1] = '0';
 338         hex_buffer[bin_size*2+2] = 'a';
 339         hex_buffer[bin_size*2+3] = 0x0;
 340
 341         gdb_put_packet(connection, hex_buffer, bin_size*2 + 3);
 342
 343         free(hex_buffer);
 344         return ERROR_OK;
 345 }
 346
 347 int gdb_target_callback_event_handler(struct target_s *target, enum target_event event, void *priv)
 348 {
 349         connection_t *connection = priv;
 350         gdb_connection_t *gdb_connection = connection->priv;
 351         char sig_reply[4];
 352         int signal;
 353
 354         switch (event)
 355         {
 356                 case TARGET_EVENT_HALTED:
 357                         if (gdb_connection->frontend_state == TARGET_RUNNING)
 358                         {
 359                                 if (gdb_connection->ctrl_c)
 360                                 {
 361                                         signal = 0x2;
 362                                         gdb_connection->ctrl_c = 0;
 363                                 }
 364                                 else
 365                                 {
 366                                         signal = gdb_last_signal(target);
 367                                 }
 368
 369                                 snprintf(sig_reply, 4, "T%2.2x", signal);
 370                                 gdb_put_packet(connection, sig_reply, 3);
 371                                 gdb_connection->frontend_state = TARGET_HALTED;
 372                         }
 373                         break;
 374                 case TARGET_EVENT_RESUMED:
 375                         if (gdb_connection->frontend_state == TARGET_HALTED)
 376                         {
 377                                 gdb_connection->frontend_state = TARGET_RUNNING;
 378                         }
 379                         break;
 380                 default:
 381                         break;
 382         }
 383
 384         return ERROR_OK;
 385 }
 386
 387 int gdb_new_connection(connection_t *connection)
 388 {
 389         gdb_connection_t *gdb_connection = malloc(sizeof(gdb_connection_t));
 390         gdb_service_t *gdb_service = connection->service->priv;
 391         int retval;
 392         int initial_ack;
 393
 394         connection->priv = gdb_connection;
 395
 396         /* initialize gdb connection information */
 397         gdb_connection->buf_p = gdb_connection->buffer;
 398         gdb_connection->buf_cnt = 0;
 399         gdb_connection->ctrl_c = 0;
 400         gdb_connection->frontend_state = TARGET_HALTED;
 401
 402         /* output goes through gdb connection */
 403         command_set_output_handler(connection->cmd_ctx, gdb_output, connection);
 404
 405         /* register callback to be informed about target events */
 406         target_register_event_callback(gdb_target_callback_event_handler, connection);
 407
 408         /* a gdb session just attached, put the target in halt mode */
 409         if (((retval = gdb_service->target->type->halt(gdb_service->target)) != ERROR_OK) &&
 410                          (retval != ERROR_TARGET_ALREADY_HALTED))
 411         {
 412                 ERROR("error when trying to halt target");
 413                 exit(-1);
 414         }
 415
 416         while (gdb_service->target->state != TARGET_HALTED)
 417         {
 418                 gdb_service->target->type->poll(gdb_service->target);
 419         }
 420
 421         /* remove the initial ACK from the incoming buffer */
 422         if ((retval = gdb_get_char(connection, &initial_ack)) != ERROR_OK)
 423                 return retval;
 424
 425         return ERROR_OK;
 426 }
 427
 428 int gdb_connection_closed(connection_t *connection)
 429 {
 430         if (connection->priv)
 431                 free(connection->priv);
 432         else
 433                 ERROR("BUG: connection->priv == NULL");
 434
 435         target_unregister_event_callback(gdb_target_callback_event_handler, connection);
 436
 437         return ERROR_OK;
 438 }
 439
 440 int gdb_last_signal_packet(connection_t *connection, target_t *target, char* packet, int packet_size)
 441 {
 442         char sig_reply[4];
 443         int signal;
 444
 445         signal = gdb_last_signal(target);
 446
 447         snprintf(sig_reply, 4, "S%2.2x", signal);
 448         gdb_put_packet(connection, sig_reply, 3);
 449
 450         return ERROR_OK;
 451 }
 452
 453 void gdb_get_registers_packet(connection_t *connection, target_t *target, char* packet, int packet_size)
 454 {
 455         reg_t **reg_list;
 456         int reg_list_size;
 457         int retval;
 458         int reg_packet_size = 0;
 459         char *reg_packet;
 460         char *reg_packet_p;
 461         int i;
 462
 463         DEBUG("");
 464
 465         if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
 466         {
 467                 switch (retval)
 468                 {
 469                         case ERROR_TARGET_NOT_HALTED:
 470                                 ERROR("gdb requested registers, but we're not halted");
 471                                 exit(-1);
 472                         default:
 473                                 ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
 474                                 exit(-1);
 475                 }
 476         }
 477
 478         for (i = 0; i < reg_list_size; i++)
 479         {
 480                 reg_packet_size += reg_list[i]->size;
 481         }
 482
 483         reg_packet = malloc(CEIL(reg_packet_size, 8) * 2);
 484         reg_packet_p = reg_packet;
 485
 486         for (i = 0; i < reg_list_size; i++)
 487         {
 488                 int j;
 489                 char *hex_buf = buf_to_char(reg_list[i]->value, reg_list[i]->size);
 490                 DEBUG("hex_buf: %s", hex_buf);
 491                 for (j = CEIL(reg_list[i]->size, 8) * 2; j > 0; j -= 2)
 492                 {
 493                         *reg_packet_p++ = hex_buf[j - 2];
 494                         *reg_packet_p++ = hex_buf[j - 1];
 495                 }
 496                 free(hex_buf);
 497         }
 498
 499         reg_packet_p = strndup(reg_packet, CEIL(reg_packet_size, 8) * 2);
 500         DEBUG("reg_packet: %s", reg_packet_p);
 501         free(reg_packet_p);
 502
 503         gdb_put_packet(connection, reg_packet, CEIL(reg_packet_size, 8) * 2);
 504         free(reg_packet);
 505
 506 }
 507
 508 void gdb_set_registers_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 509 {
 510         int i;
 511         reg_t **reg_list;
 512         int reg_list_size;
 513         int retval;
 514         char *packet_p;
 515
 516         DEBUG("");
 517
 518         /* skip command character */
 519         packet++;
 520         packet_size--;
 521
 522         if (packet_size % 2)
 523         {
 524                 WARNING("GDB set_registers packet with uneven characters received");
 525                 return;
 526         }
 527
 528         if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
 529         {
 530                 switch (retval)
 531                 {
 532                         case ERROR_TARGET_NOT_HALTED:
 533                                 ERROR("gdb requested registers, but we're not halted");
 534                                 exit(-1);
 535                         default:
 536                                 ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
 537                                 exit(-1);
 538                 }
 539         }
 540
 541         packet_p = packet;
 542         for (i = 0; i < reg_list_size; i++)
 543         {
 544                 char_to_buf(packet, CEIL(reg_list[i]->size, 8) * 2, reg_list[i]->value, reg_list[i]->size);
 545                 reg_list[i]->dirty = 1;
 546         }
 547
 548         gdb_put_packet(connection, "OK", 2);
 549 }
 550
 551 void gdb_get_register_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 552 {
 553         char *hex_buf;
 554         char *reg_packet;
 555         char *reg_packet_p;
 556         int reg_num = strtoul(packet + 1, NULL, 16);
 557         reg_t **reg_list;
 558         int reg_list_size;
 559         int retval;
 560         int i;
 561
 562         DEBUG("");
 563
 564         if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
 565         {
 566                 switch (retval)
 567                 {
 568                         case ERROR_TARGET_NOT_HALTED:
 569                                 ERROR("gdb requested registers, but we're not halted");
 570                                 exit(-1);
 571                         default:
 572                                 ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
 573                                 exit(-1);
 574                 }
 575         }
 576
 577         if (reg_list_size <= reg_num)
 578         {
 579                 ERROR("gdb requested a non-existing register");
 580                 exit(-1);
 581         }
 582
 583         hex_buf = buf_to_char(reg_list[reg_num]->value, reg_list[reg_num]->size);
 584         reg_packet = reg_packet_p = malloc(CEIL(reg_list[reg_num]->size, 8) * 2);
 585
 586         for (i = CEIL(reg_list[reg_num]->size, 8) * 2; i > 0; i -= 2)
 587         {
 588                 *reg_packet_p++ = hex_buf[i - 2];
 589                 *reg_packet_p++ = hex_buf[i - 1];
 590         }
 591
 592         gdb_put_packet(connection, reg_packet, CEIL(reg_list[reg_num]->size, 8) * 2);
 593
 594         free(reg_packet);
 595         free(hex_buf);
 596
 597 }
 598
 599 void gdb_set_register_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 600 {
 601         char *separator;
 602         int reg_num = strtoul(packet + 1, &separator, 16);
 603         reg_t **reg_list;
 604         int reg_list_size;
 605         int retval;
 606
 607         DEBUG("");
 608
 609         if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
 610         {
 611                 switch (retval)
 612                 {
 613                         case ERROR_TARGET_NOT_HALTED:
 614                                 ERROR("gdb requested registers, but we're not halted");
 615                                 exit(-1);
 616                         default:
 617                                 ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
 618                                 exit(-1);
 619                 }
 620         }
 621
 622         if (reg_list_size < reg_num)
 623         {
 624                 ERROR("gdb requested a non-existing register");
 625                 exit(-1);
 626         }
 627
 628         if (*separator != '=')
 629         {
 630                 ERROR("GDB set register packet, but no '=' following the register number");
 631                 exit(-1);
 632         }
 633
 634         char_to_buf(separator + 1, CEIL(reg_list[reg_num]->size, 8) * 2, reg_list[reg_num]->value, reg_list[reg_num]->size);
 635         reg_list[reg_num]->dirty = 1;
 636
 637         gdb_put_packet(connection, "OK", 2);
 638
 639 }
 640
 641 void gdb_read_memory_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 642 {
 643         char *separator;
 644         u32 addr = 0;
 645         u32 len = 0;
 646
 647         u8 *buffer;
 648         char *hex_buffer;
 649
 650         int i;
 651
 652         /* skip command character */
 653         packet++;
 654
 655         addr = strtoul(packet, &separator, 16);
 656
 657         if (*separator != ',')
 658                 return;
 659
 660         len = strtoul(separator+1, NULL, 16);
 661
 662         buffer = malloc(len);
 663
 664         DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
 665
 666         switch (len)
 667         {
 668                 case 4:
 669                         if ((addr % 4) == 0)
 670                                 target->type->read_memory(target, addr, 4, 1, buffer);
 671                         else
 672                                 target->type->read_memory(target, addr, 1, len, buffer);
 673                         break;
 674                 case 2:
 675                         if ((addr % 2) == 0)
 676                                 target->type->read_memory(target, addr, 2, 1, buffer);
 677                         else
 678                                 target->type->read_memory(target, addr, 1, len, buffer);
 679                         break;
 680                 default:
 681                         if (((addr % 4) == 0) && ((len % 4) == 0))
 682                                 target->type->read_memory(target, addr, 4, len / 4, buffer);
 683                         else
 684                                 target->type->read_memory(target, addr, 1, len, buffer);
 685         }
 686
 687         hex_buffer = malloc(len * 2 + 1);
 688
 689         for (i=0; i<len; i++)
 690                 snprintf(hex_buffer + 2*i, 3, "%2.2x", buffer[i]);
 691
 692         gdb_put_packet(connection, hex_buffer, len * 2);
 693
 694         free(hex_buffer);
 695         free(buffer);
 696 }
 697
 698 void gdb_write_memory_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 699 {
 700         char *separator;
 701         u32 addr = 0;
 702         u32 len = 0;
 703
 704         u8 *buffer;
 705
 706         int i;
 707
 708         /* skip command character */
 709         packet++;
 710
 711         addr = strtoul(packet, &separator, 16);
 712
 713         if (*separator != ',')
 714                 return;
 715
 716         len = strtoul(separator+1, &separator, 16);
 717
 718         if (*(separator++) != ':')
 719                 return;
 720
 721         buffer = malloc(len);
 722
 723         DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
 724
 725         for (i=0; i<len; i++)
 726         {
 727                 u32 tmp;
 728                 sscanf(separator + 2*i, "%2x", &tmp);
 729                 buffer[i] = tmp;
 730         }
 731
 732         switch (len)
 733         {
 734                 /* handle sized writes */
 735                 case 4:
 736                         if ((addr % 4) == 0)
 737                                 target->type->write_memory(target, addr, 4, 1, buffer);
 738                         else
 739                                 target->type->write_memory(target, addr, 1, len, buffer);
 740                         break;
 741                 case 2:
 742                         if ((addr % 2) == 0)
 743                                 target->type->write_memory(target, addr, 2, 1, buffer);
 744                         else
 745                                 target->type->write_memory(target, addr, 1, len, buffer);
 746                         break;
 747                 case 3:
 748                 case 1:
 749                         target->type->write_memory(target, addr, 1, len, buffer);
 750                         break;
 751                 /* handle bulk writes */
 752                 default:
 753                         target_write_buffer(target, addr, len, buffer);
 754                         break;
 755         }
 756
 757         gdb_put_packet(connection, "OK", 2);
 758
 759         free(buffer);
 760 }
 761
 762 void gdb_write_memory_binary_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 763 {
 764         char *separator;
 765         u32 addr = 0;
 766         u32 len = 0;
 767
 768         u8 *buffer;
 769
 770         /* skip command character */
 771         packet++;
 772
 773         addr = strtoul(packet, &separator, 16);
 774
 775         if (*separator != ',')
 776                 return;
 777
 778         len = strtoul(separator+1, &separator, 16);
 779
 780         if (*(separator++) != ':')
 781                 return;
 782
 783         if( len ) {
 784
 785                 buffer = malloc(len);
 786
 787                 DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
 788
 789                 memcpy( buffer, separator, len );
 790
 791                 switch (len)
 792                 {
 793                         case 4:
 794                                 if ((addr % 4) == 0)
 795                                         target->type->write_memory(target, addr, 4, 1, buffer);
 796                                 else
 797                                         target->type->write_memory(target, addr, 1, len, buffer);
 798                                 break;
 799                         case 2:
 800                                 if ((addr % 2) == 0)
 801                                         target->type->write_memory(target, addr, 2, 1, buffer);
 802                                 else
 803                                         target->type->write_memory(target, addr, 1, len, buffer);
 804                                 break;
 805                         case 3:
 806                         case 1:
 807                                 target->type->write_memory(target, addr, 1, len, buffer);
 808                                 break;
 809                         default:
 810                                 target_write_buffer(target, addr, len, buffer);
 811                                 break;
 812                 }
 813
 814                 free(buffer);
 815         }
 816
 817         gdb_put_packet(connection, "OK", 2);
 818 }
 819
 820 void gdb_step_continue_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 821 {
 822         int current = 0;
 823         u32 address = 0x0;
 824
 825         DEBUG("");
 826
 827         if (packet_size > 1)
 828         {
 829                 u32 address = 0;
 830                 packet[packet_size] = 0;
 831                 address = strtoul(packet + 1, NULL, 16);
 832         }
 833         else
 834         {
 835                 current = 1;
 836         }
 837
 838         if (packet[0] == 'c')
 839         {
 840                 DEBUG("continue");
 841                 target->type->resume(target, current, address, 0, 0); /* resume at current address, don't handle breakpoints, not debugging */
 842         }
 843         else if (packet[0] == 's')
 844         {
 845                 DEBUG("step");
 846                 target->type->step(target, current, address, 0); /* step at current or address, don't handle breakpoints */
 847         }
 848 }
 849
 850 void gdb_breakpoint_watchpoint_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 851 {
 852         int type;
 853         enum breakpoint_type bp_type;
 854         enum watchpoint_rw wp_type;
 855         u32 address;
 856         u32 size;
 857         char *separator;
 858         int retval;
 859
 860         DEBUG("");
 861
 862         type = strtoul(packet + 1, &separator, 16);
 863
 864         if (type == 0)  /* memory breakpoint */
 865                 bp_type = BKPT_SOFT;
 866         else if (type == 1) /* hardware breakpoint */
 867                 bp_type = BKPT_HARD;
 868         else if (type == 2) /* write watchpoint */
 869                 wp_type = WPT_WRITE;
 870         else if (type == 3) /* read watchpoint */
 871                 wp_type = WPT_READ;
 872         else if (type == 4) /* access watchpoint */
 873                 wp_type = WPT_ACCESS;
 874
 875         if (*separator != ',')
 876                 return;
 877
 878         address = strtoul(separator+1, &separator, 16);
 879
 880         if (*separator != ',')
 881                 return;
 882
 883         size = strtoul(separator+1, &separator, 16);
 884
 885         switch (type)
 886         {
 887                 case 0:
 888                 case 1:
 889                         if (packet[0] == 'Z')
 890                         {
 891                                 if ((retval = breakpoint_add(target, address, size, bp_type)) != ERROR_OK)
 892                                 {
 893                                         if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
 894                                         {
 895                                                 gdb_put_packet(connection, "E00", 3);
 896                                                 break;
 897                                         }
 898                                 }
 899                         }
 900                         else
 901                         {
 902                                 breakpoint_remove(target, address);
 903                         }
 904                         gdb_put_packet(connection, "OK", 2);
 905                         break;
 906                 case 2:
 907                 case 3:
 908                 case 4:
 909                 {
 910                         if (packet[0] == 'Z')
 911                                 watchpoint_add(target, address, size, type-2, 0, 0xffffffffu);
 912                         else
 913                                 watchpoint_remove(target, address);
 914                         gdb_put_packet(connection, "OK", 2);
 915                         break;
 916                 }
 917                 default:
 918                         break;
 919         }
 920
 921 }
 922
 923 void gdb_query_packet(connection_t *connection, char *packet, int packet_size)
 924 {
 925         command_context_t *cmd_ctx = connection->cmd_ctx;
 926         gdb_service_t *gdb_service = connection->service->priv;
 927         target_t *target = gdb_service->target;
 928
 929         if (strstr(packet, "qRcmd,"))
 930         {
 931                 if (packet_size > 6)
 932                 {
 933                         char *cmd;
 934                         int i;
 935                         cmd = malloc((packet_size - 6)/2 + 1);
 936                         for (i=0; i < (packet_size - 6)/2; i++)
 937                         {
 938                                 u32 tmp;
 939                                 sscanf(packet + 6 + 2*i, "%2x", &tmp);
 940                                 cmd[i] = tmp;
 941                         }
 942                         cmd[(packet_size - 6)/2] = 0x0;
 943                         command_run_line(cmd_ctx, cmd);
 944                         free(cmd);
 945                 }
 946                 gdb_put_packet(connection, "OK", 2);
 947                 return;
 948         }
 949
 950         gdb_put_packet(connection, "", 0);
 951 }
 952
 953 int gdb_input(connection_t *connection)
 954 {
 955         gdb_service_t *gdb_service = connection->service->priv;
 956         target_t *target = gdb_service->target;
 957         char packet[GDB_BUFFER_SIZE];
 958         int packet_size;
 959         int retval;
 960         gdb_connection_t *gdb_con = connection->priv;
 961
 962         /* drain input buffer */
 963         do
 964         {
 965                 packet_size = GDB_BUFFER_SIZE-1;
 966                 if ((retval = gdb_get_packet(connection, packet, &packet_size)) != ERROR_OK)
 967                 {
 968                         switch (retval)
 969                         {
 970                                 case ERROR_GDB_BUFFER_TOO_SMALL:
 971                                         ERROR("BUG: buffer supplied for gdb packet was too small");
 972                                         exit(-1);
 973                                 case ERROR_SERVER_REMOTE_CLOSED:
 974                                         return ERROR_SERVER_REMOTE_CLOSED;
 975                                 default:
 976                                         ERROR("unexpected error");
 977                                         exit(-1);
 978                         }
 979                 }
 980
 981                 /* terminate with zero */
 982                 packet[packet_size] = 0;
 983
 984                 DEBUG("recevied packet: '%s'", packet);
 985
 986                 if (packet_size > 0)
 987                 {
 988                         switch (packet[0])
 989                         {
 990                                 case 'H':
 991                                         /* Hct... -- set thread
 992                                         * we don't have threads, send empty reply */
 993                                         gdb_put_packet(connection, NULL, 0);
 994                                         break;
 995                                 case 'q':
 996                                         gdb_query_packet(connection, packet, packet_size);
 997                                         break;
 998                                 case 'g':
 999                                         gdb_get_registers_packet(connection, target, packet, packet_size);
1000                                         break;
1001                                 case 'G':
1002                                         gdb_set_registers_packet(connection, target, packet, packet_size);
1003                                         break;
1004                                 case 'p':
1005                                         gdb_get_register_packet(connection, target, packet, packet_size);
1006                                         break;
1007                                 case 'P':
1008                                         gdb_set_register_packet(connection, target, packet, packet_size);
1009                                         break;
1010                                 case 'm':
1011                                         gdb_read_memory_packet(connection, target, packet, packet_size);
1012                                         break;
1013                                 case 'M':
1014                                         gdb_write_memory_packet(connection, target, packet, packet_size);
1015                                         break;
1016                                 case 'z':
1017                                 case 'Z':
1018                                         gdb_breakpoint_watchpoint_packet(connection, target, packet, packet_size);
1019                                         break;
1020                                 case '?':
1021                                         gdb_last_signal_packet(connection, target, packet, packet_size);
1022                                 break;
1023                                 case 'c':
1024                                 case 's':
1025                                         gdb_step_continue_packet(connection, target, packet, packet_size);
1026                                         break;
1027                                 case 'D':
1028                                         target->type->resume(target, 1, 0, 1, 0);
1029                                         gdb_put_packet(connection, "OK", 2);
1030                                         break;
1031                                 case 'X':
1032                                         gdb_write_memory_binary_packet(connection, target, packet, packet_size);
1033                                         break;
1034                                 case 'k':
1035                                         gdb_put_packet(connection, "OK", 2);
1036                                         return ERROR_SERVER_REMOTE_CLOSED;
1037                                 default:
1038                                         /* ignore unkown packets */
1039                                         DEBUG("ignoring 0x%2.2x packet", packet[0]);
1040                                         gdb_put_packet(connection, NULL, 0);
1041                                         break;
1042                         }
1043                 }
1044
1045                 if (gdb_con->ctrl_c)
1046                 {
1047                         if (target->state == TARGET_RUNNING)
1048                         {
1049                                 target->type->halt(target);
1050                                 gdb_con->ctrl_c = 0;
1051                         }
1052                 }
1053
1054         } while (gdb_con->buf_cnt > 0);
1055
1056         return ERROR_OK;
1057 }
1058
1059 int gdb_init()
1060 {
1061         gdb_service_t *gdb_service;
1062         target_t *target = targets;
1063         int i = 0;
1064
1065         if (!target)
1066         {
1067                 WARNING("no gdb ports allocated as no target has been specified");
1068                 return ERROR_OK;
1069         }
1070
1071         if (gdb_port == 0)
1072         {
1073                 WARNING("no gdb port specified, using default port 3333");
1074                 gdb_port = 3333;
1075         }
1076
1077         while (target)
1078         {
1079                 char service_name[8];
1080
1081                 snprintf(service_name, 8, "gdb-%2.2i", i);
1082
1083                 gdb_service = malloc(sizeof(gdb_service_t));
1084                 gdb_service->target = target;
1085
1086                 add_service("gdb", CONNECTION_GDB, gdb_port + i, 1, gdb_new_connection, gdb_input, gdb_connection_closed, gdb_service);
1087
1088                 DEBUG("gdb service for target %s at port %i", target->type->name, gdb_port + i);
1089
1090                 target = target->next;
1091         }
1092
1093         return ERROR_OK;
1094 }
1095
1096 /* daemon configuration command gdb_port */
1097 int handle_gdb_port_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1098 {
1099         if (argc == 0)
1100                 return ERROR_OK;
1101
1102         /* only if the port wasn't overwritten by cmdline */
1103         if (gdb_port == 0)
1104                 gdb_port = strtoul(args[0], NULL, 0);
1105
1106         return ERROR_OK;
1107 }
1108
1109 int gdb_register_commands(command_context_t *command_context)
1110 {
1111         register_command(command_context, NULL, "gdb_port", handle_gdb_port_command,
1112                                          COMMAND_CONFIG, "");
1113
1114         return ERROR_OK;
1115 }