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 = '\0';
 228         char checksum[3];
 229         unsigned char my_checksum = 0;
 230         gdb_connection_t *gdb_con = connection->priv;
 231
 232         while (1)
 233         {
 234                 do
 235                 {
 236                         if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
 237                                 return retval;
 238
 239                         DEBUG("character: '%c'", character);
 240
 241                         switch (character)
 242                         {
 243                                 case '$':
 244                                         break;
 245                                 case '+':
 246                                         WARNING("acknowledgment received, but no packet pending");
 247                                         break;
 248                                 case '-':
 249                                         WARNING("negative acknowledgment, but no packet pending");
 250                                         break;
 251                                 case 0x3:
 252                                         gdb_con->ctrl_c = 1;
 253                                         *len = 0;
 254                                         return ERROR_OK;
 255                                 default:
 256                                         WARNING("ignoring character 0x%x", character);
 257                                         break;
 258                         }
 259                 } while (character != '$');
 260
 261                 my_checksum = 0;
 262
 263                 do
 264                 {
 265                         if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
 266                                 return retval;
 267
 268                         if (character == '#') break;
 269
 270                         if (character == '}')
 271                         {
 272                                 /* data transmitted in binary mode (X packet)
 273                                 * uses 0x7d as escape character */
 274                                 my_checksum += character & 0xff;
 275                                 if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
 276                                         return retval;
 277                                 my_checksum += character & 0xff;
 278                                 buffer[count++] = (character ^ 0x20) & 0xff;
 279                         }
 280                         else
 281                         {
 282                                 my_checksum += character & 0xff;
 283                                 buffer[count++] = character & 0xff;
 284                         }
 285
 286                         if (count > *len)
 287                         {
 288                                 ERROR("packet buffer too small");
 289                                 return ERROR_GDB_BUFFER_TOO_SMALL;
 290                         }
 291                 } while (1);
 292
 293                 *len = count;
 294
 295                 if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
 296                         return retval;
 297                 checksum[0] = character;
 298                 if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
 299                         return retval;
 300                 checksum[1] = character;
 301                 checksum[2] = 0;
 302
 303                 if (my_checksum == strtoul(checksum, NULL, 16))
 304                 {
 305                         write_socket(connection->fd, "+", 1);
 306                         break;
 307                 }
 308
 309                 WARNING("checksum error, requesting retransmission");
 310                 write_socket(connection->fd, "-", 1);
 311         }
 312
 313         return ERROR_OK;
 314 }
 315
 316 int gdb_output(struct command_context_s *context, char* line)
 317 {
 318         connection_t *connection = context->output_handler_priv;
 319         char *hex_buffer;
 320         int i, bin_size;
 321
 322         bin_size = strlen(line);
 323
 324         hex_buffer = malloc(bin_size*2 + 4);
 325
 326         hex_buffer[0] = 'O';
 327         for (i=0; i<bin_size; i++)
 328                 snprintf(hex_buffer + 1 + i*2, 3, "%2.2x", line[i]);
 329         hex_buffer[bin_size*2+1] = '0';
 330         hex_buffer[bin_size*2+2] = 'a';
 331         hex_buffer[bin_size*2+3] = 0x0;
 332
 333         gdb_put_packet(connection, hex_buffer, bin_size*2 + 3);
 334
 335         free(hex_buffer);
 336         return ERROR_OK;
 337 }
 338
 339 int gdb_target_callback_event_handler(struct target_s *target, enum target_event event, void *priv)
 340 {
 341         connection_t *connection = priv;
 342         gdb_connection_t *gdb_connection = connection->priv;
 343         char sig_reply[4];
 344         int signal;
 345
 346         switch (event)
 347         {
 348                 case TARGET_EVENT_HALTED:
 349                         if (gdb_connection->frontend_state == TARGET_RUNNING)
 350                         {
 351                                 if (gdb_connection->ctrl_c)
 352                                 {
 353                                         signal = 0x2;
 354                                         gdb_connection->ctrl_c = 0;
 355                                 }
 356                                 else
 357                                 {
 358                                         signal = gdb_last_signal(target);
 359                                 }
 360
 361                                 snprintf(sig_reply, 4, "T%2.2x", signal);
 362                                 gdb_put_packet(connection, sig_reply, 3);
 363                                 gdb_connection->frontend_state = TARGET_HALTED;
 364                         }
 365                         break;
 366                 case TARGET_EVENT_RESUMED:
 367                         if (gdb_connection->frontend_state == TARGET_HALTED)
 368                         {
 369                                 gdb_connection->frontend_state = TARGET_RUNNING;
 370                         }
 371                         break;
 372                 default:
 373                         break;
 374         }
 375
 376         return ERROR_OK;
 377 }
 378
 379 int gdb_new_connection(connection_t *connection)
 380 {
 381         gdb_connection_t *gdb_connection = malloc(sizeof(gdb_connection_t));
 382         gdb_service_t *gdb_service = connection->service->priv;
 383         int retval;
 384         int initial_ack;
 385
 386         connection->priv = gdb_connection;
 387
 388         /* initialize gdb connection information */
 389         gdb_connection->buf_p = gdb_connection->buffer;
 390         gdb_connection->buf_cnt = 0;
 391         gdb_connection->ctrl_c = 0;
 392         gdb_connection->frontend_state = TARGET_HALTED;
 393
 394         /* output goes through gdb connection */
 395         command_set_output_handler(connection->cmd_ctx, gdb_output, connection);
 396
 397         /* register callback to be informed about target events */
 398         target_register_event_callback(gdb_target_callback_event_handler, connection);
 399
 400         /* a gdb session just attached, put the target in halt mode */
 401         if (((retval = gdb_service->target->type->halt(gdb_service->target)) != ERROR_OK) &&
 402                          (retval != ERROR_TARGET_ALREADY_HALTED))
 403         {
 404                 ERROR("error when trying to halt target");
 405                 exit(-1);
 406         }
 407
 408         while (gdb_service->target->state != TARGET_HALTED)
 409         {
 410                 gdb_service->target->type->poll(gdb_service->target);
 411         }
 412
 413         /* remove the initial ACK from the incoming buffer */
 414         if ((retval = gdb_get_char(connection, &initial_ack)) != ERROR_OK)
 415                 return retval;
 416
 417         if (initial_ack != '+')
 418                 gdb_putback_char(connection, initial_ack);
 419
 420         return ERROR_OK;
 421 }
 422
 423 int gdb_connection_closed(connection_t *connection)
 424 {
 425         if (connection->priv)
 426                 free(connection->priv);
 427         else
 428                 ERROR("BUG: connection->priv == NULL");
 429
 430         target_unregister_event_callback(gdb_target_callback_event_handler, connection);
 431
 432         return ERROR_OK;
 433 }
 434
 435 void gdb_send_error(connection_t *connection, u8 the_error)
 436 {
 437     char err[4];
 438     snprintf(err, 4, "E%2.2X", the_error );
 439     gdb_put_packet(connection, err, 3);
 440 }
 441
 442 int gdb_last_signal_packet(connection_t *connection, target_t *target, char* packet, int packet_size)
 443 {
 444         char sig_reply[4];
 445         int signal;
 446
 447         signal = gdb_last_signal(target);
 448
 449         snprintf(sig_reply, 4, "S%2.2x", signal);
 450         gdb_put_packet(connection, sig_reply, 3);
 451
 452         return ERROR_OK;
 453 }
 454
 455 void gdb_str_to_target(target_t *target, char *str, char *tstr)
 456 {
 457         int str_len = strlen(str);
 458         int i;
 459
 460         if (str_len % 2)
 461         {
 462                 ERROR("BUG: gdb value with uneven number of characters encountered: %s", str);
 463                 exit(-1);
 464         }
 465
 466         if (target->endianness == TARGET_LITTLE_ENDIAN)
 467         {
 468                 for (i = 0; i < str_len; i+=2)
 469                 {
 470                         tstr[str_len - i - 1] = str[i + 1];
 471                         tstr[str_len - i - 2] = str[i];
 472                 }
 473         }
 474         else
 475         {
 476                 for (i = 0; i < str_len; i++)
 477                 {
 478                         tstr[i] = str[i];
 479                 }
 480         }
 481 }
 482
 483 void gdb_target_to_str(target_t *target, char *tstr, char *str)
 484 {
 485         int str_len = strlen(tstr);
 486         int i;
 487
 488         if (str_len % 2)
 489         {
 490                 ERROR("BUG: gdb value with uneven number of characters encountered");
 491                 exit(-1);
 492         }
 493
 494         if (target->endianness == TARGET_LITTLE_ENDIAN)
 495         {
 496                 for (i = 0; i < str_len; i+=2)
 497                 {
 498                         str[str_len - i - 1] = tstr[i + 1];
 499                         str[str_len - i - 2] = tstr[i];
 500                 }
 501         }
 502         else
 503         {
 504                 for (i = 0; i < str_len; i++)
 505                 {
 506                         str[i] = tstr[i];
 507                 }
 508         }
 509 }
 510
 511 int gdb_get_registers_packet(connection_t *connection, target_t *target, char* packet, int packet_size)
 512 {
 513         reg_t **reg_list;
 514         int reg_list_size;
 515         int retval;
 516         int reg_packet_size = 0;
 517         char *reg_packet;
 518         char *reg_packet_p;
 519         int i;
 520
 521         DEBUG("-");
 522
 523         if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
 524         {
 525                 switch (retval)
 526                 {
 527                         case ERROR_TARGET_NOT_HALTED:
 528                                 ERROR("gdb requested registers but we're not halted, dropping connection");
 529                                 return ERROR_SERVER_REMOTE_CLOSED;
 530                         default:
 531                                 /* this is a bug condition - get_gdb_reg_list() may not return any other error */
 532                                 ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
 533                                 exit(-1);
 534                 }
 535         }
 536
 537         for (i = 0; i < reg_list_size; i++)
 538         {
 539                 reg_packet_size += reg_list[i]->size;
 540         }
 541
 542         reg_packet = malloc(CEIL(reg_packet_size, 8) * 2);
 543         reg_packet_p = reg_packet;
 544
 545         for (i = 0; i < reg_list_size; i++)
 546         {
 547                 char *hex_buf = buf_to_str(reg_list[i]->value, reg_list[i]->size, 16);
 548                 DEBUG("hex_buf: %s", hex_buf);
 549                 gdb_str_to_target(target, hex_buf, reg_packet_p);
 550                 reg_packet_p += CEIL(reg_list[i]->size, 8) * 2;
 551                 free(hex_buf);
 552         }
 553
 554         reg_packet_p = strndup(reg_packet, CEIL(reg_packet_size, 8) * 2);
 555         DEBUG("reg_packet: %s", reg_packet_p);
 556         free(reg_packet_p);
 557
 558         gdb_put_packet(connection, reg_packet, CEIL(reg_packet_size, 8) * 2);
 559         free(reg_packet);
 560
 561         free(reg_list);
 562
 563         return ERROR_OK;
 564 }
 565
 566 int gdb_set_registers_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 567 {
 568         int i;
 569         reg_t **reg_list;
 570         int reg_list_size;
 571         int retval;
 572         char *packet_p;
 573
 574         DEBUG("-");
 575
 576         /* skip command character */
 577         packet++;
 578         packet_size--;
 579
 580         if (packet_size % 2)
 581         {
 582                 WARNING("GDB set_registers packet with uneven characters received, dropping connection");
 583                 return ERROR_SERVER_REMOTE_CLOSED;
 584         }
 585
 586         if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
 587         {
 588                 switch (retval)
 589                 {
 590                         case ERROR_TARGET_NOT_HALTED:
 591                                 ERROR("gdb tried to registers but we're not halted, dropping connection");
 592                                 return ERROR_SERVER_REMOTE_CLOSED;
 593                         default:
 594                                 /* this is a bug condition - get_gdb_reg_list() may not return any other error */
 595                                 ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
 596                                 exit(-1);
 597                 }
 598         }
 599
 600         packet_p = packet;
 601         for (i = 0; i < reg_list_size; i++)
 602         {
 603                 u8 *bin_buf;
 604                 char *hex_buf;
 605                 reg_arch_type_t *arch_type;
 606
 607                 /* convert from GDB-string (target-endian) to hex-string (big-endian) */
 608                 hex_buf = malloc(CEIL(reg_list[i]->size, 8) * 2);
 609                 gdb_target_to_str(target, packet_p, hex_buf);
 610
 611                 /* convert hex-string to binary buffer */
 612                 bin_buf = malloc(CEIL(reg_list[i]->size, 8));
 613                 str_to_buf(hex_buf, CEIL(reg_list[i]->size, 8) * 2, bin_buf, reg_list[i]->size, 16);
 614
 615                 /* get register arch_type, and call set method */
 616                 arch_type = register_get_arch_type(reg_list[i]->arch_type);
 617                 if (arch_type == NULL)
 618                 {
 619                         ERROR("BUG: encountered unregistered arch type");
 620                         exit(-1);
 621                 }
 622                 arch_type->set(reg_list[i], bin_buf);
 623
 624                 /* advance packet pointer */
 625                 packet_p += (CEIL(reg_list[i]->size, 8) * 2);
 626
 627                 free(bin_buf);
 628                 free(hex_buf);
 629         }
 630
 631         /* free reg_t *reg_list[] array allocated by get_gdb_reg_list */
 632         free(reg_list);
 633
 634         gdb_put_packet(connection, "OK", 2);
 635
 636         return ERROR_OK;
 637 }
 638
 639 int gdb_get_register_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 640 {
 641         char *reg_packet;
 642         int reg_num = strtoul(packet + 1, NULL, 16);
 643         reg_t **reg_list;
 644         int reg_list_size;
 645         int retval;
 646         char *hex_buf;
 647
 648         DEBUG("-");
 649
 650         if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
 651         {
 652                 switch (retval)
 653                 {
 654                         case ERROR_TARGET_NOT_HALTED:
 655                                 ERROR("gdb requested registers but we're not halted, dropping connection");
 656                                 return ERROR_SERVER_REMOTE_CLOSED;
 657                         default:
 658                                 /* this is a bug condition - get_gdb_reg_list() may not return any other error */
 659                                 ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
 660                                 exit(-1);
 661                 }
 662         }
 663
 664         if (reg_list_size <= reg_num)
 665         {
 666                 ERROR("gdb requested a non-existing register");
 667                 exit(-1);
 668         }
 669
 670         reg_packet = malloc(CEIL(reg_list[reg_num]->size, 8) * 2);
 671
 672         hex_buf = buf_to_str(reg_list[reg_num]->value, reg_list[reg_num]->size, 16);
 673
 674         gdb_str_to_target(target, hex_buf, reg_packet);
 675
 676         gdb_put_packet(connection, reg_packet, CEIL(reg_list[reg_num]->size, 8) * 2);
 677
 678         free(reg_list);
 679         free(reg_packet);
 680         free(hex_buf);
 681
 682         return ERROR_OK;
 683 }
 684
 685 int gdb_set_register_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 686 {
 687         char *separator;
 688         char *hex_buf;
 689         u8 *bin_buf;
 690         int reg_num = strtoul(packet + 1, &separator, 16);
 691         reg_t **reg_list;
 692         int reg_list_size;
 693         int retval;
 694         reg_arch_type_t *arch_type;
 695
 696         DEBUG("-");
 697
 698         if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
 699         {
 700                 switch (retval)
 701                 {
 702                         case ERROR_TARGET_NOT_HALTED:
 703                                 ERROR("gdb tried to set a register but we're not halted, dropping connection");
 704                                 return ERROR_SERVER_REMOTE_CLOSED;
 705                         default:
 706                                 /* this is a bug condition - get_gdb_reg_list() may not return any other error */
 707                                 ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
 708                                 exit(-1);
 709                 }
 710         }
 711
 712         if (reg_list_size < reg_num)
 713         {
 714                 ERROR("gdb requested a non-existing register");
 715                 return ERROR_SERVER_REMOTE_CLOSED;
 716         }
 717
 718         if (*separator != '=')
 719         {
 720                 ERROR("GDB 'set register packet', but no '=' following the register number");
 721                 return ERROR_SERVER_REMOTE_CLOSED;
 722         }
 723
 724         /* convert from GDB-string (target-endian) to hex-string (big-endian) */
 725         hex_buf = malloc(CEIL(reg_list[reg_num]->size, 8) * 2);
 726         gdb_target_to_str(target, separator + 1, hex_buf);
 727
 728         /* convert hex-string to binary buffer */
 729         bin_buf = malloc(CEIL(reg_list[reg_num]->size, 8));
 730         str_to_buf(hex_buf, CEIL(reg_list[reg_num]->size, 8) * 2, bin_buf, reg_list[reg_num]->size, 16);
 731
 732         /* get register arch_type, and call set method */
 733         arch_type = register_get_arch_type(reg_list[reg_num]->arch_type);
 734         if (arch_type == NULL)
 735         {
 736                 ERROR("BUG: encountered unregistered arch type");
 737                 exit(-1);
 738         }
 739         arch_type->set(reg_list[reg_num], bin_buf);
 740
 741         gdb_put_packet(connection, "OK", 2);
 742
 743         free(bin_buf);
 744         free(hex_buf);
 745         free(reg_list);
 746
 747         return ERROR_OK;
 748 }
 749
 750 int gdb_memory_packet_error(connection_t *connection, int retval)
 751 {
 752         switch (retval)
 753         {
 754                 case ERROR_TARGET_NOT_HALTED:
 755                         ERROR("gdb tried to read memory but we're not halted, dropping connection");
 756                         return ERROR_SERVER_REMOTE_CLOSED;
 757                         break;
 758                 case ERROR_TARGET_DATA_ABORT:
 759                         gdb_send_error(connection, EIO);
 760                         break;
 761                 case ERROR_TARGET_TRANSLATION_FAULT:
 762                         gdb_send_error(connection, EFAULT);
 763                         break;
 764                 case ERROR_TARGET_UNALIGNED_ACCESS:
 765                         gdb_send_error(connection, EFAULT);
 766                         break;
 767                 default:
 768                         ERROR("BUG: unexpected error %i", retval);
 769                         exit(-1);
 770         }
 771
 772         return ERROR_OK;
 773 }
 774
 775 int gdb_read_memory_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 776 {
 777         char *separator;
 778         u32 addr = 0;
 779         u32 len = 0;
 780
 781         u8 *buffer;
 782         char *hex_buffer;
 783
 784         int i;
 785         int retval;
 786
 787         /* skip command character */
 788         packet++;
 789
 790         addr = strtoul(packet, &separator, 16);
 791
 792         if (*separator != ',')
 793         {
 794                 ERROR("incomplete read memory packet received, dropping connection");
 795                 return ERROR_SERVER_REMOTE_CLOSED;
 796         }
 797
 798         len = strtoul(separator+1, NULL, 16);
 799
 800         buffer = malloc(len);
 801
 802         DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
 803
 804         switch (len)
 805         {
 806                 case 4:
 807                         if ((addr % 4) == 0)
 808                                 retval = target->type->read_memory(target, addr, 4, 1, buffer);
 809                         else
 810                                 retval = target->type->read_memory(target, addr, 1, len, buffer);
 811                         break;
 812                 case 2:
 813                         if ((addr % 2) == 0)
 814                                 retval = target->type->read_memory(target, addr, 2, 1, buffer);
 815                         else
 816                                 retval = target->type->read_memory(target, addr, 1, len, buffer);
 817                         break;
 818                 default:
 819                         if (((addr % 4) == 0) && ((len % 4) == 0))
 820                                 retval = target->type->read_memory(target, addr, 4, len / 4, buffer);
 821                         else
 822                                 retval = target->type->read_memory(target, addr, 1, len, buffer);
 823         }
 824
 825         if (retval == ERROR_OK)
 826         {
 827                 hex_buffer = malloc(len * 2 + 1);
 828
 829                 for (i=0; i<len; i++)
 830                         snprintf(hex_buffer + 2*i, 3, "%2.2x", buffer[i]);
 831
 832                 gdb_put_packet(connection, hex_buffer, len * 2);
 833
 834                 free(hex_buffer);
 835         }
 836         else
 837         {
 838                 if ((retval = gdb_memory_packet_error(connection, retval)) != ERROR_OK)
 839                         return retval;
 840         }
 841
 842         free(buffer);
 843
 844         return ERROR_OK;
 845 }
 846
 847 int gdb_write_memory_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 848 {
 849         char *separator;
 850         u32 addr = 0;
 851         u32 len = 0;
 852
 853         u8 *buffer;
 854
 855         int i;
 856         int retval;
 857
 858         /* skip command character */
 859         packet++;
 860
 861         addr = strtoul(packet, &separator, 16);
 862
 863         if (*separator != ',')
 864         {
 865                 ERROR("incomplete write memory packet received, dropping connection");
 866                 return ERROR_SERVER_REMOTE_CLOSED;
 867         }
 868
 869         len = strtoul(separator+1, &separator, 16);
 870
 871         if (*(separator++) != ':')
 872         {
 873                 ERROR("incomplete write memory packet received, dropping connection");
 874                 return ERROR_SERVER_REMOTE_CLOSED;
 875         }
 876
 877         buffer = malloc(len);
 878
 879         DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
 880
 881         for (i=0; i<len; i++)
 882         {
 883                 u32 tmp;
 884                 sscanf(separator + 2*i, "%2x", &tmp);
 885                 buffer[i] = tmp;
 886         }
 887
 888         retval = ERROR_OK;
 889         switch (len)
 890         {
 891                 /* handle sized writes */
 892                 case 4:
 893                         if ((addr % 4) == 0)
 894                                 retval = target->type->write_memory(target, addr, 4, 1, buffer);
 895                         else
 896                                 retval = target->type->write_memory(target, addr, 1, len, buffer);
 897                         break;
 898                 case 2:
 899                         if ((addr % 2) == 0)
 900                                 retval = target->type->write_memory(target, addr, 2, 1, buffer);
 901                         else
 902                                 retval = target->type->write_memory(target, addr, 1, len, buffer);
 903                         break;
 904                 case 3:
 905                 case 1:
 906                         retval = target->type->write_memory(target, addr, 1, len, buffer);
 907                         break;
 908                 /* handle bulk writes */
 909                 default:
 910                         retval = target_write_buffer(target, addr, len, buffer);
 911                         break;
 912         }
 913
 914         if (retval == ERROR_OK)
 915         {
 916                 gdb_put_packet(connection, "OK", 2);
 917         }
 918         else
 919         {
 920                 if ((retval = gdb_memory_packet_error(connection, retval)) != ERROR_OK)
 921                         return retval;
 922         }
 923
 924         free(buffer);
 925
 926         return ERROR_OK;
 927 }
 928
 929 int gdb_write_memory_binary_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
 930 {
 931         char *separator;
 932         u32 addr = 0;
 933         u32 len = 0;
 934
 935         u8 *buffer;
 936         int retval;
 937
 938         /* skip command character */
 939         packet++;
 940
 941         addr = strtoul(packet, &separator, 16);
 942
 943         if (*separator != ',')
 944         {
 945                 ERROR("incomplete write memory binary packet received, dropping connection");
 946                 return ERROR_SERVER_REMOTE_CLOSED;
 947         }
 948
 949         len = strtoul(separator+1, &separator, 16);
 950
 951         if (*(separator++) != ':')
 952         {
 953                 ERROR("incomplete write memory binary packet received, dropping connection");
 954                 return ERROR_SERVER_REMOTE_CLOSED;
 955         }
 956
 957         retval = ERROR_OK;
 958         if( len ) {
 959
 960                 buffer = malloc(len);
 961
 962                 DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
 963
 964                 memcpy( buffer, separator, len );
 965
 966                 switch (len)
 967                 {
 968                         case 4:
 969                                 if ((addr % 4) == 0)
 970                                         retval = target->type->write_memory(target, addr, 4, 1, buffer);
 971                                 else
 972                                         retval = target->type->write_memory(target, addr, 1, len, buffer);
 973                                 break;
 974                         case 2:
 975                                 if ((addr % 2) == 0)
 976                                         retval = target->type->write_memory(target, addr, 2, 1, buffer);
 977                                 else
 978                                         retval = target->type->write_memory(target, addr, 1, len, buffer);
 979                                 break;
 980                         case 3:
 981                         case 1:
 982                                 retval = target->type->write_memory(target, addr, 1, len, buffer);
 983                                 break;
 984                         default:
 985                                 retval = target_write_buffer(target, addr, len, buffer);
 986                                 break;
 987                 }
 988
 989                 free(buffer);
 990         }
 991
 992         if (retval == ERROR_OK)
 993         {
 994                 gdb_put_packet(connection, "OK", 2);
 995         }
 996         else
 997         {
 998                 if ((retval = gdb_memory_packet_error(connection, retval)) != ERROR_OK)
 999                         return retval;
1000         }
1001
1002         return ERROR_OK;
1003 }
1004
1005 void gdb_step_continue_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
1006 {
1007         int current = 0;
1008         u32 address = 0x0;
1009
1010         DEBUG("-");
1011
1012         if (packet_size > 1)
1013         {
1014                 packet[packet_size] = 0;
1015                 address = strtoul(packet + 1, NULL, 16);
1016         }
1017         else
1018         {
1019                 current = 1;
1020         }
1021
1022         if (packet[0] == 'c')
1023         {
1024                 DEBUG("continue");
1025                 target->type->resume(target, current, address, 0, 0); /* resume at current address, don't handle breakpoints, not debugging */
1026         }
1027         else if (packet[0] == 's')
1028         {
1029                 DEBUG("step");
1030                 target->type->step(target, current, address, 0); /* step at current or address, don't handle breakpoints */
1031         }
1032 }
1033
1034 int gdb_bp_wp_packet_error(connection_t *connection, int retval)
1035 {
1036         switch (retval)
1037         {
1038                 case ERROR_TARGET_NOT_HALTED:
1039                         ERROR("gdb tried to set a breakpoint but we're not halted, dropping connection");
1040                         return ERROR_SERVER_REMOTE_CLOSED;
1041                         break;
1042                 case ERROR_TARGET_RESOURCE_NOT_AVAILABLE:
1043                         gdb_send_error(connection, EBUSY);
1044                         break;
1045                 default:
1046                         ERROR("BUG: unexpected error %i", retval);
1047                         exit(-1);
1048         }
1049
1050         return ERROR_OK;
1051 }
1052
1053 int gdb_breakpoint_watchpoint_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
1054 {
1055         int type;
1056         enum breakpoint_type bp_type = BKPT_SOFT /* dummy init to avoid warning */;
1057         enum watchpoint_rw wp_type;
1058         u32 address;
1059         u32 size;
1060         char *separator;
1061         int retval;
1062
1063         DEBUG("-");
1064
1065         type = strtoul(packet + 1, &separator, 16);
1066
1067         if (type == 0)  /* memory breakpoint */
1068                 bp_type = BKPT_SOFT;
1069         else if (type == 1) /* hardware breakpoint */
1070                 bp_type = BKPT_HARD;
1071         else if (type == 2) /* write watchpoint */
1072                 wp_type = WPT_WRITE;
1073         else if (type == 3) /* read watchpoint */
1074                 wp_type = WPT_READ;
1075         else if (type == 4) /* access watchpoint */
1076                 wp_type = WPT_ACCESS;
1077
1078         if (*separator != ',')
1079         {
1080                 ERROR("incomplete breakpoint/watchpoint packet received, dropping connection");
1081                 return ERROR_SERVER_REMOTE_CLOSED;
1082         }
1083
1084         address = strtoul(separator+1, &separator, 16);
1085
1086         if (*separator != ',')
1087         {
1088                 ERROR("incomplete breakpoint/watchpoint packet received, dropping connection");
1089                 return ERROR_SERVER_REMOTE_CLOSED;
1090         }
1091
1092         size = strtoul(separator+1, &separator, 16);
1093
1094         switch (type)
1095         {
1096                 case 0:
1097                 case 1:
1098                         if (packet[0] == 'Z')
1099                         {
1100                                 if ((retval = breakpoint_add(target, address, size, bp_type)) != ERROR_OK)
1101                                 {
1102                                         if ((retval = gdb_bp_wp_packet_error(connection, retval)) != ERROR_OK)
1103                                                 return retval;
1104                                 }
1105                                 else
1106                                 {
1107                                         gdb_put_packet(connection, "OK", 2);
1108                                 }
1109                         }
1110                         else
1111                         {
1112                                 breakpoint_remove(target, address);
1113                                 gdb_put_packet(connection, "OK", 2);
1114                         }
1115                         break;
1116                 case 2:
1117                 case 3:
1118                 case 4:
1119                 {
1120                         if (packet[0] == 'Z')
1121                         {
1122                                 if ((retval = watchpoint_add(target, address, size, type-2, 0, 0xffffffffu)) != ERROR_OK)
1123                                 {
1124                                         if ((retval = gdb_bp_wp_packet_error(connection, retval)) != ERROR_OK)
1125                                                 return retval;
1126                                 }
1127                                 else
1128                                 {
1129                                         gdb_put_packet(connection, "OK", 2);
1130                                 }
1131                         }
1132                         else
1133                         {
1134                                 watchpoint_remove(target, address);
1135                                 gdb_put_packet(connection, "OK", 2);
1136                         }
1137                         break;
1138                 }
1139                 default:
1140                         break;
1141         }
1142
1143         return ERROR_OK;
1144 }
1145
1146 void gdb_query_packet(connection_t *connection, char *packet, int packet_size)
1147 {
1148         command_context_t *cmd_ctx = connection->cmd_ctx;
1149
1150         if (strstr(packet, "qRcmd,"))
1151         {
1152                 if (packet_size > 6)
1153                 {
1154                         char *cmd;
1155                         int i;
1156                         cmd = malloc((packet_size - 6)/2 + 1);
1157                         for (i=0; i < (packet_size - 6)/2; i++)
1158                         {
1159                                 u32 tmp;
1160                                 sscanf(packet + 6 + 2*i, "%2x", &tmp);
1161                                 cmd[i] = tmp;
1162                         }
1163                         cmd[(packet_size - 6)/2] = 0x0;
1164                         command_run_line(cmd_ctx, cmd);
1165                         free(cmd);
1166                 }
1167                 gdb_put_packet(connection, "OK", 2);
1168                 return;
1169         }
1170
1171         gdb_put_packet(connection, "", 0);
1172 }
1173
1174 int gdb_input(connection_t *connection)
1175 {
1176         gdb_service_t *gdb_service = connection->service->priv;
1177         target_t *target = gdb_service->target;
1178         char packet[GDB_BUFFER_SIZE];
1179         int packet_size;
1180         int retval;
1181         gdb_connection_t *gdb_con = connection->priv;
1182
1183         /* drain input buffer */
1184         do
1185         {
1186                 packet_size = GDB_BUFFER_SIZE-1;
1187                 if ((retval = gdb_get_packet(connection, packet, &packet_size)) != ERROR_OK)
1188                 {
1189                         switch (retval)
1190                         {
1191                                 case ERROR_GDB_BUFFER_TOO_SMALL:
1192                                         ERROR("BUG: buffer supplied for gdb packet was too small");
1193                                         exit(-1);
1194                                 case ERROR_SERVER_REMOTE_CLOSED:
1195                                         return ERROR_SERVER_REMOTE_CLOSED;
1196                                 default:
1197                                         ERROR("BUG: unexpected error");
1198                                         exit(-1);
1199                         }
1200                 }
1201
1202                 /* terminate with zero */
1203                 packet[packet_size] = 0;
1204
1205                 DEBUG("recevied packet: '%s'", packet);
1206
1207                 if (packet_size > 0)
1208                 {
1209                         retval = ERROR_OK;
1210                         switch (packet[0])
1211                         {
1212                                 case 'H':
1213                                         /* Hct... -- set thread
1214                                         * we don't have threads, send empty reply */
1215                                         gdb_put_packet(connection, NULL, 0);
1216                                         break;
1217                                 case 'q':
1218                                         gdb_query_packet(connection, packet, packet_size);
1219                                         break;
1220                                 case 'g':
1221                                         retval = gdb_get_registers_packet(connection, target, packet, packet_size);
1222                                         break;
1223                                 case 'G':
1224                                         retval = gdb_set_registers_packet(connection, target, packet, packet_size);
1225                                         break;
1226                                 case 'p':
1227                                         retval = gdb_get_register_packet(connection, target, packet, packet_size);
1228                                         break;
1229                                 case 'P':
1230                                         retval = gdb_set_register_packet(connection, target, packet, packet_size);
1231                                         break;
1232                                 case 'm':
1233                                         retval = gdb_read_memory_packet(connection, target, packet, packet_size);
1234                                         break;
1235                                 case 'M':
1236                                         retval = gdb_write_memory_packet(connection, target, packet, packet_size);
1237                                         break;
1238                                 case 'z':
1239                                 case 'Z':
1240                                         retval = gdb_breakpoint_watchpoint_packet(connection, target, packet, packet_size);
1241                                         break;
1242                                 case '?':
1243                                         gdb_last_signal_packet(connection, target, packet, packet_size);
1244                                 break;
1245                                 case 'c':
1246                                 case 's':
1247                                         gdb_step_continue_packet(connection, target, packet, packet_size);
1248                                         break;
1249                                 case 'D':
1250                                         target->type->resume(target, 1, 0, 1, 0);
1251                                         gdb_put_packet(connection, "OK", 2);
1252                                         break;
1253                                 case 'X':
1254                                         if ((retval = gdb_write_memory_binary_packet(connection, target, packet, packet_size)) != ERROR_OK)
1255                                                 return retval;
1256                                         break;
1257                                 case 'k':
1258                                         gdb_put_packet(connection, "OK", 2);
1259                                         return ERROR_SERVER_REMOTE_CLOSED;
1260                                 default:
1261                                         /* ignore unkown packets */
1262                                         DEBUG("ignoring 0x%2.2x packet", packet[0]);
1263                                         gdb_put_packet(connection, NULL, 0);
1264                                         break;
1265                         }
1266
1267                         /* if a packet handler returned an error, exit input loop */
1268                         if (retval != ERROR_OK)
1269                                 return retval;
1270                 }
1271
1272                 if (gdb_con->ctrl_c)
1273                 {
1274                         if (target->state == TARGET_RUNNING)
1275                         {
1276                                 target->type->halt(target);
1277                                 gdb_con->ctrl_c = 0;
1278                         }
1279                 }
1280
1281         } while (gdb_con->buf_cnt > 0);
1282
1283         return ERROR_OK;
1284 }
1285
1286 int gdb_init()
1287 {
1288         gdb_service_t *gdb_service;
1289         target_t *target = targets;
1290         int i = 0;
1291
1292         if (!target)
1293         {
1294                 WARNING("no gdb ports allocated as no target has been specified");
1295                 return ERROR_OK;
1296         }
1297
1298         if (gdb_port == 0)
1299         {
1300                 WARNING("no gdb port specified, using default port 3333");
1301                 gdb_port = 3333;
1302         }
1303
1304         while (target)
1305         {
1306                 char service_name[8];
1307
1308                 snprintf(service_name, 8, "gdb-%2.2i", i);
1309
1310                 gdb_service = malloc(sizeof(gdb_service_t));
1311                 gdb_service->target = target;
1312
1313                 add_service("gdb", CONNECTION_GDB, gdb_port + i, 1, gdb_new_connection, gdb_input, gdb_connection_closed, gdb_service);
1314
1315                 DEBUG("gdb service for target %s at port %i", target->type->name, gdb_port + i);
1316
1317                 i++;
1318                 target = target->next;
1319         }
1320
1321         return ERROR_OK;
1322 }
1323
1324 /* daemon configuration command gdb_port */
1325 int handle_gdb_port_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1326 {
1327         if (argc == 0)
1328                 return ERROR_OK;
1329
1330         /* only if the port wasn't overwritten by cmdline */
1331         if (gdb_port == 0)
1332                 gdb_port = strtoul(args[0], NULL, 0);
1333
1334         return ERROR_OK;
1335 }
1336
1337 int gdb_register_commands(command_context_t *command_context)
1338 {
1339         register_command(command_context, NULL, "gdb_port", handle_gdb_port_command,
1340                                          COMMAND_CONFIG, "");
1341
1342         return ERROR_OK;
1343 }