/* * Copyright c Realtek Semiconductor Corporation, 2002 * All rights reserved. * * Program : Form the pkt * * Original Creator: Edward Jin-Ru Chen 2002/4/29 * $Author $ * * $Revision: 1.1 $ * $Id: pktForm.c,v 1.1 2012/10/24 04:18:57 ikevin362 Exp $ * $Log: pktForm.c,v $ * Revision 1.1 2012/10/24 04:18:57 ikevin362 * initial version * * Revision 1.11 2006/05/05 08:28:00 yjlou * *: fixed the bug of l2FormCrc(): backward compatible for drvTest * * Revision 1.10 2006/05/04 13:26:09 yjlou * *: fixed the bug of modelPktTranslator(): L2 CRC should be re-calculated for every packet. * *: fixed the bug of modelPktTranslator(): pad 0x20 if packet is shorter than 60 bytes when untag VLAN and PPPoE header. * *: uniform the length field definitions: * in virtualMacInput/Output(): length is included L2 CRC. * in modelPktParser() and modelPktTranslator(): length is not included L2 CRC. * * Revision 1.9 2006/05/03 06:21:06 yjlou * *: fixed the bug of l4FormUdpBasic() and l4FormTcpBasic(). * * Revision 1.8 2006/04/17 05:21:39 yjlou * *: Since pktForm_crc32() generates reversed endian of CRC (x0 is in crc[24]), we place CRC in little endian manner. * * Revision 1.7 2006/04/13 14:00:36 yjlou * *: fixed the problem of L2 CRC: should be append at the tailing of packet. * *: fixed output packet length * *: fixed preamble from aaad to 55d5 * *: fixed the data valid bit when transmitting tailing bytes. * * Revision 1.6 2006/02/13 03:43:44 chenyl * *: bug fix: QQIC calculation when QQI > 128 * * Revision 1.5 2005/09/16 07:45:06 rupert * +: fix crc32 conflict with wirless device driver(8185) * * Revision 1.4 2005/07/20 15:29:38 yjlou * +: porting Model Code to Linux Kernel: check RTL865X_MODEL_KERNEL. * * Revision 1.3 2004/06/29 07:34:23 chenyl * +: IGMPv3 * +: igmp proxy filter : * protocol stack can let igmp-proxy * 1: trap specific multicast group * 2: ignore specific multicast group * * Revision 1.2 2004/04/30 08:58:08 chenyl * +: ip multicast/igmp proxy * * Revision 1.1 2004/02/25 14:27:24 chhuang * *** empty log message *** * * Revision 1.4 2004/02/25 06:34:22 chenyl * * tcp mss option generate for "SYN" packet * * Revision 1.3 2004/02/18 13:12:21 chenyl * *** empty log message *** * * Revision 1.2 2004/02/17 13:01:47 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 2002/11/27 14:37:39 waynelee * modify protoForm_ether -> protoForm_vsp_ether * * Revision 1.10 2002/10/25 11:22:42 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/07 12:29:12 waynelee * user-defined vcfi (vlan CFI) and L4 No checksum * * Revision 1.7 2002/10/02 15:12:32 waynelee * ipx support debug * * Revision 1.6 2002/09/16 07:31:16 waynelee * support IPX (experimental) * */ #include <net/rtl/rtl_types.h> #include "l2pkt.h" #include "l3pkt.h" #include "l4pkt.h" #include "pktForm.h" #include "utility.h" #include "crc32.h" void l2FormEtherAddress(int8 * pktBuf, int8 * da, int8 * sa) { _etherHdr_t * etherPtr = (_etherHdr_t *)pktBuf; memcpy(etherPtr->da, da, 6); memcpy(etherPtr->sa, sa, 6); } void l2FormCrc(uint32 flag, int8 * pktBuf, uint32 payloadSize) { uint32 crc, i; /* Ethernet assume a packet will longer than 60 bytes, therefore, * we must pad packet content first if the length is smaller than 60 bytes. * Then, we append L2 CRC in the last 4 bytes. */ if ( payloadSize < 60 ) { #if defined(RTL865X_MODEL_USER)||defined(RTL865X_MODEL_KERNEL) memset(&pktBuf[payloadSize], 0x20/*Ethernet standard*/, (60 - payloadSize)); #else memset(&pktBuf[payloadSize], 0x00/* backward compatible for drvTest*/, (60 - payloadSize)); #endif payloadSize = 60; } pktForm_crc32(pktBuf,payloadSize,&crc); if(flag & L2_CRC_ERR) crc = ~crc; #if 1 /* Since pktForm_crc32() generates reversed endian of CRC (x0 is in crc[24]), we place CRC in little endian manner. */ for(i=0;i<4;i++){ // rtlglue_printf("$$$$$$$$$$$$$$$$$hello\n"); pktBuf[payloadSize+i] = (crc>>(i*8)) & 0xff; /* New version, reversed again */ } #else for(i=0;i<4;i++) pktBuf[payloadSize+i] = (crc>>((3-i)*8)) & 0xff; /* Original version */ #endif } void l2FormVlanTag(uint32 flag, int8 * pktBuf, uint16 vlanTag, uint8 vlanCfi, uint16 vlanPriority) { _vlanHdr_t * vlanPtr; _etherHdr_t * etherPtr; uint16 tag; if((flag & L2_VLAN) == 0) return; etherPtr = (_etherHdr_t *)pktBuf; etherPtr->etherType = htons(0x8100); vlanPtr = (_vlanHdr_t *)(pktBuf + sizeof(_etherHdr_t)); if(vlanCfi !=0) vlanCfi =1; tag = (vlanTag & 0x0fff) | (vlanPriority & 0x7)<<13 | (vlanCfi <<12) ; vlanPtr->tag = htons(tag); } void l2FormLlc(uint32 flag, uint8 dsap, uint8 ssap, int8 * pktBuf) { int8 * tmpPtr; _llcHdr_t * llcPtr; if((flag & L2_SNAP) == 0) return; tmpPtr = pktBuf + sizeof(_etherHdr_t);//Ethernet is the MUST if((flag & L2_VLAN) == L2_VLAN) tmpPtr += sizeof(_vlanHdr_t); llcPtr = (_llcHdr_t *)tmpPtr; llcPtr->llc_dsap = dsap; llcPtr->llc_ssap = ssap; llcPtr->ctrl = 0x03; memset(llcPtr->org_code, 0, 3); } void l2FormPppoeSession(uint32 flag, int8 * pktBuf, uint16 etherType, uint16 sessionId) { int8 * tmpPtr = pktBuf + sizeof(_etherHdr_t);//Ethernet is the MUST _pppoeHdr_t * pppoePtr; _llcHdr_t * llcPtr; _vlanHdr_t * vlanPtr; _etherHdr_t * etherPtr; if((flag & L2_PPPoE) == 0) return; if((flag & L2_VLAN) == 0 && (flag & L2_SNAP) == 0) { etherPtr = (_etherHdr_t *)pktBuf; etherPtr->etherType = htons(etherType); } if((flag & L2_VLAN) == L2_VLAN) { vlanPtr = (_vlanHdr_t *)tmpPtr; tmpPtr += sizeof(_vlanHdr_t); if((flag & L2_SNAP) == 0) vlanPtr->etherType = htons(etherType); } if((flag & L2_SNAP) == L2_SNAP) { llcPtr = (_llcHdr_t *)tmpPtr; tmpPtr += sizeof(_llcHdr_t); llcPtr->etherType = htons(etherType); } pppoePtr = (_pppoeHdr_t*)tmpPtr; pppoePtr->ver = 0x1; pppoePtr->type = 0x1; pppoePtr->code = 0x0; pppoePtr->sessionId = htons(sessionId); } // void l2FormLayer2Len(uint32 flag, int8 * pktBuf, uint16 payloadLen) { int8 * tmpPtr = pktBuf + sizeof(_etherHdr_t);//Ethernet is the MUST uint16 llcLen; _pppoeHdr_t * pppoePtr; // _llcHdr_t * llcPtr; _vlanHdr_t * vlanPtr; _etherHdr_t * etherPtr; if((flag & L2_SNAP) == L2_SNAP) { if((flag & L2_PPPoE) == L2_PPPoE) llcLen = payloadLen + sizeof(_llcHdr_t) + sizeof(_pppoeHdr_t); else llcLen = payloadLen + sizeof(_llcHdr_t); if ((flag & L2_VLAN) == L2_VLAN) { vlanPtr = (_vlanHdr_t*)tmpPtr; vlanPtr->etherType = htons(llcLen); } else { etherPtr = (_etherHdr_t*)pktBuf; etherPtr->etherType = htons(llcLen); } } if((flag & L2_PPPoE) == L2_PPPoE) { if ((flag & L2_VLAN) == L2_VLAN) tmpPtr += sizeof(_vlanHdr_t); if((flag & L2_SNAP) == L2_SNAP) tmpPtr += sizeof(_llcHdr_t); pppoePtr = (_pppoeHdr_t *)tmpPtr; pppoePtr->length = htons(payloadLen + 2); // 2 is the PPP header } } void l2FormLayer3Type(uint32 flag, int8 * pktBuf, uint16 type) { int8 * tmpPtr = pktBuf + sizeof(_etherHdr_t);//Ethernet is the MUST _pppoeHdr_t * pppoePtr; _llcHdr_t * llcPtr; _vlanHdr_t * vlanPtr; _etherHdr_t * etherPtr; if((flag&(L2_VLAN | L2_SNAP | L2_PPPoE)) == 0) {//Ethernet only etherPtr = (_etherHdr_t*)pktBuf; etherPtr->etherType = htons(type); return; } if ((flag & L2_VLAN) == L2_VLAN) { vlanPtr = (_vlanHdr_t *)tmpPtr; tmpPtr += sizeof(_vlanHdr_t); if((flag & L2_SNAP) == 0 && (flag & L2_PPPoE) == 0) { vlanPtr->etherType = htons(type); return; } } if((flag & L2_SNAP) == L2_SNAP) { llcPtr = (_llcHdr_t*)tmpPtr; tmpPtr += sizeof(_llcHdr_t); if((flag & L2_PPPoE) == 0) { llcPtr->etherType = htons(type); return; } else { pppoePtr = (_pppoeHdr_t*)tmpPtr; pppoePtr->proto = htons(type); return; } } if((flag & L2_PPPoE) == L2_PPPoE) { pppoePtr = (_pppoeHdr_t*)tmpPtr; pppoePtr->proto = htons(type); return; } } static int32 l3Offset(uint32 flag) { int32 offset; offset = sizeof(_etherHdr_t); if ((flag & L2_VLAN) == L2_VLAN) offset += sizeof(_vlanHdr_t); if((flag & L2_SNAP) == L2_SNAP) offset += sizeof(_llcHdr_t); if((flag & L2_PPPoE) == L2_PPPoE) offset += sizeof(_pppoeHdr_t); return offset; } static int32 bitCount(uint32 flags) { uint32 i, cntr = 0; for(i=0;i<32;i++) if(flags & (0x1<<i)) cntr++; return cntr; } // user defined etherType void l2FormOtherEtherType(uint32 flag, int8 * pktBuf, uint16 etherType, int8 * content, uint32 payloadLen) { int8 * tmpPtr = pktBuf + l3Offset(flag); memcpy(tmpPtr,content,payloadLen); l2FormLayer2Len(flag, pktBuf, payloadLen); l2FormLayer3Type(flag, pktBuf, etherType); } void l3FormIpxBasic(uint32 flag, uint8 type, int8 * pktBuf) { int8 * tmpPtr = pktBuf + l3Offset(flag); ipxHdr_t * ipxPtr; ipxPtr = (ipxHdr_t*)tmpPtr; // memset(tmpPtr, 1, sizeof(ipxHdr_t)); ipxPtr->ipx_sum = htons(65535); ipxPtr->ipx_tctrl = 0; /* used by NetWare Router */ ipxPtr->ipx_type = type; l2FormLayer3Type(flag,pktBuf,0x8137); } void l3FormIpxDnet(uint32 flag, uint32 dnet, uint8 * dnode, uint16 dsock, int8 * pktBuf) { int8 * tmpPtr = pktBuf + l3Offset(flag); ipxHdr_t * ipxPtr; ipxPtr = (ipxHdr_t*)tmpPtr; ipxPtr->ipx_dnet = htonl(dnet); memcpy(ipxPtr->ipx_dnode,dnode,6); ipxPtr->ipx_dsock = htons(dsock); } void l3FormIpxSnet(uint32 flag, uint32 snet, uint8 * snode, uint16 ssock, int8 * pktBuf) { int8 * tmpPtr = pktBuf + l3Offset(flag); ipxHdr_t * ipxPtr; ipxPtr = (ipxHdr_t*)tmpPtr; ipxPtr->ipx_snet = htonl(snet); memcpy(ipxPtr->ipx_snode,snode,6); ipxPtr->ipx_ssock = htons(ssock); } void l3FormIpxContent(uint32 flag, int8 * pktBuf, int8 * content, uint32 size) { // ipxHdr_t * ipxPtr = (ipxHdr_t*)(pktBuf + l3Offset(flag)); int8 * tmpPtr = pktBuf + l3Offset(flag) + sizeof(ipxHdr_t); // int8 * tmpPtr = pktBuf + l3Offset(flag); // ipxHdr_t * ipxPtr; ipxHdr_t * ipxPtr = (ipxHdr_t*)(pktBuf + l3Offset(flag)); memcpy(tmpPtr, content, size); ipxPtr->ipx_len = htons(46); l2FormLayer2Len(flag,pktBuf,sizeof(ipxHdr_t)+size); // l3FormIpTotalLen(flag, pktBuf, size); // l3FormIpChecksum(flag, pktBuf); } static int32 l3FlagValidArp(uint32 flag) { if(bitCount(flag & L3_MASK) > 1 || (flag & L3_ARP) != L3_ARP) return 0; return 1; } void l3FormArpBasic(uint32 flag, uint16 op, int8 * pktBuf) { int8 * tmpPtr = pktBuf + l3Offset(flag); arpHdr_t * arpPtr; if(!l3FlagValidArp(flag) || (flag & L2_PPPoE) == L2_PPPoE) // PPPoE do not have Ethernet ARP return; memset(tmpPtr, 0, sizeof(arpHdr_t)); arpPtr = (arpHdr_t*)tmpPtr; arpPtr->ar_hrd = htons(ARPHRD_ETHER); arpPtr->ar_pro = htons(0x0800); arpPtr->ar_hln = 0x6; arpPtr->ar_pln = 0x4; // arpPtr->ar_hln = 0x6; // 0x0806 is ARP ; 8035 is RARP if(op == ARPOP_REQUEST || op == ARPOP_REPLY || op == ARPOP_INARPREQUEST || op == ARPOP_INARPREPLY) l2FormLayer3Type(flag, pktBuf, 0x0806); else l2FormLayer3Type(flag, pktBuf, 0x8035); l2FormLayer2Len(flag, pktBuf, sizeof(arpHdr_t)); } void l3FormArpSelfInfo(uint32 flag, int8 * pktBuf, uint32 ipAddr, int8 * macAddr) { int8 * tmpPtr = pktBuf + l3Offset(flag); uint32 i; arpHdr_t * arpPtr; uint32 ip; if(!l3FlagValidArp(flag)) return; arpPtr = (arpHdr_t*)tmpPtr; memcpy(arpPtr->ar_sha, macAddr, 6); #ifdef _ENDINEFREE_IP_PORT ip = htonl(ipAddr); #else ip = ipAddr; #endif for(i=0;i<4;i++) arpPtr->ar_spa[i] = (ip >> (8*(3-i)))&0xff; } void l3FormArpPeerInfo(uint32 flag, int8 * pktBuf, uint16 op, uint32 ipAddr, int8 * macAddr) { int8 * tmpPtr = pktBuf + l3Offset(flag); uint32 i; arpHdr_t * arpPtr; uint32 ip; if(!l3FlagValidArp(flag)) return; arpPtr = (arpHdr_t*)tmpPtr; arpPtr->ar_op = htons(op); if(op == ARPOP_REPLY) memcpy(arpPtr->ar_tha, macAddr, 6); else if(op == ARPOP_REQUEST) memset(arpPtr->ar_tha, 0, 6); else memcpy(arpPtr->ar_tha, macAddr, 6); #ifdef _ENDINEFREE_IP_PORT ip = htonl(ipAddr); #else ip = ipAddr; #endif for(i=0;i<4;i++) arpPtr->ar_tpa[i] = (ip >> (8*(3-i)))&0xff; } static int32 l3FlagValidIp(uint32 flag) { if(bitCount(flag & L3_MASK) > 1 || (flag & L3_IP) != L3_IP) return 0; return 1; } void l3FormIpBasic(uint32 flag, int8 * pktBuf, uint8 ver, uint8 len) { int8 * tmpPtr = pktBuf + l3Offset(flag); ipHdr_t * ipPtr; uint32 ipHeaderLen; if(!l3FlagValidIp(flag)) return; if(len%4) ipHeaderLen = len/4 + 1; else ipHeaderLen = len/4; memset(tmpPtr, 0, ipHeaderLen*4); ipPtr = (ipHdr_t*) tmpPtr; ipPtr->ip_hl = ipHeaderLen; ipPtr->ip_v = ver; if((flag & L2_PPPoE) == L2_PPPoE) l2FormLayer3Type(flag, pktBuf, 0x0021); // IP is encapsulated in PPPoE else l2FormLayer3Type(flag, pktBuf, 0x0800); // IP l2FormLayer2Len(flag, pktBuf, ipHeaderLen*4); l3FormIpChecksum(flag, pktBuf); } void l3FormIpFlow(uint32 flag, int8 *pktBuf, uint8 tos, uint8 ttl) { ipHdr_t * ipPtr = (ipHdr_t *)(pktBuf + l3Offset(flag)); if(!l3FlagValidIp(flag)) return; ipPtr->ip_tos = tos; ipPtr->ip_ttl = ttl; l3FormIpChecksum(flag, pktBuf); } void l3FormIpTotalLen(uint32 flag, int8 * pktBuf, uint16 len) { ipHdr_t * ipPtr = (ipHdr_t *)(pktBuf + l3Offset(flag)); if(!l3FlagValidIp(flag)) return; ipPtr->ip_len = htons(len + ipPtr->ip_hl*4); l3FormIpChecksum(flag, pktBuf); l2FormLayer2Len(flag, pktBuf, len + ipPtr->ip_hl*4); } //added by liujuan void l3FormIpv6TotalLen(uint32 flag, int8 * pktBuf, uint8 hdrlen, uint16 len) { int32 exthdrlen; // rtlglue_printf("in l3 Form ipv6 total lenth \n\n"); ip6Hdr_t * ipPtr = (ip6Hdr_t *)(pktBuf + l3Offset(flag)); // rtlglue_printf("the len is [%d]\n\n", len); exthdrlen=v6ExtHdrLen(flag, hdrlen); // rtlglue_printf("the extension header length is [%d]\n\n", exthdrlen); ipPtr->ip_len =htons( len + exthdrlen); // ipPtr->ip_len=exthdrlen+len; // rtlglue_printf("the len + exthdrlen is [%d]\n\n", len+exthdrlen); // rtlglue_printf("the ipv6 ip_len is [%d]\n\n", ipPtr->ip_len); l2FormLayer2Len(flag, pktBuf, len+exthdrlen+40); } void l3FormIpIdFrag(uint32 flag, int8 * pktBuf, uint16 id, uint8 df, uint8 mf, uint16 offset) { uint16 ip_off; ipHdr_t * ipPtr = (ipHdr_t *)(pktBuf + l3Offset(flag)); if(!l3FlagValidIp(flag)) return; ip_off = 0x0; if(df) ip_off |= IP_DF; else ip_off &= ~IP_DF; if(mf) ip_off |= IP_MF; else ip_off &= ~IP_MF; ip_off |= (offset & IP_OFFMASK); ipPtr->ip_id = htons(id); ipPtr->ip_off = htons(ip_off); l3FormIpChecksum(flag, pktBuf); } void l3FormIpProtocol(uint32 flag, int8 * pktBuf, int8 protocol){ int8 * tmpPtr = pktBuf + l3Offset(flag); ipHdr_t * ipPtr; if(!l3FlagValidIp(flag)) return; ipPtr = (ipHdr_t*) tmpPtr; ipPtr->ip_p = protocol; l3FormIpChecksum(flag, pktBuf); } void l3FormIpAddr(uint32 flag, int8 * pktBuf, uint32 sip, uint32 dip){ ipHdr_t * ipPtr = (ipHdr_t*)(pktBuf + l3Offset(flag)); if(!l3FlagValidIp(flag)) return; #ifdef _ENDINEFREE_IP_PORT ipPtr->ip_src = sip; ipPtr->ip_dst = dip; #else ipPtr->ip_src = htonl(sip); ipPtr->ip_dst = htonl(dip); #endif l3FormIpChecksum(flag, pktBuf); } void l3FormIpContent(uint32 flag, int8 * pktBuf, uint8 * content, uint32 size) { ipHdr_t * ipPtr = (ipHdr_t*)(pktBuf + l3Offset(flag)); int8 * tmpPtr = pktBuf + l3Offset(flag) + ipPtr->ip_hl * 4; memcpy(tmpPtr, content, size); l3FormIpTotalLen(flag, pktBuf, size); l3FormIpChecksum(flag, pktBuf); } void l3FormIpOption(uint32 flag, int8 * pktBuf, uint8 * content, uint32 size) { ipHdr_t * ipPtr = (ipHdr_t*)(pktBuf + l3Offset(flag)); int8 * tmpPtr = pktBuf + l3Offset(flag) + ipPtr->ip_hl * 4; ipPtr->ip_hl += (size/4); memcpy(tmpPtr, content, size); l3FormIpTotalLen(flag, pktBuf, size); l3FormIpChecksum(flag, pktBuf); } void l3FormIpChecksum(uint32 flag, int8 *pktBuf) { int8 * tmpPtr = pktBuf + l3Offset(flag); ipHdr_t * ipPtr; uint16 cksum; if(!l3FlagValidIp(flag)) return; ipPtr = (ipHdr_t*) tmpPtr; ipPtr->ip_sum = 0x0; cksum = ipcsum((uint16*)ipPtr, ipPtr->ip_hl * 4, 0); if(flag & L3_CKSUM_ERR) ipPtr->ip_sum = htons(cksum); else ipPtr->ip_sum = htons(~cksum); } //added by liujuan void l3FormIpv6Basic(uint32 flag, int8* pktBuf, uint8 ver){ int8 * tmpPtr = pktBuf + l3Offset(flag); ip6Hdr_t * ipPtr; ipPtr = (ip6Hdr_t*) tmpPtr; ipPtr->ip_v = ver; if((flag & L2_PPPoE) == L2_PPPoE) l2FormLayer3Type(flag, pktBuf, 0x0057); else l2FormLayer3Type(flag, pktBuf, 0x86DD); //l2FormLayer2Len(flag, pktBuf, ipHeaderLen*4); //l3FormIpChecksum(flag, pktBuf); } //added by liujuan void l3FormIpv6Flow(uint32 flag, int8* pktBuf, uint8 pri, uint8 *fl, uint8 hl ){ int8 * tmpPtr = pktBuf + l3Offset(flag); ip6Hdr_t * ipPtr; ipPtr = (ip6Hdr_t*) tmpPtr; ipPtr->ip_pri = pri; memcpy(ipPtr->ip_flow,fl,3); //??????????????????????????????????????? ipPtr->ip_hop = hl; } //added by liujuan void l3FormIpv6Addr(uint32 flag, int8* pktBuf, uint8 * sip, uint8 * dip){ int8 * tmpPtr = pktBuf + l3Offset(flag); ip6Hdr_t * ipPtr = (ip6Hdr_t *)tmpPtr; memcpy(ipPtr->ip_src, sip, 16); memcpy(ipPtr->ip_dst, dip, 16); } //added by liujuan void l3FormIpv6NextHeader(uint32 flag, int8 * pktBuf, uint8 nh) { int8 * tmpPtr = pktBuf + l3Offset(flag); ip6Hdr_t * ipPtr = (ip6Hdr_t *)tmpPtr; ipPtr->ip_next=nh; } //added by liujuan int32 v6ExtHdrLen(uint32 flag, uint8 rhdrlen) { int32 exthdrlen; exthdrlen=0; // rtlglue_printf("the flag is [%d]\n", flag); if((flag & L3_MASK)==0) return 0; if (flag & L3_IPV6) { //added by liujuan , not finish if (flag & hopbyhopHdr) exthdrlen+=16; if (flag &DesHdr) exthdrlen+=16; if (flag & RouHdr) exthdrlen+=(rhdrlen*8 + 8); if (flag & FraHdr) exthdrlen+=8; if (flag & AutHdr) exthdrlen+=20; if (flag & ESPHdr) exthdrlen+= 32; if (flag & DesHdr2) exthdrlen+=16; } // rtlglue_printf("the exthdr lenght is [%d]\n", exthdrlen); return exthdrlen; // rtlglue_printf("in v6ExtHdrLen\n"); // return 20; } //added by liujuan void l3FormIpv6Content(uint32 flag, int8 * pktBuf, uint8 hdrlen, int8 * content, uint32 size) { //ip6Hdr_t * ipPtr = (ip6Hdr_t)(pktBuf + l3Offset(flag)); int8 * tmpPtr = pktBuf + l3Offset(flag) + 40 + v6ExtHdrLen(flag, hdrlen); memcpy(tmpPtr, content, size); l3FormIpv6TotalLen(flag, pktBuf, hdrlen,size); } //added by liujuan. ????????????????????????????????????????????????????????? uint32 l3FormIpv6ExtHdr(uint32 flag, int8 * pktBuf, uint8 hdrlen, uint8 segleft, uint8 * dstip ){ int8 * tmpPtr= pktBuf + l3Offset(flag) +40; uint32 lasthdr; uint8 nh; hopbyhopHdr_t * hopbyhopptr=NULL; DesHdr_t * desptr=NULL; DesHdr_t * desptr2=NULL; RouHdr_t * rouptr=NULL; FraHdr_t * fraptr=NULL; AutHdr_t * autptr=NULL; // uint8 * p; // rtlglue_printf("the flag in form v6 entension header is [%d]\n", flag); lasthdr=L3_IPV6; if (flag&hopbyhopHdr) { hopbyhopptr = (hopbyhopHdr_t *)tmpPtr; tmpPtr +=16; nh=0; l3FormIpv6NextHeader(flag, pktBuf, nh); hopbyhopptr->hdrlen=1; lasthdr=hopbyhopHdr; } if (flag&DesHdr) { desptr = (DesHdr_t *)tmpPtr; tmpPtr += 16; nh=60; desptr->hdrlen=1; if (lasthdr==L3_IPV6) l3FormIpv6NextHeader(flag, pktBuf, nh); if (lasthdr==hopbyhopHdr) hopbyhopptr->nexthdr=nh; lasthdr=DesHdr; } if (flag& RouHdr) { rouptr = (RouHdr_t *)tmpPtr; // tmpPtr += (hdrlen*8+8); nh=43; rouptr->hdrlen= hdrlen; rouptr->routingtype=0; rouptr->segmentleft=segleft; // memcpy(rouptr->dstip, dstip, 16); // p=melloc(sizeof(uint8) * (hdrlen*8-12)); // *p=0; // rouptr->data=p; // memcpy(rouptr->dstip, dstip, 16); memset(tmpPtr+4, '0', hdrlen*8-12); // rtlglue_printf("@@@@@@@@@@@@@the flag is [%d]\n", flag&RouHdr); // rtlglue_printf("@@@@@@@@the hdrlen is [%d]\n", hdrlen); memcpy(tmpPtr+4+(hdrlen*8-12), dstip, 16); if (lasthdr==L3_IPV6) l3FormIpv6NextHeader(flag, pktBuf, nh); if (lasthdr==hopbyhopHdr) hopbyhopptr->nexthdr=nh; if (lasthdr==DesHdr) desptr->nexthdr=nh; lasthdr=RouHdr; // free(p); tmpPtr+=(hdrlen*8+8); } if (flag & FraHdr) { fraptr = (FraHdr_t *)tmpPtr; tmpPtr += 8; nh=44; fraptr->fragment=0; //??????????????????????????????????????? if (lasthdr==L3_IPV6) l3FormIpv6NextHeader(flag, pktBuf, nh); if (lasthdr==hopbyhopHdr) hopbyhopptr->nexthdr=nh; if (lasthdr==DesHdr) desptr->nexthdr=nh; if (lasthdr==RouHdr) rouptr->nexthdr=nh; lasthdr=FraHdr; } if (flag & AutHdr) { autptr = (AutHdr_t *)tmpPtr; tmpPtr+=20; nh=51; autptr->hdrlen=3; if (lasthdr==L3_IPV6) l3FormIpv6NextHeader(flag, pktBuf, nh); if (lasthdr==hopbyhopHdr) hopbyhopptr->nexthdr=nh; if (lasthdr==DesHdr) desptr->nexthdr=nh; if (lasthdr==RouHdr) rouptr->nexthdr=nh; if (lasthdr==FraHdr) fraptr->nexthdr=nh; lasthdr=AutHdr; } if (flag&DesHdr2) { desptr2 = (DesHdr_t *)tmpPtr; tmpPtr += 16; nh=60; desptr2->hdrlen=1; if (lasthdr==L3_IPV6) l3FormIpv6NextHeader(flag, pktBuf, nh); if (lasthdr==hopbyhopHdr) hopbyhopptr->nexthdr=nh; if (lasthdr==DesHdr) desptr->nexthdr=nh; if (lasthdr==RouHdr) rouptr->nexthdr=nh; if (lasthdr==FraHdr) fraptr->nexthdr=nh; if (lasthdr==AutHdr) autptr->nexthdr=nh; lasthdr=DesHdr2; } if (flag & NoNHdr) { nh=59; if (lasthdr==L3_IPV6) l3FormIpv6NextHeader(flag, pktBuf, nh); if (lasthdr==hopbyhopHdr) hopbyhopptr->nexthdr=nh; if (lasthdr==DesHdr) desptr->nexthdr=nh; if (lasthdr==RouHdr) rouptr->nexthdr=nh; if (lasthdr==FraHdr) fraptr->nexthdr=nh; if (lasthdr==AutHdr) autptr->nexthdr=nh; if (lasthdr==DesHdr2) desptr2->nexthdr=nh; lasthdr=NoNHdr; } return lasthdr; } //added by liujuan void l3FormLastHeader(uint32 flag, int8 * pktBuf, uint8 hdrlen, uint32 lasthdr, uint8 nh) { int8 * tmpPtr=pktBuf+l3Offset(flag)+40+v6ExtHdrLen(flag, hdrlen); hopbyhopHdr_t * hopbyhopptr; DesHdr_t * desptr; DesHdr_t * desptr2; RouHdr_t * rouptr; FraHdr_t * fraptr; AutHdr_t * autptr; if(lasthdr==hopbyhopHdr) { tmpPtr=tmpPtr-16; hopbyhopptr=(hopbyhopHdr_t *)tmpPtr; hopbyhopptr->nexthdr=nh; } else if(lasthdr==DesHdr) { tmpPtr=tmpPtr-16; desptr=(DesHdr_t *)tmpPtr; desptr->nexthdr=nh; } else if(lasthdr==RouHdr) { tmpPtr=tmpPtr-(hdrlen*8 + 8); rouptr=(RouHdr_t *)tmpPtr; rouptr->nexthdr=nh; } else if(lasthdr==FraHdr) { tmpPtr=tmpPtr-8; fraptr=(FraHdr_t *)tmpPtr; fraptr->nexthdr=nh; } else if(lasthdr==AutHdr) { tmpPtr=tmpPtr-20; autptr=(AutHdr_t *)tmpPtr; autptr->nexthdr=nh; } else if(lasthdr==DesHdr2) { tmpPtr=tmpPtr-16; desptr2=(DesHdr_t *)tmpPtr; desptr2->nexthdr=nh; } } static int32 l4Offset(uint32 flag, int8 * pktBuf) { int8 * tmpPtr = pktBuf + l3Offset(flag); ipHdr_t * ipPtr = (ipHdr_t *)tmpPtr; if((flag & L3_MASK) == 0) return l3Offset(flag); else if (flag & L3_ARP) return l3Offset(flag) + sizeof(arpHdr_t); else if (flag & L3_IPX) return l3Offset(flag) + sizeof(ipxHdr_t); return l3Offset(flag) + ipPtr->ip_hl * 4; } //added by liujuan static int32 l4OffsetV6(uint32 flag, int8 * pktBuf, uint8 hdrlen ) { // int8 * tmpPtr = pktBuf + l3Offset(flag); // ip6Hdr_t * ipPtr = (ip6Hdr_t *)tmpPtr; int32 exthdrlen=0; //added by liujuan if((flag & L3_MASK)==0) return l3Offset(flag); else if (flag & L3_IPV6) exthdrlen=v6ExtHdrLen(flag, hdrlen); return l3Offset(flag) +40 +exthdrlen; } static uint16 l4PayloadLen(uint32 flag, int8 * pktBuf) { ipHdr_t * ipPtr = (ipHdr_t *)(pktBuf + l3Offset(flag)); if((flag & L4_MASK) == 0) return 0; return ntohs(ipPtr->ip_len) - ipPtr->ip_hl * 4; } //added by liujuan static uint16 l4PayloadLenV6(uint32 flag, int8 * pktBuf, uint8 hdrlen) { int8 * tmpPtr = pktBuf + l3Offset(flag); ip6Hdr_t * ipPtr = (ip6Hdr_t *)tmpPtr; int32 exthdrlen=0; //added by liujuan if((flag & L4_MASK)==0) return 0; if (flag & L3_IPV6) exthdrlen=v6ExtHdrLen(flag, hdrlen); // rtlglue_printf("the extension header leghth is [%d]\n", exthdrlen); // rtlglue_printf("the ipv6 payload is [%d]\n", ntohs(ipPtr->ip_len)); return ntohs(ipPtr->ip_len) - exthdrlen; } static int32 l4FlagValidPptp(uint32 flag) { if(!l3FlagValidIp(flag) || bitCount(flag & L4_MASK) > 1 || (flag & L4_PPTP) != L4_PPTP) return 0; return 1; } void l4FormPptpBasic(uint32 flag, int8 * pktBuf, uint8 Cbit, uint8 Rbit, uint8 Kbit, uint8 Sbit, uint8 sbit, uint8 recur, uint8 Abit, uint8 otherflags, uint8 ver) { uint8 pptpflagsx1,pptpflagsx2; int8 * tmpPtr = (int8 *)(pktBuf + l4Offset(flag, pktBuf)); greHdr_t * pptpPtr = (greHdr_t *)tmpPtr; if(!l4FlagValidPptp(flag)) return; memset(tmpPtr, 0, sizeof(greHdr_t)); pptpflagsx1 = 0x00; if(Cbit) pptpflagsx1 |= GRE_C_BIT; if(Rbit) pptpflagsx1 |= GRE_R_BIT; if(Kbit) pptpflagsx1 |= GRE_K_BIT; if(Sbit) pptpflagsx1 |= GRE_S_BIT; if(sbit) pptpflagsx1 |= GRE_s_BIT; pptpflagsx1 |= (recur & ~GRE_FLAGS_x1); pptpPtr->gre_flags_x1 = pptpflagsx1; pptpflagsx2 = 0x00; if(Abit) pptpflagsx2 |= 0x80; pptpflagsx2 |= ((otherflags & 0x0f)<<3); pptpflagsx2 |= (ver & 0x07); pptpPtr->gre_flags_x2 = pptpflagsx2; pptpPtr->gre_protocol = htons(0x880b); } void l4FormPptpKey (uint32 flag, int8 * pktBuf, uint16 callid, uint32 seqno, uint32 ackno) { int8 * tmpPtr = (int8 *)(pktBuf + l4Offset(flag, pktBuf)); greHdr_t * pptpPtr = (greHdr_t *)tmpPtr; if(!l4FlagValidPptp(flag)) return; pptpPtr->gre_callid = htons(callid); pptpPtr->gre_seqno = htonl(seqno); pptpPtr->gre_ackno = htonl(ackno); } void l4FormPptpOther (uint32 flag, int8 * pktBuf, int8 * content, uint32 size) { int8 * cntPtr = (int8 *)(pktBuf + l4Offset(flag, pktBuf) + sizeof(greHdr_t)); int8 * tmpPtr = (int8 *)(pktBuf + l4Offset(flag, pktBuf)); greHdr_t * pptpPtr = (greHdr_t *)tmpPtr; if(!l4FlagValidPptp(flag)) return; pptpPtr->gre_length = htons(size); memcpy(cntPtr, content, size); l3FormIpProtocol(flag, pktBuf, IP_PROTO_PPTP); l3FormIpTotalLen(flag, pktBuf, size + sizeof(greHdr_t)); } static int32 l4FlagValidIcmp(uint32 flag) { if(bitCount(flag & L4_MASK) > 1 || (flag & L4_ICMP) != L4_ICMP) return 0; return 1; } void l4FormIcmpBasic(uint32 flag, int8 * pktBuf, uint8 type, uint8 code) { int8 * tmpPtr = pktBuf + l4Offset(flag, pktBuf); icmpHdr_t * icmpPtr = (icmpHdr_t *)tmpPtr; if(!l4FlagValidIcmp(flag)) return; // rtlglue_printf("*****************the type is [%d]\n", type); // rtlglue_printf("****************the code is [%d]\n", code); memset(tmpPtr, 0, sizeof(icmpHdr_t)); icmpPtr->icmp_type = type; icmpPtr->icmp_code = code; l3FormIpProtocol(flag, pktBuf, IP_PROTO_ICMP); l3FormIpTotalLen(flag, pktBuf, sizeof(icmpHdr_t)); l4FormIcmpChecksum(flag, pktBuf); } //added by liujuan void l4FormIcmpBasicV6(uint32 flag, int8 * pktBuf, uint8 hdrlen, uint8 type, uint8 code, uint8 * dip, uint8 segleft) { int8 *tmpPtr = pktBuf+l4OffsetV6(flag, pktBuf, hdrlen); icmpHdr_t * icmpPtr = (icmpHdr_t *)tmpPtr; if(!l4FlagValidIcmp(flag)) return; // rtlglue_printf("in form v6 icmp basic\n"); // rtlglue_printf("*****************the type is [%d]\n", type); // rtlglue_printf("****************the code is [%d]\n", code); // rtlglue_printf("the dip[0] is [%d]\n", dip[0]); // rtlglue_printf("the dip[1] is [%d]\n", dip[1]); memset(tmpPtr, 0, sizeof(icmpHdr_t)); icmpPtr->icmp_type = type; icmpPtr->icmp_code = code; l3FormIpv6TotalLen(flag, pktBuf, hdrlen, sizeof(icmpHdr_t)); l4FormIcmpChecksumV6(flag, pktBuf, hdrlen, dip, segleft); } void l4FormIcmpEcho(uint32 flag, int8 * pktBuf, uint16 id, uint16 seq) { int8 * tmpPtr = pktBuf + l4Offset(flag, pktBuf); icmpHdr_t * icmpPtr = (icmpHdr_t *)tmpPtr; if(!l4FlagValidIcmp(flag)) return; icmpPtr->content.echo_s.icd_id = htons(id); icmpPtr->content.echo_s.icd_seq = htons(seq); l3FormIpTotalLen(flag, pktBuf, sizeof(icmpHdr_t)); l4FormIcmpChecksum(flag, pktBuf); } //added by liujuan void l4FormIcmpEchoV6(uint32 flag, int8 * pktBuf, uint8 hdrlen, uint16 id, uint16 seq,uint8 * dip, uint8 segleft) { int8 * tmpPtr=pktBuf + l4OffsetV6(flag, pktBuf, hdrlen); icmpHdr_t * icmpPtr=(icmpHdr_t *)tmpPtr; if(!l4FlagValidIcmp(flag)) return; // rtlglue_printf("in form icmp echo\n"); // rtlglue_printf("*************the id is [%d]\n", id); // rtlglue_printf("****************the seq is [%d]\n", seq); // rtlglue_printf("the dip[0] is [%d]\n", dip[0]); // rtlglue_printf("the dip[1] is [%d]\n", dip[1]); icmpPtr->content.echo_s.icd_id = htons(id); icmpPtr->content.echo_s.icd_seq = htons(seq); // rtlglue_printf("*************the id is [%d]\n", icmpPtr->content.echo_s.icd_id); // rtlglue_printf("****************the seq is [%d]\n", icmpPtr->content.echo_s.icd_seq); l3FormIpv6TotalLen(flag, pktBuf, hdrlen, sizeof(icmpHdr_t)); l4FormIcmpChecksumV6(flag, pktBuf, hdrlen, dip, segleft); } void l4FormIcmpData(uint32 flag, int8 * pktBuf, int8 * data, uint32 size) { int8 * tmpPtr = (int8 *)(pktBuf + l4Offset(flag, pktBuf) + sizeof(icmpHdr_t)); // int8 * tmpPtr = (int8 *)(pktBuf + l4Offset(flag, pktBuf)); //icmpHdr_t * icmpPtr = (icmpHdr_t *)(pktBuf + l4Offset(flag, pktBuf)); if(!l4FlagValidIcmp(flag)) return; memcpy(tmpPtr, data, size); l3FormIpTotalLen(flag, pktBuf, size+sizeof(icmpHdr_t)); l4FormIcmpChecksum(flag, pktBuf); } //added by liujuan void l4FormIcmpDataV6(uint32 flag, int8 * pktBuf, uint8 hdrlen, int8 * data, uint32 size, uint8 * dip, uint8 segleft) { int8 * tmpPtr= (int8 *)(pktBuf + l4OffsetV6(flag, pktBuf, hdrlen)+ sizeof(icmpHdr_t)); if(!l4FlagValidIcmp(flag)) return; // rtlglue_printf("in form icmp data \n"); // rtlglue_printf("the data length is [%d]\n", size); memcpy(tmpPtr, data, size); // rtlglue_printf("the dip[0] is [%d]\n", dip[0]); // rtlglue_printf("the dip[1] is [%d]\n", dip[1]); l3FormIpv6TotalLen(flag, pktBuf,hdrlen, size+sizeof(icmpHdr_t)); l4FormIcmpChecksumV6(flag, pktBuf, hdrlen, dip, segleft); } void l4FormIcmpChecksum(uint32 flag, int8 * pktBuf) { ipHdr_t * ipPtr = (ipHdr_t *)(pktBuf + l3Offset(flag)); icmpHdr_t * icmpPtr = (icmpHdr_t*)(pktBuf + l4Offset(flag, pktBuf)); uint16 cksum; if(!l4FlagValidIcmp(flag)) return; icmpPtr->icmp_cksum = 0; cksum = ipcsum((uint16*)icmpPtr, ntohs(ipPtr->ip_len) - ipPtr->ip_hl*4, 0); if(flag & L4_CKSUM_ERR) icmpPtr->icmp_cksum = htons(cksum); else icmpPtr->icmp_cksum = htons(~cksum); } //added by liujuan void l4FormIcmpChecksumV6(uint32 flag, int8 * pktBuf, uint8 hdrlen, uint8 * dip, uint8 segleft) { ip6Hdr_t * ipPtr=(ip6Hdr_t *)(pktBuf + l3Offset(flag)); icmpHdr_t * icmpPtr=(icmpHdr_t *)(pktBuf + l4OffsetV6(flag, pktBuf, hdrlen)); uint16 tmp,csum; uint8 nh; uint32 len, lenb; nh=58; if(!l4FlagValidIcmp(flag)) return; // rtlglue_printf("in form icmp checkusm\n"); if ((flag & RouHdr)&& (segleft!=0)) { // rtlglue_printf("\n********************************\n"); csum=ipcsum((uint16 *)ipPtr->ip_src, 16, 0); csum=ipcsum((uint16 *)dip, 16, csum); // rtlglue_printf("the dip[0] is [%d]\n", dip[0]); // rtlglue_printf("the dip[1] is [%d]\n", dip[1]); } else csum=ipcsum((uint16 *)ipPtr->ip_src, 32, 0); len=l4PayloadLenV6(flag, pktBuf, hdrlen); lenb=htonl(len); csum=ipcsum((uint16 *)&lenb, 4, csum); tmp=nh; tmp=htons(tmp); csum=ipcsum(&tmp, 2, csum); icmpPtr->icmp_cksum=0; csum=ipcsum((uint16 *)icmpPtr, len, csum); // rtlglue_printf("the icmp checksum is [%d]", htons(~csum)); if(flag & L4_CKSUM_ERR) icmpPtr->icmp_cksum=htons(csum); else icmpPtr->icmp_cksum=htons(~csum); // rtlglue_printf("the checksum in icmp pkt is [%d]\n", icmpPtr->icmp_cksum); } static int32 l4FlagValidIgmp(uint32 flag) { if(!l3FlagValidIp(flag) || bitCount(flag & L4_MASK) > 1 || (flag & L4_IGMP) != L4_IGMP) return 0; return 1; } void l4FormIgmpBasic(uint32 flag, int8 * pktBuf, uint8 ver, uint8 type, uint8 respTime, uint32 addr, uint8 qrsq, uint8 qqic, uint16 qnofs, uint32 * qslist, uint16 rnofg) { //ipHdr_t * ipPtr = (ipHdr_t *)(pktBuf + l3Offset(flag)); igmpHdr_t * igmpPtr = (igmpHdr_t*)(pktBuf + l4Offset(flag, pktBuf)); if(!l4FlagValidIgmp(flag)) return; l3FormIpProtocol(flag, pktBuf, IP_PROTO_IGMP); switch (ver) { case 1: // V1 case 2: // V2 igmpPtr->igmp_type = type; igmpPtr->igmp_respTime = respTime; igmpPtr->igmp_cksum = 0; igmpPtr->igmp_group = htonl(addr); l3FormIpTotalLen(flag, pktBuf, 8); break; case 3: // V3 if (type == IGMP_MEMBERSHIP_QUERY) { int i; uint32 *p, *s; igmpPtr->igmp_type = type; igmpPtr->igmp_respTime = respTime; igmpPtr->igmp_cksum = 0; igmpPtr->igmp_group = htonl(addr); igmpPtr->igmp_QRSQ = qrsq; igmpPtr->igmp_QQIC = qqic; igmpPtr->igmp_QNOFS = htons(qnofs); p = &(igmpPtr->igmp_QSLIST); s = qslist; for (i = 0 ; i < qnofs ; i ++, p++, s ++) *p = htonl(*s); l3FormIpTotalLen(flag, pktBuf, (12 + (4 * qnofs))); }else if (type == IGMP_V3_MEMBERSHIP_REPORT) { igmpPtr->igmp_type = type; igmpPtr->igmp_respTime = 0; igmpPtr->igmp_cksum = 0; igmpPtr->igmp_RRESV = 0; igmpPtr->igmp_RNOFG = htons(rnofg); l3FormIpTotalLen(flag, pktBuf, 8); } break; } l4FormIgmpChecksum(flag, pktBuf); } void l4FormIgmpContent(uint32 flag, int8 * pktBuf, int8 * content, uint32 size, uint8 ver, uint8 type, uint16 qnofs) { int8 * cntPtr; int32 igmpHdrLen = 0; if(!l4FlagValidIgmp(flag)) return; switch (ver) { case 1: case 2: igmpHdrLen = 8; break; case 3: if (type == IGMP_MEMBERSHIP_QUERY) igmpHdrLen = (12 + (4 * qnofs)); else igmpHdrLen = 8; break; } cntPtr = (int8 *) (pktBuf + l4Offset(flag, pktBuf) + igmpHdrLen); memcpy(cntPtr, content, size); l3FormIpTotalLen(flag, pktBuf, (size + igmpHdrLen)); switch (ver) { case 1: case 2: l3FormIpTotalLen(flag, pktBuf, (size + 8)); break; case 3: if (type == IGMP_MEMBERSHIP_QUERY) l3FormIpTotalLen(flag, pktBuf, (size + (12 + (4 * qnofs)))); else l3FormIpTotalLen(flag, pktBuf, (size + 8)); break; } l4FormIgmpChecksum(flag, pktBuf); } void l4FormIgmpChecksum(uint32 flag, int8 * pktBuf) { ipHdr_t * ipPtr = (ipHdr_t *)(pktBuf + l3Offset(flag)); igmpHdr_t * igmpPtr = (igmpHdr_t*)(pktBuf + l4Offset(flag, pktBuf)); uint16 cksum; if(!l4FlagValidIgmp(flag)) return; igmpPtr->igmp_cksum = 0; cksum = ipcsum((uint16*)igmpPtr, ntohs(ipPtr->ip_len) - ipPtr->ip_hl*4, 0); if(flag & L4_CKSUM_ERR) igmpPtr->igmp_cksum = htons(cksum); else igmpPtr->igmp_cksum = htons(~cksum); } static int32 l4FlagValidUdp(uint32 flag) { if(bitCount(flag & L4_MASK) > 1 || (flag & L4_UDP) != L4_UDP) return 0; return 1; } void l4FormUdpBasic(uint32 flag, int8 * pktBuf, uint16 sPort, uint16 dPort) { udpHdr_t * udpPtr = (udpHdr_t*)(pktBuf + l4Offset(flag, pktBuf)); if(!l4FlagValidUdp(flag)) return; /* clean header */ memset( udpPtr, 0, sizeof(udpHdr_t) ); #ifdef _ENDINEFREE_IP_PORT udpPtr->uh_sport = sPort; udpPtr->uh_dport = dPort; #else udpPtr->uh_sport = htons(sPort); udpPtr->uh_dport = htons(dPort); #endif l3FormIpProtocol(flag, pktBuf, IP_PROTO_UDP); l3FormIpTotalLen(flag, pktBuf, sizeof(udpHdr_t)); l4FormUdpChecksum(flag, pktBuf); } //added by liujuan void l4FormUdpBasicV6(uint32 flag, int8 * pktBuf, uint8 hdrlen, uint16 sPort, uint16 dPort, uint8 * dip, uint8 segleft) { udpHdr_t * udpPtr=(udpHdr_t *)(pktBuf+l4OffsetV6(flag, pktBuf, hdrlen)); if (!l4FlagValidUdp(flag)) return; memset(udpPtr, 0 , sizeof(udpHdr_t)); #ifdef _ENDINEFREE_IP_PORT udpPtr->uh_sport = sPort; udpPtr->uh_dport = dPort; #else udpPtr->uh_sport = htons(sPort); udpPtr->uh_dport = htons(dPort); #endif l3FormIpv6TotalLen(flag, pktBuf, hdrlen, sizeof(udpHdr_t)); l4FormUdpChecksumV6(flag, pktBuf, hdrlen, dip, segleft); } void l4FormUdpContent(uint32 flag, int8 * pktBuf, int8 * content, uint32 size) { int8 * cntPtr = pktBuf + l4Offset(flag, pktBuf) + sizeof(udpHdr_t); udpHdr_t * udpPtr = (udpHdr_t*)(pktBuf + l4Offset(flag, pktBuf)); if(!l4FlagValidUdp(flag)) return; memcpy(cntPtr, content, size); udpPtr->uh_ulen = htons(size+sizeof(udpHdr_t)); l3FormIpTotalLen(flag, pktBuf, size+sizeof(udpHdr_t)); l4FormUdpChecksum(flag, pktBuf); } //added by liujuan void l4FormUdpContentV6(uint32 flag, int8 * pktBuf, uint8 hdrlen, int8 * content, uint32 size, uint8 * dip, uint8 segleft) { int8 * cntPtr=pktBuf+l4OffsetV6(flag, pktBuf, hdrlen)+sizeof(udpHdr_t); udpHdr_t * udpPtr=(udpHdr_t *)(pktBuf+l4OffsetV6(flag, pktBuf, hdrlen)); if (!l4FlagValidUdp(flag)) return; memcpy(cntPtr, content, size); udpPtr->uh_ulen=htons(size+sizeof(udpHdr_t)); l3FormIpv6TotalLen(flag, pktBuf, hdrlen, size+sizeof(udpHdr_t)); l4FormUdpChecksumV6(flag, pktBuf, hdrlen, dip, segleft); } void l4FormUdpChecksum(uint32 flag, int8 * pktBuf) { ipHdr_t * ipPtr = (ipHdr_t *)(pktBuf + l3Offset(flag)); udpHdr_t * udpPtr = (udpHdr_t*)(pktBuf + l4Offset(flag, pktBuf)); uint16 tmp, csum; if(!l4FlagValidUdp(flag)) return; csum = ipcsum((uint16 *)&ipPtr->ip_src, 8, 0); tmp = htons(IP_PROTO_UDP); csum = ipcsum(&tmp, 2, csum); csum = ipcsum((uint16 *)&udpPtr->uh_ulen, 2, csum); udpPtr->uh_sum = 0; csum = ipcsum((uint16 *)udpPtr, ntohs(udpPtr->uh_ulen), csum); if(flag & L4_NO_CKSUM){ udpPtr->uh_sum = 0; }else if(flag & L4_CKSUM_ERR){ if((csum == 0) || (csum==0xffff)) udpPtr->uh_sum = htons(csum+10); }else{ //~csum=0 defined non-checksum ,so need change to 0xffff if(((~csum)&0xffff) ==0) udpPtr->uh_sum = htons(0xffff); else udpPtr->uh_sum = htons(~csum); } } //added by liujuan void l4FormUdpChecksumV6(uint32 flag, int8 * pktBuf, uint8 hdrlen, uint8 * dip, uint8 segleft) { ip6Hdr_t * ipPtr=(ip6Hdr_t *)(pktBuf + l3Offset(flag)); udpHdr_t * udpPtr=(udpHdr_t *)(pktBuf + l4OffsetV6(flag, pktBuf, hdrlen)); uint16 tmp, csum; uint8 nh; nh=17; if(!l4FlagValidUdp(flag)) return; if ((flag & RouHdr) && (segleft!=0)){ csum=ipcsum((uint16 *) &ipPtr->ip_src, 16, 0); //tmp=(uint16) *dip; //????????????????????????????????????????????????????????????????????????????????? csum=ipcsum((uint16 *)dip, 16, csum); } else csum=ipcsum((uint16 *)&ipPtr->ip_src, 32, 0); // rtlglue_printf("the udp uh_ulen is [%d]\n", udpPtr->uh_ulen); // rtlglue_printf("the v6 payload lenth is [%d]\n", ipPtr->ip_len); csum=ipcsum((uint16 *)&udpPtr->uh_ulen, 2, csum); tmp=nh; tmp=htons(tmp); csum=ipcsum(&tmp, 2, csum); udpPtr->uh_sum=0; csum = ipcsum((uint16 *)udpPtr, ntohs(udpPtr->uh_ulen),csum); if(flag & L4_NO_CKSUM){ udpPtr->uh_sum = 0; }else if(flag & L4_CKSUM_ERR){ if((csum == 0) || (csum==0xffff)) udpPtr->uh_sum = htons(csum+10); }else{ //~csum=0 defined non-checksum ,so need change to 0xffff if(((~csum)&0xffff) ==0) udpPtr->uh_sum =0xffff; else udpPtr->uh_sum = htons(~csum); } } static int32 l4FlagValidTcp(uint32 flag) { // rtlglue_printf("the flag in l4flagvalidtcp is [%d]\n", flag); if(bitCount(flag & L4_MASK) > 1 || (flag & L4_TCP) != L4_TCP) return 0; return 1; } void l4FormTcpBasic(uint32 flag, int8 * pktBuf, uint16 sPort, uint16 dPort, uint8 hlen) { tcpHdr_t * tcpPtr = (tcpHdr_t*)(pktBuf + l4Offset(flag, pktBuf)); uint32 tcpHlen; if(!l4FlagValidTcp(flag)) return; if(hlen%4) tcpHlen = hlen/4 + 1; else tcpHlen = hlen/4; memset(tcpPtr, 0,tcpHlen*4); #ifdef _ENDINEFREE_IP_PORT tcpPtr->th_sport = sPort; tcpPtr->th_dport = dPort; #else tcpPtr->th_sport = htons(sPort); tcpPtr->th_dport = htons(dPort); #endif tcpPtr->th_off = tcpHlen; l3FormIpProtocol(flag, pktBuf, IP_PROTO_TCP); l3FormIpTotalLen(flag, pktBuf, sizeof(tcpHdr_t)); l4FormTcpChecksum(flag, pktBuf); } //added by liujuan void l4FormTcpBasicV6(uint32 flag, int8 * pktBuf, uint8 hdrlen, uint16 sPort, uint16 dPort, uint8 hlen, uint8* dip, uint8 segleft){ tcpHdr_t * tcpPtr=(tcpHdr_t *)(pktBuf + l4OffsetV6(flag, pktBuf, hdrlen)); uint32 tcpHlen; // rtlglue_printf("in form tcp basic \n\n"); // rtlglue_printf("the flag is [%d]\n\n", flag); if(!l4FlagValidTcp( flag)){ // rtlglue_printf("the tcp flag is error \n\n"); return; } if(hlen%4) tcpHlen = hlen/4 +1; else tcpHlen=hlen/4; memset(tcpPtr, 0,tcpHlen*4); #ifdef _ENDINEFREE_IP_PORT tcpPtr->th_sport = sPort; tcpPtr->th_dport = dPort; #else tcpPtr->th_sport = htons(sPort); tcpPtr->th_dport = htons(dPort); #endif // rtlglue_printf("befor form totoal len"); tcpPtr->th_off = tcpHlen; // rtlglue_printf("the ipv6 packet lenth in l4formtcpbasic is [%d]\n", sizeof(tcpHdr_t)); l3FormIpv6TotalLen(flag, pktBuf, hdrlen, sizeof(tcpHdr_t)); l4FormTcpChecksumV6(flag, pktBuf, hdrlen, dip, segleft); } void l4FormTcpFlow(uint32 flag, int8 * pktBuf, uint32 seq, uint32 ack, uint16 winSize, uint8 tcpFlag, uint16 urgPtr) { tcpHdr_t * tcpPtr = (tcpHdr_t*)(pktBuf + l4Offset(flag, pktBuf)); if(!l4FlagValidTcp(flag)) return; tcpPtr->th_seq = htonl(seq); tcpPtr->th_ack = htonl(ack); tcpPtr->th_win = htons(winSize); tcpPtr->th_flags = tcpFlag; tcpPtr->th_urp = htons(urgPtr); // rtlglue_printf("the ipv6 pkt length in l4formtcpflow is [%d]\n", sizeof(tcpHdr_t)); l3FormIpTotalLen(flag, pktBuf, sizeof(tcpHdr_t)); l4FormTcpChecksum(flag, pktBuf); } //added by liujuan void l4FormTcpFlowV6(uint32 flag, int8 * pktBuf, uint8 hdrlen, uint32 seq, uint32 ack, uint16 winSize, uint8 tcpFlag, uint16 urgPtr, uint8* dip, uint8 segleft) { tcpHdr_t * tcpPtr=(tcpHdr_t*)(pktBuf + l4OffsetV6(flag, pktBuf, hdrlen)); if(!l4FlagValidTcp(flag)) return; tcpPtr->th_seq = htonl(seq); tcpPtr->th_ack = htonl(ack); tcpPtr->th_win = htons(winSize); tcpPtr->th_flags = tcpFlag; tcpPtr->th_urp = htons(urgPtr); // rtlglue_printf("the ipv6 pkt length in l4formtcpflow is [%d]\n", sizeof(tcpHdr_t)); l3FormIpv6TotalLen(flag, pktBuf, hdrlen, sizeof(tcpHdr_t)); l4FormTcpChecksumV6(flag, pktBuf, hdrlen, dip, segleft); } void l4FormTcpOption(uint32 flag, int8 * pktBuf, int8 * option, uint8 size) { tcpHdr_t * tcpPtr = (tcpHdr_t*)(pktBuf + l4Offset(flag, pktBuf)); int8 * optPtr = pktBuf + l4Offset(flag, pktBuf) + sizeof(tcpHdr_t); if(!l4FlagValidTcp(flag)) return; if(size + sizeof(tcpHdr_t) > (uint32)(tcpPtr->th_off*4)) return; memcpy(optPtr, option, size); l3FormIpTotalLen(flag, pktBuf, (sizeof(tcpHdr_t)+size)); l4FormTcpChecksum(flag, pktBuf); } //added by liujuan void l4FormTcpOptionV6(uint32 flag, int8 * pktBuf, uint8 hdrlen, uint8 * option, uint8 size, uint8* dip, uint8 segleft) { tcpHdr_t * tcpPtr=(tcpHdr_t *)(pktBuf + l4OffsetV6(flag, pktBuf, hdrlen)); int8 * optPtr= pktBuf + l4OffsetV6(flag, pktBuf, hdrlen) + sizeof(tcpHdr_t); if(!l4FlagValidTcp(flag)) return; if(size + sizeof(tcpHdr_t) > (uint32)(tcpPtr->th_off*4)) return; memcpy(optPtr, option, size); // rtlglue_printf("the ipv6 pkt length in l4formtcpopton is [%d]\n", sizeof(tcpHdr_t)+size); l3FormIpv6TotalLen(flag, pktBuf, hdrlen, (sizeof(tcpHdr_t)+size)); l4FormTcpChecksumV6(flag, pktBuf, hdrlen, dip, segleft); // rtlglue_printf("the tcp checksum in formtcpoption is [%d\n]",tcpPtr->th_sum); } void l4FormTcpContent(uint32 flag, int8 * pktBuf, int8 * content, uint32 size) { tcpHdr_t * tcpPtr = (tcpHdr_t*)(pktBuf + l4Offset(flag, pktBuf)); int8 * cntPtr = pktBuf + l4Offset(flag, pktBuf) + tcpPtr->th_off*4; if(!l4FlagValidTcp(flag)) return; memcpy(cntPtr, content, size); l3FormIpTotalLen(flag, pktBuf, size+tcpPtr->th_off*4); l4FormTcpChecksum(flag, pktBuf); // rtlglue_printf("the tcp checksum in formtcpcontent is [%d\n]",tcpPtr->th_sum); } //added by liujuan void l4FormTcpContentV6(uint32 flag, int8 * pktBuf, uint8 hdrlen, int8 * content, uint32 size, uint8* dip, uint8 segleft) { tcpHdr_t * tcpPtr =(tcpHdr_t *)(pktBuf + l4OffsetV6(flag, pktBuf, hdrlen)); int8 *cntPtr=pktBuf + l4OffsetV6(flag, pktBuf, hdrlen)+tcpPtr->th_off*4; if(!l4FlagValidTcp(flag)) return; memcpy(cntPtr, content, size); // rtlglue_printf("the ipv6 pkt length in l4formtcpcontent is [%d]\n", size+tcpPtr->th_off*4); l3FormIpv6TotalLen(flag, pktBuf, hdrlen, size+tcpPtr->th_off*4); l4FormTcpChecksumV6(flag, pktBuf, hdrlen, dip, segleft); // rtlglue_printf("the tcp checksum in formtcpcontent is [%d\n]",tcpPtr->th_sum); } void l4FormTcpChecksum(uint32 flag, int8 * pktBuf) { ipHdr_t * ipPtr = (ipHdr_t *)(pktBuf + l3Offset(flag)); tcpHdr_t * tcpPtr = (tcpHdr_t*)(pktBuf + l4Offset(flag, pktBuf)); uint16 tmp, csum; if(!l4FlagValidTcp(flag)) return; csum = ipcsum((uint16 *)&ipPtr->ip_src, 8, 0); tmp = htons(IP_PROTO_TCP); csum = ipcsum(&tmp, 2, csum); tmp = htons(l4PayloadLen(flag, pktBuf)); //rtlglue_printf("\n\nthe tmp is [%d]\n\n", tmp); csum = ipcsum(&tmp, 2, csum); tcpPtr->th_sum = 0; csum = ipcsum((uint16 *)tcpPtr, l4PayloadLen(flag, pktBuf), csum); if(flag & L4_CKSUM_ERR) tcpPtr->th_sum = htons(csum); else tcpPtr->th_sum = htons(~csum); //rtlglue_printf("the tcp checksum is [%d]\n\n", csum); } //added by liujuan void l4FormTcpChecksumV6(uint32 flag, int8 * pktBuf, uint8 hdrlen, uint8* dip, uint8 segleft) { ip6Hdr_t * ipPtr=(ip6Hdr_t *)(pktBuf + l3Offset(flag)); tcpHdr_t * tcpPtr=(tcpHdr_t *)(pktBuf + l4OffsetV6(flag, pktBuf, hdrlen)); uint16 tmp,csum; uint32 len; uint32 lenb; // uint8 nh; if(!l4FlagValidTcp(flag)) return; //nh=6; // rtlglue_printf("hello!! in the form tcp checksum!!!\n\n"); if ((flag & RouHdr)&& (segleft!=0)) { csum=ipcsum((uint16 *)&ipPtr->ip_src, 16, 0); csum=ipcsum((uint16 *)dip, 16, csum); // rtlglue_printf(" here\n"); } else csum=ipcsum((uint16 *)&ipPtr->ip_src, 32, 0); // rtlglue_printf("the csum1 is [%d]\n", csum); // rtlglue_printf("the ip address is [%d]\n", ipPtr->ip_src[0]); // rtlglue_printf("the ip address is [%d]\n", ipPtr->ip_src[1]); // rtlglue_printf("the ip address is [%d]\n", ipPtr->ip_src[2]); // rtlglue_printf("the ip address is [%d]\n", ipPtr->ip_src[3]); // rtlglue_printf("the ip address is [%d]\n", ipPtr->ip_src[4]); // tmp=htons(l4PayloadLenV6(flag, pktBuf, hdrlen)); len=0; len=l4PayloadLenV6(flag, pktBuf, hdrlen); lenb=htonl(len); // len=htons(len); // tmp=htons(len); // rtlglue_printf("the v6 payload lenth is [%d]\n", len); // rtlglue_printf("\n\nthe tmp is [%d]\n\n", tmp); //tmp=30; //csum=ipcsum(&tmp, 4, csum); // tmp=htons(len); 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); tcpPtr->th_sum=0; // rtlglue_printf(" hi "); csum=ipcsum((uint16 *)tcpPtr, len, csum); // rtlglue_printf("the cusm4 is [%d]\n", csum); // rtlglue_printf("the tcp checksum is [%d] \n\n", csum); if(flag & L4_CKSUM_ERR){ tcpPtr->th_sum=htons(csum); // rtlglue_printf("the error tcp checksum is [%d]\n", tcpPtr->th_sum); } else { tcpPtr->th_sum=htons(~csum); // rtlglue_printf("the right tcp checksum is [%d]\n", htons(tcpPtr->th_sum)); // rtlglue_printf("the right tcp checkusm without htons is [%d]\n", tcpPtr->th_sum); } //rtlglue_printf("the tcp checksum is [ ]\n\n"); } uint32 formedPktLen(uint32 flag, int8 * pktBuf) { if(flag & L4_MASK) return l4PayloadLen(flag, pktBuf) + l4Offset(flag, pktBuf); return l4Offset(flag, pktBuf); } //added by liujuan uint32 formedpktLenV6(uint32 flag, int8 * pktBuf, uint8 hdrlen) { // rtlglue_printf("the pkttype is [%d]\n", flag); if(flag & L4_MASK){ // rtlglue_printf("the l4offset is [%d]\n", l4OffsetV6(flag, pktBuf, hdrlen)); // rtlglue_printf("the l4payload is [%d]\n", l4PayloadLenV6(flag, pktBuf, hdrlen)); return l4PayloadLenV6(flag, pktBuf, hdrlen)+ l4OffsetV6(flag, pktBuf, hdrlen); } return l4OffsetV6(flag, pktBuf, hdrlen); } uint32 formedFrameLen(uint32 flag, int8 * pktBuf, uint32 payloadLen) { return l3Offset(flag) + payloadLen; }