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_rx_thread(void *args)
{
ifnet *i = args;
cbuf *b;
t_current_set_name("IF Recv");
#if IF_PRIO
thread_set_priority(i->rx_thread, THREAD_MAX_RT_PRIORITY - 2);
#endif
//if(i->fd < 0) return -1;
for(;;) {
ssize_t len;
//len = sys_read(i->fd, i->rx_buf, 0, sizeof(i->rx_buf));
len = i->dev->dops.read(i->dev, i->rx_buf, sizeof(i->rx_buf));
#if 0||NET_CHATTY
dprintf("if_rx_thread: got ethernet packet, size %ld\n", (long)len);
#endif
if(len < 0) {
thread_snooze(10000);
continue;
}
if(len == 0)
continue;
#if LOSE_RX_PACKETS
if(rand() % 100 < LOSE_RX_PERCENTAGE) {
dprintf("if_rx_thread: purposely lost packet, size %d\n", len);
continue;
}
#endif
// check to see if we have a link layer address attached to us
if(!i->link_addr) {
#if 1||NET_CHATTY
dprintf("if_rx_thread: dumping packet because of no link address (%p)\n", i);
#endif
continue;
}
// for now just move it over into a cbuf
b = cbuf_get_chain(len);
if(!b) {
dprintf("if_rx_thread: could not allocate cbuf to hold ethernet packet\n");
continue;
}
cbuf_memcpy_to_chain(b, 0, i->rx_buf, len);
i->link_input(b, i);
}
}
static int if_rx_thread(void *args)
{
ifnet *i = args;
cbuf *b;
if(i->fd < 0)
return -1;
for(;;) {
ssize_t len;
len = sys_read(i->fd, i->rx_buf, 0, sizeof(i->rx_buf));
#if NET_CHATTY
dprintf("if_rx_thread: got ethernet packet, size %Ld\n", (long long)len);
#endif
if(len < 0) {
thread_snooze(10000);
continue;
}
if(len == 0)
continue;
#if LOSE_RX_PACKETS
if(rand() % 100 < LOSE_RX_PERCENTAGE) {
dprintf("if_rx_thread: purposely lost packet, size %d\n", len);
continue;
}
#endif
// check to see if we have a link layer address attached to us
if(!i->link_addr) {
#if NET_CHATTY
dprintf("if_rx_thread: dumping packet because of no link address (%p)\n", i);
#endif
continue;
}
// for now just move it over into a cbuf
b = cbuf_get_chain(len);
if(!b) {
dprintf("if_rx_thread: could not allocate cbuf to hold ethernet packet\n");
continue;
}
cbuf_memcpy_to_chain(b, 0, i->rx_buf, len);
i->link_input(b, i);
}
return 0;
}
int
port_write_etc(port_id id,
int32 msg_code,
void *msg_buffer,
size_t buffer_size,
uint32 flags,
bigtime_t timeout)
{
int slot;
int res;
sem_id cached_semid;
int h;
cbuf* msg_store;
int c1, c2;
int err;
if(ports_active == false)
return ERR_PORT_NOT_ACTIVE;
if(id < 0)
return ERR_INVALID_HANDLE;
// mask irrelevant flags
flags = flags & (PORT_FLAG_USE_USER_MEMCPY | PORT_FLAG_INTERRUPTABLE | PORT_FLAG_TIMEOUT);
slot = id % MAX_PORTS;
// check buffer_size
if (buffer_size > PORT_MAX_MESSAGE_SIZE)
return ERR_INVALID_ARGS;
int_disable_interrupts();
GRAB_PORT_LOCK(ports[slot]);
if(ports[slot].id != id) {
RELEASE_PORT_LOCK(ports[slot]);
int_restore_interrupts();
dprintf("write_port_etc: invalid port_id %d\n", id);
return ERR_INVALID_HANDLE;
}
if (ports[slot].closed) {
RELEASE_PORT_LOCK(ports[slot]);
int_restore_interrupts();
dprintf("write_port_etc: port %d closed\n", id);
return ERR_PORT_CLOSED;
}
// store sem_id in local variable
cached_semid = ports[slot].write_sem;
RELEASE_PORT_LOCK(ports[slot]);
int_restore_interrupts();
// XXX -> possible race condition if port gets deleted (->sem deleted too),
// and queue is full therefore sem_id is cached in local variable up here
// get 1 entry from the queue, block if needed
// assumes flags
res = sem_acquire_etc(cached_semid, 1,
flags & (SEM_FLAG_TIMEOUT | SEM_FLAG_INTERRUPTABLE), timeout, NULL);
// XXX: possible race condition if port written by two threads...
// both threads will write 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_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;
}
if (buffer_size > 0) {
msg_store = cbuf_get_chain(buffer_size);
if (msg_store == NULL)
return ERR_NO_MEMORY;
if (flags & PORT_FLAG_USE_USER_MEMCPY) {
// copy from user memory
if ((err = cbuf_user_memcpy_to_chain(msg_store, 0, msg_buffer, buffer_size)) < 0)
return err; // memory exception
} else
// copy from kernel memory
if ((err = cbuf_memcpy_to_chain(msg_store, 0, msg_buffer, buffer_size)) < 0)
return err; // memory exception
} else {
msg_store = NULL;
}
// attach copied message to queue
int_disable_interrupts();
GRAB_PORT_LOCK(ports[slot]);
h = ports[slot].head;
if (h < 0)
panic("port %id: head < 0", ports[slot].id);
if (h >= ports[slot].capacity)
panic("port %id: head > cap %d", ports[slot].id, ports[slot].capacity);
ports[slot].msg_queue[h].msg_code = msg_code;
ports[slot].msg_queue[h].data_cbuf = msg_store;
ports[slot].msg_queue[h].data_len = buffer_size;
ports[slot].head = (ports[slot].head + 1) % ports[slot].capacity;
ports[slot].total_count++;
// store sem_id in local variable
cached_semid = ports[slot].read_sem;
RELEASE_PORT_LOCK(ports[slot]);
int_restore_interrupts();
sem_get_count(ports[slot].read_sem, &c1);
sem_get_count(ports[slot].write_sem, &c2);
// release sem, allowing read (might reschedule)
sem_release(cached_semid, 1);
return NO_ERROR;
}
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;
}
