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