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 "breakpoints.h"
  32 #include "flash.h"
  33 #include "target_request.h"
  34
  35 #define __USE_GNU
  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 int gdb_last_signal(target_t *target)
  48 {
  49         switch (target->debug_reason)
  50         {
  51                 case DBG_REASON_DBGRQ:
  52                         return 0x2; /* SIGINT */
  53                 case DBG_REASON_BREAKPOINT:
  54                 case DBG_REASON_WATCHPOINT:
  55                 case DBG_REASON_WPTANDBKPT:
  56                         return 0x05; /* SIGTRAP */
  57                 case DBG_REASON_SINGLESTEP:
  58                         return 0x05; /* SIGTRAP */
  59                 case DBG_REASON_NOTHALTED:
  60                         return 0x0; /* no signal... shouldn't happen */
  61                 default:
  62                         ERROR("BUG: undefined debug reason");
  63                         exit(-1);
  64         }
  65 }
  66
  67 int gdb_get_char(connection_t *connection, int* next_char)
  68 {
  69         gdb_connection_t *gdb_con = connection->priv;
  70         char *debug_buffer;
  71
  72         if (gdb_con->buf_cnt-- > 0)
  73         {
  74                 *next_char = *(gdb_con->buf_p++);
  75                 if (gdb_con->buf_cnt > 0)
  76                         connection->input_pending = 1;
  77                 else
  78                         connection->input_pending = 0;
  79
  80 #ifdef _DEBUG_GDB_IO_
  81                 DEBUG("returned char '%c' (0x%2.2x)", *next_char, *next_char);
  82 #endif
  83
  84                 return ERROR_OK;
  85         }
  86
  87         while ((gdb_con->buf_cnt = read_socket(connection->fd, gdb_con->buffer, GDB_BUFFER_SIZE)) <= 0)
  88         {
  89                 if (gdb_con->buf_cnt == 0)
  90                         return ERROR_SERVER_REMOTE_CLOSED;
  91
  92 #ifdef _WIN32
  93                 errno = WSAGetLastError();
  94
  95                 switch(errno)
  96                 {
  97                         case WSAEWOULDBLOCK:
  98                                 usleep(1000);
  99                                 break;
 100                         case WSAECONNABORTED:
 101                                 return ERROR_SERVER_REMOTE_CLOSED;
 102                         default:
 103                                 ERROR("read: %d", errno);
 104                                 exit(-1);
 105                 }
 106 #else
 107                 switch(errno)
 108                 {
 109                         case EAGAIN:
 110                                 usleep(1000);
 111                                 break;
 112                         case ECONNABORTED:
 113                                 return ERROR_SERVER_REMOTE_CLOSED;
 114                         case ECONNRESET:
 115                                 return ERROR_SERVER_REMOTE_CLOSED;
 116                         default:
 117                                 ERROR("read: %s", strerror(errno));
 118                                 exit(-1);
 119                 }
 120 #endif
 121         }
 122
 123         debug_buffer = malloc(gdb_con->buf_cnt + 1);
 124         memcpy(debug_buffer, gdb_con->buffer, gdb_con->buf_cnt);
 125         debug_buffer[gdb_con->buf_cnt] = 0;
 126         DEBUG("received '%s'", debug_buffer);
 127         free(debug_buffer);
 128
 129         gdb_con->buf_p = gdb_con->buffer;
 130         gdb_con->buf_cnt--;
 131         *next_char = *(gdb_con->buf_p++);
 132         if (gdb_con->buf_cnt > 0)
 133                 connection->input_pending = 1;
 134         else
 135                 connection->input_pending = 0;
 136 #ifdef _DEBUG_GDB_IO_
 137         DEBUG("returned char '%c' (0x%2.2x)", *next_char, *next_char);
 138 #endif
 139
 140         return ERROR_OK;
 141 }
 142
 143 int gdb_putback_char(connection_t *connection, int last_char)
 144 {
 145         gdb_connection_t *gdb_con = connection->priv;
 146
 147         if (gdb_con->buf_p > gdb_con->buffer)
 148         {
 149                 *(--gdb_con->buf_p) = last_char;
 150                 gdb_con->buf_cnt++;
 151         }
 152         else
 153         {
 154                 ERROR("BUG: couldn't put character back");
 155         }
 156
 157         return ERROR_OK;
 158 }
 159
 160 int gdb_put_packet(connection_t *connection, char *buffer, int len)
 161 {
 162         int i;
 163         unsigned char my_checksum = 0;
 164         char checksum[3];
 165         char *debug_buffer;
 166         int reply;
 167         int retval;
 168         gdb_connection_t *gdb_con = connection->priv;
 169
 170         for (i = 0; i < len; i++)
 171                 my_checksum += buffer[i];
 172
 173         while (1)
 174         {
 175
 176                 debug_buffer = malloc(len + 1);
 177                 memcpy(debug_buffer, buffer, len);
 178                 debug_buffer[len] = 0;
 179                 DEBUG("sending packet '$%s#%2.2x'", debug_buffer, my_checksum);
 180                 free(debug_buffer);
 181
 182                 write_socket(connection->fd, "$", 1);
 183                 if (len > 0)
 184                         write_socket(connection->fd, buffer, len);
 185                 write_socket(connection->fd, "#", 1);
 186
 187                 snprintf(checksum, 3, "%2.2x", my_checksum);
 188
 189                 write_socket(connection->fd, checksum, 2);
 190
 191                 if ((retval = gdb_get_char(connection, &reply)) != ERROR_OK)
 192                         return retval;
 193
 194                 if (reply == '+')
 195                         break;
 196                 else if (reply == '-')
 197                         WARNING("negative reply, retrying");
 198                 else if (reply == 0x3)
 199                 {
 200                         gdb_con->ctrl_c = 1;
 201                         if ((retval = gdb_get_char(connection, &reply)) != ERROR_OK)
 202                                 return retval;
 203                         if (reply == '+')
 204                                 break;
 205                         else if (reply == '-')
 206                                 WARNING("negative reply, retrying");
 207                         else
 208                         {
 209                                 ERROR("unknown character 0x%2.2x in reply, dropping connection", reply);
 210                                 return ERROR_SERVER_REMOTE_CLOSED;
 211                         }
 212                 }
 213                 else
 214                 {
 215                         ERROR("unknown character 0x%2.2x in reply, dropping connection", reply);
 216                         return ERROR_SERVER_REMOTE_CLOSED;
 217                 }
 218         }
 219
 220         return ERROR_OK;
 221 }
 222
 223 int gdb_get_packet(connection_t *connection, char *buffer, int *len)
 224 {
 225         int character;
 226         int count = 0;
 227         int retval;
 228         char checksum[3];
 229         unsigned char my_checksum = 0;
 230         gdb_connection_t *gdb_con = connection->priv;
 231
 232         while (1)
 233         {
 234                 do
 235                 {
 236                         if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
 237                                 return retval;
 238
 239                         DEBUG("character: '%c'", character);
 240
 241                         switch (character)
 242                         {
 243                                 case '$':
 244                                         break;
 245                                 case '+':
 246                                         WARNING("acknowledgment received, but no packet pending");
 247                                         break;
 248                                 case '-':
 249                                         WARNING("negative acknowledgment, but no packet pending");
 250                                         break;
 251                                 case 0x3:
 252                                         gdb_con->ctrl_c = 1;
 253                                         *len = 0;
 254                                         return ERROR_OK;
 255                                 default:
 256                                         WARNING("ignoring character 0x%x", character);
 257                                         break;
 258                         }
 259                 } while (character != '$');
 260
 261                 my_checksum = 0;
 262
 263                 do
 264                 {
 265                         if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
 266                                 return retval;
 267
 268                         if (character == '#') break;
 269
 270                         if (character == '}')
 271                         {
 272                                 /* data transmitted in binary mode (X packet)
 273                                  * uses 0x7d as escape character */
 274                                 my_checksum += character & 0xff;
 275                                 if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
 276                                         return retval;
 277                                 my_checksum += character & 0xff;
 278                                 buffer[count++] = (character ^ 0x20) & 0xff;
 279                         }
 280                         else
 281                         {
 282                                 my_checksum += character & 0xff;
 283                                 buffer[count++] = character & 0xff;
 284                         }
 285
 286                         if (count > *len)
 287                         {
 288                                 ERROR("packet buffer too small");
 289                                 return ERROR_GDB_BUFFER_TOO_SMALL;
 290                         }
 291                 } while (1);
 292
 293                 *len = count;
 294
 295                 if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
 296                         return retval;
 297                 checksum[0] = character;
 298                 if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
 299                         return retval;
 300                 checksum[1] = character;
 301                 checksum[2] = 0;
 302
 303                 if (my_checksum == strtoul(checksum, NULL, 16))
 304                 {
 305                         write_socket(connection->fd, "+", 1);
 306                         break;
 307                 }
 308
 309                 WARNING("checksum error, requesting retransmission");
 310                 write_socket(connection->fd, "-", 1);
 311         }
 312
 313         return ERROR_OK;
 314 }
 315
 316 int gdb_output(struct command_context_s *context, char* line)
 317 {
 318         connection_t *connection = context->output_handler_priv;
 319         char *hex_buffer;
 320         int i, bin_size;
 321
 322         bin_size = strlen(line);
 323
 324         hex_buffer = malloc(bin_size*2 + 4);
 325
 326         hex_buffer[0] = 'O';
 327         for (i=0; i<bin_size; i++)
 328                 snprintf(hex_buffer + 1 + i*2, 3, "%2.2x", line[i]);
 329         hex_buffer[bin_size*2+1] = '0';
 330         hex_buffer[bin_size*2+2] = 'a';
 331         hex_buffer[bin_size*2+3] = 0x0;
 332
 333         gdb_put_packet(connection, hex_buffer, bin_size*2 + 3);
 334
 335         free(hex_buffer);
 336         return ERROR_OK;
 337 }
 338
 339 int gdb_target_callback_event_handler(struct target_s *target, enum target_event event, void *priv)
 340 {
 341         connection_t *connection = priv;
 342         gdb_connection_t *gdb_connection = connection->priv;
 343         char sig_reply[4];
 344         int signal;
 345
 346         switch (event)
 347         {
 348                 case TARGET_EVENT_HALTED:
 349                         if (gdb_connection->frontend_state == TARGET_RUNNING)
 350                         {
 351                                 if (gdb_connection->ctrl_c)
 352                                 {
 353                                         signal = 0x2;
 354                                         gdb_connection->ctrl_c = 0;
 355                                 }
 356                                 else
 357                                 {
 358                                         signal = gdb_last_signal(target);
 359                                 }
 360
 361                                 snprintf(sig_reply, 4, "T%2.2x", signal);
 362                                 gdb_put_packet(connection, sig_reply, 3);
 363                                 gdb_connection->frontend_state = TARGET_HALTED;
 364                         }
 365                         break;
 366                 case TARGET_EVENT_RESUMED:
 367                         if (gdb_connection->frontend_state == TARGET_HALTED)
 368                         {
 369                                 gdb_connection->frontend_state = TARGET_RUNNING;
 370                         }
 371                         break;
 372                 default:
 373                         break;
 374         }
 375
 376         return ERROR_OK;
 377 }
 378
 379 int gdb_new_connection(connection_t *connection)
 380 {
 381         gdb_connection_t *gdb_connection = malloc(sizeof(gdb_connection_t));
 382         gdb_service_t *gdb_service = connection->service->priv;
 383         int retval;
 384         int initial_ack;
 385
 386         connection->priv = gdb_connection;
 387
 388         /* initialize gdb connection information */
 389         gdb_connection->buf_p = gdb_connection->buffer;
 390         gdb_connection->buf_cnt = 0;
 391         gdb_connection->ctrl_c = 0;
 392         gdb_connection->frontend_state = TARGET_HALTED;
 393         gdb_connection->vflash_image = NULL;
 394
 395         /* output goes through gdb connection */
 396         command_set_output_handler(connection->cmd_ctx, gdb_output, connection);
 397
 398         /* register callback to be informed about target events */
 399         target_register_event_callback(gdb_target_callback_event_handler, connection);
 400
 401         /* a gdb session just attached, put the target in halt mode */
 402         if (((retval = gdb_service->target->type->halt(gdb_service->target)) != ERROR_OK) &&
 403                         (retval != ERROR_TARGET_ALREADY_HALTED))
 404         {
 405                 ERROR("error when trying to halt target");
 406                 exit(-1);
 407         }
 408
 409         while (gdb_service->target->state != TARGET_HALTED)
 410         {
 411                 gdb_service->target->type->poll(gdb_service->target);
 412         }
 413
 414         /* remove the initial ACK from the incoming buffer */
 415         if ((retval = gdb_get_char(connection, &initial_ack)) != ERROR_OK)
 416                 return retval;
 417
 418         if (initial_ack != '+')
 419                 gdb_putback_char(connection, initial_ack);
 420
 421         return ERROR_OK;
 422 }
 423
 424 int gdb_connection_closed(connection_t *connection)
 425 {
 426         gdb_service_t *gdb_service = connection->service->priv;
 427         gdb_connection_t *gdb_connection = connection->priv;
 428
 429         /* see if an image built with vFlash commands is left */
 430         if (gdb_connection->vflash_image)
 431         {
 432                 image_close(gdb_connection->vflash_image);
 433                 free(gdb_connection->vflash_image);
 434                 gdb_connection->vflash_image = NULL;
 435         }
 436
 437         /* if this connection registered a debug-message receiver delete it */
 438         delete_debug_msg_receiver(connection->cmd_ctx, gdb_service->target);
 439
 440         if (connection->priv)
 441                 free(connection->priv);
 442         else
 443                 ERROR("BUG: connection->priv == NULL");
 444
 445         target_unregister_event_callback(gdb_target_callback_event_handler, connection);
 446
 447         return ERROR_OK;
 448 }
 449
 450 void gdb_send_error(connection_t *connection, u8 the_error)
 451 {
 452         char err[4];
 453         snprintf(err, 4, "E%2.2X", the_error );
 454         gdb_put_packet(connection, err, 3);
 455 }
 456
 457 int gdb_last_signal_packet(connection_t *connection, target_t *target, char* packet, int packet_size)
 458 {
 459         char sig_reply[4];
 460         int signal;
 461
 462         signal = gdb_last_signal(target);
 463
 464         snprintf(sig_reply, 4, "S%2.2x", signal);
 465         gdb_put_packet(connection, sig_reply, 3);
 466
 467         return ERROR_OK;
 468 }
 469
 470 void gdb_str_to_target(target_t *target, char *str, char *tstr)
 471 {
 472         int str_len = strlen(str);
 473         int i;
 474
 475         if (str_len % 2)
 476         {
 477                 ERROR("BUG: gdb value with uneven number of characters encountered: %s", str);
 478                 exit(-1);
 479         }
 480
 481         if (target->endianness == TARGET_LITTLE_ENDIAN)
 482         {
 483                 for (i = 0; i < str_len; i+=2)
 484                 {
 485                         tstr[str_len - i - 1] = str[i + 1];
 486                         tstr[str_len - i - 2] = str[i];
 487                 }
 488         }
 489         else
 490         {
 491                 for (i = 0; i < str_len; i++)
 492                 {
 493                         tstr[i] = str[i];
 494                 }
 495         }
 496 }
 497
 498 void gdb_target_to_str(target_t *target, char *tstr, char *str)
 499 {
 500         int str_len = strlen(tstr);
 501         int i;
 502
 503         if (str_len % 2)
 504         {
 505                 ERROR("BUG: gdb value with uneven number of characters encountered");
 506                 exit(-1);
 507         }
 508
 509         if (target->endianness == TARGET_LITTLE_ENDIAN)
 510         {
 511                 for (i = 0; i < str_len; i+=2)
 512                 {
 513                         str[str_len - i - 1] = tstr[i + 1];
 514                         str[str_len - i - 2] = tstr[i];
 515                 }
 516         }
 517         else
 518         {
 519                 for (i = 0; i < str_len; i++)
 520                 {
 521                         str[i] = tstr[i];
 522                 }
 523         }
 524 }
 525
 526 int gdb_get_registers_packet(connection_t *connection, target_t *target, char* packet, int packet_size)
 527 {
 528         reg_t **reg_list;
 529         int reg_list_size;
 530         int retval;
 531         int reg_packet_size = 0;
 532         char *reg_packet;
 533         char *reg_packet_p;
 534         int i;
 535
 536         DEBUG("-");
 537
 538         if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
 539         {
 540                 switch (retval)
 541                 {
 542                         case ERROR_TARGET_NOT_HALTED:
 543                                 ERROR("gdb requested registers but we're not halted, dropping connection");
 544                                 return ERROR_SERVER_REMOTE_CLOSED;
 545                         default:
 546                                 /* this is a bug condition - get_gdb_reg_list() may not return any other error */
 547                                 ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
 548                                 exit(-1);
 549                 }
 550         }
 551
 552         for (i = 0; i < reg_list_size; i++)
 553         {
 554                 reg_packet_size += reg_list[i]->size;
 555         }
 556
 557         reg_packet = malloc(CEIL(reg_packet_size, 8) * 2);
 558         reg_packet_p = reg_packet;
 559
 560         for (i = 0; i < reg_list_size; i++)
 561         {
 562                 char *hex_buf = buf_to_str(reg_list[i]->value, reg_list[i]->size, 16);
 563                 DEBUG("hex_buf: %s", hex_buf);
 564                 gdb_str_to_target(target, hex_buf, reg_packet_p);
 565                 reg_packet_p += CEIL(reg_list[i]->size, 8) * 2;
 566                 free(hex_buf);
 567         }
 568
 569         reg_packet_p = strndup(reg_packet, CEIL(reg_packet_size, 8) * 2);
 570         DEBUG("reg_packet: %s", reg_packet_p);
 571         free(reg_packet_p);
 572
 573         gdb_put_packet(connection, reg_packet, CEIL(reg_packet_size, 8) * 2);
 574         free(reg_packet);
 575
 576         free(reg_list);
 577
 578         return ERROR_OK;
 579 }
 580
 581 int gdb_set_registers_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 582 {
 583         int i;
 584         reg_t **reg_list;
 585         int reg_list_size;
 586         int retval;
 587         char *packet_p;
 588
 589         DEBUG("-");
 590
 591         /* skip command character */
 592         packet++;
 593         packet_size--;
 594
 595         if (packet_size % 2)
 596         {
 597                 WARNING("GDB set_registers packet with uneven characters received, dropping connection");
 598                 return ERROR_SERVER_REMOTE_CLOSED;
 599         }
 600
 601         if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
 602         {
 603                 switch (retval)
 604                 {
 605                         case ERROR_TARGET_NOT_HALTED:
 606                                 ERROR("gdb tried to registers but we're not halted, dropping connection");
 607                                 return ERROR_SERVER_REMOTE_CLOSED;
 608                         default:
 609                                 /* this is a bug condition - get_gdb_reg_list() may not return any other error */
 610                                 ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
 611                                 exit(-1);
 612                 }
 613         }
 614
 615         packet_p = packet;
 616         for (i = 0; i < reg_list_size; i++)
 617         {
 618                 u8 *bin_buf;
 619                 char *hex_buf;
 620                 reg_arch_type_t *arch_type;
 621
 622                 /* convert from GDB-string (target-endian) to hex-string (big-endian) */
 623                 hex_buf = malloc(CEIL(reg_list[i]->size, 8) * 2);
 624                 gdb_target_to_str(target, packet_p, hex_buf);
 625
 626                 /* convert hex-string to binary buffer */
 627                 bin_buf = malloc(CEIL(reg_list[i]->size, 8));
 628                 str_to_buf(hex_buf, CEIL(reg_list[i]->size, 8) * 2, bin_buf, reg_list[i]->size, 16);
 629
 630                 /* get register arch_type, and call set method */
 631                 arch_type = register_get_arch_type(reg_list[i]->arch_type);
 632                 if (arch_type == NULL)
 633                 {
 634                         ERROR("BUG: encountered unregistered arch type");
 635                         exit(-1);
 636                 }
 637                 arch_type->set(reg_list[i], bin_buf);
 638
 639                 /* advance packet pointer */
 640                 packet_p += (CEIL(reg_list[i]->size, 8) * 2);
 641
 642                 free(bin_buf);
 643                 free(hex_buf);
 644         }
 645
 646         /* free reg_t *reg_list[] array allocated by get_gdb_reg_list */
 647         free(reg_list);
 648
 649         gdb_put_packet(connection, "OK", 2);
 650
 651         return ERROR_OK;
 652 }
 653
 654 int gdb_get_register_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 655 {
 656         char *reg_packet;
 657         int reg_num = strtoul(packet + 1, NULL, 16);
 658         reg_t **reg_list;
 659         int reg_list_size;
 660         int retval;
 661         char *hex_buf;
 662
 663         DEBUG("-");
 664
 665         if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
 666         {
 667                 switch (retval)
 668                 {
 669                         case ERROR_TARGET_NOT_HALTED:
 670                                 ERROR("gdb requested registers but we're not halted, dropping connection");
 671                                 return ERROR_SERVER_REMOTE_CLOSED;
 672                         default:
 673                                 /* this is a bug condition - get_gdb_reg_list() may not return any other error */
 674                                 ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
 675                                 exit(-1);
 676                 }
 677         }
 678
 679         if (reg_list_size <= reg_num)
 680         {
 681                 ERROR("gdb requested a non-existing register");
 682                 exit(-1);
 683         }
 684
 685         reg_packet = malloc(CEIL(reg_list[reg_num]->size, 8) * 2);
 686
 687         hex_buf = buf_to_str(reg_list[reg_num]->value, reg_list[reg_num]->size, 16);
 688
 689         gdb_str_to_target(target, hex_buf, reg_packet);
 690
 691         gdb_put_packet(connection, reg_packet, CEIL(reg_list[reg_num]->size, 8) * 2);
 692
 693         free(reg_list);
 694         free(reg_packet);
 695         free(hex_buf);
 696
 697         return ERROR_OK;
 698 }
 699
 700 int gdb_set_register_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 701 {
 702         char *separator;
 703         char *hex_buf;
 704         u8 *bin_buf;
 705         int reg_num = strtoul(packet + 1, &separator, 16);
 706         reg_t **reg_list;
 707         int reg_list_size;
 708         int retval;
 709         reg_arch_type_t *arch_type;
 710
 711         DEBUG("-");
 712
 713         if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
 714         {
 715                 switch (retval)
 716                 {
 717                         case ERROR_TARGET_NOT_HALTED:
 718                                 ERROR("gdb tried to set a register but we're not halted, dropping connection");
 719                                 return ERROR_SERVER_REMOTE_CLOSED;
 720                         default:
 721                                 /* this is a bug condition - get_gdb_reg_list() may not return any other error */
 722                                 ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
 723                                 exit(-1);
 724                 }
 725         }
 726
 727         if (reg_list_size < reg_num)
 728         {
 729                 ERROR("gdb requested a non-existing register");
 730                 return ERROR_SERVER_REMOTE_CLOSED;
 731         }
 732
 733         if (*separator != '=')
 734         {
 735                 ERROR("GDB 'set register packet', but no '=' following the register number");
 736                 return ERROR_SERVER_REMOTE_CLOSED;
 737         }
 738
 739         /* convert from GDB-string (target-endian) to hex-string (big-endian) */
 740         hex_buf = malloc(CEIL(reg_list[reg_num]->size, 8) * 2);
 741         gdb_target_to_str(target, separator + 1, hex_buf);
 742
 743         /* convert hex-string to binary buffer */
 744         bin_buf = malloc(CEIL(reg_list[reg_num]->size, 8));
 745         str_to_buf(hex_buf, CEIL(reg_list[reg_num]->size, 8) * 2, bin_buf, reg_list[reg_num]->size, 16);
 746
 747         /* get register arch_type, and call set method */
 748         arch_type = register_get_arch_type(reg_list[reg_num]->arch_type);
 749         if (arch_type == NULL)
 750         {
 751                 ERROR("BUG: encountered unregistered arch type");
 752                 exit(-1);
 753         }
 754         arch_type->set(reg_list[reg_num], bin_buf);
 755
 756         gdb_put_packet(connection, "OK", 2);
 757
 758         free(bin_buf);
 759         free(hex_buf);
 760         free(reg_list);
 761
 762         return ERROR_OK;
 763 }
 764
 765 int gdb_memory_packet_error(connection_t *connection, int retval)
 766 {
 767         switch (retval)
 768         {
 769                 case ERROR_TARGET_NOT_HALTED:
 770                         ERROR("gdb tried to read memory but we're not halted, dropping connection");
 771                         return ERROR_SERVER_REMOTE_CLOSED;
 772                         break;
 773                 case ERROR_TARGET_DATA_ABORT:
 774                         gdb_send_error(connection, EIO);
 775                         break;
 776                 case ERROR_TARGET_TRANSLATION_FAULT:
 777                         gdb_send_error(connection, EFAULT);
 778                         break;
 779                 case ERROR_TARGET_UNALIGNED_ACCESS:
 780                         gdb_send_error(connection, EFAULT);
 781                         break;
 782                 default:
 783                         ERROR("BUG: unexpected error %i", retval);
 784                         exit(-1);
 785         }
 786
 787         return ERROR_OK;
 788 }
 789
 790 int gdb_read_memory_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 791 {
 792         char *separator;
 793         u32 addr = 0;
 794         u32 len = 0;
 795
 796         u8 *buffer;
 797         char *hex_buffer;
 798
 799         int i;
 800         int retval;
 801
 802         /* skip command character */
 803         packet++;
 804
 805         addr = strtoul(packet, &separator, 16);
 806
 807         if (*separator != ',')
 808         {
 809                 ERROR("incomplete read memory packet received, dropping connection");
 810                 return ERROR_SERVER_REMOTE_CLOSED;
 811         }
 812
 813         len = strtoul(separator+1, NULL, 16);
 814
 815         buffer = malloc(len);
 816
 817         DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
 818
 819         switch (len)
 820         {
 821                 case 4:
 822                         if ((addr % 4) == 0)
 823                                 retval = target->type->read_memory(target, addr, 4, 1, buffer);
 824                         else
 825                                 retval = target->type->read_memory(target, addr, 1, len, buffer);
 826                         break;
 827                 case 2:
 828                         if ((addr % 2) == 0)
 829                                 retval = target->type->read_memory(target, addr, 2, 1, buffer);
 830                         else
 831                                 retval = target->type->read_memory(target, addr, 1, len, buffer);
 832                         break;
 833                 default:
 834                         if (((addr % 4) == 0) && ((len % 4) == 0))
 835                                 retval = target->type->read_memory(target, addr, 4, len / 4, buffer);
 836                         else
 837                                 retval = target->type->read_memory(target, addr, 1, len, buffer);
 838         }
 839
 840         if (retval == ERROR_OK)
 841         {
 842                 hex_buffer = malloc(len * 2 + 1);
 843
 844                 for (i=0; i<len; i++)
 845                         snprintf(hex_buffer + 2*i, 3, "%2.2x", buffer[i]);
 846
 847                 gdb_put_packet(connection, hex_buffer, len * 2);
 848
 849                 free(hex_buffer);
 850         }
 851         else
 852         {
 853                 if ((retval = gdb_memory_packet_error(connection, retval)) != ERROR_OK)
 854                         return retval;
 855         }
 856
 857         free(buffer);
 858
 859         return ERROR_OK;
 860 }
 861
 862 int gdb_write_memory_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 863 {
 864         char *separator;
 865         u32 addr = 0;
 866         u32 len = 0;
 867
 868         u8 *buffer;
 869
 870         int i;
 871         int retval;
 872
 873         /* skip command character */
 874         packet++;
 875
 876         addr = strtoul(packet, &separator, 16);
 877
 878         if (*separator != ',')
 879         {
 880                 ERROR("incomplete write memory packet received, dropping connection");
 881                 return ERROR_SERVER_REMOTE_CLOSED;
 882         }
 883
 884         len = strtoul(separator+1, &separator, 16);
 885
 886         if (*(separator++) != ':')
 887         {
 888                 ERROR("incomplete write memory packet received, dropping connection");
 889                 return ERROR_SERVER_REMOTE_CLOSED;
 890         }
 891
 892         buffer = malloc(len);
 893
 894         DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
 895
 896         for (i=0; i<len; i++)
 897         {
 898                 u32 tmp;
 899                 sscanf(separator + 2*i, "%2x", &tmp);
 900                 buffer[i] = tmp;
 901         }
 902
 903         retval = ERROR_OK;
 904         switch (len)
 905         {
 906                 /* handle sized writes */
 907                 case 4:
 908                         if ((addr % 4) == 0)
 909                                 retval = target->type->write_memory(target, addr, 4, 1, buffer);
 910                         else
 911                                 retval = target->type->write_memory(target, addr, 1, len, buffer);
 912                         break;
 913                 case 2:
 914                         if ((addr % 2) == 0)
 915                                 retval = target->type->write_memory(target, addr, 2, 1, buffer);
 916                         else
 917                                 retval = target->type->write_memory(target, addr, 1, len, buffer);
 918                         break;
 919                 case 3:
 920                 case 1:
 921                         retval = target->type->write_memory(target, addr, 1, len, buffer);
 922                         break;
 923                         /* handle bulk writes */
 924                 default:
 925                         retval = target_write_buffer(target, addr, len, buffer);
 926                         break;
 927         }
 928
 929         if (retval == ERROR_OK)
 930         {
 931                 gdb_put_packet(connection, "OK", 2);
 932         }
 933         else
 934         {
 935                 if ((retval = gdb_memory_packet_error(connection, retval)) != ERROR_OK)
 936                         return retval;
 937         }
 938
 939         free(buffer);
 940
 941         return ERROR_OK;
 942 }
 943
 944 int gdb_write_memory_binary_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 945 {
 946         char *separator;
 947         u32 addr = 0;
 948         u32 len = 0;
 949
 950         u8 *buffer;
 951         int retval;
 952
 953         /* skip command character */
 954         packet++;
 955
 956         addr = strtoul(packet, &separator, 16);
 957
 958         if (*separator != ',')
 959         {
 960                 ERROR("incomplete write memory binary packet received, dropping connection");
 961                 return ERROR_SERVER_REMOTE_CLOSED;
 962         }
 963
 964         len = strtoul(separator+1, &separator, 16);
 965
 966         if (*(separator++) != ':')
 967         {
 968                 ERROR("incomplete write memory binary packet received, dropping connection");
 969                 return ERROR_SERVER_REMOTE_CLOSED;
 970         }
 971
 972         retval = ERROR_OK;
 973         if( len ) {
 974
 975                 buffer = malloc(len);
 976
 977                 DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
 978
 979                 memcpy( buffer, separator, len );
 980
 981                 switch (len)
 982                 {
 983                         case 4:
 984                                 if ((addr % 4) == 0)
 985                                         retval = target->type->write_memory(target, addr, 4, 1, buffer);
 986                                 else
 987                                         retval = target->type->write_memory(target, addr, 1, len, buffer);
 988                                 break;
 989                         case 2:
 990                                 if ((addr % 2) == 0)
 991                                         retval = target->type->write_memory(target, addr, 2, 1, buffer);
 992                                 else
 993                                         retval = target->type->write_memory(target, addr, 1, len, buffer);
 994                                 break;
 995                         case 3:
 996                         case 1:
 997                                 retval = target->type->write_memory(target, addr, 1, len, buffer);
 998                                 break;
 999                         default:
1000                                 retval = target_write_buffer(target, addr, len, buffer);
1001                                 break;
1002                 }
1003
1004                 free(buffer);
1005         }
1006
1007         if (retval == ERROR_OK)
1008         {
1009                 gdb_put_packet(connection, "OK", 2);
1010         }
1011         else
1012         {
1013                 if ((retval = gdb_memory_packet_error(connection, retval)) != ERROR_OK)
1014                         return retval;
1015         }
1016
1017         return ERROR_OK;
1018 }
1019
1020 void gdb_step_continue_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
1021 {
1022         int current = 0;
1023         u32 address = 0x0;
1024
1025         DEBUG("-");
1026
1027         if (packet_size > 1)
1028         {
1029                 packet[packet_size] = 0;
1030                 address = strtoul(packet + 1, NULL, 16);
1031         }
1032         else
1033         {
1034                 current = 1;
1035         }
1036
1037         if (packet[0] == 'c')
1038         {
1039                 DEBUG("continue");
1040                 target->type->resume(target, current, address, 0, 0); /* resume at current address, don't handle breakpoints, not debugging */
1041         }
1042         else if (packet[0] == 's')
1043         {
1044                 DEBUG("step");
1045                 target->type->step(target, current, address, 0); /* step at current or address, don't handle breakpoints */
1046         }
1047 }
1048
1049 int gdb_bp_wp_packet_error(connection_t *connection, int retval)
1050 {
1051         switch (retval)
1052         {
1053                 case ERROR_TARGET_NOT_HALTED:
1054                         ERROR("gdb tried to set a breakpoint but we're not halted, dropping connection");
1055                         return ERROR_SERVER_REMOTE_CLOSED;
1056                         break;
1057                 case ERROR_TARGET_RESOURCE_NOT_AVAILABLE:
1058                         gdb_send_error(connection, EBUSY);
1059                         break;
1060                 default:
1061                         ERROR("BUG: unexpected error %i", retval);
1062                         exit(-1);
1063         }
1064
1065         return ERROR_OK;
1066 }
1067
1068 int gdb_breakpoint_watchpoint_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
1069 {
1070         int type;
1071         enum breakpoint_type bp_type = BKPT_SOFT /* dummy init to avoid warning */;
1072         enum watchpoint_rw wp_type;
1073         u32 address;
1074         u32 size;
1075         char *separator;
1076         int retval;
1077
1078         DEBUG("-");
1079
1080         type = strtoul(packet + 1, &separator, 16);
1081
1082         if (type == 0)  /* memory breakpoint */
1083                 bp_type = BKPT_SOFT;
1084         else if (type == 1) /* hardware breakpoint */
1085                 bp_type = BKPT_HARD;
1086         else if (type == 2) /* write watchpoint */
1087                 wp_type = WPT_WRITE;
1088         else if (type == 3) /* read watchpoint */
1089                 wp_type = WPT_READ;
1090         else if (type == 4) /* access watchpoint */
1091                 wp_type = WPT_ACCESS;
1092
1093         if (*separator != ',')
1094         {
1095                 ERROR("incomplete breakpoint/watchpoint packet received, dropping connection");
1096                 return ERROR_SERVER_REMOTE_CLOSED;
1097         }
1098
1099         address = strtoul(separator+1, &separator, 16);
1100
1101         if (*separator != ',')
1102         {
1103                 ERROR("incomplete breakpoint/watchpoint packet received, dropping connection");
1104                 return ERROR_SERVER_REMOTE_CLOSED;
1105         }
1106
1107         size = strtoul(separator+1, &separator, 16);
1108
1109         switch (type)
1110         {
1111                 case 0:
1112                 case 1:
1113                         if (packet[0] == 'Z')
1114                         {
1115                                 if ((retval = breakpoint_add(target, address, size, bp_type)) != ERROR_OK)
1116                                 {
1117                                         if ((retval = gdb_bp_wp_packet_error(connection, retval)) != ERROR_OK)
1118                                                 return retval;
1119                                 }
1120                                 else
1121                                 {
1122                                         gdb_put_packet(connection, "OK", 2);
1123                                 }
1124                         }
1125                         else
1126                         {
1127                                 breakpoint_remove(target, address);
1128                                 gdb_put_packet(connection, "OK", 2);
1129                         }
1130                         break;
1131                 case 2:
1132                 case 3:
1133                 case 4:
1134                 {
1135                         if (packet[0] == 'Z')
1136                         {
1137                                 if ((retval = watchpoint_add(target, address, size, type-2, 0, 0xffffffffu)) != ERROR_OK)
1138                                 {
1139                                         if ((retval = gdb_bp_wp_packet_error(connection, retval)) != ERROR_OK)
1140                                                 return retval;
1141                                 }
1142                                 else
1143                                 {
1144                                         gdb_put_packet(connection, "OK", 2);
1145                                 }
1146                         }
1147                         else
1148                         {
1149                                 watchpoint_remove(target, address);
1150                                 gdb_put_packet(connection, "OK", 2);
1151                         }
1152                         break;
1153                 }
1154                 default:
1155                         break;
1156         }
1157
1158         return ERROR_OK;
1159 }
1160
1161 void gdb_query_packet(connection_t *connection, char *packet, int packet_size)
1162 {
1163         command_context_t *cmd_ctx = connection->cmd_ctx;
1164
1165         if (strstr(packet, "qRcmd,"))
1166         {
1167                 if (packet_size > 6)
1168                 {
1169                         char *cmd;
1170                         int i;
1171                         cmd = malloc((packet_size - 6)/2 + 1);
1172                         for (i=0; i < (packet_size - 6)/2; i++)
1173                         {
1174                                 u32 tmp;
1175                                 sscanf(packet + 6 + 2*i, "%2x", &tmp);
1176                                 cmd[i] = tmp;
1177                         }
1178                         cmd[(packet_size - 6)/2] = 0x0;
1179                         command_run_line(cmd_ctx, cmd);
1180                         free(cmd);
1181                 }
1182                 gdb_put_packet(connection, "OK", 2);
1183                 return;
1184         }
1185
1186         gdb_put_packet(connection, "", 0);
1187 }
1188
1189 int gdb_v_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
1190 {
1191         gdb_connection_t *gdb_connection = connection->priv;
1192         gdb_service_t *gdb_service = connection->service->priv;
1193         int result;
1194
1195         if (strstr(packet, "vFlashErase:"))
1196         {
1197                 unsigned long addr;
1198                 unsigned long length;
1199                 char *parse = packet + 12;
1200                 if (*parse == '\0')
1201                 {
1202                         ERROR("incomplete vFlashErase packet received, dropping connection");
1203                         return ERROR_SERVER_REMOTE_CLOSED;
1204                 }
1205
1206                 addr = strtoul(parse, &parse, 16);
1207
1208                 if (*(parse++) != ',' || *parse == '\0')
1209                 {
1210                         ERROR("incomplete vFlashErase packet received, dropping connection");
1211                         return ERROR_SERVER_REMOTE_CLOSED;
1212                 }
1213
1214                 length = strtoul(parse, &parse, 16);
1215
1216                 if (*parse != '\0')
1217                 {
1218                         ERROR("incomplete vFlashErase packet received, dropping connection");
1219                         return ERROR_SERVER_REMOTE_CLOSED;
1220                 }
1221
1222                 /* perform erase */
1223                 if ((result = flash_erase(gdb_service->target, addr, length)) != ERROR_OK)
1224                 {
1225                         /* GDB doesn't evaluate the actual error number returned,
1226                          * treat a failed erase as an I/O error
1227                          */
1228                         gdb_send_error(connection, EIO);
1229                         ERROR("flash_erase returned %i", result);
1230                 }
1231                 else
1232                         gdb_put_packet(connection, "OK", 2);
1233
1234                 return ERROR_OK;
1235         }
1236
1237         if (strstr(packet, "vFlashWrite:"))
1238         {
1239                 unsigned long addr;
1240                 unsigned long length;
1241                 char *parse = packet + 12;
1242
1243                 if (*parse == '\0')
1244                 {
1245                         ERROR("incomplete vFlashErase packet received, dropping connection");
1246                         return ERROR_SERVER_REMOTE_CLOSED;
1247                 }
1248                 addr = strtoul(parse, &parse, 16);
1249                 if (*(parse++) != ':')
1250                 {
1251                         ERROR("incomplete vFlashErase packet received, dropping connection");
1252                         return ERROR_SERVER_REMOTE_CLOSED;
1253                 }
1254                 length = packet_size - (parse - packet);
1255
1256                 /* create a new image if there isn't already one */
1257                 if (gdb_connection->vflash_image == NULL)
1258                 {
1259                         gdb_connection->vflash_image = malloc(sizeof(image_t));
1260                         image_open(gdb_connection->vflash_image, "", "build");
1261                 }
1262
1263                 /* create new section with content from packet buffer */
1264                 image_add_section(gdb_connection->vflash_image, addr, length, 0x0, (u8*)parse);
1265
1266                 gdb_put_packet(connection, "OK", 2);
1267
1268                 return ERROR_OK;
1269         }
1270
1271         if (!strcmp(packet, "vFlashDone"))
1272         {
1273                 u32 image_size;
1274                 char *error_str;
1275                 u32 *failed = malloc(sizeof(u32) * gdb_connection->vflash_image->num_sections);
1276
1277                 /* process the flashing buffer */
1278                 if ((result = flash_write(gdb_service->target, gdb_connection->vflash_image, &image_size, &error_str, failed)) != ERROR_OK)
1279                 {
1280                         if (result == ERROR_FLASH_DST_OUT_OF_BANK)
1281                                 gdb_put_packet(connection, "E.memtype", 9);
1282                         else
1283                                 gdb_send_error(connection, EIO);
1284
1285                         ERROR("flash writing failed: %s", error_str);
1286                         free(error_str);
1287                 }
1288                 else
1289                 {
1290                         DEBUG("wrote %u bytes from vFlash image to flash", image_size);
1291                         gdb_put_packet(connection, "OK", 2);
1292                 }
1293
1294                 free(failed);
1295
1296                 image_close(gdb_connection->vflash_image);
1297                 free(gdb_connection->vflash_image);
1298                 gdb_connection->vflash_image = NULL;
1299
1300                 return ERROR_OK;
1301         }
1302
1303         gdb_put_packet(connection, "", 0);
1304         return ERROR_OK;
1305 }
1306
1307 int gdb_input(connection_t *connection)
1308 {
1309         gdb_service_t *gdb_service = connection->service->priv;
1310         target_t *target = gdb_service->target;
1311         char packet[GDB_BUFFER_SIZE];
1312         int packet_size;
1313         int retval;
1314         gdb_connection_t *gdb_con = connection->priv;
1315
1316         /* drain input buffer */
1317         do
1318         {
1319                 packet_size = GDB_BUFFER_SIZE-1;
1320                 if ((retval = gdb_get_packet(connection, packet, &packet_size)) != ERROR_OK)
1321                 {
1322                         switch (retval)
1323                         {
1324                                 case ERROR_GDB_BUFFER_TOO_SMALL:
1325                                         ERROR("BUG: buffer supplied for gdb packet was too small");
1326                                         exit(-1);
1327                                 case ERROR_SERVER_REMOTE_CLOSED:
1328                                         return ERROR_SERVER_REMOTE_CLOSED;
1329                                 default:
1330                                         ERROR("BUG: unexpected error");
1331                                         exit(-1);
1332                         }
1333                 }
1334
1335                 /* terminate with zero */
1336                 packet[packet_size] = 0;
1337
1338                 DEBUG("recevied packet: '%s'", packet);
1339
1340                 if (packet_size > 0)
1341                 {
1342                         retval = ERROR_OK;
1343                         switch (packet[0])
1344                         {
1345                                 case 'H':
1346                                         /* Hct... -- set thread
1347                                          * we don't have threads, send empty reply */
1348                                         gdb_put_packet(connection, NULL, 0);
1349                                         break;
1350                                 case 'q':
1351                                         gdb_query_packet(connection, packet, packet_size);
1352                                         break;
1353                                 case 'g':
1354                                         retval = gdb_get_registers_packet(connection, target, packet, packet_size);
1355                                         break;
1356                                 case 'G':
1357                                         retval = gdb_set_registers_packet(connection, target, packet, packet_size);
1358                                         break;
1359                                 case 'p':
1360                                         retval = gdb_get_register_packet(connection, target, packet, packet_size);
1361                                         break;
1362                                 case 'P':
1363                                         retval = gdb_set_register_packet(connection, target, packet, packet_size);
1364                                         break;
1365                                 case 'm':
1366                                         retval = gdb_read_memory_packet(connection, target, packet, packet_size);
1367                                         break;
1368                                 case 'M':
1369                                         retval = gdb_write_memory_packet(connection, target, packet, packet_size);
1370                                         break;
1371                                 case 'z':
1372                                 case 'Z':
1373                                         retval = gdb_breakpoint_watchpoint_packet(connection, target, packet, packet_size);
1374                                         break;
1375                                 case '?':
1376                                         gdb_last_signal_packet(connection, target, packet, packet_size);
1377                                         break;
1378                                 case 'c':
1379                                 case 's':
1380                                         gdb_step_continue_packet(connection, target, packet, packet_size);
1381                                         break;
1382                                 case 'v':
1383                                         retval = gdb_v_packet(connection, target, packet, packet_size);
1384                                         break;
1385                                 case 'D':
1386                                         target->type->resume(target, 1, 0, 1, 0);
1387                                         gdb_put_packet(connection, "OK", 2);
1388                                         break;
1389                                 case 'X':
1390                                         if ((retval = gdb_write_memory_binary_packet(connection, target, packet, packet_size)) != ERROR_OK)
1391                                                 return retval;
1392                                         break;
1393                                 case 'k':
1394                                         gdb_put_packet(connection, "OK", 2);
1395                                         return ERROR_SERVER_REMOTE_CLOSED;
1396                                 default:
1397                                         /* ignore unkown packets */
1398                                         DEBUG("ignoring 0x%2.2x packet", packet[0]);
1399                                         gdb_put_packet(connection, NULL, 0);
1400                                         break;
1401                         }
1402
1403                         /* if a packet handler returned an error, exit input loop */
1404                         if (retval != ERROR_OK)
1405                                 return retval;
1406                 }
1407
1408                 if (gdb_con->ctrl_c)
1409                 {
1410                         if (target->state == TARGET_RUNNING)
1411                         {
1412                                 target->type->halt(target);
1413                                 gdb_con->ctrl_c = 0;
1414                         }
1415                 }
1416
1417         } while (gdb_con->buf_cnt > 0);
1418
1419         return ERROR_OK;
1420 }
1421
1422 int gdb_init()
1423 {
1424         gdb_service_t *gdb_service;
1425         target_t *target = targets;
1426         int i = 0;
1427
1428         if (!target)
1429         {
1430                 WARNING("no gdb ports allocated as no target has been specified");
1431                 return ERROR_OK;
1432         }
1433
1434         if (gdb_port == 0)
1435         {
1436                 WARNING("no gdb port specified, using default port 3333");
1437                 gdb_port = 3333;
1438         }
1439
1440         while (target)
1441         {
1442                 char service_name[8];
1443
1444                 snprintf(service_name, 8, "gdb-%2.2i", i);
1445
1446                 gdb_service = malloc(sizeof(gdb_service_t));
1447                 gdb_service->target = target;
1448
1449                 add_service("gdb", CONNECTION_GDB, gdb_port + i, 1, gdb_new_connection, gdb_input, gdb_connection_closed, gdb_service);
1450
1451                 DEBUG("gdb service for target %s at port %i", target->type->name, gdb_port + i);
1452
1453                 i++;
1454                 target = target->next;
1455         }
1456
1457         return ERROR_OK;
1458 }
1459
1460 /* daemon configuration command gdb_port */
1461 int handle_gdb_port_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1462 {
1463         if (argc == 0)
1464                 return ERROR_OK;
1465
1466         /* only if the port wasn't overwritten by cmdline */
1467         if (gdb_port == 0)
1468                 gdb_port = strtoul(args[0], NULL, 0);
1469
1470         return ERROR_OK;
1471 }
1472
1473 int gdb_register_commands(command_context_t *command_context)
1474 {
1475         register_command(command_context, NULL, "gdb_port", handle_gdb_port_command,
1476                         COMMAND_CONFIG, "");
1477
1478         return ERROR_OK;
1479 }