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