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
  30 #include "config.h"
  31 #ifdef TARGET_I386
  32 #include "cpu-i386.h"
  33 #endif
  34 #ifdef TARGET_ARM
  35 #include "cpu-arm.h"
  36 #endif
  37 #include "thunk.h"
  38 #include "exec.h"
  39
  40 //#define DEBUG_GDB
  41
  42 int gdbstub_fd = -1;
  43
  44 /* return 0 if OK */
  45 static int gdbstub_open(int port)
  46 {
  47     struct sockaddr_in sockaddr;
  48     socklen_t len;
  49     int fd, val, ret;
  50
  51     fd = socket(PF_INET, SOCK_STREAM, 0);
  52     if (fd < 0) {
  53         perror("socket");
  54         return -1;
  55     }
  56
  57     /* allow fast reuse */
  58     val = 1;
  59     setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &val, sizeof(val));
  60
  61     sockaddr.sin_family = AF_INET;
  62     sockaddr.sin_port = htons(port);
  63     sockaddr.sin_addr.s_addr = 0;
  64     ret = bind(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
  65     if (ret < 0) {
  66         perror("bind");
  67         return -1;
  68     }
  69     ret = listen(fd, 0);
  70     if (ret < 0) {
  71         perror("listen");
  72         return -1;
  73     }
  74
  75     /* now wait for one connection */
  76     for(;;) {
  77         len = sizeof(sockaddr);
  78         gdbstub_fd = accept(fd, (struct sockaddr *)&sockaddr, &len);
  79         if (gdbstub_fd < 0 && errno != EINTR) {
  80             perror("accept");
  81             return -1;
  82         } else if (gdbstub_fd >= 0) {
  83             break;
  84         }
  85     }
  86
  87     /* set short latency */
  88     val = 1;
  89     setsockopt(gdbstub_fd, SOL_TCP, TCP_NODELAY, &val, sizeof(val));
  90     return 0;
  91 }
  92
  93 static int get_char(void)
  94 {
  95     uint8_t ch;
  96     int ret;
  97
  98     for(;;) {
  99         ret = read(gdbstub_fd, &ch, 1);
 100         if (ret < 0) {
 101             if (errno != EINTR && errno != EAGAIN)
 102                 return -1;
 103         } else if (ret == 0) {
 104             return -1;
 105         } else {
 106             break;
 107         }
 108     }
 109     return ch;
 110 }
 111
 112 static void put_buffer(const uint8_t *buf, int len)
 113 {
 114     int ret;
 115
 116     while (len > 0) {
 117         ret = write(gdbstub_fd, buf, len);
 118         if (ret < 0) {
 119             if (errno != EINTR && errno != EAGAIN)
 120                 return;
 121         } else {
 122             buf += ret;
 123             len -= ret;
 124         }
 125     }
 126 }
 127
 128 static inline int fromhex(int v)
 129 {
 130     if (v >= '0' && v <= '9')
 131         return v - '0';
 132     else if (v >= 'A' && v <= 'F')
 133         return v - 'A' + 10;
 134     else if (v >= 'a' && v <= 'f')
 135         return v - 'a' + 10;
 136     else
 137         return 0;
 138 }
 139
 140 static inline int tohex(int v)
 141 {
 142     if (v < 10)
 143         return v + '0';
 144     else
 145         return v - 10 + 'a';
 146 }
 147
 148 static void memtohex(char *buf, const uint8_t *mem, int len)
 149 {
 150     int i, c;
 151     char *q;
 152     q = buf;
 153     for(i = 0; i < len; i++) {
 154         c = mem[i];
 155         *q++ = tohex(c >> 4);
 156         *q++ = tohex(c & 0xf);
 157     }
 158     *q = '\0';
 159 }
 160
 161 static void hextomem(uint8_t *mem, const char *buf, int len)
 162 {
 163     int i;
 164
 165     for(i = 0; i < len; i++) {
 166         mem[i] = (fromhex(buf[0]) << 4) | fromhex(buf[1]);
 167         buf += 2;
 168     }
 169 }
 170
 171 /* return -1 if error or EOF */
 172 static int get_packet(char *buf, int buf_size)
 173 {
 174     int ch, len, csum, csum1;
 175     char reply[1];
 176
 177     for(;;) {
 178         for(;;) {
 179             ch = get_char();
 180             if (ch < 0)
 181                 return -1;
 182             if (ch == '$')
 183                 break;
 184         }
 185         len = 0;
 186         csum = 0;
 187         for(;;) {
 188             ch = get_char();
 189             if (ch < 0)
 190                 return -1;
 191             if (ch == '#')
 192                 break;
 193             if (len > buf_size - 1)
 194                 return -1;
 195             buf[len++] = ch;
 196             csum += ch;
 197         }
 198         buf[len] = '\0';
 199         ch = get_char();
 200         if (ch < 0)
 201             return -1;
 202         csum1 = fromhex(ch) << 4;
 203         ch = get_char();
 204         if (ch < 0)
 205             return -1;
 206         csum1 |= fromhex(ch);
 207         if ((csum & 0xff) != csum1) {
 208             reply[0] = '-';
 209             put_buffer(reply, 1);
 210         } else {
 211             reply[0] = '+';
 212             put_buffer(reply, 1);
 213             break;
 214         }
 215     }
 216 #ifdef DEBUG_GDB
 217     printf("command='%s'\n", buf);
 218 #endif
 219     return len;
 220 }
 221
 222 /* return -1 if error, 0 if OK */
 223 static int put_packet(char *buf)
 224 {
 225     char buf1[3];
 226     int len, csum, ch, i;
 227
 228 #ifdef DEBUG_GDB
 229     printf("reply='%s'\n", buf);
 230 #endif
 231
 232     for(;;) {
 233         buf1[0] = '$';
 234         put_buffer(buf1, 1);
 235         len = strlen(buf);
 236         put_buffer(buf, len);
 237         csum = 0;
 238         for(i = 0; i < len; i++) {
 239             csum += buf[i];
 240         }
 241         buf1[0] = '#';
 242         buf1[1] = tohex((csum >> 4) & 0xf);
 243         buf1[2] = tohex((csum) & 0xf);
 244
 245         put_buffer(buf1, 3);
 246
 247         ch = get_char();
 248         if (ch < 0)
 249             return -1;
 250         if (ch == '+')
 251             break;
 252     }
 253     return 0;
 254 }
 255
 256 static int memory_rw(uint8_t *buf, uint32_t addr, int len, int is_write)
 257 {
 258     int l, flags;
 259     uint32_t page;
 260
 261     while (len > 0) {
 262         page = addr & TARGET_PAGE_MASK;
 263         l = (page + TARGET_PAGE_SIZE) - addr;
 264         if (l > len)
 265             l = len;
 266         flags = page_get_flags(page);
 267         if (!(flags & PAGE_VALID))
 268             return -1;
 269         if (is_write) {
 270             if (!(flags & PAGE_WRITE))
 271                 return -1;
 272             memcpy((uint8_t *)addr, buf, l);
 273         } else {
 274             if (!(flags & PAGE_READ))
 275                 return -1;
 276             memcpy(buf, (uint8_t *)addr, l);
 277         }
 278         len -= l;
 279         buf += l;
 280         addr += l;
 281     }
 282     return 0;
 283 }
 284
 285 /* port = 0 means default port */
 286 int cpu_gdbstub(void *opaque, void (*main_loop)(void *opaque), int port)
 287 {
 288     CPUState *env;
 289     const char *p;
 290     int ret, ch, nb_regs, i;
 291     char buf[4096];
 292     uint8_t mem_buf[2000];
 293     uint32_t *registers;
 294     uint32_t addr, len;
 295
 296     printf("Waiting gdb connection on port %d\n", port);
 297     if (gdbstub_open(port) < 0)
 298         return -1;
 299     printf("Connected\n");
 300     for(;;) {
 301         ret = get_packet(buf, sizeof(buf));
 302         if (ret < 0)
 303             break;
 304         p = buf;
 305         ch = *p++;
 306         switch(ch) {
 307         case '?':
 308             snprintf(buf, sizeof(buf), "S%02x", SIGTRAP);
 309             put_packet(buf);
 310             break;
 311         case 'c':
 312             main_loop(opaque);
 313             snprintf(buf, sizeof(buf), "S%02x", 0);
 314             put_packet(buf);
 315             break;
 316         case 'g':
 317             env = cpu_gdbstub_get_env(opaque);
 318             registers = (void *)mem_buf;
 319 #if defined(TARGET_I386)
 320             for(i = 0; i < 8; i++) {
 321                 registers[i] = tswapl(env->regs[i]);
 322             }
 323             registers[8] = env->eip;
 324             registers[9] = env->eflags;
 325             registers[10] = env->segs[R_CS].selector;
 326             registers[11] = env->segs[R_SS].selector;
 327             registers[12] = env->segs[R_DS].selector;
 328             registers[13] = env->segs[R_ES].selector;
 329             registers[14] = env->segs[R_FS].selector;
 330             registers[15] = env->segs[R_GS].selector;
 331             nb_regs = 16;
 332 #endif
 333             memtohex(buf, (const uint8_t *)registers,
 334                      sizeof(registers[0]) * nb_regs);
 335             put_packet(buf);
 336             break;
 337         case 'G':
 338             env = cpu_gdbstub_get_env(opaque);
 339             registers = (void *)mem_buf;
 340 #if defined(TARGET_I386)
 341             hextomem((uint8_t *)registers, p, 16 * 4);
 342             for(i = 0; i < 8; i++) {
 343                 env->regs[i] = tswapl(registers[i]);
 344             }
 345             env->eip = registers[8];
 346             env->eflags = registers[9];
 347 #define LOAD_SEG(index, sreg)\
 348             if (tswapl(registers[index]) != env->segs[sreg].selector)\
 349                 cpu_x86_load_seg(env, sreg, tswapl(registers[index]));
 350             LOAD_SEG(10, R_CS);
 351             LOAD_SEG(11, R_SS);
 352             LOAD_SEG(12, R_DS);
 353             LOAD_SEG(13, R_ES);
 354             LOAD_SEG(14, R_FS);
 355             LOAD_SEG(15, R_GS);
 356 #endif
 357             put_packet("OK");
 358             break;
 359         case 'm':
 360             addr = strtoul(p, (char **)&p, 16);
 361             if (*p == ',')
 362                 p++;
 363             len = strtoul(p, NULL, 16);
 364             if (memory_rw(mem_buf, addr, len, 0) != 0)
 365                 memset(mem_buf, 0, len);
 366             memtohex(buf, mem_buf, len);
 367             put_packet(buf);
 368             break;
 369         case 'M':
 370             addr = strtoul(p, (char **)&p, 16);
 371             if (*p == ',')
 372                 p++;
 373             len = strtoul(p, (char **)&p, 16);
 374             if (*p == ',')
 375                 p++;
 376             hextomem(mem_buf, p, len);
 377             if (memory_rw(mem_buf, addr, len, 1) != 0)
 378                 put_packet("ENN");
 379             else
 380                 put_packet("OK");
 381             break;
 382         default:
 383             /* put empty packet */
 384             buf[0] = '\0';
 385             put_packet(buf);
 386             break;
 387         }
 388     }
 389     return 0;
 390 }