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