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