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