git.gag.com Git - fw/openocd/blob - src/server/gdb_server.c

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