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);
}
static void if_tx_thread(void *args)
{
ifnet *i = args;
cbuf *buf;
ssize_t len;
t_current_set_name("IF Xmit");
#if IF_PRIO
thread_set_priority(i->tx_thread, THREAD_MAX_RT_PRIORITY - 2);
#endif
//if(i->fd < 0) return -1;
//printf("if %x tx thread inited", i);
for(;;) {
sem_acquire(i->tx_queue_sem);
//printf("if %x tx thread gogo\n", i);
for(;;) {
// pull a packet out of the queue
mutex_lock(&i->tx_queue_lock);
buf = fixed_queue_dequeue(&i->tx_queue);
mutex_unlock(&i->tx_queue_lock);
if(!buf)
break;
#if LOSE_TX_PACKETS
if(rand() % 100 < LOSE_TX_PERCENTAGE) {
cbuf_free_chain(buf);
continue;
}
#endif
// put the cbuf chain into a flat buffer
len = cbuf_get_len(buf);
cbuf_memcpy_from_chain(i->tx_buf, buf, 0, len);
cbuf_free_chain(buf);
#if 0||NET_CHATTY
dprintf("if_tx_thread: sending packet size %ld\n", (long)len);
#endif
//sys_write(i->fd, i->tx_buf, 0, len);
i->dev->dops.write(i->dev, i->tx_buf, len);
}
}
}
int if_output(cbuf *b, ifnet *i)
{
bool release_sem = false;
bool enqueue_failed = false;
//printf("if out");
// stick the buffer on a transmit queue
mutex_lock(&i->tx_queue_lock);
if(fixed_queue_enqueue(&i->tx_queue, b) < 0)
enqueue_failed = true;
if(i->tx_queue.count == 1)
release_sem = true;
mutex_unlock(&i->tx_queue_lock);
if(enqueue_failed) {
cbuf_free_chain(b);
return ERR_NO_MEMORY;
}
if(release_sem)
sem_release(i->tx_queue_sem);
//printf("if %x out ok\n", i);
return NO_ERROR;
}
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
port_delete(port_id id)
{
int slot;
sem_id r_sem, w_sem;
int capacity;
int i;
char *old_name;
struct port_msg *q;
if(ports_active == false)
return ERR_PORT_NOT_ACTIVE;
if(id < 0)
return ERR_INVALID_HANDLE;
slot = id % MAX_PORTS;
int_disable_interrupts();
GRAB_PORT_LOCK(ports[slot]);
if(ports[slot].id != id) {
RELEASE_PORT_LOCK(ports[slot]);
int_restore_interrupts();
dprintf("port_delete: invalid port_id %d\n", id);
return ERR_INVALID_HANDLE;
}
/* mark port as invalid */
ports[slot].id = -1;
old_name = ports[slot].name;
q = ports[slot].msg_queue;
r_sem = ports[slot].read_sem;
w_sem = ports[slot].write_sem;
capacity = ports[slot].capacity;
ports[slot].name = NULL;
RELEASE_PORT_LOCK(ports[slot]);
int_restore_interrupts();
// delete the cbuf's that are left in the queue (if any)
for (i=0; i<capacity; i++) {
if (q[i].data_cbuf != NULL)
cbuf_free_chain(q[i].data_cbuf);
}
kfree(q);
kfree(old_name);
// release the threads that were blocking on this port by deleting the sem
// read_port() will see the ERR_SEM_DELETED acq_sem() return value, and act accordingly
sem_delete(r_sem);
sem_delete(w_sem);
return NO_ERROR;
}
static int if_tx_thread(void *args)
{
ifnet *i = args;
cbuf *buf;
ssize_t len;
if(i->fd < 0)
return -1;
for(;;) {
sem_acquire(i->tx_queue_sem, 1);
for(;;) {
// pull a packet out of the queue
mutex_lock(&i->tx_queue_lock);
buf = fixed_queue_dequeue(&i->tx_queue);
mutex_unlock(&i->tx_queue_lock);
if(!buf)
break;
#if LOSE_TX_PACKETS
if(rand() % 100 < LOSE_TX_PERCENTAGE) {
cbuf_free_chain(buf);
continue;
}
#endif
// put the cbuf chain into a flat buffer
len = cbuf_get_len(buf);
cbuf_memcpy_from_chain(i->tx_buf, buf, 0, len);
cbuf_free_chain(buf);
#if NET_CHATTY
dprintf("if_tx_thread: sending packet size %Ld\n", (long long)len);
#endif
sys_write(i->fd, i->tx_buf, 0, len);
}
}
}
static void udp_endpoint_release_ref(udp_endpoint *e)
{
if(atomic_add(&e->ref_count, -1) == 1) {
udp_queue_elem *qe;
mutex_destroy(&e->lock);
sem_delete(e->blocking_sem);
// clear out the queue of packets
for(qe = udp_queue_pop(&e->q); qe; qe = udp_queue_pop(&e->q)) {
if(qe->buf)
cbuf_free_chain(qe->buf);
kfree(qe);
}
}
}
ssize_t
port_read_etc(port_id id,
int32 *msg_code,
void *msg_buffer,
size_t buffer_size,
uint32 flags,
bigtime_t timeout)
{
int slot;
sem_id cached_semid;
size_t siz;
int res;
int t;
cbuf* msg_store;
int32 code;
int err;
if(ports_active == false)
return ERR_PORT_NOT_ACTIVE;
if(id < 0)
return ERR_INVALID_HANDLE;
if(msg_code == NULL)
return ERR_INVALID_ARGS;
if((msg_buffer == NULL) && (buffer_size > 0))
return ERR_INVALID_ARGS;
if (timeout < 0)
return ERR_INVALID_ARGS;
flags = flags & (PORT_FLAG_USE_USER_MEMCPY | PORT_FLAG_INTERRUPTABLE | PORT_FLAG_TIMEOUT);
slot = id % MAX_PORTS;
int_disable_interrupts();
GRAB_PORT_LOCK(ports[slot]);
if(ports[slot].id != id) {
RELEASE_PORT_LOCK(ports[slot]);
int_restore_interrupts();
dprintf("read_port_etc: invalid port_id %d\n", id);
return ERR_INVALID_HANDLE;
}
// store sem_id in local variable
cached_semid = ports[slot].read_sem;
// unlock port && enable ints/
RELEASE_PORT_LOCK(ports[slot]);
int_restore_interrupts();
// XXX -> possible race condition if port gets deleted (->sem deleted too), therefore
// sem_id is cached in local variable up here
// get 1 entry from the queue, block if needed
res = sem_acquire_etc(cached_semid, 1,
flags, timeout, NULL);
// XXX: possible race condition if port read by two threads...
// both threads will read in 2 different slots allocated above, simultaneously
// slot is a thread-local variable
if (res == ERR_SEM_DELETED) {
// somebody deleted the port
return ERR_PORT_DELETED;
}
if (res == ERR_INTERRUPTED) {
// XXX: somebody signaled the process the port belonged to, deleting the sem ?
return ERR_INTERRUPTED;
}
if (res == ERR_SEM_TIMED_OUT) {
// timed out, or, if timeout=0, 'would block'
return ERR_PORT_TIMED_OUT;
}
if (res != NO_ERROR) {
dprintf("write_port_etc: res unknown error %d\n", res);
return res;
}
int_disable_interrupts();
GRAB_PORT_LOCK(ports[slot]);
t = ports[slot].tail;
if (t < 0)
panic("port %id: tail < 0", ports[slot].id);
if (t > ports[slot].capacity)
panic("port %id: tail > cap %d", ports[slot].id, ports[slot].capacity);
ports[slot].tail = (ports[slot].tail + 1) % ports[slot].capacity;
msg_store = ports[slot].msg_queue[t].data_cbuf;
code = ports[slot].msg_queue[t].msg_code;
// mark queue entry unused
ports[slot].msg_queue[t].data_cbuf = NULL;
// check output buffer size
siz = min(buffer_size, ports[slot].msg_queue[t].data_len);
cached_semid = ports[slot].write_sem;
RELEASE_PORT_LOCK(ports[slot]);
int_restore_interrupts();
// copy message
*msg_code = code;
if (siz > 0) {
if (flags & PORT_FLAG_USE_USER_MEMCPY) {
if ((err = cbuf_user_memcpy_from_chain(msg_buffer, msg_store, 0, siz) < 0)) {
// leave the port intact, for other threads that might not crash
cbuf_free_chain(msg_store);
sem_release(cached_semid, 1);
return err;
}
} else
cbuf_memcpy_from_chain(msg_buffer, msg_store, 0, siz);
}
// free the cbuf
cbuf_free_chain(msg_store);
// make one spot in queue available again for write
sem_release(cached_semid, 1);
return siz;
}
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;
}
ssize_t udp_recvfrom(
void *prot_data,
void *buf, ssize_t len,
i4sockaddr *saddr,
int flags, bigtime_t timeout)
{
udp_endpoint *e = prot_data;
udp_queue_elem *qe;
int err;
ssize_t ret;
retry:
//#warning timeout ignored
#if 1
if(flags & SOCK_FLAG_TIMEOUT)
err = hal_sem_acquire_etc( &e->blocking_sem, 1, SEM_FLAG_TIMEOUT, timeout );
else
#endif
err = sem_acquire(e->blocking_sem);
//if(err < 0)
if(err)
return -err;
// pop an item off the list, if there are any
mutex_lock(&e->lock);
qe = udp_queue_pop(&e->q);
mutex_unlock(&e->lock);
if(!qe)
{
#if 1||NET_CHATTY
printf("UDP read retry");
#endif
goto retry;
}
// we have the data, copy it out
//err = cbuf_user_memcpy_from_chain(buf, qe->buf, 0, min(qe->len, len));
err = cbuf_memcpy_from_chain(buf, qe->buf, 0, min(qe->len, len));
if(err < 0) {
ret = err;
goto out;
}
ret = qe->len;
// copy the address out
if(saddr) {
saddr->addr.len = 4;
saddr->addr.type = ADDR_TYPE_IP;
NETADDR_TO_IPV4(saddr->addr) = qe->src_address;
saddr->port = qe->src_port;
}
out:
// free this queue entry
cbuf_free_chain(qe->buf);
kfree(qe);
return ret;
}
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;
}