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