source: bootcd/isolinux/syslinux-6.03/com32/gdbstub/gdbstub.c

Last change on this file was e16e8f2, checked in by Edwin Eefting <edwin@datux.nl>, 3 years ago

bootstuff
  • Property mode set to 100644
File size: 14.3 KB
Line 
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
30typedef uint32_t gdbreg_t;
31
32enum {
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. */
41enum {
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 */
63enum {
64    GDBMACH_BPMEM,
65    GDBMACH_BPHW,
66    GDBMACH_WATCH,
67    GDBMACH_RWATCH,
68    GDBMACH_AWATCH,
69};
70
71struct 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 */
89struct 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
96static struct hwbp hwbps[4];
97static gdbreg_t dr7 = DR7_CLEAR;
98
99static inline void gdbmach_set_pc(gdbreg_t * regs, gdbreg_t pc)
100{
101    regs[GDBMACH_EIP] = pc;
102}
103
104static 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
110static inline void gdbmach_breakpoint(void)
111{
112    __asm__ __volatile__("int $3\n");
113}
114
115static 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
131static 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
164int 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
199static void gdbmach_disable_hwbps(void)
200{
201    /* Store and clear hardware breakpoints */
202    __asm__ __volatile__("movl %0, %%dr7\n"::"r"(DR7_CLEAR));
203}
204
205static 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 */
215static void gdbstub_state_new(struct gdbstub *stub, char ch);
216static void gdbstub_state_data(struct gdbstub *stub, char ch);
217static void gdbstub_state_cksum1(struct gdbstub *stub, char ch);
218static void gdbstub_state_cksum2(struct gdbstub *stub, char ch);
219static void gdbstub_state_wait_ack(struct gdbstub *stub, char ch);
220
221static void serial_write(void *buf, size_t len)
222{
223    char *p = buf;
224    while (len-- > 0)
225        serial_putc(*p++);
226}
227
228static 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
238static 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
249static 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
276static 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
303static 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
312static 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 */
324static 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
332static 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 */
342static 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
369static void gdbstub_send_errno(struct gdbstub *stub, int errno)
370{
371    gdbstub_send_num_packet(stub, 'E', errno);
372}
373
374static void gdbstub_report_signal(struct gdbstub *stub)
375{
376    gdbstub_send_num_packet(stub, 'S', stub->signo);
377}
378
379static 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
386static 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
397static 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
411static 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
426static 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
438static 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
456static 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 */
495static 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
503static 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
517static 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
523static 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
542static 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
553void 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}
Note: See TracBrowser for help on using the repository browser.