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