static void gdbudp_send ( const char *buf, size_t len ) {
struct io_buffer *iob;
struct ethhdr *ethhdr;
struct iphdr *iphdr;
struct udp_header *udphdr;
/* Check that we are connected */
if ( dest_addr.sin_port == 0 ) {
return;
}
gdbudp_ensure_netdev_open ( netdev );
iob = alloc_iob ( sizeof ( *ethhdr ) + sizeof ( *iphdr ) + sizeof ( *udphdr ) + len );
if ( !iob ) {
return;
}
/* Payload */
iob_reserve ( iob, sizeof ( *ethhdr ) + sizeof ( *iphdr ) + sizeof ( *udphdr ) );
memcpy ( iob_put ( iob, len ), buf, len );
/* UDP header */
udphdr = iob_push ( iob, sizeof ( *udphdr ) );
udphdr->src = source_addr.sin_port;
udphdr->dest = dest_addr.sin_port;
udphdr->len = htons ( iob_len ( iob ) );
udphdr->chksum = 0; /* optional and we are not using it */
/* IP header */
iphdr = iob_push ( iob, sizeof ( *iphdr ) );
memset ( iphdr, 0, sizeof ( *iphdr ) );
iphdr->verhdrlen = ( IP_VER | ( sizeof ( *iphdr ) / 4 ) );
iphdr->service = IP_TOS;
iphdr->len = htons ( iob_len ( iob ) );
iphdr->ttl = IP_TTL;
iphdr->protocol = IP_UDP;
iphdr->dest.s_addr = dest_addr.sin_addr.s_addr;
iphdr->src.s_addr = source_addr.sin_addr.s_addr;
iphdr->chksum = tcpip_chksum ( iphdr, sizeof ( *iphdr ) );
/* Ethernet header */
ethhdr = iob_push ( iob, sizeof ( *ethhdr ) );
memcpy ( ethhdr->h_dest, dest_eth, ETH_ALEN );
memcpy ( ethhdr->h_source, netdev->ll_addr, ETH_ALEN );
ethhdr->h_protocol = htons ( ETH_P_IP );
netdev_tx ( netdev, iob );
}
/**
* Transmit packet
*
* @v dm96xx DM96xx device
* @v iobuf I/O buffer
* @ret rc Return status code
*/
static int dm96xx_out_transmit ( struct dm96xx_device *dm96xx,
struct io_buffer *iobuf ) {
struct dm96xx_tx_header *header;
size_t len = iob_len ( iobuf );
int rc;
/* Prepend header */
if ( ( rc = iob_ensure_headroom ( iobuf, sizeof ( *header ) ) ) != 0 )
return rc;
header = iob_push ( iobuf, sizeof ( *header ) );
header->len = cpu_to_le16 ( len );
/* Enqueue I/O buffer */
if ( ( rc = usb_stream ( &dm96xx->usbnet.out, iobuf, 0 ) ) != 0 )
return rc;
return 0;
}
/**
* Transmit packet
*
* @v smsc95xx SMSC95xx device
* @v iobuf I/O buffer
* @ret rc Return status code
*/
static int smsc95xx_out_transmit ( struct smsc95xx_device *smsc95xx,
struct io_buffer *iobuf ) {
struct smsc95xx_tx_header *header;
size_t len = iob_len ( iobuf );
int rc;
/* Profile transmissions */
profile_start ( &smsc95xx_out_profiler );
/* Prepend header */
if ( ( rc = iob_ensure_headroom ( iobuf, sizeof ( *header ) ) ) != 0 )
return rc;
header = iob_push ( iobuf, sizeof ( *header ) );
header->command = cpu_to_le32 ( SMSC95XX_TX_FIRST | SMSC95XX_TX_LAST |
SMSC95XX_TX_LEN ( len ) );
header->len = cpu_to_le32 ( len );
/* Enqueue I/O buffer */
if ( ( rc = usb_stream ( &smsc95xx->usbnet.out, iobuf, 0 ) ) != 0 )
return rc;
profile_stop ( &smsc95xx_out_profiler );
return 0;
}
static size_t gdbudp_recv ( char *buf, size_t len ) {
struct io_buffer *iob;
struct ethhdr *ethhdr;
struct arphdr *arphdr;
struct iphdr *iphdr;
struct udp_header *udphdr;
size_t payload_len;
gdbudp_ensure_netdev_open ( netdev );
for ( ; ; ) {
netdev_poll ( netdev );
while ( ( iob = netdev_rx_dequeue ( netdev ) ) != NULL ) {
/* Ethernet header */
if ( iob_len ( iob ) < sizeof ( *ethhdr ) ) {
goto bad_packet;
}
ethhdr = iob->data;
iob_pull ( iob, sizeof ( *ethhdr ) );
/* Handle ARP requests so the client can find our MAC */
if ( ethhdr->h_protocol == htons ( ETH_P_ARP ) ) {
arphdr = iob->data;
if ( iob_len ( iob ) < sizeof ( *arphdr ) + 2 * ( ETH_ALEN + sizeof ( struct in_addr ) ) ||
arphdr->ar_hrd != htons ( ARPHRD_ETHER ) ||
arphdr->ar_pro != htons ( ETH_P_IP ) ||
arphdr->ar_hln != ETH_ALEN ||
arphdr->ar_pln != sizeof ( struct in_addr ) ||
arphdr->ar_op != htons ( ARPOP_REQUEST ) ||
* ( uint32_t * ) arp_target_pa ( arphdr ) != source_addr.sin_addr.s_addr ) {
goto bad_packet;
}
/* Generate an ARP reply */
arphdr->ar_op = htons ( ARPOP_REPLY );
memswap ( arp_sender_pa ( arphdr ), arp_target_pa ( arphdr ), sizeof ( struct in_addr ) );
memcpy ( arp_target_ha ( arphdr ), arp_sender_ha ( arphdr ), ETH_ALEN );
memcpy ( arp_sender_ha ( arphdr ), netdev->ll_addr, ETH_ALEN );
/* Fix up ethernet header */
ethhdr = iob_push ( iob, sizeof ( *ethhdr ) );
memcpy ( ethhdr->h_dest, ethhdr->h_source, ETH_ALEN );
memcpy ( ethhdr->h_source, netdev->ll_addr, ETH_ALEN );
netdev_tx ( netdev, iob );
continue; /* no need to free iob */
}
if ( ethhdr->h_protocol != htons ( ETH_P_IP ) ) {
goto bad_packet;
}
/* IP header */
if ( iob_len ( iob ) < sizeof ( *iphdr ) ) {
goto bad_packet;
}
iphdr = iob->data;
iob_pull ( iob, sizeof ( *iphdr ) );
if ( iphdr->protocol != IP_UDP || iphdr->dest.s_addr != source_addr.sin_addr.s_addr ) {
goto bad_packet;
}
/* UDP header */
if ( iob_len ( iob ) < sizeof ( *udphdr ) ) {
goto bad_packet;
}
udphdr = iob->data;
if ( udphdr->dest != source_addr.sin_port ) {
goto bad_packet;
}
/* Learn the remote connection details */
memcpy ( dest_eth, ethhdr->h_source, ETH_ALEN );
dest_addr.sin_addr.s_addr = iphdr->src.s_addr;
dest_addr.sin_port = udphdr->src;
/* Payload */
payload_len = ntohs ( udphdr->len );
if ( payload_len < sizeof ( *udphdr ) || payload_len > iob_len ( iob ) ) {
goto bad_packet;
}
payload_len -= sizeof ( *udphdr );
iob_pull ( iob, sizeof ( *udphdr ) );
if ( payload_len > len ) {
goto bad_packet;
}
memcpy ( buf, iob->data, payload_len );
free_iob ( iob );
return payload_len;
bad_packet:
free_iob ( iob );
}
cpu_nap();
}
}
static uint8_t eth_broadcast[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
/**
* Add Ethernet link-layer header
*
* @v netdev Network device
* @v iobuf I/O buffer
* @v ll_dest Link-layer destination address
* @v ll_source Source link-layer address
* @v net_proto Network-layer protocol, in network-byte order
* @ret rc Return status code
*/
static int eth_push ( struct net_device *netdev __unused,
struct io_buffer *iobuf, const void *ll_dest,
const void *ll_source, uint16_t net_proto ) {
struct ethhdr *ethhdr = iob_push ( iobuf, sizeof ( *ethhdr ) );
/* Build Ethernet header */
memcpy ( ethhdr->h_dest, ll_dest, ETH_ALEN );
memcpy ( ethhdr->h_source, ll_source, ETH_ALEN );
ethhdr->h_protocol = net_proto;
return 0;
}
/**
* Remove Ethernet link-layer header
*
* @v netdev Network device
* @v iobuf I/O buffer
* @ret ll_dest Link-layer destination address
