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 #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", 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 void gdb_send_error(connection_t *connection, u8 the_error)
 468 {
 469     char err[4];
 470     snprintf(err, 4, "E%2.2X", the_error );
 471     gdb_put_packet(connection, err, 3);
 472 }
 473
 474 int gdb_last_signal_packet(connection_t *connection, target_t *target, char* packet, int packet_size)
 475 {
 476         char sig_reply[4];
 477         int signal;
 478
 479         signal = gdb_last_signal(target);
 480
 481         snprintf(sig_reply, 4, "S%2.2x", signal);
 482         gdb_put_packet(connection, sig_reply, 3);
 483
 484         return ERROR_OK;
 485 }
 486
 487 void gdb_str_to_target(target_t *target, char *str, char *tstr)
 488 {
 489         int str_len = strlen(str);
 490         int i;
 491
 492         if (str_len % 2)
 493         {
 494                 ERROR("BUG: gdb value with uneven number of characters encountered: %s", str);
 495                 exit(-1);
 496         }
 497
 498         if (target->endianness == TARGET_LITTLE_ENDIAN)
 499         {
 500                 for (i = 0; i < str_len; i+=2)
 501                 {
 502                         tstr[str_len - i - 1] = str[i + 1];
 503                         tstr[str_len - i - 2] = str[i];
 504                 }
 505         }
 506         else
 507         {
 508                 for (i = 0; i < str_len; i++)
 509                 {
 510                         tstr[i] = str[i];
 511                 }
 512         }
 513 }
 514
 515 void gdb_target_to_str(target_t *target, char *tstr, char *str)
 516 {
 517         int str_len = strlen(tstr);
 518         int i;
 519
 520         if (str_len % 2)
 521         {
 522                 ERROR("BUG: gdb value with uneven number of characters encountered");
 523                 exit(-1);
 524         }
 525
 526         if (target->endianness == TARGET_LITTLE_ENDIAN)
 527         {
 528                 for (i = 0; i < str_len; i+=2)
 529                 {
 530                         str[str_len - i - 1] = tstr[i + 1];
 531                         str[str_len - i - 2] = tstr[i];
 532                 }
 533         }
 534         else
 535         {
 536                 for (i = 0; i < str_len; i++)
 537                 {
 538                         str[i] = tstr[i];
 539                 }
 540         }
 541 }
 542
 543 int gdb_get_registers_packet(connection_t *connection, target_t *target, char* packet, int packet_size)
 544 {
 545         reg_t **reg_list;
 546         int reg_list_size;
 547         int retval;
 548         int reg_packet_size = 0;
 549         char *reg_packet;
 550         char *reg_packet_p;
 551         int i;
 552
 553         DEBUG("");
 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, dropping connection");
 561                                 return ERROR_SERVER_REMOTE_CLOSED;
 562                         default:
 563                                 /* this is a bug condition - get_gdb_reg_list() may not return any other error */
 564                                 ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
 565                                 exit(-1);
 566                 }
 567         }
 568
 569         for (i = 0; i < reg_list_size; i++)
 570         {
 571                 reg_packet_size += reg_list[i]->size;
 572         }
 573
 574         reg_packet = malloc(CEIL(reg_packet_size, 8) * 2);
 575         reg_packet_p = reg_packet;
 576
 577         for (i = 0; i < reg_list_size; i++)
 578         {
 579                 char *hex_buf = buf_to_str(reg_list[i]->value, reg_list[i]->size, 16);
 580                 DEBUG("hex_buf: %s", hex_buf);
 581                 gdb_str_to_target(target, hex_buf, reg_packet_p);
 582                 reg_packet_p += CEIL(reg_list[i]->size, 8) * 2;
 583                 free(hex_buf);
 584         }
 585
 586         reg_packet_p = strndup(reg_packet, CEIL(reg_packet_size, 8) * 2);
 587         DEBUG("reg_packet: %s", reg_packet_p);
 588         free(reg_packet_p);
 589
 590         gdb_put_packet(connection, reg_packet, CEIL(reg_packet_size, 8) * 2);
 591         free(reg_packet);
 592
 593         free(reg_list);
 594
 595         return ERROR_OK;
 596 }
 597
 598 int gdb_set_registers_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 599 {
 600         int i;
 601         reg_t **reg_list;
 602         int reg_list_size;
 603         int retval;
 604         char *packet_p;
 605
 606         DEBUG("");
 607
 608         /* skip command character */
 609         packet++;
 610         packet_size--;
 611
 612         if (packet_size % 2)
 613         {
 614                 WARNING("GDB set_registers packet with uneven characters received, dropping connection");
 615                 return ERROR_SERVER_REMOTE_CLOSED;
 616         }
 617
 618         if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
 619         {
 620                 switch (retval)
 621                 {
 622                         case ERROR_TARGET_NOT_HALTED:
 623                                 ERROR("gdb tried to registers but we're not halted, dropping connection");
 624                                 return ERROR_SERVER_REMOTE_CLOSED;
 625                         default:
 626                                 /* this is a bug condition - get_gdb_reg_list() may not return any other error */
 627                                 ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
 628                                 exit(-1);
 629                 }
 630         }
 631
 632         packet_p = packet;
 633         for (i = 0; i < reg_list_size; i++)
 634         {
 635                 u8 *bin_buf;
 636                 char *hex_buf;
 637                 reg_arch_type_t *arch_type;
 638
 639                 /* convert from GDB-string (target-endian) to hex-string (big-endian) */
 640                 hex_buf = malloc(CEIL(reg_list[i]->size, 8) * 2);
 641                 gdb_target_to_str(target, packet_p, hex_buf);
 642
 643                 /* convert hex-string to binary buffer */
 644                 bin_buf = malloc(CEIL(reg_list[i]->size, 8));
 645                 str_to_buf(hex_buf, CEIL(reg_list[i]->size, 8) * 2, bin_buf, reg_list[i]->size, 16);
 646
 647                 /* get register arch_type, and call set method */
 648                 arch_type = register_get_arch_type(reg_list[i]->arch_type);
 649                 if (arch_type == NULL)
 650                 {
 651                         ERROR("BUG: encountered unregistered arch type");
 652                         exit(-1);
 653                 }
 654                 arch_type->set(reg_list[i], bin_buf);
 655
 656                 /* advance packet pointer */
 657                 packet_p += (CEIL(reg_list[i]->size, 8) * 2);
 658
 659                 free(bin_buf);
 660                 free(hex_buf);
 661         }
 662
 663         /* free reg_t *reg_list[] array allocated by get_gdb_reg_list */
 664         free(reg_list);
 665
 666         gdb_put_packet(connection, "OK", 2);
 667
 668         return ERROR_OK;
 669 }
 670
 671 int gdb_get_register_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 672 {
 673         char *reg_packet;
 674         int reg_num = strtoul(packet + 1, NULL, 16);
 675         reg_t **reg_list;
 676         int reg_list_size;
 677         int retval;
 678         char *hex_buf;
 679
 680         DEBUG("");
 681
 682         if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
 683         {
 684                 switch (retval)
 685                 {
 686                         case ERROR_TARGET_NOT_HALTED:
 687                                 ERROR("gdb requested registers but we're not halted, dropping connection");
 688                                 return ERROR_SERVER_REMOTE_CLOSED;
 689                         default:
 690                                 /* this is a bug condition - get_gdb_reg_list() may not return any other error */
 691                                 ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
 692                                 exit(-1);
 693                 }
 694         }
 695
 696         if (reg_list_size <= reg_num)
 697         {
 698                 ERROR("gdb requested a non-existing register");
 699                 exit(-1);
 700         }
 701
 702         reg_packet = malloc(CEIL(reg_list[reg_num]->size, 8) * 2);
 703
 704         hex_buf = buf_to_str(reg_list[reg_num]->value, reg_list[reg_num]->size, 16);
 705
 706         gdb_str_to_target(target, hex_buf, reg_packet);
 707
 708         gdb_put_packet(connection, reg_packet, CEIL(reg_list[reg_num]->size, 8) * 2);
 709
 710         free(reg_list);
 711         free(reg_packet);
 712         free(hex_buf);
 713
 714         return ERROR_OK;
 715 }
 716
 717 int gdb_set_register_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 718 {
 719         char *separator;
 720         char *hex_buf;
 721         u8 *bin_buf;
 722         int reg_num = strtoul(packet + 1, &separator, 16);
 723         reg_t **reg_list;
 724         int reg_list_size;
 725         int retval;
 726         reg_arch_type_t *arch_type;
 727
 728         DEBUG("");
 729
 730         if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
 731         {
 732                 switch (retval)
 733                 {
 734                         case ERROR_TARGET_NOT_HALTED:
 735                                 ERROR("gdb tried to set a register but we're not halted, dropping connection");
 736                                 return ERROR_SERVER_REMOTE_CLOSED;
 737                         default:
 738                                 /* this is a bug condition - get_gdb_reg_list() may not return any other error */
 739                                 ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
 740                                 exit(-1);
 741                 }
 742         }
 743
 744         if (reg_list_size < reg_num)
 745         {
 746                 ERROR("gdb requested a non-existing register");
 747                 return ERROR_SERVER_REMOTE_CLOSED;
 748         }
 749
 750         if (*separator != '=')
 751         {
 752                 ERROR("GDB 'set register packet', but no '=' following the register number");
 753                 return ERROR_SERVER_REMOTE_CLOSED;
 754         }
 755
 756         /* convert from GDB-string (target-endian) to hex-string (big-endian) */
 757         hex_buf = malloc(CEIL(reg_list[reg_num]->size, 8) * 2);
 758         gdb_target_to_str(target, separator + 1, hex_buf);
 759
 760         /* convert hex-string to binary buffer */
 761         bin_buf = malloc(CEIL(reg_list[reg_num]->size, 8));
 762         str_to_buf(hex_buf, CEIL(reg_list[reg_num]->size, 8) * 2, bin_buf, reg_list[reg_num]->size, 16);
 763
 764         /* get register arch_type, and call set method */
 765         arch_type = register_get_arch_type(reg_list[reg_num]->arch_type);
 766         if (arch_type == NULL)
 767         {
 768                 ERROR("BUG: encountered unregistered arch type");
 769                 exit(-1);
 770         }
 771         arch_type->set(reg_list[reg_num], bin_buf);
 772
 773         gdb_put_packet(connection, "OK", 2);
 774
 775         free(bin_buf);
 776         free(hex_buf);
 777         free(reg_list);
 778
 779         return ERROR_OK;
 780 }
 781
 782 int gdb_memory_packet_error(connection_t *connection, int retval)
 783 {
 784         switch (retval)
 785         {
 786                 case ERROR_TARGET_NOT_HALTED:
 787                         ERROR("gdb tried to read memory but we're not halted, dropping connection");
 788                         return ERROR_SERVER_REMOTE_CLOSED;
 789                         break;
 790                 case ERROR_TARGET_DATA_ABORT:
 791                         gdb_send_error(connection, EIO);
 792                         break;
 793                 case ERROR_TARGET_TRANSLATION_FAULT:
 794                         gdb_send_error(connection, EFAULT);
 795                         break;
 796                 case ERROR_TARGET_UNALIGNED_ACCESS:
 797                         gdb_send_error(connection, EFAULT);
 798                         break;
 799                 default:
 800                         ERROR("BUG: unexpected error %i", retval);
 801                         exit(-1);
 802         }
 803
 804         return ERROR_OK;
 805 }
 806
 807 int gdb_read_memory_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 808 {
 809         char *separator;
 810         u32 addr = 0;
 811         u32 len = 0;
 812
 813         u8 *buffer;
 814         char *hex_buffer;
 815
 816         int i;
 817         int retval;
 818
 819         /* skip command character */
 820         packet++;
 821
 822         addr = strtoul(packet, &separator, 16);
 823
 824         if (*separator != ',')
 825         {
 826                 ERROR("incomplete read memory packet received, dropping connection");
 827                 return ERROR_SERVER_REMOTE_CLOSED;
 828         }
 829
 830         len = strtoul(separator+1, NULL, 16);
 831
 832         buffer = malloc(len);
 833
 834         DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
 835
 836         switch (len)
 837         {
 838                 case 4:
 839                         if ((addr % 4) == 0)
 840                                 retval = target->type->read_memory(target, addr, 4, 1, buffer);
 841                         else
 842                                 retval = target->type->read_memory(target, addr, 1, len, buffer);
 843                         break;
 844                 case 2:
 845                         if ((addr % 2) == 0)
 846                                 retval = target->type->read_memory(target, addr, 2, 1, buffer);
 847                         else
 848                                 retval = target->type->read_memory(target, addr, 1, len, buffer);
 849                         break;
 850                 default:
 851                         if (((addr % 4) == 0) && ((len % 4) == 0))
 852                                 retval = target->type->read_memory(target, addr, 4, len / 4, buffer);
 853                         else
 854                                 retval = target->type->read_memory(target, addr, 1, len, buffer);
 855         }
 856
 857         if (retval == ERROR_OK)
 858         {
 859                 hex_buffer = malloc(len * 2 + 1);
 860
 861                 for (i=0; i<len; i++)
 862                         snprintf(hex_buffer + 2*i, 3, "%2.2x", buffer[i]);
 863
 864                 gdb_put_packet(connection, hex_buffer, len * 2);
 865
 866                 free(hex_buffer);
 867         }
 868         else
 869         {
 870                 if ((retval = gdb_memory_packet_error(connection, retval)) != ERROR_OK)
 871                         return retval;
 872         }
 873
 874         free(buffer);
 875
 876         return ERROR_OK;
 877 }
 878
 879 int gdb_write_memory_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 880 {
 881         char *separator;
 882         u32 addr = 0;
 883         u32 len = 0;
 884
 885         u8 *buffer;
 886
 887         int i;
 888         int retval;
 889
 890         /* skip command character */
 891         packet++;
 892
 893         addr = strtoul(packet, &separator, 16);
 894
 895         if (*separator != ',')
 896         {
 897                 ERROR("incomplete write memory packet received, dropping connection");
 898                 return ERROR_SERVER_REMOTE_CLOSED;
 899         }
 900
 901         len = strtoul(separator+1, &separator, 16);
 902
 903         if (*(separator++) != ':')
 904         {
 905                 ERROR("incomplete write memory packet received, dropping connection");
 906                 return ERROR_SERVER_REMOTE_CLOSED;
 907         }
 908
 909         buffer = malloc(len);
 910
 911         DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
 912
 913         for (i=0; i<len; i++)
 914         {
 915                 u32 tmp;
 916                 sscanf(separator + 2*i, "%2x", &tmp);
 917                 buffer[i] = tmp;
 918         }
 919
 920         retval = ERROR_OK;
 921         switch (len)
 922         {
 923                 /* handle sized writes */
 924                 case 4:
 925                         if ((addr % 4) == 0)
 926                                 retval = target->type->write_memory(target, addr, 4, 1, buffer);
 927                         else
 928                                 retval = target->type->write_memory(target, addr, 1, len, buffer);
 929                         break;
 930                 case 2:
 931                         if ((addr % 2) == 0)
 932                                 retval = target->type->write_memory(target, addr, 2, 1, buffer);
 933                         else
 934                                 retval = target->type->write_memory(target, addr, 1, len, buffer);
 935                         break;
 936                 case 3:
 937                 case 1:
 938                         retval = target->type->write_memory(target, addr, 1, len, buffer);
 939                         break;
 940                 /* handle bulk writes */
 941                 default:
 942                         retval = target_write_buffer(target, addr, len, buffer);
 943                         break;
 944         }
 945
 946         if (retval == ERROR_OK)
 947         {
 948                 gdb_put_packet(connection, "OK", 2);
 949         }
 950         else
 951         {
 952                 if ((retval = gdb_memory_packet_error(connection, retval)) != ERROR_OK)
 953                         return retval;
 954         }
 955
 956         free(buffer);
 957
 958         return ERROR_OK;
 959 }
 960
 961 int gdb_write_memory_binary_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 962 {
 963         char *separator;
 964         u32 addr = 0;
 965         u32 len = 0;
 966
 967         u8 *buffer;
 968         int retval;
 969
 970         /* skip command character */
 971         packet++;
 972
 973         addr = strtoul(packet, &separator, 16);
 974
 975         if (*separator != ',')
 976         {
 977                 ERROR("incomplete write memory binary packet received, dropping connection");
 978                 return ERROR_SERVER_REMOTE_CLOSED;
 979         }
 980
 981         len = strtoul(separator+1, &separator, 16);
 982
 983         if (*(separator++) != ':')
 984         {
 985                 ERROR("incomplete write memory binary packet received, dropping connection");
 986                 return ERROR_SERVER_REMOTE_CLOSED;
 987         }
 988
 989         retval = ERROR_OK;
 990         if( len ) {
 991
 992                 buffer = malloc(len);
 993
 994                 DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
 995
 996                 memcpy( buffer, separator, len );
 997
 998                 switch (len)
 999                 {
1000                         case 4:
1001                                 if ((addr % 4) == 0)
1002                                         retval = target->type->write_memory(target, addr, 4, 1, buffer);
1003                                 else
1004                                         retval = target->type->write_memory(target, addr, 1, len, buffer);
1005                                 break;
1006                         case 2:
1007                                 if ((addr % 2) == 0)
1008                                         retval = target->type->write_memory(target, addr, 2, 1, buffer);
1009                                 else
1010                                         retval = target->type->write_memory(target, addr, 1, len, buffer);
1011                                 break;
1012                         case 3:
1013                         case 1:
1014                                 retval = target->type->write_memory(target, addr, 1, len, buffer);
1015                                 break;
1016                         default:
1017                                 retval = target_write_buffer(target, addr, len, buffer);
1018                                 break;
1019                 }
1020
1021                 free(buffer);
1022         }
1023
1024         if (retval == ERROR_OK)
1025         {
1026                 gdb_put_packet(connection, "OK", 2);
1027         }
1028         else
1029         {
1030                 if ((retval = gdb_memory_packet_error(connection, retval)) != ERROR_OK)
1031                         return retval;
1032         }
1033
1034         return ERROR_OK;
1035 }
1036
1037 void gdb_step_continue_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
1038 {
1039         int current = 0;
1040         u32 address = 0x0;
1041
1042         DEBUG("");
1043
1044         if (packet_size > 1)
1045         {
1046                 packet[packet_size] = 0;
1047                 address = strtoul(packet + 1, NULL, 16);
1048         }
1049         else
1050         {
1051                 current = 1;
1052         }
1053
1054         if (packet[0] == 'c')
1055         {
1056                 DEBUG("continue");
1057                 target->type->resume(target, current, address, 0, 0); /* resume at current address, don't handle breakpoints, not debugging */
1058         }
1059         else if (packet[0] == 's')
1060         {
1061                 DEBUG("step");
1062                 target->type->step(target, current, address, 0); /* step at current or address, don't handle breakpoints */
1063         }
1064 }
1065
1066 int gdb_bp_wp_packet_error(connection_t *connection, int retval)
1067 {
1068         switch (retval)
1069         {
1070                 case ERROR_TARGET_NOT_HALTED:
1071                         ERROR("gdb tried to set a breakpoint but we're not halted, dropping connection");
1072                         return ERROR_SERVER_REMOTE_CLOSED;
1073                         break;
1074                 case ERROR_TARGET_RESOURCE_NOT_AVAILABLE:
1075                         gdb_send_error(connection, EBUSY);
1076                         break;
1077                 default:
1078                         ERROR("BUG: unexpected error %i", retval);
1079                         exit(-1);
1080         }
1081
1082         return ERROR_OK;
1083 }
1084
1085 int gdb_breakpoint_watchpoint_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
1086 {
1087         int type;
1088         enum breakpoint_type bp_type;
1089         enum watchpoint_rw wp_type;
1090         u32 address;
1091         u32 size;
1092         char *separator;
1093         int retval;
1094
1095         DEBUG("");
1096
1097         type = strtoul(packet + 1, &separator, 16);
1098
1099         if (type == 0)  /* memory breakpoint */
1100                 bp_type = BKPT_SOFT;
1101         else if (type == 1) /* hardware breakpoint */
1102                 bp_type = BKPT_HARD;
1103         else if (type == 2) /* write watchpoint */
1104                 wp_type = WPT_WRITE;
1105         else if (type == 3) /* read watchpoint */
1106                 wp_type = WPT_READ;
1107         else if (type == 4) /* access watchpoint */
1108                 wp_type = WPT_ACCESS;
1109
1110         if (*separator != ',')
1111         {
1112                 ERROR("incomplete breakpoint/watchpoint packet received, dropping connection");
1113                 return ERROR_SERVER_REMOTE_CLOSED;
1114         }
1115
1116         address = strtoul(separator+1, &separator, 16);
1117
1118         if (*separator != ',')
1119         {
1120                 ERROR("incomplete breakpoint/watchpoint packet received, dropping connection");
1121                 return ERROR_SERVER_REMOTE_CLOSED;
1122         }
1123
1124         size = strtoul(separator+1, &separator, 16);
1125
1126         switch (type)
1127         {
1128                 case 0:
1129                 case 1:
1130                         if (packet[0] == 'Z')
1131                         {
1132                                 if ((retval = breakpoint_add(target, address, size, bp_type)) != ERROR_OK)
1133                                 {
1134                                         if ((retval = gdb_bp_wp_packet_error(connection, retval)) != ERROR_OK)
1135                                                 return retval;
1136                                 }
1137                                 else
1138                                 {
1139                                         gdb_put_packet(connection, "OK", 2);
1140                                 }
1141                         }
1142                         else
1143                         {
1144                                 breakpoint_remove(target, address);
1145                                 gdb_put_packet(connection, "OK", 2);
1146                         }
1147                         break;
1148                 case 2:
1149                 case 3:
1150                 case 4:
1151                 {
1152                         if (packet[0] == 'Z')
1153                         {
1154                                 if ((retval = watchpoint_add(target, address, size, type-2, 0, 0xffffffffu)) != ERROR_OK)
1155                                 {
1156                                         if ((retval = gdb_bp_wp_packet_error(connection, retval)) != ERROR_OK)
1157                                                 return retval;
1158                                 }
1159                                 else
1160                                 {
1161                                         gdb_put_packet(connection, "OK", 2);
1162                                 }
1163                         }
1164                         else
1165                         {
1166                                 watchpoint_remove(target, address);
1167                                 gdb_put_packet(connection, "OK", 2);
1168                         }
1169                         break;
1170                 }
1171                 default:
1172                         break;
1173         }
1174
1175         return ERROR_OK;
1176 }
1177
1178 void gdb_query_packet(connection_t *connection, char *packet, int packet_size)
1179 {
1180         command_context_t *cmd_ctx = connection->cmd_ctx;
1181
1182         if (strstr(packet, "qRcmd,"))
1183         {
1184                 if (packet_size > 6)
1185                 {
1186                         char *cmd;
1187                         int i;
1188                         cmd = malloc((packet_size - 6)/2 + 1);
1189                         for (i=0; i < (packet_size - 6)/2; i++)
1190                         {
1191                                 u32 tmp;
1192                                 sscanf(packet + 6 + 2*i, "%2x", &tmp);
1193                                 cmd[i] = tmp;
1194                         }
1195                         cmd[(packet_size - 6)/2] = 0x0;
1196                         command_run_line(cmd_ctx, cmd);
1197                         free(cmd);
1198                 }
1199                 gdb_put_packet(connection, "OK", 2);
1200                 return;
1201         }
1202
1203         gdb_put_packet(connection, "", 0);
1204 }
1205
1206 int gdb_input(connection_t *connection)
1207 {
1208         gdb_service_t *gdb_service = connection->service->priv;
1209         target_t *target = gdb_service->target;
1210         char packet[GDB_BUFFER_SIZE];
1211         int packet_size;
1212         int retval;
1213         gdb_connection_t *gdb_con = connection->priv;
1214
1215         /* drain input buffer */
1216         do
1217         {
1218                 packet_size = GDB_BUFFER_SIZE-1;
1219                 if ((retval = gdb_get_packet(connection, packet, &packet_size)) != ERROR_OK)
1220                 {
1221                         switch (retval)
1222                         {
1223                                 case ERROR_GDB_BUFFER_TOO_SMALL:
1224                                         ERROR("BUG: buffer supplied for gdb packet was too small");
1225                                         exit(-1);
1226                                 case ERROR_SERVER_REMOTE_CLOSED:
1227                                         return ERROR_SERVER_REMOTE_CLOSED;
1228                                 default:
1229                                         ERROR("BUG: unexpected error");
1230                                         exit(-1);
1231                         }
1232                 }
1233
1234                 /* terminate with zero */
1235                 packet[packet_size] = 0;
1236
1237                 DEBUG("recevied packet: '%s'", packet);
1238
1239                 if (packet_size > 0)
1240                 {
1241                         retval = ERROR_OK;
1242                         switch (packet[0])
1243                         {
1244                                 case 'H':
1245                                         /* Hct... -- set thread
1246                                         * we don't have threads, send empty reply */
1247                                         gdb_put_packet(connection, NULL, 0);
1248                                         break;
1249                                 case 'q':
1250                                         gdb_query_packet(connection, packet, packet_size);
1251                                         break;
1252                                 case 'g':
1253                                         retval = gdb_get_registers_packet(connection, target, packet, packet_size);
1254                                         break;
1255                                 case 'G':
1256                                         retval = gdb_set_registers_packet(connection, target, packet, packet_size);
1257                                         break;
1258                                 case 'p':
1259                                         retval = gdb_get_register_packet(connection, target, packet, packet_size);
1260                                         break;
1261                                 case 'P':
1262                                         retval = gdb_set_register_packet(connection, target, packet, packet_size);
1263                                         break;
1264                                 case 'm':
1265                                         retval = gdb_read_memory_packet(connection, target, packet, packet_size);
1266                                         break;
1267                                 case 'M':
1268                                         retval = gdb_write_memory_packet(connection, target, packet, packet_size);
1269                                         break;
1270                                 case 'z':
1271                                 case 'Z':
1272                                         retval = gdb_breakpoint_watchpoint_packet(connection, target, packet, packet_size);
1273                                         break;
1274                                 case '?':
1275                                         gdb_last_signal_packet(connection, target, packet, packet_size);
1276                                 break;
1277                                 case 'c':
1278                                 case 's':
1279                                         gdb_step_continue_packet(connection, target, packet, packet_size);
1280                                         break;
1281                                 case 'D':
1282                                         target->type->resume(target, 1, 0, 1, 0);
1283                                         gdb_put_packet(connection, "OK", 2);
1284                                         break;
1285                                 case 'X':
1286                                         if ((retval = gdb_write_memory_binary_packet(connection, target, packet, packet_size)) != ERROR_OK)
1287                                                 return retval;
1288                                         break;
1289                                 case 'k':
1290                                         gdb_put_packet(connection, "OK", 2);
1291                                         return ERROR_SERVER_REMOTE_CLOSED;
1292                                 default:
1293                                         /* ignore unkown packets */
1294                                         DEBUG("ignoring 0x%2.2x packet", packet[0]);
1295                                         gdb_put_packet(connection, NULL, 0);
1296                                         break;
1297                         }
1298
1299                         /* if a packet handler returned an error, exit input loop */
1300                         if (retval != ERROR_OK)
1301                                 return retval;
1302                 }
1303
1304                 if (gdb_con->ctrl_c)
1305                 {
1306                         if (target->state == TARGET_RUNNING)
1307                         {
1308                                 target->type->halt(target);
1309                                 gdb_con->ctrl_c = 0;
1310                         }
1311                 }
1312
1313         } while (gdb_con->buf_cnt > 0);
1314
1315         return ERROR_OK;
1316 }
1317
1318 int gdb_init()
1319 {
1320         gdb_service_t *gdb_service;
1321         target_t *target = targets;
1322         int i = 0;
1323
1324         if (!target)
1325         {
1326                 WARNING("no gdb ports allocated as no target has been specified");
1327                 return ERROR_OK;
1328         }
1329
1330         if (gdb_port == 0)
1331         {
1332                 WARNING("no gdb port specified, using default port 3333");
1333                 gdb_port = 3333;
1334         }
1335
1336         while (target)
1337         {
1338                 char service_name[8];
1339
1340                 snprintf(service_name, 8, "gdb-%2.2i", i);
1341
1342                 gdb_service = malloc(sizeof(gdb_service_t));
1343                 gdb_service->target = target;
1344
1345                 add_service("gdb", CONNECTION_GDB, gdb_port + i, 1, gdb_new_connection, gdb_input, gdb_connection_closed, gdb_service);
1346
1347                 DEBUG("gdb service for target %s at port %i", target->type->name, gdb_port + i);
1348
1349                 i++;
1350                 target = target->next;
1351         }
1352
1353         return ERROR_OK;
1354 }
1355
1356 /* daemon configuration command gdb_port */
1357 int handle_gdb_port_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1358 {
1359         if (argc == 0)
1360                 return ERROR_OK;
1361
1362         /* only if the port wasn't overwritten by cmdline */
1363         if (gdb_port == 0)
1364                 gdb_port = strtoul(args[0], NULL, 0);
1365
1366         return ERROR_OK;
1367 }
1368
1369 int gdb_register_commands(command_context_t *command_context)
1370 {
1371         register_command(command_context, NULL, "gdb_port", handle_gdb_port_command,
1372                                          COMMAND_CONFIG, "");
1373
1374         return ERROR_OK;
1375 }