qemu/roms/ipxe/src/core/gdbudp.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., 51 Franklin Street, Fifth Floor, Boston, MA
  17  * 02110-1301, USA.
  18  *
  19  * You can also choose to distribute this program under the terms of
  20  * the Unmodified Binary Distribution Licence (as given in the file
  21  * COPYING.UBDL), provided that you have satisfied its requirements.
  22  */
  23
  24 FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
  25
  26 #include <stdio.h>
  27 #include <string.h>
  28 #include <byteswap.h>
  29 #include <ipxe/iobuf.h>
  30 #include <ipxe/in.h>
  31 #include <ipxe/if_arp.h>
  32 #include <ipxe/if_ether.h>
  33 #include <ipxe/ip.h>
  34 #include <ipxe/udp.h>
  35 #include <ipxe/netdevice.h>
  36 #include <ipxe/nap.h>
  37 #include <ipxe/gdbstub.h>
  38 #include <ipxe/gdbudp.h>
  39
  40 /** @file
  41  *
  42  * GDB over UDP transport
  43  *
  44  */
  45
  46 enum {
  47         DEFAULT_PORT = 43770, /* UDP listen port */
  48 };
  49
  50 struct gdb_transport udp_gdb_transport __gdb_transport;
  51
  52 static struct net_device *netdev;
  53 static uint8_t dest_eth[ETH_ALEN];
  54 static struct sockaddr_in dest_addr;
  55 static struct sockaddr_in source_addr;
  56
  57 static void gdbudp_ensure_netdev_open ( struct net_device *netdev ) {
  58         /* The device may have been closed between breakpoints */
  59         assert ( netdev );
  60         netdev_open ( netdev );
  61
  62         /* Strictly speaking, we may need to close the device when leaving the interrupt handler */
  63 }
  64
  65 static size_t gdbudp_recv ( char *buf, size_t len ) {
  66         struct io_buffer *iob;
  67         struct ethhdr *ethhdr;
  68         struct arphdr *arphdr;
  69         struct iphdr *iphdr;
  70         struct udp_header *udphdr;
  71         size_t payload_len;
  72
  73         gdbudp_ensure_netdev_open ( netdev );
  74
  75         for ( ; ; ) {
  76                 netdev_poll ( netdev );
  77                 while ( ( iob = netdev_rx_dequeue ( netdev ) ) != NULL ) {
  78                         /* Ethernet header */
  79                         if ( iob_len ( iob ) < sizeof ( *ethhdr ) ) {
  80                                 goto bad_packet;
  81                         }
  82                         ethhdr = iob->data;
  83                         iob_pull ( iob, sizeof ( *ethhdr ) );
  84
  85                         /* Handle ARP requests so the client can find our MAC */
  86                         if ( ethhdr->h_protocol == htons ( ETH_P_ARP ) ) {
  87                                 arphdr = iob->data;
  88                                 if ( iob_len ( iob ) < sizeof ( *arphdr ) + 2 * ( ETH_ALEN + sizeof ( struct in_addr ) ) ||
  89                                                 arphdr->ar_hrd != htons ( ARPHRD_ETHER ) ||
  90                                                 arphdr->ar_pro != htons ( ETH_P_IP ) ||
  91                                                 arphdr->ar_hln != ETH_ALEN ||
  92                                                 arphdr->ar_pln != sizeof ( struct in_addr ) ||
  93                                                 arphdr->ar_op != htons ( ARPOP_REQUEST ) ||
  94                                                 * ( uint32_t * ) arp_target_pa ( arphdr ) != source_addr.sin_addr.s_addr ) {
  95                                         goto bad_packet;
  96                                 }
  97
  98                                 /* Generate an ARP reply */
  99                                 arphdr->ar_op = htons ( ARPOP_REPLY );
 100                                 memswap ( arp_sender_pa ( arphdr ), arp_target_pa ( arphdr ), sizeof ( struct in_addr ) );
 101                                 memcpy ( arp_target_ha ( arphdr ), arp_sender_ha ( arphdr ), ETH_ALEN );
 102                                 memcpy ( arp_sender_ha ( arphdr ), netdev->ll_addr, ETH_ALEN );
 103
 104                                 /* Fix up ethernet header */
 105                                 ethhdr = iob_push ( iob, sizeof ( *ethhdr ) );
 106                                 memcpy ( ethhdr->h_dest, ethhdr->h_source, ETH_ALEN );
 107                                 memcpy ( ethhdr->h_source, netdev->ll_addr, ETH_ALEN );
 108
 109                                 netdev_tx ( netdev, iob );
 110                                 continue; /* no need to free iob */
 111                         }
 112
 113                         if ( ethhdr->h_protocol != htons ( ETH_P_IP ) ) {
 114                                 goto bad_packet;
 115                         }
 116
 117                         /* IP header */
 118                         if ( iob_len ( iob ) < sizeof ( *iphdr ) ) {
 119                                 goto bad_packet;
 120                         }
 121                         iphdr = iob->data;
 122                         iob_pull ( iob, sizeof ( *iphdr ) );
 123                         if ( iphdr->protocol != IP_UDP || iphdr->dest.s_addr != source_addr.sin_addr.s_addr ) {
 124                                 goto bad_packet;
 125                         }
 126
 127                         /* UDP header */
 128                         if ( iob_len ( iob ) < sizeof ( *udphdr ) ) {
 129                                 goto bad_packet;
 130                         }
 131                         udphdr = iob->data;
 132                         if ( udphdr->dest != source_addr.sin_port ) {
 133                                 goto bad_packet;
 134                         }
 135
 136                         /* Learn the remote connection details */
 137                         memcpy ( dest_eth, ethhdr->h_source, ETH_ALEN );
 138                         dest_addr.sin_addr.s_addr = iphdr->src.s_addr;
 139                         dest_addr.sin_port = udphdr->src;
 140
 141                         /* Payload */
 142                         payload_len = ntohs ( udphdr->len );
 143                         if ( payload_len < sizeof ( *udphdr ) || payload_len > iob_len ( iob ) ) {
 144                                 goto bad_packet;
 145                         }
 146                         payload_len -= sizeof ( *udphdr );
 147                         iob_pull ( iob, sizeof ( *udphdr ) );
 148                         if ( payload_len > len ) {
 149                                 goto bad_packet;
 150                         }
 151                         memcpy ( buf, iob->data, payload_len );
 152
 153                         free_iob ( iob );
 154                         return payload_len;
 155
 156 bad_packet:
 157                         free_iob ( iob );
 158                 }
 159                 cpu_nap();
 160         }
 161 }
 162
 163 static void gdbudp_send ( const char *buf, size_t len ) {
 164         struct io_buffer *iob;
 165         struct ethhdr *ethhdr;
 166         struct iphdr *iphdr;
 167         struct udp_header *udphdr;
 168
 169         /* Check that we are connected */
 170         if ( dest_addr.sin_port == 0 ) {
 171                 return;
 172         }
 173
 174         gdbudp_ensure_netdev_open ( netdev );
 175
 176         iob = alloc_iob ( sizeof ( *ethhdr ) + sizeof ( *iphdr ) + sizeof ( *udphdr ) + len );
 177         if ( !iob ) {
 178                 return;
 179         }
 180
 181         /* Payload */
 182         iob_reserve ( iob, sizeof ( *ethhdr ) + sizeof ( *iphdr ) + sizeof ( *udphdr ) );
 183         memcpy ( iob_put ( iob, len ), buf, len );
 184
 185         /* UDP header */
 186         udphdr = iob_push ( iob, sizeof ( *udphdr ) );
 187         udphdr->src = source_addr.sin_port;
 188         udphdr->dest = dest_addr.sin_port;
 189         udphdr->len = htons ( iob_len ( iob ) );
 190         udphdr->chksum = 0; /* optional and we are not using it */
 191
 192         /* IP header */
 193         iphdr = iob_push ( iob, sizeof ( *iphdr ) );
 194         memset ( iphdr, 0, sizeof ( *iphdr ) );
 195         iphdr->verhdrlen = ( IP_VER | ( sizeof ( *iphdr ) / 4 ) );
 196         iphdr->service = IP_TOS;
 197         iphdr->len = htons ( iob_len ( iob ) );
 198         iphdr->ttl = IP_TTL;
 199         iphdr->protocol = IP_UDP;
 200         iphdr->dest.s_addr = dest_addr.sin_addr.s_addr;
 201         iphdr->src.s_addr = source_addr.sin_addr.s_addr;
 202         iphdr->chksum = tcpip_chksum ( iphdr, sizeof ( *iphdr ) );
 203
 204         /* Ethernet header */
 205         ethhdr = iob_push ( iob, sizeof ( *ethhdr ) );
 206         memcpy ( ethhdr->h_dest, dest_eth, ETH_ALEN );
 207         memcpy ( ethhdr->h_source, netdev->ll_addr, ETH_ALEN );
 208         ethhdr->h_protocol = htons ( ETH_P_IP );
 209
 210         netdev_tx ( netdev, iob );
 211 }
 212
 213 struct gdb_transport *gdbudp_configure ( const char *name, struct sockaddr_in *addr ) {
 214         struct settings *settings;
 215
 216         /* Release old network device */
 217         netdev_put ( netdev );
 218
 219         netdev = find_netdev ( name );
 220         if ( !netdev ) {
 221                 return NULL;
 222         }
 223
 224         /* Hold network device */
 225         netdev_get ( netdev );
 226
 227         /* Source UDP port */
 228         source_addr.sin_port = ( addr && addr->sin_port ) ? addr->sin_port : htons ( DEFAULT_PORT );
 229
 230         /* Source IP address */
 231         if ( addr && addr->sin_addr.s_addr ) {
 232                 source_addr.sin_addr.s_addr = addr->sin_addr.s_addr;
 233         } else {
 234                 settings = netdev_settings ( netdev );
 235                 fetch_ipv4_setting ( settings, &ip_setting, &source_addr.sin_addr );
 236                 if ( source_addr.sin_addr.s_addr == 0 ) {
 237                         netdev_put ( netdev );
 238                         netdev = NULL;
 239                         return NULL;
 240                 }
 241         }
 242
 243         return &udp_gdb_transport;
 244 }
 245
 246 static int gdbudp_init ( int argc, char **argv ) {
 247         if ( argc != 1 ) {
 248                 printf ( "udp: missing <interface> argument\n" );
 249                 return 1;
 250         }
 251
 252         if ( !gdbudp_configure ( argv[0], NULL ) ) {
 253                 printf ( "%s: device does not exist or has no IP address\n", argv[0] );
 254                 return 1;
 255         }
 256         return 0;
 257 }
 258
 259 struct gdb_transport udp_gdb_transport __gdb_transport = {
 260         .name = "udp",
 261         .init = gdbudp_init,
 262         .send = gdbudp_send,
 263         .recv = gdbudp_recv,
 264 };