/*
* Copyright c                  Realtek Semiconductor Corporation, 2002  
* All rights reserved.                                                
* 
* Program : parse pkt
*
* Original Creator: Edward Jin-Ru Chen 2002/4/29
* $Author: ikevin362 $
*
* $Revision: 1.1 $
* $Id: pktParse.c,v 1.1 2012/10/24 04:18:57 ikevin362 Exp $
* $Log: pktParse.c,v $
* Revision 1.1  2012/10/24 04:18:57  ikevin362
* initial version
*
* Revision 1.2  2005/07/20 15:29:38  yjlou
* +: porting Model Code to Linux Kernel: check RTL865X_MODEL_KERNEL.
*
* Revision 1.1  2004/02/25 14:27:24  chhuang
* *** empty log message ***
*
* Revision 1.3  2004/02/18 13:12:21  chenyl
* *** empty log message ***
*
* Revision 1.1  2004/02/13 05:18:47  chenyl
* + Pkt generating functions
*
* Revision 1.12  2003/10/06 10:16:23  waynelee
* add pptp gre support
*
* Revision 1.11  2003/09/30 09:40:55  waynelee
* support llc_other
*
* Revision 1.10  2002/10/25 11:22:12  waynelee
* change the rule of filling the content of payload
*
* Revision 1.9  2002/10/20 07:32:20  waynelee
* users can define the content of payload (but only indicate one byte)
*
* Revision 1.8  2002/10/03 13:34:54  waynelee
* support l3cks,l4cks,eflag fields
*
* Revision 1.7  2002/10/02 15:12:31  waynelee
* ipx support debug
*
* Revision 1.6  2002/10/01 07:47:33  waynelee
* support user-defined ipid, df, mf, ipoff
*
* Revision 1.5  2002/09/16 07:31:16  waynelee
* support IPX (experimental)
*
*/

#include "pktParse.h"
#include "utility.h"
// #include "pktForm.h"


void pktParse(int8 * pkt, pktParse_t * result, uint32 totalLen) {
	uint32	type, len, lenb;
	uint16 tmp, csum;
	int8 * nextPtr;
	uint8 nh, exhdrlen;
	uint8 dip[16];
	hopbyhopHdr_t * hopPtr;
	DesHdr_t * desPtr;
	DesHdr_t * desPtr2;
	RouHdr_t * rouPtr;
	FraHdr_t * fraPtr;
	AutHdr_t * autPtr;
	uint8 segmentleft=0;

	memset(result, 0, sizeof(pktParse_t));

	// total length
	result->totalLen = totalLen;

	//Parse layer2 headers
	result->ether2 = (_etherHdr_t *)pkt;
	nextPtr = pkt + sizeof(_etherHdr_t);
	type = ntohs(result->ether2->etherType);

	if(ntohs(result->ether2->etherType) == 0x8100) {
		result->vlan = (_vlanHdr_t *)nextPtr;
		result->pktFlag |= L2_VLAN;
		nextPtr += sizeof(_vlanHdr_t);
		type = ntohs(result->vlan->etherType);
	}

	if(type < 0x0600) {//802.3-2000 p.79
		result->llc = (_llcHdr_t *)nextPtr;
		result->pktFlag |= L2_SNAP;
		nextPtr += sizeof(_llcHdr_t);
		type = ntohs(result->llc->etherType);
	}

	if(type == 0x8864 || type == 0x8863 ) {
		result->pppoe = (_pppoeHdr_t *)nextPtr;
		result->pktFlag |= L2_PPPoE;
		nextPtr += sizeof(_pppoeHdr_t);
		type = ntohs(result->pppoe->proto);
	}

	//Parse layer3 headers, IP, ARP
	if( ((result->pktFlag & L2_PPPoE) == L2_PPPoE && type == 0x0021) ||
		((result->pktFlag & L2_PPPoE) == 0 && 
		  (((result->pktFlag & L2_SNAP) == L2_SNAP && result->llc->llc_dsap == 0xAA &&	result->llc->llc_ssap == 0xAA && result->llc->ctrl == 0x03) || 
		   (result->pktFlag & L2_SNAP) == 0) &&
			type == 0x0800) ) {
		result->ip = (ipHdr_t *)nextPtr;
		result->pktFlag = 0;
		result->pktFlag |= L3_IP;
		nextPtr += result->ip->ip_hl*4;
		type = result->ip->ip_p;
		len = ntohs(result->ip->ip_len) - result->ip->ip_hl*4;
		// store ip data length to dataLen
		result->dataLen = len;
		// ip payload
		result->payload = (uint8 *)nextPtr;
		if(ipcsum((uint16 *)result->ip, result->ip->ip_hl*4, 0) == 0)
			result->l3Flag |= PKT_PARSE_L3_CKSUM_OK;
		else {
			uint16 orgCksum = result->ip->ip_sum;
			
			// printf("Find l3 header error\n");
			result->l3Flag |= PKT_PARSE_L3_CKSUM_ERR;
			result->ip->ip_sum = 0;
			result->l3Cksum = ~ipcsum((uint16 *)result->ip, result->ip->ip_hl*4, 0);
			result->ip->ip_sum = orgCksum;
		}
	}
	else if  ((result->pktFlag & L2_PPPoE) == 0 && (type == 0x0806 || type == 0x8035)) {
		result->arp = (arpHdr_t *)nextPtr;
		result->pktFlag |= L3_ARP;
		nextPtr = NULL;//Arp do not parse futher
		return;
	}
	else if  (type == 0x8137) {
		result->ipx = (ipxHdr_t *)nextPtr;
		nextPtr = NULL;
		return;
	}
	//added by liujuan;
	else if ( ((result->pktFlag & L2_PPPoE) == L2_PPPoE && type == 0x0057) ||
                ((result->pktFlag & L2_PPPoE) == 0 &&
                  (((result->pktFlag & L2_SNAP) == L2_SNAP && result->llc->llc_dsap == 0xAA &&  result->llc->llc_ssap == 0xAA && result->llc->ctrl == 0x03) ||
                   (result->pktFlag & L2_SNAP) == 0) && type == 0x86DD)) {
		result->ipv6 =(ip6Hdr_t *)nextPtr;
		result->pktFlag |= L3_IPV6;

		nh=result->ipv6->ip_next;
		nextPtr= nextPtr+40;

	//	rtlglue_printf("nh is [%d]\n", nh);
		exhdrlen=0;
		while(1) {
			if(nh==0) {
				result->exheader |= hopbyhopHdr;
		//		nextPtr= nextPtr+40;
				hopPtr=(hopbyhopHdr_t *)nextPtr;
				nh=hopPtr->nexthdr;
				nextPtr=nextPtr+16;
				exhdrlen=exhdrlen+16;
			}
			else if (nh==60) {
				if(result->exheader & DesHdr) {
					result->exheader |=DesHdr2;
			//		nextPtr=nextPtr + 16;
					desPtr2=(DesHdr_t *)nextPtr;
					nh=desPtr2->nexthdr;
					nextPtr= nextPtr+16;
					exhdrlen=exhdrlen+16;
				}
				else {
					result->exheader |= DesHdr;
					desPtr=(DesHdr_t *)nextPtr;
					nh=desPtr->nexthdr;
					nextPtr=nextPtr+16;
					exhdrlen=exhdrlen+16;
				}
			}
			else if(nh==43) {
				result->exheader |= RouHdr;
				rouPtr = (RouHdr_t *)nextPtr;
				nh=rouPtr->nexthdr;
				segmentleft=rouPtr->segmentleft;
				memcpy(dip, nextPtr-8+rouPtr->hdrlen*8, 16);
				nextPtr=nextPtr + ((rouPtr->hdrlen * 8) +8);
				exhdrlen=exhdrlen+rouPtr->hdrlen*8+8;
		//		dip=rouPtr->dstip;
			}
			else if(nh==44) {
				result->exheader |= FraHdr;
				fraPtr=(FraHdr_t *)nextPtr;
				nh=fraPtr->nexthdr;
				nextPtr=nextPtr+8;
				exhdrlen=exhdrlen+8;
			}
			else if(nh==51) {
				result->exheader |= AutHdr;
				autPtr=(AutHdr_t *)nextPtr;
				nh=autPtr->nexthdr;
				nextPtr=nextPtr+20;
				exhdrlen=exhdrlen+20;
			}
			else {
		//		rtlglue_printf("IPV6 packet with error next header\n");
				break;
			}
		}  
		//if (result->ipv6->ip_next==hopbyhopHdr) {
			//result->exheader |= hopbyhopHdr;
	//		nextPtr = nextPtr + 40;
		//	hopPtr =(hopbyhopHdr_t *)nextPtr;
//		}

		if (nh==6)
			type=0x06;
		else if (nh==17)
			type=0x11;
		else if (nh==58)
			type=0x3a;
		else if (nh==59) {
			nextPtr=NULL;
			return;
		}
	//	rtlglue_printf("the ipv6 iplength is [%d]\n", result->ipv6->ip_len);
	//	rtlglue_printf("the ipv6 ip_len is [%d]\n", result->ipv6->ip_len);
	//	rtlglue_printf("the length of extension header is [%d]\n", exhdrlen);
		len=ntohs(result->ipv6->ip_len) - exhdrlen;
		//len=26-exhdrlen;
	//	rtlglue_printf("the len is [%d]\n", len);
		result->dataLen=len;
		
	}		
	else {
		result->payload = (uint8 *)nextPtr;
		// caculate the data len
		result->dataLen = totalLen - sizeof(_etherHdr_t);
		if((result->pktFlag & L2_VLAN) == L2_VLAN)
			result->dataLen -= sizeof(_vlanHdr_t);
		if((result->pktFlag & L2_SNAP) == L2_SNAP)
			result->dataLen -= sizeof(_llcHdr_t);
		if((result->pktFlag & L2_PPPoE) == L2_PPPoE)
			result->dataLen -= sizeof(_pppoeHdr_t);
		nextPtr = NULL;
		return;
	}

	//Parse layer4 headers
	if((result->pktFlag & L3_IP) == L3_IP) {
		uint16 orgCksum, phCksum;

		result->l4Flag = 0;
		switch(type) {
		case IP_PROTO_PPTP:
			result->pptp = (greHdr_t *) nextPtr;
			nextPtr += sizeof(greHdr_t);
			result->payload = (uint8 *)nextPtr;
			result->dataLen -= sizeof(greHdr_t);
			result->pktFlag |= L4_PPTP;
			break;
		case IP_PROTO_ICMP:
			result->icmp = (icmpHdr_t *) nextPtr;
			if(ipcsum((uint16 *)result->icmp, len, 0)==0)
				result->l4Flag |= PKT_PARSE_L4_CKSUM_OK;
			else {
				orgCksum = result->icmp->icmp_cksum;

				result->l4Flag |= PKT_PARSE_L4_CKSUM_ERR;
				result->icmp->icmp_cksum = 0;
				result->l4Cksum = ~ipcsum((uint16 *)result->icmp, len, 0);
				result->icmp->icmp_cksum = orgCksum;
			}
			nextPtr += sizeof(icmpHdr_t);
			result->payload = (uint8 *)nextPtr;
			result->dataLen -= sizeof(icmpHdr_t);
			result->pktFlag |= L4_ICMP;
			break;
		case IP_PROTO_IGMP:
			result->igmp = (igmpHdr_t *) nextPtr;
			if(ipcsum((uint16 *)result->igmp, len, 0)==0)
				result->l4Flag |= PKT_PARSE_L4_CKSUM_OK;
			else {
				orgCksum = result->igmp->igmp_cksum;
				result->l4Flag |= PKT_PARSE_L4_CKSUM_ERR;
				result->igmp->igmp_cksum = 0;
				result->l4Cksum = ~ipcsum((uint16 *)result->igmp, len, 0);
				result->igmp->igmp_cksum = orgCksum;
			}
			nextPtr += sizeof(igmpHdr_t);
//			result->payload = (uint8 *)nextPtr;
//			result->dataLen -= sizeof(igmpHdr_t);
			result->pktFlag |= L4_IGMP;
			break;
		case IP_PROTO_TCP:
			result->tcp = (tcpHdr_t *) nextPtr;
			phCksum = ipcsum((uint16 *)&result->ip->ip_src, 8, 0);
			tmp = htons(IP_PROTO_TCP);
			phCksum = ipcsum(&tmp, 2, phCksum);
			tmp = len;
//			printf("tmp %x ",tmp);
			tmp = htons(tmp);
//			printf("tmp after %x\n",tmp);
			phCksum = ipcsum(&tmp, 2, phCksum);
			csum = ipcsum((uint16 *)result->tcp, len, phCksum);
			if(csum == 0)
				result->l4Flag |= PKT_PARSE_L4_CKSUM_OK;
			else {
				orgCksum = result->tcp->th_sum;
				result->l4Flag |= PKT_PARSE_L4_CKSUM_ERR;
				result->tcp->th_sum = 0;
				result->l4Cksum = ~ipcsum((uint16 *)result->tcp, len, phCksum);
				result->tcp->th_sum = orgCksum;
			}
			// nextPtr += (sizeof(tcpHdr_t) + result->tcp->th_off*4);
			nextPtr += result->tcp->th_off*4;
			result->pktFlag |= L4_TCP;
			result->dataLen -= result->tcp->th_off*4;
			result->payload = (uint8 *)nextPtr;
			break;
		case IP_PROTO_UDP:
			result->udp = (udpHdr_t *) nextPtr;
			if((uint16)len != ntohs(result->udp->uh_ulen))
				result->l4Flag |= PKT_PARSE_UDP_LEN_ERR;
			phCksum = ipcsum((uint16 *)&result->ip->ip_src, 8, 0);
			tmp = htons(IP_PROTO_UDP);
			phCksum = ipcsum(&tmp, 2, phCksum);
			phCksum = ipcsum((uint16 *)&result->udp->uh_ulen, 2, phCksum);
			csum = ipcsum((uint16 *)result->udp, len, phCksum);
			if(result->udp->uh_sum == 0)
				result->l4Flag |= PKT_PARSE_UDP_NO_CKSUM;
			else if(csum == 0)
				result->l4Flag |= PKT_PARSE_L4_CKSUM_OK;
			else {
				orgCksum = result->udp->uh_sum;
				result->l4Flag |= PKT_PARSE_L4_CKSUM_ERR;
				result->udp->uh_sum = 0;
				result->l4Cksum = ~ipcsum((uint16 *)result->udp, len, phCksum);
				result->udp->uh_sum = orgCksum;
			}
			nextPtr += sizeof(udpHdr_t);
			result->pktFlag |= L4_UDP;
			result->dataLen -= sizeof(udpHdr_t);
			result->payload = (uint8 *)nextPtr;
			break;
		}
	}
	else if((result->pktFlag & L3_IPV6)==L3_IPV6) {
		uint16 orgCksum, phCksum;
		result->l4Flag=0;
		switch (type) {
		case IP_PROTO_V6ICMP:
			result->icmp = (icmpHdr_t *) nextPtr;
			if((result->exheader & RouHdr) && (segmentleft !=0)) {
				csum=ipcsum((uint16 *)result->ipv6->ip_src, 16, 0);
				csum=ipcsum((uint16 *)dip, 16, csum);
				lenb=htonl(len);
				csum=ipcsum((uint16 *)&lenb, 4, csum);
				tmp=58;
				tmp=htons(tmp);
				csum=ipcsum(&tmp, 2, csum);
				phCksum=ipcsum((uint16 *)result->icmp, len, csum);
			}
			else {
				csum=ipcsum((uint16 *) result->ipv6->ip_src, 32, 0);
				lenb=htonl(len);
				csum=ipcsum((uint16 *)&lenb, 4, csum);
				tmp=58;
				tmp=htons(tmp);
				csum=ipcsum(&tmp, 2, csum);
				phCksum=ipcsum((uint16 *)result->icmp, len, csum);
			}
			if(phCksum==0)
				result->l4Flag |= PKT_PARSE_L4_CKSUM_OK;
			else {
				orgCksum = result->icmp->icmp_cksum;

				result->l4Flag |= PKT_PARSE_L4_CKSUM_ERR;
				result->icmp->icmp_cksum = 0;
				result->l4Cksum = ~ipcsum((uint16 *)result->icmp, len, csum);
				result->icmp->icmp_cksum = orgCksum;
			}
			nextPtr += sizeof(icmpHdr_t);
			result->payload = (uint8 *)nextPtr;
			result->dataLen -= sizeof(icmpHdr_t);
			result->pktFlag |= L4_ICMP; 
			break; 
		case IP_PROTO_TCP:
			result->tcp = (tcpHdr_t *) nextPtr;

/*			nh=6;
        	//	rtlglue_printf("hello!! in the form tcp checksum!!!\n\n");
        		if (result->exheader & RouHdr) {
               			 csum=ipcsum((uint16 *)&result->ipv6->ip_src, 16, 0);
             			 csum=ipcsum((uint16 *)dip, 16, csum);
       			 }
       			 else
               			 csum=ipcsum((uint16 *)&result->ipv6->ip_src, 32, 0);
       			 tmp=htons(len);
      			 rtlglue_printf("\n\nthe tmp is [%d]\n\n", tmp);
        //tmp=30;
       			 csum=ipcsum(&tmp, 4, csum);
       			 tmp=nh;
       			 csum=ipcsum(&tmp, 1, csum);
			 phCksum=ipcsum((uint16 *)result->tcp, len, csum);
       			 result->tcp->th_sum=0;
        	//	rtlglue_printf("              hi             ");
       			 csum=ipcsum((uint16 *)result->tcp, len, csum);
      			 rtlglue_printf("the tcp checksum is [%d] \n\n", csum);*/

			if ((result->exheader & RouHdr)&& (segmentleft !=0)) {
				csum=ipcsum((uint16 *)&result->ipv6->ip_src, 16, 0);
				csum=ipcsum((uint16 *) dip, 16, csum);
			//	rtlglue_printf("             here\n");
			}
			else
				csum=ipcsum((uint16 *)&result->ipv6->ip_src, 32, 0);
	//		rtlglue_printf("the csum1  is  [%d]\n", csum);
	//		rtlglue_printf("the ip address is [%d]\n", result->ipv6->ip_src[0]);
	//		rtlglue_printf("the ip address is [%d]\n", result->ipv6->ip_src[1]);
	//		rtlglue_printf("the ip address is [%d]\n", result->ipv6->ip_src[2]);
	//		rtlglue_printf("the ip address is [%d]\n", result->ipv6->ip_src[3]);
	//		rtlglue_printf("the ip address is [%d]\n", result->ipv6->ip_src[4]); 
		//	tmp=htons(len);
			lenb=htonl(len);
	//		rtlglue_printf("the lenb is [%d]\n", lenb);
			csum=ipcsum((uint16 *)&lenb, 4, csum);
	//		rtlglue_printf("the csum2 is [%d]\n", csum);
			tmp=6;
			tmp=htons(tmp);
			csum=ipcsum(&tmp, 2, csum);
	//		rtlglue_printf("the csum3 is [%d]\n", csum);
	//		rtlglue_printf("the checksum in tcp packet is [%d]\n ",htons(result->tcp->th_sum));
			
			phCksum=ipcsum((uint16 *) result->tcp, len, csum);
			
	//		result->tcp->th_sum=0;
	//		csum= ipcsum((uint16 *)result->tcp, len, csum);
	//		rtlglue_printf("the csum4 is [%d]\n", csum);
	//		rtlglue_printf("the checksum without th_sum is [%d]\n ", csum);			  
	//		phCksum = ipcsum((uint16 *)&result->ip->ip_src, 8, 0);
//			tmp = htons(IP_PROTO_TCP);
//			phCksum = ipcsum(&tmp, 2, phCksum);
//			tmp = len;
//			printf("tmp %x ",tmp);
//			tmp = htons(tmp);
//			printf("tmp after %x\n",tmp);
//			phCksum = ipcsum(&tmp, 2, phCksum);
//			csum = ipcsum((uint16 *)result->tcp, len, phCksum);
			if(phCksum == 0)
				result->l4Flag |= PKT_PARSE_L4_CKSUM_OK;
			else {
				orgCksum = result->tcp->th_sum;
				result->l4Flag |= PKT_PARSE_L4_CKSUM_ERR;
				result->tcp->th_sum = 0;
				result->l4Cksum = ~ipcsum((uint16 *)result->tcp, len, csum);
				result->tcp->th_sum = orgCksum;
		//		rtlglue_printf("the error checksum result is [%d]\n", phCksum);
			}
			// nextPtr += (sizeof(tcpHdr_t) + result->tcp->th_off*4);
			nextPtr += result->tcp->th_off*4;
			result->pktFlag |= L4_TCP;
			result->dataLen -= result->tcp->th_off*4;
			result->payload = (uint8 *)nextPtr;
			break; 
		case IP_PROTO_UDP:
			result->udp = (udpHdr_t *) nextPtr;
			if((uint16)len != ntohs(result->udp->uh_ulen))
				result->l4Flag |= PKT_PARSE_UDP_LEN_ERR;
	//		phCksum = ipcsum((uint16 *)&result->ip->ip_src, 8, 0);
	//		tmp = htons(IP_PROTO_UDP);
	//		phCksum = ipcsum(&tmp, 2, phCksum);
	//		phCksum = ipcsum((uint16 *)&result->udp->uh_ulen, 2, phCksum);
	//		csum = ipcsum((uint16 *)result->udp, len, phCksum);
			if((result->exheader & RouHdr)&& (segmentleft!=0)) {
				csum=ipcsum((uint16 *)result->ipv6->ip_src, 16, 0);
				csum=ipcsum((uint16 *)dip, 16, csum);
			}
			else
				csum=ipcsum((uint16 *)result->ipv6->ip_src, 32, 0);
			tmp=htons(len);
			csum=ipcsum(&tmp, 2, csum);
			tmp=17;
			tmp=htons(tmp);
			csum=ipcsum(&tmp, 2, csum);
			phCksum=ipcsum((uint16 *)result->udp, len, csum);
			if(result->udp->uh_sum == 0)
				result->l4Flag |= PKT_PARSE_UDP_NO_CKSUM;
			else if(phCksum == 0)
				result->l4Flag |= PKT_PARSE_L4_CKSUM_OK;
			else {
				orgCksum = result->udp->uh_sum;
				result->l4Flag |= PKT_PARSE_L4_CKSUM_ERR;
				result->udp->uh_sum = 0;
				result->l4Cksum = ~ipcsum((uint16 *)result->udp, len, csum);
				result->udp->uh_sum = orgCksum;
			}
			nextPtr += sizeof(udpHdr_t);
			result->pktFlag |= L4_UDP;
			result->dataLen -= sizeof(udpHdr_t);
			result->payload = (uint8 *)nextPtr;
			break;
		}
		
	}
}