git.sur5r.net Git - openocd/blob - src/server/gdb_server.c

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