int ethernet_input(cbuf *buf, ifnet *i)
{
int err;
ethernet2_header *e2_head;
uint16 type;
if(cbuf_get_len(buf) < MIN_ETHERNET2_LEN)
{
cbuf_free_chain(buf);
return -1;
}
e2_head = cbuf_get_ptr(buf, 0);
type = ntohs(e2_head->type);
dump_ethernet_header(e2_head);
// strip out the ethernet header
buf = cbuf_truncate_head(buf, sizeof(ethernet2_header), true);
switch(type) {
case PROT_TYPE_IPV4:
err = ipv4_input(buf, i);
break;
case PROT_TYPE_ARP:
err = arp_input(buf, i);
break;
default:
dprintf("ethernet_receive: unknown ethernet type 0x%x\n", type);
err = -1;
}
return err;
}
int ethernet_output(cbuf *buf, ifnet *i, netaddr *target, int protocol_type)
{
cbuf *eheader_buf;
ethernet2_header *eheader;
//printf("eth out");
if(target->type != ADDR_TYPE_ETHERNET) {
cbuf_free_chain(buf);
return ERR_INVALID_ARGS;
}
eheader_buf = cbuf_get_chain(sizeof(ethernet2_header));
if(!eheader_buf) {
dprintf("ethernet_output: error allocating cbuf for eheader\n");
cbuf_free_chain(buf);
return ERR_NO_MEMORY;
}
// put together an ethernet header
eheader = (ethernet2_header *)cbuf_get_ptr(eheader_buf, 0);
memcpy(&eheader->dest, &target->addr[0], 6);
memcpy(&eheader->src, &i->link_addr->addr.addr[0], 6);
eheader->type = htons(protocol_type);
// chain the buffers together
buf = cbuf_merge_chains(eheader_buf, buf);
return if_output(buf, i);
}
ssize_t udp_sendto(void *prot_data, const void *inbuf, ssize_t len, i4sockaddr *toaddr)
{
udp_endpoint *e = prot_data;
udp_header *header;
int total_len;
cbuf *buf;
udp_pseudo_header pheader;
ipv4_addr srcaddr;
int err;
// make sure the args make sense
if(len < 0 || len + sizeof(udp_header) > 0xffff)
return ERR_INVALID_ARGS;
if(toaddr->port < 0 || toaddr->port > 0xffff)
return ERR_INVALID_ARGS;
// allocate a buffer to hold the data + header
total_len = len + sizeof(udp_header);
buf = cbuf_get_chain(total_len);
if(!buf)
return ERR_NO_MEMORY;
// copy the data to this new buffer
//err = cbuf_user_memcpy_to_chain(buf, sizeof(udp_header), inbuf, len);
err = cbuf_memcpy_to_chain(buf, sizeof(udp_header), inbuf, len);
if(err < 0) {
cbuf_free_chain(buf);
return ERR_VM_BAD_USER_MEMORY;
}
// set up the udp pseudo header
if(ipv4_lookup_srcaddr_for_dest(NETADDR_TO_IPV4(toaddr->addr), &srcaddr) < 0) {
cbuf_free_chain(buf);
return ERR_NET_NO_ROUTE;
}
pheader.source_addr = htonl(srcaddr);
pheader.dest_addr = htonl(NETADDR_TO_IPV4(toaddr->addr));
pheader.zero = 0;
pheader.protocol = IP_PROT_UDP;
pheader.udp_length = htons(total_len);
// start setting up the header
header = cbuf_get_ptr(buf, 0);
header->source_port = htons(e->port);
header->dest_port = htons(toaddr->port);
header->length = htons(total_len);
header->checksum = 0;
header->checksum = cbuf_ones_cksum16_2(buf, 0, total_len, &pheader, sizeof(pheader));
if(header->checksum == 0)
header->checksum = 0xffff;
#if NET_CHATTY
printf("UDP SEND port %d to %d, len %d (%d)\n", e->port, toaddr->port, total_len, len );
#endif
// send it away
err = ipv4_output(buf, NETADDR_TO_IPV4(toaddr->addr), IP_PROT_UDP);
STAT_INC_CNT(STAT_CNT_UDP_TX);
// if it returns ARP_QUEUED, then it's actually okay
if(err == ERR_NET_ARP_QUEUED) {
err = 0;
}
return err;
}
int udp_input(cbuf *buf, ifnet *i, ipv4_addr source_address, ipv4_addr target_address)
{
(void) i;
udp_header *header;
udp_endpoint *e;
udp_queue_elem *qe;
uint16 port;
int err;
STAT_INC_CNT(STAT_CNT_UDP_RX);
header = cbuf_get_ptr(buf, 0);
#if NET_CHATTY
dprintf("udp_input: src port %d, dest port %d, len %d, buf len %d, checksum 0x%x\n",
ntohs(header->source_port), ntohs(header->dest_port), ntohs(header->length), (int)cbuf_get_len(buf), ntohs(header->checksum));
#endif
if(ntohs(header->length) > (uint16)cbuf_get_len(buf)) {
err = ERR_NET_BAD_PACKET;
goto ditch_packet;
}
// deal with the checksum check
if(header->checksum) {
udp_pseudo_header pheader;
uint16 checksum;
// set up the pseudo header for checksum purposes
pheader.source_addr = htonl(source_address);
pheader.dest_addr = htonl(target_address);
pheader.zero = 0;
pheader.protocol = IP_PROT_UDP;
pheader.udp_length = header->length;
checksum = cbuf_ones_cksum16_2(buf, 0, ntohs(header->length), &pheader, sizeof(pheader));
if(checksum != 0) {
#if NET_CHATTY
dprintf("udp_receive: packet failed checksum\n");
#endif
err = ERR_NET_BAD_PACKET;
goto ditch_packet;
}
}
// see if we have an endpoint
port = ntohs(header->dest_port);
mutex_lock(&endpoints_lock);
e = hash_lookup(endpoints, &port);
if(e)
udp_endpoint_acquire_ref(e);
mutex_unlock(&endpoints_lock);
if(!e) {
err = NO_ERROR;
#if NET_CHATTY
dprintf("udp_receive: no endpoint found\n");
#endif
goto ditch_packet;
}
// okay, we have an endpoint, lets queue our stuff up and move on
qe = kmalloc(sizeof(udp_queue_elem));
if(!qe) {
udp_endpoint_release_ref(e);
err = ERR_NO_MEMORY;
goto ditch_packet;
}
qe->src_port = ntohs(header->source_port);
qe->target_port = port;
qe->src_address = source_address;
qe->target_address = target_address;
qe->len = ntohs(header->length) - sizeof(udp_header);
// trim off the udp header
buf = cbuf_truncate_head(buf, sizeof(udp_header), true);
qe->buf = buf;
mutex_lock(&e->lock);
udp_queue_push(&e->q, qe);
mutex_unlock(&e->lock);
sem_release(e->blocking_sem);
udp_endpoint_release_ref(e);
err = NO_ERROR;
return err;
ditch_packet:
#if NET_CHATTY
dprintf("udp_receive: packet thrown away\n");
#endif
cbuf_free_chain(buf);
return err;
}
