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

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