znwk.c

zigbee精简协议栈代码

}

void forward_rreq( RREQ *request){

	//du 转发route_request包
	macDestInfo.addrMode = MAC_DST_SHORT_ADDR;
	macDestInfo.panID = macInfo.panID;
	macDestInfo.shortAddr.Val = 0xffff;
		
    NWKPutHeader(&macDestInfo,
            NWK_FRAME_CMD |
            NWK_PROT_VER,
            0x00,
			macDestInfo.shortAddr);
	
    NWKPut(NWK_CMD_ROUTE_REQUEST);
	MACPutArray((BYTE *)request, (BYTE)sizeof(RREQ));
	NWKFlush();

	ConsolePutString("forward route request\n");
}

void send_rrep( RREQ * request)
{
	RREP *reply = (RREP*)malloc(sizeof(RREP));
	if (reply == NULL){
		//ConsolePutString("malloc new RREP failed\n");
		return;
	}
	reply->type = AODV_RREP;
	reply->flag.bit.a = FALSE;
	reply->flag.bit.r = FALSE;
	reply->reserved = 0;
	reply->prefix = 0;
	reply->dest_ip = request->dest_ip;
	struct	Route_Table_Entry *dest_route = lookup_route( request->dest_ip);
	if (seq_greater(request->dest_seq, dest_route->dest_seq)){
		dest_route->dest_seq = request->dest_seq+1;
	}
	reply->dest_seq = dest_route->dest_seq;
	reply->hopCount = 0;
	reply->source_ip = request->source_ip;
	reply->life_time = 0;//生存时间没定义


	macDestInfo.addrMode = MAC_DST_SHORT_ADDR;
	macDestInfo.panID = macInfo.panID;
	//du 到请求发起节点路径的下一跳
	macDestInfo.shortAddr.Val = lookup_route( request->source_ip)->next_ip;
	NWKPutHeader(&macDestInfo,
                NWK_FRAME_CMD |
                NWK_PROT_VER,
                0x00,
				macDestInfo.shortAddr);
	NWKPut(NWK_CMD_ROUTE_REPLY);
	NWKPutArray((BYTE *)reply, (BYTE)sizeof(RREP));
	HFRAME frame = NWKFlush();
	//du mac ask如何处理
							while (1){
								MACTask();
								if(MACFrameIsAcked(frame)){
									MACFrameRemove(frame);
									
									break;
								}
								else if(MACFrameIsTimedOut(frame)){
									//du 产生路由错误
									
									MACFrameRemove(frame);
									break;	
								}
							}			
	ConsolePutString("send route reply\n");
	
}

void forward_rrep(RREP *reply){
	macDestInfo.addrMode = MAC_DST_SHORT_ADDR;
	macDestInfo.panID = macInfo.panID;
	//du 到请求发起节点路径的下一跳
	macDestInfo.shortAddr.Val = lookup_route( reply->source_ip)->next_ip;
	
	NWKPutHeader(&macDestInfo,
                NWK_FRAME_CMD |
                NWK_PROT_VER,
                0x00,
				macDestInfo.shortAddr);
	NWKPut(NWK_CMD_ROUTE_REPLY);
	NWKPutArray((BYTE *)reply, (BYTE)sizeof(RREP));
	HFRAME frame = NWKFlush();
	//du mac ask如何处理
							while (1){
								MACTask();
								if(MACFrameIsAcked(frame)){
									MACFrameRemove(frame);
									
									break;
								}
								else if(MACFrameIsTimedOut(frame)){
									//du 产生路由错误
									
									MACFrameRemove(frame);
									break;	
								}
							}
	//du mac ask如何处理
	ConsolePutString("forward route reply\n");
}

void send_rrep_ack( WORD dest_ip){
	RREP_ACK *ack = (RREP_ACK *) malloc(sizeof(RREP_ACK));
	ack->type = AODV_RREP_ACK;
	ack->reserved = 0;
	
	macDestInfo.addrMode = MAC_DST_SHORT_ADDR;
	macDestInfo.panID = macInfo.panID;
	//du 到请求发起节点路径的下一跳
	macDestInfo.shortAddr.Val = dest_ip; 
	NWKPutHeader(&macDestInfo,
                NWK_FRAME_CMD |
                NWK_PROT_VER,
                0x00,
				macDestInfo.shortAddr);
	NWKPut(NWK_CMD_ROUTE_ACK);
	NWKPutArray((BYTE *)ack, (BYTE)sizeof(RREP_ACK));
	NWKFlush();
	//du mac ask如何处理
	ConsolePutString("send route ack\n");


}

void gen_rerr( WORD break_ip){
struct	Route_Table_Entry * tmp_route = route_table;
struct	Err_Dest_List * err_list = NULL;
struct	Err_Dest_List * tmp_list = NULL;
	WORD err_count = 0;
	while (tmp_route){
		if (tmp_route->next_ip == break_ip && !tmp_route->flag.bit.is_self_route && tmp_route->flag.bit.is_route_valid){
			tmp_route->flag.bit.is_route_valid = FALSE;
			tmp_route->dest_seq++;
			tmp_list = (struct Err_Dest_List *)malloc(sizeof(struct Err_Dest_List));
			if (tmp_list == NULL){
				//ConsolePutString("malloc new Err_Dest_List error\n");
				return;
			}
			tmp_list->err_dest.unreach_dest_ip = tmp_route->dest_ip;
			tmp_list->err_dest.unreach_dest_seq = tmp_route->dest_seq;
			tmp_list->next = err_list;
			err_list = tmp_list;
			
				
	
			err_count++;
		}
		tmp_route = tmp_route->next;
	}
	if (err_count)
		send_rerr( err_list, err_count);
}

void send_rerr(struct Err_Dest_List *err_list,  WORD dest_count){
struct Err_Dest * err_dest = NULL;
struct Err_Dest_List *dead_list = NULL;
WORD length = sizeof(RERR) + (sizeof(struct Err_Dest))*dest_count;
char * data = NULL;
	if (dest_count){
		data = (char *)malloc(length);
		if (data == NULL){
			//ConsolePutString("malloc new RERR error\n");
			return;
		}
		RERR * rerr = rerr = (RERR *)data;
		
		rerr->type = AODV_RERR;
		rerr->flag.bit.n = FALSE;
		rerr->reserved = 0;
		rerr->dest_count = dest_count;
		err_dest = (struct Err_Dest *)(data + sizeof(RERR));
		while (dest_count){
			//ConsolePutString("node%d failed route\n", node_index+1);

			err_dest->unreach_dest_ip = err_list->err_dest.unreach_dest_ip;
			//ConsolePutString("route dest_ip = %d\n", err_dest->unreach_dest_ip);
			err_dest->unreach_dest_seq = err_list->err_dest.unreach_dest_seq;
			//ConsolePutString("route dest_ip_seq = %d\n", err_dest->unreach_dest_seq);
			dead_list = err_list;
			err_list = err_list->next;
			err_dest = err_dest+1;
			dest_count--;
			free(dead_list);
		}
		macDestInfo.addrMode = MAC_DST_SHORT_ADDR;
		macDestInfo.panID = macInfo.panID;
		macDestInfo.shortAddr.Val = 0xffff;
		
	    NWKPutHeader(&macDestInfo,
                NWK_FRAME_CMD |
                NWK_PROT_VER,
                0x00,
				macDestInfo.shortAddr);
		NWKPut(NWK_CMD_ROUTE_ERROR);
		NWKPutArray((BYTE *)data, (BYTE)length);
		NWKFlush();
		ConsolePutString("send route error");
	}
}

void init_route_table(){
	route_table = NULL;
struct	Route_Table_Entry *tmp_route = create_route( macInfo.shortAddr.Val);
	tmp_route->flag.bit.is_self_route = TRUE;
	tmp_route->flag.bit.is_route_valid = TRUE;
	tmp_route->flag.bit.is_route_seq_valid = TRUE;
	tmp_route->next_ip = macInfo.shortAddr.Val;
}

struct Route_Table_Entry *create_route( WORD ip){
struct	Route_Table_Entry * new_route = (struct Route_Table_Entry *)malloc(sizeof(struct Route_Table_Entry ));
	if (new_route == NULL){
		//ConsolePutString("create route failer\n");
		return NULL;
	}
	new_route->dest_ip = ip;
	new_route->dest_seq = 0;
	new_route->hopCount = 0;
	new_route->life_time = 0;
	new_route->next_ip = 0;
	new_route->req_id = 0;
	new_route->flag.bit.is_self_route = FALSE;
	new_route->flag.bit.is_route_valid = FALSE;
	new_route->flag.bit.is_route_seq_valid = FALSE;
	new_route->next = NULL;
	new_route->prev = NULL;
	insert_route( new_route);
	return new_route;
}

void insert_route(struct Route_Table_Entry * new_route){

	if (route_table == NULL){
		route_table = new_route;
		return;
	}
struct	Route_Table_Entry *tmp_route = route_table;
struct	Route_Table_Entry *prev_route = NULL;

	while (tmp_route && tmp_route->dest_ip < new_route->dest_ip){
		prev_route = tmp_route;
		tmp_route = tmp_route->next;
	}
	if (tmp_route && tmp_route == route_table){
		tmp_route->prev = new_route;
		new_route->next = tmp_route;
		route_table = new_route;
	
	}else if (tmp_route == NULL){
		prev_route->next = new_route;
		new_route->prev = prev_route;
	}else{
		prev_route->next = new_route;
		new_route->prev = prev_route;
		new_route->next = tmp_route;
		tmp_route->prev = new_route;
	}
}

struct Route_Table_Entry *lookup_route(WORD dest_ip){
struct	Route_Table_Entry * tmp_route = route_table;
struct	Route_Table_Entry * dest_route = NULL;
	while (tmp_route && tmp_route->dest_ip <= dest_ip){
		if (tmp_route->dest_ip == dest_ip){
			dest_route = tmp_route;
		}
		tmp_route = tmp_route->next;
	}
	return dest_route;
}

void cleanup_route_table(){
struct	Route_Table_Entry * tmp_route = route_table;
	while (tmp_route){
		tmp_route = tmp_route->next;
		free(tmp_route->prev);
	}
	route_table = NULL;
}

void delete_route( WORD dest_ip){
struct	Route_Table_Entry * tmp_route = route_table;
struct	Route_Table_Entry * del_route = NULL;
	while(tmp_route && tmp_route->dest_ip <= dest_ip){
		if (tmp_route->dest_ip == dest_ip)
			del_route = tmp_route;
		tmp_route = tmp_route->next;
	}
	if (del_route){
		if (del_route == route_table){
			route_table = route_table->next;
			route_table->prev = NULL;
		}
		if (del_route->prev){
			del_route->prev->next = del_route->next;
		}
		if (del_route->next){
			del_route->next->prev = del_route->prev;
		}
		free(del_route);
	}

	
}

void update_route( WORD dest_ip, WORD next_ip, WORD dest_seq, BYTE hopCount)
{
struct Route_Table_Entry * tmp_route = NULL;
tmp_route = lookup_route( dest_ip);
if (tmp_route == NULL)
{
	tmp_route = create_route( dest_ip);
}
if (!tmp_route->flag.bit.is_route_seq_valid);
else if (seq_greater(dest_seq, tmp_route->dest_seq));
else if (dest_seq == tmp_route->dest_seq && (!tmp_route->flag.bit.is_route_valid || tmp_route->hopCount > hopCount));
else
	return;
tmp_route->dest_ip = dest_ip;
tmp_route->dest_seq = dest_seq;
tmp_route->hopCount = hopCount;
tmp_route->next_ip = next_ip;
tmp_route->precuror = NULL;
tmp_route->life_time = 0;
tmp_route->flag.bit.is_route_seq_valid = TRUE;
tmp_route->flag.bit.is_route_valid = TRUE;
}

void init_req_id_queue(){
req_id_queue = NULL;
}
void insert_req_id( WORD src_ip, WORD dst_ip, WORD id){
flush_req_id_queue();
struct REQ_ID * req_id = (struct REQ_ID *)malloc(sizeof(struct REQ_ID));
if (req_id == NULL){
	//ConsolePutString("malloc new req_id failed");
	return;
}
req_id->dest_ip = dst_ip;
req_id->life_time = TickGet();//du 记录请求时间
req_id->req_id = id;
req_id->source_ip = src_ip;
req_id->next = req_id_queue;
req_id_queue = req_id;
}
struct REQ_ID *find_req_id( WORD src_ip, WORD id){
struct REQ_ID * tmp_req_id = req_id_queue;
while (tmp_req_id != NULL){
	if (tmp_req_id->source_ip == src_ip && tmp_req_id->req_id == id)
		return tmp_req_id;
	tmp_req_id = tmp_req_id->next;
}
return NULL;
}

void cleanup_req_id_queue(){
struct REQ_ID * tmp_req_id = req_id_queue;
struct REQ_ID * req_id = NULL;
while (tmp_req_id != NULL){
	req_id = tmp_req_id;
	tmp_req_id = tmp_req_id->next;
	free(req_id);
}
req_id_queue = NULL;
}

//du 一次只删除一个req_id
void flush_req_id_queue(){
struct REQ_ID * tmp_req_id = req_id_queue;
struct REQ_ID * prev_req_id = NULL;
struct REQ_ID * req_id = NULL;

while (tmp_req_id != NULL){
	if ((TickGet()-tmp_req_id->life_time) >= NWK_ROUTE_REQUEST_ID_PERIOD){
		req_id = tmp_req_id;
		
		if (tmp_req_id == req_id_queue)
			req_id_queue = tmp_req_id->next;
		else{
			prev_req_id->next = tmp_req_id->next;
			
		}
		free(req_id);
		ConsolePutString("remove req id\n");
		return;
	}
		prev_req_id = tmp_req_id;
		tmp_req_id = tmp_req_id->next;
	
	
}
ConsolePutString("no req id expire\n");
return;
}

BOOL seq_less_or_equal(WORD seq_one, WORD seq_two)
{
    SWORD *comp_seq_one = (SWORD *)&seq_one;
    SWORD *comp_seq_two = (SWORD *)&seq_two;

    if (  ( *comp_seq_one - *comp_seq_two ) > 0 )
    {
        return FALSE;
    }
    else
        return TRUE;
}

BOOL seq_greater(WORD seq_one,WORD seq_two)
{
    SWORD *comp_seq_one = (SWORD *)&seq_one;
   SWORD *comp_seq_two = (SWORD *)&seq_two;

    if (  ( *comp_seq_one - *comp_seq_two ) <= 0 )
        return FALSE;
    else
        return TRUE;
}