#include <linux/if_ether.h>
#include <net/if_arp.h>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <assert.h>
#include <ctype.h>

#include "netdissect-stdinc.h"
#include "netdissect.h"
#include "stream_base.h"
#include "mesa_net.h"
#include "ip.h"
#include "ip6.h"
#include "tcp.h"
#include "udp.h"
#include "ppp.h"



#ifdef __cplusplus
extern "C" {
#endif

static int eth_jump_to_layer(const char *raw_data,  int raw_layer_type, int expect_layer_type);
static int vlan8021q_jump_to_layer(const char *raw_data,  int raw_layer_type, int expect_layer_type);
static int ipv4_jump_to_layer(const char *raw_data,  int raw_layer_type, int expect_layer_type);
static int ipv6_jump_to_layer(const char *raw_data,  int raw_layer_type, int expect_layer_type);
static int gre_jump_to_layer(const char *raw_data,  int raw_layer_type, int expect_layer_type);

int treat_vlan_as_mac_in_mac_sw = 0; /* 将vlan强制按mac_in_mac格式解析, 先不传递给sapp, 因为影响sapp全局处理开关 */

/* ascii字符转16进制 */
char MESA_ascii_to_hex(char ascii)
{
	char c = 0;
	
	switch(ascii)
	{
		case '0':
		case '1':
		case '2':
		case '3':
		case '4':
		case '5':
		case '6':
		case '7':
		case '8':
		case '9':
			c = ascii - 0x30;
		break;

		case 'a':
		case 'b':
		case 'c':
		case 'd':
		case 'e':
		case 'f':
			c = 10 + ascii - 0x61;
		break;

		case 'A':
		case 'B':
		case 'C':
		case 'D':
		case 'E':
		case 'F':
			c = 10 + ascii - 0x41;
		break;
	}

	return c;
}



static int arp_jump_to_layer(const char *raw_data,  int raw_layer_type, int expect_layer_type)
{
	/* arp协议不承载任何上层其他协议 */
	return -1;
}



static int gtp_jump_to_layer(const char *raw_data,  int raw_layer_type, int expect_layer_type)
{
	const struct gtp_hdr *gh = (struct gtp_hdr *)raw_data;
	int opt_len = 0; /* 可选项长度 */
	const unsigned char *next_ip_layer_hdr;
	int skip_len;

	if(ADDR_TYPE_GPRS_TUNNEL == expect_layer_type){
		return 0;
	}
	
	if(gh->flags & 0x2){
		opt_len += sizeof(int);  /* sequence */
	}

	next_ip_layer_hdr = (unsigned char *)raw_data + opt_len + sizeof(struct gtp_hdr);

	if((*next_ip_layer_hdr & 0x40) == 0x40){
		skip_len = ipv4_jump_to_layer((char *)next_ip_layer_hdr, __ADDR_TYPE_IP_PAIR_V4, expect_layer_type);
	}else if((*next_ip_layer_hdr & 0x60) == 0x60){
		skip_len = ipv6_jump_to_layer((char *)next_ip_layer_hdr, __ADDR_TYPE_IP_PAIR_V6, expect_layer_type);
	}else{
		printf("TODO: jmp unsupport type in GTP, 0x%x!\n", (*next_ip_layer_hdr));
		return -1;
	}

	return opt_len + sizeof(struct gtp_hdr) + skip_len;
}

#define TEREDO_AUTH_HDR_FLAG (0x0001)
#define TEREDO_INDICATION_HDR_FLAG (0x0000)

#define TEREDO_INDICATION_HDR_LEN (8)

struct teredo_auth_hdr{
#if __BYTE_ORDER == __LITTLE_ENDIAN
	unsigned short flags;
	unsigned char id_len;
	unsigned char au_len;
#elif __BYTE_ORDER == __BIG_ENDIAN
	unsigned char au_len;
    unsigned char id_len;
	unsigned short flags;
#else
#error "Please check <endian.h>"
#endif
};


static int udp_jump_to_layer(const char *raw_data,  int raw_layer_type, int expect_layer_type)
{
	const struct mesa_udp_hdr *uh = (const struct mesa_udp_hdr *)raw_data;
	unsigned short usport, udport;
	int skip_len;

	if(ADDR_TYPE_UDP == expect_layer_type){
		return 0;
	}

	usport = ntohs(uh->uh_sport);
	udport = ntohs(uh->uh_dport);

	if((2152 == usport) && (2152 == udport)){
		skip_len = gtp_jump_to_layer(raw_data+sizeof(struct mesa_udp_hdr), ADDR_TYPE_UDP, expect_layer_type);
	}else if(4789 == udport){
		/* vxlan模式暂时只支持ethernet. TODO: 如果是hdlc, ppp封装, 需要实现一个单独的vxlan_jump_to_layer()函数 */
		skip_len = eth_jump_to_layer(raw_data+sizeof(struct mesa_udp_hdr)+8, ADDR_TYPE_MAC, expect_layer_type);
		if(skip_len < 0){
			return -1;
		}
		skip_len += 8; /* skip vxlan header */
	}else if((3544 == usport) || (3544 == udport)){
		/* teredo实际没有数据包头, 直接跳转到ipv6层即可 , before 20181204*/
		const char *next_layer_hdr = raw_data+sizeof(struct mesa_udp_hdr);
        struct teredo_auth_hdr *p_teredo_hdr;
		int teredo_layer_len = 0, tmp_hdr_len = 0;
		while((*next_layer_hdr & 0xF0) != 0x60)
		{
			p_teredo_hdr = (struct teredo_auth_hdr *)next_layer_hdr;
			if(p_teredo_hdr->flags == ntohs(TEREDO_AUTH_HDR_FLAG))
			{
				//rfc4380 5.1.1 teredo 负载0x0001时为Teredo authentication headers，需要跳过
				tmp_hdr_len += sizeof(struct teredo_auth_hdr) + ntohs(p_teredo_hdr->au_len) + ntohs
				(p_teredo_hdr->id_len) + 8 + 1;
				next_layer_hdr += tmp_hdr_len;
				teredo_layer_len += tmp_hdr_len;
			}
			else if(p_teredo_hdr->flags == ntohs(TEREDO_INDICATION_HDR_FLAG))
			{
				//rfc4380 teredo 负载0x0000时为Teredo indication headers，需要跳过
				next_layer_hdr += TEREDO_INDICATION_HDR_LEN;
				teredo_layer_len += TEREDO_INDICATION_HDR_LEN;
			}
            else
            {
                printf("udp_jump_to_layer(): unsupport teredo hdr:0x%d!\n", *(unsigned int *)(next_layer_hdr));
                return -1;
            }
		}
		skip_len = ipv6_jump_to_layer(next_layer_hdr, __ADDR_TYPE_IP_PAIR_V6, expect_layer_type);
		if(skip_len < 0){
			return -1;
		}
		skip_len += teredo_layer_len;
	}else{
		/* 其他UDP类型不支持再跳转 */
		return -1;
	}

	return skip_len + sizeof(struct mesa_udp_hdr);
}

static int ipv4_jump_to_layer(const char *raw_data,  int raw_layer_type, int expect_layer_type)
{
	struct mesa_ip4_hdr *p_ip_hdr = (struct mesa_ip4_hdr *)raw_data;
	int skip_len = 0;
	int ip_hdr_len = p_ip_hdr->ip_hl * 4;
	//const char *next_layer_data = raw_data + ip_hdr_len;

	if(raw_layer_type == expect_layer_type){
		return 0;
	}

	if((ntohs(p_ip_hdr->ip_off) & IP_MF ) || (ntohs(p_ip_hdr->ip_off) & IP_OFFMASK)){
		/* IP分片不再继续向内层跳转 */
		return -1;
	}
				
	switch(p_ip_hdr->ip_p){
		case IPPROTO_TCP:
			if(ADDR_TYPE_TCP == expect_layer_type){
				skip_len = 0;
				break;
			}else{
				skip_len = -1; /* tcp 层之上不承载其他协议 */
			}
		break;

		case IPPROTO_UDP:
			if(ADDR_TYPE_UDP == expect_layer_type){
				skip_len = 0;
				break;
			}else{
				skip_len = udp_jump_to_layer(raw_data+ip_hdr_len, ADDR_TYPE_UDP, expect_layer_type); 
			}
		break;

		case IPPROTO_IPV6:
			if(__ADDR_TYPE_IP_PAIR_V6 == expect_layer_type){
				skip_len = 0;
				break;
			}else{
				skip_len = ipv6_jump_to_layer(raw_data+ip_hdr_len, __ADDR_TYPE_IP_PAIR_V6, expect_layer_type);
			}
		break;
		case IPPROTO_GRE:
			if((ADDR_TYPE_GRE == expect_layer_type) || (ADDR_TYPE_PPTP == expect_layer_type)){
				skip_len = 0;
				break;
			}else{
				skip_len = gre_jump_to_layer(raw_data+ip_hdr_len, ADDR_TYPE_GRE, expect_layer_type);
			}		
		break;
		
		default:
			skip_len = -1;
		break;
	}

	if(skip_len < 0){
		return -1;
	}

	return skip_len + sizeof(struct ip);
}

/*
 *	NextHeader field of IPv6 header
 */
#define NEXTHDR_HOP			0	/* Hop-by-hop option header. */
#define NEXTHDR_IPIP			4	/* IPIP header. */
#define NEXTHDR_TCP			6	/* TCP segment. */
#define NEXTHDR_UDP			17	/* UDP message. */
#define NEXTHDR_IPV6			41	/* IPv6 in IPv6 */
#define NEXTHDR_ROUTING		43	/* Routing header. */
#define NEXTHDR_FRAGMENT		44	/* Fragmentation/reassembly header. */
#define NEXTHDR_ESP			50	/* Encapsulating security payload. */
#define NEXTHDR_AUTH			51	/* Authentication header. */
#define NEXTHDR_ICMP			58	/* ICMP for IPv6. */
#define NEXTHDR_NONE			59	/* No next header */
#define NEXTHDR_DEST			60	/* Destination options header. */
#define NEXTHDR_MOBILITY		135	/* Mobility header. */

static int ipv6_jump_to_layer(const char *raw_data,  int raw_layer_type, int expect_layer_type)
{
	const struct mesa_ip6_hdr *a_packet = (const struct mesa_ip6_hdr *)raw_data;
	UINT8 next_hdr_type = a_packet->ip6_nxt_hdr;
	UINT8 *next_hdr_ptr = (UINT8 *)a_packet + sizeof(struct mesa_ip6_hdr);	
	int skip_len = 0;
	int offset_to_ip6 = 0;
	
	if(raw_layer_type == expect_layer_type){
		return 0;
	}

	while(1){
		offset_to_ip6 = 0;
		switch(next_hdr_type)
		{
			case NEXTHDR_HOP:
			case NEXTHDR_ROUTING:
			case NEXTHDR_AUTH:
			case NEXTHDR_DEST:
				offset_to_ip6 = (*(next_hdr_ptr + 1))*8 + 8; /* 选项长度以8字节为单位 */				
			break;

			case NEXTHDR_IPIP:
				if(__ADDR_TYPE_IP_PAIR_V4 == expect_layer_type){
					skip_len = next_hdr_ptr - (UINT8 *)raw_data;
				}else{
					skip_len = ipv4_jump_to_layer((const char *)next_hdr_ptr, __ADDR_TYPE_IP_PAIR_V4, expect_layer_type);
					if(skip_len < 0){
						return -1;
					}else{
						return skip_len + next_hdr_ptr - (UINT8 *)raw_data;
					}
				}
				goto done;
			break;

			case NEXTHDR_NONE:
				skip_len = -1;
				goto done;					
			break;

			case NEXTHDR_ICMP: /* IMCP不再承载其他协议 */
				skip_len = -1;
				goto done;
			break;
			
			case NEXTHDR_TCP:
				if(ADDR_TYPE_TCP == expect_layer_type){
					skip_len = next_hdr_ptr - (UINT8 *)raw_data;
				}else{
					skip_len = -1;
				}
				goto done;
			break;
				
			case NEXTHDR_UDP:
				if(ADDR_TYPE_UDP == expect_layer_type){
					skip_len = next_hdr_ptr - (UINT8 *)raw_data;
				}else{
					/* TODO: IPv6的其他隧道模式 */
					skip_len = -1;
				}
				goto done;
			break;

			case NEXTHDR_FRAGMENT:	
				/* IP分片不再继续向内层跳转 */
				skip_len = -1;
				goto done;
			break;

			case NEXTHDR_ESP:
				skip_len = -1;
				goto done;

			default:
				printf("ipv6_jump_to_layer(): unknown IPv6 header type:0x%x!\n", next_hdr_type);		
				skip_len = -1;
				goto done;
			break;
		}

		next_hdr_type = *next_hdr_ptr;
		next_hdr_ptr += offset_to_ip6;
	}	

done:
	if(skip_len < 0){
		return -1;
	}
	
	return skip_len;
}

static int ppp_jump_to_layer(const char *raw_data,  int raw_layer_type, int expect_layer_type)
{
	int skip_len = 0;
	struct mesa_pppoe_session_hdr *pppoe_ses_hdr;
	char *next_hdr;
	
	if(raw_layer_type == expect_layer_type){
		return 0;
	}
	pppoe_ses_hdr = (struct mesa_pppoe_session_hdr *)raw_data;
	next_hdr = (char *)raw_data + sizeof(struct mesa_pppoe_session_hdr);

	switch(ntohs(pppoe_ses_hdr->ppp_protocol)){
		case PPP_PROTOCOL_IPv4:
			if(__ADDR_TYPE_IP_PAIR_V4 == expect_layer_type){
				break;
			}else{
				skip_len = ipv4_jump_to_layer(next_hdr, __ADDR_TYPE_IP_PAIR_V4, expect_layer_type);
			}
		break;

		case PPP_IPV6:
			if(__ADDR_TYPE_IP_PAIR_V6 == expect_layer_type){
				break;
			}else{
				skip_len = ipv6_jump_to_layer(next_hdr, __ADDR_TYPE_IP_PAIR_V4, expect_layer_type);
			}		
		break;

		case PPP_COMP:
		case PPP_CCP:
		case PPP_IPCP:
		case PPP_PAP:
		case PPP_CHAP:
		case PPP_LQM:
		case PPP_PROTOCOL_LCP: 
		
			/* 不承载应用层协议 */
			skip_len = -1;
		break;

		default:
			printf("ppp_jump_to_layer(): unsupport ppp pro:0x%x!\n", ntohs(pppoe_ses_hdr->ppp_protocol));
		break;
		
	}

	if(skip_len < 0){
		return -1;
	}
	
	return skip_len + sizeof(struct mesa_pppoe_session_hdr);
}
static int guess_mpls_with_control_word(const unsigned char *maybe_eth_hdr)
{
	const struct mesa_ethernet_hdr *ehdr = (struct mesa_ethernet_hdr *)(maybe_eth_hdr);

	/* 
	    MPLS没有字段表示承载的协议类型, 靠猜!!
	    RFC4623, or https://wiki.mikrotik.com/wiki/Manual:VPLS_Control_Word
	    除标准的ipv4, ipv6之外, 还有可能是ethernet, 还有可能是带PW Ethernet Control Word的, 格式如下:
	    
        0               1               2                 3
		0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2
		+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
		|0 0 0 0| flags |frag|len(6bit) |    Sequence Number(16bit)     |
		+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+

		实际情况证明, 靠4bit是否==0为条件, 还是可能猜错, 再判断一下以太类型是否为常见类型.
	*/

	switch(ntohs(ehdr->ether_type)){
		case ETH_P_IP:
		case ETH_P_IPV6:
		case ETH_P_8021Q:
		case ETH_P_MPLS_UC:
		case ETH_P_PPP_SES:
			return 0; /* 合法的ethernet类型, 不含CW */
		break;
	}

	ehdr = (struct mesa_ethernet_hdr *)(maybe_eth_hdr + 4);
	switch(ntohs(ehdr->ether_type)){
		case ETH_P_IP:
		case ETH_P_IPV6:
		case ETH_P_8021Q:
		case ETH_P_MPLS_UC:
		case ETH_P_PPP_SES:
			return 1; /* 合法的ethernet类型, 包含CW */
		break;
	}

	/* TODO: 以上都不是, 此处应该返回个什么值? */
	return 0;
}

static int set_mpls_addr(struct layer_addr_mpls *addr, const unsigned char *raw_mpls_pkt_data)
{
	const struct mesa_mpls_hdr *this_mpls_hdr;
	int i;
	
    memset(addr, 0, sizeof(struct layer_addr_mpls));
	for(i = 0; i < MAX_MPLS_ADDR_LAYER; i++)
	{
		this_mpls_hdr = (const struct mesa_mpls_hdr *)raw_mpls_pkt_data;
		memcpy(&addr->src_mpls_pkt[i], raw_mpls_pkt_data,  sizeof(struct mesa_mpls_hdr));
		addr->src_mpls_layer_num += 1;
		if(1 == this_mpls_hdr->mpls_bls){
			raw_mpls_pkt_data += sizeof(struct mesa_mpls_hdr); /* 为了后面方便判断是否还有ctrl word */
			break;
		}
		raw_mpls_pkt_data += sizeof(struct mesa_mpls_hdr);
	}

	if(1 != this_mpls_hdr->mpls_bls) /* 超过MAX_MPLS_ADDR_LAYER, MPLS还没有结束 */
	{
		printf("MPLS layer number over load, only support %d\n", MAX_MPLS_ADDR_LAYER);
		return -1;
	}

	if(((*raw_mpls_pkt_data & 0xF0) != 0x40) && ((*raw_mpls_pkt_data & 0xF0) != 0x60)){ //VPLS, MPLS with Control Word
		if(guess_mpls_with_control_word(raw_mpls_pkt_data) > 0){
			memcpy(&addr->src_mpls_ctrl_word, raw_mpls_pkt_data,  sizeof(int));
			addr->src_has_ctrl_word = 1;
		}
	}

	return 0;
}

static int mpls_jump_to_layer(const char *raw_data,  int raw_layer_type, int expect_layer_type)
{
	int skip_len = 0;
	struct layer_addr_mpls mpls_addr = {};
	const char *next_layer_data;
	int mpls_layer_len;

	if(raw_layer_type == expect_layer_type){
		return 0;
	}

	set_mpls_addr(&mpls_addr, (unsigned char *)raw_data);
	mpls_layer_len = mpls_addr.src_mpls_layer_num * sizeof(int);
	if(mpls_addr.src_has_ctrl_word){
		mpls_layer_len += sizeof(int);
	}

	next_layer_data = raw_data + mpls_layer_len;

	/* MPLS没有字段标识下一层是什么, 靠猜测下一层的IP类型 */
	if((*next_layer_data & 0xF0) == 0x40){
		skip_len = ipv4_jump_to_layer(next_layer_data, __ADDR_TYPE_IP_PAIR_V4, expect_layer_type);
	}else if((*next_layer_data & 0xF0) == 0x60){
		skip_len = ipv6_jump_to_layer(next_layer_data, __ADDR_TYPE_IP_PAIR_V6, expect_layer_type);
	}else{	
		/* VPLS和不带control word一并处理, 在set_mpls_addr()函数中, 如果包括Control word, next_layer_data已经跳过了四字节的control word  */
		skip_len = eth_jump_to_layer(next_layer_data, ADDR_TYPE_MAC, expect_layer_type);
		if(skip_len < 0){
			printf("WARNING: jmp unsupport type in MPLS to Ethernet, 0x%x!\n",  
							(unsigned char)(*next_layer_data));
			return -1;
		}
	}

	return skip_len + mpls_layer_len;
}

static int __common_eth_type_dispatch(UINT16 eth_type, const char *next_layer_data, int raw_layer_type, int expect_layer_type)
{
	int skip_len = 0;
	
	switch(eth_type){
		case ETH_P_ARP:
			if(ADDR_TYPE_ARP == expect_layer_type){
				break;
			}else{
				skip_len = arp_jump_to_layer(next_layer_data, ADDR_TYPE_ARP, expect_layer_type);
			}
		break;

		case ETH_P_8021Q:
			if(ADDR_TYPE_VLAN == expect_layer_type){
				break;
			}else{
				if(treat_vlan_as_mac_in_mac_sw){
					skip_len = eth_jump_to_layer(next_layer_data, ADDR_TYPE_MAC, expect_layer_type);
				}else{
					skip_len = vlan8021q_jump_to_layer(next_layer_data, ADDR_TYPE_VLAN, expect_layer_type);
				}
			}
		break;
		
		case ETH_P_IP:
			if(__ADDR_TYPE_IP_PAIR_V4 == expect_layer_type){
				break;
			}else{
				skip_len = ipv4_jump_to_layer(next_layer_data, __ADDR_TYPE_IP_PAIR_V4, expect_layer_type);
			}
		break;

		case ETH_P_IPV6:
			if(__ADDR_TYPE_IP_PAIR_V6 == expect_layer_type){
				break;
			}else{
				skip_len = ipv6_jump_to_layer(next_layer_data, __ADDR_TYPE_IP_PAIR_V6, expect_layer_type);
			}
		break;

		case ETH_P_PPP_SES:
			if(ADDR_TYPE_PPPOE_SES == expect_layer_type){
				break;
			}else{
				skip_len = ppp_jump_to_layer(next_layer_data, ADDR_TYPE_PPPOE_SES, expect_layer_type);
			}
		break;		

		case 0x88A8: /* MAC_IN_MAC */
			skip_len = eth_jump_to_layer(next_layer_data, ADDR_TYPE_MAC, expect_layer_type);
		break;

		case 0x8847: /* MPLS, ETH_P_MPLS_UC */
			skip_len = mpls_jump_to_layer(next_layer_data, ADDR_TYPE_MPLS, expect_layer_type);
		break;
		
		default:
			skip_len = -1;
		break;
	}

	return skip_len;
}

static int set_vlan_addr(struct layer_addr_vlan *addr, const unsigned char *vlan_tag)
{
	int i;
	const struct mesa_vlan_hdr *vhdr;
	int vlan_layer_len = 0;

	memset(addr, 0, sizeof(struct layer_addr_vlan));

	addr->vlan_id = ntohs(*(unsigned short *)vlan_tag) & VLAN_ID_MASK; //compat old value

	for(i = 0; i < MAX_VLAN_ADDR_LAYER; i++){
		vhdr = (struct mesa_vlan_hdr *)vlan_tag;
		memcpy(&addr->src_vlan_pkt[i], vlan_tag, sizeof(struct mesa_vlan_hdr));
		vlan_tag += sizeof(struct mesa_vlan_hdr);
		addr->src_vlan_layer_num++;
		vlan_layer_len += sizeof(struct mesa_vlan_hdr);
		if(ETH_P_8021Q != ntohs(vhdr->type)){
			break;
		}
	}
	
	return vlan_layer_len;
}

static int vlan8021q_jump_to_layer(const char *raw_data,  int raw_layer_type, int expect_layer_type)
{
	int skip_len = 0;
	struct layer_addr_vlan vlan_addr;
	const char *next_layer_data;
	const struct mesa_vlan_hdr *vhdr;
	unsigned short next_layer_type;
	int vlan_layer_len;

	if(raw_layer_type == expect_layer_type){
		return 0;
	}

	set_vlan_addr(&vlan_addr, raw_data);
	vlan_layer_len = sizeof(struct mesa_vlan_hdr) * vlan_addr.src_vlan_layer_num;
	next_layer_data = raw_data + vlan_layer_len;
	vhdr = (struct mesa_vlan_hdr *)&vlan_addr.src_vlan_pkt[vlan_addr.src_vlan_layer_num-1];
	next_layer_type = ntohs(vhdr->type);
	
	switch(next_layer_type){
		case ETH_P_ARP:
			if(ADDR_TYPE_ARP == expect_layer_type){
				break;
			}else{
				skip_len = arp_jump_to_layer(next_layer_data, ADDR_TYPE_ARP, expect_layer_type);
			}
		break;
		
		case ETH_P_IP:
			if(__ADDR_TYPE_IP_PAIR_V4 == expect_layer_type){
				break;
			}else{
				skip_len = ipv4_jump_to_layer(next_layer_data, __ADDR_TYPE_IP_PAIR_V4, expect_layer_type);
			}
		break;

		case ETH_P_IPV6:
			if(__ADDR_TYPE_IP_PAIR_V6 == expect_layer_type){
				break;
			}else{
				skip_len = ipv6_jump_to_layer(next_layer_data, __ADDR_TYPE_IP_PAIR_V6, expect_layer_type);
			}
		break;

		case ETH_P_PPP_SES:
			if(ADDR_TYPE_PPPOE_SES == expect_layer_type){
				break;
			}else{
				skip_len = ppp_jump_to_layer(next_layer_data, ADDR_TYPE_PPPOE_SES, expect_layer_type);
			}
		break;		

		case ETH_P_PPP_DISC: /* pppoe发现阶段 */
			skip_len = -1;
		break;

		/* QinQ */
		case ETH_P_8021Q:
			printf("vlan8021q_jump_to_layer(): multiple VLAN combine to one layer!\n");
			assert(0);
		break;

		case ETH_P_MPLS_UC:
			skip_len = mpls_jump_to_layer(next_layer_data, ADDR_TYPE_MPLS, expect_layer_type);
		break;
		
		default:
			printf("vlan8021q_jump_to_layer(): unsupport type: 0x%x!\n", next_layer_type);
			skip_len = -1;
	}

	if(skip_len < 0){
		return -1;
	}

	return skip_len + vlan_layer_len;
}

static int eth_jump_to_layer(const char *raw_data,  int raw_layer_type, int expect_layer_type)
{
	struct ethhdr *p_eth_hdr = (struct ethhdr *)raw_data;
	unsigned short eth_type = ntohs(p_eth_hdr->h_proto);
	//int skip_len = -1;
	const char *next_layer_data = raw_data + sizeof(struct ethhdr);
	int layer_skip_len;

	if(raw_layer_type == expect_layer_type){
		return 0;
	}
	
	layer_skip_len =  __common_eth_type_dispatch(eth_type, next_layer_data, raw_layer_type, expect_layer_type);
	if(layer_skip_len < 0){
		return -1;
	}

	return layer_skip_len + sizeof(struct ethhdr);
}


static int mac_in_mac_jump_to_layer(const char *raw_data,  int raw_layer_type, int expect_layer_type)
{
	struct ethhdr *inner_eth_hdr = (struct ethhdr *)(raw_data + sizeof(struct ethhdr ));
	unsigned short inner_eth_type = ntohs(inner_eth_hdr->h_proto);
	//int skip_len = -1;
	const char *next_layer_data = raw_data + sizeof(struct ethhdr);
	int layer_skip_len;

	if(raw_layer_type == expect_layer_type){
		return 0;
	}
	
	layer_skip_len =  __common_eth_type_dispatch(inner_eth_type, next_layer_data, raw_layer_type, expect_layer_type);
	if(layer_skip_len < 0){
		return -1;
	}

	return layer_skip_len + sizeof(struct ethhdr) * 2;
}

static inline int calc_gre_hdr_len_v0(const struct mesa_gre_base_hdr_v0  *gre_base_hdr_v0)
{
	int gre_hdr_len = sizeof(struct mesa_gre_base_hdr_v0);

	if(gre_base_hdr_v0->checksum_flag || gre_base_hdr_v0->route_flag){
		gre_hdr_len += sizeof(short) * 2;	/* checksum and offset  */	
	}

	if(gre_base_hdr_v0->key_flag){
		gre_hdr_len += sizeof(int );
	}
	
	if(gre_base_hdr_v0->seq_flag){
		gre_hdr_len += sizeof(int );
	}

	return gre_hdr_len;	
}

static inline int calc_gre_hdr_len_v1(const struct mesa_gre_base_hdr_v1  *gre_base_hdr_v1)
{
	int gre_hdr_len = sizeof(struct mesa_gre_base_hdr_v1);

	if(gre_base_hdr_v1->checksum_flag != 0){
		printf("error! in gre version1, checksum flag not zero!\n");
		return -1;
	}

	if(gre_base_hdr_v1->route_flag != 0){
		printf("error! in gre version1, route flag not zero!\n");
		return -1;
	}

	if(gre_base_hdr_v1->recur != 0){
		printf("error! in gre version1, recur flag not zero!\n");
		return -1;
	}
	
	if(gre_base_hdr_v1->strict_src_route_flag != 0){
		printf("error! in gre version1, strict_src_route flag not zero!\n");
		return -1;
	}
	
	if(ntohs(gre_base_hdr_v1->protocol) != GRE_PRO_PPP){
		printf("error! in gre version1, protocol not 0x%x!\n", GRE_PRO_PPP);
		return -1;
	}	

	gre_hdr_len += sizeof(short) * 2;	/* payload_len and callid */
	
	if(gre_base_hdr_v1->seq_flag){
		gre_hdr_len += sizeof(int);
	}
	
	/* version 1 has ack number */
	if(gre_base_hdr_v1->ack_flag){
		gre_hdr_len += sizeof(int);
	}	

	return gre_hdr_len;
}

int calc_gre_hdr_len(const struct mesa_gre_hdr *gre_hdr)
{
	int gre_hdr_len = -1;

	if(0 == gre_hdr->gre_base.version){
		gre_hdr_len = calc_gre_hdr_len_v0(&gre_hdr->gre_base);
	}else if(1 == gre_hdr->gre_base.version){
		gre_hdr_len = calc_gre_hdr_len_v1((const struct mesa_gre_base_hdr_v1 *)&gre_hdr->gre_base);
	}else{
		gre_hdr_len = -1;
	}

	return gre_hdr_len;
}

static int set_gre_hdr_ver0(struct mesa_gre_hdr *stack_gre_addr, const struct mesa_gre_hdr *net_gre_addr)
{
	struct mesa_gre_extend_hdr *stack_gre_ext = &stack_gre_addr->gre_extend;
	const struct mesa_gre_base_hdr_v0 *net_gre_base = &net_gre_addr->gre_base;
	const char *net_ext_hdr_value = (const char *)(&net_gre_addr->gre_extend);
	//const struct gre_source_route_entry_hdr *rse_hdr;

	if(net_gre_base->checksum_flag || net_gre_base->route_flag){
		stack_gre_ext->checksum = *((unsigned short *)net_ext_hdr_value);
		net_ext_hdr_value += sizeof(short);

		/* 如果checksum存在, 则offset也必存在 */
		stack_gre_ext->offset = *((unsigned short *)net_ext_hdr_value);
		net_ext_hdr_value += sizeof(short);		
	}

	if(net_gre_base->key_flag){
		stack_gre_ext->key = *((unsigned int *)net_ext_hdr_value);
		net_ext_hdr_value += sizeof(int);
	}
	
	if(net_gre_base->seq_flag){
		stack_gre_ext->seq_num = *((unsigned int *)net_ext_hdr_value);
		net_ext_hdr_value += sizeof(int);
	}
	
	/* SRE信息在GRE头部的最后, 因为不定长, 不定数量 */
	if(net_gre_base->route_flag){
		//rse_hdr = (const struct gre_source_route_entry_hdr *)net_ext_hdr_value;
		//TODO 1, copy SRE
		printf("found GRE SRE data, but not parse yet, TODO!\n");
		return -1;
	}

	return sizeof(struct mesa_gre_base_hdr_v1) + (net_ext_hdr_value - (char *)&net_gre_addr->gre_extend);	
}


static int set_gre_hdr_ver1(struct mesa_gre_hdr *stack_gre_addr, const struct mesa_gre_hdr *net_gre_addr)
{
	//struct mesa_gre_base_hdr_v1 *stack_gre_base = (struct mesa_gre_base_hdr_v1 *)&stack_gre_addr->gre_base;
	struct mesa_gre_extend_hdr *stack_gre_ext = &stack_gre_addr->gre_extend;
	const struct mesa_gre_base_hdr_v1 *net_gre_base = (struct mesa_gre_base_hdr_v1 *)&net_gre_addr->gre_base;
	//const struct mesa_gre_extend_hdr *net_gre_ext = &net_gre_addr->gre_extend;
	const char *net_ext_hdr_value = (const char *)(&net_gre_addr->gre_extend);

	if(net_gre_base->checksum_flag != 0){
		printf("error! in gre version1, checksum flag not zero!\n");
		return -1;
	}

	if(net_gre_base->route_flag != 0){
		printf("error! in gre version1, route flag not zero!\n");
		return -1;
	}

	if(net_gre_base->recur != 0){
		printf("error! in gre version1, recur flag not zero!\n");
		return -1;
	}
	
	if(net_gre_base->strict_src_route_flag != 0){
		printf("error! in gre version1, strict_src_route flag not zero!\n");
		return -1;
	}
	
	if(ntohs(net_gre_base->protocol) != GRE_PRO_PPP){
		printf("error! in gre version1, protocol not 0x%x!\n", GRE_PRO_PPP);
		return -1;
	}	

	stack_gre_ext->payload_len = *((unsigned short *)net_ext_hdr_value);
	net_ext_hdr_value += sizeof(short);	
	
	stack_gre_ext->call_id = *((unsigned short *)net_ext_hdr_value);
	net_ext_hdr_value += sizeof(short);	
	
	if(net_gre_base->seq_flag){
		stack_gre_ext->seq_num = *((unsigned int *)net_ext_hdr_value);
		net_ext_hdr_value += sizeof(int);
	}
	
	/* version 1 has ack number */
	if(net_gre_base->ack_flag){
		stack_gre_ext->ack_num = *((unsigned int *)net_ext_hdr_value);
		net_ext_hdr_value += sizeof(int);
	}

	return sizeof(struct mesa_gre_base_hdr_v1) + (net_ext_hdr_value - (char *)&net_gre_addr->gre_extend);
}


int set_gre_hdr(struct mesa_gre_hdr *stack_gre_addr, const void *this_layer_data)
{
	int gre_hdr_len = 0;
	const struct mesa_gre_hdr *net_gre_addr = (const struct mesa_gre_hdr *)this_layer_data;

	memcpy(&stack_gre_addr->gre_base, &net_gre_addr->gre_base, sizeof(struct mesa_gre_base_hdr_v0));
	memset(&stack_gre_addr->gre_extend, 0, sizeof(struct mesa_gre_extend_hdr));
	
	if(0 == net_gre_addr->gre_base.version){
		gre_hdr_len = set_gre_hdr_ver0(stack_gre_addr, net_gre_addr);
	}else if(1 == net_gre_addr->gre_base.version){
		gre_hdr_len = set_gre_hdr_ver1(stack_gre_addr, net_gre_addr);
	}else{
		//TODO 1, 未知版本
		printf("Unknown gre hdr version:%d\n", net_gre_addr->gre_base.version);
		gre_hdr_len = -1;
	}

	return gre_hdr_len;
}

static int gre_jump_to_layer(const char *raw_data,  int raw_layer_type, int expect_layer_type)
{
	int skip_len = 0;
	int this_gre_layer_len;
	struct mesa_gre_hdr this_layer_hdr;
	
	if(raw_layer_type == expect_layer_type){
		return 0;
	}

	this_gre_layer_len = set_gre_hdr(&this_layer_hdr, (void *)raw_data);
	if(this_gre_layer_len < 0){
		return -1;
	}

	switch(ntohs(this_layer_hdr.gre_base.protocol))
	{
		case GRE_PRO_IPV4:
			if(expect_layer_type == ADDR_TYPE_IPV4){
				skip_len = 0;
				break;
			}else{
				skip_len=ipv4_jump_to_layer(raw_data+this_gre_layer_len, __ADDR_TYPE_IP_PAIR_V4, expect_layer_type);
			}
		break;

		case GRE_PRO_IPV6:
			if(expect_layer_type == ADDR_TYPE_IPV4){
				skip_len = 0;
				break;
			}else{
				skip_len=ipv6_jump_to_layer(raw_data+this_gre_layer_len, __ADDR_TYPE_IP_PAIR_V6, expect_layer_type);
			}
		break;
		
		case GRE_PRO_PPP:
			if((expect_layer_type == ADDR_TYPE_PPP) || (expect_layer_type == ADDR_TYPE_PPTP)){
				skip_len = this_gre_layer_len;
				break;
			}else{
				skip_len = ppp_jump_to_layer(raw_data+this_gre_layer_len, ADDR_TYPE_PPP, expect_layer_type);
			}	
		break;
		
		default:
			printf("gre_jump_to_layer(): unknown gre protocol:0x%x!\n", ntohs(this_layer_hdr.gre_base.protocol));		
			return -1;
			break;
	}

	if(skip_len < 0){
		printf("gre_jump_to_layer() error!\n");	
		return -1;
	}

	return skip_len + this_gre_layer_len;
}
/*
	return value:
		Non-NULL: the pointer to expect layer;
		NULL: not found expect layer.
*/
const void *MESA_net_jump_to_layer(const void *raw_data,  int raw_layer_type, int expect_layer_type)
{
	int  ret;

	if(raw_layer_type <= __ADDR_TYPE_INIT || raw_layer_type >= __ADDR_TYPE_MAX){
		return NULL;
	}

	if(expect_layer_type <= __ADDR_TYPE_INIT || expect_layer_type >= __ADDR_TYPE_MAX){
		return NULL;
	}


	if(ADDR_TYPE_IPV4 == expect_layer_type){
		/* 转成纯IPv4地址类型 */
		expect_layer_type = __ADDR_TYPE_IP_PAIR_V4;
	}

	if(ADDR_TYPE_IPV6 == expect_layer_type){
		/* 转成纯IPv6地址类型 */
		expect_layer_type = __ADDR_TYPE_IP_PAIR_V6;
	}

	if(raw_layer_type == expect_layer_type){
		return raw_data;
	}
	
	switch(raw_layer_type){
		case ADDR_TYPE_MAC:
			ret = eth_jump_to_layer((const char *)raw_data, raw_layer_type, expect_layer_type);
		break;
		
		case ADDR_TYPE_ARP:
			ret = arp_jump_to_layer((const char *)raw_data, raw_layer_type, expect_layer_type);
		break;
		case ADDR_TYPE_VLAN:
			if(treat_vlan_as_mac_in_mac_sw == 1){
				ret = mac_in_mac_jump_to_layer((const char *)raw_data, raw_layer_type, expect_layer_type);
			}else{
				ret = vlan8021q_jump_to_layer((const char *)raw_data, raw_layer_type, expect_layer_type);
			}
		break;
		
		case __ADDR_TYPE_IP_PAIR_V4:
			ret = ipv4_jump_to_layer((const char *)raw_data, raw_layer_type, expect_layer_type);
		break;

		case __ADDR_TYPE_IP_PAIR_V6:
			ret = ipv6_jump_to_layer((const char *)raw_data, raw_layer_type, expect_layer_type);
		break;

		case ADDR_TYPE_MAC_IN_MAC:
			ret = mac_in_mac_jump_to_layer((const char *)raw_data, raw_layer_type, expect_layer_type);
		break;

		case ADDR_TYPE_UDP:
			ret = udp_jump_to_layer((const char *)raw_data, raw_layer_type, expect_layer_type);
		break;

		case ADDR_TYPE_MPLS:	
			ret = mpls_jump_to_layer((const char *)raw_data, raw_layer_type, expect_layer_type);
		break;
		
		case ADDR_TYPE_GRE:
			ret = gre_jump_to_layer((const char *)raw_data, raw_layer_type, expect_layer_type);
		break;
		
		case ADDR_TYPE_PPPOE_SES:	
			return NULL;
		break;
		default:
			printf("MESA_net_jump_to_layer(): unsupport raw_layer_type:%d in MESA_net_jump_to_layer()!\n", raw_layer_type);
			return NULL;
	}

	if(ret < 0){
		return NULL;
	}

	return ((const char *)raw_data + ret);
}

/*
	与MESA_net_jump_to_layer()的区别:
		MESA_net_jump_to_layer()从最外层开始, 找到第一个符合条件的层就退出;
		MESA_net_jump_to_layer_greedy()会一直尝试找最内层协议头, 适合隧道模式.
	
	return value:
		Non-NULL: the pointer to expect layer;
		NULL: not found expect layer.
*/
const void *MESA_net_jump_to_layer_greedy(const void *raw_data, int raw_layer_type, int expect_layer_type)
{
	const void *expect_layer;
	const void *success_layer = NULL; /* 最后一次成功找到的层 */
	int new_raw_layer_type = raw_layer_type; /* 在跳转过程中, 可能会更新中间层信息 */
	const char *new_next_layer_data = (char *)raw_data;

	expect_layer = MESA_net_jump_to_layer(new_next_layer_data, new_raw_layer_type, expect_layer_type);
	while(expect_layer){
		success_layer = expect_layer;

		switch(expect_layer_type){
			case __ADDR_TYPE_IP_PAIR_V4:
			{
				const struct mesa_ip4_hdr *ip4hdr = (const struct mesa_ip4_hdr *)expect_layer;
				if((ntohs(ip4hdr->ip_off) & IP_MF ) || (ntohs(ip4hdr->ip_off) & IP_OFFMASK)){
					/* IP分片不再继续向内层跳转 */
					goto done;
				}				
				if(IPPROTO_UDP == ip4hdr->ip_p){
					new_next_layer_data = (char *)expect_layer + ip4hdr->ip_hl * 4;
					new_raw_layer_type = ADDR_TYPE_UDP; /* IP层如果继续向下一层偏移, 只支持UDP, IPIP, GRE, L2TPv3. */
				}else if(IPPROTO_GRE == ip4hdr->ip_p){
					new_next_layer_data = (char *)expect_layer + ip4hdr->ip_hl * 4;
					new_raw_layer_type = ADDR_TYPE_GRE; /* GRE */					
				}else{
					//TODO 2, IPIP, L2TPv3
					goto done;
				}
			}
			break;

			case __ADDR_TYPE_IP_PAIR_V6:
			{
				//TODO2,
				goto done;		
			}
			break;

			default:
				printf("MESA_net_jump_to_layer_greedy() not support layer type:%d\n", expect_layer_type);
				goto done;
		}

		expect_layer = MESA_net_jump_to_layer(new_next_layer_data, new_raw_layer_type, expect_layer_type);
	}

done:
	return success_layer;
}

UINT8 net_layer_to_ipv4_protocol(int addr_type)
{
	UINT8 proto = 0;

	switch(addr_type){
		case __ADDR_TYPE_IP_PAIR_V4:
			proto = IPPROTO_IPIP;
		break;

		case __ADDR_TYPE_IP_PAIR_V6:
			proto = IPPROTO_IPV6;
		break;

		case ADDR_TYPE_TCP:
			proto = IPPROTO_TCP;
		break;

		case ADDR_TYPE_UDP:
			proto = IPPROTO_UDP;
		break;

		case ADDR_TYPE_GRE:
			proto = IPPROTO_GRE;
		break;

		default:
			printf("net_layer_to_ipv4_protocol(): unknown ip4 protocolr:%d\n", addr_type);
			proto = 0xFF;
		break;
	}

	return proto;
}

UINT8 net_layer_to_ipv6_protocol(int addr_type)
{
	UINT8 proto = 0;

	switch(addr_type){
		case ADDR_TYPE_TCP:
			proto = NEXTHDR_TCP;
		break;

		case ADDR_TYPE_UDP:
			proto = NEXTHDR_UDP;
		break;

		case __ADDR_TYPE_IP_PAIR_V4:
			proto = NEXTHDR_IPIP;
		break;

		default:
			printf("net_layer_to_ipv6_protocol(): unknown ip6 next-hdr:%d\n", addr_type);
			return 0xFF;
		break;
	}

	return proto;
}

/*
	将MESA地址类型转换为标准Ethernet类型;
	
	return value:
		ethernet type, host order.
*/
UINT16 net_layer_to_ethernet_protocol(int addr_type)
{
	UINT16 ether_type = 0;

	switch(addr_type){
		case __ADDR_TYPE_IP_PAIR_V4:
		case ADDR_TYPE_IPV4:
			ether_type = ETH_P_IP;
		break;

		case __ADDR_TYPE_IP_PAIR_V6:
		case ADDR_TYPE_IPV6:
			ether_type = ETH_P_IPV6;
		break;

		case ADDR_TYPE_VLAN:
			ether_type = ETHERTYPE_VLAN;
		break;

		case ADDR_TYPE_TCP:
		case ADDR_TYPE_UDP:
			printf("%s:%d: Ethernet can't carry addr type:%d directly!\n", __FILE__, __LINE__,addr_type);
			//assert(0);
			ether_type = -1;
		break;

		case ADDR_TYPE_PPPOE_SES:
			ether_type = ETH_P_PPP_SES;
		break;

		case ADDR_TYPE_MPLS:
			ether_type = ETH_P_MPLS_UC;
		break;

		case ADDR_TYPE_ARP:
			ether_type = ETH_P_ARP;
		break;

		case ADDR_TYPE_MAC:
			if(treat_vlan_as_mac_in_mac_sw==1){
				ether_type = ETHERTYPE_VLAN;
			}else{
				ether_type = ETHERTYPE_PANGU_MAC_IN_MAC;
			}
		break;
		
		default:
			/* to do, unknown type */
			printf("%s:%d: unknown ethernet carry addr type:%d!\n", __FILE__, __LINE__, addr_type);
			ether_type = -1;
		break;
	}

	return ether_type;
}


/*
	将标准Ethernet类型转换为MESA地址类型;
*/
enum addr_type_t ethernet_protocol_to_net_layer(UINT16 ether_type_host)
{
	enum addr_type_t addrtype = __ADDR_TYPE_INIT;

	switch(ether_type_host){
		case ETH_P_IP:
			addrtype = __ADDR_TYPE_IP_PAIR_V4;
		break;

		case ETH_P_IPV6:
			addrtype = __ADDR_TYPE_IP_PAIR_V6;
		break;

		case ETHERTYPE_VLAN:
			addrtype = ADDR_TYPE_VLAN;
		break;

		case ETH_P_PPP_SES:
			addrtype =  ADDR_TYPE_PPPOE_SES;
		break;

		case ETH_P_MPLS_UC :
			addrtype = ADDR_TYPE_MPLS;
		break;

		case ETH_P_ARP:
			addrtype = ADDR_TYPE_ARP;
		break;

		case ETHERTYPE_PANGU_MAC_IN_MAC:
			addrtype = ADDR_TYPE_MAC_IN_MAC;
		break;
		
		default:
			/* to do, unknown type */
			printf("%s:%d: unknown ethernet carry addr type:0x%x\n", __FILE__, __LINE__, ether_type_host);
			;
		break;
	}

	return addrtype;
}


/* 删除末尾的换行符"\r\n" */
void del_last_rn(char *data, int max_len)
{
    int i;
    for(i = 0; i < max_len; i++){
        if(('\r' == data[i]) || ('\n' == data[i])){
            data[i] = '\0';
            return;
        }
    }

    return;
}

/*	
	返回值:
		ethernet长度, 包括eth头部.
*/
int get_pkt_len_from_eth_hdr(const struct mesa_ethernet_hdr *ehdr)
{
	int raw_pkt_len = -1;

	switch(ntohs(ehdr->ether_type)){
		case ETHERTYPE_IP:
		{
			struct mesa_ip4_hdr *ip4hdr = (struct mesa_ip4_hdr *)((char *)ehdr + sizeof(struct mesa_ethernet_hdr));
			raw_pkt_len = ntohs(ip4hdr->ip_len) + sizeof(struct mesa_ethernet_hdr);
		}
		break;

		case ETHERTYPE_IPv6:
		{
			struct mesa_ip6_hdr *ip6hdr = (struct mesa_ip6_hdr *)((char *)ehdr + sizeof(struct mesa_ethernet_hdr));
			raw_pkt_len = ntohs(ip6hdr->ip6_payload_len) + sizeof(struct mesa_ip6_hdr) + sizeof(struct mesa_ethernet_hdr);	
		}
		break;

		default:
		break;
	}

	return raw_pkt_len;
}




#ifdef __cplusplus
}
#endif