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         USER(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         retval = target_read_buffer(target, addr, len, buffer);
1060
1061 #if 0
1062         if (retval == ERROR_TARGET_DATA_ABORT)
1063         {
1064                 /* TODO : Here we have to lie and send back all zero's lest stack traces won't work.
1065                  * At some point this might be fixed in GDB, in which case this code can be removed.
1066                  *
1067                  * OpenOCD developers are acutely aware of this problem, but there is nothing
1068                  * gained by involving the user in this problem that hopefully will get resolved
1069                  * eventually
1070                  *
1071                  * http://sourceware.org/cgi-bin/gnatsweb.pl?cmd=view%20audit-trail&database=gdb&pr=2395
1072                  */
1073                 memset(buffer, 0, len);
1074                 retval = ERROR_OK;
1075         }
1076 #endif
1077
1078         if (retval == ERROR_OK)
1079         {
1080                 hex_buffer = malloc(len * 2 + 1);
1081
1082                 int i;
1083                 for (i = 0; i < len; i++)
1084                 {
1085                         u8 t = buffer[i];
1086                         hex_buffer[2 * i] = DIGITS[(t >> 4) & 0xf];
1087                         hex_buffer[2 * i + 1] = DIGITS[t & 0xf];
1088                 }
1089
1090                 gdb_put_packet(connection, hex_buffer, len * 2);
1091
1092                 free(hex_buffer);
1093         }
1094         else
1095         {
1096                 retval = gdb_memory_packet_error(connection, retval);
1097         }
1098
1099         free(buffer);
1100
1101         return retval;
1102 }
1103
1104 int gdb_write_memory_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
1105 {
1106         char *separator;
1107         u32 addr = 0;
1108         u32 len = 0;
1109
1110         u8 *buffer;
1111
1112         int i;
1113         int retval;
1114
1115         /* skip command character */
1116         packet++;
1117
1118         addr = strtoul(packet, &separator, 16);
1119
1120         if (*separator != ',')
1121         {
1122                 ERROR("incomplete write memory packet received, dropping connection");
1123                 return ERROR_SERVER_REMOTE_CLOSED;
1124         }
1125
1126         len = strtoul(separator+1, &separator, 16);
1127
1128         if (*(separator++) != ':')
1129         {
1130                 ERROR("incomplete write memory packet received, dropping connection");
1131                 return ERROR_SERVER_REMOTE_CLOSED;
1132         }
1133
1134         buffer = malloc(len);
1135
1136         DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
1137
1138         for (i=0; i<len; i++)
1139         {
1140                 u32 tmp;
1141                 sscanf(separator + 2*i, "%2x", &tmp);
1142                 buffer[i] = tmp;
1143         }
1144
1145         retval = target_write_buffer(target, addr, len, buffer);
1146
1147         if (retval == ERROR_OK)
1148         {
1149                 gdb_put_packet(connection, "OK", 2);
1150         }
1151         else
1152         {
1153                 if ((retval = gdb_memory_packet_error(connection, retval)) != ERROR_OK)
1154                         return retval;
1155         }
1156
1157         free(buffer);
1158
1159         return ERROR_OK;
1160 }
1161
1162 int gdb_write_memory_binary_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
1163 {
1164         char *separator;
1165         u32 addr = 0;
1166         u32 len = 0;
1167
1168         int retval;
1169
1170         /* skip command character */
1171         packet++;
1172
1173         addr = strtoul(packet, &separator, 16);
1174
1175         if (*separator != ',')
1176         {
1177                 ERROR("incomplete write memory binary packet received, dropping connection");
1178                 return ERROR_SERVER_REMOTE_CLOSED;
1179         }
1180
1181         len = strtoul(separator+1, &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         retval = ERROR_OK;
1190         if( len ) {
1191
1192                 DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
1193
1194                 retval = target_write_buffer(target, addr, len, separator);
1195         }
1196
1197         if (retval == ERROR_OK)
1198         {
1199                 gdb_put_packet(connection, "OK", 2);
1200         }
1201         else
1202         {
1203                 if ((retval = gdb_memory_packet_error(connection, retval)) != ERROR_OK)
1204                         return retval;
1205         }
1206
1207         return ERROR_OK;
1208 }
1209
1210 void gdb_step_continue_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
1211 {
1212         int current = 0;
1213         u32 address = 0x0;
1214
1215         DEBUG("-");
1216
1217         if (packet_size > 1)
1218         {
1219                 packet[packet_size] = 0;
1220                 address = strtoul(packet + 1, NULL, 16);
1221         }
1222         else
1223         {
1224                 current = 1;
1225         }
1226
1227         if (packet[0] == 'c')
1228         {
1229                 DEBUG("continue");
1230                 target->type->resume(target, current, address, 0, 0); /* resume at current address, don't handle breakpoints, not debugging */
1231         }
1232         else if (packet[0] == 's')
1233         {
1234                 DEBUG("step");
1235                 target->type->step(target, current, address, 0); /* step at current or address, don't handle breakpoints */
1236         }
1237 }
1238
1239 int gdb_bp_wp_packet_error(connection_t *connection, int retval)
1240 {
1241         switch (retval)
1242         {
1243                 case ERROR_TARGET_NOT_HALTED:
1244                         ERROR("gdb tried to set a breakpoint but we're not halted, dropping connection");
1245                         return ERROR_SERVER_REMOTE_CLOSED;
1246                         break;
1247                 case ERROR_TARGET_RESOURCE_NOT_AVAILABLE:
1248                         gdb_send_error(connection, EBUSY);
1249                         break;
1250                 default:
1251                         ERROR("BUG: unexpected error %i", retval);
1252                         exit(-1);
1253         }
1254
1255         return ERROR_OK;
1256 }
1257
1258 int gdb_breakpoint_watchpoint_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
1259 {
1260         int type;
1261         enum breakpoint_type bp_type = BKPT_SOFT /* dummy init to avoid warning */;
1262         enum watchpoint_rw wp_type;
1263         u32 address;
1264         u32 size;
1265         char *separator;
1266         int retval;
1267
1268         DEBUG("-");
1269
1270         type = strtoul(packet + 1, &separator, 16);
1271
1272         if (type == 0)  /* memory breakpoint */
1273                 bp_type = BKPT_SOFT;
1274         else if (type == 1) /* hardware breakpoint */
1275                 bp_type = BKPT_HARD;
1276         else if (type == 2) /* write watchpoint */
1277                 wp_type = WPT_WRITE;
1278         else if (type == 3) /* read watchpoint */
1279                 wp_type = WPT_READ;
1280         else if (type == 4) /* access watchpoint */
1281                 wp_type = WPT_ACCESS;
1282
1283         if (*separator != ',')
1284         {
1285                 ERROR("incomplete breakpoint/watchpoint packet received, dropping connection");
1286                 return ERROR_SERVER_REMOTE_CLOSED;
1287         }
1288
1289         address = strtoul(separator+1, &separator, 16);
1290
1291         if (*separator != ',')
1292         {
1293                 ERROR("incomplete breakpoint/watchpoint packet received, dropping connection");
1294                 return ERROR_SERVER_REMOTE_CLOSED;
1295         }
1296
1297         size = strtoul(separator+1, &separator, 16);
1298
1299         switch (type)
1300         {
1301                 case 0:
1302                 case 1:
1303                         if (packet[0] == 'Z')
1304                         {
1305                                 if ((retval = breakpoint_add(target, address, size, bp_type)) != ERROR_OK)
1306                                 {
1307                                         if ((retval = gdb_bp_wp_packet_error(connection, retval)) != ERROR_OK)
1308                                                 return retval;
1309                                 }
1310                                 else
1311                                 {
1312                                         gdb_put_packet(connection, "OK", 2);
1313                                 }
1314                         }
1315                         else
1316                         {
1317                                 breakpoint_remove(target, address);
1318                                 gdb_put_packet(connection, "OK", 2);
1319                         }
1320                         break;
1321                 case 2:
1322                 case 3:
1323                 case 4:
1324                 {
1325                         if (packet[0] == 'Z')
1326                         {
1327                                 if ((retval = watchpoint_add(target, address, size, type-2, 0, 0xffffffffu)) != ERROR_OK)
1328                                 {
1329                                         if ((retval = gdb_bp_wp_packet_error(connection, retval)) != ERROR_OK)
1330                                                 return retval;
1331                                 }
1332                                 else
1333                                 {
1334                                         gdb_put_packet(connection, "OK", 2);
1335                                 }
1336                         }
1337                         else
1338                         {
1339                                 watchpoint_remove(target, address);
1340                                 gdb_put_packet(connection, "OK", 2);
1341                         }
1342                         break;
1343                 }
1344                 default:
1345                         break;
1346         }
1347
1348         return ERROR_OK;
1349 }
1350
1351 /* print out a string and allocate more space as needed, mainly used for XML at this point */
1352 void xml_printf(int *retval, char **xml, int *pos, int *size, const char *fmt, ...)
1353 {
1354         if (*retval != ERROR_OK)
1355         {
1356                 return;
1357         }
1358         int first = 1;
1359
1360         for (;;)
1361         {
1362                 if ((*xml == NULL) || (!first))
1363                 {
1364                         /* start by 0 to exercise all the code paths.
1365                          * Need minimum 2 bytes to fit 1 char and 0 terminator. */
1366
1367                         *size = *size * 2 + 2;
1368                         char *t = *xml;
1369                         *xml = realloc(*xml, *size);
1370                         if (*xml == NULL)
1371                         {
1372                                 if (t)
1373                                         free(t);
1374                                 *retval = ERROR_SERVER_REMOTE_CLOSED;
1375                                 return;
1376                         }
1377                 }
1378
1379             va_list ap;
1380             int ret;
1381             va_start(ap, fmt);
1382             ret = vsnprintf(*xml + *pos, *size - *pos, fmt, ap);
1383             va_end(ap);
1384             if ((ret > 0) && ((ret + 1) < *size - *pos))
1385             {
1386                 *pos += ret;
1387                 return;
1388             }
1389             /* there was just enough or not enough space, allocate more. */
1390             first = 0;
1391         }
1392 }
1393
1394 static int decode_xfer_read (char *buf, char **annex, int *ofs, unsigned int *len)
1395 {
1396         char *separator;
1397
1398         /* Extract and NUL-terminate the annex. */
1399         *annex = buf;
1400         while (*buf && *buf != ':')
1401                 buf++;
1402         if (*buf == '\0')
1403                 return -1;
1404         *buf++ = 0;
1405
1406         /* After the read marker and annex, qXfer looks like a
1407          * traditional 'm' packet. */
1408
1409         *ofs = strtoul(buf, &separator, 16);
1410
1411         if (*separator != ',')
1412                 return -1;
1413
1414         *len = strtoul(separator+1, NULL, 16);
1415
1416         return 0;
1417 }
1418
1419 int gdb_calc_blocksize(flash_bank_t *bank)
1420 {
1421         int i;
1422         int block_size = 0xffffffff;
1423
1424         /* loop through all sectors and return smallest sector size */
1425
1426         for (i = 0; i < bank->num_sectors; i++)
1427         {
1428                 if (bank->sectors[i].size < block_size)
1429                         block_size = bank->sectors[i].size;
1430         }
1431
1432         return block_size;
1433 }
1434
1435 int gdb_query_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
1436 {
1437         command_context_t *cmd_ctx = connection->cmd_ctx;
1438
1439         if (strstr(packet, "qRcmd,"))
1440         {
1441                 if (packet_size > 6)
1442                 {
1443                         char *cmd;
1444                         int i;
1445                         cmd = malloc((packet_size - 6)/2 + 1);
1446                         for (i=0; i < (packet_size - 6)/2; i++)
1447                         {
1448                                 u32 tmp;
1449                                 sscanf(packet + 6 + 2*i, "%2x", &tmp);
1450                                 cmd[i] = tmp;
1451                         }
1452                         cmd[(packet_size - 6)/2] = 0x0;
1453
1454                         /* We want to print all debug output to GDB connection */
1455                         log_setCallback(gdb_log_callback, connection);
1456                         target_call_timer_callbacks();
1457                         command_run_line(cmd_ctx, cmd);
1458                         free(cmd);
1459                 }
1460                 gdb_put_packet(connection, "OK", 2);
1461                 return ERROR_OK;
1462         }
1463         else if (strstr(packet, "qCRC:"))
1464         {
1465                 if (packet_size > 5)
1466                 {
1467                         int retval;
1468                         u8 gdb_reply[10];
1469                         char *separator;
1470                         u32 checksum;
1471                         u32 addr = 0;
1472                         u32 len = 0;
1473
1474                         /* skip command character */
1475                         packet += 5;
1476
1477                         addr = strtoul(packet, &separator, 16);
1478
1479                         if (*separator != ',')
1480                         {
1481                                 ERROR("incomplete read memory packet received, dropping connection");
1482                                 return ERROR_SERVER_REMOTE_CLOSED;
1483                         }
1484
1485                         len = strtoul(separator + 1, NULL, 16);
1486
1487                         retval = target_checksum_memory(target, addr, len, &checksum);
1488
1489                         if (retval == ERROR_OK)
1490                         {
1491                                 snprintf(gdb_reply, 10, "C%8.8x", checksum);
1492                                 gdb_put_packet(connection, gdb_reply, 9);
1493                         }
1494                         else
1495                         {
1496                                 if ((retval = gdb_memory_packet_error(connection, retval)) != ERROR_OK)
1497                                         return retval;
1498                         }
1499
1500                         return ERROR_OK;
1501                 }
1502         }
1503         else if (strstr(packet, "qSupported"))
1504         {
1505                 /* we currently support packet size and qXfer:memory-map:read (if enabled)
1506                  * disable qXfer:features:read for the moment */
1507                 int retval = ERROR_OK;
1508                 char *buffer = NULL;
1509                 int pos = 0;
1510                 int size = 0;
1511
1512                 xml_printf(&retval, &buffer, &pos, &size,
1513                                 "PacketSize=%x;qXfer:memory-map:read%c;qXfer:features:read-",
1514                                 (GDB_BUFFER_SIZE - 1), gdb_use_memory_map == 1 ? '+' : '-');
1515
1516                 if (retval != ERROR_OK)
1517                 {
1518                         gdb_send_error(connection, 01);
1519                         return ERROR_OK;
1520                 }
1521
1522                 gdb_put_packet(connection, buffer, strlen(buffer));
1523                 free(buffer);
1524
1525                 return ERROR_OK;
1526         }
1527         else if (strstr(packet, "qXfer:memory-map:read::"))
1528         {
1529                 /* We get away with only specifying flash here. Regions that are not
1530                  * specified are treated as if we provided no memory map(if not we
1531                  * could detect the holes and mark them as RAM).
1532                  * Normally we only execute this code once, but no big deal if we
1533                  * have to regenerate it a couple of times. */
1534
1535                 flash_bank_t *p;
1536                 char *xml = NULL;
1537                 int size = 0;
1538                 int pos = 0;
1539                 int retval = ERROR_OK;
1540
1541                 int offset;
1542                 int length;
1543                 char *separator;
1544                 int blocksize;
1545
1546                 /* skip command character */
1547                 packet += 23;
1548
1549                 offset = strtoul(packet, &separator, 16);
1550                 length = strtoul(separator + 1, &separator, 16);
1551
1552                 xml_printf(&retval, &xml, &pos, &size, "<memory-map>\n");
1553
1554                 int i = 0;
1555                 for (;;)
1556                 {
1557                         p = get_flash_bank_by_num(i);
1558                         if (p == NULL)
1559                                 break;
1560
1561                         /* if device has uneven sector sizes, eg. str7, lpc
1562                          * we pass the smallest sector size to gdb memory map */
1563                         blocksize = gdb_calc_blocksize(p);
1564
1565                         xml_printf(&retval, &xml, &pos, &size, "<memory type=\"flash\" start=\"0x%x\" length=\"0x%x\">\n" \
1566                                 "<property name=\"blocksize\">0x%x</property>\n" \
1567                                 "</memory>\n", \
1568                                 p->base, p->size, blocksize);
1569                         i++;
1570                 }
1571
1572                 xml_printf(&retval, &xml, &pos, &size, "</memory-map>\n");
1573
1574                 if (retval != ERROR_OK)
1575                 {
1576                         gdb_send_error(connection, retval);
1577                         return retval;
1578                 }
1579
1580                 if (offset + length > pos)
1581                 {
1582                         length = pos - offset;
1583                 }
1584
1585                 char *t = malloc(length + 1);
1586                 t[0] = 'l';
1587                 memcpy(t + 1, xml + offset, length);
1588                 gdb_put_packet(connection, t, length + 1);
1589
1590                 free(t);
1591                 free(xml);
1592                 return ERROR_OK;
1593         }
1594         else if (strstr(packet, "qXfer:features:read:"))
1595         {
1596                 char *xml = NULL;
1597                 int size = 0;
1598                 int pos = 0;
1599                 int retval = ERROR_OK;
1600
1601                 int offset;
1602                 int length;
1603                 char *annex;
1604
1605                 /* skip command character */
1606                 packet += 20;
1607
1608                 if (decode_xfer_read( packet, &annex, &offset, &length ) < 0)
1609                 {
1610                         gdb_send_error(connection, 01);
1611                         return ERROR_OK;
1612                 }
1613
1614                 if (strcmp(annex, "target.xml") != 0)
1615                 {
1616                         gdb_send_error(connection, 01);
1617                         return ERROR_OK;
1618                 }
1619
1620                 xml_printf(&retval, &xml, &pos, &size, \
1621                         "l<target version=\"1.0\">\n<architecture>arm</architecture>\n</target>\n");
1622
1623                 if (retval != ERROR_OK)
1624                 {
1625                         gdb_send_error(connection, retval);
1626                         return retval;
1627                 }
1628
1629                 gdb_put_packet(connection, xml, strlen(xml) + 1);
1630
1631                 free(xml);
1632                 return ERROR_OK;
1633         }
1634
1635         gdb_put_packet(connection, "", 0);
1636         return ERROR_OK;
1637 }
1638
1639 int gdb_v_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
1640 {
1641         gdb_connection_t *gdb_connection = connection->priv;
1642         gdb_service_t *gdb_service = connection->service->priv;
1643         int result;
1644
1645         /* if flash programming disabled - send a empty reply */
1646
1647         if (gdb_flash_program == 0)
1648         {
1649                 gdb_put_packet(connection, "", 0);
1650                 return ERROR_OK;
1651         }
1652
1653         if (strstr(packet, "vFlashErase:"))
1654         {
1655                 unsigned long addr;
1656                 unsigned long length;
1657
1658                 char *parse = packet + 12;
1659                 if (*parse == '\0')
1660                 {
1661                         ERROR("incomplete vFlashErase packet received, dropping connection");
1662                         return ERROR_SERVER_REMOTE_CLOSED;
1663                 }
1664
1665                 addr = strtoul(parse, &parse, 16);
1666
1667                 if (*(parse++) != ',' || *parse == '\0')
1668                 {
1669                         ERROR("incomplete vFlashErase packet received, dropping connection");
1670                         return ERROR_SERVER_REMOTE_CLOSED;
1671                 }
1672
1673                 length = strtoul(parse, &parse, 16);
1674
1675                 if (*parse != '\0')
1676                 {
1677                         ERROR("incomplete vFlashErase packet received, dropping connection");
1678                         return ERROR_SERVER_REMOTE_CLOSED;
1679                 }
1680
1681                 /* assume all sectors need erasing - stops any problems
1682                  * when flash_write is called multiple times */
1683                 flash_set_dirty();
1684
1685                 /* perform any target specific operations before the erase */
1686                 target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_PROGRAM);
1687
1688                 /* perform erase */
1689                 if ((result = flash_erase_address_range(gdb_service->target, addr, length)) != ERROR_OK)
1690                 {
1691                         /* GDB doesn't evaluate the actual error number returned,
1692                          * treat a failed erase as an I/O error
1693                          */
1694                         gdb_send_error(connection, EIO);
1695                         ERROR("flash_erase returned %i", result);
1696                 }
1697                 else
1698                         gdb_put_packet(connection, "OK", 2);
1699
1700                 return ERROR_OK;
1701         }
1702
1703         if (strstr(packet, "vFlashWrite:"))
1704         {
1705                 unsigned long addr;
1706                 unsigned long length;
1707                 char *parse = packet + 12;
1708
1709                 if (*parse == '\0')
1710                 {
1711                         ERROR("incomplete vFlashErase packet received, dropping connection");
1712                         return ERROR_SERVER_REMOTE_CLOSED;
1713                 }
1714                 addr = strtoul(parse, &parse, 16);
1715                 if (*(parse++) != ':')
1716                 {
1717                         ERROR("incomplete vFlashErase packet received, dropping connection");
1718                         return ERROR_SERVER_REMOTE_CLOSED;
1719                 }
1720                 length = packet_size - (parse - packet);
1721
1722                 /* create a new image if there isn't already one */
1723                 if (gdb_connection->vflash_image == NULL)
1724                 {
1725                         gdb_connection->vflash_image = malloc(sizeof(image_t));
1726                         image_open(gdb_connection->vflash_image, "", "build");
1727                 }
1728
1729                 /* create new section with content from packet buffer */
1730                 image_add_section(gdb_connection->vflash_image, addr, length, 0x0, (u8*)parse);
1731
1732                 gdb_put_packet(connection, "OK", 2);
1733
1734                 return ERROR_OK;
1735         }
1736
1737         if (!strcmp(packet, "vFlashDone"))
1738         {
1739                 u32 written;
1740                 char *error_str;
1741
1742                 /* process the flashing buffer. No need to erase as GDB
1743                  * always issues a vFlashErase first. */
1744                 if ((result = flash_write(gdb_service->target, gdb_connection->vflash_image, &written, &error_str, NULL, 0)) != ERROR_OK)
1745                 {
1746                         if (result == ERROR_FLASH_DST_OUT_OF_BANK)
1747                                 gdb_put_packet(connection, "E.memtype", 9);
1748                         else
1749                                 gdb_send_error(connection, EIO);
1750
1751                         if (error_str)
1752                         {
1753                                 ERROR("flash writing failed: %s", error_str);
1754                                 free(error_str);
1755                         }
1756                 }
1757                 else
1758                 {
1759                         DEBUG("wrote %u bytes from vFlash image to flash", written);
1760                         gdb_put_packet(connection, "OK", 2);
1761                 }
1762
1763                 image_close(gdb_connection->vflash_image);
1764                 free(gdb_connection->vflash_image);
1765                 gdb_connection->vflash_image = NULL;
1766
1767                 return ERROR_OK;
1768         }
1769
1770         gdb_put_packet(connection, "", 0);
1771         return ERROR_OK;
1772 }
1773
1774 int gdb_detach(connection_t *connection, target_t *target)
1775 {
1776         switch( detach_mode )
1777         {
1778                 case GDB_DETACH_RESUME:
1779                         target->type->resume(target, 1, 0, 1, 0);
1780                         break;
1781
1782                 case GDB_DETACH_RESET:
1783                         target_process_reset(connection->cmd_ctx);
1784                         break;
1785
1786                 case GDB_DETACH_HALT:
1787                         target->type->halt(target);
1788                         break;
1789
1790                 case GDB_DETACH_NOTHING:
1791                         break;
1792         }
1793
1794         gdb_put_packet(connection, "OK", 2);
1795
1796         return ERROR_OK;
1797 }
1798
1799 static void gdb_log_callback(void *priv, const char *file, int line,
1800                 const char *function, const char *format, va_list args)
1801 {
1802         connection_t *connection = priv;
1803         gdb_connection_t *gdb_con = connection->priv;
1804
1805         if (gdb_con->busy)
1806         {
1807                 /* do not reply this using the O packet */
1808                 return;
1809         }
1810
1811         char *t = allocPrintf(format, args);
1812         if (t == NULL)
1813                 return;
1814
1815         gdb_output_con(connection, t);
1816
1817         free(t);
1818 }
1819
1820 int gdb_input_inner(connection_t *connection)
1821 {
1822         gdb_service_t *gdb_service = connection->service->priv;
1823         target_t *target = gdb_service->target;
1824         char packet[GDB_BUFFER_SIZE];
1825         int packet_size;
1826         int retval;
1827         gdb_connection_t *gdb_con = connection->priv;
1828         static int extended_protocol = 0;
1829
1830         /* drain input buffer */
1831         do
1832         {
1833                 packet_size = GDB_BUFFER_SIZE-1;
1834                 if ((retval = gdb_get_packet(connection, packet, &packet_size)) != ERROR_OK)
1835                 {
1836                         return retval;
1837                 }
1838
1839                 /* terminate with zero */
1840                 packet[packet_size] = 0;
1841
1842                 DEBUG("received packet: '%s'", packet);
1843
1844                 if (packet_size > 0)
1845                 {
1846                         retval = ERROR_OK;
1847                         switch (packet[0])
1848                         {
1849                                 case 'H':
1850                                         /* Hct... -- set thread
1851                                          * we don't have threads, send empty reply */
1852                                         gdb_put_packet(connection, NULL, 0);
1853                                         break;
1854                                 case 'q':
1855                                         retval = gdb_query_packet(connection, target, packet, packet_size);
1856                                         break;
1857                                 case 'g':
1858                                         retval = gdb_get_registers_packet(connection, target, packet, packet_size);
1859                                         break;
1860                                 case 'G':
1861                                         retval = gdb_set_registers_packet(connection, target, packet, packet_size);
1862                                         break;
1863                                 case 'p':
1864                                         retval = gdb_get_register_packet(connection, target, packet, packet_size);
1865                                         break;
1866                                 case 'P':
1867                                         retval = gdb_set_register_packet(connection, target, packet, packet_size);
1868                                         break;
1869                                 case 'm':
1870                                         retval = gdb_read_memory_packet(connection, target, packet, packet_size);
1871                                         break;
1872                                 case 'M':
1873                                         retval = gdb_write_memory_packet(connection, target, packet, packet_size);
1874                                         break;
1875                                 case 'z':
1876                                 case 'Z':
1877                                         retval = gdb_breakpoint_watchpoint_packet(connection, target, packet, packet_size);
1878                                         break;
1879                                 case '?':
1880                                         gdb_last_signal_packet(connection, target, packet, packet_size);
1881                                         break;
1882                                 case 'c':
1883                                 case 's':
1884                                         {
1885                                         /* We're running/stepping, in which case we can
1886                                          * forward log output until the target is halted */
1887                                                 gdb_connection_t *gdb_con = connection->priv;
1888                                                 gdb_con->frontend_state = TARGET_RUNNING;
1889                                                 log_setCallback(gdb_log_callback, connection);
1890                                                 gdb_step_continue_packet(connection, target, packet, packet_size);
1891                                         }
1892                                         break;
1893                                 case 'v':
1894                                         retval = gdb_v_packet(connection, target, packet, packet_size);
1895                                         break;
1896                                 case 'D':
1897                                         retval = gdb_detach(connection, target);
1898                                         extended_protocol = 0;
1899                                         break;
1900                                 case 'X':
1901                                         if ((retval = gdb_write_memory_binary_packet(connection, target, packet, packet_size)) != ERROR_OK)
1902                                                 return retval;
1903                                         break;
1904                                 case 'k':
1905                                         if (extended_protocol != 0)
1906                                                 break;
1907                                         gdb_put_packet(connection, "OK", 2);
1908                                         return ERROR_SERVER_REMOTE_CLOSED;
1909                                 case '!':
1910                                         /* handle extended remote protocol */
1911                                         extended_protocol = 1;
1912                                         gdb_put_packet(connection, "OK", 2);
1913                                         break;
1914                                 case 'R':
1915                                         /* handle extended restart packet */
1916                                         target_process_reset(connection->cmd_ctx);
1917                                         break;
1918                                 default:
1919                                         /* ignore unkown packets */
1920                                         DEBUG("ignoring 0x%2.2x packet", packet[0]);
1921                                         gdb_put_packet(connection, NULL, 0);
1922                                         break;
1923                         }
1924
1925                         /* if a packet handler returned an error, exit input loop */
1926                         if (retval != ERROR_OK)
1927                                 return retval;
1928                 }
1929
1930                 if (gdb_con->ctrl_c)
1931                 {
1932                         if (target->state == TARGET_RUNNING)
1933                         {
1934                                 target->type->halt(target);
1935                                 gdb_con->ctrl_c = 0;
1936                         }
1937                 }
1938
1939         } while (gdb_con->buf_cnt > 0);
1940
1941         return ERROR_OK;
1942 }
1943
1944 int gdb_input(connection_t *connection)
1945 {
1946         int retval = gdb_input_inner(connection);
1947         if (retval == ERROR_SERVER_REMOTE_CLOSED)
1948                 return retval;
1949         /* we'll recover from any other errors(e.g. temporary timeouts, etc.) */
1950         return ERROR_OK;
1951 }
1952
1953 int gdb_init()
1954 {
1955         gdb_service_t *gdb_service;
1956         target_t *target = targets;
1957         int i = 0;
1958
1959         if (!target)
1960         {
1961                 WARNING("no gdb ports allocated as no target has been specified");
1962                 return ERROR_OK;
1963         }
1964
1965         if (gdb_port == 0)
1966         {
1967                 WARNING("no gdb port specified, using default port 3333");
1968                 gdb_port = 3333;
1969         }
1970
1971         while (target)
1972         {
1973                 char service_name[8];
1974
1975                 snprintf(service_name, 8, "gdb-%2.2i", i);
1976
1977                 gdb_service = malloc(sizeof(gdb_service_t));
1978                 gdb_service->target = target;
1979
1980                 add_service("gdb", CONNECTION_GDB, gdb_port + i, 1, gdb_new_connection, gdb_input, gdb_connection_closed, gdb_service);
1981
1982                 DEBUG("gdb service for target %s at port %i", target->type->name, gdb_port + i);
1983
1984                 i++;
1985                 target = target->next;
1986         }
1987
1988         return ERROR_OK;
1989 }
1990
1991 /* daemon configuration command gdb_port */
1992 int handle_gdb_port_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1993 {
1994         if (argc == 0)
1995                 return ERROR_OK;
1996
1997         /* only if the port wasn't overwritten by cmdline */
1998         if (gdb_port == 0)
1999                 gdb_port = strtoul(args[0], NULL, 0);
2000
2001         return ERROR_OK;
2002 }
2003
2004 int handle_gdb_detach_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2005 {
2006         if (argc == 1)
2007         {
2008                 if (strcmp(args[0], "resume") == 0)
2009                 {
2010                         detach_mode = GDB_DETACH_RESUME;
2011                         return ERROR_OK;
2012                 }
2013                 else if (strcmp(args[0], "reset") == 0)
2014                 {
2015                         detach_mode = GDB_DETACH_RESET;
2016                         return ERROR_OK;
2017                 }
2018                 else if (strcmp(args[0], "halt") == 0)
2019                 {
2020                         detach_mode = GDB_DETACH_HALT;
2021                         return ERROR_OK;
2022                 }
2023                 else if (strcmp(args[0], "nothing") == 0)
2024                 {
2025                         detach_mode = GDB_DETACH_NOTHING;
2026                         return ERROR_OK;
2027                 }
2028         }
2029
2030         WARNING("invalid gdb_detach configuration directive: %s", args[0]);
2031         return ERROR_OK;
2032 }
2033
2034 int handle_gdb_memory_map_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2035 {
2036         if (argc == 1)
2037         {
2038                 if (strcmp(args[0], "enable") == 0)
2039                 {
2040                         gdb_use_memory_map = 1;
2041                         return ERROR_OK;
2042                 }
2043                 else if (strcmp(args[0], "disable") == 0)
2044                 {
2045                         gdb_use_memory_map = 0;
2046                         return ERROR_OK;
2047                 }
2048         }
2049
2050         WARNING("invalid gdb_memory_map configuration directive: %s", args[0]);
2051         return ERROR_OK;
2052 }
2053
2054 int handle_gdb_flash_program_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2055 {
2056         if (argc == 1)
2057         {
2058                 if (strcmp(args[0], "enable") == 0)
2059                 {
2060                         gdb_flash_program = 1;
2061                         return ERROR_OK;
2062                 }
2063                 else if (strcmp(args[0], "disable") == 0)
2064                 {
2065                         gdb_flash_program = 0;
2066                         return ERROR_OK;
2067                 }
2068         }
2069
2070         WARNING("invalid gdb_memory_map configuration directive: %s", args[0]);
2071         return ERROR_OK;
2072 }
2073
2074 int gdb_register_commands(command_context_t *command_context)
2075 {
2076         register_command(command_context, NULL, "gdb_port", handle_gdb_port_command,
2077                         COMMAND_CONFIG, "");
2078         register_command(command_context, NULL, "gdb_detach", handle_gdb_detach_command,
2079                         COMMAND_CONFIG, "");
2080         register_command(command_context, NULL, "gdb_memory_map", handle_gdb_memory_map_command,
2081                         COMMAND_CONFIG, "");
2082         register_command(command_context, NULL, "gdb_flash_program", handle_gdb_flash_program_command,
2083                         COMMAND_CONFIG, "");
2084         return ERROR_OK;
2085 }