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
 723 int gdb_new_connection(connection_t *connection)
 724 {
 725         gdb_connection_t *gdb_connection = malloc(sizeof(gdb_connection_t));
 726         gdb_service_t *gdb_service = connection->service->priv;
 727         int retval;
 728         int initial_ack;
 729
 730         connection->priv = gdb_connection;
 731
 732         /* initialize gdb connection information */
 733         gdb_connection->buf_p = gdb_connection->buffer;
 734         gdb_connection->buf_cnt = 0;
 735         gdb_connection->ctrl_c = 0;
 736         gdb_connection->frontend_state = TARGET_HALTED;
 737         gdb_connection->vflash_image = NULL;
 738         gdb_connection->closed = 0;
 739         gdb_connection->busy = 0;
 740         gdb_connection->noack_mode = 0;
 741
 742         /* send ACK to GDB for debug request */
 743         gdb_write(connection, "+", 1);
 744
 745         /* output goes through gdb connection */
 746         command_set_output_handler(connection->cmd_ctx, gdb_output, connection);
 747
 748         /* we must remove all breakpoints registered to the target as a previous
 749          * GDB session could leave dangling breakpoints if e.g. communication
 750          * timed out.
 751          */
 752         breakpoint_clear_target(gdb_service->target);
 753         watchpoint_clear_target(gdb_service->target);
 754
 755         /* register callback to be informed about target events */
 756         target_register_event_callback(gdb_target_callback_event_handler, connection);
 757
 758         /* a gdb session just attached, try to put the target in halt mode.
 759          *
 760          * DANGER!!!!
 761          *
 762          * If the halt fails(e.g. target needs a reset, JTAG communication not
 763          * working, etc.), then the GDB connect will succeed as
 764          * the get_gdb_reg_list() will lie and return a register list with
 765          * dummy values.
 766          *
 767          * This allows GDB monitor commands to be run from a GDB init script to
 768          * initialize the target
 769          *
 770          * Also, since the halt() is asynchronous target connect will be
 771          * instantaneous and thus avoiding annoying timeout problems during
 772          * connect.
 773          */
 774         target_halt(gdb_service->target);
 775         /* FIX!!!! could extended-remote work better here?
 776          *
 777          *  wait a tiny bit for halted state or we just continue. The
 778          * GDB register packet will then contain garbage
 779          */
 780         target_wait_state(gdb_service->target, TARGET_HALTED, 500);
 781
 782         /* remove the initial ACK from the incoming buffer */
 783         if ((retval = gdb_get_char(connection, &initial_ack)) != ERROR_OK)
 784                 return retval;
 785
 786         /* FIX!!!??? would we actually ever receive a + here???
 787          * Not observed.
 788          */
 789         if (initial_ack != '+')
 790                 gdb_putback_char(connection, initial_ack);
 791         target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_ATTACH );
 792         return ERROR_OK;
 793 }
 794
 795 int gdb_connection_closed(connection_t *connection)
 796 {
 797         gdb_service_t *gdb_service = connection->service->priv;
 798         gdb_connection_t *gdb_connection = connection->priv;
 799
 800         /* see if an image built with vFlash commands is left */
 801         if (gdb_connection->vflash_image)
 802         {
 803                 image_close(gdb_connection->vflash_image);
 804                 free(gdb_connection->vflash_image);
 805                 gdb_connection->vflash_image = NULL;
 806         }
 807
 808         /* if this connection registered a debug-message receiver delete it */
 809         delete_debug_msg_receiver(connection->cmd_ctx, gdb_service->target);
 810
 811         if (connection->priv)
 812         {
 813                 free(connection->priv);
 814                 connection->priv = NULL;
 815         }
 816         else
 817         {
 818                 LOG_ERROR("BUG: connection->priv == NULL");
 819         }
 820
 821         target_unregister_event_callback(gdb_target_callback_event_handler, connection);
 822         target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_END);
 823         log_remove_callback(gdb_log_callback, connection);
 824
 825         target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_DETACH );
 826
 827         return ERROR_OK;
 828 }
 829
 830 void gdb_send_error(connection_t *connection, u8 the_error)
 831 {
 832         char err[4];
 833         snprintf(err, 4, "E%2.2X", the_error );
 834         gdb_put_packet(connection, err, 3);
 835 }
 836
 837 int gdb_last_signal_packet(connection_t *connection, target_t *target, char* packet, int packet_size)
 838 {
 839         char sig_reply[4];
 840         int signal;
 841
 842         signal = gdb_last_signal(target);
 843
 844         snprintf(sig_reply, 4, "S%2.2x", signal);
 845         gdb_put_packet(connection, sig_reply, 3);
 846
 847         return ERROR_OK;
 848 }
 849
 850
 851 static int gdb_reg_pos(target_t *target, int pos, int len)
 852 {
 853         if (target->endianness == TARGET_LITTLE_ENDIAN)
 854                 return pos;
 855         else
 856                 return len - 1 - pos;
 857 }
 858
 859 /* Convert register to string of bytes. NB! The # of bits in the
 860  * register might be non-divisible by 8(a byte), in which
 861  * case an entire byte is shown.
 862  *
 863  * NB! the format on the wire is the target endianess
 864  *
 865  * The format of reg->value is little endian
 866  *
 867  */
 868 void gdb_str_to_target(target_t *target, char *tstr, reg_t *reg)
 869 {
 870         int i;
 871
 872         u8 *buf;
 873         int buf_len;
 874         buf = reg->value;
 875         buf_len = CEIL(reg->size, 8);
 876
 877         for (i = 0; i < buf_len; i++)
 878         {
 879                 int j = gdb_reg_pos(target, i, buf_len);
 880                 tstr[i*2]   = DIGITS[(buf[j]>>4) & 0xf];
 881                 tstr[i*2+1] = DIGITS[buf[j]&0xf];
 882         }
 883 }
 884
 885 static int hextoint(char c)
 886 {
 887         if (c>='0'&&c<='9')
 888         {
 889                 return c-'0';
 890         }
 891         c=toupper(c);
 892         if (c>='A'&&c<='F')
 893         {
 894                 return c-'A'+10;
 895         }
 896         LOG_ERROR("BUG: invalid register value %08x", c);
 897         return 0;
 898 }
 899
 900 /* copy over in register buffer */
 901 void gdb_target_to_reg(target_t *target, char *tstr, int str_len, u8 *bin)
 902 {
 903         if (str_len % 2)
 904         {
 905                 LOG_ERROR("BUG: gdb value with uneven number of characters encountered");
 906                 exit(-1);
 907         }
 908
 909         int i;
 910         for (i = 0; i < str_len; i+=2)
 911         {
 912                 u8 t = hextoint(tstr[i])<<4;
 913                 t |= hextoint(tstr[i]);
 914
 915                 int j = gdb_reg_pos(target, i/2, str_len/2);
 916                 bin[j] = t;
 917         }
 918 }
 919
 920 int gdb_get_registers_packet(connection_t *connection, target_t *target, char* packet, int packet_size)
 921 {
 922         reg_t **reg_list;
 923         int reg_list_size;
 924         int retval;
 925         int reg_packet_size = 0;
 926         char *reg_packet;
 927         char *reg_packet_p;
 928         int i;
 929
 930 #ifdef _DEBUG_GDB_IO_
 931         LOG_DEBUG("-");
 932 #endif
 933
 934         if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
 935         {
 936                 return gdb_error(connection, retval);
 937         }
 938
 939         for (i = 0; i < reg_list_size; i++)
 940         {
 941                 reg_packet_size += reg_list[i]->size;
 942         }
 943
 944         reg_packet = malloc(CEIL(reg_packet_size, 8) * 2);
 945         reg_packet_p = reg_packet;
 946
 947         for (i = 0; i < reg_list_size; i++)
 948         {
 949                 gdb_str_to_target(target, reg_packet_p, reg_list[i]);
 950                 reg_packet_p += CEIL(reg_list[i]->size, 8) * 2;
 951         }
 952
 953 #ifdef _DEBUG_GDB_IO_
 954         {
 955                 char *reg_packet_p;
 956                 reg_packet_p = strndup(reg_packet, CEIL(reg_packet_size, 8) * 2);
 957                 LOG_DEBUG("reg_packet: %s", reg_packet_p);
 958                 free(reg_packet_p);
 959         }
 960 #endif
 961
 962         gdb_put_packet(connection, reg_packet, CEIL(reg_packet_size, 8) * 2);
 963         free(reg_packet);
 964
 965         free(reg_list);
 966
 967         return ERROR_OK;
 968 }
 969
 970 int gdb_set_registers_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 971 {
 972         int i;
 973         reg_t **reg_list;
 974         int reg_list_size;
 975         int retval;
 976         char *packet_p;
 977
 978 #ifdef _DEBUG_GDB_IO_
 979         LOG_DEBUG("-");
 980 #endif
 981
 982         /* skip command character */
 983         packet++;
 984         packet_size--;
 985
 986         if (packet_size % 2)
 987         {
 988                 LOG_WARNING("GDB set_registers packet with uneven characters received, dropping connection");
 989                 return ERROR_SERVER_REMOTE_CLOSED;
 990         }
 991
 992         if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
 993         {
 994                 return gdb_error(connection, retval);
 995         }
 996
 997         packet_p = packet;
 998         for (i = 0; i < reg_list_size; i++)
 999         {
1000                 u8 *bin_buf;
1001                 int chars = (CEIL(reg_list[i]->size, 8) * 2);
1002
1003                 if (packet_p + chars > packet + packet_size)
1004                 {
1005                         LOG_ERROR("BUG: register packet is too small for registers");
1006                 }
1007
1008                 reg_arch_type_t *arch_type;
1009                 bin_buf = malloc(CEIL(reg_list[i]->size, 8));
1010                 gdb_target_to_reg(target, packet_p, chars, bin_buf);
1011
1012                 /* get register arch_type, and call set method */
1013                 arch_type = register_get_arch_type(reg_list[i]->arch_type);
1014
1015                 arch_type->set(reg_list[i], bin_buf);
1016
1017                 /* advance packet pointer */
1018                 packet_p += chars;
1019
1020
1021                 free(bin_buf);
1022         }
1023
1024         /* free reg_t *reg_list[] array allocated by get_gdb_reg_list */
1025         free(reg_list);
1026
1027         gdb_put_packet(connection, "OK", 2);
1028
1029         return ERROR_OK;
1030 }
1031
1032 int gdb_get_register_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
1033 {
1034         char *reg_packet;
1035         int reg_num = strtoul(packet + 1, NULL, 16);
1036         reg_t **reg_list;
1037         int reg_list_size;
1038         int retval;
1039
1040 #ifdef _DEBUG_GDB_IO_
1041         LOG_DEBUG("-");
1042 #endif
1043
1044         if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
1045         {
1046                 return gdb_error(connection, retval);
1047         }
1048
1049         if (reg_list_size <= reg_num)
1050         {
1051                 LOG_ERROR("gdb requested a non-existing register");
1052                 exit(-1);
1053         }
1054
1055         reg_packet = malloc(CEIL(reg_list[reg_num]->size, 8) * 2);
1056
1057         gdb_str_to_target(target, reg_packet, reg_list[reg_num]);
1058
1059         gdb_put_packet(connection, reg_packet, CEIL(reg_list[reg_num]->size, 8) * 2);
1060
1061         free(reg_list);
1062         free(reg_packet);
1063
1064         return ERROR_OK;
1065 }
1066
1067 int gdb_set_register_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
1068 {
1069         char *separator;
1070         u8 *bin_buf;
1071         int reg_num = strtoul(packet + 1, &separator, 16);
1072         reg_t **reg_list;
1073         int reg_list_size;
1074         int retval;
1075         reg_arch_type_t *arch_type;
1076
1077         LOG_DEBUG("-");
1078
1079         if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
1080         {
1081                 return gdb_error(connection, retval);
1082         }
1083
1084         if (reg_list_size < reg_num)
1085         {
1086                 LOG_ERROR("gdb requested a non-existing register");
1087                 return ERROR_SERVER_REMOTE_CLOSED;
1088         }
1089
1090         if (*separator != '=')
1091         {
1092                 LOG_ERROR("GDB 'set register packet', but no '=' following the register number");
1093                 return ERROR_SERVER_REMOTE_CLOSED;
1094         }
1095
1096         /* convert from GDB-string (target-endian) to hex-string (big-endian) */
1097         bin_buf = malloc(CEIL(reg_list[reg_num]->size, 8));
1098         int chars = (CEIL(reg_list[reg_num]->size, 8) * 2);
1099
1100         /* fix!!! add some sanity checks on packet size here */
1101
1102         gdb_target_to_reg(target, separator + 1, chars, bin_buf);
1103
1104                 /* get register arch_type, and call set method */
1105         arch_type = register_get_arch_type(reg_list[reg_num]->arch_type);
1106         arch_type->set(reg_list[reg_num], bin_buf);
1107
1108         gdb_put_packet(connection, "OK", 2);
1109
1110         free(bin_buf);
1111         free(reg_list);
1112
1113         return ERROR_OK;
1114 }
1115
1116 int gdb_error(connection_t *connection, int retval)
1117 {
1118         switch (retval)
1119         {
1120                 case ERROR_TARGET_DATA_ABORT:
1121                         gdb_send_error(connection, EIO);
1122                         break;
1123                 case ERROR_TARGET_TRANSLATION_FAULT:
1124                         gdb_send_error(connection, EFAULT);
1125                         break;
1126                 case ERROR_TARGET_UNALIGNED_ACCESS:
1127                         gdb_send_error(connection, EFAULT);
1128                         break;
1129                 case ERROR_TARGET_NOT_HALTED:
1130                         gdb_send_error(connection, EFAULT);
1131                         break;
1132                 default:
1133                         /* This could be that the target reset itself. */
1134                         LOG_ERROR("unexpected error %i", retval);
1135                         gdb_send_error(connection, EFAULT);
1136                         break;
1137         }
1138
1139         return ERROR_OK;
1140 }
1141
1142 /* We don't have to worry about the default 2 second timeout for GDB packets,
1143  * because GDB breaks up large memory reads into smaller reads.
1144  *
1145  * 8191 bytes by the looks of it. Why 8191 bytes instead of 8192?????
1146  */
1147 int gdb_read_memory_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
1148 {
1149         char *separator;
1150         u32 addr = 0;
1151         u32 len = 0;
1152
1153         u8 *buffer;
1154         char *hex_buffer;
1155
1156         int retval = ERROR_OK;
1157
1158         /* skip command character */
1159         packet++;
1160
1161         addr = strtoul(packet, &separator, 16);
1162
1163         if (*separator != ',')
1164         {
1165                 LOG_ERROR("incomplete read memory packet received, dropping connection");
1166                 return ERROR_SERVER_REMOTE_CLOSED;
1167         }
1168
1169         len = strtoul(separator+1, NULL, 16);
1170
1171         buffer = malloc(len);
1172
1173         LOG_DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
1174
1175         retval = target_read_buffer(target, addr, len, buffer);
1176
1177         if ((retval!=ERROR_OK)&&!gdb_report_data_abort)
1178         {
1179                 /* TODO : Here we have to lie and send back all zero's lest stack traces won't work.
1180                  * At some point this might be fixed in GDB, in which case this code can be removed.
1181                  *
1182                  * OpenOCD developers are acutely aware of this problem, but there is nothing
1183                  * gained by involving the user in this problem that hopefully will get resolved
1184                  * eventually
1185                  *
1186                  * http://sourceware.org/cgi-bin/gnatsweb.pl?cmd=view%20audit-trail&database=gdb&pr=2395
1187                  *
1188                  * For now, the default is to fix up things to make current GDB versions work.
1189                  * This can be overwritten using the gdb_report_data_abort <'enable'|'disable'> command.
1190                  */
1191                 memset(buffer, 0, len);
1192                 retval = ERROR_OK;
1193         }
1194
1195         if (retval == ERROR_OK)
1196         {
1197                 hex_buffer = malloc(len * 2 + 1);
1198
1199                 int i;
1200                 for (i = 0; i < len; i++)
1201                 {
1202                         u8 t = buffer[i];
1203                         hex_buffer[2 * i] = DIGITS[(t >> 4) & 0xf];
1204                         hex_buffer[2 * i + 1] = DIGITS[t & 0xf];
1205                 }
1206
1207                 gdb_put_packet(connection, hex_buffer, len * 2);
1208
1209                 free(hex_buffer);
1210         }
1211         else
1212         {
1213                 retval = gdb_error(connection, retval);
1214         }
1215
1216         free(buffer);
1217
1218         return retval;
1219 }
1220
1221 int gdb_write_memory_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
1222 {
1223         char *separator;
1224         u32 addr = 0;
1225         u32 len = 0;
1226
1227         u8 *buffer;
1228
1229         int i;
1230         int retval;
1231
1232         /* skip command character */
1233         packet++;
1234
1235         addr = strtoul(packet, &separator, 16);
1236
1237         if (*separator != ',')
1238         {
1239                 LOG_ERROR("incomplete write memory packet received, dropping connection");
1240                 return ERROR_SERVER_REMOTE_CLOSED;
1241         }
1242
1243         len = strtoul(separator+1, &separator, 16);
1244
1245         if (*(separator++) != ':')
1246         {
1247                 LOG_ERROR("incomplete write memory packet received, dropping connection");
1248                 return ERROR_SERVER_REMOTE_CLOSED;
1249         }
1250
1251         buffer = malloc(len);
1252
1253         LOG_DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
1254
1255         for (i=0; i<len; i++)
1256         {
1257                 u32 tmp;
1258                 sscanf(separator + 2*i, "%2x", &tmp);
1259                 buffer[i] = tmp;
1260         }
1261
1262         retval = target_write_buffer(target, addr, len, buffer);
1263
1264         if (retval == ERROR_OK)
1265         {
1266                 gdb_put_packet(connection, "OK", 2);
1267         }
1268         else
1269         {
1270                 retval = gdb_error(connection, retval);
1271         }
1272
1273         free(buffer);
1274
1275         return retval;
1276 }
1277
1278 int gdb_write_memory_binary_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
1279 {
1280         char *separator;
1281         u32 addr = 0;
1282         u32 len = 0;
1283
1284         int retval;
1285
1286         /* skip command character */
1287         packet++;
1288
1289         addr = strtoul(packet, &separator, 16);
1290
1291         if (*separator != ',')
1292         {
1293                 LOG_ERROR("incomplete write memory binary packet received, dropping connection");
1294                 return ERROR_SERVER_REMOTE_CLOSED;
1295         }
1296
1297         len = strtoul(separator+1, &separator, 16);
1298
1299         if (*(separator++) != ':')
1300         {
1301                 LOG_ERROR("incomplete write memory binary packet received, dropping connection");
1302                 return ERROR_SERVER_REMOTE_CLOSED;
1303         }
1304
1305         retval = ERROR_OK;
1306         if (len)
1307         {
1308                 LOG_DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
1309
1310                 retval = target_write_buffer(target, addr, len, (u8*)separator);
1311         }
1312
1313         if (retval == ERROR_OK)
1314         {
1315                 gdb_put_packet(connection, "OK", 2);
1316         }
1317         else
1318         {
1319                 if ((retval = gdb_error(connection, retval)) != ERROR_OK)
1320                         return retval;
1321         }
1322
1323         return ERROR_OK;
1324 }
1325
1326 int gdb_step_continue_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
1327 {
1328         int current = 0;
1329         u32 address = 0x0;
1330         int retval=ERROR_OK;
1331
1332         LOG_DEBUG("-");
1333
1334         if (packet_size > 1)
1335         {
1336                 packet[packet_size] = 0;
1337                 address = strtoul(packet + 1, NULL, 16);
1338         }
1339         else
1340         {
1341                 current = 1;
1342         }
1343
1344         if (packet[0] == 'c')
1345         {
1346                 LOG_DEBUG("continue");
1347                 target_handle_event( target, TARGET_EVENT_OLD_pre_resume );
1348                 retval=target_resume(target, current, address, 0, 0); /* resume at current address, don't handle breakpoints, not debugging */
1349         }
1350         else if (packet[0] == 's')
1351         {
1352                 LOG_DEBUG("step");
1353                 retval=target->type->step(target, current, address, 0); /* step at current or address, don't handle breakpoints */
1354         }
1355         return retval;
1356 }
1357
1358 int gdb_breakpoint_watchpoint_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
1359 {
1360         int type;
1361         enum breakpoint_type bp_type = BKPT_SOFT /* dummy init to avoid warning */;
1362         enum watchpoint_rw wp_type;
1363         u32 address;
1364         u32 size;
1365         char *separator;
1366         int retval;
1367
1368         LOG_DEBUG("-");
1369
1370         type = strtoul(packet + 1, &separator, 16);
1371
1372         if (type == 0)  /* memory breakpoint */
1373                 bp_type = BKPT_SOFT;
1374         else if (type == 1) /* hardware breakpoint */
1375                 bp_type = BKPT_HARD;
1376         else if (type == 2) /* write watchpoint */
1377                 wp_type = WPT_WRITE;
1378         else if (type == 3) /* read watchpoint */
1379                 wp_type = WPT_READ;
1380         else if (type == 4) /* access watchpoint */
1381                 wp_type = WPT_ACCESS;
1382
1383         if (gdb_breakpoint_override&&((bp_type==BKPT_SOFT)||(bp_type==BKPT_HARD)))
1384         {
1385                 bp_type=gdb_breakpoint_override_type;
1386         }
1387
1388         if (*separator != ',')
1389         {
1390                 LOG_ERROR("incomplete breakpoint/watchpoint packet received, dropping connection");
1391                 return ERROR_SERVER_REMOTE_CLOSED;
1392         }
1393
1394         address = strtoul(separator+1, &separator, 16);
1395
1396         if (*separator != ',')
1397         {
1398                 LOG_ERROR("incomplete breakpoint/watchpoint packet received, dropping connection");
1399                 return ERROR_SERVER_REMOTE_CLOSED;
1400         }
1401
1402         size = strtoul(separator+1, &separator, 16);
1403
1404         switch (type)
1405         {
1406                 case 0:
1407                 case 1:
1408                         if (packet[0] == 'Z')
1409                         {
1410                                 if ((retval = breakpoint_add(target, address, size, bp_type)) != ERROR_OK)
1411                                 {
1412                                         if ((retval = gdb_error(connection, retval)) != ERROR_OK)
1413                                                 return retval;
1414                                 }
1415                                 else
1416                                 {
1417                                         gdb_put_packet(connection, "OK", 2);
1418                                 }
1419                         }
1420                         else
1421                         {
1422                                 breakpoint_remove(target, address);
1423                                 gdb_put_packet(connection, "OK", 2);
1424                         }
1425                         break;
1426                 case 2:
1427                 case 3:
1428                 case 4:
1429                 {
1430                         if (packet[0] == 'Z')
1431                         {
1432                                 if ((retval = watchpoint_add(target, address, size, type-2, 0, 0xffffffffu)) != ERROR_OK)
1433                                 {
1434                                         if ((retval = gdb_error(connection, retval)) != ERROR_OK)
1435                                                 return retval;
1436                                 }
1437                                 else
1438                                 {
1439                                         gdb_put_packet(connection, "OK", 2);
1440                                 }
1441                         }
1442                         else
1443                         {
1444                                 watchpoint_remove(target, address);
1445                                 gdb_put_packet(connection, "OK", 2);
1446                         }
1447                         break;
1448                 }
1449                 default:
1450                         break;
1451         }
1452
1453         return ERROR_OK;
1454 }
1455
1456 /* print out a string and allocate more space as needed, mainly used for XML at this point */
1457 void xml_printf(int *retval, char **xml, int *pos, int *size, const char *fmt, ...)
1458 {
1459         if (*retval != ERROR_OK)
1460         {
1461                 return;
1462         }
1463         int first = 1;
1464
1465         for (;;)
1466         {
1467                 if ((*xml == NULL) || (!first))
1468                 {
1469                         /* start by 0 to exercise all the code paths.
1470                          * Need minimum 2 bytes to fit 1 char and 0 terminator. */
1471
1472                         *size = *size * 2 + 2;
1473                         char *t = *xml;
1474                         *xml = realloc(*xml, *size);
1475                         if (*xml == NULL)
1476                         {
1477                                 if (t)
1478                                         free(t);
1479                                 *retval = ERROR_SERVER_REMOTE_CLOSED;
1480                                 return;
1481                         }
1482                 }
1483
1484                 va_list ap;
1485                 int ret;
1486                 va_start(ap, fmt);
1487                 ret = vsnprintf(*xml + *pos, *size - *pos, fmt, ap);
1488                 va_end(ap);
1489                 if ((ret > 0) && ((ret + 1) < *size - *pos))
1490                 {
1491                         *pos += ret;
1492                         return;
1493                 }
1494                 /* there was just enough or not enough space, allocate more. */
1495                 first = 0;
1496         }
1497 }
1498
1499 static int decode_xfer_read(char *buf, char **annex, int *ofs, unsigned int *len)
1500 {
1501         char *separator;
1502
1503         /* Extract and NUL-terminate the annex. */
1504         *annex = buf;
1505         while (*buf && *buf != ':')
1506                 buf++;
1507         if (*buf == '\0')
1508                 return -1;
1509         *buf++ = 0;
1510
1511         /* After the read marker and annex, qXfer looks like a
1512          * traditional 'm' packet. */
1513
1514         *ofs = strtoul(buf, &separator, 16);
1515
1516         if (*separator != ',')
1517                 return -1;
1518
1519         *len = strtoul(separator+1, NULL, 16);
1520
1521         return 0;
1522 }
1523
1524 int gdb_calc_blocksize(flash_bank_t *bank)
1525 {
1526         int i;
1527         int block_size = 0xffffffff;
1528
1529         /* loop through all sectors and return smallest sector size */
1530
1531         for (i = 0; i < bank->num_sectors; i++)
1532         {
1533                 if (bank->sectors[i].size < block_size)
1534                         block_size = bank->sectors[i].size;
1535         }
1536
1537         return block_size;
1538 }
1539
1540 static int compare_bank (const void * a, const void * b)
1541 {
1542         flash_bank_t *b1, *b2;
1543         b1=*((flash_bank_t **)a);
1544         b2=*((flash_bank_t **)b);
1545
1546         if (b1->base==b2->base)
1547         {
1548                 return 0;
1549         } else if (b1->base>b2->base)
1550         {
1551                 return 1;
1552         } else
1553         {
1554                 return -1;
1555         }
1556 }
1557
1558 int gdb_query_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
1559 {
1560         command_context_t *cmd_ctx = connection->cmd_ctx;
1561         gdb_connection_t *gdb_connection = connection->priv;
1562
1563         if (strstr(packet, "qRcmd,"))
1564         {
1565                 if (packet_size > 6)
1566                 {
1567                         char *cmd;
1568                         int i;
1569                         cmd = malloc((packet_size - 6)/2 + 1);
1570                         for (i=0; i < (packet_size - 6)/2; i++)
1571                         {
1572                                 u32 tmp;
1573                                 sscanf(packet + 6 + 2*i, "%2x", &tmp);
1574                                 cmd[i] = tmp;
1575                         }
1576                         cmd[(packet_size - 6)/2] = 0x0;
1577
1578                         /* We want to print all debug output to GDB connection */
1579                         log_add_callback(gdb_log_callback, connection);
1580                         target_call_timer_callbacks_now();
1581                         command_run_line(cmd_ctx, cmd);
1582                         target_call_timer_callbacks_now();
1583                         log_remove_callback(gdb_log_callback, connection);
1584                         free(cmd);
1585                 }
1586                 gdb_put_packet(connection, "OK", 2);
1587                 return ERROR_OK;
1588         }
1589         else if (strstr(packet, "qCRC:"))
1590         {
1591                 if (packet_size > 5)
1592                 {
1593                         int retval;
1594                         char gdb_reply[10];
1595                         char *separator;
1596                         u32 checksum;
1597                         u32 addr = 0;
1598                         u32 len = 0;
1599
1600                         /* skip command character */
1601                         packet += 5;
1602
1603                         addr = strtoul(packet, &separator, 16);
1604
1605                         if (*separator != ',')
1606                         {
1607                                 LOG_ERROR("incomplete read memory packet received, dropping connection");
1608                                 return ERROR_SERVER_REMOTE_CLOSED;
1609                         }
1610
1611                         len = strtoul(separator + 1, NULL, 16);
1612
1613                         retval = target_checksum_memory(target, addr, len, &checksum);
1614
1615                         if (retval == ERROR_OK)
1616                         {
1617                                 snprintf(gdb_reply, 10, "C%8.8x", checksum);
1618                                 gdb_put_packet(connection, gdb_reply, 9);
1619                         }
1620                         else
1621                         {
1622                                 if ((retval = gdb_error(connection, retval)) != ERROR_OK)
1623                                         return retval;
1624                         }
1625
1626                         return ERROR_OK;
1627                 }
1628         }
1629         else if (strstr(packet, "qSupported"))
1630         {
1631                 /* we currently support packet size and qXfer:memory-map:read (if enabled)
1632                  * disable qXfer:features:read for the moment */
1633                 int retval = ERROR_OK;
1634                 char *buffer = NULL;
1635                 int pos = 0;
1636                 int size = 0;
1637
1638                 xml_printf(&retval, &buffer, &pos, &size,
1639                                 "PacketSize=%x;qXfer:memory-map:read%c;qXfer:features:read-;QStartNoAckMode+",
1640                                 (GDB_BUFFER_SIZE - 1), ((gdb_use_memory_map == 1)&&(flash_get_bank_count()>0)) ? '+' : '-');
1641
1642                 if (retval != ERROR_OK)
1643                 {
1644                         gdb_send_error(connection, 01);
1645                         return ERROR_OK;
1646                 }
1647
1648                 gdb_put_packet(connection, buffer, strlen(buffer));
1649                 free(buffer);
1650
1651                 return ERROR_OK;
1652         }
1653         else if (strstr(packet, "qXfer:memory-map:read::")&&(flash_get_bank_count()>0))
1654         {
1655                 /* We get away with only specifying flash here. Regions that are not
1656                  * specified are treated as if we provided no memory map(if not we
1657                  * could detect the holes and mark them as RAM).
1658                  * Normally we only execute this code once, but no big deal if we
1659                  * have to regenerate it a couple of times. */
1660
1661                 flash_bank_t *p;
1662                 char *xml = NULL;
1663                 int size = 0;
1664                 int pos = 0;
1665                 int retval = ERROR_OK;
1666
1667                 int offset;
1668                 int length;
1669                 char *separator;
1670                 int blocksize;
1671
1672                 /* skip command character */
1673                 packet += 23;
1674
1675                 offset = strtoul(packet, &separator, 16);
1676                 length = strtoul(separator + 1, &separator, 16);
1677
1678                 xml_printf(&retval, &xml, &pos, &size, "<memory-map>\n");
1679
1680                 /*
1681                 sort banks in ascending order, we need to make non-flash memory be ram(or rather
1682                 read/write) by default for GDB.
1683                 GDB does not have a concept of non-cacheable read/write memory.
1684                  */
1685                 flash_bank_t **banks=malloc(sizeof(flash_bank_t *)*flash_get_bank_count());
1686                 int i;
1687
1688                 for (i=0; i<flash_get_bank_count(); i++)
1689                 {
1690                         p = get_flash_bank_by_num(i);
1691                         if (p == NULL)
1692                         {
1693                                 free(banks);
1694                                 retval = ERROR_FAIL;
1695                                 gdb_send_error(connection, retval);
1696                                 return retval;
1697                         }
1698                         banks[i]=p;
1699                 }
1700
1701                 qsort(banks, flash_get_bank_count(), sizeof(flash_bank_t *), compare_bank);
1702
1703                 u32 ram_start=0;
1704                 for (i=0; i<flash_get_bank_count(); i++)
1705                 {
1706                         p = banks[i];
1707
1708                         if (ram_start<p->base)
1709                         {
1710                                 xml_printf(&retval, &xml, &pos, &size, "<memory type=\"ram\" start=\"0x%x\" length=\"0x%x\"/>\n",
1711                                         ram_start, p->base-ram_start);
1712                         }
1713
1714                         /* if device has uneven sector sizes, eg. str7, lpc
1715                          * we pass the smallest sector size to gdb memory map */
1716                         blocksize = gdb_calc_blocksize(p);
1717
1718                         xml_printf(&retval, &xml, &pos, &size, "<memory type=\"flash\" start=\"0x%x\" length=\"0x%x\">\n" \
1719                                 "<property name=\"blocksize\">0x%x</property>\n" \
1720                                 "</memory>\n", \
1721                                 p->base, p->size, blocksize);
1722                         ram_start=p->base+p->size;
1723                 }
1724                 if (ram_start!=0)
1725                 {
1726                         xml_printf(&retval, &xml, &pos, &size, "<memory type=\"ram\" start=\"0x%x\" length=\"0x%x\"/>\n",
1727                                 ram_start, 0-ram_start);
1728                 } else
1729                 {
1730                         /* a flash chip could be at the very end of the 32 bit address space, in which case
1731                         ram_start will be precisely 0 */
1732                 }
1733
1734                 free(banks);
1735                 banks = NULL;
1736
1737                 xml_printf(&retval, &xml, &pos, &size, "</memory-map>\n");
1738
1739                 if (retval != ERROR_OK)
1740                 {
1741                         gdb_send_error(connection, retval);
1742                         return retval;
1743                 }
1744
1745                 if (offset + length > pos)
1746                 {
1747                         length = pos - offset;
1748                 }
1749
1750                 char *t = malloc(length + 1);
1751                 t[0] = 'l';
1752                 memcpy(t + 1, xml + offset, length);
1753                 gdb_put_packet(connection, t, length + 1);
1754
1755                 free(t);
1756                 free(xml);
1757                 return ERROR_OK;
1758         }
1759         else if (strstr(packet, "qXfer:features:read:"))
1760         {
1761                 char *xml = NULL;
1762                 int size = 0;
1763                 int pos = 0;
1764                 int retval = ERROR_OK;
1765
1766                 int offset;
1767                 unsigned int length;
1768                 char *annex;
1769
1770                 /* skip command character */
1771                 packet += 20;
1772
1773                 if (decode_xfer_read(packet, &annex, &offset, &length) < 0)
1774                 {
1775                         gdb_send_error(connection, 01);
1776                         return ERROR_OK;
1777                 }
1778
1779                 if (strcmp(annex, "target.xml") != 0)
1780                 {
1781                         gdb_send_error(connection, 01);
1782                         return ERROR_OK;
1783                 }
1784
1785                 xml_printf(&retval, &xml, &pos, &size, \
1786                         "l<target version=\"1.0\">\n<architecture>arm</architecture>\n</target>\n");
1787
1788                 if (retval != ERROR_OK)
1789                 {
1790                         gdb_send_error(connection, retval);
1791                         return retval;
1792                 }
1793
1794                 gdb_put_packet(connection, xml, strlen(xml));
1795
1796                 free(xml);
1797                 return ERROR_OK;
1798         }
1799         else if (strstr(packet, "QStartNoAckMode"))
1800         {
1801                 gdb_connection->noack_mode = 1;
1802                 gdb_put_packet(connection, "OK", 2);
1803                 return ERROR_OK;
1804         }
1805
1806         gdb_put_packet(connection, "", 0);
1807         return ERROR_OK;
1808 }
1809
1810 int gdb_v_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
1811 {
1812         gdb_connection_t *gdb_connection = connection->priv;
1813         gdb_service_t *gdb_service = connection->service->priv;
1814         int result;
1815
1816         /* if flash programming disabled - send a empty reply */
1817
1818         if (gdb_flash_program == 0)
1819         {
1820                 gdb_put_packet(connection, "", 0);
1821                 return ERROR_OK;
1822         }
1823
1824         if (strstr(packet, "vFlashErase:"))
1825         {
1826                 unsigned long addr;
1827                 unsigned long length;
1828
1829                 char *parse = packet + 12;
1830                 if (*parse == '\0')
1831                 {
1832                         LOG_ERROR("incomplete vFlashErase packet received, dropping connection");
1833                         return ERROR_SERVER_REMOTE_CLOSED;
1834                 }
1835
1836                 addr = strtoul(parse, &parse, 16);
1837
1838                 if (*(parse++) != ',' || *parse == '\0')
1839                 {
1840                         LOG_ERROR("incomplete vFlashErase packet received, dropping connection");
1841                         return ERROR_SERVER_REMOTE_CLOSED;
1842                 }
1843
1844                 length = strtoul(parse, &parse, 16);
1845
1846                 if (*parse != '\0')
1847                 {
1848                         LOG_ERROR("incomplete vFlashErase packet received, dropping connection");
1849                         return ERROR_SERVER_REMOTE_CLOSED;
1850                 }
1851
1852                 /* assume all sectors need erasing - stops any problems
1853                  * when flash_write is called multiple times */
1854                 flash_set_dirty();
1855
1856                 /* perform any target specific operations before the erase */
1857                 target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_FLASH_ERASE_START);
1858                 result = flash_erase_address_range(gdb_service->target, addr, length );
1859                 target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_FLASH_ERASE_END);
1860
1861                 /* perform erase */
1862                 if (result != ERROR_OK)
1863                 {
1864                         /* GDB doesn't evaluate the actual error number returned,
1865                          * treat a failed erase as an I/O error
1866                          */
1867                         gdb_send_error(connection, EIO);
1868                         LOG_ERROR("flash_erase returned %i", result);
1869                 }
1870                 else
1871                         gdb_put_packet(connection, "OK", 2);
1872
1873                 return ERROR_OK;
1874         }
1875
1876         if (strstr(packet, "vFlashWrite:"))
1877         {
1878                 int retval;
1879                 unsigned long addr;
1880                 unsigned long length;
1881                 char *parse = packet + 12;
1882
1883                 if (*parse == '\0')
1884                 {
1885                         LOG_ERROR("incomplete vFlashErase packet received, dropping connection");
1886                         return ERROR_SERVER_REMOTE_CLOSED;
1887                 }
1888                 addr = strtoul(parse, &parse, 16);
1889                 if (*(parse++) != ':')
1890                 {
1891                         LOG_ERROR("incomplete vFlashErase packet received, dropping connection");
1892                         return ERROR_SERVER_REMOTE_CLOSED;
1893                 }
1894                 length = packet_size - (parse - packet);
1895
1896                 /* create a new image if there isn't already one */
1897                 if (gdb_connection->vflash_image == NULL)
1898                 {
1899                         gdb_connection->vflash_image = malloc(sizeof(image_t));
1900                         image_open(gdb_connection->vflash_image, "", "build");
1901                 }
1902
1903                 /* create new section with content from packet buffer */
1904                 if((retval = image_add_section(gdb_connection->vflash_image, addr, length, 0x0, (u8*)parse)) != ERROR_OK)
1905                 {
1906                         return retval;
1907                 }
1908
1909                 gdb_put_packet(connection, "OK", 2);
1910
1911                 return ERROR_OK;
1912         }
1913
1914         if (!strcmp(packet, "vFlashDone"))
1915         {
1916                 u32 written;
1917
1918                 /* process the flashing buffer. No need to erase as GDB
1919                  * always issues a vFlashErase first. */
1920                 target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_FLASH_WRITE_START);
1921                 result = flash_write(gdb_service->target, gdb_connection->vflash_image, &written, 0);
1922                 target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_FLASH_WRITE_END);
1923                 if ( result != ERROR_OK)
1924                 {
1925                         if (result == ERROR_FLASH_DST_OUT_OF_BANK)
1926                                 gdb_put_packet(connection, "E.memtype", 9);
1927                         else
1928                                 gdb_send_error(connection, EIO);
1929                         }
1930                 else
1931                 {
1932                         LOG_DEBUG("wrote %u bytes from vFlash image to flash", written);
1933                         gdb_put_packet(connection, "OK", 2);
1934                 }
1935
1936                 image_close(gdb_connection->vflash_image);
1937                 free(gdb_connection->vflash_image);
1938                 gdb_connection->vflash_image = NULL;
1939
1940                 return ERROR_OK;
1941         }
1942
1943         gdb_put_packet(connection, "", 0);
1944         return ERROR_OK;
1945 }
1946
1947 int gdb_detach(connection_t *connection, target_t *target)
1948 {
1949
1950         switch( detach_mode )
1951         {
1952                 case GDB_DETACH_RESUME:
1953                         target_handle_event( target, TARGET_EVENT_OLD_pre_resume );
1954                         target_resume(target, 1, 0, 1, 0);
1955                         break;
1956
1957                 case GDB_DETACH_RESET:
1958                         /* FIX?? make this configurable?? */
1959                         target_process_reset(connection->cmd_ctx, RESET_HALT);
1960                         break;
1961
1962                 case GDB_DETACH_HALT:
1963                         target_halt(target);
1964                         break;
1965
1966                 case GDB_DETACH_NOTHING:
1967                         break;
1968         }
1969
1970         gdb_put_packet(connection, "OK", 2);
1971         return ERROR_OK;
1972 }
1973
1974 static void gdb_log_callback(void *priv, const char *file, int line,
1975                 const char *function, const char *string)
1976 {
1977         connection_t *connection = priv;
1978         gdb_connection_t *gdb_con = connection->priv;
1979
1980         if (gdb_con->busy)
1981         {
1982                 /* do not reply this using the O packet */
1983                 return;
1984         }
1985
1986         gdb_output_con(connection, string);
1987 }
1988
1989 /* Do not allocate this on the stack */
1990 char gdb_packet_buffer[GDB_BUFFER_SIZE];
1991
1992 static void gdb_sig_halted(connection_t *connection)
1993 {
1994         char sig_reply[4];
1995         snprintf(sig_reply, 4, "T%2.2x", 2);
1996         gdb_put_packet(connection, sig_reply, 3);
1997
1998 }
1999
2000 int gdb_input_inner(connection_t *connection)
2001 {
2002         gdb_service_t *gdb_service = connection->service->priv;
2003         target_t *target = gdb_service->target;
2004         char *packet=gdb_packet_buffer;
2005         int packet_size;
2006         int retval;
2007         gdb_connection_t *gdb_con = connection->priv;
2008         static int extended_protocol = 0;
2009
2010         /* drain input buffer */
2011         do
2012         {
2013                 packet_size = GDB_BUFFER_SIZE-1;
2014                 if ((retval = gdb_get_packet(connection, packet, &packet_size)) != ERROR_OK)
2015                 {
2016                         return retval;
2017                 }
2018
2019                 /* terminate with zero */
2020                 packet[packet_size] = 0;
2021
2022                 if( LOG_LEVEL_IS( LOG_LVL_DEBUG ) ){
2023                         if( packet[0] == 'X' ){
2024                                 // binary packets spew junk into the debug log stream
2025                                 char buf[ 50 ];
2026                                 int x;
2027                                 for( x = 0 ; (x < 49) && (packet[x] != ':') ; x++ ){
2028                                         buf[x] = packet[x];
2029                                 }
2030                                 buf[x] = 0;
2031                                 LOG_DEBUG("received packet: '%s:<binary-data>'", buf );
2032                         } else {
2033                                 LOG_DEBUG("received packet: '%s'", packet );
2034                         }
2035                 }
2036
2037                 if (packet_size > 0)
2038                 {
2039                         retval = ERROR_OK;
2040                         switch (packet[0])
2041                         {
2042                                 case 'H':
2043                                         /* Hct... -- set thread
2044                                          * we don't have threads, send empty reply */
2045                                         gdb_put_packet(connection, NULL, 0);
2046                                         break;
2047                                 case 'q':
2048                                 case 'Q':
2049                                         retval = gdb_query_packet(connection, target, packet, packet_size);
2050                                         break;
2051                                 case 'g':
2052                                         retval = gdb_get_registers_packet(connection, target, packet, packet_size);
2053                                         break;
2054                                 case 'G':
2055                                         retval = gdb_set_registers_packet(connection, target, packet, packet_size);
2056                                         break;
2057                                 case 'p':
2058                                         retval = gdb_get_register_packet(connection, target, packet, packet_size);
2059                                         break;
2060                                 case 'P':
2061                                         retval = gdb_set_register_packet(connection, target, packet, packet_size);
2062                                         break;
2063                                 case 'm':
2064                                         retval = gdb_read_memory_packet(connection, target, packet, packet_size);
2065                                         break;
2066                                 case 'M':
2067                                         retval = gdb_write_memory_packet(connection, target, packet, packet_size);
2068                                         break;
2069                                 case 'z':
2070                                 case 'Z':
2071                                         retval = gdb_breakpoint_watchpoint_packet(connection, target, packet, packet_size);
2072                                         break;
2073                                 case '?':
2074                                         gdb_last_signal_packet(connection, target, packet, packet_size);
2075                                         break;
2076                                 case 'c':
2077                                 case 's':
2078                                         {
2079                                                 if (target->state != TARGET_HALTED)
2080                                                 {
2081                                                         /* If the target isn't in the halted state, then we can't
2082                                                          * step/continue. This might be early setup, etc.
2083                                                          */
2084                                                         gdb_sig_halted(connection);
2085                                                 } else
2086                                                 {
2087                                                         /* We're running/stepping, in which case we can
2088                                                          * forward log output until the target is halted
2089                                                          */
2090                                                         gdb_connection_t *gdb_con = connection->priv;
2091                                                         gdb_con->frontend_state = TARGET_RUNNING;
2092                                                         log_add_callback(gdb_log_callback, connection);
2093                                                         target_call_event_callbacks(target, TARGET_EVENT_GDB_START);
2094                                                         int retval=gdb_step_continue_packet(connection, target, packet, packet_size);
2095                                                         if (retval!=ERROR_OK)
2096                                                         {
2097                                                                 /* we'll never receive a halted condition... issue a false one.. */
2098                                                                 gdb_frontend_halted(target, connection);
2099                                                         }
2100                                                 }
2101                                         }
2102                                         break;
2103                                 case 'v':
2104                                         retval = gdb_v_packet(connection, target, packet, packet_size);
2105                                         break;
2106                                 case 'D':
2107                                         retval = gdb_detach(connection, target);
2108                                         extended_protocol = 0;
2109                                         break;
2110                                 case 'X':
2111                                         if ((retval = gdb_write_memory_binary_packet(connection, target, packet, packet_size)) != ERROR_OK)
2112                                                 return retval;
2113                                         break;
2114                                 case 'k':
2115                                         if (extended_protocol != 0)
2116                                                 break;
2117                                         gdb_put_packet(connection, "OK", 2);
2118                                         return ERROR_SERVER_REMOTE_CLOSED;
2119                                 case '!':
2120                                         /* handle extended remote protocol */
2121                                         extended_protocol = 1;
2122                                         gdb_put_packet(connection, "OK", 2);
2123                                         break;
2124                                 case 'R':
2125                                         /* handle extended restart packet */
2126                                         breakpoint_clear_target(gdb_service->target);
2127                                         watchpoint_clear_target(gdb_service->target);
2128                                         command_run_linef(connection->cmd_ctx, "ocd_gdb_restart %d", get_num_by_target(target));
2129                                         break;
2130                                 default:
2131                                         /* ignore unkown packets */
2132                                         LOG_DEBUG("ignoring 0x%2.2x packet", packet[0]);
2133                                         gdb_put_packet(connection, NULL, 0);
2134                                         break;
2135                         }
2136
2137                         /* if a packet handler returned an error, exit input loop */
2138                         if (retval != ERROR_OK)
2139                                 return retval;
2140                 }
2141
2142                 if (gdb_con->ctrl_c)
2143                 {
2144                         if (target->state == TARGET_RUNNING)
2145                         {
2146                                 target_halt(target);
2147                                 gdb_con->ctrl_c = 0;
2148                         }
2149                 }
2150
2151         } while (gdb_con->buf_cnt > 0);
2152
2153         return ERROR_OK;
2154 }
2155
2156 int gdb_input(connection_t *connection)
2157 {
2158         int retval = gdb_input_inner(connection);
2159         gdb_connection_t *gdb_con = connection->priv;
2160         if (retval == ERROR_SERVER_REMOTE_CLOSED)
2161                 return retval;
2162
2163         /* logging does not propagate the error, yet can set th gdb_con->closed flag */
2164         if (gdb_con->closed)
2165                 return ERROR_SERVER_REMOTE_CLOSED;
2166
2167         /* we'll recover from any other errors(e.g. temporary timeouts, etc.) */
2168         return ERROR_OK;
2169 }
2170
2171 int gdb_init(void)
2172 {
2173         gdb_service_t *gdb_service;
2174         target_t *target = all_targets;
2175
2176         if (!target)
2177         {
2178                 LOG_WARNING("no gdb ports allocated as no target has been specified");
2179                 return ERROR_OK;
2180         }
2181
2182         if (gdb_port == 0)
2183         {
2184                 LOG_WARNING("no gdb port specified, using default port 3333");
2185                 gdb_port = 3333;
2186         }
2187
2188         while (target)
2189         {
2190                 char service_name[8];
2191
2192                 snprintf(service_name, 8, "gdb-%2.2i", target->target_number);
2193
2194                 gdb_service = malloc(sizeof(gdb_service_t));
2195                 gdb_service->target = target;
2196
2197                 add_service("gdb", CONNECTION_GDB,
2198                             gdb_port + target->target_number,
2199                             1, gdb_new_connection, gdb_input,
2200                             gdb_connection_closed,
2201                             gdb_service);
2202
2203                 LOG_DEBUG("gdb service for target %s at port %i",
2204                           target->type->name,
2205                           gdb_port + target->target_number);
2206
2207                 target = target->next;
2208         }
2209
2210         return ERROR_OK;
2211 }
2212
2213 /* daemon configuration command gdb_port */
2214 int handle_gdb_port_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2215 {
2216         if (argc == 0)
2217         {
2218                 command_print(cmd_ctx, "%d", gdb_port);
2219                 return ERROR_OK;
2220         }
2221
2222         /* only if the port wasn't overwritten by cmdline */
2223         if (gdb_port == 0)
2224                 gdb_port = strtoul(args[0], NULL, 0);
2225
2226         return ERROR_OK;
2227 }
2228
2229 int handle_gdb_detach_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2230 {
2231         if (argc == 1)
2232         {
2233                 if (strcmp(args[0], "resume") == 0)
2234                 {
2235                         detach_mode = GDB_DETACH_RESUME;
2236                         return ERROR_OK;
2237                 }
2238                 else if (strcmp(args[0], "reset") == 0)
2239                 {
2240                         detach_mode = GDB_DETACH_RESET;
2241                         return ERROR_OK;
2242                 }
2243                 else if (strcmp(args[0], "halt") == 0)
2244                 {
2245                         detach_mode = GDB_DETACH_HALT;
2246                         return ERROR_OK;
2247                 }
2248                 else if (strcmp(args[0], "nothing") == 0)
2249                 {
2250                         detach_mode = GDB_DETACH_NOTHING;
2251                         return ERROR_OK;
2252                 }
2253                 else
2254                         LOG_WARNING("invalid gdb_detach configuration directive: %s", args[0]);
2255         }
2256
2257         return ERROR_COMMAND_SYNTAX_ERROR;
2258 }
2259
2260 int handle_gdb_memory_map_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2261 {
2262         if (argc == 1)
2263         {
2264                 if (strcmp(args[0], "enable") == 0)
2265                 {
2266                         gdb_use_memory_map = 1;
2267                         return ERROR_OK;
2268                 }
2269                 else if (strcmp(args[0], "disable") == 0)
2270                 {
2271                         gdb_use_memory_map = 0;
2272                         return ERROR_OK;
2273                 }
2274                 else
2275                         LOG_WARNING("invalid gdb_memory_map configuration directive %s", args[0]);
2276         }
2277
2278         return ERROR_COMMAND_SYNTAX_ERROR;
2279 }
2280
2281 int handle_gdb_flash_program_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2282 {
2283         if (argc == 1)
2284         {
2285                 if (strcmp(args[0], "enable") == 0)
2286                 {
2287                         gdb_flash_program = 1;
2288                         return ERROR_OK;
2289                 }
2290                 else if (strcmp(args[0], "disable") == 0)
2291                 {
2292                         gdb_flash_program = 0;
2293                         return ERROR_OK;
2294                 }
2295                 else
2296                         LOG_WARNING("invalid gdb_flash_program configuration directive: %s", args[0]);
2297         }
2298
2299         return ERROR_COMMAND_SYNTAX_ERROR;
2300 }
2301
2302 int handle_gdb_report_data_abort_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2303 {
2304         if (argc == 1)
2305         {
2306                 if (strcmp(args[0], "enable") == 0)
2307                 {
2308                         gdb_report_data_abort = 1;
2309                         return ERROR_OK;
2310                 }
2311                 else if (strcmp(args[0], "disable") == 0)
2312                 {
2313                         gdb_report_data_abort = 0;
2314                         return ERROR_OK;
2315                 }
2316                 else
2317                         LOG_WARNING("invalid gdb_report_data_abort configuration directive: %s", args[0]);
2318         }
2319
2320         return ERROR_COMMAND_SYNTAX_ERROR;
2321 }
2322
2323 /* gdb_breakpoint_override */
2324 int handle_gdb_breakpoint_override_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2325 {
2326         if (argc == 0)
2327         {
2328
2329         } else if (argc==1)
2330         {
2331                 gdb_breakpoint_override = 1;
2332                 if (strcmp(args[0], "hard")==0)
2333                 {
2334                         gdb_breakpoint_override_type=BKPT_HARD;
2335                 } else if (strcmp(args[0], "soft")==0)
2336                 {
2337                         gdb_breakpoint_override_type=BKPT_SOFT;
2338                 } else if (strcmp(args[0], "disable") == 0)
2339                 {
2340                         gdb_breakpoint_override = 0;
2341                 }
2342         } else
2343         {
2344                 return ERROR_COMMAND_SYNTAX_ERROR;
2345         }
2346         if (gdb_breakpoint_override)
2347         {
2348                 LOG_USER("force %s breakpoints", (gdb_breakpoint_override_type==BKPT_HARD)?"hard":"soft");
2349         } else
2350         {
2351                 LOG_USER("breakpoint type is not overriden");
2352         }
2353
2354         return ERROR_OK;
2355 }
2356
2357
2358 int gdb_register_commands(command_context_t *command_context)
2359 {
2360         register_command(command_context, NULL, "gdb_port", handle_gdb_port_command,
2361                         COMMAND_CONFIG, "daemon configuration command gdb_port");
2362         register_command(command_context, NULL, "gdb_detach", handle_gdb_detach_command,
2363                         COMMAND_CONFIG, "");
2364         register_command(command_context, NULL, "gdb_memory_map", handle_gdb_memory_map_command,
2365                         COMMAND_CONFIG, "enable or disable memory map");
2366         register_command(command_context, NULL, "gdb_flash_program", handle_gdb_flash_program_command,
2367                         COMMAND_CONFIG, "enable or disable flash program");
2368         register_command(command_context, NULL, "gdb_report_data_abort", handle_gdb_report_data_abort_command,
2369                         COMMAND_CONFIG, "enable or disable report data");
2370         register_command(command_context, NULL, "gdb_breakpoint_override", handle_gdb_breakpoint_override_command,
2371                         COMMAND_EXEC, "hard/soft/disabled - force breakpoint type for gdb 'break' commands."
2372                         "The raison d'etre for this option is to support GDB GUI's without "
2373                         "a hard/soft breakpoint concept where the default OpenOCD behaviour "
2374                         "is not sufficient");
2375         return ERROR_OK;
2376 }