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 compare_bank (const void * a, const void * b)
1617 {
1618         struct flash_bank *b1, *b2;
1619         b1=*((struct flash_bank **)a);
1620         b2=*((struct flash_bank **)b);
1621
1622         if (b1->base == b2->base)
1623         {
1624                 return 0;
1625         } else if (b1->base > b2->base)
1626         {
1627                 return 1;
1628         } else
1629         {
1630                 return -1;
1631         }
1632 }
1633
1634 static int gdb_memory_map(struct connection *connection,
1635                 struct target *target, char *packet, int packet_size)
1636 {
1637         /* We get away with only specifying flash here. Regions that are not
1638          * specified are treated as if we provided no memory map(if not we
1639          * could detect the holes and mark them as RAM).
1640          * Normally we only execute this code once, but no big deal if we
1641          * have to regenerate it a couple of times.
1642          */
1643
1644         struct flash_bank *p;
1645         char *xml = NULL;
1646         int size = 0;
1647         int pos = 0;
1648         int retval = ERROR_OK;
1649         struct flash_bank **banks;
1650         int offset;
1651         int length;
1652         char *separator;
1653         uint32_t ram_start = 0;
1654         int i;
1655
1656         /* skip command character */
1657         packet += 23;
1658
1659         offset = strtoul(packet, &separator, 16);
1660         length = strtoul(separator + 1, &separator, 16);
1661
1662         xml_printf(&retval, &xml, &pos, &size, "<memory-map>\n");
1663
1664         /* Sort banks in ascending order.  We need to report non-flash
1665          * memory as ram (or rather read/write) by default for GDB, since
1666          * it has no concept of non-cacheable read/write memory (i/o etc).
1667          *
1668          * FIXME Most non-flash addresses are *NOT* RAM!  Don't lie.
1669          * Current versions of GDB assume unlisted addresses are RAM...
1670          */
1671         banks = malloc(sizeof(struct flash_bank *)*flash_get_bank_count());
1672
1673         for (i = 0; i < flash_get_bank_count(); i++) {
1674                 p = get_flash_bank_by_num(i);
1675                 if (p == NULL) {
1676                         free(banks);
1677                         retval = ERROR_FAIL;
1678                         gdb_send_error(connection, retval);
1679                         return retval;
1680                 }
1681                 banks[i] = p;
1682         }
1683
1684         qsort(banks, flash_get_bank_count(), sizeof(struct flash_bank *),
1685                         compare_bank);
1686
1687         for (i = 0; i < flash_get_bank_count(); i++) {
1688                 int j;
1689                 unsigned sector_size = 0;
1690                 uint32_t start, end;
1691
1692                 p = banks[i];
1693                 start = p->base;
1694                 end = p->base + p->size;
1695
1696                 if (ram_start < p->base)
1697                         xml_printf(&retval, &xml, &pos, &size,
1698                                 "<memory type=\"ram\" start=\"0x%x\" "
1699                                         "length=\"0x%x\"/>\n",
1700                                 ram_start, p->base - ram_start);
1701
1702                 /* Report adjacent groups of same-size sectors.  So for
1703                  * example top boot CFI flash will list an initial region
1704                  * with several large sectors (maybe 128KB) and several
1705                  * smaller ones at the end (maybe 32KB).  STR7 will have
1706                  * regions with 8KB, 32KB, and 64KB sectors; etc.
1707                  */
1708                 for (j = 0; j < p->num_sectors; j++) {
1709                         unsigned group_len;
1710
1711                         /* Maybe start a new group of sectors. */
1712                         if (sector_size == 0) {
1713                                 start = p->base + p->sectors[j].offset;
1714                                 xml_printf(&retval, &xml, &pos, &size,
1715                                         "<memory type=\"flash\" "
1716                                                 "start=\"0x%x\" ",
1717                                         start);
1718                                 sector_size = p->sectors[j].size;
1719                         }
1720
1721                         /* Does this finish a group of sectors?
1722                          * If not, continue an already-started group.
1723                          */
1724                         if (j == p->num_sectors -1)
1725                                 group_len = (p->base + p->size) - start;
1726                         else if (p->sectors[j + 1].size != sector_size)
1727                                 group_len = p->base + p->sectors[j + 1].offset
1728                                                 - start;
1729                         else
1730                                 continue;
1731
1732                         xml_printf(&retval, &xml, &pos, &size,
1733                                 "length=\"0x%x\">\n"
1734                                 "<property name=\"blocksize\">"
1735                                         "0x%x</property>\n"
1736                                 "</memory>\n",
1737                                 group_len,
1738                                 sector_size);
1739                         sector_size = 0;
1740                 }
1741
1742                 ram_start = p->base + p->size;
1743         }
1744
1745         if (ram_start != 0)
1746                 xml_printf(&retval, &xml, &pos, &size,
1747                         "<memory type=\"ram\" start=\"0x%x\" "
1748                                 "length=\"0x%x\"/>\n",
1749                         ram_start, 0-ram_start);
1750         /* ELSE a flash chip could be at the very end of the 32 bit address
1751          * space, in which case ram_start will be precisely 0
1752          */
1753
1754         free(banks);
1755         banks = NULL;
1756
1757         xml_printf(&retval, &xml, &pos, &size, "</memory-map>\n");
1758
1759         if (retval != ERROR_OK) {
1760                 gdb_send_error(connection, retval);
1761                 return retval;
1762         }
1763
1764         if (offset + length > pos)
1765                 length = pos - offset;
1766
1767         char *t = malloc(length + 1);
1768         t[0] = 'l';
1769         memcpy(t + 1, xml + offset, length);
1770         gdb_put_packet(connection, t, length + 1);
1771
1772         free(t);
1773         free(xml);
1774         return ERROR_OK;
1775 }
1776
1777 static int gdb_query_packet(struct connection *connection,
1778         struct target *target, char *packet, int packet_size)
1779 {
1780         struct command_context *cmd_ctx = connection->cmd_ctx;
1781         struct gdb_connection *gdb_connection = connection->priv;
1782
1783         if (strstr(packet, "qRcmd,"))
1784         {
1785                 if (packet_size > 6)
1786                 {
1787                         char *cmd;
1788                         int i;
1789                         cmd = malloc((packet_size - 6)/2 + 1);
1790                         for (i = 0; i < (packet_size - 6)/2; i++)
1791                         {
1792                                 uint32_t tmp;
1793                                 sscanf(packet + 6 + 2*i, "%2" SCNx32 , &tmp);
1794                                 cmd[i] = tmp;
1795                         }
1796                         cmd[(packet_size - 6)/2] = 0x0;
1797
1798                         /* We want to print all debug output to GDB connection */
1799                         log_add_callback(gdb_log_callback, connection);
1800                         target_call_timer_callbacks_now();
1801                         /* some commands need to know the GDB connection, make note of current
1802                          * GDB connection. */
1803                         current_gdb_connection = gdb_connection;
1804                         command_run_line(cmd_ctx, cmd);
1805                         current_gdb_connection = NULL;
1806                         target_call_timer_callbacks_now();
1807                         log_remove_callback(gdb_log_callback, connection);
1808                         free(cmd);
1809                 }
1810                 gdb_put_packet(connection, "OK", 2);
1811                 return ERROR_OK;
1812         }
1813         else if (strstr(packet, "qCRC:"))
1814         {
1815                 if (packet_size > 5)
1816                 {
1817                         int retval;
1818                         char gdb_reply[10];
1819                         char *separator;
1820                         uint32_t checksum;
1821                         uint32_t addr = 0;
1822                         uint32_t len = 0;
1823
1824                         /* skip command character */
1825                         packet += 5;
1826
1827                         addr = strtoul(packet, &separator, 16);
1828
1829                         if (*separator != ',')
1830                         {
1831                                 LOG_ERROR("incomplete read memory packet received, dropping connection");
1832                                 return ERROR_SERVER_REMOTE_CLOSED;
1833                         }
1834
1835                         len = strtoul(separator + 1, NULL, 16);
1836
1837                         retval = target_checksum_memory(target, addr, len, &checksum);
1838
1839                         if (retval == ERROR_OK)
1840                         {
1841                                 snprintf(gdb_reply, 10, "C%8.8" PRIx32 "", checksum);
1842                                 gdb_put_packet(connection, gdb_reply, 9);
1843                         }
1844                         else
1845                         {
1846                                 if ((retval = gdb_error(connection, retval)) != ERROR_OK)
1847                                         return retval;
1848                         }
1849
1850                         return ERROR_OK;
1851                 }
1852         }
1853         else if (strstr(packet, "qSupported"))
1854         {
1855                 /* we currently support packet size and qXfer:memory-map:read (if enabled)
1856                  * disable qXfer:features:read for the moment */
1857                 int retval = ERROR_OK;
1858                 char *buffer = NULL;
1859                 int pos = 0;
1860                 int size = 0;
1861
1862                 xml_printf(&retval, &buffer, &pos, &size,
1863                                 "PacketSize=%x;qXfer:memory-map:read%c;qXfer:features:read-;QStartNoAckMode+",
1864                                 (GDB_BUFFER_SIZE - 1), ((gdb_use_memory_map == 1) && (flash_get_bank_count() > 0)) ? '+' : '-');
1865
1866                 if (retval != ERROR_OK)
1867                 {
1868                         gdb_send_error(connection, 01);
1869                         return ERROR_OK;
1870                 }
1871
1872                 gdb_put_packet(connection, buffer, strlen(buffer));
1873                 free(buffer);
1874
1875                 return ERROR_OK;
1876         }
1877         else if (strstr(packet, "qXfer:memory-map:read::")
1878                         && (flash_get_bank_count() > 0))
1879                 return gdb_memory_map(connection, target, packet, packet_size);
1880         else if (strstr(packet, "qXfer:features:read:"))
1881         {
1882                 char *xml = NULL;
1883                 int size = 0;
1884                 int pos = 0;
1885                 int retval = ERROR_OK;
1886
1887                 int offset;
1888                 unsigned int length;
1889                 char *annex;
1890
1891                 /* skip command character */
1892                 packet += 20;
1893
1894                 if (decode_xfer_read(packet, &annex, &offset, &length) < 0)
1895                 {
1896                         gdb_send_error(connection, 01);
1897                         return ERROR_OK;
1898                 }
1899
1900                 if (strcmp(annex, "target.xml") != 0)
1901                 {
1902                         gdb_send_error(connection, 01);
1903                         return ERROR_OK;
1904                 }
1905
1906                 xml_printf(&retval, &xml, &pos, &size, \
1907                         "l < target version=\"1.0\">\n < architecture > arm</architecture>\n</target>\n");
1908
1909                 if (retval != ERROR_OK)
1910                 {
1911                         gdb_send_error(connection, retval);
1912                         return retval;
1913                 }
1914
1915                 gdb_put_packet(connection, xml, strlen(xml));
1916
1917                 free(xml);
1918                 return ERROR_OK;
1919         }
1920         else if (strstr(packet, "QStartNoAckMode"))
1921         {
1922                 gdb_connection->noack_mode = 1;
1923                 gdb_put_packet(connection, "OK", 2);
1924                 return ERROR_OK;
1925         }
1926
1927         gdb_put_packet(connection, "", 0);
1928         return ERROR_OK;
1929 }
1930
1931 static int gdb_v_packet(struct connection *connection,
1932                 struct target *target, char *packet, int packet_size)
1933 {
1934         struct gdb_connection *gdb_connection = connection->priv;
1935         struct gdb_service *gdb_service = connection->service->priv;
1936         int result;
1937
1938         /* if flash programming disabled - send a empty reply */
1939
1940         if (gdb_flash_program == 0)
1941         {
1942                 gdb_put_packet(connection, "", 0);
1943                 return ERROR_OK;
1944         }
1945
1946         if (strstr(packet, "vFlashErase:"))
1947         {
1948                 unsigned long addr;
1949                 unsigned long length;
1950
1951                 char *parse = packet + 12;
1952                 if (*parse == '\0')
1953                 {
1954                         LOG_ERROR("incomplete vFlashErase packet received, dropping connection");
1955                         return ERROR_SERVER_REMOTE_CLOSED;
1956                 }
1957
1958                 addr = strtoul(parse, &parse, 16);
1959
1960                 if (*(parse++) != ',' || *parse == '\0')
1961                 {
1962                         LOG_ERROR("incomplete vFlashErase packet received, dropping connection");
1963                         return ERROR_SERVER_REMOTE_CLOSED;
1964                 }
1965
1966                 length = strtoul(parse, &parse, 16);
1967
1968                 if (*parse != '\0')
1969                 {
1970                         LOG_ERROR("incomplete vFlashErase packet received, dropping connection");
1971                         return ERROR_SERVER_REMOTE_CLOSED;
1972                 }
1973
1974                 /* assume all sectors need erasing - stops any problems
1975                  * when flash_write is called multiple times */
1976                 flash_set_dirty();
1977
1978                 /* perform any target specific operations before the erase */
1979                 target_call_event_callbacks(gdb_service->target,
1980                                 TARGET_EVENT_GDB_FLASH_ERASE_START);
1981
1982                 /* vFlashErase:addr,length messages require region start and
1983                  * end to be "block" aligned ... if padding is ever needed,
1984                  * GDB will have become dangerously confused.
1985                  */
1986                 result = flash_erase_address_range(gdb_service->target,
1987                                 false, addr, length);
1988
1989                 /* perform any target specific operations after the erase */
1990                 target_call_event_callbacks(gdb_service->target,
1991                                 TARGET_EVENT_GDB_FLASH_ERASE_END);
1992
1993                 /* perform erase */
1994                 if (result != ERROR_OK)
1995                 {
1996                         /* GDB doesn't evaluate the actual error number returned,
1997                          * treat a failed erase as an I/O error
1998                          */
1999                         gdb_send_error(connection, EIO);
2000                         LOG_ERROR("flash_erase returned %i", result);
2001                 }
2002                 else
2003                         gdb_put_packet(connection, "OK", 2);
2004
2005                 return ERROR_OK;
2006         }
2007
2008         if (strstr(packet, "vFlashWrite:"))
2009         {
2010                 int retval;
2011                 unsigned long addr;
2012                 unsigned long length;
2013                 char *parse = packet + 12;
2014
2015                 if (*parse == '\0')
2016                 {
2017                         LOG_ERROR("incomplete vFlashErase packet received, dropping connection");
2018                         return ERROR_SERVER_REMOTE_CLOSED;
2019                 }
2020                 addr = strtoul(parse, &parse, 16);
2021                 if (*(parse++) != ':')
2022                 {
2023                         LOG_ERROR("incomplete vFlashErase packet received, dropping connection");
2024                         return ERROR_SERVER_REMOTE_CLOSED;
2025                 }
2026                 length = packet_size - (parse - packet);
2027
2028                 /* create a new image if there isn't already one */
2029                 if (gdb_connection->vflash_image == NULL)
2030                 {
2031                         gdb_connection->vflash_image = malloc(sizeof(struct image));
2032                         image_open(gdb_connection->vflash_image, "", "build");
2033                 }
2034
2035                 /* create new section with content from packet buffer */
2036                 if ((retval = image_add_section(gdb_connection->vflash_image, addr, length, 0x0, (uint8_t*)parse)) != ERROR_OK)
2037                 {
2038                         return retval;
2039                 }
2040
2041                 gdb_put_packet(connection, "OK", 2);
2042
2043                 return ERROR_OK;
2044         }
2045
2046         if (!strcmp(packet, "vFlashDone"))
2047         {
2048                 uint32_t written;
2049
2050                 /* process the flashing buffer. No need to erase as GDB
2051                  * always issues a vFlashErase first. */
2052                 target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_FLASH_WRITE_START);
2053                 result = flash_write(gdb_service->target, gdb_connection->vflash_image, &written, 0);
2054                 target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_FLASH_WRITE_END);
2055                 if (result != ERROR_OK)
2056                 {
2057                         if (result == ERROR_FLASH_DST_OUT_OF_BANK)
2058                                 gdb_put_packet(connection, "E.memtype", 9);
2059                         else
2060                                 gdb_send_error(connection, EIO);
2061                         }
2062                 else
2063                 {
2064                         LOG_DEBUG("wrote %u bytes from vFlash image to flash", (unsigned)written);
2065                         gdb_put_packet(connection, "OK", 2);
2066                 }
2067
2068                 image_close(gdb_connection->vflash_image);
2069                 free(gdb_connection->vflash_image);
2070                 gdb_connection->vflash_image = NULL;
2071
2072                 return ERROR_OK;
2073         }
2074
2075         gdb_put_packet(connection, "", 0);
2076         return ERROR_OK;
2077 }
2078
2079 static int gdb_detach(struct connection *connection, struct target *target)
2080 {
2081         struct gdb_service *gdb_service = connection->service->priv;
2082
2083         target_call_event_callbacks(gdb_service->target,
2084                         TARGET_EVENT_GDB_DETACH);
2085
2086         return gdb_put_packet(connection, "OK", 2);
2087 }
2088
2089 static void gdb_log_callback(void *priv, const char *file, unsigned line,
2090                 const char *function, const char *string)
2091 {
2092         struct connection *connection = priv;
2093         struct gdb_connection *gdb_con = connection->priv;
2094
2095         if (gdb_con->busy)
2096         {
2097                 /* do not reply this using the O packet */
2098                 return;
2099         }
2100
2101         gdb_output_con(connection, string);
2102 }
2103
2104 static void gdb_sig_halted(struct connection *connection)
2105 {
2106         char sig_reply[4];
2107         snprintf(sig_reply, 4, "T%2.2x", 2);
2108         gdb_put_packet(connection, sig_reply, 3);
2109
2110 }
2111
2112 static int gdb_input_inner(struct connection *connection)
2113 {
2114         /* Do not allocate this on the stack */
2115         static char gdb_packet_buffer[GDB_BUFFER_SIZE];
2116
2117         struct gdb_service *gdb_service = connection->service->priv;
2118         struct target *target = gdb_service->target;
2119         char *packet = gdb_packet_buffer;
2120         int packet_size;
2121         int retval;
2122         struct gdb_connection *gdb_con = connection->priv;
2123         static int extended_protocol = 0;
2124
2125         /* drain input buffer */
2126         do
2127         {
2128                 packet_size = GDB_BUFFER_SIZE-1;
2129                 retval = gdb_get_packet(connection, packet, &packet_size);
2130                 if (retval != ERROR_OK)
2131                         return retval;
2132
2133                 /* terminate with zero */
2134                 packet[packet_size] = 0;
2135
2136                 if (LOG_LEVEL_IS(LOG_LVL_DEBUG)) {
2137                         if (packet[0] == 'X') {
2138                                 // binary packets spew junk into the debug log stream
2139                                 char buf[ 50 ];
2140                                 int x;
2141                                 for (x = 0 ; (x < 49) && (packet[x] != ':') ; x++) {
2142                                         buf[x] = packet[x];
2143                                 }
2144                                 buf[x] = 0;
2145                                 LOG_DEBUG("received packet: '%s:<binary-data>'", buf);
2146                         } else {
2147                                 LOG_DEBUG("received packet: '%s'", packet);
2148                         }
2149                 }
2150
2151                 if (packet_size > 0)
2152                 {
2153                         retval = ERROR_OK;
2154                         switch (packet[0])
2155                         {
2156                                 case 'H':
2157                                         /* Hct... -- set thread
2158                                          * we don't have threads, send empty reply */
2159                                         gdb_put_packet(connection, NULL, 0);
2160                                         break;
2161                                 case 'q':
2162                                 case 'Q':
2163                                         retval = gdb_query_packet(connection,
2164                                                         target, packet,
2165                                                         packet_size);
2166                                         break;
2167                                 case 'g':
2168                                         retval = gdb_get_registers_packet(
2169                                                         connection, target,
2170                                                         packet, packet_size);
2171                                         break;
2172                                 case 'G':
2173                                         retval = gdb_set_registers_packet(
2174                                                         connection, target,
2175                                                         packet, packet_size);
2176                                         break;
2177                                 case 'p':
2178                                         retval = gdb_get_register_packet(
2179                                                         connection, target,
2180                                                         packet, packet_size);
2181                                         break;
2182                                 case 'P':
2183                                         retval = gdb_set_register_packet(
2184                                                         connection, target,
2185                                                         packet, packet_size);
2186                                         break;
2187                                 case 'm':
2188                                         retval = gdb_read_memory_packet(
2189                                                         connection, target,
2190                                                         packet, packet_size);
2191                                         break;
2192                                 case 'M':
2193                                         retval = gdb_write_memory_packet(
2194                                                         connection, target,
2195                                                         packet, packet_size);
2196                                         break;
2197                                 case 'z':
2198                                 case 'Z':
2199                                         retval = gdb_breakpoint_watchpoint_packet(connection, target, packet, packet_size);
2200                                         break;
2201                                 case '?':
2202                                         gdb_last_signal_packet(
2203                                                         connection, target,
2204                                                         packet, packet_size);
2205                                         break;
2206                                 case 'c':
2207                                 case 's':
2208                                         {
2209                                                 int retval = ERROR_OK;
2210
2211                                                 struct gdb_connection *gdb_con = connection->priv;
2212                                                 log_add_callback(gdb_log_callback, connection);
2213
2214                                                 bool nostep = false;
2215                                                 bool already_running = false;
2216                                                 if (target->state == TARGET_RUNNING)
2217                                                 {
2218                                                         LOG_WARNING("WARNING! The target is already running. "
2219                                                                         "All changes GDB did to registers will be discarded! "
2220                                                                         "Waiting for target to halt.");
2221                                                         already_running = true;
2222                                                 } else if (target->state != TARGET_HALTED)
2223                                                 {
2224                                                         LOG_WARNING("The target is not in the halted nor running stated, stepi/continue ignored.");
2225                                                         nostep = true;
2226                                                 } else if ((packet[0] == 's') && gdb_con->sync)
2227                                                 {
2228                                                         /* Hmm..... when you issue a continue in GDB, then a "stepi" is
2229                                                          * sent by GDB first to OpenOCD, thus defeating the check to
2230                                                          * make only the single stepping have the sync feature...
2231                                                          */
2232                                                         nostep = true;
2233                                                         LOG_WARNING("stepi ignored. GDB will now fetch the register state from the target.");
2234                                                 }
2235                                                 gdb_con->sync = false;
2236
2237                                                 if ((retval!=ERROR_OK) || (!already_running && nostep))
2238                                                 {
2239                                                         /* Either the target isn't in the halted state, then we can't
2240                                                          * step/continue. This might be early setup, etc.
2241                                                          *
2242                                                          * Or we want to allow GDB to pick up a fresh set of
2243                                                          * register values without modifying the target state.
2244                                                          *
2245                                                          */
2246                                                         gdb_sig_halted(connection);
2247
2248                                                         /* stop forwarding log packets! */
2249                                                         log_remove_callback(gdb_log_callback, connection);
2250                                                 } else
2251                                                 {
2252                                                         /* We're running/stepping, in which case we can
2253                                                          * forward log output until the target is halted
2254                                                          */
2255                                                         gdb_con->frontend_state = TARGET_RUNNING;
2256                                                         target_call_event_callbacks(target, TARGET_EVENT_GDB_START);
2257
2258                                                         if (!already_running)
2259                                                         {
2260                                                                 int retval = gdb_step_continue_packet(connection, target, packet, packet_size);
2261                                                                 if (retval != ERROR_OK)
2262                                                                 {
2263                                                                         /* we'll never receive a halted condition... issue a false one.. */
2264                                                                         gdb_frontend_halted(target, connection);
2265                                                                 }
2266                                                         }
2267                                                 }
2268                                         }
2269                                         break;
2270                                 case 'v':
2271                                         retval = gdb_v_packet(
2272                                                         connection, target,
2273                                                         packet, packet_size);
2274                                         break;
2275                                 case 'D':
2276                                         retval = gdb_detach(connection, target);
2277                                         extended_protocol = 0;
2278                                         break;
2279                                 case 'X':
2280                                         retval = gdb_write_memory_binary_packet(
2281                                                         connection, target,
2282                                                         packet, packet_size);
2283                                         if (retval != ERROR_OK)
2284                                                 return retval;
2285                                         break;
2286                                 case 'k':
2287                                         if (extended_protocol != 0)
2288                                                 break;
2289                                         gdb_put_packet(connection, "OK", 2);
2290                                         return ERROR_SERVER_REMOTE_CLOSED;
2291                                 case '!':
2292                                         /* handle extended remote protocol */
2293                                         extended_protocol = 1;
2294                                         gdb_put_packet(connection, "OK", 2);
2295                                         break;
2296                                 case 'R':
2297                                         /* handle extended restart packet */
2298                                         breakpoint_clear_target(gdb_service->target);
2299                                         watchpoint_clear_target(gdb_service->target);
2300                                         command_run_linef(connection->cmd_ctx,
2301                                                         "ocd_gdb_restart %s",
2302                                                         target_name(target));
2303                                         break;
2304                                 default:
2305                                         /* ignore unknown packets */
2306                                         LOG_DEBUG("ignoring 0x%2.2x packet", packet[0]);
2307                                         gdb_put_packet(connection, NULL, 0);
2308                                         break;
2309                         }
2310
2311                         /* if a packet handler returned an error, exit input loop */
2312                         if (retval != ERROR_OK)
2313                                 return retval;
2314                 }
2315
2316                 if (gdb_con->ctrl_c)
2317                 {
2318                         if (target->state == TARGET_RUNNING)
2319                         {
2320                                 retval = target_halt(target);
2321                                 if (retval != ERROR_OK)
2322                                 {
2323                                         target_call_event_callbacks(target, TARGET_EVENT_GDB_HALT);
2324                                 }
2325                                 gdb_con->ctrl_c = 0;
2326                         } else
2327                         {
2328                                 LOG_INFO("The target is not running when halt was requested, stopping GDB.");
2329                                 target_call_event_callbacks(target, TARGET_EVENT_GDB_HALT);
2330                         }
2331                 }
2332
2333         } while (gdb_con->buf_cnt > 0);
2334
2335         return ERROR_OK;
2336 }
2337
2338 static int gdb_input(struct connection *connection)
2339 {
2340         int retval = gdb_input_inner(connection);
2341         struct gdb_connection *gdb_con = connection->priv;
2342         if (retval == ERROR_SERVER_REMOTE_CLOSED)
2343                 return retval;
2344
2345         /* logging does not propagate the error, yet can set the gdb_con->closed flag */
2346         if (gdb_con->closed)
2347                 return ERROR_SERVER_REMOTE_CLOSED;
2348
2349         /* we'll recover from any other errors(e.g. temporary timeouts, etc.) */
2350         return ERROR_OK;
2351 }
2352
2353 static int gdb_target_start(struct target *target, uint16_t port)
2354 {
2355         bool use_pipes = 0 == port;
2356         struct gdb_service *gdb_service = malloc(sizeof(struct gdb_service));
2357         if (NULL == gdb_service)
2358                 return -ENOMEM;
2359
2360         gdb_service->target = target;
2361
2362         add_service("gdb", use_pipes ? CONNECTION_PIPE : CONNECTION_TCP,
2363                         port, 1, &gdb_new_connection, &gdb_input,
2364                         &gdb_connection_closed, gdb_service);
2365
2366         const char *name = target_name(target);
2367         if (use_pipes)
2368                 LOG_DEBUG("gdb service for target '%s' using pipes", name);
2369         else
2370                 LOG_DEBUG("gdb service for target '%s' on TCP port %u", name, port);
2371         return ERROR_OK;
2372 }
2373
2374 static int gdb_target_add_one(struct target *target)
2375 {
2376         if (gdb_port == 0 && server_use_pipes == 0)
2377         {
2378                 LOG_INFO("gdb port disabled");
2379                 return ERROR_OK;
2380         }
2381         if (0 == gdb_port_next)
2382                 gdb_port_next = gdb_port;
2383
2384         bool use_pipes = server_use_pipes;
2385         static bool server_started_with_pipes = false;
2386         if (server_started_with_pipes)
2387         {
2388                 LOG_WARNING("gdb service permits one target when using pipes");
2389                 if (0 == gdb_port)
2390                         return ERROR_OK;
2391
2392                 use_pipes = false;
2393         }
2394
2395         int e = gdb_target_start(target, use_pipes ? 0 : gdb_port_next);
2396         if (ERROR_OK == e)
2397         {
2398                 server_started_with_pipes |= use_pipes;
2399                 gdb_port_next++;
2400         }
2401         return e;
2402 }
2403
2404 int gdb_target_add_all(struct target *target)
2405 {
2406         if (NULL == target)
2407         {
2408                 LOG_WARNING("gdb services need one or more targets defined");
2409                 return ERROR_OK;
2410         }
2411
2412         while (NULL != target)
2413         {
2414                 int retval = gdb_target_add_one(target);
2415                 if (ERROR_OK != retval)
2416                         return retval;
2417
2418                 target = target->next;
2419         }
2420
2421         return ERROR_OK;
2422 }
2423
2424 COMMAND_HANDLER(handle_gdb_sync_command)
2425 {
2426         if (CMD_ARGC != 0)
2427         {
2428                 return ERROR_COMMAND_SYNTAX_ERROR;
2429         }
2430
2431         if (current_gdb_connection == NULL)
2432         {
2433                 command_print(CMD_CTX,
2434                                 "gdb_sync command can only be run from within gdb using \"monitor gdb_sync\"");
2435                 return ERROR_FAIL;
2436         }
2437
2438         current_gdb_connection->sync = true;
2439
2440         return ERROR_OK;
2441 }
2442
2443 /* daemon configuration command gdb_port */
2444 COMMAND_HANDLER(handle_gdb_port_command)
2445 {
2446         int retval = CALL_COMMAND_HANDLER(server_port_command, &gdb_port);
2447         if (ERROR_OK == retval)
2448                 gdb_port_next = gdb_port;
2449         return retval;
2450 }
2451
2452 COMMAND_HANDLER(handle_gdb_memory_map_command)
2453 {
2454         if (CMD_ARGC == 1)
2455                 COMMAND_PARSE_ENABLE(CMD_ARGV[0], gdb_use_memory_map);
2456
2457         return ERROR_COMMAND_SYNTAX_ERROR;
2458 }
2459
2460 COMMAND_HANDLER(handle_gdb_flash_program_command)
2461 {
2462         if (CMD_ARGC == 1)
2463                 COMMAND_PARSE_ENABLE(CMD_ARGV[0], gdb_flash_program);
2464
2465         return ERROR_COMMAND_SYNTAX_ERROR;
2466 }
2467
2468 COMMAND_HANDLER(handle_gdb_report_data_abort_command)
2469 {
2470         if (CMD_ARGC == 1)
2471                 COMMAND_PARSE_ENABLE(CMD_ARGV[0], gdb_report_data_abort);
2472
2473         return ERROR_COMMAND_SYNTAX_ERROR;
2474 }
2475
2476 /* gdb_breakpoint_override */
2477 COMMAND_HANDLER(handle_gdb_breakpoint_override_command)
2478 {
2479         if (CMD_ARGC == 0)
2480         {
2481
2482         } else if (CMD_ARGC == 1)
2483         {
2484                 gdb_breakpoint_override = 1;
2485                 if (strcmp(CMD_ARGV[0], "hard") == 0)
2486                 {
2487                         gdb_breakpoint_override_type = BKPT_HARD;
2488                 } else if (strcmp(CMD_ARGV[0], "soft") == 0)
2489                 {
2490                         gdb_breakpoint_override_type = BKPT_SOFT;
2491                 } else if (strcmp(CMD_ARGV[0], "disable") == 0)
2492                 {
2493                         gdb_breakpoint_override = 0;
2494                 }
2495         } else
2496         {
2497                 return ERROR_COMMAND_SYNTAX_ERROR;
2498         }
2499         if (gdb_breakpoint_override)
2500         {
2501                 LOG_USER("force %s breakpoints", (gdb_breakpoint_override_type == BKPT_HARD)?"hard":"soft");
2502         } else
2503         {
2504                 LOG_USER("breakpoint type is not overridden");
2505         }
2506
2507         return ERROR_OK;
2508 }
2509
2510 static const struct command_registration gdb_command_handlers[] = {
2511         {
2512                 .name = "gdb_sync",
2513                 .handler = handle_gdb_sync_command,
2514                 .mode = COMMAND_ANY,
2515                 .help = "next stepi will return immediately allowing "
2516                         "GDB to fetch register state without affecting "
2517                         "target state",
2518         },
2519         {
2520                 .name = "gdb_port",
2521                 .handler = handle_gdb_port_command,
2522                 .mode = COMMAND_ANY,
2523                 .help = "Display or specify base port on which to listen "
2524                         "for incoming GDB connections.  "
2525                         "No arguments reports GDB port; zero disables.",
2526                 .usage = "[port_num]",
2527         },
2528         {
2529                 .name = "gdb_memory_map",
2530                 .handler = handle_gdb_memory_map_command,
2531                 .mode = COMMAND_CONFIG,
2532                 .help = "enable or disable memory map",
2533                 .usage = "('enable'|'disable')"
2534         },
2535         {
2536                 .name = "gdb_flash_program",
2537                 .handler = handle_gdb_flash_program_command,
2538                 .mode = COMMAND_CONFIG,
2539                 .help = "enable or disable flash program",
2540                 .usage = "('enable'|'disable')"
2541         },
2542         {
2543                 .name = "gdb_report_data_abort",
2544                 .handler = handle_gdb_report_data_abort_command,
2545                 .mode = COMMAND_CONFIG,
2546                 .help = "enable or disable reporting data aborts",
2547                 .usage = "('enable'|'disable')"
2548         },
2549         {
2550                 .name = "gdb_breakpoint_override",
2551                 .handler = handle_gdb_breakpoint_override_command,
2552                 .mode = COMMAND_ANY,
2553                 .help = "Display or specify type of breakpoint "
2554                         "to be used by gdb 'break' commands.",
2555                 .usage = "('hard'|'soft'|'disable')"
2556         },
2557         COMMAND_REGISTRATION_DONE
2558 };
2559
2560 int gdb_register_commands(struct command_context *cmd_ctx)
2561 {
2562         return register_commands(cmd_ctx, NULL, gdb_command_handlers);
2563 }