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  *   Copyright (C) 2007,2008 Øyvind Harboe                                 *
   6  *   oyvind.harboe@zylin.com                                               *
   7  *                                                                         *
   8  *   Copyright (C) 2008 by Spencer Oliver                                  *
   9  *   spen@spen-soft.co.uk                                                  *
  10  *                                                                         *
  11  *   This program is free software; you can redistribute it and/or modify  *
  12  *   it under the terms of the GNU General Public License as published by  *
  13  *   the Free Software Foundation; either version 2 of the License, or     *
  14  *   (at your option) any later version.                                   *
  15  *                                                                         *
  16  *   This program is distributed in the hope that it will be useful,       *
  17  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  18  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  19  *   GNU General Public License for more details.                          *
  20  *                                                                         *
  21  *   You should have received a copy of the GNU General Public License     *
  22  *   along with this program; if not, write to the                         *
  23  *   Free Software Foundation, Inc.,                                       *
  24  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  25  ***************************************************************************/
  26 #ifdef HAVE_CONFIG_H
  27 #include "config.h"
  28 #endif
  29
  30 #include "gdb_server.h"
  31 #include "target_request.h"
  32 #include "register.h"
  33 #include "server.h"
  34 #include "flash.h"
  35 #include "image.h"
  36 #include "jtag.h"
  37
  38
  39 #if 0
  40 #define _DEBUG_GDB_IO_
  41 #endif
  42
  43 static struct gdb_connection *current_gdb_connection;
  44
  45 static int gdb_breakpoint_override;
  46 static enum breakpoint_type gdb_breakpoint_override_type;
  47
  48 extern int gdb_error(struct connection *connection, int retval);
  49 static unsigned short gdb_port = 3333;
  50 static const char *DIGITS = "0123456789abcdef";
  51
  52 static void gdb_log_callback(void *priv, const char *file, unsigned line,
  53                 const char *function, const char *string);
  54
  55 /* number of gdb connections, mainly to supress gdb related debugging spam
  56  * in helper/log.c when no gdb connections are actually active */
  57 int gdb_actual_connections;
  58
  59 /* set if we are sending a memory map to gdb
  60  * via qXfer:memory-map:read packet */
  61 /* enabled by default*/
  62 int gdb_use_memory_map = 1;
  63 /* enabled by default*/
  64 int gdb_flash_program = 1;
  65
  66 /* if set, data aborts cause an error to be reported in memory read packets
  67  * see the code in gdb_read_memory_packet() for further explanations */
  68 int gdb_report_data_abort = 0;
  69
  70 int gdb_last_signal(target_t *target)
  71 {
  72         switch (target->debug_reason)
  73         {
  74                 case DBG_REASON_DBGRQ:
  75                         return 0x2; /* SIGINT */
  76                 case DBG_REASON_BREAKPOINT:
  77                 case DBG_REASON_WATCHPOINT:
  78                 case DBG_REASON_WPTANDBKPT:
  79                         return 0x05; /* SIGTRAP */
  80                 case DBG_REASON_SINGLESTEP:
  81                         return 0x05; /* SIGTRAP */
  82                 case DBG_REASON_NOTHALTED:
  83                         return 0x0; /* no signal... shouldn't happen */
  84                 default:
  85                         LOG_USER("undefined debug reason %d - target needs reset", target->debug_reason);
  86                         return 0x0;
  87         }
  88 }
  89
  90 int check_pending(struct connection *connection, int timeout_s, int *got_data)
  91 {
  92         /* a non-blocking socket will block if there is 0 bytes available on the socket,
  93          * but return with as many bytes as are available immediately
  94          */
  95         struct timeval tv;
  96         fd_set read_fds;
  97         struct gdb_connection *gdb_con = connection->priv;
  98         int t;
  99         if (got_data == NULL)
 100                 got_data=&t;
 101         *got_data = 0;
 102
 103         if (gdb_con->buf_cnt > 0)
 104         {
 105                 *got_data = 1;
 106                 return ERROR_OK;
 107         }
 108
 109         FD_ZERO(&read_fds);
 110         FD_SET(connection->fd, &read_fds);
 111
 112         tv.tv_sec = timeout_s;
 113         tv.tv_usec = 0;
 114         if (socket_select(connection->fd + 1, &read_fds, NULL, NULL, &tv) == 0)
 115         {
 116                 /* This can typically be because a "monitor" command took too long
 117                  * before printing any progress messages
 118                  */
 119                 if (timeout_s > 0)
 120                 {
 121                         return ERROR_GDB_TIMEOUT;
 122                 } else
 123                 {
 124                         return ERROR_OK;
 125                 }
 126         }
 127         *got_data = FD_ISSET(connection->fd, &read_fds) != 0;
 128         return ERROR_OK;
 129 }
 130
 131 int gdb_get_char(struct connection *connection, int* next_char)
 132 {
 133         struct gdb_connection *gdb_con = connection->priv;
 134         int retval = ERROR_OK;
 135
 136 #ifdef _DEBUG_GDB_IO_
 137         char *debug_buffer;
 138 #endif
 139
 140         if (gdb_con->buf_cnt-- > 0)
 141         {
 142                 *next_char = *(gdb_con->buf_p++);
 143                 if (gdb_con->buf_cnt > 0)
 144                         connection->input_pending = 1;
 145                 else
 146                         connection->input_pending = 0;
 147
 148 #ifdef _DEBUG_GDB_IO_
 149                 LOG_DEBUG("returned char '%c' (0x%2.2x)", *next_char, *next_char);
 150 #endif
 151
 152                 return ERROR_OK;
 153         }
 154
 155         for (;;)
 156         {
 157                 if (connection->service->type == CONNECTION_PIPE)
 158                 {
 159                         gdb_con->buf_cnt = read(connection->fd, gdb_con->buffer, GDB_BUFFER_SIZE);
 160                 }
 161                 else
 162                 {
 163                         retval = check_pending(connection, 1, NULL);
 164                         if (retval != ERROR_OK)
 165                                 return retval;
 166                         gdb_con->buf_cnt = read_socket(connection->fd, gdb_con->buffer, GDB_BUFFER_SIZE);
 167                 }
 168
 169                 if (gdb_con->buf_cnt > 0)
 170                 {
 171                         break;
 172                 }
 173                 if (gdb_con->buf_cnt == 0)
 174                 {
 175                         gdb_con->closed = 1;
 176                         return ERROR_SERVER_REMOTE_CLOSED;
 177                 }
 178
 179 #ifdef _WIN32
 180                 errno = WSAGetLastError();
 181
 182                 switch (errno)
 183                 {
 184                         case WSAEWOULDBLOCK:
 185                                 usleep(1000);
 186                                 break;
 187                         case WSAECONNABORTED:
 188                                 gdb_con->closed = 1;
 189                                 return ERROR_SERVER_REMOTE_CLOSED;
 190                         case WSAECONNRESET:
 191                                 gdb_con->closed = 1;
 192                                 return ERROR_SERVER_REMOTE_CLOSED;
 193                         default:
 194                                 LOG_ERROR("read: %d", errno);
 195                                 exit(-1);
 196                 }
 197 #else
 198                 switch (errno)
 199                 {
 200                         case EAGAIN:
 201                                 usleep(1000);
 202                                 break;
 203                         case ECONNABORTED:
 204                                 gdb_con->closed = 1;
 205                                 return ERROR_SERVER_REMOTE_CLOSED;
 206                         case ECONNRESET:
 207                                 gdb_con->closed = 1;
 208                                 return ERROR_SERVER_REMOTE_CLOSED;
 209                         default:
 210                                 LOG_ERROR("read: %s", strerror(errno));
 211                                 gdb_con->closed = 1;
 212                                 return ERROR_SERVER_REMOTE_CLOSED;
 213                 }
 214 #endif
 215         }
 216
 217 #ifdef _DEBUG_GDB_IO_
 218         debug_buffer = malloc(gdb_con->buf_cnt + 1);
 219         memcpy(debug_buffer, gdb_con->buffer, gdb_con->buf_cnt);
 220         debug_buffer[gdb_con->buf_cnt] = 0;
 221         LOG_DEBUG("received '%s'", debug_buffer);
 222         free(debug_buffer);
 223 #endif
 224
 225         gdb_con->buf_p = gdb_con->buffer;
 226         gdb_con->buf_cnt--;
 227         *next_char = *(gdb_con->buf_p++);
 228         if (gdb_con->buf_cnt > 0)
 229                 connection->input_pending = 1;
 230         else
 231                 connection->input_pending = 0;
 232 #ifdef _DEBUG_GDB_IO_
 233         LOG_DEBUG("returned char '%c' (0x%2.2x)", *next_char, *next_char);
 234 #endif
 235
 236         return retval;
 237 }
 238
 239 int gdb_putback_char(struct connection *connection, int last_char)
 240 {
 241         struct gdb_connection *gdb_con = connection->priv;
 242
 243         if (gdb_con->buf_p > gdb_con->buffer)
 244         {
 245                 *(--gdb_con->buf_p) = last_char;
 246                 gdb_con->buf_cnt++;
 247         }
 248         else
 249         {
 250                 LOG_ERROR("BUG: couldn't put character back");
 251         }
 252
 253         return ERROR_OK;
 254 }
 255
 256 /* The only way we can detect that the socket is closed is the first time
 257  * we write to it, we will fail. Subsequent write operations will
 258  * succeed. Shudder! */
 259 int gdb_write(struct connection *connection, void *data, int len)
 260 {
 261         struct gdb_connection *gdb_con = connection->priv;
 262         if (gdb_con->closed)
 263                 return ERROR_SERVER_REMOTE_CLOSED;
 264
 265         if (connection->service->type == CONNECTION_PIPE)
 266         {
 267                 /* write to stdout */
 268                 if (write(STDOUT_FILENO, data, len) == len)
 269                 {
 270                         return ERROR_OK;
 271                 }
 272         }
 273         else
 274         {
 275                 if (write_socket(connection->fd, data, len) == len)
 276                 {
 277                         return ERROR_OK;
 278                 }
 279         }
 280         gdb_con->closed = 1;
 281         return ERROR_SERVER_REMOTE_CLOSED;
 282 }
 283
 284 int gdb_put_packet_inner(struct connection *connection, char *buffer, int len)
 285 {
 286         int i;
 287         unsigned char my_checksum = 0;
 288 #ifdef _DEBUG_GDB_IO_
 289         char *debug_buffer;
 290 #endif
 291         int reply;
 292         int retval;
 293         struct gdb_connection *gdb_con = connection->priv;
 294
 295         for (i = 0; i < len; i++)
 296                 my_checksum += buffer[i];
 297
 298 #ifdef _DEBUG_GDB_IO_
 299         /*
 300          * At this point we should have nothing in the input queue from GDB,
 301          * however sometimes '-' is sent even though we've already received
 302          * an ACK (+) for everything we've sent off.
 303          */
 304         int gotdata;
 305         for (;;)
 306         {
 307                 if ((retval = check_pending(connection, 0, &gotdata)) != ERROR_OK)
 308                         return retval;
 309                 if (!gotdata)
 310                         break;
 311                 if ((retval = gdb_get_char(connection, &reply)) != ERROR_OK)
 312                         return retval;
 313                 if (reply == '$') {
 314                         /* fix a problem with some IAR tools */
 315                         gdb_putback_char(connection, reply);
 316                         LOG_DEBUG("Unexpected start of new packet");
 317                         break;
 318                 }
 319
 320                 LOG_WARNING("Discard unexpected char %c", reply);
 321         }
 322 #endif
 323
 324         while (1)
 325         {
 326 #ifdef _DEBUG_GDB_IO_
 327                 debug_buffer = malloc(len + 1);
 328                 memcpy(debug_buffer, buffer, len);
 329                 debug_buffer[len] = 0;
 330                 LOG_DEBUG("sending packet '$%s#%2.2x'", debug_buffer, my_checksum);
 331                 free(debug_buffer);
 332 #endif
 333
 334                 char local_buffer[1024];
 335                 local_buffer[0] = '$';
 336                 if ((size_t)len + 4 <= sizeof(local_buffer))
 337                 {
 338                         /* performance gain on smaller packets by only a single call to gdb_write() */
 339                         memcpy(local_buffer + 1, buffer, len++);
 340                         local_buffer[len++] = '#';
 341                         local_buffer[len++] = DIGITS[(my_checksum >> 4) & 0xf];
 342                         local_buffer[len++] = DIGITS[my_checksum & 0xf];
 343                         if ((retval = gdb_write(connection, local_buffer, len)) != ERROR_OK)
 344                         {
 345                                 return retval;
 346                         }
 347                 }
 348                 else
 349                 {
 350                         /* larger packets are transmitted directly from caller supplied buffer
 351                            by several calls to gdb_write() to avoid dynamic allocation */
 352                         local_buffer[1] = '#';
 353                         local_buffer[2] = DIGITS[(my_checksum >> 4) & 0xf];
 354                         local_buffer[3] = DIGITS[my_checksum & 0xf];
 355                         if ((retval = gdb_write(connection, local_buffer, 1)) != ERROR_OK)
 356                         {
 357                                 return retval;
 358                         }
 359                         if ((retval = gdb_write(connection, buffer, len)) != ERROR_OK)
 360                         {
 361                                 return retval;
 362                         }
 363                         if ((retval = gdb_write(connection, local_buffer + 1, 3)) != ERROR_OK)
 364                         {
 365                                 return retval;
 366                         }
 367                 }
 368
 369                 if (gdb_con->noack_mode)
 370                         break;
 371
 372                 if ((retval = gdb_get_char(connection, &reply)) != ERROR_OK)
 373                         return retval;
 374
 375                 if (reply == '+')
 376                         break;
 377                 else if (reply == '-')
 378                 {
 379                         /* Stop sending output packets for now */
 380                         log_remove_callback(gdb_log_callback, connection);
 381                         LOG_WARNING("negative reply, retrying");
 382                 }
 383                 else if (reply == 0x3)
 384                 {
 385                         gdb_con->ctrl_c = 1;
 386                         if ((retval = gdb_get_char(connection, &reply)) != ERROR_OK)
 387                                 return retval;
 388                         if (reply == '+')
 389                                 break;
 390                         else if (reply == '-')
 391                         {
 392                                 /* Stop sending output packets for now */
 393                                 log_remove_callback(gdb_log_callback, connection);
 394                                 LOG_WARNING("negative reply, retrying");
 395                         }
 396                         else if (reply == '$') {
 397                                 LOG_ERROR("GDB missing ack(1) - assumed good");
 398                                 gdb_putback_char(connection, reply);
 399                                 return ERROR_OK;
 400                         } else {
 401
 402                                 LOG_ERROR("unknown character(1) 0x%2.2x in reply, dropping connection", reply);
 403                                 gdb_con->closed = 1;
 404                                 return ERROR_SERVER_REMOTE_CLOSED;
 405                         }
 406                 }
 407                 else if (reply == '$') {
 408                         LOG_ERROR("GDB missing ack(2) - assumed good");
 409                         gdb_putback_char(connection, reply);
 410                         return ERROR_OK;
 411                 }
 412                 else
 413                 {
 414                         LOG_ERROR("unknown character(2) 0x%2.2x in reply, dropping connection", reply);
 415                         gdb_con->closed = 1;
 416                         return ERROR_SERVER_REMOTE_CLOSED;
 417                 }
 418         }
 419         if (gdb_con->closed)
 420                 return ERROR_SERVER_REMOTE_CLOSED;
 421
 422         return ERROR_OK;
 423 }
 424
 425 int gdb_put_packet(struct connection *connection, char *buffer, int len)
 426 {
 427         struct gdb_connection *gdb_con = connection->priv;
 428         gdb_con->busy = 1;
 429         int retval = gdb_put_packet_inner(connection, buffer, len);
 430         gdb_con->busy = 0;
 431
 432         /* we sent some data, reset timer for keep alive messages */
 433         kept_alive();
 434
 435         return retval;
 436 }
 437
 438 static __inline__ int fetch_packet(struct connection *connection, int *checksum_ok, int noack, int *len, char *buffer)
 439 {
 440         unsigned char my_checksum = 0;
 441         char checksum[3];
 442         int character;
 443         int retval;
 444
 445         struct gdb_connection *gdb_con = connection->priv;
 446         my_checksum = 0;
 447         int count = 0;
 448         count = 0;
 449         for (;;)
 450         {
 451                 /* The common case is that we have an entire packet with no escape chars.
 452                  * We need to leave at least 2 bytes in the buffer to have
 453                  * gdb_get_char() update various bits and bobs correctly.
 454                  */
 455                 if ((gdb_con->buf_cnt > 2) && ((gdb_con->buf_cnt + count) < *len))
 456                 {
 457                         /* The compiler will struggle a bit with constant propagation and
 458                          * aliasing, so we help it by showing that these values do not
 459                          * change inside the loop
 460                          */
 461                         int i;
 462                         char *buf = gdb_con->buf_p;
 463                         int run = gdb_con->buf_cnt - 2;
 464                         i = 0;
 465                         int done = 0;
 466                         while (i < run)
 467                         {
 468                                 character = *buf++;
 469                                 i++;
 470                                 if (character == '#')
 471                                 {
 472                                         /* Danger! character can be '#' when esc is
 473                                          * used so we need an explicit boolean for done here.
 474                                          */
 475                                         done = 1;
 476                                         break;
 477                                 }
 478
 479                                 if (character == '}')
 480                                 {
 481                                         /* data transmitted in binary mode (X packet)
 482                                          * uses 0x7d as escape character */
 483                                         my_checksum += character & 0xff;
 484                                         character = *buf++;
 485                                         i++;
 486                                         my_checksum += character & 0xff;
 487                                         buffer[count++] = (character ^ 0x20) & 0xff;
 488                                 }
 489                                 else
 490                                 {
 491                                         my_checksum += character & 0xff;
 492                                         buffer[count++] = character & 0xff;
 493                                 }
 494                         }
 495                         gdb_con->buf_p += i;
 496                         gdb_con->buf_cnt -= i;
 497                         if (done)
 498                                 break;
 499                 }
 500                 if (count > *len)
 501                 {
 502                         LOG_ERROR("packet buffer too small");
 503                         return ERROR_GDB_BUFFER_TOO_SMALL;
 504                 }
 505
 506                 if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
 507                         return retval;
 508
 509                 if (character == '#')
 510                         break;
 511
 512                 if (character == '}')
 513                 {
 514                         /* data transmitted in binary mode (X packet)
 515                          * uses 0x7d as escape character */
 516                         my_checksum += character & 0xff;
 517                         if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
 518                                 return retval;
 519                         my_checksum += character & 0xff;
 520                         buffer[count++] = (character ^ 0x20) & 0xff;
 521                 }
 522                 else
 523                 {
 524                         my_checksum += character & 0xff;
 525                         buffer[count++] = character & 0xff;
 526                 }
 527         }
 528
 529         *len = count;
 530
 531         if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
 532                 return retval;
 533         checksum[0] = character;
 534         if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
 535                 return retval;
 536         checksum[1] = character;
 537         checksum[2] = 0;
 538
 539         if (!noack)
 540         {
 541                 *checksum_ok = (my_checksum == strtoul(checksum, NULL, 16));
 542         }
 543
 544         return ERROR_OK;
 545 }
 546
 547 int gdb_get_packet_inner(struct connection *connection, char *buffer, int *len)
 548 {
 549         int character;
 550         int retval;
 551         struct gdb_connection *gdb_con = connection->priv;
 552
 553         while (1)
 554         {
 555                 do
 556                 {
 557                         if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
 558                                 return retval;
 559
 560 #ifdef _DEBUG_GDB_IO_
 561                         LOG_DEBUG("character: '%c'", character);
 562 #endif
 563
 564                         switch (character)
 565                         {
 566                                 case '$':
 567                                         break;
 568                                 case '+':
 569                                         /* gdb sends a dummy ack '+' at every remote connect - see remote_start_remote (remote.c)
 570                                          * incase anyone tries to debug why they receive this warning every time */
 571                                         LOG_WARNING("acknowledgment received, but no packet pending");
 572                                         break;
 573                                 case '-':
 574                                         LOG_WARNING("negative acknowledgment, but no packet pending");
 575                                         break;
 576                                 case 0x3:
 577                                         gdb_con->ctrl_c = 1;
 578                                         *len = 0;
 579                                         return ERROR_OK;
 580                                 default:
 581                                         LOG_WARNING("ignoring character 0x%x", character);
 582                                         break;
 583                         }
 584                 } while (character != '$');
 585
 586
 587
 588                 int checksum_ok = 0;
 589                 /* explicit code expansion here to get faster inlined code in -O3 by not
 590                  * calculating checksum
 591                  */
 592                 if (gdb_con->noack_mode)
 593                 {
 594                         if ((retval = fetch_packet(connection, &checksum_ok, 1, len, buffer)) != ERROR_OK)
 595                                 return retval;
 596                 } else
 597                 {
 598                         if ((retval = fetch_packet(connection, &checksum_ok, 0, len, buffer)) != ERROR_OK)
 599                                 return retval;
 600                 }
 601
 602                 if (gdb_con->noack_mode)
 603                 {
 604                         /* checksum is not checked in noack mode */
 605                         break;
 606                 }
 607                 if (checksum_ok)
 608                 {
 609                         if ((retval = gdb_write(connection, "+", 1)) != ERROR_OK)
 610                         {
 611                                 return retval;
 612                         }
 613                         break;
 614                 }
 615         }
 616         if (gdb_con->closed)
 617                 return ERROR_SERVER_REMOTE_CLOSED;
 618
 619         return ERROR_OK;
 620 }
 621
 622 int gdb_get_packet(struct connection *connection, char *buffer, int *len)
 623 {
 624         struct gdb_connection *gdb_con = connection->priv;
 625         gdb_con->busy = 1;
 626         int retval = gdb_get_packet_inner(connection, buffer, len);
 627         gdb_con->busy = 0;
 628         return retval;
 629 }
 630
 631 int gdb_output_con(struct connection *connection, const char* line)
 632 {
 633         char *hex_buffer;
 634         int i, bin_size;
 635
 636         bin_size = strlen(line);
 637
 638         hex_buffer = malloc(bin_size*2 + 2);
 639         if (hex_buffer == NULL)
 640                 return ERROR_GDB_BUFFER_TOO_SMALL;
 641
 642         hex_buffer[0] = 'O';
 643         for (i = 0; i < bin_size; i++)
 644                 snprintf(hex_buffer + 1 + i*2, 3, "%2.2x", line[i]);
 645         hex_buffer[bin_size*2 + 1] = 0;
 646
 647         int retval = gdb_put_packet(connection, hex_buffer, bin_size*2 + 1);
 648
 649         free(hex_buffer);
 650         return retval;
 651 }
 652
 653 int gdb_output(struct command_context_s *context, const char* line)
 654 {
 655         /* this will be dumped to the log and also sent as an O packet if possible */
 656         LOG_USER_N("%s", line);
 657         return ERROR_OK;
 658 }
 659
 660
 661 static void gdb_frontend_halted(struct target_s *target, struct connection *connection)
 662 {
 663         struct gdb_connection *gdb_connection = connection->priv;
 664
 665         /* In the GDB protocol when we are stepping or continuing execution,
 666          * we have a lingering reply. Upon receiving a halted event
 667          * when we have that lingering packet, we reply to the original
 668          * step or continue packet.
 669          *
 670          * Executing monitor commands can bring the target in and
 671          * out of the running state so we'll see lots of TARGET_EVENT_XXX
 672          * that are to be ignored.
 673          */
 674         if (gdb_connection->frontend_state == TARGET_RUNNING)
 675         {
 676                 char sig_reply[4];
 677                 int signal;
 678
 679                 /* stop forwarding log packets! */
 680                 log_remove_callback(gdb_log_callback, connection);
 681
 682                 if (gdb_connection->ctrl_c)
 683                 {
 684                         signal = 0x2;
 685                         gdb_connection->ctrl_c = 0;
 686                 }
 687                 else
 688                 {
 689                         signal = gdb_last_signal(target);
 690                 }
 691
 692                 snprintf(sig_reply, 4, "T%2.2x", signal);
 693                 gdb_put_packet(connection, sig_reply, 3);
 694                 gdb_connection->frontend_state = TARGET_HALTED;
 695         }
 696 }
 697
 698 int gdb_target_callback_event_handler(struct target_s *target, enum target_event event, void *priv)
 699 {
 700         int retval;
 701         struct connection *connection = priv;
 702
 703         target_handle_event(target, event);
 704         switch (event)
 705         {
 706                 case TARGET_EVENT_GDB_HALT:
 707                         gdb_frontend_halted(target, connection);
 708                         break;
 709                 case TARGET_EVENT_HALTED:
 710                         target_call_event_callbacks(target, TARGET_EVENT_GDB_END);
 711                         break;
 712                 case TARGET_EVENT_GDB_FLASH_ERASE_START:
 713                         target_handle_event(target, TARGET_EVENT_OLD_gdb_program_config);
 714                         if ((retval = jtag_execute_queue()) != ERROR_OK)
 715                         {
 716                                 return retval;
 717                         }
 718                         break;
 719                 default:
 720                         break;
 721         }
 722
 723         return ERROR_OK;
 724 }
 725
 726 int gdb_new_connection(struct connection *connection)
 727 {
 728         struct gdb_connection *gdb_connection = malloc(sizeof(struct gdb_connection));
 729         struct gdb_service *gdb_service = connection->service->priv;
 730         int retval;
 731         int initial_ack;
 732
 733         connection->priv = gdb_connection;
 734
 735         /* initialize gdb connection information */
 736         gdb_connection->buf_p = gdb_connection->buffer;
 737         gdb_connection->buf_cnt = 0;
 738         gdb_connection->ctrl_c = 0;
 739         gdb_connection->frontend_state = TARGET_HALTED;
 740         gdb_connection->vflash_image = NULL;
 741         gdb_connection->closed = 0;
 742         gdb_connection->busy = 0;
 743         gdb_connection->noack_mode = 0;
 744         gdb_connection->sync = true;
 745
 746         /* send ACK to GDB for debug request */
 747         gdb_write(connection, "+", 1);
 748
 749         /* output goes through gdb connection */
 750         command_set_output_handler(connection->cmd_ctx, gdb_output, connection);
 751
 752         /* we must remove all breakpoints registered to the target as a previous
 753          * GDB session could leave dangling breakpoints if e.g. communication
 754          * timed out.
 755          */
 756         breakpoint_clear_target(gdb_service->target);
 757         watchpoint_clear_target(gdb_service->target);
 758
 759         /* register callback to be informed about target events */
 760         target_register_event_callback(gdb_target_callback_event_handler, connection);
 761
 762         /* remove the initial ACK from the incoming buffer */
 763         if ((retval = gdb_get_char(connection, &initial_ack)) != ERROR_OK)
 764                 return retval;
 765
 766         /* FIX!!!??? would we actually ever receive a + here???
 767          * Not observed.
 768          */
 769         if (initial_ack != '+')
 770                 gdb_putback_char(connection, initial_ack);
 771         target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_ATTACH);
 772
 773         gdb_actual_connections++;
 774         LOG_DEBUG("New GDB Connection: %d, Target %s, state: %s",
 775                   gdb_actual_connections,
 776                   gdb_service->target->cmd_name,
 777                   target_state_name(gdb_service->target));
 778
 779         return ERROR_OK;
 780 }
 781
 782 int gdb_connection_closed(struct connection *connection)
 783 {
 784         struct gdb_service *gdb_service = connection->service->priv;
 785         struct gdb_connection *gdb_connection = connection->priv;
 786
 787         /* we're done forwarding messages. Tear down callback before
 788          * cleaning up connection.
 789          */
 790         log_remove_callback(gdb_log_callback, connection);
 791
 792         gdb_actual_connections--;
 793         LOG_DEBUG("GDB Close, Target: %s, state: %s, gdb_actual_connections=%d",
 794                   gdb_service->target->cmd_name,
 795                   target_state_name(gdb_service->target),
 796                   gdb_actual_connections);
 797
 798         /* see if an image built with vFlash commands is left */
 799         if (gdb_connection->vflash_image)
 800         {
 801                 image_close(gdb_connection->vflash_image);
 802                 free(gdb_connection->vflash_image);
 803                 gdb_connection->vflash_image = NULL;
 804         }
 805
 806         /* if this connection registered a debug-message receiver delete it */
 807         delete_debug_msg_receiver(connection->cmd_ctx, gdb_service->target);
 808
 809         if (connection->priv)
 810         {
 811                 free(connection->priv);
 812                 connection->priv = NULL;
 813         }
 814         else
 815         {
 816                 LOG_ERROR("BUG: connection->priv == NULL");
 817         }
 818
 819
 820         target_unregister_event_callback(gdb_target_callback_event_handler, connection);
 821
 822         target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_END);
 823
 824         target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_DETACH);
 825
 826         return ERROR_OK;
 827 }
 828
 829 void gdb_send_error(struct connection *connection, uint8_t the_error)
 830 {
 831         char err[4];
 832         snprintf(err, 4, "E%2.2X", the_error);
 833         gdb_put_packet(connection, err, 3);
 834 }
 835
 836 int gdb_last_signal_packet(struct connection *connection, target_t *target, char* packet, int packet_size)
 837 {
 838         char sig_reply[4];
 839         int signal;
 840
 841         signal = gdb_last_signal(target);
 842
 843         snprintf(sig_reply, 4, "S%2.2x", signal);
 844         gdb_put_packet(connection, sig_reply, 3);
 845
 846         return ERROR_OK;
 847 }
 848
 849 static int gdb_reg_pos(target_t *target, int pos, int len)
 850 {
 851         if (target->endianness == TARGET_LITTLE_ENDIAN)
 852                 return pos;
 853         else
 854                 return len - 1 - pos;
 855 }
 856
 857 /* Convert register to string of bytes. NB! The # of bits in the
 858  * register might be non-divisible by 8(a byte), in which
 859  * case an entire byte is shown.
 860  *
 861  * NB! the format on the wire is the target endianess
 862  *
 863  * The format of reg->value is little endian
 864  *
 865  */
 866 void gdb_str_to_target(target_t *target, char *tstr, reg_t *reg)
 867 {
 868         int i;
 869
 870         uint8_t *buf;
 871         int buf_len;
 872         buf = reg->value;
 873         buf_len = CEIL(reg->size, 8);
 874
 875         for (i = 0; i < buf_len; i++)
 876         {
 877                 int j = gdb_reg_pos(target, i, buf_len);
 878                 tstr[i*2]   = DIGITS[(buf[j]>>4) & 0xf];
 879                 tstr[i*2 + 1] = DIGITS[buf[j]&0xf];
 880         }
 881 }
 882
 883 static int hextoint(char c)
 884 {
 885         if (c>='0'&&c<='9')
 886         {
 887                 return c-'0';
 888         }
 889         c = toupper(c);
 890         if (c>='A'&&c<='F')
 891         {
 892                 return c-'A'+10;
 893         }
 894         LOG_ERROR("BUG: invalid register value %08x", c);
 895         return 0;
 896 }
 897
 898 /* copy over in register buffer */
 899 void gdb_target_to_reg(target_t *target, char *tstr, int str_len, uint8_t *bin)
 900 {
 901         if (str_len % 2)
 902         {
 903                 LOG_ERROR("BUG: gdb value with uneven number of characters encountered");
 904                 exit(-1);
 905         }
 906
 907         int i;
 908         for (i = 0; i < str_len; i += 2)
 909         {
 910                 uint8_t t = hextoint(tstr[i]) << 4;
 911                 t |= hextoint(tstr[i + 1]);
 912
 913                 int j = gdb_reg_pos(target, i/2, str_len/2);
 914                 bin[j] = t;
 915         }
 916 }
 917
 918 int gdb_get_registers_packet(struct connection *connection, target_t *target, char* packet, int packet_size)
 919 {
 920         reg_t **reg_list;
 921         int reg_list_size;
 922         int retval;
 923         int reg_packet_size = 0;
 924         char *reg_packet;
 925         char *reg_packet_p;
 926         int i;
 927
 928 #ifdef _DEBUG_GDB_IO_
 929         LOG_DEBUG("-");
 930 #endif
 931
 932         if ((retval = target_get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
 933         {
 934                 return gdb_error(connection, retval);
 935         }
 936
 937         for (i = 0; i < reg_list_size; i++)
 938         {
 939                 reg_packet_size += reg_list[i]->size;
 940         }
 941
 942         reg_packet = malloc(CEIL(reg_packet_size, 8) * 2);
 943         reg_packet_p = reg_packet;
 944
 945         for (i = 0; i < reg_list_size; i++)
 946         {
 947                 gdb_str_to_target(target, reg_packet_p, reg_list[i]);
 948                 reg_packet_p += CEIL(reg_list[i]->size, 8) * 2;
 949         }
 950
 951 #ifdef _DEBUG_GDB_IO_
 952         {
 953                 char *reg_packet_p;
 954                 reg_packet_p = strndup(reg_packet, CEIL(reg_packet_size, 8) * 2);
 955                 LOG_DEBUG("reg_packet: %s", reg_packet_p);
 956                 free(reg_packet_p);
 957         }
 958 #endif
 959
 960         gdb_put_packet(connection, reg_packet, CEIL(reg_packet_size, 8) * 2);
 961         free(reg_packet);
 962
 963         free(reg_list);
 964
 965         return ERROR_OK;
 966 }
 967
 968 int gdb_set_registers_packet(struct connection *connection, target_t *target, char *packet, int packet_size)
 969 {
 970         int i;
 971         reg_t **reg_list;
 972         int reg_list_size;
 973         int retval;
 974         char *packet_p;
 975
 976 #ifdef _DEBUG_GDB_IO_
 977         LOG_DEBUG("-");
 978 #endif
 979
 980         /* skip command character */
 981         packet++;
 982         packet_size--;
 983
 984         if (packet_size % 2)
 985         {
 986                 LOG_WARNING("GDB set_registers packet with uneven characters received, dropping connection");
 987                 return ERROR_SERVER_REMOTE_CLOSED;
 988         }
 989
 990         if ((retval = target_get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
 991         {
 992                 return gdb_error(connection, retval);
 993         }
 994
 995         packet_p = packet;
 996         for (i = 0; i < reg_list_size; i++)
 997         {
 998                 uint8_t *bin_buf;
 999                 int chars = (CEIL(reg_list[i]->size, 8) * 2);
1000
1001                 if (packet_p + chars > packet + packet_size)
1002                 {
1003                         LOG_ERROR("BUG: register packet is too small for registers");
1004                 }
1005
1006                 struct reg_arch_type *arch_type;
1007                 bin_buf = malloc(CEIL(reg_list[i]->size, 8));
1008                 gdb_target_to_reg(target, packet_p, chars, bin_buf);
1009
1010                 /* get register arch_type, and call set method */
1011                 arch_type = register_get_arch_type(reg_list[i]->arch_type);
1012
1013                 arch_type->set(reg_list[i], bin_buf);
1014
1015                 /* advance packet pointer */
1016                 packet_p += chars;
1017
1018
1019                 free(bin_buf);
1020         }
1021
1022         /* free reg_t *reg_list[] array allocated by get_gdb_reg_list */
1023         free(reg_list);
1024
1025         gdb_put_packet(connection, "OK", 2);
1026
1027         return ERROR_OK;
1028 }
1029
1030 int gdb_get_register_packet(struct connection *connection, target_t *target, char *packet, int packet_size)
1031 {
1032         char *reg_packet;
1033         int reg_num = strtoul(packet + 1, NULL, 16);
1034         reg_t **reg_list;
1035         int reg_list_size;
1036         int retval;
1037
1038 #ifdef _DEBUG_GDB_IO_
1039         LOG_DEBUG("-");
1040 #endif
1041
1042         if ((retval = target_get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
1043         {
1044                 return gdb_error(connection, retval);
1045         }
1046
1047         if (reg_list_size <= reg_num)
1048         {
1049                 LOG_ERROR("gdb requested a non-existing register");
1050                 exit(-1);
1051         }
1052
1053         reg_packet = malloc(CEIL(reg_list[reg_num]->size, 8) * 2);
1054
1055         gdb_str_to_target(target, reg_packet, reg_list[reg_num]);
1056
1057         gdb_put_packet(connection, reg_packet, CEIL(reg_list[reg_num]->size, 8) * 2);
1058
1059         free(reg_list);
1060         free(reg_packet);
1061
1062         return ERROR_OK;
1063 }
1064
1065 int gdb_set_register_packet(struct connection *connection, target_t *target, char *packet, int packet_size)
1066 {
1067         char *separator;
1068         uint8_t *bin_buf;
1069         int reg_num = strtoul(packet + 1, &separator, 16);
1070         reg_t **reg_list;
1071         int reg_list_size;
1072         int retval;
1073         struct reg_arch_type *arch_type;
1074
1075         LOG_DEBUG("-");
1076
1077         if ((retval = target_get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
1078         {
1079                 return gdb_error(connection, retval);
1080         }
1081
1082         if (reg_list_size < reg_num)
1083         {
1084                 LOG_ERROR("gdb requested a non-existing register");
1085                 return ERROR_SERVER_REMOTE_CLOSED;
1086         }
1087
1088         if (*separator != '=')
1089         {
1090                 LOG_ERROR("GDB 'set register packet', but no '=' following the register number");
1091                 return ERROR_SERVER_REMOTE_CLOSED;
1092         }
1093
1094         /* convert from GDB-string (target-endian) to hex-string (big-endian) */
1095         bin_buf = malloc(CEIL(reg_list[reg_num]->size, 8));
1096         int chars = (CEIL(reg_list[reg_num]->size, 8) * 2);
1097
1098         /* fix!!! add some sanity checks on packet size here */
1099
1100         gdb_target_to_reg(target, separator + 1, chars, bin_buf);
1101
1102                 /* get register arch_type, and call set method */
1103         arch_type = register_get_arch_type(reg_list[reg_num]->arch_type);
1104         arch_type->set(reg_list[reg_num], bin_buf);
1105
1106         gdb_put_packet(connection, "OK", 2);
1107
1108         free(bin_buf);
1109         free(reg_list);
1110
1111         return ERROR_OK;
1112 }
1113
1114 int gdb_error(struct connection *connection, int retval)
1115 {
1116         switch (retval)
1117         {
1118                 case ERROR_TARGET_DATA_ABORT:
1119                         gdb_send_error(connection, EIO);
1120                         break;
1121                 case ERROR_TARGET_TRANSLATION_FAULT:
1122                         gdb_send_error(connection, EFAULT);
1123                         break;
1124                 case ERROR_TARGET_UNALIGNED_ACCESS:
1125                         gdb_send_error(connection, EFAULT);
1126                         break;
1127                 case ERROR_TARGET_NOT_HALTED:
1128                         gdb_send_error(connection, EFAULT);
1129                         break;
1130                 default:
1131                         /* This could be that the target reset itself. */
1132                         LOG_ERROR("unexpected error %i", retval);
1133                         gdb_send_error(connection, EFAULT);
1134                         break;
1135         }
1136
1137         return ERROR_OK;
1138 }
1139
1140 /* We don't have to worry about the default 2 second timeout for GDB packets,
1141  * because GDB breaks up large memory reads into smaller reads.
1142  *
1143  * 8191 bytes by the looks of it. Why 8191 bytes instead of 8192?????
1144  */
1145 int gdb_read_memory_packet(struct connection *connection, target_t *target, char *packet, int packet_size)
1146 {
1147         char *separator;
1148         uint32_t addr = 0;
1149         uint32_t len = 0;
1150
1151         uint8_t *buffer;
1152         char *hex_buffer;
1153
1154         int retval = ERROR_OK;
1155
1156         /* skip command character */
1157         packet++;
1158
1159         addr = strtoul(packet, &separator, 16);
1160
1161         if (*separator != ',')
1162         {
1163                 LOG_ERROR("incomplete read memory packet received, dropping connection");
1164                 return ERROR_SERVER_REMOTE_CLOSED;
1165         }
1166
1167         len = strtoul(separator + 1, NULL, 16);
1168
1169         buffer = malloc(len);
1170
1171         LOG_DEBUG("addr: 0x%8.8" PRIx32 ", len: 0x%8.8" PRIx32 "", addr, len);
1172
1173         retval = target_read_buffer(target, addr, len, buffer);
1174
1175         if ((retval != ERROR_OK)&&!gdb_report_data_abort)
1176         {
1177                 /* TODO : Here we have to lie and send back all zero's lest stack traces won't work.
1178                  * At some point this might be fixed in GDB, in which case this code can be removed.
1179                  *
1180                  * OpenOCD developers are acutely aware of this problem, but there is nothing
1181                  * gained by involving the user in this problem that hopefully will get resolved
1182                  * eventually
1183                  *
1184                  * http://sourceware.org/cgi-bin/gnatsweb.pl?cmd = view%20audit-trail&database = gdb&pr = 2395
1185                  *
1186                  * For now, the default is to fix up things to make current GDB versions work.
1187                  * This can be overwritten using the gdb_report_data_abort <'enable'|'disable'> command.
1188                  */
1189                 memset(buffer, 0, len);
1190                 retval = ERROR_OK;
1191         }
1192
1193         if (retval == ERROR_OK)
1194         {
1195                 hex_buffer = malloc(len * 2 + 1);
1196
1197                 uint32_t i;
1198                 for (i = 0; i < len; i++)
1199                 {
1200                         uint8_t t = buffer[i];
1201                         hex_buffer[2 * i] = DIGITS[(t >> 4) & 0xf];
1202                         hex_buffer[2 * i + 1] = DIGITS[t & 0xf];
1203                 }
1204
1205                 gdb_put_packet(connection, hex_buffer, len * 2);
1206
1207                 free(hex_buffer);
1208         }
1209         else
1210         {
1211                 retval = gdb_error(connection, retval);
1212         }
1213
1214         free(buffer);
1215
1216         return retval;
1217 }
1218
1219 int gdb_write_memory_packet(struct connection *connection, target_t *target, char *packet, int packet_size)
1220 {
1221         char *separator;
1222         uint32_t addr = 0;
1223         uint32_t len = 0;
1224
1225         uint8_t *buffer;
1226
1227         uint32_t i;
1228         int retval;
1229
1230         /* skip command character */
1231         packet++;
1232
1233         addr = strtoul(packet, &separator, 16);
1234
1235         if (*separator != ',')
1236         {
1237                 LOG_ERROR("incomplete write memory packet received, dropping connection");
1238                 return ERROR_SERVER_REMOTE_CLOSED;
1239         }
1240
1241         len = strtoul(separator + 1, &separator, 16);
1242
1243         if (*(separator++) != ':')
1244         {
1245                 LOG_ERROR("incomplete write memory packet received, dropping connection");
1246                 return ERROR_SERVER_REMOTE_CLOSED;
1247         }
1248
1249         buffer = malloc(len);
1250
1251         LOG_DEBUG("addr: 0x%8.8" PRIx32 ", len: 0x%8.8" PRIx32 "", addr, len);
1252
1253         for (i = 0; i < len; i++)
1254         {
1255                 uint32_t tmp;
1256                 sscanf(separator + 2*i, "%2" SCNx32 , &tmp);
1257                 buffer[i] = tmp;
1258         }
1259
1260         retval = target_write_buffer(target, addr, len, buffer);
1261
1262         if (retval == ERROR_OK)
1263         {
1264                 gdb_put_packet(connection, "OK", 2);
1265         }
1266         else
1267         {
1268                 retval = gdb_error(connection, retval);
1269         }
1270
1271         free(buffer);
1272
1273         return retval;
1274 }
1275
1276 int gdb_write_memory_binary_packet(struct connection *connection, target_t *target, char *packet, int packet_size)
1277 {
1278         char *separator;
1279         uint32_t addr = 0;
1280         uint32_t len = 0;
1281
1282         int retval;
1283
1284         /* skip command character */
1285         packet++;
1286
1287         addr = strtoul(packet, &separator, 16);
1288
1289         if (*separator != ',')
1290         {
1291                 LOG_ERROR("incomplete write memory binary packet received, dropping connection");
1292                 return ERROR_SERVER_REMOTE_CLOSED;
1293         }
1294
1295         len = strtoul(separator + 1, &separator, 16);
1296
1297         if (*(separator++) != ':')
1298         {
1299                 LOG_ERROR("incomplete write memory binary packet received, dropping connection");
1300                 return ERROR_SERVER_REMOTE_CLOSED;
1301         }
1302
1303         retval = ERROR_OK;
1304         if (len)
1305         {
1306                 LOG_DEBUG("addr: 0x%8.8" PRIx32 ", len: 0x%8.8" PRIx32 "", addr, len);
1307
1308                 retval = target_write_buffer(target, addr, len, (uint8_t*)separator);
1309         }
1310
1311         if (retval == ERROR_OK)
1312         {
1313                 gdb_put_packet(connection, "OK", 2);
1314         }
1315         else
1316         {
1317                 if ((retval = gdb_error(connection, retval)) != ERROR_OK)
1318                         return retval;
1319         }
1320
1321         return ERROR_OK;
1322 }
1323
1324 int gdb_step_continue_packet(struct connection *connection, target_t *target, char *packet, int packet_size)
1325 {
1326         int current = 0;
1327         uint32_t address = 0x0;
1328         int retval = ERROR_OK;
1329
1330         LOG_DEBUG("-");
1331
1332         if (packet_size > 1)
1333         {
1334                 packet[packet_size] = 0;
1335                 address = strtoul(packet + 1, NULL, 16);
1336         }
1337         else
1338         {
1339                 current = 1;
1340         }
1341
1342         if (packet[0] == 'c')
1343         {
1344                 LOG_DEBUG("continue");
1345                 target_handle_event(target, TARGET_EVENT_OLD_pre_resume);
1346                 retval = target_resume(target, current, address, 0, 0); /* resume at current address, don't handle breakpoints, not debugging */
1347         }
1348         else if (packet[0] == 's')
1349         {
1350                 LOG_DEBUG("step");
1351                 /* step at current or address, don't handle breakpoints */
1352                 retval = target_step(target, current, address, 0);
1353         }
1354         return retval;
1355 }
1356
1357 int gdb_breakpoint_watchpoint_packet(struct connection *connection, target_t *target, char *packet, int packet_size)
1358 {
1359         int type;
1360         enum breakpoint_type bp_type = BKPT_SOFT /* dummy init to avoid warning */;
1361         enum watchpoint_rw wp_type;
1362         uint32_t address;
1363         uint32_t size;
1364         char *separator;
1365         int retval;
1366
1367         LOG_DEBUG("-");
1368
1369         type = strtoul(packet + 1, &separator, 16);
1370
1371         if (type == 0)  /* memory breakpoint */
1372                 bp_type = BKPT_SOFT;
1373         else if (type == 1) /* hardware breakpoint */
1374                 bp_type = BKPT_HARD;
1375         else if (type == 2) /* write watchpoint */
1376                 wp_type = WPT_WRITE;
1377         else if (type == 3) /* read watchpoint */
1378                 wp_type = WPT_READ;
1379         else if (type == 4) /* access watchpoint */
1380                 wp_type = WPT_ACCESS;
1381
1382         if (gdb_breakpoint_override && ((bp_type == BKPT_SOFT)||(bp_type == BKPT_HARD)))
1383         {
1384                 bp_type = gdb_breakpoint_override_type;
1385         }
1386
1387         if (*separator != ',')
1388         {
1389                 LOG_ERROR("incomplete breakpoint/watchpoint packet received, dropping connection");
1390                 return ERROR_SERVER_REMOTE_CLOSED;
1391         }
1392
1393         address = strtoul(separator + 1, &separator, 16);
1394
1395         if (*separator != ',')
1396         {
1397                 LOG_ERROR("incomplete breakpoint/watchpoint packet received, dropping connection");
1398                 return ERROR_SERVER_REMOTE_CLOSED;
1399         }
1400
1401         size = strtoul(separator + 1, &separator, 16);
1402
1403         switch (type)
1404         {
1405                 case 0:
1406                 case 1:
1407                         if (packet[0] == 'Z')
1408                         {
1409                                 if ((retval = breakpoint_add(target, address, size, bp_type)) != ERROR_OK)
1410                                 {
1411                                         if ((retval = gdb_error(connection, retval)) != ERROR_OK)
1412                                                 return retval;
1413                                 }
1414                                 else
1415                                 {
1416                                         gdb_put_packet(connection, "OK", 2);
1417                                 }
1418                         }
1419                         else
1420                         {
1421                                 breakpoint_remove(target, address);
1422                                 gdb_put_packet(connection, "OK", 2);
1423                         }
1424                         break;
1425                 case 2:
1426                 case 3:
1427                 case 4:
1428                 {
1429                         if (packet[0] == 'Z')
1430                         {
1431                                 if ((retval = watchpoint_add(target, address, size, type-2, 0, 0xffffffffu)) != ERROR_OK)
1432                                 {
1433                                         if ((retval = gdb_error(connection, retval)) != ERROR_OK)
1434                                                 return retval;
1435                                 }
1436                                 else
1437                                 {
1438                                         gdb_put_packet(connection, "OK", 2);
1439                                 }
1440                         }
1441                         else
1442                         {
1443                                 watchpoint_remove(target, address);
1444                                 gdb_put_packet(connection, "OK", 2);
1445                         }
1446                         break;
1447                 }
1448                 default:
1449                         break;
1450         }
1451
1452         return ERROR_OK;
1453 }
1454
1455 /* print out a string and allocate more space as needed, mainly used for XML at this point */
1456 void xml_printf(int *retval, char **xml, int *pos, int *size, const char *fmt, ...)
1457 {
1458         if (*retval != ERROR_OK)
1459         {
1460                 return;
1461         }
1462         int first = 1;
1463
1464         for (;;)
1465         {
1466                 if ((*xml == NULL) || (!first))
1467                 {
1468                         /* start by 0 to exercise all the code paths.
1469                          * Need minimum 2 bytes to fit 1 char and 0 terminator. */
1470
1471                         *size = *size * 2 + 2;
1472                         char *t = *xml;
1473                         *xml = realloc(*xml, *size);
1474                         if (*xml == NULL)
1475                         {
1476                                 if (t)
1477                                         free(t);
1478                                 *retval = ERROR_SERVER_REMOTE_CLOSED;
1479                                 return;
1480                         }
1481                 }
1482
1483                 va_list ap;
1484                 int ret;
1485                 va_start(ap, fmt);
1486                 ret = vsnprintf(*xml + *pos, *size - *pos, fmt, ap);
1487                 va_end(ap);
1488                 if ((ret > 0) && ((ret + 1) < *size - *pos))
1489                 {
1490                         *pos += ret;
1491                         return;
1492                 }
1493                 /* there was just enough or not enough space, allocate more. */
1494                 first = 0;
1495         }
1496 }
1497
1498 static int decode_xfer_read(char *buf, char **annex, int *ofs, unsigned int *len)
1499 {
1500         char *separator;
1501
1502         /* Extract and NUL-terminate the annex. */
1503         *annex = buf;
1504         while (*buf && *buf != ':')
1505                 buf++;
1506         if (*buf == '\0')
1507                 return -1;
1508         *buf++ = 0;
1509
1510         /* After the read marker and annex, qXfer looks like a
1511          * traditional 'm' packet. */
1512
1513         *ofs = strtoul(buf, &separator, 16);
1514
1515         if (*separator != ',')
1516                 return -1;
1517
1518         *len = strtoul(separator + 1, NULL, 16);
1519
1520         return 0;
1521 }
1522
1523 int gdb_calc_blocksize(flash_bank_t *bank)
1524 {
1525         uint32_t i;
1526         uint32_t block_size = 0xffffffff;
1527
1528         /* loop through all sectors and return smallest sector size */
1529
1530         for (i = 0; i < (uint32_t)bank->num_sectors; i++)
1531         {
1532                 if (bank->sectors[i].size < block_size)
1533                         block_size = bank->sectors[i].size;
1534         }
1535
1536         return block_size;
1537 }
1538
1539 static int compare_bank (const void * a, const void * b)
1540 {
1541         flash_bank_t *b1, *b2;
1542         b1=*((flash_bank_t **)a);
1543         b2=*((flash_bank_t **)b);
1544
1545         if (b1->base == b2->base)
1546         {
1547                 return 0;
1548         } else if (b1->base > b2->base)
1549         {
1550                 return 1;
1551         } else
1552         {
1553                 return -1;
1554         }
1555 }
1556
1557 int gdb_query_packet(struct connection *connection, target_t *target, char *packet, int packet_size)
1558 {
1559         command_context_t *cmd_ctx = connection->cmd_ctx;
1560         struct gdb_connection *gdb_connection = connection->priv;
1561
1562         if (strstr(packet, "qRcmd,"))
1563         {
1564                 if (packet_size > 6)
1565                 {
1566                         char *cmd;
1567                         int i;
1568                         cmd = malloc((packet_size - 6)/2 + 1);
1569                         for (i = 0; i < (packet_size - 6)/2; i++)
1570                         {
1571                                 uint32_t tmp;
1572                                 sscanf(packet + 6 + 2*i, "%2" SCNx32 , &tmp);
1573                                 cmd[i] = tmp;
1574                         }
1575                         cmd[(packet_size - 6)/2] = 0x0;
1576
1577                         /* We want to print all debug output to GDB connection */
1578                         log_add_callback(gdb_log_callback, connection);
1579                         target_call_timer_callbacks_now();
1580                         /* some commands need to know the GDB connection, make note of current
1581                          * GDB connection. */
1582                         current_gdb_connection = gdb_connection;
1583                         command_run_line(cmd_ctx, cmd);
1584                         current_gdb_connection = NULL;
1585                         target_call_timer_callbacks_now();
1586                         log_remove_callback(gdb_log_callback, connection);
1587                         free(cmd);
1588                 }
1589                 gdb_put_packet(connection, "OK", 2);
1590                 return ERROR_OK;
1591         }
1592         else if (strstr(packet, "qCRC:"))
1593         {
1594                 if (packet_size > 5)
1595                 {
1596                         int retval;
1597                         char gdb_reply[10];
1598                         char *separator;
1599                         uint32_t checksum;
1600                         uint32_t addr = 0;
1601                         uint32_t len = 0;
1602
1603                         /* skip command character */
1604                         packet += 5;
1605
1606                         addr = strtoul(packet, &separator, 16);
1607
1608                         if (*separator != ',')
1609                         {
1610                                 LOG_ERROR("incomplete read memory packet received, dropping connection");
1611                                 return ERROR_SERVER_REMOTE_CLOSED;
1612                         }
1613
1614                         len = strtoul(separator + 1, NULL, 16);
1615
1616                         retval = target_checksum_memory(target, addr, len, &checksum);
1617
1618                         if (retval == ERROR_OK)
1619                         {
1620                                 snprintf(gdb_reply, 10, "C%8.8" PRIx32 "", checksum);
1621                                 gdb_put_packet(connection, gdb_reply, 9);
1622                         }
1623                         else
1624                         {
1625                                 if ((retval = gdb_error(connection, retval)) != ERROR_OK)
1626                                         return retval;
1627                         }
1628
1629                         return ERROR_OK;
1630                 }
1631         }
1632         else if (strstr(packet, "qSupported"))
1633         {
1634                 /* we currently support packet size and qXfer:memory-map:read (if enabled)
1635                  * disable qXfer:features:read for the moment */
1636                 int retval = ERROR_OK;
1637                 char *buffer = NULL;
1638                 int pos = 0;
1639                 int size = 0;
1640
1641                 xml_printf(&retval, &buffer, &pos, &size,
1642                                 "PacketSize=%x;qXfer:memory-map:read%c;qXfer:features:read-;QStartNoAckMode+",
1643                                 (GDB_BUFFER_SIZE - 1), ((gdb_use_memory_map == 1) && (flash_get_bank_count() > 0)) ? '+' : '-');
1644
1645                 if (retval != ERROR_OK)
1646                 {
1647                         gdb_send_error(connection, 01);
1648                         return ERROR_OK;
1649                 }
1650
1651                 gdb_put_packet(connection, buffer, strlen(buffer));
1652                 free(buffer);
1653
1654                 return ERROR_OK;
1655         }
1656         else if (strstr(packet, "qXfer:memory-map:read::") && (flash_get_bank_count() > 0))
1657         {
1658                 /* We get away with only specifying flash here. Regions that are not
1659                  * specified are treated as if we provided no memory map(if not we
1660                  * could detect the holes and mark them as RAM).
1661                  * Normally we only execute this code once, but no big deal if we
1662                  * have to regenerate it a couple of times. */
1663
1664                 flash_bank_t *p;
1665                 char *xml = NULL;
1666                 int size = 0;
1667                 int pos = 0;
1668                 int retval = ERROR_OK;
1669
1670                 int offset;
1671                 int length;
1672                 char *separator;
1673                 int blocksize;
1674
1675                 /* skip command character */
1676                 packet += 23;
1677
1678                 offset = strtoul(packet, &separator, 16);
1679                 length = strtoul(separator + 1, &separator, 16);
1680
1681                 xml_printf(&retval, &xml, &pos, &size, "<memory-map>\n");
1682
1683                 /*
1684                 sort banks in ascending order, we need to make non-flash memory be ram(or rather
1685                 read/write) by default for GDB.
1686                 GDB does not have a concept of non-cacheable read/write memory.
1687                  */
1688                 flash_bank_t **banks = malloc(sizeof(flash_bank_t *)*flash_get_bank_count());
1689                 int i;
1690
1691                 for (i = 0; i < flash_get_bank_count(); i++)
1692                 {
1693                         p = get_flash_bank_by_num(i);
1694                         if (p == NULL)
1695                         {
1696                                 free(banks);
1697                                 retval = ERROR_FAIL;
1698                                 gdb_send_error(connection, retval);
1699                                 return retval;
1700                         }
1701                         banks[i]=p;
1702                 }
1703
1704                 qsort(banks, flash_get_bank_count(), sizeof(flash_bank_t *), compare_bank);
1705
1706                 uint32_t ram_start = 0;
1707                 for (i = 0; i < flash_get_bank_count(); i++)
1708                 {
1709                         p = banks[i];
1710
1711                         if (ram_start < p->base)
1712                         {
1713                                 xml_printf(&retval, &xml, &pos, &size, "<memory type=\"ram\" start=\"0x%x\" length=\"0x%x\"/>\n",
1714                                         ram_start, p->base-ram_start);
1715                         }
1716
1717                         /* if device has uneven sector sizes, eg. str7, lpc
1718                          * we pass the smallest sector size to gdb memory map */
1719                         blocksize = gdb_calc_blocksize(p);
1720
1721                         xml_printf(&retval, &xml, &pos, &size, "<memory type=\"flash\" start=\"0x%x\" length=\"0x%x\">\n" \
1722                                 "<property name=\"blocksize\">0x%x</property>\n" \
1723                                 "</memory>\n", \
1724                                 p->base, p->size, blocksize);
1725                         ram_start = p->base + p->size;
1726                 }
1727                 if (ram_start != 0)
1728                 {
1729                         xml_printf(&retval, &xml, &pos, &size, "<memory type=\"ram\" start=\"0x%x\" length=\"0x%x\"/>\n",
1730                                 ram_start, 0-ram_start);
1731                 } else
1732                 {
1733                         /* a flash chip could be at the very end of the 32 bit address space, in which case
1734                         ram_start will be precisely 0 */
1735                 }
1736
1737                 free(banks);
1738                 banks = NULL;
1739
1740                 xml_printf(&retval, &xml, &pos, &size, "</memory-map>\n");
1741
1742                 if (retval != ERROR_OK)
1743                 {
1744                         gdb_send_error(connection, retval);
1745                         return retval;
1746                 }
1747
1748                 if (offset + length > pos)
1749                 {
1750                         length = pos - offset;
1751                 }
1752
1753                 char *t = malloc(length + 1);
1754                 t[0] = 'l';
1755                 memcpy(t + 1, xml + offset, length);
1756                 gdb_put_packet(connection, t, length + 1);
1757
1758                 free(t);
1759                 free(xml);
1760                 return ERROR_OK;
1761         }
1762         else if (strstr(packet, "qXfer:features:read:"))
1763         {
1764                 char *xml = NULL;
1765                 int size = 0;
1766                 int pos = 0;
1767                 int retval = ERROR_OK;
1768
1769                 int offset;
1770                 unsigned int length;
1771                 char *annex;
1772
1773                 /* skip command character */
1774                 packet += 20;
1775
1776                 if (decode_xfer_read(packet, &annex, &offset, &length) < 0)
1777                 {
1778                         gdb_send_error(connection, 01);
1779                         return ERROR_OK;
1780                 }
1781
1782                 if (strcmp(annex, "target.xml") != 0)
1783                 {
1784                         gdb_send_error(connection, 01);
1785                         return ERROR_OK;
1786                 }
1787
1788                 xml_printf(&retval, &xml, &pos, &size, \
1789                         "l < target version=\"1.0\">\n < architecture > arm</architecture>\n</target>\n");
1790
1791                 if (retval != ERROR_OK)
1792                 {
1793                         gdb_send_error(connection, retval);
1794                         return retval;
1795                 }
1796
1797                 gdb_put_packet(connection, xml, strlen(xml));
1798
1799                 free(xml);
1800                 return ERROR_OK;
1801         }
1802         else if (strstr(packet, "QStartNoAckMode"))
1803         {
1804                 gdb_connection->noack_mode = 1;
1805                 gdb_put_packet(connection, "OK", 2);
1806                 return ERROR_OK;
1807         }
1808
1809         gdb_put_packet(connection, "", 0);
1810         return ERROR_OK;
1811 }
1812
1813 int gdb_v_packet(struct connection *connection, target_t *target, char *packet, int packet_size)
1814 {
1815         struct gdb_connection *gdb_connection = connection->priv;
1816         struct gdb_service *gdb_service = connection->service->priv;
1817         int result;
1818
1819         /* if flash programming disabled - send a empty reply */
1820
1821         if (gdb_flash_program == 0)
1822         {
1823                 gdb_put_packet(connection, "", 0);
1824                 return ERROR_OK;
1825         }
1826
1827         if (strstr(packet, "vFlashErase:"))
1828         {
1829                 unsigned long addr;
1830                 unsigned long length;
1831
1832                 char *parse = packet + 12;
1833                 if (*parse == '\0')
1834                 {
1835                         LOG_ERROR("incomplete vFlashErase packet received, dropping connection");
1836                         return ERROR_SERVER_REMOTE_CLOSED;
1837                 }
1838
1839                 addr = strtoul(parse, &parse, 16);
1840
1841                 if (*(parse++) != ',' || *parse == '\0')
1842                 {
1843                         LOG_ERROR("incomplete vFlashErase packet received, dropping connection");
1844                         return ERROR_SERVER_REMOTE_CLOSED;
1845                 }
1846
1847                 length = strtoul(parse, &parse, 16);
1848
1849                 if (*parse != '\0')
1850                 {
1851                         LOG_ERROR("incomplete vFlashErase packet received, dropping connection");
1852                         return ERROR_SERVER_REMOTE_CLOSED;
1853                 }
1854
1855                 /* assume all sectors need erasing - stops any problems
1856                  * when flash_write is called multiple times */
1857                 flash_set_dirty();
1858
1859                 /* perform any target specific operations before the erase */
1860                 target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_FLASH_ERASE_START);
1861                 result = flash_erase_address_range(gdb_service->target, addr, length);
1862                 target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_FLASH_ERASE_END);
1863
1864                 /* perform erase */
1865                 if (result != ERROR_OK)
1866                 {
1867                         /* GDB doesn't evaluate the actual error number returned,
1868                          * treat a failed erase as an I/O error
1869                          */
1870                         gdb_send_error(connection, EIO);
1871                         LOG_ERROR("flash_erase returned %i", result);
1872                 }
1873                 else
1874                         gdb_put_packet(connection, "OK", 2);
1875
1876                 return ERROR_OK;
1877         }
1878
1879         if (strstr(packet, "vFlashWrite:"))
1880         {
1881                 int retval;
1882                 unsigned long addr;
1883                 unsigned long length;
1884                 char *parse = packet + 12;
1885
1886                 if (*parse == '\0')
1887                 {
1888                         LOG_ERROR("incomplete vFlashErase packet received, dropping connection");
1889                         return ERROR_SERVER_REMOTE_CLOSED;
1890                 }
1891                 addr = strtoul(parse, &parse, 16);
1892                 if (*(parse++) != ':')
1893                 {
1894                         LOG_ERROR("incomplete vFlashErase packet received, dropping connection");
1895                         return ERROR_SERVER_REMOTE_CLOSED;
1896                 }
1897                 length = packet_size - (parse - packet);
1898
1899                 /* create a new image if there isn't already one */
1900                 if (gdb_connection->vflash_image == NULL)
1901                 {
1902                         gdb_connection->vflash_image = malloc(sizeof(struct image));
1903                         image_open(gdb_connection->vflash_image, "", "build");
1904                 }
1905
1906                 /* create new section with content from packet buffer */
1907                 if ((retval = image_add_section(gdb_connection->vflash_image, addr, length, 0x0, (uint8_t*)parse)) != ERROR_OK)
1908                 {
1909                         return retval;
1910                 }
1911
1912                 gdb_put_packet(connection, "OK", 2);
1913
1914                 return ERROR_OK;
1915         }
1916
1917         if (!strcmp(packet, "vFlashDone"))
1918         {
1919                 uint32_t written;
1920
1921                 /* process the flashing buffer. No need to erase as GDB
1922                  * always issues a vFlashErase first. */
1923                 target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_FLASH_WRITE_START);
1924                 result = flash_write(gdb_service->target, gdb_connection->vflash_image, &written, 0);
1925                 target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_FLASH_WRITE_END);
1926                 if (result != ERROR_OK)
1927                 {
1928                         if (result == ERROR_FLASH_DST_OUT_OF_BANK)
1929                                 gdb_put_packet(connection, "E.memtype", 9);
1930                         else
1931                                 gdb_send_error(connection, EIO);
1932                         }
1933                 else
1934                 {
1935                         LOG_DEBUG("wrote %u bytes from vFlash image to flash", (unsigned)written);
1936                         gdb_put_packet(connection, "OK", 2);
1937                 }
1938
1939                 image_close(gdb_connection->vflash_image);
1940                 free(gdb_connection->vflash_image);
1941                 gdb_connection->vflash_image = NULL;
1942
1943                 return ERROR_OK;
1944         }
1945
1946         gdb_put_packet(connection, "", 0);
1947         return ERROR_OK;
1948 }
1949
1950 int gdb_detach(struct connection *connection, target_t *target)
1951 {
1952         struct gdb_service *gdb_service = connection->service->priv;
1953
1954         target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_DETACH);
1955
1956         return gdb_put_packet(connection, "OK", 2);
1957 }
1958
1959 static void gdb_log_callback(void *priv, const char *file, unsigned line,
1960                 const char *function, const char *string)
1961 {
1962         struct connection *connection = priv;
1963         struct gdb_connection *gdb_con = connection->priv;
1964
1965         if (gdb_con->busy)
1966         {
1967                 /* do not reply this using the O packet */
1968                 return;
1969         }
1970
1971         gdb_output_con(connection, string);
1972 }
1973
1974 /* Do not allocate this on the stack */
1975 char gdb_packet_buffer[GDB_BUFFER_SIZE];
1976
1977 static void gdb_sig_halted(struct connection *connection)
1978 {
1979         char sig_reply[4];
1980         snprintf(sig_reply, 4, "T%2.2x", 2);
1981         gdb_put_packet(connection, sig_reply, 3);
1982
1983 }
1984
1985 int gdb_input_inner(struct connection *connection)
1986 {
1987         struct gdb_service *gdb_service = connection->service->priv;
1988         target_t *target = gdb_service->target;
1989         char *packet = gdb_packet_buffer;
1990         int packet_size;
1991         int retval;
1992         struct gdb_connection *gdb_con = connection->priv;
1993         static int extended_protocol = 0;
1994
1995         /* drain input buffer */
1996         do
1997         {
1998                 packet_size = GDB_BUFFER_SIZE-1;
1999                 if ((retval = gdb_get_packet(connection, packet, &packet_size)) != ERROR_OK)
2000                 {
2001                         return retval;
2002                 }
2003
2004                 /* terminate with zero */
2005                 packet[packet_size] = 0;
2006
2007                 if (LOG_LEVEL_IS(LOG_LVL_DEBUG)) {
2008                         if (packet[0] == 'X') {
2009                                 // binary packets spew junk into the debug log stream
2010                                 char buf[ 50 ];
2011                                 int x;
2012                                 for (x = 0 ; (x < 49) && (packet[x] != ':') ; x++) {
2013                                         buf[x] = packet[x];
2014                                 }
2015                                 buf[x] = 0;
2016                                 LOG_DEBUG("received packet: '%s:<binary-data>'", buf);
2017                         } else {
2018                                 LOG_DEBUG("received packet: '%s'", packet);
2019                         }
2020                 }
2021
2022                 if (packet_size > 0)
2023                 {
2024                         retval = ERROR_OK;
2025                         switch (packet[0])
2026                         {
2027                                 case 'H':
2028                                         /* Hct... -- set thread
2029                                          * we don't have threads, send empty reply */
2030                                         gdb_put_packet(connection, NULL, 0);
2031                                         break;
2032                                 case 'q':
2033                                 case 'Q':
2034                                         retval = gdb_query_packet(connection, target, packet, packet_size);
2035                                         break;
2036                                 case 'g':
2037                                         retval = gdb_get_registers_packet(connection, target, packet, packet_size);
2038                                         break;
2039                                 case 'G':
2040                                         retval = gdb_set_registers_packet(connection, target, packet, packet_size);
2041                                         break;
2042                                 case 'p':
2043                                         retval = gdb_get_register_packet(connection, target, packet, packet_size);
2044                                         break;
2045                                 case 'P':
2046                                         retval = gdb_set_register_packet(connection, target, packet, packet_size);
2047                                         break;
2048                                 case 'm':
2049                                         retval = gdb_read_memory_packet(connection, target, packet, packet_size);
2050                                         break;
2051                                 case 'M':
2052                                         retval = gdb_write_memory_packet(connection, target, packet, packet_size);
2053                                         break;
2054                                 case 'z':
2055                                 case 'Z':
2056                                         retval = gdb_breakpoint_watchpoint_packet(connection, target, packet, packet_size);
2057                                         break;
2058                                 case '?':
2059                                         gdb_last_signal_packet(connection, target, packet, packet_size);
2060                                         break;
2061                                 case 'c':
2062                                 case 's':
2063                                         {
2064                                                 int retval = ERROR_OK;
2065
2066                                                 struct gdb_connection *gdb_con = connection->priv;
2067                                                 log_add_callback(gdb_log_callback, connection);
2068
2069                                                 bool nostep = false;
2070                                                 if (target->state == TARGET_RUNNING)
2071                                                 {
2072                                                         LOG_WARNING("The target is already running. Halt target before stepi/continue.");
2073                                                         retval = target_halt(target);
2074                                                         if (retval == ERROR_OK)
2075                                                                 retval = target_wait_state(target, TARGET_HALTED, 100);
2076                                                 } else if (target->state != TARGET_HALTED)
2077                                                 {
2078                                                         LOG_WARNING("The target is not in the halted nor running stated, stepi/continue ignored.");
2079                                                         nostep = true;
2080                                                 } else if ((packet[0] == 's') && gdb_con->sync)
2081                                                 {
2082                                                         /* Hmm..... when you issue a continue in GDB, then a "stepi" is
2083                                                          * sent by GDB first to OpenOCD, thus defeating the check to
2084                                                          * make only the single stepping have the sync feature...
2085                                                          */
2086                                                         nostep = true;
2087                                                         LOG_WARNING("stepi ignored. GDB will now fetch the register state from the target.");
2088                                                 }
2089                                                 gdb_con->sync = false;
2090
2091                                                 if ((retval!=ERROR_OK) || nostep)
2092                                                 {
2093                                                         /* Either the target isn't in the halted state, then we can't
2094                                                          * step/continue. This might be early setup, etc.
2095                                                          *
2096                                                          * Or we want to allow GDB to pick up a fresh set of
2097                                                          * register values without modifying the target state.
2098                                                          *
2099                                                          */
2100                                                         gdb_sig_halted(connection);
2101
2102                                                         /* stop forwarding log packets! */
2103                                                         log_remove_callback(gdb_log_callback, connection);
2104                                                 } else
2105                                                 {
2106                                                         /* We're running/stepping, in which case we can
2107                                                          * forward log output until the target is halted
2108                                                          */
2109                                                         gdb_con->frontend_state = TARGET_RUNNING;
2110                                                         target_call_event_callbacks(target, TARGET_EVENT_GDB_START);
2111                                                         int retval = gdb_step_continue_packet(connection, target, packet, packet_size);
2112                                                         if (retval != ERROR_OK)
2113                                                         {
2114                                                                 /* we'll never receive a halted condition... issue a false one.. */
2115                                                                 gdb_frontend_halted(target, connection);
2116                                                         }
2117                                                 }
2118                                         }
2119                                         break;
2120                                 case 'v':
2121                                         retval = gdb_v_packet(connection, target, packet, packet_size);
2122                                         break;
2123                                 case 'D':
2124                                         retval = gdb_detach(connection, target);
2125                                         extended_protocol = 0;
2126                                         break;
2127                                 case 'X':
2128                                         if ((retval = gdb_write_memory_binary_packet(connection, target, packet, packet_size)) != ERROR_OK)
2129                                                 return retval;
2130                                         break;
2131                                 case 'k':
2132                                         if (extended_protocol != 0)
2133                                                 break;
2134                                         gdb_put_packet(connection, "OK", 2);
2135                                         return ERROR_SERVER_REMOTE_CLOSED;
2136                                 case '!':
2137                                         /* handle extended remote protocol */
2138                                         extended_protocol = 1;
2139                                         gdb_put_packet(connection, "OK", 2);
2140                                         break;
2141                                 case 'R':
2142                                         /* handle extended restart packet */
2143                                         breakpoint_clear_target(gdb_service->target);
2144                                         watchpoint_clear_target(gdb_service->target);
2145                                         command_run_linef(connection->cmd_ctx,
2146                                                         "ocd_gdb_restart %s",
2147                                                         target->cmd_name);
2148                                         break;
2149                                 default:
2150                                         /* ignore unkown packets */
2151                                         LOG_DEBUG("ignoring 0x%2.2x packet", packet[0]);
2152                                         gdb_put_packet(connection, NULL, 0);
2153                                         break;
2154                         }
2155
2156                         /* if a packet handler returned an error, exit input loop */
2157                         if (retval != ERROR_OK)
2158                                 return retval;
2159                 }
2160
2161                 if (gdb_con->ctrl_c)
2162                 {
2163                         if (target->state == TARGET_RUNNING)
2164                         {
2165                                 retval = target_halt(target);
2166                                 if (retval != ERROR_OK)
2167                                 {
2168                                         target_call_event_callbacks(target, TARGET_EVENT_GDB_HALT);
2169                                 }
2170                                 gdb_con->ctrl_c = 0;
2171                         } else
2172                         {
2173                                 LOG_INFO("The target is not running when halt was requested, stopping GDB.");
2174                                 target_call_event_callbacks(target, TARGET_EVENT_GDB_HALT);
2175                         }
2176                 }
2177
2178         } while (gdb_con->buf_cnt > 0);
2179
2180         return ERROR_OK;
2181 }
2182
2183 int gdb_input(struct connection *connection)
2184 {
2185         int retval = gdb_input_inner(connection);
2186         struct gdb_connection *gdb_con = connection->priv;
2187         if (retval == ERROR_SERVER_REMOTE_CLOSED)
2188                 return retval;
2189
2190         /* logging does not propagate the error, yet can set the gdb_con->closed flag */
2191         if (gdb_con->closed)
2192                 return ERROR_SERVER_REMOTE_CLOSED;
2193
2194         /* we'll recover from any other errors(e.g. temporary timeouts, etc.) */
2195         return ERROR_OK;
2196 }
2197
2198 int gdb_init(void)
2199 {
2200         struct gdb_service *gdb_service;
2201         target_t *target = all_targets;
2202
2203         if (!target)
2204         {
2205                 LOG_WARNING("no gdb ports allocated as no target has been specified");
2206                 return ERROR_OK;
2207         }
2208
2209         if (gdb_port == 0 && server_use_pipes == 0)
2210         {
2211                 LOG_INFO("gdb port disabled");
2212                 return ERROR_OK;
2213         }
2214
2215         if (server_use_pipes)
2216         {
2217                 /* only a single gdb connection when using a pipe */
2218
2219                 gdb_service = malloc(sizeof(struct gdb_service));
2220                 gdb_service->target = target;
2221
2222                 add_service("gdb", CONNECTION_PIPE, 0, 1, gdb_new_connection, gdb_input, gdb_connection_closed, gdb_service);
2223
2224                 LOG_DEBUG("gdb service for target %s using pipes",
2225                                 target_get_name(target));
2226         }
2227         else
2228         {
2229                 unsigned short port = gdb_port;
2230
2231                 while (target)
2232                 {
2233                         gdb_service = malloc(sizeof(struct gdb_service));
2234                         gdb_service->target = target;
2235
2236                         add_service("gdb", CONNECTION_TCP,
2237                                         port, 1,
2238                                         gdb_new_connection, gdb_input,
2239                                         gdb_connection_closed, gdb_service);
2240
2241                         LOG_DEBUG("gdb service for target %s at TCP port %i",
2242                                         target_get_name(target),
2243                                         port);
2244                         target = target->next;
2245                         port++;
2246                 }
2247         }
2248
2249         return ERROR_OK;
2250 }
2251
2252 COMMAND_HANDLER(handle_gdb_sync_command)
2253 {
2254         if (argc != 0)
2255         {
2256                 return ERROR_COMMAND_SYNTAX_ERROR;
2257         }
2258
2259         if (current_gdb_connection == NULL)
2260         {
2261                 command_print(cmd_ctx,
2262                                 "gdb_sync command can only be run from within gdb using \"monitor gdb_sync\"");
2263                 return ERROR_FAIL;
2264         }
2265
2266         current_gdb_connection->sync = true;
2267
2268         return ERROR_OK;
2269 }
2270
2271 /* daemon configuration command gdb_port */
2272 COMMAND_HANDLER(handle_gdb_port_command)
2273 {
2274         return CALL_COMMAND_HANDLER(server_port_command, &gdb_port);
2275 }
2276
2277 COMMAND_HANDLER(handle_gdb_memory_map_command)
2278 {
2279         if (argc == 1)
2280         {
2281                 if (strcmp(args[0], "enable") == 0)
2282                 {
2283                         gdb_use_memory_map = 1;
2284                         return ERROR_OK;
2285                 }
2286                 else if (strcmp(args[0], "disable") == 0)
2287                 {
2288                         gdb_use_memory_map = 0;
2289                         return ERROR_OK;
2290                 }
2291                 else
2292                         LOG_WARNING("invalid gdb_memory_map configuration directive %s", args[0]);
2293         }
2294
2295         return ERROR_COMMAND_SYNTAX_ERROR;
2296 }
2297
2298 COMMAND_HANDLER(handle_gdb_flash_program_command)
2299 {
2300         if (argc == 1)
2301         {
2302                 if (strcmp(args[0], "enable") == 0)
2303                 {
2304                         gdb_flash_program = 1;
2305                         return ERROR_OK;
2306                 }
2307                 else if (strcmp(args[0], "disable") == 0)
2308                 {
2309                         gdb_flash_program = 0;
2310                         return ERROR_OK;
2311                 }
2312                 else
2313                         LOG_WARNING("invalid gdb_flash_program configuration directive: %s", args[0]);
2314         }
2315
2316         return ERROR_COMMAND_SYNTAX_ERROR;
2317 }
2318
2319 COMMAND_HANDLER(handle_gdb_report_data_abort_command)
2320 {
2321         if (argc == 1)
2322         {
2323                 if (strcmp(args[0], "enable") == 0)
2324                 {
2325                         gdb_report_data_abort = 1;
2326                         return ERROR_OK;
2327                 }
2328                 else if (strcmp(args[0], "disable") == 0)
2329                 {
2330                         gdb_report_data_abort = 0;
2331                         return ERROR_OK;
2332                 }
2333                 else
2334                         LOG_WARNING("invalid gdb_report_data_abort configuration directive: %s", args[0]);
2335         }
2336
2337         return ERROR_COMMAND_SYNTAX_ERROR;
2338 }
2339
2340 /* gdb_breakpoint_override */
2341 COMMAND_HANDLER(handle_gdb_breakpoint_override_command)
2342 {
2343         if (argc == 0)
2344         {
2345
2346         } else if (argc == 1)
2347         {
2348                 gdb_breakpoint_override = 1;
2349                 if (strcmp(args[0], "hard") == 0)
2350                 {
2351                         gdb_breakpoint_override_type = BKPT_HARD;
2352                 } else if (strcmp(args[0], "soft") == 0)
2353                 {
2354                         gdb_breakpoint_override_type = BKPT_SOFT;
2355                 } else if (strcmp(args[0], "disable") == 0)
2356                 {
2357                         gdb_breakpoint_override = 0;
2358                 }
2359         } else
2360         {
2361                 return ERROR_COMMAND_SYNTAX_ERROR;
2362         }
2363         if (gdb_breakpoint_override)
2364         {
2365                 LOG_USER("force %s breakpoints", (gdb_breakpoint_override_type == BKPT_HARD)?"hard":"soft");
2366         } else
2367         {
2368                 LOG_USER("breakpoint type is not overriden");
2369         }
2370
2371         return ERROR_OK;
2372 }
2373
2374 int gdb_register_commands(command_context_t *command_context)
2375 {
2376         register_command(command_context, NULL, "gdb_sync",
2377                         handle_gdb_sync_command, COMMAND_ANY,
2378                         "next stepi will return immediately allowing GDB to "
2379                         "fetch register state without affecting target state");
2380         register_command(command_context, NULL, "gdb_port",
2381                         handle_gdb_port_command, COMMAND_ANY,
2382                         "daemon configuration command gdb_port");
2383         register_command(command_context, NULL, "gdb_memory_map",
2384                         handle_gdb_memory_map_command, COMMAND_CONFIG,
2385                         "enable or disable memory map");
2386         register_command(command_context, NULL, "gdb_flash_program",
2387                         handle_gdb_flash_program_command, COMMAND_CONFIG,
2388                         "enable or disable flash program");
2389         register_command(command_context, NULL, "gdb_report_data_abort",
2390                         handle_gdb_report_data_abort_command, COMMAND_CONFIG,
2391                         "enable or disable reporting data aborts");
2392         register_command(command_context, NULL, "gdb_breakpoint_override",
2393                         handle_gdb_breakpoint_override_command, COMMAND_EXEC,
2394                         "hard/soft/disable - force type of breakpoint "
2395                         "used by gdb 'break' commands.");
2396         return ERROR_OK;
2397 }