/* Execute a group entry of type INDIRECT. */
static void
execute_indirect(struct group_entry *entry, struct packet *pkt) {
if (entry->desc->buckets_num > 0) {
struct ofl_bucket *bucket = entry->desc->buckets[0];
struct packet *p = packet_clone(pkt);
if (VLOG_IS_DBG_ENABLED(LOG_MODULE)) {
char *b = ofl_structs_bucket_to_string(bucket, entry->dp->exp);
VLOG_DBG_RL(LOG_MODULE, &rl, "Writing bucket: %s.", b);
free(b);
}
action_set_write_actions(p->action_set, bucket->actions_num, bucket->actions);
entry->stats->byte_count += p->buffer->size;
entry->stats->packet_count++;
entry->stats->counters[0]->byte_count += p->buffer->size;
entry->stats->counters[0]->packet_count++;
action_set_execute(p->action_set, p);
packet_destroy(p);
} else {
VLOG_DBG_RL(LOG_MODULE, &rl, "No bucket in group.");
}
}
/* ************************************************** */
void rx(call_t *c, packet_t *packet) {
struct nodedata *nodedata = get_node_private_data(c);
struct _802_15_4_header *header = (struct _802_15_4_header *) packet->data;
array_t *up = get_entity_bindings_up(c);
int i = up->size;
/* cf p171 ref 802.15.4-2006: discard packet while in backoff */
if (nodedata->state != STATE_IDLE) {
packet_dealloc(packet);
return;
}
if ((header->dst != c->node) && (header->dst != BROADCAST_ADDR)) {
packet_dealloc(packet);
return;
}
while (i--) {
call_t c_up = {up->elts[i], c->node, c->entity};
packet_t *packet_up;
if (i > 0) {
packet_up = packet_clone(packet);
} else {
packet_up = packet;
}
RX(&c_up, packet_up);
}
return;
}
/* Execute a group entry of type FAILFAST. */
static void
execute_ff(struct group_entry *entry, struct packet *pkt) {
size_t b = select_from_ff_group(entry);
if (b != -1) {
struct ofl_bucket *bucket = entry->desc->buckets[b];
struct packet *p = packet_clone(pkt);
if (VLOG_IS_DBG_ENABLED(LOG_MODULE)) {
char *b = ofl_structs_bucket_to_string(bucket, entry->dp->exp);
VLOG_DBG_RL(LOG_MODULE, &rl, "Writing bucket: %s.", b);
free(b);
}
action_set_write_actions(p->action_set, bucket->actions_num, bucket->actions);
entry->stats->byte_count += p->buffer->size;
entry->stats->packet_count++;
entry->stats->counters[b]->byte_count += p->buffer->size;
entry->stats->counters[b]->packet_count++;
/* Cookie field is set 0xffffffffffffffff
because we cannot associate to any
particular flow */
action_set_execute(p->action_set, p, 0xffffffffffffffff);
packet_destroy(p);
} else {
VLOG_DBG_RL(LOG_MODULE, &rl, "No bucket in group.");
}
}
/* ************************************************** */
void tx(call_t *c, packet_t *packet) {
struct nodedata *nodedata = get_node_private_data(c);
array_t *down = get_entity_bindings_down(c);
int i = down->size;
/* radio sleep */
if (nodedata->sleep) {
packet_dealloc(packet);
return;
}
/* radio activity */
cs_init(c);
nodedata->tx_busy = packet->id;
/* log tx */
PRINT_REPLAY("radio-tx0 %"PRId64" %d 50\n", get_time(), c->node);
/* transmit to antenna */
while (i--) {
packet_t *packet_down;
if (i > 0) {
packet_down = packet_clone(packet);
} else {
packet_down = packet;
}
c->from = down->elts[i];
MEDIA_TX(c, packet_down);
}
return;
}
/* ************************************************** */
void rx(call_t *c, packet_t *packet) {
struct nodedata *nodedata = get_node_private_data(c);
array_t *up = get_entity_bindings_up(c);
int i = up->size;
struct routing_header *header = (struct routing_header *)
(packet->data + nodedata->overhead);
/* Forward packet if node is not the recipient */
if ((header->dst != BROADCAST_ADDR) && (header->dst != c->node) ) {
forward(c, packet);
return;
}
while (i--) {
call_t c_up = {up->elts[i], c->node, c->entity};
packet_t *packet_up;
if (i > 0) {
packet_up = packet_clone(packet);
} else {
packet_up = packet;
}
RX(&c_up, packet_up);
}
return;
}
/* Executes a group entry of type ALL. */
static void
execute_all(struct group_entry *entry, struct packet *pkt) {
size_t i;
/* TODO Zoltan: Currently packets are always cloned. However it should
* be possible to see if cloning is necessary, or not, based on bucket actions. */
for (i=0; i<entry->desc->buckets_num; i++) {
struct ofl_bucket *bucket = entry->desc->buckets[i];
struct packet *p = packet_clone(pkt);
if (VLOG_IS_DBG_ENABLED(LOG_MODULE)) {
char *b = ofl_structs_bucket_to_string(bucket, entry->dp->exp);
VLOG_DBG_RL(LOG_MODULE, &rl, "Writing bucket: %s.", b);
free(b);
}
action_set_write_actions(p->action_set, bucket->actions_num, bucket->actions);
entry->stats->byte_count += p->buffer->size;
entry->stats->packet_count++;
entry->stats->counters[i]->byte_count += p->buffer->size;
entry->stats->counters[i]->packet_count++;
/* Cookie field is set 0xffffffffffffffff
because we cannot associate to any
particular flow */
action_set_execute(p->action_set, p, 0xffffffffffffffff);
packet_destroy(p);
}
}
/* ************************************************** */
void rx(call_t *c, packet_t *packet) {
struct nodedata *nodedata = get_node_private_data(c);
array_t *up = get_entity_bindings_up(c);
int i = up->size;
/* radio sleep */
if (nodedata->sleep) {
packet_dealloc(packet);
return;
}
/* half-duplex */
if (nodedata->tx_busy != -1) {
packet_dealloc(packet);
return;
}
/* handle carrier sense */
if (nodedata->rx_busy == packet->id) {
nodedata->rx_busy = -1;
nodedata->rxdBm = MIN_DBM;
/* log rx */
PRINT_REPLAY("radio-rx1 %"PRId64" %d\n", get_time(), c->node);
/* consume energy */
battery_consume_rx(c, packet->duration);
} else {
packet_dealloc(packet);
return;
}
/* check wether the reception has killed us */
if (!is_node_alive(c->node)) {
packet_dealloc(packet);
return;
}
/* drop packet depending on the FER */
if (get_random_double() < packet->PER) {
packet_dealloc(packet);
return;
}
/* forward to upper layers */
while (i--) {
call_t c_up = {up->elts[i], c->node, c->entity};
packet_t *packet_up;
if (i > 0) {
packet_up = packet_clone(packet);
} else {
packet_up = packet;
}
RX(&c_up, packet_up);
}
return;
}
/* ************************************************** */
void forward(call_t *c, packet_t *packet) {
struct nodedata *nodedata = get_node_private_data(c);
call_t c0 = {get_entity_bindings_down(c)->elts[0], c->node, c->entity};
struct routing_header *header = (struct routing_header *) (packet->data + nodedata->overhead);
struct neighbor *n_hop = get_nexthop(c, &(header->dst_pos));
destination_t destination;
/* delivers packet to application layer */
if (n_hop == NULL) {
array_t *up = get_entity_bindings_up(c);
int i = up->size;
while (i--) {
call_t c_up = {up->elts[i], c->node, c->entity};
packet_t *packet_up;
if (i > 0) {
packet_up = packet_clone(packet);
} else {
packet_up = packet;
}
RX(&c_up, packet_up);
}
return;
}
/* update hop count */
header->hop--;
if (header->hop == 0) {
nodedata->data_hop++;
packet_dealloc(packet);
return;
}
/* set mac header */
destination.id = n_hop->id;
destination.position.x = -1;
destination.position.y = -1;
destination.position.z = -1;
if (SET_HEADER(&c0, packet, &destination) == -1) {
packet_dealloc(packet);
return;
}
/* forwarding packet */
nodedata->data_tx++;
TX(&c0, packet);
}
static packet_t wpk_clone(packet_t p){
if(packet_type(p) == WPACKET){
wpacket_t _w = (wpacket_t)p;
wpacket_t w = (wpacket_t)CALLOC(g_wpk_allocator,1,sizeof(*w));//calloc(1,sizeof(*w));
//w->writebuf = NULL;
//w->len = NULL;
packet_begpos(w) = packet_begpos(_w);
packet_buf(w) = buffer_acquire(NULL,packet_buf(_w));
//packet_next(w) = NULL;
packet_type(w) = WPACKET;
packet_datasize(w) = packet_datasize(_w);
packet_clone(w) = wpk_clone;
packet_makeforwrite(w) = wpk_clone;
packet_makeforread(w) = wpk_makeforread;
return (packet_t)w;
}else
return NULL;
}
void rx(call_t *c, packet_t *packet) {
array_t *up = get_entity_bindings_up(c);
int i = up->size;
while (i--) {
call_t c_up = {up->elts[i], c->node, c->entity};
packet_t *packet_up;
if (i > 0) {
packet_up = packet_clone(packet);
} else {
packet_up = packet;
}
RX(&c_up, packet_up);
}
return;
}
packet_t rpk_makeforwrite(packet_t p){
if(packet_type(p) == RPACKET){
rpacket_t r = (rpacket_t)p;
wpacket_t w = (wpacket_t)CALLOC(g_wpk_allocator,1,sizeof(*w));//calloc(1,sizeof(*w));
//w->writebuf = NULL;
//w->len = NULL;//触发拷贝之前len没有作用
//w->wpos = 0;
//packet_next(w) = NULL;
packet_type(w) = WPACKET;
packet_begpos(w) = packet_begpos(r);
packet_buf(w) = buffer_acquire(NULL,packet_buf(r));
packet_datasize(w) = kn_ntoh32(r->len) + sizeof(r->len);
packet_clone(w) = wpk_clone;
packet_makeforwrite(w) = wpk_clone;
packet_makeforread(w) = wpk_makeforread;
return (packet_t)w;
}else
return NULL;
}
int node_disconnect(struct node *node)
{
int i, ret;
struct device *dev;
struct packet *pkt, *notify;
struct daemon *daemon = node->daemon;
log_error("%s: disconnect node %d\n", __func__, node->id);
pkt = alloc_packet0();
if (!pkt) {
log_error("Error: %s alloc packet failed", __func__);
return -ENOMEM;
}
pkt->type = P_KMOD_DISCONN;
pkt->dev_id = MAX_DEVICES;
pkt->node_from = node->id;
pkt->kmod_from = INVALID_ID; //FIXME 是否导入了不一致?
pkt->node_to = daemon->local_node->id;
for (i = 0; i < daemon->dev_list->dev_num; i++) {
dev = daemon->dev_list->devs[i];
if (!device_want_recv(dev))
continue;
notify = packet_clone(pkt);
if (!notify) {
log_error("alloc disconnect notify packet failed.");
ret = -ENOMEM;
goto out;
}
notify->kmod_to = (1 << dev->id);
ret = dev_put_meta_packet(dev, notify);
if (ret < 0) {
log_error("Error: put meta packet failed.");
goto out;
}
}
out:
free_packet(pkt);
return ret;
}
wpacket_t wpk_create(uint32_t size)
{
size = size_of_pow2(size);
if(size < 64) size = 64;
wpacket_t w = (wpacket_t)CALLOC(g_wpk_allocator,1,sizeof(*w));//calloc(1,sizeof(*w));
packet_buf(w) = buffer_create(size);
w->writebuf = packet_buf(w);//buffer_acquire(NULL,packet_buf(w));
//packet_begpos(w)= 0;
//packet_next(w) = NULL;
packet_type(w) = WPACKET;
w->wpos = sizeof(*(w->len));
w->len = (uint32_t*)packet_buf(w)->buf;
*(w->len) = 0;
packet_buf(w)->size = sizeof(*(w->len));
packet_datasize(w) = sizeof(*(w->len));
packet_clone(w) = wpk_clone;
packet_makeforwrite(w) = wpk_clone;
packet_makeforread(w) = wpk_makeforread;
return w;
}
wpacket_t wpk_create_by_bin(int8_t *addr,uint32_t size)
{
uint32_t datasize = size;
size = size_of_pow2(size);
if(size < 64) size = 64;
wpacket_t w = (wpacket_t)CALLOC(g_wpk_allocator,1,sizeof(*w));//calloc(1,sizeof(*w));
packet_buf(w) = buffer_create(size);
w->writebuf = packet_buf(w);//buffer_acquire(NULL,packet_buf(w));
//packet_begpos(w)= 0;
//packet_next(w) = NULL;
packet_type(w) = WPACKET;
memcpy(&w->writebuf->buf[0],addr,datasize);
w->len = (uint32_t*)packet_buf(w)->buf;
w->wpos = datasize;
packet_buf(w)->size = datasize;
packet_datasize(w) = datasize;
packet_clone(w) = wpk_clone;
packet_makeforwrite(w) = wpk_clone;
packet_makeforread(w) = wpk_makeforread;
return w;
}
/* ************************************************** */
void rx(call_t *c, packet_t *packet) {
struct nodedata *nodedata = get_node_private_data(c);
array_t *up = get_entity_bindings_up(c);
int i = up->size;
struct routing_header *header = (struct routing_header *) (packet->data + nodedata->overhead);
switch(header->type) {
case HELLO_PACKET:
nodedata->hello_rx++;
add_neighbor(c, header);
packet_dealloc(packet);
break;
case DATA_PACKET :
nodedata->data_rx++;
if ((header->dst != BROADCAST_ADDR) && (header->dst != c->node) ) {
forward(c, packet);
return;
}
while (i--) {
call_t c_up = {up->elts[i], c->node, c->entity};
packet_t *packet_up;
if (i > 0) {
packet_up = packet_clone(packet);
} else {
packet_up = packet;
}
RX(&c_up, packet_up);
}
break;
default :
break;
}
return;
}
int packet_stream_process_packet(struct packet_stream *stream, struct packet *pkt, struct proto_process_stack *stack, unsigned int stack_index, uint32_t seq, uint32_t ack) {
if (!stream || !pkt || !stack)
return PROTO_ERR;
debug_stream("thread %p, entry %p, packet %u.%06u, seq %u, ack %u : start", pthread_self(), stream, pkt->ts.tv_sec, pkt->ts.tv_usec, seq, ack);
struct proto_process_stack *cur_stack = &stack[stack_index];
int direction = cur_stack->direction;
int must_wait = 0;
pom_mutex_lock(&stream->wait_lock);
int res = pthread_mutex_trylock(&stream->lock);
if (res == EBUSY) {
// Already locked, let's wait a bit
must_wait = 1;
} else if (res) {
pomlog(POMLOG_ERR "Error while locking packet stream lock : %s", pom_strerror(errno));
abort();
return POM_ERR;
} else {
// We got the processing lock. But was it really this thread's turn ?
struct packet_stream_thread_wait *tmp = stream->wait_list_head;
// A thread with a packet preceding ours is waiting
if (tmp && (tmp->ts.tv_sec < pkt->ts.tv_sec || (tmp->ts.tv_sec == pkt->ts.tv_sec && tmp->ts.tv_usec < pkt->ts.tv_usec))) {
// No it wasn't, release it and signal the right thread
must_wait = 2;
pom_mutex_unlock(&stream->lock);
debug_stream("thread %p, entry %p : signaling thread %p", pthread_self(), stream, stream->wait_list_head->thread);
pthread_cond_broadcast(&stream->wait_list_head->cond);
} else {
// Yes it was. YAY !
pom_mutex_unlock(&stream->wait_lock);
}
}
if (must_wait) {
// Add ourself in the waiting list
struct packet_stream_thread_wait *lst = NULL;
if (stream->wait_list_unused) {
lst = stream->wait_list_unused;
stream->wait_list_unused = lst->next;
lst->next = NULL;
} else {
lst = malloc(sizeof(struct packet_stream_thread_wait));
if (!lst) {
pom_oom(sizeof(struct packet_stream_thread_wait));
pom_mutex_unlock(&stream->wait_lock);
return POM_ERR;
}
memset(lst, 0, sizeof(struct packet_stream_thread_wait));
if (pthread_cond_init(&lst->cond, NULL)) {
pomlog(POMLOG_ERR "Error while initializing wait list condition : %s", pom_strerror(errno));
free(lst);
return POM_ERR;
}
}
memcpy(&lst->ts, &pkt->ts, sizeof(struct timeval));
lst->thread = pthread_self();
struct packet_stream_thread_wait *tmp;
for (tmp = stream->wait_list_head; tmp && (tmp->ts.tv_sec < lst->ts.tv_sec || (tmp->ts.tv_sec == lst->ts.tv_sec && tmp->ts.tv_usec < lst->ts.tv_usec)); tmp = tmp->next);
if (tmp) {
lst->prev = tmp->prev;
if (lst->prev)
lst->prev->next = lst;
else
stream->wait_list_head = lst;
lst->next = tmp;
lst->next->prev = lst;
} else {
lst->prev = stream->wait_list_tail;
if (lst->prev)
lst->prev->next = lst;
else
stream->wait_list_head = lst;
stream->wait_list_tail = lst;
}
while (1) {
debug_stream("thread %p, entry %p, packet %u.%06u, seq %u, ack %u : waiting", pthread_self(), stream, pkt->ts.tv_sec, pkt->ts.tv_usec, seq, ack);
if (pthread_cond_wait(&lst->cond, &stream->wait_lock)) {
pomlog(POMLOG_ERR "Error while waiting for the packet stream wait cond : %s", pom_strerror(errno));
abort();
return POM_ERR;
}
if (stream->wait_list_head != lst) {
// There is a small chance that another stream lock stream->wait_lock while pthread_cond_wait acquires it
// If we are not the right thread, then simply signal the right one and wait again for our turn
debug_stream("thread %p, entry %p, packet %u.%06u, seq %u, ack %u : wrong thread woke up", pthread_self(), stream, pkt->ts.tv_sec, pkt->ts.tv_usec, seq, ack);
pthread_cond_broadcast(&stream->wait_list_head->cond);
continue;
}
break;
}
tmp = stream->wait_list_head;
stream->wait_list_head = tmp->next;
if (stream->wait_list_head)
stream->wait_list_head->prev = NULL;
else
stream->wait_list_tail = NULL;
tmp->next = stream->wait_list_unused;
tmp->prev = NULL;
stream->wait_list_unused = tmp;
pom_mutex_unlock(&stream->wait_lock);
pom_mutex_lock(&stream->lock);
}
debug_stream("thread %p, entry %p, packet %u.%06u, seq %u, ack %u : start locked", pthread_self(), stream, pkt->ts.tv_sec, pkt->ts.tv_usec, seq, ack);
// Update the stream flags
if (stream->flags & PACKET_FLAG_STREAM_BIDIR) {
// Update flags
if (direction == POM_DIR_FWD && !(stream->flags & PACKET_FLAG_STREAM_GOT_FWD_DIR)) {
stream->flags |= PACKET_FLAG_STREAM_GOT_FWD_DIR;
} else if (direction == POM_DIR_REV && !(stream->flags & PACKET_FLAG_STREAM_GOT_REV_DIR)) {
stream->flags |= PACKET_FLAG_STREAM_GOT_REV_DIR;
}
}
// Put this packet in our struct packet_stream_pkt
struct packet_stream_pkt spkt = {0};
spkt.pkt = pkt;
spkt.seq = seq;
spkt.ack = ack;
spkt.plen = cur_stack->plen;
spkt.stack = stack;
spkt.stack_index = stack_index;
// Check if the packet is worth processing
uint32_t cur_seq = stream->cur_seq[direction];
if (cur_seq != seq) {
if (packet_stream_is_packet_old_dupe(stream, &spkt, direction)) {
// cur_seq is after the end of the packet, discard it
packet_stream_end_process_packet(stream);
debug_stream("thread %p, entry %p, packet %u.%06u, seq %u, ack %u : discard", pthread_self(), stream, pkt->ts.tv_sec, pkt->ts.tv_usec, seq, ack);
return PROTO_OK;
}
if (packet_stream_remove_dupe_bytes(stream, &spkt, direction) == POM_ERR) {
packet_stream_end_process_packet(stream);
return PROTO_ERR;
}
}
// Ok let's process it then
// Check if it is the packet we're waiting for
if (packet_stream_is_packet_next(stream, &spkt, direction)) {
// Process it
stream->cur_seq[direction] += cur_stack->plen;
stream->cur_ack[direction] = ack;
debug_stream("thread %p, entry %p, packet %u.%06u, seq %u, ack %u : process", pthread_self(), stream, pkt->ts.tv_sec, pkt->ts.tv_usec, seq, ack);
int res = stream->handler(stream->ce, pkt, stack, stack_index);
if (res == PROTO_ERR) {
packet_stream_end_process_packet(stream);
return PROTO_ERR;
}
// Check if additional packets can be processed
struct packet_stream_pkt *p = NULL;
unsigned int cur_dir = direction, additional_processed = 0;
while ((p = packet_stream_get_next(stream, &cur_dir))) {
debug_stream("thread %p, entry %p, packet %u.%06u, seq %u, ack %u : process additional", pthread_self(), stream, p->pkt->ts.tv_sec, p->pkt->ts.tv_usec, p->seq, p->ack);
if (stream->handler(stream->ce, p->pkt, p->stack, p->stack_index) == POM_ERR) {
packet_stream_end_process_packet(stream);
return PROTO_ERR;
}
stream->cur_seq[cur_dir] += p->plen;
stream->cur_ack[cur_dir] = p->ack;
packet_stream_free_packet(p);
additional_processed = 1;
}
if (additional_processed) {
if (!stream->head[POM_DIR_FWD] && !stream->head[POM_DIR_REV])
conntrack_timer_dequeue(stream->t);
else
conntrack_timer_queue(stream->t, stream->same_dir_timeout);
}
packet_stream_end_process_packet(stream);
debug_stream("thread %p, entry %p, packet %u.%06u, seq %u, ack %u : done processed", pthread_self(), stream, pkt->ts.tv_sec, pkt->ts.tv_usec, seq, ack);
return res;
}
// Queue the packet then
debug_stream("thread %p, entry %p, packet %u.%06u, seq %u, ack %u : queue", pthread_self(), stream, pkt->ts.tv_sec, pkt->ts.tv_usec, seq, ack);
struct packet_stream_pkt *p = malloc(sizeof(struct packet_stream_pkt));
if (!p) {
pom_oom(sizeof(struct packet_stream_pkt));
packet_stream_end_process_packet(stream);
return PROTO_ERR;
}
memset(p, 0 , sizeof(struct packet_stream_pkt));
if (cur_stack->plen) {
// No need to backup this if there is no payload
p->pkt = packet_clone(pkt, stream->flags);
if (!p->pkt) {
packet_stream_end_process_packet(stream);
free(p);
return PROTO_ERR;
}
p->stack = core_stack_backup(stack, pkt, p->pkt);
if (!p->stack) {
packet_stream_end_process_packet(stream);
packet_pool_release(p->pkt);
free(p);
return PROTO_ERR;
}
}
p->plen = cur_stack->plen;
p->seq = seq;
p->ack = ack;
p->stack_index = stack_index;
if (!stream->tail[direction]) {
stream->head[direction] = p;
stream->tail[direction] = p;
} else {
struct packet_stream_pkt *tmp = stream->tail[direction];
while ( tmp &&
((tmp->seq >= seq && tmp->seq - seq < PACKET_HALF_SEQ)
|| (tmp->seq <= seq && seq - tmp->seq > PACKET_HALF_SEQ))) {
tmp = tmp->prev;
}
if (!tmp) {
// Packet goes at the begining of the list
p->next = stream->head[direction];
if (p->next)
p->next->prev = p;
else
stream->tail[direction] = p;
stream->head[direction] = p;
} else {
// Insert the packet after the current one
p->next = tmp->next;
p->prev = tmp;
if (p->next)
p->next->prev = p;
else
stream->tail[direction] = p;
tmp->next = p;
}
}
stream->cur_buff_size += cur_stack->plen;
if (stream->cur_buff_size >= stream->max_buff_size) {
// Buffer overflow
debug_stream("thread %p, entry %p, packet %u.%06u, seq %u, ack %u : buffer overflow, forced dequeue", pthread_self(), stream, pkt->ts.tv_sec, pkt->ts.tv_usec, seq, ack);
if (packet_stream_force_dequeue(stream) != POM_OK) {
packet_stream_end_process_packet(stream);
return POM_ERR;
}
if (stream->t)
conntrack_timer_dequeue(stream->t);
}
// Add timeout
if (stream->t && (stream->head[POM_DIR_FWD] || stream->head[POM_DIR_REV]))
conntrack_timer_queue(stream->t, stream->same_dir_timeout);
packet_stream_end_process_packet(stream);
debug_stream("thread %p, entry %p, packet %u.%06u, seq %u, ack %u : done queued", pthread_self(), stream, pkt->ts.tv_sec, pkt->ts.tv_usec, seq, ack);
return PROTO_OK;
}
/* 从对端 server 的 data 链路收到数据，把它放到对应的 kmod 节点中 */
void node_data_handler(evutil_socket_t fd, short event, void *args)
{
struct node *node;
struct daemon *daemon;
struct device_list *dev_list;
struct device *dev;
struct packet *pkt;
struct packet *pkt_clone;
int i, ret, full;
cb_fn *cb;
node = (struct node *)args;
daemon = node->daemon;
dev_list = daemon->dev_list;
pkt = handler_recv(node->dfd);
if (!pkt) {
node_del_data_event(node);
log_error("receive data from server node %d failed", node->id);
return;
}
/* FIXME 如果某个device的queue满了，在这里就会等待，从而造成两个问题：
* 1. 这是event的call back函数，如果等待会造成event base的阻塞
* 2. 单个device队列满而影响到所有其它device，不公平
* 是否可以这样？
* 提前获取队列一个空位，如果失败的话，直接返回，
* 当队列有剩余空间时再加入该event
* 但是，device互相影响的问题还是没能解决，满了直接断连？
*/
full = 0;
for (i = 0; i < dev_list->dev_num; i++) {
//FIXME 查找效率很低，可考虑序号对应下标，并且判读是否是只有单个device
dev = dev_list->devs[i];
if (!device_is_target(dev, pkt->kmod_to))
continue;
if (!device_want_recv(dev))
continue;
pkt_clone = packet_clone(pkt);
if (!pkt_clone) {
log_error("send to kmod node %d failed: no memory", dev->id);
free_packet(pkt_clone);
goto out;
}
cb = full ? NULL : node_data_handle_force_cb;
ret = dev_put_data_packet_force(dev, pkt_clone, cb, node);
if (ret < 0) {
log_error("kmod node %d data queue is NOT start", dev->id);
goto out;
} else if (ret > 0) {
log_info("%s force handle for node %d", __func__, node->id);
full = 1;
}
}
out:
free_packet(pkt);
if (!full) {
ret = event_add(node->data_event, NULL);
if (ret < 0) {
log_error("failed to add data event on node %d", node->id);
node_del_data_event(node);
}
}
}
int packet_multipart_add_packet(struct packet_multipart *multipart, struct packet *pkt, size_t offset, size_t len, size_t pkt_buff_offset) {
struct packet_multipart_pkt *tmp = multipart->tail;
// Check where to add the packet
while (tmp) {
if (tmp->offset + tmp->len <= offset)
break; // Packet is after is one
if (tmp->offset == offset) {
if (tmp->len != len)
pomlog(POMLOG_WARN "Size missmatch for packet already in the buffer");
return POM_OK;
}
tmp = tmp->prev;
}
struct packet_multipart_pkt *res = malloc(sizeof(struct packet_multipart_pkt));
if (!res) {
pom_oom(sizeof(struct packet_multipart_pkt));
return POM_ERR;
}
memset(res, 0, sizeof(struct packet_multipart_pkt));
res->offset = offset;
res->pkt_buff_offset = pkt_buff_offset;
res->len = len;
// Copy the packet
res->pkt = packet_clone(pkt, multipart->flags);
if (!res->pkt) {
free(res);
return POM_ERR;
}
multipart->cur += len;
if (tmp) {
// Packet is after this one, add it
res->prev = tmp;
res->next = tmp->next;
tmp->next = res;
if (res->next) {
res->next->prev = res;
if ((res->next->offset == res->offset + res->len) &&
(res->prev->offset + res->prev->len == res->offset))
// A gap was filled
multipart->gaps--;
else if ((res->next->offset > res->offset + res->len) &&
(res->prev->offset + res->prev->len < res->offset))
// A gap was created
multipart->gaps++;
} else {
if (tmp->offset + tmp->len < res->offset)
multipart->gaps++;
multipart->tail = res;
}
return POM_OK;
} else {
// Add it at the head
res->next = multipart->head;
if (res->next)
res->next->prev = res;
else
multipart->tail = res;
multipart->head = res;
if (res->offset) {
// There is a gap at the begining
multipart->gaps++;
} else if (res->next && res->len == res->next->offset) {
// Gap filled
multipart->gaps--;
}
}
return POM_OK;
}
/* ************************************************** */
void rx(call_t *c, packet_t *packet) {
if(!is_node_alive(c->node))
return;
struct nodedata *nodedata = get_node_private_data(c);
struct protocoleData *entitydata = get_entity_private_data(c);
array_t *up = get_entity_bindings_up(c);
packet_PROTOCOLE *data = (packet_PROTOCOLE *) (packet->data + nodedata->overhead);
switch(data->type)
{
case HELLO:
{
//printf("BIP - Paquet de type HELLO recu par %d depuis %d\n", c->node, data->src);
rx_hello(c,packet);
break;
}
case HELLO2:
{
//printf("BIP - Paquet de type HELLO2 recu par %d depuis %d\n", c->node, data->src);
rx_two_hop(c,packet);
break;
}
case APP:
{
//printf("DLBIP - Paquet de type APP recu par %d depuis %d ; source : %d et destine a %d\n", c->node, data->pred, data->src, data->dst);
nodedata->energiesRem[data->pred] = data->energyRem;
if(nodedata->lastIDs[data->src] == data->id || data->src == c->node)
{
break;
}
else
{
nodedata->lastIDs[data->src] = data->id;
}
if(data->dst == BROADCAST_ADDR)
{
SHOW_GRAPH("G: %d %d\n",data->pred,c->node);
double cout;
listeNodes* trans = data->askedToRedirect;
listeNodes* trans2 = data->needsToBeCovered;
while(trans != 0)
{
position_t p1, p2;
p1.x = trans->values.x;
p1.y = trans->values.y;
p1.z = trans->values.z;
p2.x = trans2->values.x;
p2.y = trans2->values.y;
p2.z = trans2->values.z;
cout = getCoutFromDistance(distance(&p1, &p2), entitydata->alpha, entitydata->c);
//printf("cout estime de (%d,%d)\n", );
nodedata->energiesRem[trans->values.node] -= cout;
trans = trans->suiv;
trans2 = trans2->suiv;
}
//SHOW_GRAPH("G: %d %d\n",data->pred,c->node);
// faire remonter le paquet a la couche application
call_t c_up = {up->elts[0], c->node, c->entity};
RX(&c_up, packet_clone(packet));
if(listeNodes_recherche(data->askedToRedirect, c->node)) // si le paquet contient des instructions pour ce noeud
{
forward(c, packet);
}
}
else
{
printf("Message non broadcaste pas traite ... TODO\n");
}
//packet_dealloc(packet);
break;
}
default:
printf("%d a recu un packet de type %d NON reconnu\n", c->node, data->type);
break;
}
/*printf("estimation de l'energie restante depuis %d :\n", c->node);
int i;
for(i = 0 ; i < get_node_count() ; i++)
{
printf("\t%d : %.1lf\n", i, nodedata->energiesRem[i]);
}*/
}
