void runloop_idle(void *globals)
{
globals_t *g = (globals_t*)globals;
if (QUEUE_COUNT(&(g->packets_to_forward)) > 0)
dequeue_packet(g);
if (!g->option.hide_captured_packets_statistics)
print_stats(g);
}
/**
* Process tx and rx packets at the low-level interrupt.
*
* Should be called from the Stellaris Ethernet Interrupt Handler. This
* function will read packets from the Stellaris Ethernet fifo and place them
* into a pbuf queue. If the transmitter is idle and there is at least one packet
* on the transmit queue, it will place it in the transmit fifo and start the
* transmitter.
*
*/
void
stellarisif_interrupt(struct netif *netif)
{
struct ethernetif *ethernetif;
struct pbuf *p;
/* setup pointer to the if state data */
ethernetif = netif->state;
/**
* Process the transmit and receive queues as long as there is receive
* data available
*
*/
p = low_level_receive(netif);
while(p != NULL) {
/* Add the rx packet to the rx queue */
if(!enqueue_packet(p, ðernetif->rxq)) {
/* Could not place the packet on the queue, bail out. */
pbuf_free(p);
break;
}
/* Check if TX fifo is empty and packet available */
if((HWREG(ETH_BASE + MAC_O_TR) & MAC_TR_NEWTX) == 0) {
p = dequeue_packet(ðernetif->txq);
if(p != NULL) {
low_level_transmit(netif, p);
}
}
/* Read another packet from the RX fifo */
p = low_level_receive(netif);
}
/* One more check of the transmit queue/fifo */
if((HWREG(ETH_BASE + MAC_O_TR) & MAC_TR_NEWTX) == 0) {
p = dequeue_packet(ðernetif->txq);
if(p != NULL) {
low_level_transmit(netif, p);
}
}
}
static void
rate_limit_periodic_cb(void *rl_)
{
struct rate_limiter *rl = rl_;
int i;
/* Drain some packets out of the bucket if possible, but limit the number
* of iterations to allow other code to get work done too. */
refill_bucket(rl);
for (i = 0; rl->n_queued && get_token(rl) && i < 50; i++) {
/* Use a small, arbitrary limit for the amount of queuing to do here,
* because the TCP connection is responsible for buffering and there is
* no point in trying to transmit faster than the TCP connection can
* handle. */
struct ofpbuf *b = dequeue_packet(rl);
if (rconn_send_with_limit(rl->remote_rconn, b, &rl->n_txq, 10)) {
rl->n_tx_dropped++;
}
}
}
static void ether_do_interrupt(void)
{
// Call protocol handler for received packets
EthernetPacket ether_packet;
uint32 packet = ether_packet.addr();
ssize_t length;
for (;;) {
// Read packet from Ethernet device
length = dequeue_packet(Mac2HostAddr(packet));
if (length < 14)
break;
#if MONITOR
bug("Receiving Ethernet packet (%d bytes):\n",(int)length);
dump_packet( Mac2HostAddr(packet), length );
#endif
// Dispatch packet
ether_dispatch_packet(packet, length);
}
}
/**
* This function should be called when a packet is ready to be read
* from the interface. It uses the function low_level_input() that
* should handle the actual reception of bytes from the network
* interface. Then the type of the received packet is determined and
* the appropriate input function is called.
*
* @param netif the lwip network interface structure for this ethernetif
*/
int
stellarisif_input(struct netif *netif)
{
struct ethernetif *ethernetif;
struct pbuf *p;
int count = 0;
ethernetif = netif->state;
/* move received packet into a new pbuf */
while((p = dequeue_packet(ðernetif->rxq)) != NULL) {
count++;
/* process the packet. */
if (ethernet_input(p, netif)!=ERR_OK) {
LWIP_DEBUGF(NETIF_DEBUG, ("stellarisif_input: input error\n"));
pbuf_free(p);
p = NULL;
}
}
return(count);
}
int esp_mainloop(struct openconnect_info *vpninfo, int *timeout)
{
struct esp *esp = &vpninfo->esp_in[vpninfo->current_esp_in];
struct esp *old_esp = &vpninfo->esp_in[vpninfo->current_esp_in ^ 1];
struct pkt *this;
int work_done = 0;
int ret;
if (vpninfo->dtls_state == DTLS_SLEEPING) {
int when = vpninfo->new_dtls_started + vpninfo->dtls_attempt_period - time(NULL);
if (when <= 0 || vpninfo->dtls_need_reconnect) {
vpn_progress(vpninfo, PRG_DEBUG, _("Send ESP probes\n"));
esp_send_probes(vpninfo);
when = vpninfo->dtls_attempt_period;
}
if (*timeout > when * 1000)
*timeout = when * 1000;
}
if (vpninfo->dtls_fd == -1)
return 0;
while (1) {
int len = vpninfo->ip_info.mtu + vpninfo->pkt_trailer;
int i;
struct pkt *pkt;
if (!vpninfo->dtls_pkt) {
vpninfo->dtls_pkt = malloc(sizeof(struct pkt) + len);
if (!vpninfo->dtls_pkt) {
vpn_progress(vpninfo, PRG_ERR, _("Allocation failed\n"));
break;
}
}
pkt = vpninfo->dtls_pkt;
len = recv(vpninfo->dtls_fd, (void *)&pkt->esp, len + sizeof(pkt->esp), 0);
if (len <= 0)
break;
vpn_progress(vpninfo, PRG_TRACE, _("Received ESP packet of %d bytes\n"),
len);
work_done = 1;
if (len <= sizeof(pkt->esp) + 12)
continue;
len -= sizeof(pkt->esp) + 12;
pkt->len = len;
if (pkt->esp.spi == esp->spi) {
if (decrypt_esp_packet(vpninfo, esp, pkt))
continue;
} else if (pkt->esp.spi == old_esp->spi &&
ntohl(pkt->esp.seq) + esp->seq < vpninfo->old_esp_maxseq) {
vpn_progress(vpninfo, PRG_TRACE,
_("Consider SPI 0x%x, seq %u against outgoing ESP setup\n"),
(unsigned)ntohl(old_esp->spi), (unsigned)ntohl(pkt->esp.seq));
if (decrypt_esp_packet(vpninfo, old_esp, pkt))
continue;
} else {
vpn_progress(vpninfo, PRG_DEBUG,
_("Received ESP packet with invalid SPI 0x%08x\n"),
(unsigned)ntohl(pkt->esp.spi));
continue;
}
if (pkt->data[len - 1] != 0x04 && pkt->data[len - 1] != 0x29 &&
pkt->data[len - 1] != 0x05) {
vpn_progress(vpninfo, PRG_ERR,
_("Received ESP packet with unrecognised payload type %02x\n"),
pkt->data[len-1]);
continue;
}
if (len <= 2 + pkt->data[len - 2]) {
vpn_progress(vpninfo, PRG_ERR,
_("Invalid padding length %02x in ESP\n"),
pkt->data[len - 2]);
continue;
}
pkt->len = len - 2 - pkt->data[len - 2];
for (i = 0 ; i < pkt->data[len - 2]; i++) {
if (pkt->data[pkt->len + i] != i + 1)
break; /* We can't just 'continue' here because it
* would only break out of this 'for' loop */
}
if (i != pkt->data[len - 2]) {
vpn_progress(vpninfo, PRG_ERR,
_("Invalid padding bytes in ESP\n"));
continue; /* We can here, though */
}
vpninfo->dtls_times.last_rx = time(NULL);
if (pkt->len == 1 && pkt->data[0] == 0) {
if (vpninfo->dtls_state == DTLS_SLEEPING) {
vpn_progress(vpninfo, PRG_INFO,
_("ESP session established with server\n"));
queue_esp_control(vpninfo, 1);
vpninfo->dtls_state = DTLS_CONNECTING;
}
continue;
}
if (pkt->data[len - 1] == 0x05) {
struct pkt *newpkt = malloc(sizeof(*pkt) + vpninfo->ip_info.mtu + vpninfo->pkt_trailer);
int newlen = vpninfo->ip_info.mtu;
if (!newpkt) {
vpn_progress(vpninfo, PRG_ERR,
_("Failed to allocate memory to decrypt ESP packet\n"));
continue;
}
if (av_lzo1x_decode(newpkt->data, &newlen,
pkt->data, &pkt->len) || pkt->len) {
vpn_progress(vpninfo, PRG_ERR,
_("LZO decompression of ESP packet failed\n"));
free(newpkt);
continue;
}
newpkt->len = vpninfo->ip_info.mtu - newlen;
vpn_progress(vpninfo, PRG_TRACE,
_("LZO decompressed %d bytes into %d\n"),
len - 2 - pkt->data[len-2], newpkt->len);
queue_packet(&vpninfo->incoming_queue, newpkt);
} else {
queue_packet(&vpninfo->incoming_queue, pkt);
vpninfo->dtls_pkt = NULL;
}
}
if (vpninfo->dtls_state != DTLS_CONNECTED)
return 0;
switch (keepalive_action(&vpninfo->dtls_times, timeout)) {
case KA_REKEY:
vpn_progress(vpninfo, PRG_ERR, _("Rekey not implemented for ESP\n"));
break;
case KA_DPD_DEAD:
vpn_progress(vpninfo, PRG_ERR, _("ESP detected dead peer\n"));
queue_esp_control(vpninfo, 0);
esp_close(vpninfo);
esp_send_probes(vpninfo);
return 1;
case KA_DPD:
vpn_progress(vpninfo, PRG_DEBUG, _("Send ESP probes for DPD\n"));
esp_send_probes(vpninfo);
work_done = 1;
break;
case KA_KEEPALIVE:
vpn_progress(vpninfo, PRG_ERR, _("Keepalive not implemented for ESP\n"));
break;
case KA_NONE:
break;
}
unmonitor_write_fd(vpninfo, dtls);
while ((this = dequeue_packet(&vpninfo->outgoing_queue))) {
int len;
len = encrypt_esp_packet(vpninfo, this);
if (len > 0) {
ret = send(vpninfo->dtls_fd, (void *)&this->esp, len, 0);
if (ret < 0) {
/* Not that this is likely to happen with UDP, but... */
if (errno == ENOBUFS || errno == EAGAIN || errno == EWOULDBLOCK) {
monitor_write_fd(vpninfo, dtls);
/* XXX: Keep the packet somewhere? */
free(this);
return work_done;
} else {
/* A real error in sending. Fall back to TCP? */
vpn_progress(vpninfo, PRG_ERR,
_("Failed to send ESP packet: %s\n"),
strerror(errno));
}
} else {
vpninfo->dtls_times.last_tx = time(NULL);
vpn_progress(vpninfo, PRG_TRACE, _("Sent ESP packet of %d bytes\n"),
len);
}
} else {
/* XXX: Fall back to TCP transport? */
}
free(this);
work_done = 1;
}
return work_done;
}
/**
* Process tx and rx packets at the low-level interrupt.
*
* Should be called from the Stellaris Ethernet Interrupt Handler. This
* function will read packets from the Stellaris Ethernet fifo and place them
* into a pbuf queue. If the transmitter is idle and there is at least one packet
* on the transmit queue, it will place it in the transmit fifo and start the
* transmitter.
*
*/
void
stellarisif_interrupt(struct netif *netif)
{
struct stellarisif *stellarisif;
struct pbuf *p;
/* setup pointer to the if state data */
stellarisif = netif->state;
/**
* Process the transmit and receive queues as long as there is receive
* data available
*
*/
p = stellarisif_receive(netif);
while(p != NULL) {
/* process the packet */
#if NO_SYS
if(ethernet_input(p, netif)!=ERR_OK) {
#else
if(tcpip_input(p, netif)!=ERR_OK) {
#endif
/* drop the packet */
LWIP_DEBUGF(NETIF_DEBUG, ("stellarisif_input: input error\n"));
pbuf_free(p);
/* Adjust the link statistics */
LINK_STATS_INC(link.memerr);
LINK_STATS_INC(link.drop);
}
/* Check if TX fifo is empty and packet available */
if((HWREG(ETH_BASE + MAC_O_TR) & MAC_TR_NEWTX) == 0) {
p = dequeue_packet(&stellarisif->txq);
if(p != NULL) {
stellarisif_transmit(netif, p);
}
}
/* Read another packet from the RX fifo */
p = stellarisif_receive(netif);
}
/* One more check of the transmit queue/fifo */
if((HWREG(ETH_BASE + MAC_O_TR) & MAC_TR_NEWTX) == 0) {
p = dequeue_packet(&stellarisif->txq);
if(p != NULL) {
stellarisif_transmit(netif, p);
}
}
}
#if NETIF_DEBUG
/* Print an IP header by using LWIP_DEBUGF
* @param p an IP packet, p->payload pointing to the IP header
*/
void
stellarisif_debug_print(struct pbuf *p)
{
struct eth_hdr *ethhdr = (struct eth_hdr *)p->payload;
u16_t *plen = (u16_t *)p->payload;
LWIP_DEBUGF(NETIF_DEBUG, ("ETH header:\n"));
LWIP_DEBUGF(NETIF_DEBUG, ("Packet Length:%5"U16_F" \n",*plen));
LWIP_DEBUGF(NETIF_DEBUG, ("Destination: %02"X8_F"-%02"X8_F"-%02"X8_F"-%02"X8_F"-%02"X8_F"-%02"X8_F"\n",
ethhdr->dest.addr[0],
ethhdr->dest.addr[1],
ethhdr->dest.addr[2],
ethhdr->dest.addr[3],
ethhdr->dest.addr[4],
ethhdr->dest.addr[5]));
LWIP_DEBUGF(NETIF_DEBUG, ("Source: %02"X8_F"-%02"X8_F"-%02"X8_F"-%02"X8_F"-%02"X8_F"-%02"X8_F"\n",
ethhdr->src.addr[0],
ethhdr->src.addr[1],
ethhdr->src.addr[2],
ethhdr->src.addr[3],
ethhdr->src.addr[4],
ethhdr->src.addr[5]));
LWIP_DEBUGF(NETIF_DEBUG, ("Packet Type:0x%04"U16_F" \n", ethhdr->type));
}
void *worker_thread(void *dummyPtr) {
struct processor *me = NULL;
struct packet *thispacket = NULL;
struct session *thissession = NULL;
struct iphdr *iph = NULL;
struct tcphdr *tcph = NULL;
__u32 largerIP, smallerIP;
__u16 largerIPPort, smallerIPPort;
char *remoteID = NULL;
char message[LOGSZ];
qlz_state_compress *state_compress = (qlz_state_compress *) malloc(
sizeof(qlz_state_compress));
qlz_state_decompress *state_decompress = (qlz_state_decompress *) malloc(
sizeof(qlz_state_decompress));
me = (struct processor*) dummyPtr;
me->lzbuffer = calloc(1, BUFSIZE + 400);
/* Sharwan J: QuickLZ buffer needs (original data size + 400 bytes) buffer */
if (me->lzbuffer == NULL) {
sprintf(message, "Worker: Couldn't allocate buffer");
logger(LOG_INFO, message);
exit(1);
}
/*
* Register the worker threads metrics so they get updated.
*/
register_counter(counter_updateworkermetrics, (t_counterdata) & me->metrics);
if (me->lzbuffer != NULL) {
while (me->state >= STOPPING) {
thispacket = dequeue_packet(&me->queue, true);
if (thispacket != NULL) { // If a packet was taken from the queue.
iph = (struct iphdr *) thispacket->data;
tcph = (struct tcphdr *) (((u_int32_t *) iph) + iph->ihl);
if (DEBUG_WORKER == true) {
sprintf(message, "Worker: IP Packet length is: %u\n",
ntohs(iph->tot_len));
logger(LOG_INFO, message);
}
me->metrics.bytesin += ntohs(iph->tot_len);
//remoteID = (__u32) __get_tcp_option((__u8 *)iph,30);/* Check what IP address is larger. */
remoteID = get_nod_header_data((__u8 *)iph, ONOP).data;
sort_sockets(&largerIP, &largerIPPort, &smallerIP, &smallerIPPort,
iph->saddr,tcph->source,iph->daddr,tcph->dest);
if (DEBUG_WORKER == true) {
sprintf(message, "Worker: Searching for session.\n");
logger(LOG_INFO, message);
}
thissession = getsession(largerIP, largerIPPort, smallerIP,smallerIPPort);
if (thissession != NULL) {
if (DEBUG_WORKER == true) {
sprintf(message, "Worker: Found a session.\n");
logger(LOG_INFO, message);
}
if ((tcph->syn == 0) && (tcph->ack == 1) && (tcph->fin == 0)) {
if ((remoteID == NULL) || verify_neighbor_in_domain(remoteID) == false) {
/*
* An accelerator ID was NOT found.
* This is the first accelerator in the traffic path.
* This will soon be tested against a list of opennop neighbors.
* Traffic is sent through the optimize functions.
*/
saveacceleratorid(largerIP, (char*)get_opennop_id(), iph, thissession);
//binary_dump("worker.c IP Packet: ", (char*)iph, ntohs(iph->tot_len));
//__set_tcp_option((__u8 *)iph,30,6,localID); // Add the Accelerator ID to this packet.
set_nod_header_data((__u8 *)iph, ONOP, get_opennop_id(), OPENNOP_IPC_ID_LENGTH);
//binary_dump("worker.c IP Packet: ", (char*)iph, ntohs(iph->tot_len));
if ((((iph->saddr == largerIP) &&
//(thissession->larger.accelerator == localID) &&
(compare_opennopid((char*)&thissession->larger.accelerator, (char*)get_opennop_id()) == 1) &&
//(thissession->smaller.accelerator != 0) &&
(check_opennopid((char*)&thissession->smaller.accelerator) == 1) &&
//(thissession->smaller.accelerator != localID)) ||
(compare_opennopid((char*)&thissession->smaller.accelerator, (char*)get_opennop_id()) != 1))
||
((iph->saddr == smallerIP) &&
//(thissession->smaller.accelerator == localID) &&
(compare_opennopid((char*)&thissession->smaller.accelerator, (char*)get_opennop_id()) == 1) &&
//(thissession->larger.accelerator != 0) &&
(check_opennopid((char*)&thissession->larger.accelerator) == 1) &&
//(thissession->larger.accelerator != localID))) &&
(compare_opennopid((char*)&thissession->larger.accelerator, (char*)get_opennop_id()) != 1))) &&
(thissession->state == TCP_ESTABLISHED)) {
/*
* Do some acceleration!
*/
if (DEBUG_WORKER == true) {
sprintf(message, "Worker: Compressing packet.\n");
logger(LOG_INFO, message);
}
updateseq(largerIP, iph, tcph, thissession);
tcp_compress((__u8 *)iph, me->lzbuffer,state_compress);
} else {
updateseq(largerIP, iph, tcph, thissession);
if (DEBUG_WORKER == true) {
sprintf(message, "Worker: Not compressing packet.\n");
logger(LOG_INFO, message);
}
}
/*
* End of what should be the optimize function.
*/
} else if(verify_neighbor_in_domain(remoteID) == true) {
/*
* An accelerator ID WAS found.
* Traffic is sent through the de-optimize functions.
*/
saveacceleratorid(largerIP, remoteID, iph, thissession);
if (__get_tcp_option((__u8 *)iph,31) != 0) { // Packet is flagged as compressed.
if (DEBUG_WORKER == true) {
sprintf(message, "Worker: Packet is compressed.\n");
logger(LOG_INFO, message);
}
if (((iph->saddr == largerIP) &&
//(thissession->smaller.accelerator == localID)) ||
(compare_opennopid((char*)&thissession->smaller.accelerator, (char*)get_opennop_id()) == 1))||
((iph->saddr == smallerIP) &&
//(thissession->larger.accelerator == localID))) {
(compare_opennopid((char*)&thissession->larger.accelerator, (char*)get_opennop_id()) == 1))) {
/*
* Decompress this packet!
*/
if (tcp_decompress((__u8 *)iph, me->lzbuffer, state_decompress) == 0) { // Decompression failed if 0.
nfq_set_verdict(thispacket->hq, thispacket->id, NF_DROP, 0, NULL); // Decompression failed drop.
put_freepacket_buffer(thispacket);
thispacket = NULL;
}else{
updateseq(largerIP, iph, tcph, thissession); // Only update the sequence after decompression.
}
}
}else{
updateseq(largerIP, iph, tcph, thissession); // Also update sequences if packet is not optimized.
}
/*
* End of what should be the deoptimize function.
*/
}
}
if (tcph->rst == 1) { // Session was reset.
if (DEBUG_WORKER == true) {
sprintf(message, "Worker: Session was reset.\n");
logger(LOG_INFO, message);
}
thissession = clearsession(thissession);
}
/* Normal session closing sequence. */
if (tcph->fin == 1) {
thissession = closingsession(tcph, thissession);
}
if (thispacket != NULL) {
/*
* Changing anything requires the IP and TCP
* checksum to need recalculated.
*/
checksum(thispacket->data);
me->metrics.bytesout += ntohs(iph->tot_len);
nfq_set_verdict(thispacket->hq, thispacket->id, NF_ACCEPT, ntohs(iph->tot_len), (unsigned char *)thispacket->data);
put_freepacket_buffer(thispacket);
thispacket = NULL;
}
} /* End NULL session check. */
else { /* Session was NULL. */
me->metrics.bytesout += ntohs(iph->tot_len);
nfq_set_verdict(thispacket->hq, thispacket->id, NF_ACCEPT, 0, NULL);
put_freepacket_buffer(thispacket);
thispacket = NULL;
}
me->metrics.packets++;
} /* End NULL packet check. */
} /* End working loop. */
free(me->lzbuffer);
free(state_compress);
free(state_decompress);
me->lzbuffer = NULL;
}
return NULL;
}
