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