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