iPXE
gdbstub.c
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2008 Stefan Hajnoczi <stefanha@gmail.com>.
00003  *
00004  * This program is free software; you can redistribute it and/or
00005  * modify it under the terms of the GNU General Public License as
00006  * published by the Free Software Foundation; either version 2 of the
00007  * License, or any later version.
00008  *
00009  * This program is distributed in the hope that it will be useful, but
00010  * WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00012  * General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU General Public License
00015  * along with this program; if not, write to the Free Software
00016  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
00017  * 02110-1301, USA.
00018  *
00019  * You can also choose to distribute this program under the terms of
00020  * the Unmodified Binary Distribution Licence (as given in the file
00021  * COPYING.UBDL), provided that you have satisfied its requirements.
00022  */
00023 
00024 FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
00025 
00026 /**
00027  * @file
00028  *
00029  * GDB stub for remote debugging
00030  *
00031  */
00032 
00033 #include <stdlib.h>
00034 #include <stdio.h>
00035 #include <string.h>
00036 #include <ctype.h>
00037 #include <byteswap.h>
00038 #include <ipxe/gdbstub.h>
00039 #include "gdbmach.h"
00040 
00041 enum {
00042         POSIX_EINVAL = 0x1c,  /* used to report bad arguments to GDB */
00043         SIZEOF_PAYLOAD = 512, /* buffer size of GDB payload data */
00044 };
00045 
00046 struct gdbstub {
00047         struct gdb_transport *trans;
00048         int exit_handler; /* leave interrupt handler */
00049 
00050         int signo;
00051         gdbreg_t *regs;
00052 
00053         void ( * parse ) ( struct gdbstub *stub, char ch );
00054         uint8_t cksum1;
00055 
00056         /* Buffer for payload data when parsing a packet.  Once the
00057          * packet has been received, this buffer is used to hold
00058          * the reply payload. */
00059         char buf [ SIZEOF_PAYLOAD + 4 ]; /* $...PAYLOAD...#XX */
00060         char *payload;                   /* start of payload */
00061         int len;                         /* length of payload */
00062 };
00063 
00064 /* Packet parser states */
00065 static void gdbstub_state_new ( struct gdbstub *stub, char ch );
00066 static void gdbstub_state_data ( struct gdbstub *stub, char ch );
00067 static void gdbstub_state_cksum1 ( struct gdbstub *stub, char ch );
00068 static void gdbstub_state_cksum2 ( struct gdbstub *stub, char ch );
00069 static void gdbstub_state_wait_ack ( struct gdbstub *stub, char ch );
00070 
00071 static uint8_t gdbstub_from_hex_digit ( char ch ) {
00072         return ( isdigit ( ch ) ? ch - '0' : tolower ( ch ) - 'a' + 0xa ) & 0xf;
00073 }
00074 
00075 static uint8_t gdbstub_to_hex_digit ( uint8_t b ) {
00076         b &= 0xf;
00077         return ( b < 0xa ? '0' : 'a' - 0xa ) + b;
00078 }
00079 
00080 /*
00081  * To make reading/writing device memory atomic, we check for
00082  * 2- or 4-byte aligned operations and handle them specially.
00083  */
00084 
00085 static void gdbstub_from_hex_buf ( char *dst, char *src, int lenbytes ) {
00086         if ( lenbytes == 2 && ( ( unsigned long ) dst & 0x1 ) == 0 ) {
00087                 uint16_t i = gdbstub_from_hex_digit ( src [ 2 ] ) << 12 |
00088                         gdbstub_from_hex_digit ( src [ 3 ] ) << 8 |
00089                         gdbstub_from_hex_digit ( src [ 0 ] ) << 4 |
00090                         gdbstub_from_hex_digit ( src [ 1 ] );
00091                 * ( uint16_t * ) dst = cpu_to_le16 ( i );
00092         } else if ( lenbytes == 4 && ( ( unsigned long ) dst & 0x3 ) == 0 ) {
00093                 uint32_t i = gdbstub_from_hex_digit ( src [ 6 ] ) << 28 |
00094                         gdbstub_from_hex_digit ( src [ 7 ] ) << 24 |
00095                         gdbstub_from_hex_digit ( src [ 4 ] ) << 20 |
00096                         gdbstub_from_hex_digit ( src [ 5 ] ) << 16 |
00097                         gdbstub_from_hex_digit ( src [ 2 ] ) << 12 |
00098                         gdbstub_from_hex_digit ( src [ 3 ] ) << 8 |
00099                         gdbstub_from_hex_digit ( src [ 0 ] ) << 4 |
00100                         gdbstub_from_hex_digit ( src [ 1 ] );
00101                 * ( uint32_t * ) dst = cpu_to_le32 ( i );
00102         } else {
00103                 while ( lenbytes-- > 0 ) {
00104                         *dst++ = gdbstub_from_hex_digit ( src [ 0 ] ) << 4 |
00105                                 gdbstub_from_hex_digit ( src [ 1 ] );
00106                         src += 2;
00107                 }
00108         }
00109 }
00110 
00111 static void gdbstub_to_hex_buf ( char *dst, char *src, int lenbytes ) {
00112         if ( lenbytes == 2 && ( ( unsigned long ) src & 0x1 ) == 0 ) {
00113                 uint16_t i = cpu_to_le16 ( * ( uint16_t * ) src );
00114                 dst [ 0 ] = gdbstub_to_hex_digit ( i >> 4 );
00115                 dst [ 1 ] = gdbstub_to_hex_digit ( i );
00116                 dst [ 2 ] = gdbstub_to_hex_digit ( i >> 12 );
00117                 dst [ 3 ] = gdbstub_to_hex_digit ( i >> 8 );
00118         } else if ( lenbytes == 4 && ( ( unsigned long ) src & 0x3 ) == 0 ) {
00119                 uint32_t i = cpu_to_le32 ( * ( uint32_t * ) src );
00120                 dst [ 0 ] = gdbstub_to_hex_digit ( i >> 4 );
00121                 dst [ 1 ] = gdbstub_to_hex_digit ( i );
00122                 dst [ 2 ] = gdbstub_to_hex_digit ( i >> 12 );
00123                 dst [ 3 ] = gdbstub_to_hex_digit ( i >> 8 );
00124                 dst [ 4 ] = gdbstub_to_hex_digit ( i >> 20 );
00125                 dst [ 5 ] = gdbstub_to_hex_digit ( i >> 16);
00126                 dst [ 6 ] = gdbstub_to_hex_digit ( i >> 28 );
00127                 dst [ 7 ] = gdbstub_to_hex_digit ( i >> 24 );
00128         } else {
00129                 while ( lenbytes-- > 0 ) {
00130                         *dst++ = gdbstub_to_hex_digit ( *src >> 4 );
00131                         *dst++ = gdbstub_to_hex_digit ( *src );
00132                         src++;
00133                 }
00134         }
00135 }
00136 
00137 static uint8_t gdbstub_cksum ( char *data, int len ) {
00138         uint8_t cksum = 0;
00139         while ( len-- > 0 ) {
00140                 cksum += ( uint8_t ) *data++;
00141         }
00142         return cksum;
00143 }
00144 
00145 static void gdbstub_tx_packet ( struct gdbstub *stub ) {
00146         uint8_t cksum = gdbstub_cksum ( stub->payload, stub->len );
00147         stub->buf [ 0 ] = '$';
00148         stub->buf [ stub->len + 1 ] = '#';
00149         stub->buf [ stub->len + 2 ] = gdbstub_to_hex_digit ( cksum >> 4 );
00150         stub->buf [ stub->len + 3 ] = gdbstub_to_hex_digit ( cksum );
00151         stub->trans->send ( stub->buf, stub->len + 4 );
00152         stub->parse = gdbstub_state_wait_ack;
00153 }
00154 
00155 /* GDB commands */
00156 static void gdbstub_send_ok ( struct gdbstub *stub ) {
00157         stub->payload [ 0 ] = 'O';
00158         stub->payload [ 1 ] = 'K';
00159         stub->len = 2;
00160         gdbstub_tx_packet ( stub );
00161 }
00162 
00163 static void gdbstub_send_num_packet ( struct gdbstub *stub, char reply, int num ) {
00164         stub->payload [ 0 ] = reply;
00165         stub->payload [ 1 ] = gdbstub_to_hex_digit ( ( char ) num >> 4 );
00166         stub->payload [ 2 ] = gdbstub_to_hex_digit ( ( char ) num );
00167         stub->len = 3;
00168         gdbstub_tx_packet ( stub );
00169 }
00170 
00171 /* Format is arg1,arg2,...,argn:data where argn are hex integers and data is not an argument */
00172 static int gdbstub_get_packet_args ( struct gdbstub *stub, unsigned long *args, int nargs, int *stop_idx ) {
00173         int i;
00174         char ch = 0;
00175         int argc = 0;
00176         unsigned long val = 0;
00177         for ( i = 1; i < stub->len && argc < nargs; i++ ) {
00178                 ch = stub->payload [ i ];
00179                 if ( ch == ':' ) {
00180                         break;
00181                 } else if ( ch == ',' ) {
00182                         args [ argc++ ] = val;
00183                         val = 0;
00184                 } else {
00185                         val = ( val << 4 ) | gdbstub_from_hex_digit ( ch );
00186                 }
00187         }
00188         if ( stop_idx ) {
00189                 *stop_idx = i;
00190         }
00191         if ( argc < nargs ) {
00192                 args [ argc++ ] = val;
00193         }
00194         return ( ( i == stub->len || ch == ':' ) && argc == nargs );
00195 }
00196 
00197 static void gdbstub_send_errno ( struct gdbstub *stub, int errno ) {
00198         gdbstub_send_num_packet ( stub, 'E', errno );
00199 }
00200 
00201 static void gdbstub_report_signal ( struct gdbstub *stub ) {
00202         gdbstub_send_num_packet ( stub, 'S', stub->signo );
00203 }
00204 
00205 static void gdbstub_read_regs ( struct gdbstub *stub ) {
00206         gdbstub_to_hex_buf ( stub->payload, ( char * ) stub->regs, GDBMACH_SIZEOF_REGS );
00207         stub->len = GDBMACH_SIZEOF_REGS * 2;
00208         gdbstub_tx_packet ( stub );
00209 }
00210 
00211 static void gdbstub_write_regs ( struct gdbstub *stub ) {
00212         if ( stub->len != 1 + GDBMACH_SIZEOF_REGS * 2 ) {
00213                 gdbstub_send_errno ( stub, POSIX_EINVAL );
00214                 return;
00215         }
00216         gdbstub_from_hex_buf ( ( char * ) stub->regs, &stub->payload [ 1 ], GDBMACH_SIZEOF_REGS );
00217         gdbstub_send_ok ( stub );
00218 }
00219 
00220 static void gdbstub_read_mem ( struct gdbstub *stub ) {
00221         unsigned long args [ 2 ];
00222         if ( !gdbstub_get_packet_args ( stub, args, sizeof args / sizeof args [ 0 ], NULL ) ) {
00223                 gdbstub_send_errno ( stub, POSIX_EINVAL );
00224                 return;
00225         }
00226         args [ 1 ] = ( args [ 1 ] < SIZEOF_PAYLOAD / 2 ) ? args [ 1 ] : SIZEOF_PAYLOAD / 2;
00227         gdbstub_to_hex_buf ( stub->payload, ( char * ) args [ 0 ], args [ 1 ] );
00228         stub->len = args [ 1 ] * 2;
00229         gdbstub_tx_packet ( stub );
00230 }
00231 
00232 static void gdbstub_write_mem ( struct gdbstub *stub ) {
00233         unsigned long args [ 2 ];
00234         int colon;
00235         if ( !gdbstub_get_packet_args ( stub, args, sizeof args / sizeof args [ 0 ], &colon ) ||
00236                         colon >= stub->len || stub->payload [ colon ] != ':' ||
00237                         ( stub->len - colon - 1 ) % 2 != 0 ) {
00238                 gdbstub_send_errno ( stub, POSIX_EINVAL );
00239                 return;
00240         }
00241         gdbstub_from_hex_buf ( ( char * ) args [ 0 ], &stub->payload [ colon + 1 ], ( stub->len - colon - 1 ) / 2 );
00242         gdbstub_send_ok ( stub );
00243 }
00244 
00245 static void gdbstub_continue ( struct gdbstub *stub, int single_step ) {
00246         gdbreg_t pc;
00247         if ( stub->len > 1 && gdbstub_get_packet_args ( stub, &pc, 1, NULL ) ) {
00248                 gdbmach_set_pc ( stub->regs, pc );
00249         }
00250         gdbmach_set_single_step ( stub->regs, single_step );
00251         stub->exit_handler = 1;
00252         /* Reply will be sent when we hit the next breakpoint or interrupt */
00253 }
00254 
00255 static void gdbstub_breakpoint ( struct gdbstub *stub ) {
00256         unsigned long args [ 3 ];
00257         int enable = stub->payload [ 0 ] == 'Z' ? 1 : 0;
00258         int rc;
00259 
00260         if ( !gdbstub_get_packet_args ( stub, args, sizeof args / sizeof args [ 0 ], NULL ) ) {
00261                 gdbstub_send_errno ( stub, POSIX_EINVAL );
00262                 return;
00263         }
00264         if ( ( rc = gdbmach_set_breakpoint ( args [ 0 ], args [ 1 ],
00265                                              args [ 2 ], enable ) ) != 0 ) {
00266                 /* Not supported */
00267                 stub->len = 0;
00268                 gdbstub_tx_packet ( stub );
00269                 return;
00270         }
00271         gdbstub_send_ok ( stub );
00272 }
00273 
00274 static void gdbstub_rx_packet ( struct gdbstub *stub ) {
00275         switch ( stub->payload [ 0 ] ) {
00276                 case '?':
00277                         gdbstub_report_signal ( stub );
00278                         break;
00279                 case 'g':
00280                         gdbstub_read_regs ( stub );
00281                         break;
00282                 case 'G':
00283                         gdbstub_write_regs ( stub );
00284                         break;
00285                 case 'm':
00286                         gdbstub_read_mem ( stub );
00287                         break;
00288                 case 'M':
00289                         gdbstub_write_mem ( stub );
00290                         break;
00291                 case 'c': /* Continue */
00292                 case 'k': /* Kill */
00293                 case 's': /* Step */
00294                 case 'D': /* Detach */
00295                         gdbstub_continue ( stub, stub->payload [ 0 ] == 's' );
00296                         if ( stub->payload [ 0 ] == 'D' ) {
00297                                 gdbstub_send_ok ( stub );
00298                         }
00299                         break;
00300                 case 'Z': /* Insert breakpoint */
00301                 case 'z': /* Remove breakpoint */
00302                         gdbstub_breakpoint ( stub );
00303                         break;
00304                 default:
00305                         stub->len = 0;
00306                         gdbstub_tx_packet ( stub );
00307                         break;
00308         }
00309 }
00310 
00311 /* GDB packet parser */
00312 static void gdbstub_state_new ( struct gdbstub *stub, char ch ) {
00313         if ( ch == '$' ) {
00314                 stub->len = 0;
00315                 stub->parse = gdbstub_state_data;
00316         }
00317 }
00318 
00319 static void gdbstub_state_data ( struct gdbstub *stub, char ch ) {
00320         if ( ch == '#' ) {
00321                 stub->parse = gdbstub_state_cksum1;
00322         } else if ( ch == '$' ) {
00323                 stub->len = 0; /* retry new packet */
00324         } else {
00325                 /* If the length exceeds our buffer, let the checksum fail */
00326                 if ( stub->len < SIZEOF_PAYLOAD ) {
00327                         stub->payload [ stub->len++ ] = ch;
00328                 }
00329         }
00330 }
00331 
00332 static void gdbstub_state_cksum1 ( struct gdbstub *stub, char ch ) {
00333         stub->cksum1 = gdbstub_from_hex_digit ( ch ) << 4;
00334         stub->parse = gdbstub_state_cksum2;
00335 }
00336 
00337 static void gdbstub_state_cksum2 ( struct gdbstub *stub, char ch ) {
00338         uint8_t their_cksum;
00339         uint8_t our_cksum;
00340 
00341         stub->parse = gdbstub_state_new;
00342         their_cksum = stub->cksum1 + gdbstub_from_hex_digit ( ch );
00343         our_cksum = gdbstub_cksum ( stub->payload, stub->len );
00344         if ( their_cksum == our_cksum ) {
00345                 stub->trans->send ( "+", 1 );
00346                 if ( stub->len > 0 ) {
00347                         gdbstub_rx_packet ( stub );
00348                 }
00349         } else {
00350                 stub->trans->send ( "-", 1 );
00351         }
00352 }
00353 
00354 static void gdbstub_state_wait_ack ( struct gdbstub *stub, char ch ) {
00355         if ( ch == '+' ) {
00356                 stub->parse = gdbstub_state_new;
00357         } else {
00358                 /* This retransmit is very aggressive but necessary to keep
00359                  * in sync with GDB. */
00360                 gdbstub_tx_packet ( stub );
00361         }
00362 }
00363 
00364 static void gdbstub_parse ( struct gdbstub *stub, char ch ) {
00365         stub->parse ( stub, ch );
00366 }
00367 
00368 static struct gdbstub stub = {
00369         .parse = gdbstub_state_new
00370 };
00371 
00372 void gdbstub_handler ( int signo, gdbreg_t *regs ) {
00373         char packet [ SIZEOF_PAYLOAD + 4 ];
00374         size_t len, i;
00375 
00376         /* A transport must be set up */
00377         if ( !stub.trans ) {
00378                 return;
00379         }
00380 
00381         stub.signo = signo;
00382         stub.regs = regs;
00383         stub.exit_handler = 0;
00384         gdbstub_report_signal ( &stub );
00385         while ( !stub.exit_handler && ( len = stub.trans->recv ( packet, sizeof ( packet ) ) ) > 0 ) {
00386                 for ( i = 0; i < len; i++ ) {
00387                         gdbstub_parse ( &stub, packet [ i ] );
00388                 }
00389         }
00390 }
00391 
00392 struct gdb_transport *find_gdb_transport ( const char *name ) {
00393         struct gdb_transport *trans;
00394 
00395         for_each_table_entry ( trans, GDB_TRANSPORTS ) {
00396                 if ( strcmp ( trans->name, name ) == 0 ) {
00397                         return trans;
00398                 }
00399         }
00400         return NULL;
00401 }
00402 
00403 void gdbstub_start ( struct gdb_transport *trans ) {
00404         stub.trans = trans;
00405         stub.payload = &stub.buf [ 1 ];
00406         gdbmach_init();
00407         gdbmach_breakpoint();
00408 }