void update_slots_used(squeue *q, uint32_t *bitmap, struct netmap_ring *ring) {
struct msg_transaction_update_data *msg_data;
uint16_t i;
uint32_t *mask;
size_t bitmap_blocks_per_data_block;
bitmap_blocks_per_data_block = (MSG_BLOCKSIZE * CHAR_BIT) / BITMAP_BLOCKSIZE;
mask = bitmap_new(ring->num_slots);
assert(mask);
// reconsitute the mask
for (i=0; i < BITMAP_BLOCKS(ring->num_slots); i++) {
if (i % bitmap_blocks_per_data_block == 0) {
msg_data = squeue_get_next_pop_slot(q);
assert(msg_data);
}
mask[i] = msg_data->data[i%bitmap_blocks_per_data_block];
}
bitmap_clearmask(bitmap, ring->num_slots, mask, ring->num_slots);
// reduce the range of slots in use as indicated by the bitmap
while (!bitmap_get(bitmap,
(ring->num_slots + ring->cur - ring->reserved) % ring->num_slots)) {
ring->reserved--;
if (!ring->reserved)
break;
}
}
int main() {
squeue *q;
int rc = 0;
int i;
int *p;
size_t n;
int d;
q = squeue_new(1024, 4);
if (!q) {
printf("squeue_new failed\n");
exit(EXIT_FAILURE);
}
uint32_t data[10] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 0 };
rc = squeue_enter(q, 1);
if (rc) {
for (p = squeue_get_next_push_slot(q), i=0;
p;
p = squeue_get_next_push_slot(q), i=(i+1)%10) {
*p = data[i];
}
squeue_exit(q);
}
rc = squeue_enter(q, 1);
if (rc) {
for (p = squeue_get_next_pop_slot(q); p; p = squeue_get_next_pop_slot(q)) {
printf("%u ", *p);
}
squeue_exit(q);
}
printf("\n********************\n");
do {
n = squeue_push_many(q, data, 1, 10);
} while (n == 10);
for (n = squeue_pop(q, &d, 1); n > 0; n = squeue_pop(q, &d, 0)) {
printf("%u ", d);
}
printf("\n********************\n");
exit(EXIT_SUCCESS);
}
void* squeue_trypop_slot(squeue *q) {
int rc;
assert(q);
rc = pthread_mutex_trylock(&q->lock);
if(rc != 0)
return NULL;
return squeue_get_next_pop_slot(q);
}
void *dispatcher(void *threadarg) {
assert(threadarg);
struct thread_context *context;
struct thread_context *contexts;
int rv;
struct netmap_ring *rxring;
struct ethernet_pkt *etherpkt;
struct pollfd pfd;
struct dispatcher_data *data;
uint32_t *slots_used, *open_transactions;
uint32_t i, arpd_idx, num_threads;
char *buf;
struct msg_hdr *msg;
struct ether_addr *mac;
context = (struct thread_context *)threadarg;
contexts = context->shared->contexts;
data = context->data;
arpd_idx = context->shared->arpd_idx;
mac = &context->shared->if_info->mac;
num_threads = context->shared->num_threads;
struct transaction *transactions[num_threads];
uint64_t dropped[num_threads];
for (i=0; i < num_threads; i++) {
transactions[i] = NULL;
dropped[i] = 0;
}
rv = dispatcher_init(context);
if (!rv) {
pthread_exit(NULL);
}
rxring = NETMAP_RXRING(data->nifp, 0);
slots_used = bitmap_new(rxring->num_slots);
if (!slots_used)
pthread_exit(NULL);
open_transactions = bitmap_new(num_threads);
if (!open_transactions)
pthread_exit(NULL);
pfd.fd = data->fd;
pfd.events = (POLLIN);
printf("dispatcher[%d]: initialized\n", context->thread_id);
// signal to main() that we are initialized
atomic_store_explicit(&context->initialized, 1, memory_order_release);
for (;;) {
rv = poll(&pfd, 1, POLL_TIMEOUT);
// read all packets from the ring
if (rv > 0) {
for (; rxring->avail > 0; rxring->avail--) {
i = rxring->cur;
rxring->cur = NETMAP_RING_NEXT(rxring, i);
rxring->reserved++;
buf = NETMAP_BUF(rxring, rxring->slot[i].buf_idx);
etherpkt = (struct ethernet_pkt *)(void *)buf;
// TODO: consider pushing this check to the workers
if (!ethernet_is_valid(etherpkt, mac)) {
if (rxring->reserved == 1)
rxring->reserved = 0;
continue;
}
// TODO: dispatch to n workers instead of just 0
switch (etherpkt->h.ether_type) {
case IP4_ETHERTYPE:
rv = tqueue_insert(contexts[0].pkt_recv_q,
&transactions[0], (char *) NULL + i);
switch (rv) {
case TQUEUE_SUCCESS:
bitmap_set(slots_used, i);
bitmap_set(open_transactions, 0);
break;
case TQUEUE_TRANSACTION_FULL:
bitmap_set(slots_used, i);
bitmap_clear(open_transactions, 0);
break;
case TQUEUE_FULL:
// just drop packet and do accounting
dropped[0]++;
if (rxring->reserved == 1)
rxring->reserved = 0;
break;
}
break;
case ARP_ETHERTYPE:
rv = tqueue_insert(contexts[arpd_idx].pkt_recv_q,
&transactions[arpd_idx], (char *) NULL + i);
switch (rv) {
case TQUEUE_SUCCESS:
tqueue_publish_transaction(contexts[arpd_idx].pkt_recv_q,
&transactions[arpd_idx]);
bitmap_set(slots_used, i);
break;
case TQUEUE_TRANSACTION_FULL:
bitmap_set(slots_used, i);
break;
case TQUEUE_FULL:
// just drop packet and do accounting
dropped[arpd_idx]++;
if (rxring->reserved == 1)
rxring->reserved = 0;
break;
}
break;
default:
printf("dispatcher[%d]: unknown/unsupported ethertype %hu\n",
context->thread_id, etherpkt->h.ether_type);
if (rxring->reserved == 1)
rxring->reserved = 0;
} // switch (ethertype)
} // for (rxring)
// publish any open transactions so that the worker can start on it
for (i=0; i < num_threads; i++) {
if (bitmap_get(open_transactions, i))
tqueue_publish_transaction(contexts[i].pkt_recv_q, &transactions[i]);
}
bitmap_clearall(open_transactions, num_threads);
} // if (packets)
// read the message queue
rv = squeue_enter(context->msg_q, 1);
if (!rv)
continue;
while ((msg = squeue_get_next_pop_slot(context->msg_q)) != NULL) {
switch (msg->msg_type) {
case MSG_TRANSACTION_UPDATE:
update_slots_used(context->msg_q, slots_used, rxring);
break;
case MSG_TRANSACTION_UPDATE_SINGLE:
update_slots_used_single((void *)msg, slots_used, rxring);
break;
default:
printf("dispatcher: unknown message %hu\n", msg->msg_type);
}
}
squeue_exit(context->msg_q);
} // for(;;)
pthread_exit(NULL);
}
// public function definitions
void *arpd(void *threadarg) {
assert(threadarg);
struct thread_context *context;
struct thread_context *contexts;
struct arpd_data *data;
struct in_addr *my_addr;
int rv;
struct transaction *transaction = NULL;
struct ethernet_pkt *etherpkt;
struct arp_pkt *arp;
struct netmap_ring *rxring;
void *ring_idx;
uint32_t dispatcher_idx;
struct msg_hdr *msg_hdr;
context = (struct thread_context *)threadarg;
contexts = context->shared->contexts;
data = context->data;
rxring = data->rxring;
dispatcher_idx = context->shared->dispatcher_idx;
my_addr = &context->shared->inet_info->addr;
rv = arpd_init(context);
if (!rv) {
pthread_exit(NULL);
}
printf("arpd[%d]: initialized\n", context->thread_id);
// signal to main() that we are initialized
atomic_store_explicit(&context->initialized, 1, memory_order_release);
// main event loop
for (;;) {
// read all the incoming packets
while (tqueue_remove(context->pkt_recv_q, &transaction,
&ring_idx) > 0) {
etherpkt = (struct ethernet_pkt *) NETMAP_BUF(rxring,
rxring->slot[(uint32_t)ring_idx].buf_idx);
arp = (struct arp_pkt*) etherpkt->data;
if (!arp_is_valid(arp)) {
send_msg_transaction_update_single(&contexts[dispatcher_idx],
(uint32_t)ring_idx);
continue;
}
if (arp->arp_h.ar_op == ARP_OP_REQUEST) {
if (arp->tpa.s_addr != my_addr->s_addr) {
send_msg_transaction_update_single(&contexts[dispatcher_idx],
(uint32_t) ring_idx);
continue;
}
printf("R)");
arp_print_line(arp);
// send_pkt_arp_reply could fail when xmit queue is full,
// however, the sender should just resend a request
send_pkt_arp_reply(context->pkt_xmit_q, &arp->spa, &arp->sha);
} else { // ARP_OP_REPLY
if (!arp_reply_filter(arp, my_addr)) {
send_msg_transaction_update_single(&contexts[dispatcher_idx],
(uint32_t) ring_idx);
continue;
}
printf("R)");
arp_print_line(arp);
// TODO: also check against a list of my outstanding arp requests
// prior to insertion in the arp cache
recv_pkt_arp_reply(arp, data->arp_cache, contexts);
}
send_msg_transaction_update_single(&contexts[dispatcher_idx],
(uint32_t) ring_idx);
} // while (packets)
// resend outstanding requests and refresh expiring entries
update_arp_cache(data->arp_cache, contexts, context->pkt_xmit_q);
// TODO: read all the messages
rv = squeue_enter(context->msg_q, 1);
if (!rv)
continue;
while ((msg_hdr = squeue_get_next_pop_slot(context->msg_q)) != NULL) {
switch (msg_hdr->msg_type) {
case MSG_ARPD_GET_MAC:
recv_msg_get_mac((void *)msg_hdr, data->arp_cache,
contexts, context->pkt_xmit_q);
break;
default:
printf("arpd: unknown message %hu\n", msg_hdr->msg_type);
}
}
squeue_exit(context->msg_q);
usleep(ARP_CACHE_RETRY_INTERVAL);
} // for (;;)
pthread_exit(NULL);
}
// public function definitions
void *worker(void *threadarg) {
assert(threadarg);
struct thread_context *context;
struct thread_context *contexts;
struct thread_context *dispatcher;
struct worker_data *data;
int rv;
struct transaction *transaction = NULL;
struct netmap_ring *rxring;
void *ring_idx;
uint32_t *slots_read;
pktbuff *pktbuff_in, *pktbuff_out;
int pktbuff_used = 1;
struct pcb *pcb;
struct msg_hdr *msg_hdr;
context = (struct thread_context *)threadarg;
contexts = context->shared->contexts;
data = context->data;
dispatcher = &contexts[context->shared->dispatcher_idx];
rxring = data->rxring;
slots_read = bitmap_new(rxring->num_slots);
if (!slots_read)
pthread_exit(NULL);
rv = worker_init(context);
if (!rv) {
pthread_exit(NULL);
}
printf("worker[%d]: initialized\n", context->thread_id);
// signal to main() that we are initialized
atomic_store_explicit(&context->initialized, 1, memory_order_release);
for (;;) {
if (pktbuff_used)
pktbuff_out = pktbuff_allocator_borrow(&data->pktbuff_allocator);
if (pktbuff_out) {
pktbuff_out->thread_id = context->thread_id;
pktbuff_used = 0;
// read all the incoming packets
while ((rv = tqueue_remove(context->pkt_recv_q, &transaction, &ring_idx))
!= TQUEUE_EMPTY) {
pktbuff_in = (void *)NETMAP_BUF(rxring,
rxring->slot[(uint32_t)ring_idx].buf_idx);
recv_pktbuff(pktbuff_in, pktbuff_out, &pcb, &pktbuff_used, context);
if (pktbuff_used) {
if (send_pktbuff(pktbuff_out, pcb, context, data) < 0)
pktbuff_used = 0;
}
bitmap_set(slots_read, (uint32_t)ring_idx);
if (rv == TQUEUE_TRANSACTION_EMPTY) {
send_msg_transaction_update(dispatcher, slots_read,
rxring->num_slots);
bitmap_clearall(slots_read, rxring->num_slots);
}
} // while (packets)
} // pktbuff_out
// read all the messages
// TODO: handle MSG_ARPD_GET_MAC_REPLY, MSG_PACKET_SENT
rv = squeue_enter(context->msg_q, 1);
if (!rv)
continue;
while ((msg_hdr = squeue_get_next_pop_slot(context->msg_q)) != NULL) {
switch (msg_hdr->msg_type) {
case MSG_PACKET_SENT:
pktbuff_allocator_return(&data->pktbuff_allocator,
((struct msg_packet_sent *)msg_hdr)->pktbuff);
break;
case MSG_ARPD_GET_MAC_REPLY: // for now, just ignore
break;
default:
printf("worker[%d]: unknown message %hu\n", context->thread_id,
msg_hdr->msg_type);
}
}
squeue_exit(context->msg_q);
usleep(1000);
} // for (;;)
pthread_exit(NULL);
}
