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