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