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