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