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