[GDB] Add watch and rwatch hardware watchpoints
[people/andreif/gpxe.git] / src / core / gdbstub.c
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 <stdio.h>
28 #include <string.h>
29 #include <ctype.h>
30 #include <byteswap.h>
31 #include <gpxe/gdbstub.h>
32 #include "gdbmach.h"
33
34 enum {
35         POSIX_EINVAL = 0x1c,  /* used to report bad arguments to GDB */
36         SIZEOF_PAYLOAD = 256, /* buffer size of GDB payload data */
37 };
38
39 struct gdbstub {
40         struct gdb_transport *trans;
41         int exit_handler; /* leave interrupt handler */
42
43         int signo;
44         gdbreg_t *regs;
45
46         void ( * parse ) ( struct gdbstub *stub, char ch );
47         uint8_t cksum1;
48
49         /* Buffer for payload data when parsing a packet.  Once the
50          * packet has been received, this buffer is used to hold
51          * the reply payload. */
52         char buf [ SIZEOF_PAYLOAD + 4 ]; /* $...PAYLOAD...#XX */
53         char *payload;                   /* start of payload */
54         int len;                         /* length of payload */
55 };
56
57 /* Transports */
58 static struct gdb_transport gdb_transport_start[0] __table_start ( struct gdb_transport, gdb_transports );
59 static struct gdb_transport gdb_transport_end[0] __table_end ( struct gdb_transport, gdb_transports );
60
61 /* Packet parser states */
62 static void gdbstub_state_new ( struct gdbstub *stub, char ch );
63 static void gdbstub_state_data ( struct gdbstub *stub, char ch );
64 static void gdbstub_state_cksum1 ( struct gdbstub *stub, char ch );
65 static void gdbstub_state_cksum2 ( struct gdbstub *stub, char ch );
66 static void gdbstub_state_wait_ack ( struct gdbstub *stub, char ch );
67
68 static uint8_t gdbstub_from_hex_digit ( char ch ) {
69         return ( isdigit ( ch ) ? ch - '0' : tolower ( ch ) - 'a' + 0xa ) & 0xf;
70 }
71
72 static uint8_t gdbstub_to_hex_digit ( uint8_t b ) {
73         b &= 0xf;
74         return ( b < 0xa ? '0' : 'a' - 0xa ) + b;
75 }
76
77 /*
78  * To make reading/writing device memory atomic, we check for
79  * 2- or 4-byte aligned operations and handle them specially.
80  */
81
82 static void gdbstub_from_hex_buf ( char *dst, char *src, int lenbytes ) {
83         if ( lenbytes == 2 && ( ( unsigned long ) dst & 0x1 ) == 0 ) {
84                 uint16_t i = gdbstub_from_hex_digit ( src [ 2 ] ) << 12 |
85                         gdbstub_from_hex_digit ( src [ 3 ] ) << 8 |
86                         gdbstub_from_hex_digit ( src [ 0 ] ) << 4 |
87                         gdbstub_from_hex_digit ( src [ 1 ] );
88                 * ( uint16_t * ) dst = cpu_to_le16 ( i );
89         } else if ( lenbytes == 4 && ( ( unsigned long ) dst & 0x3 ) == 0 ) {
90                 uint32_t i = gdbstub_from_hex_digit ( src [ 6 ] ) << 28 |
91                         gdbstub_from_hex_digit ( src [ 7 ] ) << 24 |
92                         gdbstub_from_hex_digit ( src [ 4 ] ) << 20 |
93                         gdbstub_from_hex_digit ( src [ 5 ] ) << 16 |
94                         gdbstub_from_hex_digit ( src [ 2 ] ) << 12 |
95                         gdbstub_from_hex_digit ( src [ 3 ] ) << 8 |
96                         gdbstub_from_hex_digit ( src [ 0 ] ) << 4 |
97                         gdbstub_from_hex_digit ( src [ 1 ] );
98                 * ( uint32_t * ) dst = cpu_to_le32 ( i );
99         } else {
100                 while ( lenbytes-- > 0 ) {
101                         *dst++ = gdbstub_from_hex_digit ( src [ 0 ] ) << 4 |
102                                 gdbstub_from_hex_digit ( src [ 1 ] );
103                         src += 2;
104                 }
105         }
106 }
107
108 static void gdbstub_to_hex_buf ( char *dst, char *src, int lenbytes ) {
109         if ( lenbytes == 2 && ( ( unsigned long ) src & 0x1 ) == 0 ) {
110                 uint16_t i = cpu_to_le16 ( * ( uint16_t * ) src );
111                 dst [ 0 ] = gdbstub_to_hex_digit ( i >> 4 );
112                 dst [ 1 ] = gdbstub_to_hex_digit ( i );
113                 dst [ 2 ] = gdbstub_to_hex_digit ( i >> 12 );
114                 dst [ 3 ] = gdbstub_to_hex_digit ( i >> 8 );
115         } else if ( lenbytes == 4 && ( ( unsigned long ) src & 0x3 ) == 0 ) {
116                 uint32_t i = cpu_to_le32 ( * ( uint32_t * ) src );
117                 dst [ 0 ] = gdbstub_to_hex_digit ( i >> 4 );
118                 dst [ 1 ] = gdbstub_to_hex_digit ( i );
119                 dst [ 2 ] = gdbstub_to_hex_digit ( i >> 12 );
120                 dst [ 3 ] = gdbstub_to_hex_digit ( i >> 8 );
121                 dst [ 4 ] = gdbstub_to_hex_digit ( i >> 20 );
122                 dst [ 5 ] = gdbstub_to_hex_digit ( i >> 16);
123                 dst [ 6 ] = gdbstub_to_hex_digit ( i >> 28 );
124                 dst [ 7 ] = gdbstub_to_hex_digit ( i >> 24 );
125         } else {
126                 while ( lenbytes-- > 0 ) {
127                         *dst++ = gdbstub_to_hex_digit ( *src >> 4 );
128                         *dst++ = gdbstub_to_hex_digit ( *src );
129                         src++;
130                 }
131         }
132 }
133
134 static uint8_t gdbstub_cksum ( char *data, int len ) {
135         uint8_t cksum = 0;
136         while ( len-- > 0 ) {
137                 cksum += ( uint8_t ) *data++;
138         }
139         return cksum;
140 }
141
142 static void gdbstub_tx_packet ( struct gdbstub *stub ) {
143         uint8_t cksum = gdbstub_cksum ( stub->payload, stub->len );
144         stub->buf [ 0 ] = '$';
145         stub->buf [ stub->len + 1 ] = '#';
146         stub->buf [ stub->len + 2 ] = gdbstub_to_hex_digit ( cksum >> 4 );
147         stub->buf [ stub->len + 3 ] = gdbstub_to_hex_digit ( cksum );
148         stub->trans->send ( stub->buf, stub->len + 4 );
149         stub->parse = gdbstub_state_wait_ack;
150 }
151
152 /* GDB commands */
153 static void gdbstub_send_ok ( struct gdbstub *stub ) {
154         stub->payload [ 0 ] = 'O';
155         stub->payload [ 1 ] = 'K';
156         stub->len = 2;
157         gdbstub_tx_packet ( stub );
158 }
159
160 static void gdbstub_send_num_packet ( struct gdbstub *stub, char reply, int num ) {
161         stub->payload [ 0 ] = reply;
162         stub->payload [ 1 ] = gdbstub_to_hex_digit ( ( char ) num >> 4 );
163         stub->payload [ 2 ] = gdbstub_to_hex_digit ( ( char ) num );
164         stub->len = 3;
165         gdbstub_tx_packet ( stub );
166 }
167
168 /* Format is arg1,arg2,...,argn:data where argn are hex integers and data is not an argument */
169 static int gdbstub_get_packet_args ( struct gdbstub *stub, unsigned long *args, int nargs, int *stop_idx ) {
170         int i;
171         char ch = 0;
172         int argc = 0;
173         unsigned long val = 0;
174         for ( i = 1; i < stub->len && argc < nargs; i++ ) {
175                 ch = stub->payload [ i ];
176                 if ( ch == ':' ) {
177                         break;
178                 } else if ( ch == ',' ) {
179                         args [ argc++ ] = val;
180                         val = 0;
181                 } else {
182                         val = ( val << 4 ) | gdbstub_from_hex_digit ( ch );
183                 }
184         }
185         if ( stop_idx ) {
186                 *stop_idx = i;
187         }
188         if ( argc < nargs ) {
189                 args [ argc++ ] = val;
190         }
191         return ( ( i == stub->len || ch == ':' ) && argc == nargs );
192 }
193
194 static void gdbstub_send_errno ( struct gdbstub *stub, int errno ) {
195         gdbstub_send_num_packet ( stub, 'E', errno );
196 }
197
198 static void gdbstub_report_signal ( struct gdbstub *stub ) {
199         gdbstub_send_num_packet ( stub, 'S', stub->signo );
200 }
201
202 static void gdbstub_read_regs ( struct gdbstub *stub ) {
203         gdbstub_to_hex_buf ( stub->payload, ( char * ) stub->regs, GDBMACH_SIZEOF_REGS );
204         stub->len = GDBMACH_SIZEOF_REGS * 2;
205         gdbstub_tx_packet ( stub );
206 }
207
208 static void gdbstub_write_regs ( struct gdbstub *stub ) {
209         if ( stub->len != 1 + GDBMACH_SIZEOF_REGS * 2 ) {
210                 gdbstub_send_errno ( stub, POSIX_EINVAL );
211                 return;
212         }
213         gdbstub_from_hex_buf ( ( char * ) stub->regs, &stub->payload [ 1 ], GDBMACH_SIZEOF_REGS );
214         gdbstub_send_ok ( stub );
215 }
216
217 static void gdbstub_read_mem ( struct gdbstub *stub ) {
218         unsigned long args [ 2 ];
219         if ( !gdbstub_get_packet_args ( stub, args, sizeof args / sizeof args [ 0 ], NULL ) ) {
220                 gdbstub_send_errno ( stub, POSIX_EINVAL );
221                 return;
222         }
223         args [ 1 ] = ( args [ 1 ] < SIZEOF_PAYLOAD / 2 ) ? args [ 1 ] : SIZEOF_PAYLOAD / 2;
224         gdbstub_to_hex_buf ( stub->payload, ( char * ) args [ 0 ], args [ 1 ] );
225         stub->len = args [ 1 ] * 2;
226         gdbstub_tx_packet ( stub );
227 }
228
229 static void gdbstub_write_mem ( struct gdbstub *stub ) {
230         unsigned long args [ 2 ];
231         int colon;
232         if ( !gdbstub_get_packet_args ( stub, args, sizeof args / sizeof args [ 0 ], &colon ) ||
233                         colon >= stub->len || stub->payload [ colon ] != ':' ||
234                         ( stub->len - colon - 1 ) % 2 != 0 ) {
235                 gdbstub_send_errno ( stub, POSIX_EINVAL );
236                 return;
237         }
238         gdbstub_from_hex_buf ( ( char * ) args [ 0 ], &stub->payload [ colon + 1 ], ( stub->len - colon - 1 ) / 2 );
239         gdbstub_send_ok ( stub );
240 }
241
242 static void gdbstub_continue ( struct gdbstub *stub, int single_step ) {
243         gdbreg_t pc;
244         if ( stub->len > 1 && gdbstub_get_packet_args ( stub, &pc, 1, NULL ) ) {
245                 gdbmach_set_pc ( stub->regs, pc );
246         }
247         gdbmach_set_single_step ( stub->regs, single_step );
248         stub->exit_handler = 1;
249         /* Reply will be sent when we hit the next breakpoint or interrupt */
250 }
251
252 static void gdbstub_breakpoint ( struct gdbstub *stub ) {
253         unsigned long args [ 3 ];
254         int enable = stub->payload [ 0 ] == 'Z' ? 1 : 0;
255         if ( !gdbstub_get_packet_args ( stub, args, sizeof args / sizeof args [ 0 ], NULL ) ) {
256                 gdbstub_send_errno ( stub, POSIX_EINVAL );
257                 return;
258         }
259         if ( gdbmach_set_breakpoint ( args [ 0 ], args [ 1 ], args [ 2 ], enable ) ) {
260                 gdbstub_send_ok ( stub );
261         } else {
262                 /* Not supported */
263                 stub->len = 0;
264                 gdbstub_tx_packet ( stub );
265         }
266 }
267
268 static void gdbstub_rx_packet ( struct gdbstub *stub ) {
269         switch ( stub->payload [ 0 ] ) {
270                 case '?':
271                         gdbstub_report_signal ( stub );
272                         break;
273                 case 'g':
274                         gdbstub_read_regs ( stub );
275                         break;
276                 case 'G':
277                         gdbstub_write_regs ( stub );
278                         break;
279                 case 'm':
280                         gdbstub_read_mem ( stub );
281                         break;
282                 case 'M':
283                         gdbstub_write_mem ( stub );
284                         break;
285                 case 'c': /* Continue */
286                 case 'k': /* Kill */
287                 case 's': /* Step */
288                 case 'D': /* Detach */
289                         gdbstub_continue ( stub, stub->payload [ 0 ] == 's' );
290                         if ( stub->payload [ 0 ] == 'D' ) {
291                                 gdbstub_send_ok ( stub );
292                         }
293                         break;
294                 case 'Z': /* Insert breakpoint */
295                 case 'z': /* Remove breakpoint */
296                         gdbstub_breakpoint ( stub );
297                         break;
298                 default:
299                         stub->len = 0;
300                         gdbstub_tx_packet ( stub );
301                         break;
302         }
303 }
304
305 /* GDB packet parser */
306 static void gdbstub_state_new ( struct gdbstub *stub, char ch ) {
307         if ( ch == '$' ) {
308                 stub->len = 0;
309                 stub->parse = gdbstub_state_data;
310         }
311 }
312
313 static void gdbstub_state_data ( struct gdbstub *stub, char ch ) {
314         if ( ch == '#' ) {
315                 stub->parse = gdbstub_state_cksum1;
316         } else if ( ch == '$' ) {
317                 stub->len = 0; /* retry new packet */
318         } else {
319                 /* If the length exceeds our buffer, let the checksum fail */
320                 if ( stub->len < SIZEOF_PAYLOAD ) {
321                         stub->payload [ stub->len++ ] = ch;
322                 }
323         }
324 }
325
326 static void gdbstub_state_cksum1 ( struct gdbstub *stub, char ch ) {
327         stub->cksum1 = gdbstub_from_hex_digit ( ch ) << 4;
328         stub->parse = gdbstub_state_cksum2;
329 }
330
331 static void gdbstub_state_cksum2 ( struct gdbstub *stub, char ch ) {
332         uint8_t their_cksum;
333         uint8_t our_cksum;
334
335         stub->parse = gdbstub_state_new;
336         their_cksum = stub->cksum1 + gdbstub_from_hex_digit ( ch );
337         our_cksum = gdbstub_cksum ( stub->payload, stub->len );
338         if ( their_cksum == our_cksum ) {
339                 stub->trans->send ( "+", 1 );
340                 if ( stub->len > 0 ) {
341                         gdbstub_rx_packet ( stub );
342                 }
343         } else {
344                 stub->trans->send ( "-", 1 );
345         }
346 }
347
348 static void gdbstub_state_wait_ack ( struct gdbstub *stub, char ch ) {
349         if ( ch == '+' ) {
350                 stub->parse = gdbstub_state_new;
351         } else if ( ch == '-' ) {
352                 gdbstub_tx_packet ( stub ); /* retransmit */
353         }
354 }
355
356 static void gdbstub_parse ( struct gdbstub *stub, char ch ) {
357         stub->parse ( stub, ch );
358 }
359
360 static struct gdbstub stub = {
361         .parse = gdbstub_state_new
362 };
363
364 void gdbstub_handler ( int signo, gdbreg_t *regs ) {
365         char packet [ SIZEOF_PAYLOAD + 4 ];
366         size_t len, i;
367
368         /* A transport must be set up */
369         if ( !stub.trans ) {
370                 return;
371         }
372
373         stub.signo = signo;
374         stub.regs = regs;
375         stub.exit_handler = 0;
376         gdbstub_report_signal ( &stub );
377         while ( !stub.exit_handler && ( len = stub.trans->recv ( packet, sizeof ( packet ) ) ) > 0 ) {
378                 for ( i = 0; i < len; i++ ) {
379                         gdbstub_parse ( &stub, packet [ i ] );
380                 }
381         }
382 }
383
384 struct gdb_transport *find_gdb_transport ( const char *name ) {
385         struct gdb_transport *trans;
386         for ( trans = gdb_transport_start; trans < gdb_transport_end; trans++ ) {
387                 if ( strcmp ( trans->name, name ) == 0 ) {
388                         return trans;
389                 }
390         }
391         return NULL;
392 }
393
394 void gdbstub_start ( struct gdb_transport *trans ) {
395         stub.trans = trans;
396         stub.payload = &stub.buf [ 1 ];
397         gdbmach_breakpoint();
398 }