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
  33 #define __USE_GNU
  34 #include <string.h>
  35 #include <errno.h>
  36 #include <unistd.h>
  37 #include <stdlib.h>
  38
  39 #if 0
  40 #define _DEBUG_GDB_IO_
  41 #endif
  42
  43 static unsigned short gdb_port;
  44
  45 int gdb_last_signal(target_t *target)
  46 {
  47         switch (target->debug_reason)
  48         {
  49                 case DBG_REASON_DBGRQ:
  50                         return 0x2; /* SIGINT */
  51                 case DBG_REASON_BREAKPOINT:
  52                 case DBG_REASON_WATCHPOINT:
  53                 case DBG_REASON_WPTANDBKPT:
  54                         return 0x05; /* SIGTRAP */
  55                 case DBG_REASON_SINGLESTEP:
  56                         return 0x05; /* SIGTRAP */
  57                 case DBG_REASON_NOTHALTED:
  58                         return 0x0; /* no signal... shouldn't happen */
  59                 default:
  60                         ERROR("BUG: undefined debug reason");
  61                         exit(-1);
  62         }
  63 }
  64
  65 int gdb_get_char(connection_t *connection, int* next_char)
  66 {
  67         gdb_connection_t *gdb_con = connection->priv;
  68         char *debug_buffer;
  69
  70         if (gdb_con->buf_cnt-- > 0)
  71         {
  72                 *next_char = *(gdb_con->buf_p++);
  73                 if (gdb_con->buf_cnt > 0)
  74                         connection->input_pending = 1;
  75                 else
  76                         connection->input_pending = 0;
  77
  78 #ifdef _DEBUG_GDB_IO_
  79                 DEBUG("returned char '%c' (0x%2.2x)", *next_char, *next_char);
  80 #endif
  81
  82                 return ERROR_OK;
  83         }
  84
  85         while ((gdb_con->buf_cnt = read_socket(connection->fd, gdb_con->buffer, GDB_BUFFER_SIZE)) <= 0)
  86         {
  87                 if (gdb_con->buf_cnt == 0)
  88                         return ERROR_SERVER_REMOTE_CLOSED;
  89
  90 #ifdef _WIN32
  91                 errno = WSAGetLastError();
  92
  93                 switch(errno)
  94                 {
  95                         case WSAEWOULDBLOCK:
  96                                 usleep(1000);
  97                                 break;
  98                         case WSAECONNABORTED:
  99                                 return ERROR_SERVER_REMOTE_CLOSED;
 100                         default:
 101                                 ERROR("read: %d", strerror(errno));
 102                                 exit(-1);
 103                 }
 104 #else
 105                 switch(errno)
 106                 {
 107                         case EAGAIN:
 108                                 usleep(1000);
 109                                 break;
 110                         case ECONNABORTED:
 111                                 return ERROR_SERVER_REMOTE_CLOSED;
 112                         case ECONNRESET:
 113                                 return ERROR_SERVER_REMOTE_CLOSED;
 114                         default:
 115                                 ERROR("read: %s", strerror(errno));
 116                                 exit(-1);
 117                 }
 118 #endif
 119         }
 120
 121         debug_buffer = malloc(gdb_con->buf_cnt + 1);
 122         memcpy(debug_buffer, gdb_con->buffer, gdb_con->buf_cnt);
 123         debug_buffer[gdb_con->buf_cnt] = 0;
 124         DEBUG("received '%s'", debug_buffer);
 125         free(debug_buffer);
 126
 127         gdb_con->buf_p = gdb_con->buffer;
 128         gdb_con->buf_cnt--;
 129         *next_char = *(gdb_con->buf_p++);
 130         if (gdb_con->buf_cnt > 0)
 131                 connection->input_pending = 1;
 132         else
 133                 connection->input_pending = 0;
 134 #ifdef _DEBUG_GDB_IO_
 135                 DEBUG("returned char '%c' (0x%2.2x)", *next_char, *next_char);
 136 #endif
 137
 138         return ERROR_OK;
 139 }
 140
 141 int gdb_putback_char(connection_t *connection, int last_char)
 142 {
 143         gdb_connection_t *gdb_con = connection->priv;
 144
 145         if (gdb_con->buf_p > gdb_con->buffer)
 146         {
 147                 *(--gdb_con->buf_p) = last_char;
 148                 gdb_con->buf_cnt++;
 149         }
 150         else
 151         {
 152                 ERROR("BUG: couldn't put character back");
 153         }
 154
 155         return ERROR_OK;
 156 }
 157
 158 int gdb_put_packet(connection_t *connection, char *buffer, int len)
 159 {
 160         int i;
 161         unsigned char my_checksum = 0;
 162         char checksum[3];
 163         char *debug_buffer;
 164         int reply;
 165         int retval;
 166         gdb_connection_t *gdb_con = connection->priv;
 167
 168         for (i = 0; i < len; i++)
 169                 my_checksum += buffer[i];
 170
 171         while (1)
 172         {
 173
 174                 debug_buffer = malloc(len + 1);
 175                 memcpy(debug_buffer, buffer, len);
 176                 debug_buffer[len] = 0;
 177                 DEBUG("sending packet '$%s#%2.2x'", debug_buffer, my_checksum);
 178                 free(debug_buffer);
 179
 180                 write_socket(connection->fd, "$", 1);
 181                 if (len > 0)
 182                         write_socket(connection->fd, buffer, len);
 183                 write_socket(connection->fd, "#", 1);
 184
 185                 snprintf(checksum, 3, "%2.2x", my_checksum);
 186
 187                 write_socket(connection->fd, checksum, 2);
 188
 189                 if ((retval = gdb_get_char(connection, &reply)) != ERROR_OK)
 190                         return retval;
 191
 192                 if (reply == '+')
 193                         break;
 194                 else if (reply == '-')
 195                         WARNING("negative reply, retrying");
 196                 else if (reply == 0x3)
 197                 {
 198                         gdb_con->ctrl_c = 1;
 199                         if ((retval = gdb_get_char(connection, &reply)) != ERROR_OK)
 200                                 return retval;
 201                         if (reply == '+')
 202                                 break;
 203                         else if (reply == '-')
 204                                 WARNING("negative reply, retrying");
 205                         else
 206                         {
 207                                 ERROR("unknown character 0x%2.2x in reply, dropping connection", reply);
 208                                 return ERROR_SERVER_REMOTE_CLOSED;
 209                         }
 210                 }
 211                 else
 212                 {
 213                         ERROR("unknown character 0x%2.2x in reply, dropping connection", reply);
 214                         return ERROR_SERVER_REMOTE_CLOSED;
 215                 }
 216         }
 217
 218         return ERROR_OK;
 219 }
 220
 221 int gdb_get_packet(connection_t *connection, char *buffer, int *len)
 222 {
 223         int character;
 224         int count = 0;
 225         int retval;
 226         int first_char = 0;
 227         int packet_type;
 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( !first_char ) {
 269                                 packet_type = character;
 270                                 first_char = 1;
 271                         }
 272
 273                         if( packet_type == 'X' )
 274                         {
 275                                 switch (character)
 276                                 {
 277                                         case '#':
 278                                                 break;
 279                                         case 0x7d:
 280                                                 /* data transmitted in binary mode (X packet)
 281                                                 * uses 0x7d as escape character */
 282                                                 my_checksum += character & 0xff;
 283                                                 gdb_get_char(connection, &character);
 284                                                 my_checksum += character & 0xff;
 285                                                 buffer[count++] = (character ^ 0x20) & 0xff;
 286                                                 if (count > *len)
 287                                                 {
 288                                                         ERROR("packet buffer too small");
 289                                                         return ERROR_GDB_BUFFER_TOO_SMALL;
 290                                                 }
 291                                                 break;
 292                                         default:
 293                                                 buffer[count++] = character & 0xff;
 294                                                 my_checksum += character & 0xff;
 295                                                 if (count > *len)
 296                                                 {
 297                                                         ERROR("packet buffer too small");
 298                                                         return ERROR_GDB_BUFFER_TOO_SMALL;
 299                                                 }
 300                                                 break;
 301                                 }
 302                         }
 303                         else
 304                         {
 305                                 switch (character)
 306                                 {
 307                                         case '#':
 308                                                 break;
 309                                         case 0x3:
 310                                                 gdb_con->ctrl_c = 1;
 311                                                 break;
 312                                         default:
 313                                                 buffer[count++] = character & 0xff;
 314                                                 my_checksum += character & 0xff;
 315                                                 if (count > *len)
 316                                                 {
 317                                                         ERROR("packet buffer too small");
 318                                                         return ERROR_GDB_BUFFER_TOO_SMALL;
 319                                                 }
 320                                                 break;
 321                                 }
 322                         }
 323                 } while (character != '#');
 324
 325                 *len = count;
 326
 327                 if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
 328                         return retval;
 329                 checksum[0] = character;
 330                 if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
 331                         return retval;
 332                 checksum[1] = character;
 333                 checksum[2] = 0;
 334
 335                 if (my_checksum == strtoul(checksum, NULL, 16))
 336                 {
 337                         write_socket(connection->fd, "+", 1);
 338                         break;
 339                 }
 340
 341                 WARNING("checksum error, requesting retransmission");
 342                 write_socket(connection->fd, "-", 1);
 343         }
 344
 345         return ERROR_OK;
 346 }
 347
 348 int gdb_output(struct command_context_s *context, char* line)
 349 {
 350         connection_t *connection = context->output_handler_priv;
 351         char *hex_buffer;
 352         int i, bin_size;
 353
 354         bin_size = strlen(line);
 355
 356         hex_buffer = malloc(bin_size*2 + 4);
 357
 358         hex_buffer[0] = 'O';
 359         for (i=0; i<bin_size; i++)
 360                 snprintf(hex_buffer + 1 + i*2, 3, "%2.2x", line[i]);
 361         hex_buffer[bin_size*2+1] = '0';
 362         hex_buffer[bin_size*2+2] = 'a';
 363         hex_buffer[bin_size*2+3] = 0x0;
 364
 365         gdb_put_packet(connection, hex_buffer, bin_size*2 + 3);
 366
 367         free(hex_buffer);
 368         return ERROR_OK;
 369 }
 370
 371 int gdb_target_callback_event_handler(struct target_s *target, enum target_event event, void *priv)
 372 {
 373         connection_t *connection = priv;
 374         gdb_connection_t *gdb_connection = connection->priv;
 375         char sig_reply[4];
 376         int signal;
 377
 378         switch (event)
 379         {
 380                 case TARGET_EVENT_HALTED:
 381                         if (gdb_connection->frontend_state == TARGET_RUNNING)
 382                         {
 383                                 if (gdb_connection->ctrl_c)
 384                                 {
 385                                         signal = 0x2;
 386                                         gdb_connection->ctrl_c = 0;
 387                                 }
 388                                 else
 389                                 {
 390                                         signal = gdb_last_signal(target);
 391                                 }
 392
 393                                 snprintf(sig_reply, 4, "T%2.2x", signal);
 394                                 gdb_put_packet(connection, sig_reply, 3);
 395                                 gdb_connection->frontend_state = TARGET_HALTED;
 396                         }
 397                         break;
 398                 case TARGET_EVENT_RESUMED:
 399                         if (gdb_connection->frontend_state == TARGET_HALTED)
 400                         {
 401                                 gdb_connection->frontend_state = TARGET_RUNNING;
 402                         }
 403                         break;
 404                 default:
 405                         break;
 406         }
 407
 408         return ERROR_OK;
 409 }
 410
 411 int gdb_new_connection(connection_t *connection)
 412 {
 413         gdb_connection_t *gdb_connection = malloc(sizeof(gdb_connection_t));
 414         gdb_service_t *gdb_service = connection->service->priv;
 415         int retval;
 416         int initial_ack;
 417
 418         connection->priv = gdb_connection;
 419
 420         /* initialize gdb connection information */
 421         gdb_connection->buf_p = gdb_connection->buffer;
 422         gdb_connection->buf_cnt = 0;
 423         gdb_connection->ctrl_c = 0;
 424         gdb_connection->frontend_state = TARGET_HALTED;
 425
 426         /* output goes through gdb connection */
 427         command_set_output_handler(connection->cmd_ctx, gdb_output, connection);
 428
 429         /* register callback to be informed about target events */
 430         target_register_event_callback(gdb_target_callback_event_handler, connection);
 431
 432         /* a gdb session just attached, put the target in halt mode */
 433         if (((retval = gdb_service->target->type->halt(gdb_service->target)) != ERROR_OK) &&
 434                          (retval != ERROR_TARGET_ALREADY_HALTED))
 435         {
 436                 ERROR("error when trying to halt target");
 437                 exit(-1);
 438         }
 439
 440         while (gdb_service->target->state != TARGET_HALTED)
 441         {
 442                 gdb_service->target->type->poll(gdb_service->target);
 443         }
 444
 445         /* remove the initial ACK from the incoming buffer */
 446         if ((retval = gdb_get_char(connection, &initial_ack)) != ERROR_OK)
 447                 return retval;
 448
 449         if (initial_ack != '+')
 450                 gdb_putback_char(connection, initial_ack);
 451
 452         return ERROR_OK;
 453 }
 454
 455 int gdb_connection_closed(connection_t *connection)
 456 {
 457         if (connection->priv)
 458                 free(connection->priv);
 459         else
 460                 ERROR("BUG: connection->priv == NULL");
 461
 462         target_unregister_event_callback(gdb_target_callback_event_handler, connection);
 463
 464         return ERROR_OK;
 465 }
 466
 467 int gdb_last_signal_packet(connection_t *connection, target_t *target, char* packet, int packet_size)
 468 {
 469         char sig_reply[4];
 470         int signal;
 471
 472         signal = gdb_last_signal(target);
 473
 474         snprintf(sig_reply, 4, "S%2.2x", signal);
 475         gdb_put_packet(connection, sig_reply, 3);
 476
 477         return ERROR_OK;
 478 }
 479
 480 void gdb_get_registers_packet(connection_t *connection, target_t *target, char* packet, int packet_size)
 481 {
 482         reg_t **reg_list;
 483         int reg_list_size;
 484         int retval;
 485         int reg_packet_size = 0;
 486         char *reg_packet;
 487         char *reg_packet_p;
 488         int i;
 489
 490         DEBUG("");
 491
 492         if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
 493         {
 494                 switch (retval)
 495                 {
 496                         case ERROR_TARGET_NOT_HALTED:
 497                                 ERROR("gdb requested registers, but we're not halted");
 498                                 exit(-1);
 499                         default:
 500                                 ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
 501                                 exit(-1);
 502                 }
 503         }
 504
 505         for (i = 0; i < reg_list_size; i++)
 506         {
 507                 reg_packet_size += reg_list[i]->size;
 508         }
 509
 510         reg_packet = malloc(CEIL(reg_packet_size, 8) * 2);
 511         reg_packet_p = reg_packet;
 512
 513         for (i = 0; i < reg_list_size; i++)
 514         {
 515                 int j;
 516                 char *hex_buf = buf_to_char(reg_list[i]->value, reg_list[i]->size);
 517                 DEBUG("hex_buf: %s", hex_buf);
 518                 for (j = CEIL(reg_list[i]->size, 8) * 2; j > 0; j -= 2)
 519                 {
 520                         *reg_packet_p++ = hex_buf[j - 2];
 521                         *reg_packet_p++ = hex_buf[j - 1];
 522                 }
 523                 free(hex_buf);
 524         }
 525
 526         reg_packet_p = strndup(reg_packet, CEIL(reg_packet_size, 8) * 2);
 527         DEBUG("reg_packet: %s", reg_packet_p);
 528         free(reg_packet_p);
 529
 530         gdb_put_packet(connection, reg_packet, CEIL(reg_packet_size, 8) * 2);
 531         free(reg_packet);
 532
 533 }
 534
 535 void gdb_set_registers_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 536 {
 537         int i;
 538         reg_t **reg_list;
 539         int reg_list_size;
 540         int retval;
 541         char *packet_p;
 542
 543         DEBUG("");
 544
 545         /* skip command character */
 546         packet++;
 547         packet_size--;
 548
 549         if (packet_size % 2)
 550         {
 551                 WARNING("GDB set_registers packet with uneven characters received");
 552                 return;
 553         }
 554
 555         if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
 556         {
 557                 switch (retval)
 558                 {
 559                         case ERROR_TARGET_NOT_HALTED:
 560                                 ERROR("gdb requested registers, but we're not halted");
 561                                 exit(-1);
 562                         default:
 563                                 ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
 564                                 exit(-1);
 565                 }
 566         }
 567
 568         packet_p = packet;
 569         for (i = 0; i < reg_list_size; i++)
 570         {
 571                 char_to_buf(packet, CEIL(reg_list[i]->size, 8) * 2, reg_list[i]->value, reg_list[i]->size);
 572                 reg_list[i]->dirty = 1;
 573         }
 574
 575         gdb_put_packet(connection, "OK", 2);
 576 }
 577
 578 void gdb_get_register_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 579 {
 580         char *hex_buf;
 581         char *reg_packet;
 582         char *reg_packet_p;
 583         int reg_num = strtoul(packet + 1, NULL, 16);
 584         reg_t **reg_list;
 585         int reg_list_size;
 586         int retval;
 587         int i;
 588
 589         DEBUG("");
 590
 591         if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
 592         {
 593                 switch (retval)
 594                 {
 595                         case ERROR_TARGET_NOT_HALTED:
 596                                 ERROR("gdb requested registers, but we're not halted");
 597                                 exit(-1);
 598                         default:
 599                                 ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
 600                                 exit(-1);
 601                 }
 602         }
 603
 604         if (reg_list_size <= reg_num)
 605         {
 606                 ERROR("gdb requested a non-existing register");
 607                 exit(-1);
 608         }
 609
 610         hex_buf = buf_to_char(reg_list[reg_num]->value, reg_list[reg_num]->size);
 611         reg_packet = reg_packet_p = malloc(CEIL(reg_list[reg_num]->size, 8) * 2);
 612
 613         for (i = CEIL(reg_list[reg_num]->size, 8) * 2; i > 0; i -= 2)
 614         {
 615                 *reg_packet_p++ = hex_buf[i - 2];
 616                 *reg_packet_p++ = hex_buf[i - 1];
 617         }
 618
 619         gdb_put_packet(connection, reg_packet, CEIL(reg_list[reg_num]->size, 8) * 2);
 620
 621         free(reg_packet);
 622         free(hex_buf);
 623
 624 }
 625
 626 void gdb_set_register_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 627 {
 628         char *separator;
 629         int reg_num = strtoul(packet + 1, &separator, 16);
 630         reg_t **reg_list;
 631         int reg_list_size;
 632         int retval;
 633
 634         DEBUG("");
 635
 636         if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
 637         {
 638                 switch (retval)
 639                 {
 640                         case ERROR_TARGET_NOT_HALTED:
 641                                 ERROR("gdb requested registers, but we're not halted");
 642                                 exit(-1);
 643                         default:
 644                                 ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
 645                                 exit(-1);
 646                 }
 647         }
 648
 649         if (reg_list_size < reg_num)
 650         {
 651                 ERROR("gdb requested a non-existing register");
 652                 exit(-1);
 653         }
 654
 655         if (*separator != '=')
 656         {
 657                 ERROR("GDB set register packet, but no '=' following the register number");
 658                 exit(-1);
 659         }
 660
 661         char_to_buf(separator + 1, CEIL(reg_list[reg_num]->size, 8) * 2, reg_list[reg_num]->value, reg_list[reg_num]->size);
 662         reg_list[reg_num]->dirty = 1;
 663
 664         gdb_put_packet(connection, "OK", 2);
 665
 666 }
 667
 668 void gdb_read_memory_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 669 {
 670         char *separator;
 671         u32 addr = 0;
 672         u32 len = 0;
 673
 674         u8 *buffer;
 675         char *hex_buffer;
 676
 677         int i;
 678
 679         /* skip command character */
 680         packet++;
 681
 682         addr = strtoul(packet, &separator, 16);
 683
 684         if (*separator != ',')
 685                 return;
 686
 687         len = strtoul(separator+1, NULL, 16);
 688
 689         buffer = malloc(len);
 690
 691         DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
 692
 693         switch (len)
 694         {
 695                 case 4:
 696                         if ((addr % 4) == 0)
 697                                 target->type->read_memory(target, addr, 4, 1, buffer);
 698                         else
 699                                 target->type->read_memory(target, addr, 1, len, buffer);
 700                         break;
 701                 case 2:
 702                         if ((addr % 2) == 0)
 703                                 target->type->read_memory(target, addr, 2, 1, buffer);
 704                         else
 705                                 target->type->read_memory(target, addr, 1, len, buffer);
 706                         break;
 707                 default:
 708                         if (((addr % 4) == 0) && ((len % 4) == 0))
 709                                 target->type->read_memory(target, addr, 4, len / 4, buffer);
 710                         else
 711                                 target->type->read_memory(target, addr, 1, len, buffer);
 712         }
 713
 714         hex_buffer = malloc(len * 2 + 1);
 715
 716         for (i=0; i<len; i++)
 717                 snprintf(hex_buffer + 2*i, 3, "%2.2x", buffer[i]);
 718
 719         gdb_put_packet(connection, hex_buffer, len * 2);
 720
 721         free(hex_buffer);
 722         free(buffer);
 723 }
 724
 725 void gdb_write_memory_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 726 {
 727         char *separator;
 728         u32 addr = 0;
 729         u32 len = 0;
 730
 731         u8 *buffer;
 732
 733         int i;
 734
 735         /* skip command character */
 736         packet++;
 737
 738         addr = strtoul(packet, &separator, 16);
 739
 740         if (*separator != ',')
 741                 return;
 742
 743         len = strtoul(separator+1, &separator, 16);
 744
 745         if (*(separator++) != ':')
 746                 return;
 747
 748         buffer = malloc(len);
 749
 750         DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
 751
 752         for (i=0; i<len; i++)
 753         {
 754                 u32 tmp;
 755                 sscanf(separator + 2*i, "%2x", &tmp);
 756                 buffer[i] = tmp;
 757         }
 758
 759         switch (len)
 760         {
 761                 /* handle sized writes */
 762                 case 4:
 763                         if ((addr % 4) == 0)
 764                                 target->type->write_memory(target, addr, 4, 1, buffer);
 765                         else
 766                                 target->type->write_memory(target, addr, 1, len, buffer);
 767                         break;
 768                 case 2:
 769                         if ((addr % 2) == 0)
 770                                 target->type->write_memory(target, addr, 2, 1, buffer);
 771                         else
 772                                 target->type->write_memory(target, addr, 1, len, buffer);
 773                         break;
 774                 case 3:
 775                 case 1:
 776                         target->type->write_memory(target, addr, 1, len, buffer);
 777                         break;
 778                 /* handle bulk writes */
 779                 default:
 780                         target_write_buffer(target, addr, len, buffer);
 781                         break;
 782         }
 783
 784         gdb_put_packet(connection, "OK", 2);
 785
 786         free(buffer);
 787 }
 788
 789 void gdb_write_memory_binary_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 790 {
 791         char *separator;
 792         u32 addr = 0;
 793         u32 len = 0;
 794
 795         u8 *buffer;
 796
 797         /* skip command character */
 798         packet++;
 799
 800         addr = strtoul(packet, &separator, 16);
 801
 802         if (*separator != ',')
 803                 return;
 804
 805         len = strtoul(separator+1, &separator, 16);
 806
 807         if (*(separator++) != ':')
 808                 return;
 809
 810         if( len ) {
 811
 812                 buffer = malloc(len);
 813
 814                 DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
 815
 816                 memcpy( buffer, separator, len );
 817
 818                 switch (len)
 819                 {
 820                         case 4:
 821                                 if ((addr % 4) == 0)
 822                                         target->type->write_memory(target, addr, 4, 1, buffer);
 823                                 else
 824                                         target->type->write_memory(target, addr, 1, len, buffer);
 825                                 break;
 826                         case 2:
 827                                 if ((addr % 2) == 0)
 828                                         target->type->write_memory(target, addr, 2, 1, buffer);
 829                                 else
 830                                         target->type->write_memory(target, addr, 1, len, buffer);
 831                                 break;
 832                         case 3:
 833                         case 1:
 834                                 target->type->write_memory(target, addr, 1, len, buffer);
 835                                 break;
 836                         default:
 837                                 target_write_buffer(target, addr, len, buffer);
 838                                 break;
 839                 }
 840
 841                 free(buffer);
 842         }
 843
 844         gdb_put_packet(connection, "OK", 2);
 845 }
 846
 847 void gdb_step_continue_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 848 {
 849         int current = 0;
 850         u32 address = 0x0;
 851
 852         DEBUG("");
 853
 854         if (packet_size > 1)
 855         {
 856                 u32 address = 0;
 857                 packet[packet_size] = 0;
 858                 address = strtoul(packet + 1, NULL, 16);
 859         }
 860         else
 861         {
 862                 current = 1;
 863         }
 864
 865         if (packet[0] == 'c')
 866         {
 867                 DEBUG("continue");
 868                 target->type->resume(target, current, address, 0, 0); /* resume at current address, don't handle breakpoints, not debugging */
 869         }
 870         else if (packet[0] == 's')
 871         {
 872                 DEBUG("step");
 873                 target->type->step(target, current, address, 0); /* step at current or address, don't handle breakpoints */
 874         }
 875 }
 876
 877 void gdb_breakpoint_watchpoint_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 878 {
 879         int type;
 880         enum breakpoint_type bp_type;
 881         enum watchpoint_rw wp_type;
 882         u32 address;
 883         u32 size;
 884         char *separator;
 885         int retval;
 886
 887         DEBUG("");
 888
 889         type = strtoul(packet + 1, &separator, 16);
 890
 891         if (type == 0)  /* memory breakpoint */
 892                 bp_type = BKPT_SOFT;
 893         else if (type == 1) /* hardware breakpoint */
 894                 bp_type = BKPT_HARD;
 895         else if (type == 2) /* write watchpoint */
 896                 wp_type = WPT_WRITE;
 897         else if (type == 3) /* read watchpoint */
 898                 wp_type = WPT_READ;
 899         else if (type == 4) /* access watchpoint */
 900                 wp_type = WPT_ACCESS;
 901
 902         if (*separator != ',')
 903                 return;
 904
 905         address = strtoul(separator+1, &separator, 16);
 906
 907         if (*separator != ',')
 908                 return;
 909
 910         size = strtoul(separator+1, &separator, 16);
 911
 912         switch (type)
 913         {
 914                 case 0:
 915                 case 1:
 916                         if (packet[0] == 'Z')
 917                         {
 918                                 if ((retval = breakpoint_add(target, address, size, bp_type)) != ERROR_OK)
 919                                 {
 920                                         if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
 921                                         {
 922                                                 gdb_put_packet(connection, "E00", 3);
 923                                                 break;
 924                                         }
 925                                 }
 926                         }
 927                         else
 928                         {
 929                                 breakpoint_remove(target, address);
 930                         }
 931                         gdb_put_packet(connection, "OK", 2);
 932                         break;
 933                 case 2:
 934                 case 3:
 935                 case 4:
 936                 {
 937                         if (packet[0] == 'Z')
 938                                 watchpoint_add(target, address, size, type-2, 0, 0xffffffffu);
 939                         else
 940                                 watchpoint_remove(target, address);
 941                         gdb_put_packet(connection, "OK", 2);
 942                         break;
 943                 }
 944                 default:
 945                         break;
 946         }
 947
 948 }
 949
 950 void gdb_query_packet(connection_t *connection, char *packet, int packet_size)
 951 {
 952         command_context_t *cmd_ctx = connection->cmd_ctx;
 953         gdb_service_t *gdb_service = connection->service->priv;
 954         target_t *target = gdb_service->target;
 955
 956         if (strstr(packet, "qRcmd,"))
 957         {
 958                 if (packet_size > 6)
 959                 {
 960                         char *cmd;
 961                         int i;
 962                         cmd = malloc((packet_size - 6)/2 + 1);
 963                         for (i=0; i < (packet_size - 6)/2; i++)
 964                         {
 965                                 u32 tmp;
 966                                 sscanf(packet + 6 + 2*i, "%2x", &tmp);
 967                                 cmd[i] = tmp;
 968                         }
 969                         cmd[(packet_size - 6)/2] = 0x0;
 970                         command_run_line(cmd_ctx, cmd);
 971                         free(cmd);
 972                 }
 973                 gdb_put_packet(connection, "OK", 2);
 974                 return;
 975         }
 976
 977         gdb_put_packet(connection, "", 0);
 978 }
 979
 980 int gdb_input(connection_t *connection)
 981 {
 982         gdb_service_t *gdb_service = connection->service->priv;
 983         target_t *target = gdb_service->target;
 984         char packet[GDB_BUFFER_SIZE];
 985         int packet_size;
 986         int retval;
 987         gdb_connection_t *gdb_con = connection->priv;
 988
 989         /* drain input buffer */
 990         do
 991         {
 992                 packet_size = GDB_BUFFER_SIZE-1;
 993                 if ((retval = gdb_get_packet(connection, packet, &packet_size)) != ERROR_OK)
 994                 {
 995                         switch (retval)
 996                         {
 997                                 case ERROR_GDB_BUFFER_TOO_SMALL:
 998                                         ERROR("BUG: buffer supplied for gdb packet was too small");
 999                                         exit(-1);
1000                                 case ERROR_SERVER_REMOTE_CLOSED:
1001                                         return ERROR_SERVER_REMOTE_CLOSED;
1002                                 default:
1003                                         ERROR("unexpected error");
1004                                         exit(-1);
1005                         }
1006                 }
1007
1008                 /* terminate with zero */
1009                 packet[packet_size] = 0;
1010
1011                 DEBUG("recevied packet: '%s'", packet);
1012
1013                 if (packet_size > 0)
1014                 {
1015                         switch (packet[0])
1016                         {
1017                                 case 'H':
1018                                         /* Hct... -- set thread
1019                                         * we don't have threads, send empty reply */
1020                                         gdb_put_packet(connection, NULL, 0);
1021                                         break;
1022                                 case 'q':
1023                                         gdb_query_packet(connection, packet, packet_size);
1024                                         break;
1025                                 case 'g':
1026                                         gdb_get_registers_packet(connection, target, packet, packet_size);
1027                                         break;
1028                                 case 'G':
1029                                         gdb_set_registers_packet(connection, target, packet, packet_size);
1030                                         break;
1031                                 case 'p':
1032                                         gdb_get_register_packet(connection, target, packet, packet_size);
1033                                         break;
1034                                 case 'P':
1035                                         gdb_set_register_packet(connection, target, packet, packet_size);
1036                                         break;
1037                                 case 'm':
1038                                         gdb_read_memory_packet(connection, target, packet, packet_size);
1039                                         break;
1040                                 case 'M':
1041                                         gdb_write_memory_packet(connection, target, packet, packet_size);
1042                                         break;
1043                                 case 'z':
1044                                 case 'Z':
1045                                         gdb_breakpoint_watchpoint_packet(connection, target, packet, packet_size);
1046                                         break;
1047                                 case '?':
1048                                         gdb_last_signal_packet(connection, target, packet, packet_size);
1049                                 break;
1050                                 case 'c':
1051                                 case 's':
1052                                         gdb_step_continue_packet(connection, target, packet, packet_size);
1053                                         break;
1054                                 case 'D':
1055                                         target->type->resume(target, 1, 0, 1, 0);
1056                                         gdb_put_packet(connection, "OK", 2);
1057                                         break;
1058                                 case 'X':
1059                                         gdb_write_memory_binary_packet(connection, target, packet, packet_size);
1060                                         break;
1061                                 case 'k':
1062                                         gdb_put_packet(connection, "OK", 2);
1063                                         return ERROR_SERVER_REMOTE_CLOSED;
1064                                 default:
1065                                         /* ignore unkown packets */
1066                                         DEBUG("ignoring 0x%2.2x packet", packet[0]);
1067                                         gdb_put_packet(connection, NULL, 0);
1068                                         break;
1069                         }
1070                 }
1071
1072                 if (gdb_con->ctrl_c)
1073                 {
1074                         if (target->state == TARGET_RUNNING)
1075                         {
1076                                 target->type->halt(target);
1077                                 gdb_con->ctrl_c = 0;
1078                         }
1079                 }
1080
1081         } while (gdb_con->buf_cnt > 0);
1082
1083         return ERROR_OK;
1084 }
1085
1086 int gdb_init()
1087 {
1088         gdb_service_t *gdb_service;
1089         target_t *target = targets;
1090         int i = 0;
1091
1092         if (!target)
1093         {
1094                 WARNING("no gdb ports allocated as no target has been specified");
1095                 return ERROR_OK;
1096         }
1097
1098         if (gdb_port == 0)
1099         {
1100                 WARNING("no gdb port specified, using default port 3333");
1101                 gdb_port = 3333;
1102         }
1103
1104         while (target)
1105         {
1106                 char service_name[8];
1107
1108                 snprintf(service_name, 8, "gdb-%2.2i", i);
1109
1110                 gdb_service = malloc(sizeof(gdb_service_t));
1111                 gdb_service->target = target;
1112
1113                 add_service("gdb", CONNECTION_GDB, gdb_port + i, 1, gdb_new_connection, gdb_input, gdb_connection_closed, gdb_service);
1114
1115                 DEBUG("gdb service for target %s at port %i", target->type->name, gdb_port + i);
1116
1117                 i++;
1118                 target = target->next;
1119         }
1120
1121         return ERROR_OK;
1122 }
1123
1124 /* daemon configuration command gdb_port */
1125 int handle_gdb_port_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1126 {
1127         if (argc == 0)
1128                 return ERROR_OK;
1129
1130         /* only if the port wasn't overwritten by cmdline */
1131         if (gdb_port == 0)
1132                 gdb_port = strtoul(args[0], NULL, 0);
1133
1134         return ERROR_OK;
1135 }
1136
1137 int gdb_register_commands(command_context_t *command_context)
1138 {
1139         register_command(command_context, NULL, "gdb_port", handle_gdb_port_command,
1140                                          COMMAND_CONFIG, "");
1141
1142         return ERROR_OK;
1143 }