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