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