hgpsrouter.cc

在Linux下做的QuadTree的程序

#include "hgpsrouter.h"

// Returns a random number between 0 and max, implemented by dmaltz
static inline double
jitter (double max, int be_random_)
{
      return (be_random_ ? Random::uniform(max) : 0);
}

static bool
inSameGrid(location src, location dst)
{
     int min_mask;
     int mask;
     
     if (src.mask > dst.mask) {
	 min_mask = dst.mask;
     }else{
	 min_mask = src.mask;
     }
    
     mask = 0xffffffff << (BIGGEST_MASK - min_mask);
     return (((src.loc & mask) == (dst.loc & mask))? true:false);
}


//this function converts grid location to a geographical distance
static inline void
loctodistance(int &x, int &y, location loc)
{
    int stop =loc.mask;
    int shift = BIGGEST_MASK;
    unsigned int tmp_mask = 0xffffffff;
    int the_loc = loc.loc;

    x = 0;
    y = 0;

    while (stop>=2) {
	shift = shift - 2;
	x = x *2 + 2* (the_loc >> (shift+1));
	y = y *2 + 2* ((the_loc & 0x55555555)>>shift);
	stop -= 2;
	//clear the examined two bits
	tmp_mask = tmp_mask >> 2;
	the_loc = the_loc & tmp_mask;
    }
    if (shift == 0) {
	x = x +1;
	y = y +1;
    }else {
	x = x*((int)pow(2,(shift/2)));
	x = x+((int)pow(2,(shift/2)));
	y = y*((int)pow(2,(shift/2)));
	y = y+((int)pow(2,(shift/2)));
    }
#ifdef HGPS_DEBUG100
    assert((x>=1)&&(x<=7));
    assert((y>=1)&&(y<=7));
#endif
#ifdef HGPS_DEBUG150
    if (loc.mask == 32) {
	assert((x>=1)&&(x<=11));
	assert((y>=1)&&(y<=11));
    }
#endif

    
}

HGPSRouterPeriodicHandler::HGPSRouterPeriodicHandler(HGPSRouter *r)
{
    myrouter = r;
}

void
HGPSRouterPeriodicHandler::handle(Event *e)
{
    //send out one hop neighbor table periodically!
    myrouter->broadcastOneHopRT();
    //reschedule the next periodic event
    Scheduler::instance().schedule(this,e,BROADCAST_INTEVAL);
}

HGPSRouter::HGPSRouter(HGPSAgent *a)
{
    parent = a;
    //initializing my routing table
    for (int i=0;i<MAX_RT_ENTRIES;i++) {
	nt[i].lastupdate_time = -2*NEIGHBOR_TIMEOUT_INTEVAL;
	nt[i].next_hop = NULL;
    }
    for (int i=0;i<MAX_ADJ_GRID;i++) {
	rt_1[i].loc.loc = -1;
	rt_1[i].lastupdate_time = -2*NEIGHBOR_TIMEOUT_INTEVAL;
	rt_1[i].next_hop = NULL;
    }

    router_periodic_event = new Event();
    router_periodic_handler = new HGPSRouterPeriodicHandler(this);
}

void
HGPSRouter::setLocServer(LocServer *s)
{
    locserver = s;
}
void
HGPSRouter::start()
{
    //periodic broadcasting
    Scheduler::instance().schedule(router_periodic_handler,
	    router_periodic_event,jitter(HGPS_ROUTER_STARTUP_JITTER, 1));
}

void
HGPSRouter::sleep()
{
    Scheduler::instance().cancel(router_periodic_event);
}

void
HGPSRouter::broadcastOneHopRT()
{
    Scheduler & s = Scheduler::instance();
    double now = s.clock();
    double x, y;

    Packet *pkt = parent->allocpkt();
    hdr_hgps *hgpsh =  (hdr_hgps*)pkt->access(parent->off_hgps_);
    hdr_ip *iph =  (hdr_ip*)pkt->access(parent->off_ip_); 
    hdr_cmn *cmnh =  (hdr_cmn*)pkt->access(parent->off_cmn_);
    location myloc;
    
    int num = 0;

    //send out all my one hop neighbors from routing table,
    //be careful about out-dated entries
    hgpsh->init();
    hgpsh->type_ = ROUTE_UPDATE;
    myloc = locserver->getGridPos(x,y);
    nsaddr_t hop;
    for (int i=0;i<MAX_RT_ENTRIES;i++) {
	if (((now - nt[i].lastupdate_time) < NEIGHBOR_TIMEOUT_INTEVAL) && 
		(hop=getNextHopNode(nt[i])) && (hop == nt[i].dst)) {
    	    hgpsh->neighbor_ids_[num] = nt[i].dst;
    	    hgpsh->neighbor_locs_[num] = nt[i].loc;
    	    num++;
	}
    }
    assert(num <= MAX_NEIGHBOR);
    hgpsh->neighbor_num_ = num;
    
    //update my own location
    hgpsh->src_loc_ = myloc;
    hgpsh->src_x_ = x;
    hgpsh->src_y_ = y;
    hgpsh->src_speed_ = locserver->getSpeed();

    //update commond headers etc.
    cmnh->next_hop_ = MAC_BROADCAST;
    cmnh->addr_type_ = AF_INET;
    cmnh->ptype() = PT_HGPS;

    
    iph->dst() = IP_BROADCAST;
    iph->dport_ = RT_PORT;
    iph->src() = parent->myaddr_;

    //ask loc server to piggy back any fp information they want
    locserver->piggybackFP(pkt);

    //update the pkt size
    cmnh->size() = hgpsh->size();

    //send out the packet immediately, yeah!!!!!!!
    s.schedule(parent->ll,pkt,0);
}

void
HGPSRouter::recvNeighborUpdate(Packet *pkt)
{
    hdr_hgps *hgpsh =  (hdr_hgps*)pkt->access(parent->off_hgps_);
    hdr_ip *iph =  (hdr_ip*)pkt->access(parent->off_ip_); 

    //update my little routing tables :P
    addUpdateEntry(hgpsh->src_loc_,iph->src(),hgpsh->src_x_,hgpsh->src_y_,iph->src(),1, hgpsh->src_speed_);

    for (int i=0;i<hgpsh->neighbor_num_;i++) {
	addUpdateEntry(hgpsh->neighbor_locs_[i],iph->src(),hgpsh->src_x_,hgpsh->src_y_,hgpsh->neighbor_ids_[i],2, 0);
    }

    if (hgpsh->fp_num_>0) {
	locserver->recvFPUpdate(pkt);
    }
    Packet::free(pkt);
}

void
HGPSRouter::deleteEntry(nsaddr_t nextnode)
{

    nexthop_entry *node_pt;
    nexthop_entry *node_tmp_pt;
    double now = Scheduler::instance().clock();

    for (int i = 0;i<MAX_RT_ENTRIES;i++) {
	node_pt = nt[i].next_hop;
	while (node_pt) {
	    if (((now - node_pt->lastupdate_time) > NEIGHBOR_TIMEOUT_INTEVAL)||(node_pt->next_node == nextnode)) {
		//delete delete
		if (node_pt->prev) {
		    node_pt->prev->next = node_pt->next;
		}else{
		    nt[i].next_hop = node_pt->next;
		}

		if (node_pt->next) {
		    node_pt->next->prev = node_pt->prev;
	        }
		node_tmp_pt = node_pt;
		node_pt = node_pt->next;
		delete node_tmp_pt; 
	    }else{
		node_pt = node_pt->next;
	    }
	}
	if (!nt[i].next_hop) {
	    nt[i].dst = 0;
	    nt[i].lastupdate_time = -10*NEIGHBOR_TIMEOUT_INTEVAL;
	}
    }

    
    //delete location entry
    for (int i=0;i<MAX_ADJ_GRID;i++) {
	node_pt = rt_1[i].next_hop;
	while (node_pt) {
	    if (((now - node_pt->lastupdate_time) > NEIGHBOR_TIMEOUT_INTEVAL)||(node_pt->next_node == nextnode)) {
		//delete delete
		if (node_pt->prev) {
		    node_pt->prev->next = node_pt->next;
		}else{
		    rt_1[i].next_hop = node_pt->next;
		}

		if (node_pt->next) {
		    node_pt->next->prev = node_pt->prev;
	        }

		node_tmp_pt = node_pt;
		node_pt = node_pt->next;
		delete node_tmp_pt; 
	    }else{
		node_pt = node_pt->next;
	    }
	}
	if (!rt_1[i].next_hop) {
	    rt_1[i].dst = 0;
	    rt_1[i].lastupdate_time = -10*NEIGHBOR_TIMEOUT_INTEVAL;
	}
    }
}

void
HGPSRouter::deleteEntry(nsaddr_t fromnode, nsaddr_t tonode)
{

    nexthop_entry *node_pt;
    nexthop_entry *node_tmp_pt;
    double now = Scheduler::instance().clock();
    int j;

    if (fromnode == tonode) {
	return;
    }

    //delete delete
    for (int i = 0;i<MAX_RT_ENTRIES;i++) {
	j = (i + tonode) % MAX_RT_ENTRIES;
	if ((nt[j].dst == tonode)) {
	    node_pt = nt[j].next_hop;
	    while (node_pt) {
		if (((now - node_pt->lastupdate_time) > NEIGHBOR_TIMEOUT_INTEVAL)||(node_pt->next_node == fromnode)) {
		    //delete delete
		    if (node_pt->prev) {
			node_pt->prev->next = node_pt->next;
		    }else{
			nt[j].next_hop = node_pt->next;
		    }

		    if (node_pt->next) {
			node_pt->next->prev = node_pt->prev;
		    }
		    node_tmp_pt = node_pt;
		    node_pt = node_pt->next;
		    delete node_tmp_pt; 
		}else{
		    node_pt = node_pt->next;
		}
	    }
	    if (!nt[j].next_hop) {
		nt[j].dst = 0;
		nt[j].lastupdate_time = -3*NEIGHBOR_TIMEOUT_INTEVAL;
	    }
	    return;
	}
    }

}


void
HGPSRouter::addUpdateEntry(location loc,nsaddr_t next_hop,double x, double y,nsaddr_t dst, int distance, double speed)
{
    double now = Scheduler::instance().clock();
    int freeslot = -1;
    int i;
   
    assert(dst>0);

    nexthop_entry *nexthop_pt;
    nexthop_entry *new_pt;

    for (int j=0;j<MAX_RT_ENTRIES;j++) {
	i = (j + dst) % MAX_RT_ENTRIES;
    	if ((nt[i].dst== dst)) {
	    //update the entry
	    nt[i].lastupdate_time = now;
	    nt[i].loc = loc;
	    nexthop_pt = nt[i].next_hop;
	    if ((distance == 1) || (!nexthop_pt)) {
		//insert in the front of the list
		if (nexthop_pt && (nexthop_pt->next_node == next_hop)){
		    //the entry already exists, just update the timer
		    nexthop_pt->lastupdate_time = now;
		    nexthop_pt->x = x;
		    nexthop_pt->y = y;
		    nexthop_pt->speed = speed;
		}else {
		    new_pt = new nexthop_entry;
		    new_pt->dist = distance;
		    new_pt->next_node = next_hop;
		    new_pt->lastupdate_time = now;
		    new_pt->x = x;
		    new_pt->y = y;
		    new_pt->speed = speed;
		    new_pt->prev = NULL;
		    new_pt->next = nexthop_pt;
		    nt[i].next_hop = new_pt;
		    if (nexthop_pt) {
		       nexthop_pt->prev = new_pt;
		    }
		}
	    }else{
		while((nexthop_pt->next)&&(nexthop_pt->next_node != next_hop)) {
		    nexthop_pt = nexthop_pt->next;
		}
		if (nexthop_pt->next_node == next_hop) {
		    nexthop_pt->x = x;
		    nexthop_pt->y = y;
		    nexthop_pt->lastupdate_time = now;
		    nexthop_pt->speed = speed;
		}else{
		    assert(nexthop_pt->next == NULL);
		    //insert the node at the end of nexthop_pt, the last entry
		    new_pt = new nexthop_entry;
		    new_pt->dist = distance;
		    new_pt->next_node = next_hop;
		    new_pt->lastupdate_time = now;
		    new_pt->speed = speed;
		    new_pt->x = x;
		    new_pt->y = y;
		    nexthop_pt->next = new_pt;
		    new_pt->next = NULL;
		    new_pt->prev = nexthop_pt;
		}
	    }
	    goto NEXT;
	}else if ((freeslot == -1) && ((now - nt[i].lastupdate_time) > NEIGHBOR_TIMEOUT_INTEVAL)) {
	    nexthop_pt = nt[i].next_hop;
	    //delete the next_hop list
	    while(nexthop_pt) {
		nexthop_pt = nexthop_pt->next;
		if (nexthop_pt) {
		    delete nexthop_pt->prev;
		}
	    }
	    nt[i].next_hop = NULL;
	    freeslot = i;
    	}
    }

    assert(freeslot>=0);

    //add it to freeslot
    new_pt = new nexthop_entry;
    new_pt->lastupdate_time = now;
    new_pt->x = x;
    new_pt->y = y;
    new_pt->speed = speed;
    new_pt->next_node = next_hop;
    new_pt->dist = distance;
    
    new_pt->prev = NULL;
    new_pt->next = NULL;
    
    nt[freeslot].loc = loc;
    nt[freeslot].next_hop = new_pt;
    nt[freeslot].lastupdate_time = now;
    nt[freeslot].dst = dst;

NEXT:    freeslot = -1;
    //update adjacent location grid
    for (int i = 0;i<MAX_ADJ_GRID;i++) {

	if (rt_1[i].loc.loc == loc.loc) {
	    rt_1[i].lastupdate_time = now;
	    nexthop_pt = rt_1[i].next_hop;
	    if (!nexthop_pt) {
		new_pt = new nexthop_entry;
		new_pt->dist = distance;
		new_pt->next_node = next_hop;
		new_pt->lastupdate_time = now;
		new_pt->speed = speed;
		new_pt->x = x;
		new_pt->y = y;
		new_pt->prev = NULL;
		new_pt->next = NULL;
		rt_1[i].next_hop = new_pt;
	    }else{
		while (nexthop_pt->next && (nexthop_pt->next_node != next_hop) 
			&& (nexthop_pt->dist <= distance)) {
		    nexthop_pt = nexthop_pt->next;
		}
	
		if (nexthop_pt->next_node == next_hop) {
		    nexthop_pt->lastupdate_time = now;
		    nexthop_pt->speed = speed;
		    nexthop_pt->x = x;
		    nexthop_pt->y = y;
		}else {
		    new_pt = new nexthop_entry;
		    new_pt->dist = distance;
		    new_pt->next_node = next_hop;
		    new_pt->lastupdate_time = now;
		    new_pt->speed = speed;
		    new_pt->x = x;
		    new_pt->y = y;
		    //if distance is 1, insert in front
		    if (distance == 1) {
			new_pt->next = rt_1[i].next_hop;
			if (rt_1[i].next_hop) {
			    rt_1[i].next_hop->prev = new_pt;