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