dvdrp.c

MANTIS是由科罗拉多大学开发的传感器网络嵌入式操作系统。 这是mantis的0.9.5版本的源码。

    return ret;
}

static uint8_t dvdrp_match(dvdrp_mesg_pkt *dvdrp_ptr, dvdrp_route* r) {
    dvdrp_filter_list     *f_list;
    dvdrp_constraint_list *c_list;
    uint8_t counter, filter_size, i;

    //for each filter in the predicate
    for(f_list = r->pred; f_list; f_list = f_list->next) {
	//for each constraint in the filter
//	printf("\nf.");
	counter = filter_size = 0;   //reset size of filter
	for(c_list = f_list->constraints; c_list; c_list = c_list->next) {
	    filter_size++;
	    //for each attribute in the message
//	    printf("c.");
	    for(i = 0; i < dvdrp_ptr->num_attribs; ++i) {
//		printf("a.");
		//check to see if attribute is covered by the constraint
		if(dvdrp_attr_match(&dvdrp_ptr->attributes[i], c_list)) {
		    counter++;
		}
	    }
	}
	
	//if all contraints are covered, packet needs to filtered down
	//interface
	if(counter == filter_size) {
//	    printf("  Fwd match: recv = %d.\n", r->receiver_id);
	    //if this filter matches, skip the rest of the predicate
	    return TRUE;
	}
    }
    
    return FALSE;    
}

static uint8_t dvdrp_attr_match(dvdrp_attrib *attr,
				dvdrp_constraint_list *constraint)
{
    uint8_t ret = FALSE;

    if(attr->name == constraint->name) {
	switch (constraint->compare_type) {
	case DVDRP_COMPARE_EQ:
	    ret = (attr->value == constraint->value);
	    break;
	case DVDRP_COMPARE_GT:
	    ret = (attr->value > constraint->value);
	    break;
	case DVDRP_COMPARE_LT:
	    ret = (attr->value < constraint->value);
	    break;
	case DVDRP_COMPARE_NE:
	    ret = (attr->value != constraint->value);
	    break;
	}
    }

    return ret;
}

/////////////////////////
//Route table funcs
//

//get free bv id for a new route - returns 32 (DVDRP_INVALID_BV_ID) if
//no free bv
static uint8_t dvdrp_get_free_bv(uint8_t additional) {
    uint32_t bv = 0;
    uint8_t i, j;

    //first marked all used bits
    if(additional != DVDRP_INVALID_BV_ID) {
	mask_set(bv, additional);
    }
    
    for(i = 0; i < DVDRP_RT_MAX_PREDICATES; ++i) {
	if(route_tbl[i].pred) {
	    mask_set(bv, route_tbl[i].bv_pos);
	}
    }

    //now find free bv
    j = 0;
    if(bv == 0xFFFF) {  //we're full, so sorry
	i = DVDRP_INVALID_BV_ID;
    } else {
	i = rand() % 32;

	for( ; j < 32; ++j) {
	    if(!mask_set(bv, (i + j) % 32)) {
		break;
	    }
	}
    }

    return (i + j) % 32;
}

//pass NULL for local init
static void dvdrp_route_init(dvdrp_route *route, dvdrp_advert_pkt *pkt) {
    uint8_t i;

    if(!route)
	return;

    if(pkt) {
	route->receiver_id = pkt->receiver_id;
	route->bv_pos = pkt->bv_pos;  
	route->paths[0].next_hop = pkt->prev_hop;
	route->paths[0].distance = DVDRP_MAX_HTL - pkt->header.htl;
	route->seq = pkt->seq_num;
	route->min_delay = pkt->min_delay;
    } else {
	route->receiver_id = node_id;
	route->bv_pos = node_bv;  
	route->paths[0].next_hop = node_id;
	route->paths[0].distance = 0;
	route->seq = node_seq_num;
	route->min_delay = node_delay;
    }

    route->paths[0].fail_cnt = 0;
    route->last_pkt = 0;
    for(i = 1; i < DVDRP_RT_MAX_PATHS; ++i) {
	route->paths[i].next_hop = DVDRP_INVALID_NODE_ID;
	route->paths[i].distance = DVDRP_MAX_HTL;
	route->paths[i].fail_cnt = 0;
    }
}

static uint8_t dvdrp_route_local_update(dvdrp_route *route,
					dvdrp_filter_list *pred)
{
    if(!route) {
	//the local node has not yet set a route, lets do so
	route = dvdrp_route_find_unused();
	node_bv = dvdrp_get_free_bv(DVDRP_INVALID_BV_ID);

//	printf(".bvid;%C\n", node_bv);

	if(!route || node_bv == DVDRP_INVALID_BV_ID)  { //no unused routes
	    dvdrp_route_free_pred(pred);
	    
	    return FALSE;
	}

	dvdrp_route_init(route, NULL); 
    }

//    printf("pred_passed: %x\n", pred);
    
    //predicate is localy allocated by the application - use their
    //version of the pred to save space and force ioctl for updates
    route->pred = pred;
    route->seq = ++node_seq_num;

    return dvdrp_send_route_advert(route, DVDRP_MAX_HTL);
}

static uint8_t dvdrp_route_update(dvdrp_route *route, dvdrp_advert_pkt *pkt) {
    uint8_t pkt_dist;
    uint8_t i, j;
    uint8_t ret = FALSE;

    pkt_dist = DVDRP_MAX_HTL - pkt->header.htl;
    if(!route) {
	//first we need to check for and resolve any BV conflicts
	//also, make sure there is an available route
	if(!dvdrp_route_resolve_bv_conflicts(pkt) ||    //bv conflicts?
	   !(route = dvdrp_route_find_unused()))        //free route?
	{
	    return FALSE;
	}
	
//	printf("New sub (rcv = %d)\n", pkt->receiver_id);
	
	dvdrp_route_init(route, pkt);

	dvdrp_route_copy_pkt_pred(route, pkt);
	dvdrp_send_route_advert(route, pkt->header.htl);

	///!!!! Note the return here - below code does not run
	return TRUE;
    }
    
    for(i = 0; i < DVDRP_RT_MAX_PATHS; ++i) {
	if(pkt_dist < route->paths[i].distance || pkt->seq_num > route->seq) {
//	    printf("Updating sub, idx %C: %C < %C (%C > %C)\n",
//		   i, pkt_dist, route->paths[i].distance, pkt->seq_num,
//		   route->seq);
//	    printf("(rcv = %d)\n", pkt->receiver_id);

	    j = DVDRP_RT_MAX_PATHS - 1;
	    while(j > i) {
		dvdrp_path_copy(&route->paths[j], &route->paths[j-1]);
		--j;
	    }
	    
	    route->paths[i].next_hop = pkt->prev_hop;
	    route->paths[i].distance = pkt_dist;
	    
	    if(pkt->seq_num > route->seq) {  //note: i == 0 gaureenteed
		for(j = 1; j < DVDRP_RT_MAX_PATHS; ++j) {
		    route->paths[j].next_hop = DVDRP_INVALID_NODE_ID;
		    route->paths[j].distance = DVDRP_MAX_HTL;
		}

		if(pkt->pred.num_filters == 0) {
		    //we need to kill this route!
		    route->receiver_id = DVDRP_INVALID_NODE_ID;
		    dvdrp_route_free_pred(route->pred);  //free the old pred
		    route_tbl[i].pred = NULL;
		} else {
		    dvdrp_route_free_pred(route->pred);  //free the old pred
		    dvdrp_route_copy_pkt_pred(route, pkt);
		    route->seq = pkt->seq_num;
		    route->min_delay = pkt->min_delay;

		    //there are timing situation in bv arbitration
		    //that force this to be updated here
		    route->bv_pos = pkt->bv_pos;
		}
	    }

	    //we only want to propigate the subscription if we update
	    //the PRIMARY - a second broadcast only adds noise to an
	    //already received route.
	    if(i == 0) {
		dvdrp_send_route_advert(route, pkt->header.htl);
	    }

	    ret = TRUE;
	    break;
	}
	
	//protect against a next_hop being listed twice
	if(route->paths[i].next_hop == pkt->prev_hop) {
	    break;
	}

    }

    //make sure that there arn't multiple paths using the same node
    for(j = i+1; j < DVDRP_RT_MAX_PATHS; ++j) {
	if(route->paths[j].next_hop == route->paths[i].next_hop) {
	    route->paths[j].next_hop = DVDRP_INVALID_NODE_ID;
	    route->paths[j].distance = DVDRP_MAX_HTL;
	}
    }
    
    if(!ret) {
//	printf("Dropping subscription pkt.\n");
    }

    return ret;
}

static uint8_t dvdrp_route_resolve_bv_conflicts(dvdrp_advert_pkt *pkt) {
    uint8_t i, ret = TRUE;
    
    // Here's the layout:
    // 1) Check for any other route with the same bv
    //   a) if found, check receiver id;
    //   b) if new_mesg.recv_id < old_bv
    //     i) check if *we* were the loser - readvertise with new bv
    //    ii) else kill the old route - they need to readvertise
    //   c) else return FALSE, as this new advert is the loser

    for(i = 0; i < DVDRP_RT_MAX_PREDICATES; ++i) {
	//1)
	if(route_tbl[i].bv_pos == pkt->bv_pos) {
	    //a)
	    if(pkt->receiver_id < route_tbl[i].receiver_id) {
		//b)
		if(route_tbl[i].receiver_id == node_id) {
		    //i)
		    //get a new node_bv
		    node_bv = dvdrp_get_free_bv(pkt->bv_pos);
		    route_tbl[i].bv_pos = node_bv;

		    //send replacement advertisement
		    dvdrp_send_route_advert(&route_tbl[i], DVDRP_MAX_HTL);
		} else {
		    //kill old route
		    route_tbl[i].receiver_id = DVDRP_INVALID_NODE_ID;
		    dvdrp_route_free_pred(route_tbl[i].pred);
		    route_tbl[i].pred = NULL;
		}
		break;
	    } else {
		//c)
		ret = FALSE;
		break;
	    }
	}
    }
    
    return ret;
}

static dvdrp_route *dvdrp_route_find(node_id_t receiver_id) {
    uint8_t i;

    for(i = 0; i < DVDRP_RT_MAX_PREDICATES; ++i) {
	if(route_tbl[i].receiver_id == receiver_id) {
	    return &route_tbl[i];
	}
    }

    return NULL;
}

static dvdrp_route *dvdrp_route_find_unused() {
    uint8_t i;

    for(i = 0; i < DVDRP_RT_MAX_PREDICATES; ++i) {
	if(route_tbl[i].receiver_id == DVDRP_INVALID_NODE_ID) {
	    return &route_tbl[i];
	}
    }

    return NULL;
}

static void dvdrp_route_copy_pkt_pred(dvdrp_route *route,
				      dvdrp_advert_pkt *pkt)
{
    dvdrp_filter_list     **f_list;
    dvdrp_filter           *cur_filter;
    dvdrp_constraint_list **c_list;
    uint8_t                 i, j, size;

    f_list = &route->pred;
    cur_filter = &(pkt->pred.filters[0]);
//    printf("\np %C\n", pkt->pred.num_filters);
    for(i = 0; i < pkt->pred.num_filters; ++i) {
	*f_list = pool_malloc(&filter_pool);
	size = sizeof(uint8_t);
	c_list = &((*f_list)->constraints);

//	printf(" f %C (%d)\n", cur_filter->num_constraints,
//	       (void*)cur_filter - (void*)pkt);
 	for(j = 0; j < cur_filter->num_constraints; ++j) {
	    *c_list = pool_malloc(&constraint_pool);
	    (*c_list)->name = cur_filter->constraints[j].name;
	    (*c_list)->value = cur_filter->constraints[j].value;
	    (*c_list)->compare_type = cur_filter->constraints[j].compare_type;

//	    printf("   c %C %C %C\n", cur_filter->constraints[j].name,
//		   cur_filter->constraints[j].value,
//		   cur_filter->constraints[j].compare_type);
	    
	    c_list = &((*c_list)->next);
	    size += sizeof(dvdrp_constraint);
	}

	*c_list = NULL;
	cur_filter += size;
	f_list = &(*f_list)->next;
    }
//    printf("\\p\n");
    
    *f_list = NULL;
}

static void dvdrp_route_free_pred(dvdrp_filter_list *pred) {
    dvdrp_filter_list *f_list;
    dvdrp_constraint_list *c_list;
    void *temp_mem;

    f_list = pred;
    while(f_list) {
	c_list = f_list->constraints;
	while(c_list) {
	    temp_mem = c_list;
	    c_list = c_list->next;
	    pool_free(&constraint_pool, temp_mem);
	}
	temp_mem = f_list;
	f_list = f_list->next;
	pool_free(&filter_pool, temp_mem);
    }
}

static void dvdrp_path_copy(dvdrp_path *dest, dvdrp_path *src) {
    dest->next_hop = src->next_hop;
    dest->distance = src->distance;
    dest->fail_cnt = src->fail_cnt;
}

///////////////////////////////////////
// Cache maintance.
////
/** @brief Check the cache for the given packet, and cache it if we haven't
 * already seen it. 
 * @param pkt Packet 
 * @return FALSE if this is an old or non-existent packet, else return TRUE
 */
int8_t cache_check(dvdrp_pkt * pkt)
{
    return FALSE;
}


///////////////////////////////////////
// Pool maintance functions.
////
void dvdrp_init_mem_pools() {
    constraint_pool.num_elems = DVDRP_MEM_CONSTRAINTS;
    constraint_pool.type_size = sizeof(dvdrp_constraint_list);
    constraint_pool.free_vec  = 0xFFFF;
    constraint_pool.pool = &constraint_mem[0];

    filter_pool.num_elems = DVDRP_MEM_FILTERS;
    filter_pool.type_size = sizeof(dvdrp_filter_list);
    filter_pool.free_vec  = 0xFFFF;
    filter_pool.pool = &filter_mem[0];

    combuf_pool.num_elems = DVDRP_MEM_COMBUFS;
    combuf_pool.type_size = sizeof(comBuf);
    combuf_pool.free_vec  = 0xFFFF;
    combuf_pool.pool = &combuf_mem[0];
}

void *pool_malloc(mem_pool *pool) {
    uint8_t i;

    for(i = 0; i < pool->num_elems; ++i) {
	if(mask_query(pool->free_vec, i)) {
	    mask_unset(pool->free_vec, i);
	    return (pool->pool + (i * pool->type_size));
	}
    }

    return NULL;
}

void pool_free(mem_pool *pool, void *mem) {
    uint8_t bit;

    bit = (mem - pool->pool) / pool->type_size;
    mask_set(pool->free_vec, bit);
}

void print_packet(comBuf *buf) {
    uint8_t i;
    
    printf("\n");
    for(i = 0; i < buf->size; ++i) {
	printf("%C ", buf->data[i]);
    }
}