1 /*
2 * Copyright (C) 2008 Stefan Hajnoczi <stefanha@gmail.com>.
3 *
4 * This program is free software; you can redistribute it and/or
5 * modify it under the terms of the GNU General Public License as
6 * published by the Free Software Foundation; either version 2 of the
7 * License, or any later version.
8 *
9 * This program is distributed in the hope that it will be useful, but
10 * WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 * General Public License for more details.
13 *
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
17 */
18
19 /**
20 * @file
21 *
22 * GDB stub for remote debugging
23 *
24 */
25
26 #include <stdlib.h>
27 #include <stdint.h>
28 #include "serial.h"
29
30 typedef uint32_t gdbreg_t;
31
32 enum {
33 POSIX_EINVAL = 0x1c, /* used to report bad arguments to GDB */
34 SIZEOF_PAYLOAD = 256, /* buffer size of GDB payload data */
35 DR7_CLEAR = 0x00000400, /* disable hardware breakpoints */
36 DR6_CLEAR = 0xffff0ff0, /* clear breakpoint status */
37 };
38
39 /* The register snapshot, this must be in sync with interrupt handler and the
40 * GDB protocol. */
41 enum {
42 GDBMACH_EAX,
43 GDBMACH_ECX,
44 GDBMACH_EDX,
45 GDBMACH_EBX,
46 GDBMACH_ESP,
47 GDBMACH_EBP,
48 GDBMACH_ESI,
49 GDBMACH_EDI,
50 GDBMACH_EIP,
51 GDBMACH_EFLAGS,
52 GDBMACH_CS,
53 GDBMACH_SS,
54 GDBMACH_DS,
55 GDBMACH_ES,
56 GDBMACH_FS,
57 GDBMACH_GS,
58 GDBMACH_NREGS,
59 GDBMACH_SIZEOF_REGS = GDBMACH_NREGS * sizeof(gdbreg_t)
60 };
61
62 /* Breakpoint types */
63 enum {
64 GDBMACH_BPMEM,
65 GDBMACH_BPHW,
66 GDBMACH_WATCH,
67 GDBMACH_RWATCH,
68 GDBMACH_AWATCH,
69 };
70
71 struct gdbstub {
72 int exit_handler; /* leave interrupt handler */
73
74 int signo;
75 gdbreg_t *regs;
76
77 void (*parse) (struct gdbstub * stub, char ch);
78 uint8_t cksum1;
79
80 /* Buffer for payload data when parsing a packet. Once the
81 * packet has been received, this buffer is used to hold
82 * the reply payload. */
83 char buf[SIZEOF_PAYLOAD + 4]; /* $...PAYLOAD...#XX */
84 char *payload; /* start of payload */
85 int len; /* length of payload */
86 };
87
88 /** Hardware breakpoint, fields stored in x86 bit pattern form */
89 struct hwbp {
90 int type; /* type (1=write watchpoint, 3=access watchpoint) */
91 unsigned long addr; /* linear address */
92 size_t len; /* length (0=1-byte, 1=2-byte, 3=4-byte) */
93 int enabled;
94 };
95
96 static struct hwbp hwbps[4];
97 static gdbreg_t dr7 = DR7_CLEAR;
98
gdbmach_set_pc(gdbreg_t * regs,gdbreg_t pc)99 static inline void gdbmach_set_pc(gdbreg_t * regs, gdbreg_t pc)
100 {
101 regs[GDBMACH_EIP] = pc;
102 }
103
gdbmach_set_single_step(gdbreg_t * regs,int step)104 static inline void gdbmach_set_single_step(gdbreg_t * regs, int step)
105 {
106 regs[GDBMACH_EFLAGS] &= ~(1 << 8); /* Trace Flag (TF) */
107 regs[GDBMACH_EFLAGS] |= (step << 8);
108 }
109
gdbmach_breakpoint(void)110 static inline void gdbmach_breakpoint(void)
111 {
112 __asm__ __volatile__("int $3\n");
113 }
114
gdbmach_find_hwbp(int type,unsigned long addr,size_t len)115 static struct hwbp *gdbmach_find_hwbp(int type, unsigned long addr, size_t len)
116 {
117 struct hwbp *available = NULL;
118 unsigned int i;
119 for (i = 0; i < sizeof hwbps / sizeof hwbps[0]; i++) {
120 if (hwbps[i].type == type && hwbps[i].addr == addr
121 && hwbps[i].len == len) {
122 return &hwbps[i];
123 }
124 if (!hwbps[i].enabled) {
125 available = &hwbps[i];
126 }
127 }
128 return available;
129 }
130
gdbmach_commit_hwbp(struct hwbp * bp)131 static void gdbmach_commit_hwbp(struct hwbp *bp)
132 {
133 int regnum = bp - hwbps;
134
135 /* Set breakpoint address */
136 switch (regnum) {
137 case 0:
138 __asm__ __volatile__("movl %0, %%dr0\n": :"r"(bp->addr));
139 break;
140 case 1:
141 __asm__ __volatile__("movl %0, %%dr1\n": :"r"(bp->addr));
142 break;
143 case 2:
144 __asm__ __volatile__("movl %0, %%dr2\n": :"r"(bp->addr));
145 break;
146 case 3:
147 __asm__ __volatile__("movl %0, %%dr3\n": :"r"(bp->addr));
148 break;
149 }
150
151 /* Set type */
152 dr7 &= ~(0x3 << (16 + 4 * regnum));
153 dr7 |= bp->type << (16 + 4 * regnum);
154
155 /* Set length */
156 dr7 &= ~(0x3 << (18 + 4 * regnum));
157 dr7 |= bp->len << (18 + 4 * regnum);
158
159 /* Set/clear local enable bit */
160 dr7 &= ~(0x3 << 2 * regnum);
161 dr7 |= bp->enabled << 2 * regnum;
162 }
163
gdbmach_set_breakpoint(int type,unsigned long addr,size_t len,int enable)164 int gdbmach_set_breakpoint(int type, unsigned long addr, size_t len, int enable)
165 {
166 struct hwbp *bp;
167
168 /* Check and convert breakpoint type to x86 type */
169 switch (type) {
170 case GDBMACH_WATCH:
171 type = 0x1;
172 break;
173 case GDBMACH_AWATCH:
174 type = 0x3;
175 break;
176 default:
177 return 0; /* unsupported breakpoint type */
178 }
179
180 /* Only lengths 1, 2, and 4 are supported */
181 if (len != 2 && len != 4) {
182 len = 1;
183 }
184 len--; /* convert to x86 breakpoint length bit pattern */
185
186 /* Set up the breakpoint */
187 bp = gdbmach_find_hwbp(type, addr, len);
188 if (!bp) {
189 return 0; /* ran out of hardware breakpoints */
190 }
191 bp->type = type;
192 bp->addr = addr;
193 bp->len = len;
194 bp->enabled = enable;
195 gdbmach_commit_hwbp(bp);
196 return 1;
197 }
198
gdbmach_disable_hwbps(void)199 static void gdbmach_disable_hwbps(void)
200 {
201 /* Store and clear hardware breakpoints */
202 __asm__ __volatile__("movl %0, %%dr7\n"::"r"(DR7_CLEAR));
203 }
204
gdbmach_enable_hwbps(void)205 static void gdbmach_enable_hwbps(void)
206 {
207 /* Clear breakpoint status register */
208 __asm__ __volatile__("movl %0, %%dr6\n"::"r"(DR6_CLEAR));
209
210 /* Restore hardware breakpoints */
211 __asm__ __volatile__("movl %0, %%dr7\n"::"r"(dr7));
212 }
213
214 /* Packet parser states */
215 static void gdbstub_state_new(struct gdbstub *stub, char ch);
216 static void gdbstub_state_data(struct gdbstub *stub, char ch);
217 static void gdbstub_state_cksum1(struct gdbstub *stub, char ch);
218 static void gdbstub_state_cksum2(struct gdbstub *stub, char ch);
219 static void gdbstub_state_wait_ack(struct gdbstub *stub, char ch);
220
serial_write(void * buf,size_t len)221 static void serial_write(void *buf, size_t len)
222 {
223 char *p = buf;
224 while (len-- > 0)
225 serial_putc(*p++);
226 }
227
gdbstub_from_hex_digit(char ch)228 static uint8_t gdbstub_from_hex_digit(char ch)
229 {
230 if (ch >= '0' && ch <= '9')
231 return ch - '0';
232 else if (ch >= 'A' && ch <= 'F')
233 return ch - 'A' + 0xa;
234 else
235 return (ch - 'a' + 0xa) & 0xf;
236 }
237
gdbstub_to_hex_digit(uint8_t b)238 static uint8_t gdbstub_to_hex_digit(uint8_t b)
239 {
240 b &= 0xf;
241 return (b < 0xa ? '0' : 'a' - 0xa) + b;
242 }
243
244 /*
245 * To make reading/writing device memory atomic, we check for
246 * 2- or 4-byte aligned operations and handle them specially.
247 */
248
gdbstub_from_hex_buf(char * dst,char * src,int lenbytes)249 static void gdbstub_from_hex_buf(char *dst, char *src, int lenbytes)
250 {
251 if (lenbytes == 2 && ((unsigned long)dst & 0x1) == 0) {
252 uint16_t i = gdbstub_from_hex_digit(src[2]) << 12 |
253 gdbstub_from_hex_digit(src[3]) << 8 |
254 gdbstub_from_hex_digit(src[0]) << 4 |
255 gdbstub_from_hex_digit(src[1]);
256 *(uint16_t *) dst = i;
257 } else if (lenbytes == 4 && ((unsigned long)dst & 0x3) == 0) {
258 uint32_t i = gdbstub_from_hex_digit(src[6]) << 28 |
259 gdbstub_from_hex_digit(src[7]) << 24 |
260 gdbstub_from_hex_digit(src[4]) << 20 |
261 gdbstub_from_hex_digit(src[5]) << 16 |
262 gdbstub_from_hex_digit(src[2]) << 12 |
263 gdbstub_from_hex_digit(src[3]) << 8 |
264 gdbstub_from_hex_digit(src[0]) << 4 |
265 gdbstub_from_hex_digit(src[1]);
266 *(uint32_t *) dst = i;
267 } else {
268 while (lenbytes-- > 0) {
269 *dst++ = gdbstub_from_hex_digit(src[0]) << 4 |
270 gdbstub_from_hex_digit(src[1]);
271 src += 2;
272 }
273 }
274 }
275
gdbstub_to_hex_buf(char * dst,char * src,int lenbytes)276 static void gdbstub_to_hex_buf(char *dst, char *src, int lenbytes)
277 {
278 if (lenbytes == 2 && ((unsigned long)src & 0x1) == 0) {
279 uint16_t i = *(uint16_t *) src;
280 dst[0] = gdbstub_to_hex_digit(i >> 4);
281 dst[1] = gdbstub_to_hex_digit(i);
282 dst[2] = gdbstub_to_hex_digit(i >> 12);
283 dst[3] = gdbstub_to_hex_digit(i >> 8);
284 } else if (lenbytes == 4 && ((unsigned long)src & 0x3) == 0) {
285 uint32_t i = *(uint32_t *) src;
286 dst[0] = gdbstub_to_hex_digit(i >> 4);
287 dst[1] = gdbstub_to_hex_digit(i);
288 dst[2] = gdbstub_to_hex_digit(i >> 12);
289 dst[3] = gdbstub_to_hex_digit(i >> 8);
290 dst[4] = gdbstub_to_hex_digit(i >> 20);
291 dst[5] = gdbstub_to_hex_digit(i >> 16);
292 dst[6] = gdbstub_to_hex_digit(i >> 28);
293 dst[7] = gdbstub_to_hex_digit(i >> 24);
294 } else {
295 while (lenbytes-- > 0) {
296 *dst++ = gdbstub_to_hex_digit(*src >> 4);
297 *dst++ = gdbstub_to_hex_digit(*src);
298 src++;
299 }
300 }
301 }
302
gdbstub_cksum(char * data,int len)303 static uint8_t gdbstub_cksum(char *data, int len)
304 {
305 uint8_t cksum = 0;
306 while (len-- > 0) {
307 cksum += (uint8_t) * data++;
308 }
309 return cksum;
310 }
311
gdbstub_tx_packet(struct gdbstub * stub)312 static void gdbstub_tx_packet(struct gdbstub *stub)
313 {
314 uint8_t cksum = gdbstub_cksum(stub->payload, stub->len);
315 stub->buf[0] = '$';
316 stub->buf[stub->len + 1] = '#';
317 stub->buf[stub->len + 2] = gdbstub_to_hex_digit(cksum >> 4);
318 stub->buf[stub->len + 3] = gdbstub_to_hex_digit(cksum);
319 serial_write(stub->buf, stub->len + 4);
320 stub->parse = gdbstub_state_wait_ack;
321 }
322
323 /* GDB commands */
gdbstub_send_ok(struct gdbstub * stub)324 static void gdbstub_send_ok(struct gdbstub *stub)
325 {
326 stub->payload[0] = 'O';
327 stub->payload[1] = 'K';
328 stub->len = 2;
329 gdbstub_tx_packet(stub);
330 }
331
gdbstub_send_num_packet(struct gdbstub * stub,char reply,int num)332 static void gdbstub_send_num_packet(struct gdbstub *stub, char reply, int num)
333 {
334 stub->payload[0] = reply;
335 stub->payload[1] = gdbstub_to_hex_digit((char)num >> 4);
336 stub->payload[2] = gdbstub_to_hex_digit((char)num);
337 stub->len = 3;
338 gdbstub_tx_packet(stub);
339 }
340
341 /* Format is arg1,arg2,...,argn:data where argn are hex integers and data is not an argument */
gdbstub_get_packet_args(struct gdbstub * stub,unsigned long * args,int nargs,int * stop_idx)342 static int gdbstub_get_packet_args(struct gdbstub *stub, unsigned long *args,
343 int nargs, int *stop_idx)
344 {
345 int i;
346 char ch = 0;
347 int argc = 0;
348 unsigned long val = 0;
349 for (i = 1; i < stub->len && argc < nargs; i++) {
350 ch = stub->payload[i];
351 if (ch == ':') {
352 break;
353 } else if (ch == ',') {
354 args[argc++] = val;
355 val = 0;
356 } else {
357 val = (val << 4) | gdbstub_from_hex_digit(ch);
358 }
359 }
360 if (stop_idx) {
361 *stop_idx = i;
362 }
363 if (argc < nargs) {
364 args[argc++] = val;
365 }
366 return ((i == stub->len || ch == ':') && argc == nargs);
367 }
368
gdbstub_send_errno(struct gdbstub * stub,int errno)369 static void gdbstub_send_errno(struct gdbstub *stub, int errno)
370 {
371 gdbstub_send_num_packet(stub, 'E', errno);
372 }
373
gdbstub_report_signal(struct gdbstub * stub)374 static void gdbstub_report_signal(struct gdbstub *stub)
375 {
376 gdbstub_send_num_packet(stub, 'S', stub->signo);
377 }
378
gdbstub_read_regs(struct gdbstub * stub)379 static void gdbstub_read_regs(struct gdbstub *stub)
380 {
381 gdbstub_to_hex_buf(stub->payload, (char *)stub->regs, GDBMACH_SIZEOF_REGS);
382 stub->len = GDBMACH_SIZEOF_REGS * 2;
383 gdbstub_tx_packet(stub);
384 }
385
gdbstub_write_regs(struct gdbstub * stub)386 static void gdbstub_write_regs(struct gdbstub *stub)
387 {
388 if (stub->len != 1 + GDBMACH_SIZEOF_REGS * 2) {
389 gdbstub_send_errno(stub, POSIX_EINVAL);
390 return;
391 }
392 gdbstub_from_hex_buf((char *)stub->regs, &stub->payload[1],
393 GDBMACH_SIZEOF_REGS);
394 gdbstub_send_ok(stub);
395 }
396
gdbstub_read_mem(struct gdbstub * stub)397 static void gdbstub_read_mem(struct gdbstub *stub)
398 {
399 unsigned long args[2];
400 if (!gdbstub_get_packet_args
401 (stub, args, sizeof args / sizeof args[0], NULL)) {
402 gdbstub_send_errno(stub, POSIX_EINVAL);
403 return;
404 }
405 args[1] = (args[1] < SIZEOF_PAYLOAD / 2) ? args[1] : SIZEOF_PAYLOAD / 2;
406 gdbstub_to_hex_buf(stub->payload, (char *)args[0], args[1]);
407 stub->len = args[1] * 2;
408 gdbstub_tx_packet(stub);
409 }
410
gdbstub_write_mem(struct gdbstub * stub)411 static void gdbstub_write_mem(struct gdbstub *stub)
412 {
413 unsigned long args[2];
414 int colon;
415 if (!gdbstub_get_packet_args
416 (stub, args, sizeof args / sizeof args[0], &colon) || colon >= stub->len
417 || stub->payload[colon] != ':' || (stub->len - colon - 1) % 2 != 0) {
418 gdbstub_send_errno(stub, POSIX_EINVAL);
419 return;
420 }
421 gdbstub_from_hex_buf((char *)args[0], &stub->payload[colon + 1],
422 (stub->len - colon - 1) / 2);
423 gdbstub_send_ok(stub);
424 }
425
gdbstub_continue(struct gdbstub * stub,int single_step)426 static void gdbstub_continue(struct gdbstub *stub, int single_step)
427 {
428 gdbreg_t pc;
429 if (stub->len > 1
430 && gdbstub_get_packet_args(stub, (unsigned long *)&pc, 1, NULL)) {
431 gdbmach_set_pc(stub->regs, pc);
432 }
433 gdbmach_set_single_step(stub->regs, single_step);
434 stub->exit_handler = 1;
435 /* Reply will be sent when we hit the next breakpoint or interrupt */
436 }
437
gdbstub_breakpoint(struct gdbstub * stub)438 static void gdbstub_breakpoint(struct gdbstub *stub)
439 {
440 unsigned long args[3];
441 int enable = stub->payload[0] == 'Z' ? 1 : 0;
442 if (!gdbstub_get_packet_args
443 (stub, args, sizeof args / sizeof args[0], NULL)) {
444 gdbstub_send_errno(stub, POSIX_EINVAL);
445 return;
446 }
447 if (gdbmach_set_breakpoint(args[0], args[1], args[2], enable)) {
448 gdbstub_send_ok(stub);
449 } else {
450 /* Not supported */
451 stub->len = 0;
452 gdbstub_tx_packet(stub);
453 }
454 }
455
gdbstub_rx_packet(struct gdbstub * stub)456 static void gdbstub_rx_packet(struct gdbstub *stub)
457 {
458 switch (stub->payload[0]) {
459 case '?':
460 gdbstub_report_signal(stub);
461 break;
462 case 'g':
463 gdbstub_read_regs(stub);
464 break;
465 case 'G':
466 gdbstub_write_regs(stub);
467 break;
468 case 'm':
469 gdbstub_read_mem(stub);
470 break;
471 case 'M':
472 gdbstub_write_mem(stub);
473 break;
474 case 'c': /* Continue */
475 case 'k': /* Kill */
476 case 's': /* Step */
477 case 'D': /* Detach */
478 gdbstub_continue(stub, stub->payload[0] == 's');
479 if (stub->payload[0] == 'D') {
480 gdbstub_send_ok(stub);
481 }
482 break;
483 case 'Z': /* Insert breakpoint */
484 case 'z': /* Remove breakpoint */
485 gdbstub_breakpoint(stub);
486 break;
487 default:
488 stub->len = 0;
489 gdbstub_tx_packet(stub);
490 break;
491 }
492 }
493
494 /* GDB packet parser */
gdbstub_state_new(struct gdbstub * stub,char ch)495 static void gdbstub_state_new(struct gdbstub *stub, char ch)
496 {
497 if (ch == '$') {
498 stub->len = 0;
499 stub->parse = gdbstub_state_data;
500 }
501 }
502
gdbstub_state_data(struct gdbstub * stub,char ch)503 static void gdbstub_state_data(struct gdbstub *stub, char ch)
504 {
505 if (ch == '#') {
506 stub->parse = gdbstub_state_cksum1;
507 } else if (ch == '$') {
508 stub->len = 0; /* retry new packet */
509 } else {
510 /* If the length exceeds our buffer, let the checksum fail */
511 if (stub->len < SIZEOF_PAYLOAD) {
512 stub->payload[stub->len++] = ch;
513 }
514 }
515 }
516
gdbstub_state_cksum1(struct gdbstub * stub,char ch)517 static void gdbstub_state_cksum1(struct gdbstub *stub, char ch)
518 {
519 stub->cksum1 = gdbstub_from_hex_digit(ch) << 4;
520 stub->parse = gdbstub_state_cksum2;
521 }
522
gdbstub_state_cksum2(struct gdbstub * stub,char ch)523 static void gdbstub_state_cksum2(struct gdbstub *stub, char ch)
524 {
525 uint8_t their_cksum;
526 uint8_t our_cksum;
527
528 stub->parse = gdbstub_state_new;
529 their_cksum = stub->cksum1 + gdbstub_from_hex_digit(ch);
530 our_cksum = gdbstub_cksum(stub->payload, stub->len);
531
532 if (their_cksum == our_cksum) {
533 serial_write("+", 1);
534 if (stub->len > 0) {
535 gdbstub_rx_packet(stub);
536 }
537 } else {
538 serial_write("-", 1);
539 }
540 }
541
gdbstub_state_wait_ack(struct gdbstub * stub,char ch)542 static void gdbstub_state_wait_ack(struct gdbstub *stub, char ch)
543 {
544 if (ch == '+') {
545 stub->parse = gdbstub_state_new;
546 } else {
547 /* This retransmit is very aggressive but necessary to keep
548 * in sync with GDB. */
549 gdbstub_tx_packet(stub);
550 }
551 }
552
gdbstub_handler(int signo,gdbreg_t * regs)553 void gdbstub_handler(int signo, gdbreg_t * regs)
554 {
555 struct gdbstub stub;
556
557 gdbmach_disable_hwbps();
558
559 stub.parse = gdbstub_state_new;
560 stub.payload = &stub.buf[1];
561 stub.signo = signo;
562 stub.regs = regs;
563 stub.exit_handler = 0;
564 gdbstub_report_signal(&stub);
565 while (!stub.exit_handler)
566 stub.parse(&stub, serial_getc());
567
568 gdbmach_enable_hwbps();
569 }
570