gdbstub.c

   1 /*
   2  * gdb server stub
   3  *
   4  * Copyright (c) 2003 Fabrice Bellard
   5  *
   6  * This library is free software; you can redistribute it and/or
   7  * modify it under the terms of the GNU Lesser General Public
   8  * License as published by the Free Software Foundation; either
   9  * version 2 of the License, or (at your option) any later version.
  10  *
  11  * This library is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  14  * Lesser General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU Lesser General Public
  17  * License along with this library; if not, write to the Free Software
  18  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  19  */
  20 #include <stdlib.h>
  21 #include <stdio.h>
  22 #include <string.h>
  23 #include <unistd.h>
  24 #include <errno.h>
  25 #include <sys/socket.h>
  26 #include <netinet/in.h>
  27 #include <netinet/tcp.h>
  28 #include <signal.h>
  29 #include <fcntl.h>
  30
  31 #include "vl.h"
  32
  33 //#define DEBUG_GDB
  34
  35 enum RSState {
  36     RS_IDLE,
  37     RS_GETLINE,
  38     RS_CHKSUM1,
  39     RS_CHKSUM2,
  40 };
  41
  42 static int gdbserver_fd;
  43
  44 typedef struct GDBState {
  45     enum RSState state;
  46     int fd;
  47     char line_buf[4096];
  48     int line_buf_index;
  49     int line_csum;
  50 } GDBState;
  51
  52 static int get_char(GDBState *s)
  53 {
  54     uint8_t ch;
  55     int ret;
  56
  57     for(;;) {
  58         ret = read(s->fd, &ch, 1);
  59         if (ret < 0) {
  60             if (errno != EINTR && errno != EAGAIN)
  61                 return -1;
  62         } else if (ret == 0) {
  63             return -1;
  64         } else {
  65             break;
  66         }
  67     }
  68     return ch;
  69 }
  70
  71 static void put_buffer(GDBState *s, const uint8_t *buf, int len)
  72 {
  73     int ret;
  74
  75     while (len > 0) {
  76         ret = write(s->fd, buf, len);
  77         if (ret < 0) {
  78             if (errno != EINTR && errno != EAGAIN)
  79                 return;
  80         } else {
  81             buf += ret;
  82             len -= ret;
  83         }
  84     }
  85 }
  86
  87 static inline int fromhex(int v)
  88 {
  89     if (v >= '0' && v <= '9')
  90         return v - '0';
  91     else if (v >= 'A' && v <= 'F')
  92         return v - 'A' + 10;
  93     else if (v >= 'a' && v <= 'f')
  94         return v - 'a' + 10;
  95     else
  96         return 0;
  97 }
  98
  99 static inline int tohex(int v)
 100 {
 101     if (v < 10)
 102         return v + '0';
 103     else
 104         return v - 10 + 'a';
 105 }
 106
 107 static void memtohex(char *buf, const uint8_t *mem, int len)
 108 {
 109     int i, c;
 110     char *q;
 111     q = buf;
 112     for(i = 0; i < len; i++) {
 113         c = mem[i];
 114         *q++ = tohex(c >> 4);
 115         *q++ = tohex(c & 0xf);
 116     }
 117     *q = '\0';
 118 }
 119
 120 static void hextomem(uint8_t *mem, const char *buf, int len)
 121 {
 122     int i;
 123
 124     for(i = 0; i < len; i++) {
 125         mem[i] = (fromhex(buf[0]) << 4) | fromhex(buf[1]);
 126         buf += 2;
 127     }
 128 }
 129
 130 /* return -1 if error, 0 if OK */
 131 static int put_packet(GDBState *s, char *buf)
 132 {
 133     char buf1[3];
 134     int len, csum, ch, i;
 135
 136 #ifdef DEBUG_GDB
 137     printf("reply='%s'\n", buf);
 138 #endif
 139
 140     for(;;) {
 141         buf1[0] = '$';
 142         put_buffer(s, buf1, 1);
 143         len = strlen(buf);
 144         put_buffer(s, buf, len);
 145         csum = 0;
 146         for(i = 0; i < len; i++) {
 147             csum += buf[i];
 148         }
 149         buf1[0] = '#';
 150         buf1[1] = tohex((csum >> 4) & 0xf);
 151         buf1[2] = tohex((csum) & 0xf);
 152
 153         put_buffer(s, buf1, 3);
 154
 155         ch = get_char(s);
 156         if (ch < 0)
 157             return -1;
 158         if (ch == '+')
 159             break;
 160     }
 161     return 0;
 162 }
 163
 164 #if defined(TARGET_I386)
 165
 166 static void to_le32(uint8_t *p, int v)
 167 {
 168     p[0] = v;
 169     p[1] = v >> 8;
 170     p[2] = v >> 16;
 171     p[3] = v >> 24;
 172 }
 173
 174 static int cpu_gdb_read_registers(CPUState *env, uint8_t *mem_buf)
 175 {
 176     int i, fpus;
 177
 178     for(i = 0; i < 8; i++) {
 179         to_le32(mem_buf + i * 4, env->regs[i]);
 180     }
 181     to_le32(mem_buf + 8 * 4, env->eip);
 182     to_le32(mem_buf + 9 * 4, env->eflags);
 183     to_le32(mem_buf + 10 * 4, env->segs[R_CS].selector);
 184     to_le32(mem_buf + 11 * 4, env->segs[R_SS].selector);
 185     to_le32(mem_buf + 12 * 4, env->segs[R_DS].selector);
 186     to_le32(mem_buf + 13 * 4, env->segs[R_ES].selector);
 187     to_le32(mem_buf + 14 * 4, env->segs[R_FS].selector);
 188     to_le32(mem_buf + 15 * 4, env->segs[R_GS].selector);
 189     /* XXX: convert floats */
 190     for(i = 0; i < 8; i++) {
 191         memcpy(mem_buf + 16 * 4 + i * 10, &env->fpregs[i], 10);
 192     }
 193     to_le32(mem_buf + 36 * 4, env->fpuc);
 194     fpus = (env->fpus & ~0x3800) | (env->fpstt & 0x7) << 11;
 195     to_le32(mem_buf + 37 * 4, fpus);
 196     to_le32(mem_buf + 38 * 4, 0); /* XXX: convert tags */
 197     to_le32(mem_buf + 39 * 4, 0); /* fiseg */
 198     to_le32(mem_buf + 40 * 4, 0); /* fioff */
 199     to_le32(mem_buf + 41 * 4, 0); /* foseg */
 200     to_le32(mem_buf + 42 * 4, 0); /* fooff */
 201     to_le32(mem_buf + 43 * 4, 0); /* fop */
 202     return 44 * 4;
 203 }
 204
 205 static void cpu_gdb_write_registers(CPUState *env, uint8_t *mem_buf, int size)
 206 {
 207     uint32_t *registers = (uint32_t *)mem_buf;
 208     int i;
 209
 210     for(i = 0; i < 8; i++) {
 211         env->regs[i] = tswapl(registers[i]);
 212     }
 213     env->eip = registers[8];
 214     env->eflags = registers[9];
 215 #if defined(CONFIG_USER_ONLY)
 216 #define LOAD_SEG(index, sreg)\
 217             if (tswapl(registers[index]) != env->segs[sreg].selector)\
 218                 cpu_x86_load_seg(env, sreg, tswapl(registers[index]));
 219             LOAD_SEG(10, R_CS);
 220             LOAD_SEG(11, R_SS);
 221             LOAD_SEG(12, R_DS);
 222             LOAD_SEG(13, R_ES);
 223             LOAD_SEG(14, R_FS);
 224             LOAD_SEG(15, R_GS);
 225 #endif
 226 }
 227
 228 #elif defined (TARGET_PPC)
 229 static void to_le32(uint8_t *p, int v)
 230 {
 231     p[3] = v;
 232     p[2] = v >> 8;
 233     p[1] = v >> 16;
 234     p[0] = v >> 24;
 235 }
 236
 237 static int cpu_gdb_read_registers(CPUState *env, uint8_t *mem_buf)
 238 {
 239     uint32_t tmp;
 240     int i;
 241
 242     /* fill in gprs */
 243     for(i = 0; i < 8; i++) {
 244         to_le32(mem_buf + i * 4, env->gpr[i]);
 245     }
 246     /* fill in fprs */
 247     for (i = 0; i < 32; i++) {
 248         to_le32(mem_buf + (i * 2) + 32, *((uint32_t *)&env->fpr[i]));
 249         to_le32(mem_buf + (i * 2) + 33, *((uint32_t *)&env->fpr[i] + 1));
 250     }
 251     /* nip, msr, ccr, lnk, ctr, xer, mq */
 252     to_le32(mem_buf + 96, tswapl(env->nip));
 253     to_le32(mem_buf + 97, tswapl(_load_msr()));
 254     to_le32(mem_buf + 98, 0);
 255     tmp = 0;
 256     for (i = 0; i < 8; i++)
 257         tmp |= env->crf[i] << (32 - (i * 4));
 258     to_le32(mem_buf + 98, tmp);
 259     to_le32(mem_buf + 99, tswapl(env->lr));
 260     to_le32(mem_buf + 100, tswapl(env->ctr));
 261     to_le32(mem_buf + 101, tswapl(_load_xer()));
 262     to_le32(mem_buf + 102, 0);
 263
 264     return 102;
 265 }
 266
 267 static void cpu_gdb_write_registers(CPUState *env, uint8_t *mem_buf, int size)
 268 {
 269     uint32_t *registers = (uint32_t *)mem_buf;
 270     int i;
 271
 272     /* fill in gprs */
 273     for (i = 0; i < 32; i++) {
 274         env->gpr[i] = tswapl(registers[i]);
 275     }
 276     /* fill in fprs */
 277     for (i = 0; i < 32; i++) {
 278         *((uint32_t *)&env->fpr[i]) = tswapl(registers[(i * 2) + 32]);
 279         *((uint32_t *)&env->fpr[i] + 1) = tswapl(registers[(i * 2) + 33]);
 280     }
 281     /* nip, msr, ccr, lnk, ctr, xer, mq */
 282     env->nip = tswapl(registers[96]);
 283     _store_msr(tswapl(registers[97]));
 284     registers[98] = tswapl(registers[98]);
 285     for (i = 0; i < 8; i++)
 286         env->crf[i] = (registers[98] >> (32 - (i * 4))) & 0xF;
 287     env->lr = tswapl(registers[99]);
 288     env->ctr = tswapl(registers[100]);
 289     _store_xer(tswapl(registers[101]));
 290 }
 291 #else
 292
 293 static int cpu_gdb_read_registers(CPUState *env, uint8_t *mem_buf)
 294 {
 295     return 0;
 296 }
 297
 298 static void cpu_gdb_write_registers(CPUState *env, uint8_t *mem_buf, int size)
 299 {
 300 }
 301
 302 #endif
 303
 304 /* port = 0 means default port */
 305 static int gdb_handle_packet(GDBState *s, const char *line_buf)
 306 {
 307     CPUState *env = cpu_single_env;
 308     const char *p;
 309     int ch, reg_size, type;
 310     char buf[4096];
 311     uint8_t mem_buf[2000];
 312     uint32_t *registers;
 313     uint32_t addr, len;
 314
 315 #ifdef DEBUG_GDB
 316     printf("command='%s'\n", line_buf);
 317 #endif
 318     p = line_buf;
 319     ch = *p++;
 320     switch(ch) {
 321     case '?':
 322         snprintf(buf, sizeof(buf), "S%02x", SIGTRAP);
 323         put_packet(s, buf);
 324         break;
 325     case 'c':
 326         if (*p != '\0') {
 327             addr = strtoul(p, (char **)&p, 16);
 328 #if defined(TARGET_I386)
 329             env->eip = addr;
 330 #elif defined (TARGET_PPC)
 331             env->nip = addr;
 332 #endif
 333         }
 334         vm_start();
 335         break;
 336     case 's':
 337         if (*p != '\0') {
 338             addr = strtoul(p, (char **)&p, 16);
 339 #if defined(TARGET_I386)
 340             env->eip = addr;
 341 #elif defined (TARGET_PPC)
 342             env->nip = addr;
 343 #endif
 344         }
 345         cpu_single_step(env, 1);
 346         vm_start();
 347         break;
 348     case 'g':
 349         reg_size = cpu_gdb_read_registers(env, mem_buf);
 350         memtohex(buf, mem_buf, reg_size);
 351         put_packet(s, buf);
 352         break;
 353     case 'G':
 354         registers = (void *)mem_buf;
 355         len = strlen(p) / 2;
 356         hextomem((uint8_t *)registers, p, len);
 357         cpu_gdb_write_registers(env, mem_buf, len);
 358         put_packet(s, "OK");
 359         break;
 360     case 'm':
 361         addr = strtoul(p, (char **)&p, 16);
 362         if (*p == ',')
 363             p++;
 364         len = strtoul(p, NULL, 16);
 365         if (cpu_memory_rw_debug(env, addr, mem_buf, len, 0) != 0)
 366             memset(mem_buf, 0, len);
 367         memtohex(buf, mem_buf, len);
 368         put_packet(s, buf);
 369         break;
 370     case 'M':
 371         addr = strtoul(p, (char **)&p, 16);
 372         if (*p == ',')
 373             p++;
 374         len = strtoul(p, (char **)&p, 16);
 375         if (*p == ',')
 376             p++;
 377         hextomem(mem_buf, p, len);
 378         if (cpu_memory_rw_debug(env, addr, mem_buf, len, 1) != 0)
 379             put_packet(s, "ENN");
 380         else
 381             put_packet(s, "OK");
 382         break;
 383     case 'Z':
 384         type = strtoul(p, (char **)&p, 16);
 385         if (*p == ',')
 386             p++;
 387         addr = strtoul(p, (char **)&p, 16);
 388         if (*p == ',')
 389             p++;
 390         len = strtoul(p, (char **)&p, 16);
 391         if (type == 0 || type == 1) {
 392             if (cpu_breakpoint_insert(env, addr) < 0)
 393                 goto breakpoint_error;
 394             put_packet(s, "OK");
 395         } else {
 396         breakpoint_error:
 397             put_packet(s, "ENN");
 398         }
 399         break;
 400     case 'z':
 401         type = strtoul(p, (char **)&p, 16);
 402         if (*p == ',')
 403             p++;
 404         addr = strtoul(p, (char **)&p, 16);
 405         if (*p == ',')
 406             p++;
 407         len = strtoul(p, (char **)&p, 16);
 408         if (type == 0 || type == 1) {
 409             cpu_breakpoint_remove(env, addr);
 410             put_packet(s, "OK");
 411         } else {
 412             goto breakpoint_error;
 413         }
 414         break;
 415     default:
 416         //        unknown_command:
 417         /* put empty packet */
 418         buf[0] = '\0';
 419         put_packet(s, buf);
 420         break;
 421     }
 422     return RS_IDLE;
 423 }
 424
 425 static void gdb_vm_stopped(void *opaque, int reason)
 426 {
 427     GDBState *s = opaque;
 428     char buf[256];
 429     int ret;
 430
 431     /* disable single step if it was enable */
 432     cpu_single_step(cpu_single_env, 0);
 433
 434     if (reason == EXCP_DEBUG)
 435         ret = SIGTRAP;
 436     else
 437         ret = 0;
 438     snprintf(buf, sizeof(buf), "S%02x", ret);
 439     put_packet(s, buf);
 440 }
 441
 442 static void gdb_read_byte(GDBState *s, int ch)
 443 {
 444     int i, csum;
 445     char reply[1];
 446
 447     if (vm_running) {
 448         /* when the CPU is running, we cannot do anything except stop
 449            it when receiving a char */
 450         vm_stop(EXCP_INTERRUPT);
 451     } else {
 452         switch(s->state) {
 453         case RS_IDLE:
 454             if (ch == '$') {
 455                 s->line_buf_index = 0;
 456                 s->state = RS_GETLINE;
 457             }
 458             break;
 459         case RS_GETLINE:
 460             if (ch == '#') {
 461             s->state = RS_CHKSUM1;
 462             } else if (s->line_buf_index >= sizeof(s->line_buf) - 1) {
 463                 s->state = RS_IDLE;
 464             } else {
 465             s->line_buf[s->line_buf_index++] = ch;
 466             }
 467             break;
 468         case RS_CHKSUM1:
 469             s->line_buf[s->line_buf_index] = '\0';
 470             s->line_csum = fromhex(ch) << 4;
 471             s->state = RS_CHKSUM2;
 472             break;
 473         case RS_CHKSUM2:
 474             s->line_csum |= fromhex(ch);
 475             csum = 0;
 476             for(i = 0; i < s->line_buf_index; i++) {
 477                 csum += s->line_buf[i];
 478             }
 479             if (s->line_csum != (csum & 0xff)) {
 480                 reply[0] = '-';
 481                 put_buffer(s, reply, 1);
 482                 s->state = RS_IDLE;
 483             } else {
 484                 reply[0] = '+';
 485                 put_buffer(s, reply, 1);
 486                 s->state = gdb_handle_packet(s, s->line_buf);
 487             }
 488             break;
 489         }
 490     }
 491 }
 492
 493 static int gdb_can_read(void *opaque)
 494 {
 495     return 256;
 496 }
 497
 498 static void gdb_read(void *opaque, const uint8_t *buf, int size)
 499 {
 500     GDBState *s = opaque;
 501     int i;
 502     if (size == 0) {
 503         /* end of connection */
 504         qemu_del_vm_stop_handler(gdb_vm_stopped, s);
 505         qemu_del_fd_read_handler(s->fd);
 506         qemu_free(s);
 507         vm_start();
 508     } else {
 509         for(i = 0; i < size; i++)
 510             gdb_read_byte(s, buf[i]);
 511     }
 512 }
 513
 514 static void gdb_accept(void *opaque, const uint8_t *buf, int size)
 515 {
 516     GDBState *s;
 517     struct sockaddr_in sockaddr;
 518     socklen_t len;
 519     int val, fd;
 520
 521     for(;;) {
 522         len = sizeof(sockaddr);
 523         fd = accept(gdbserver_fd, (struct sockaddr *)&sockaddr, &len);
 524         if (fd < 0 && errno != EINTR) {
 525             perror("accept");
 526             return;
 527         } else if (fd >= 0) {
 528             break;
 529         }
 530     }
 531
 532     /* set short latency */
 533     val = 1;
 534     setsockopt(fd, SOL_TCP, TCP_NODELAY, &val, sizeof(val));
 535
 536     s = qemu_mallocz(sizeof(GDBState));
 537     if (!s) {
 538         close(fd);
 539         return;
 540     }
 541     s->fd = fd;
 542
 543     fcntl(fd, F_SETFL, O_NONBLOCK);
 544
 545     /* stop the VM */
 546     vm_stop(EXCP_INTERRUPT);
 547
 548     /* start handling I/O */
 549     qemu_add_fd_read_handler(s->fd, gdb_can_read, gdb_read, s);
 550     /* when the VM is stopped, the following callback is called */
 551     qemu_add_vm_stop_handler(gdb_vm_stopped, s);
 552 }
 553
 554 static int gdbserver_open(int port)
 555 {
 556     struct sockaddr_in sockaddr;
 557     int fd, val, ret;
 558
 559     fd = socket(PF_INET, SOCK_STREAM, 0);
 560     if (fd < 0) {
 561         perror("socket");
 562         return -1;
 563     }
 564
 565     /* allow fast reuse */
 566     val = 1;
 567     setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &val, sizeof(val));
 568
 569     sockaddr.sin_family = AF_INET;
 570     sockaddr.sin_port = htons(port);
 571     sockaddr.sin_addr.s_addr = 0;
 572     ret = bind(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
 573     if (ret < 0) {
 574         perror("bind");
 575         return -1;
 576     }
 577     ret = listen(fd, 0);
 578     if (ret < 0) {
 579         perror("listen");
 580         return -1;
 581     }
 582     fcntl(fd, F_SETFL, O_NONBLOCK);
 583     return fd;
 584 }
 585
 586 int gdbserver_start(int port)
 587 {
 588     gdbserver_fd = gdbserver_open(port);
 589     if (gdbserver_fd < 0)
 590         return -1;
 591     /* accept connections */
 592     qemu_add_fd_read_handler(gdbserver_fd, NULL, gdb_accept, NULL);
 593     return 0;
 594 }