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