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