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