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