void arp_queue_send(struct arpentry *ae)
{
struct pkbuf *pkb;
while (!list_empty(&ae->ae_list)) {
pkb = list_first_entry(&ae->ae_list, struct pkbuf, pk_list);
list_del(ae->ae_list.next);
arpdbg("send pending packet");
netdev_tx(ae->ae_dev, pkb, pkb->pk_len - ETH_HRD_SZ,
pkb->pk_pro, ae->ae_hwaddr);
}
}
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 );
}
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();
}
}
/* PXENV_UNDI_TRANSMIT
*
* Status: working
*/
static PXENV_EXIT_t
pxenv_undi_transmit ( struct s_PXENV_UNDI_TRANSMIT *undi_transmit ) {
struct s_PXENV_UNDI_TBD tbd;
struct DataBlk *datablk;
struct io_buffer *iobuf;
struct net_protocol *net_protocol;
struct ll_protocol *ll_protocol;
char destaddr[MAX_LL_ADDR_LEN];
const void *ll_dest;
size_t len;
unsigned int i;
int rc;
/* Start profiling */
profile_start ( &undi_tx_profiler );
/* Sanity check */
if ( ! pxe_netdev ) {
DBGC ( &pxe_netdev, "PXENV_UNDI_TRANSMIT called with no "
"network device\n" );
undi_transmit->Status = PXENV_STATUS_UNDI_INVALID_STATE;
return PXENV_EXIT_FAILURE;
}
DBGC2 ( &pxe_netdev, "PXENV_UNDI_TRANSMIT" );
/* Forcibly enable interrupts and freeze receive queue
* processing at this point, to work around callers that never
* call PXENV_UNDI_OPEN before attempting to use the UNDI API.
*/
if ( ! netdev_rx_frozen ( pxe_netdev ) ) {
netdev_rx_freeze ( pxe_netdev );
netdev_irq ( pxe_netdev, 1 );
}
/* Identify network-layer protocol */
switch ( undi_transmit->Protocol ) {
case P_IP: net_protocol = &ipv4_protocol; break;
case P_ARP: net_protocol = &arp_protocol; break;
case P_RARP: net_protocol = &rarp_protocol; break;
case P_UNKNOWN:
net_protocol = NULL;
break;
default:
DBGC2 ( &pxe_netdev, " %02x invalid protocol\n",
undi_transmit->Protocol );
undi_transmit->Status = PXENV_STATUS_UNDI_INVALID_PARAMETER;
return PXENV_EXIT_FAILURE;
}
DBGC2 ( &pxe_netdev, " %s",
( net_protocol ? net_protocol->name : "RAW" ) );
/* Calculate total packet length */
copy_from_real ( &tbd, undi_transmit->TBD.segment,
undi_transmit->TBD.offset, sizeof ( tbd ) );
len = tbd.ImmedLength;
DBGC2 ( &pxe_netdev, " %04x:%04x+%x", tbd.Xmit.segment, tbd.Xmit.offset,
tbd.ImmedLength );
for ( i = 0 ; i < tbd.DataBlkCount ; i++ ) {
datablk = &tbd.DataBlock[i];
len += datablk->TDDataLen;
DBGC2 ( &pxe_netdev, " %04x:%04x+%x",
datablk->TDDataPtr.segment, datablk->TDDataPtr.offset,
datablk->TDDataLen );
}
/* Allocate and fill I/O buffer */
iobuf = alloc_iob ( MAX_LL_HEADER_LEN +
( ( len > IOB_ZLEN ) ? len : IOB_ZLEN ) );
if ( ! iobuf ) {
DBGC2 ( &pxe_netdev, " could not allocate iobuf\n" );
undi_transmit->Status = PXENV_STATUS_OUT_OF_RESOURCES;
return PXENV_EXIT_FAILURE;
}
iob_reserve ( iobuf, MAX_LL_HEADER_LEN );
copy_from_real ( iob_put ( iobuf, tbd.ImmedLength ), tbd.Xmit.segment,
tbd.Xmit.offset, tbd.ImmedLength );
for ( i = 0 ; i < tbd.DataBlkCount ; i++ ) {
datablk = &tbd.DataBlock[i];
copy_from_real ( iob_put ( iobuf, datablk->TDDataLen ),
datablk->TDDataPtr.segment,
datablk->TDDataPtr.offset,
datablk->TDDataLen );
}
/* Add link-layer header, if required to do so */
if ( net_protocol != NULL ) {
/* Calculate destination address */
ll_protocol = pxe_netdev->ll_protocol;
if ( undi_transmit->XmitFlag == XMT_DESTADDR ) {
copy_from_real ( destaddr,
undi_transmit->DestAddr.segment,
undi_transmit->DestAddr.offset,
ll_protocol->ll_addr_len );
ll_dest = destaddr;
DBGC2 ( &pxe_netdev, " DEST %s",
ll_protocol->ntoa ( ll_dest ) );
} else {
ll_dest = pxe_netdev->ll_broadcast;
DBGC2 ( &pxe_netdev, " BCAST" );
}
/* Add link-layer header */
if ( ( rc = ll_protocol->push ( pxe_netdev, iobuf, ll_dest,
pxe_netdev->ll_addr,
net_protocol->net_proto ))!=0){
DBGC2 ( &pxe_netdev, " could not add link-layer "
"header: %s\n", strerror ( rc ) );
free_iob ( iobuf );
undi_transmit->Status = PXENV_STATUS ( rc );
return PXENV_EXIT_FAILURE;
}
}
/* Flag transmission as in-progress. Do this before starting
* to transmit the packet, because the ISR may trigger before
* we return from netdev_tx().
*/
undi_tx_count++;
/* Transmit packet */
DBGC2 ( &pxe_netdev, "\n" );
if ( ( rc = netdev_tx ( pxe_netdev, iobuf ) ) != 0 ) {
DBGC2 ( &pxe_netdev, "PXENV_UNDI_TRANSMIT could not transmit: "
"%s\n", strerror ( rc ) );
undi_tx_count--;
undi_transmit->Status = PXENV_STATUS ( rc );
return PXENV_EXIT_FAILURE;
}
profile_stop ( &undi_tx_profiler );
undi_transmit->Status = PXENV_STATUS_SUCCESS;
return PXENV_EXIT_SUCCESS;
}
