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