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