ICMP 协议栈实现代码 最新完整版

/*********************************************************************************************************************************** *项目名称：TCP/IP协议栈 *模块名称：ICMP模块 *模块名称：传递ICMP差错报文和查询报文 *作者：汪志伟 *时间:2011-8-5 ***********************************************************************************************************************************/ #include <icmp.h> #include <IP.h> #include <Configure.h> #include <mbuf.h> uint32 Time_EchoReplyRecv = 0; uint32 Time_EchoSend = 0; /*********************************************************************************************************************************** *函数名：ICMP_Error() *接口函数 *功能描述：受运输层或者IP协议调用产生ICMP差错请求报文 *参数：传入的mbuf指针，ICMP首部类型和代码 *返回值：void ***********************************************************************************************************************************/ void ICMP_Error(struct mbuf *n,uint8 type,uint8 code){ //struct ip_header *oip = mtod(n,struct ip *); struct ip_header * nip; uint8 oiplen = 20; struct icmp *icp; struct mbuf *m; uint8 icmplen; /*icp = mtod(m,struct icmp *); icp->icmp_type = type; icp->icmp_code = code; m->m_data -= sizeof(struct ip); m->m_len += sizeof(struct ip);*/ //uint8 oiplen = oip->ip_len; /*if (oip->protocal == ICMP && n->m_len >= oiplen + ICMP_MINLEN && !ICMP_INFOTYPE(((struct icmp *)(oip + oiplen))->icmp_type)) { goto freeit; }*/ /* Don't send error in response to a multicast or broadcast packet */ //if (n->m_flags & (BROADCAST|MULTICAST)) //goto freeit; icp->icmp_type = type; icp->icmp_code = code; uint8* tempicp; uint8* tempip; uint8* tempiphead; struct mbuf* tempicpbuf; struct mbuf* tempipbuf; struct mbuf* tempipheadbuf; strToBuf((char*)icp,tempicp,ICMP_MINLEN); Mbuf_GetData(&n,tempip,28); Mbuf_GetData(&n,tempiphead,20); //strToBuf((char*)n,tempip,28); //strToBuf((char*)n,tempiphead,20); tempicpbuf = Mbuf_Create(tempicp,ICMP_TYPE,ICMP_MINLEN); tempipbuf = Mbuf_Create(tempip,ICMP_IP,28); Mbuf_Add(tempicpbuf,tempipbuf); tempipheadbuf = Mbuf_Create(tempiphead,ICMP_IP_HEADER,20); nip = mtod(tempipheadbuf,struct ip_header *); nip->ip_len = 56; //nip->versionLength = sizeof(struct ip); nip->protocol = ICMP; nip->ip_tos = 0; Mbuf_Add = (tempipheadbuf,tempicpbuf); uint8* tempsegment; Mbuf_GetData(&tempipheadbuf,tempsegment,56); Reflect(tempsegment); freeit: Mbuf_Free(n); } /*********************************************************************************************************************************** *函数名：ICMP_Input() *接口函数 *功能描述：受运输层或者IP协议调用，对收到的不同ICMP报文进行处理 *参数：传入的mbuf指针 *返回值：void ***********************************************************************************************************************************/ void ICMP_Input(struct mbuf *m){ uint8* temphead; uint8* temp; uint8 i; struct icmp *icp; struct mbuf *m; uint8* tempiphead; uint8* tempicmphead = temphead + 20; uint8 length; //strTobuf((char *)m,tempiphead,20); Mbuf_GetData(&m,tempiphead,20); struct mbuf* tempbuf; tempbuf = Mbuf_Create(tempiphead,ICMP_IP_HEADER,20); struct ip_header *tempipheader = mtod(tempbuf,struct ip_header *); length = tempipheader->ip_len; //strToBuf((char*)m,temphead,length); Mbuf_GetData(&m,temphead,length); for(i=0;i<8;i++){ *temp = *tempicmphead; temp++; tempicmphead++; } m = Mbuf_Create(temp,ICMP_TYPE,ICMP_MINLEN); icp = mtod(m,struct icmp *); /*struct icmp *icp; struct ip *ip; //= mtod(m,struct ip *); int32 icmplen = ip->ip_len; //icp = mtod(m,struct icmp *); int32 i; int32 code; if(icmplen < ICMP_MINLEN){ icmpstat.icps_tooshort++; goto freeit; } ip = mtod(m,struct ip *); m->m_len -= hlen; m->m_data += hlen; icp = mtod(m,struct icmp *); if(Checksum(m,icmplen)){ icmpstat.icps_checksum++; goto freeit; } m->m_len += hlen; m->m_data -= hlen;*/ code = icp->icmp_code; switch (icp->icmp_type){ case ICMP_UNREACH: switch (code){ case ICMP_UNREACH _NET: case ICMP_UNREACH_HOST: case ICMP_UNREACH_PROTOCOL: case ICMP_UNREACH_PORT: case ICMP_UNREACH_SRCFAIL: case ICMP_UNREACH_NEEDFRAG: case ICMP_UNREACH_NET_UNKNOWN: case ICMP_UNREACH_NET_PROHIB: case ICMP_UNREACH_TOSNET: case ICMP_UNREACH_HOST_UNKNOWN: case ICMP_UNREACH_ISOLATED: case ICMP_UNREACH_HOST_PROHIB: case ICMP_UNREACH_TOSHOST: break; default: goto badcode; } goto deliver; case ICMP_TIMXCEED: if(code>1) goto badcode; goto deliver; deliver: icmpstat.icps_badlen++; goto freeit; badcode: icmpstat.icps_badcode++; break; case ICMP_ECHO: icp->icmp_type = ICMP_ECHOREPLY; goto reflect; reflect: uint8* tempicp; uint8 j; strToBuf((char*)icp,tempicp,8); for(;j<8;j++){ *tempicmphead = *tempicp; tempicmphead++; tempicp++; } Reflect(temphead); return; case ICMP_ECHOREPLY: //回显应答待处理 Time_EchoReplyRecv = OSTimeGet(); default: break; } freeit: Mbuf_Free(m); } /*********************************************************************************************************************************** *函数名：Reflect() *功能描述：交换传入的IP报文首部的源地址和目的地址 *参数： *返回值：void ***********************************************************************************************************************************/ void Reflect(uint8* Segment){ struct mbuf* tempiphead; struct ip_header *ip; uint8 i,j; tempiphead = Mbuf_Create(Segment,ICMP_IP_HEADER,20); ip = mtod(tempiphead,struct ip_header *); uint8 Temp_addr[4]; for(i=0;i<4;i++){ Temp_addr[i] = (ip->ip_dst)++; } ip->ip_dst = ip->ip_src; for(j=0;j<4;j++){ (ip->ip_src)++ = Temp_addr[j]; } Send(Segment); } //计算ICMP校验和，打包ICMP报文并发送 void Send(uint8* Segment){ struct mbuf *tempip; struct ip_header *ip; uint8* temp; uint8 i; uint8 length; struct icmp *icp; struct mbuf *tempicp; tempip = Mbuf_Create(Segment,ICMP_IP_HEADER,20); ip = mtod(tempip,struct ip_header *); length = ip->ip_len; uint8* tempicmphead = Segment + 20; for(i=0;i<8;i++){ *temp = *tempicmphead; tempicmphead++; temp++; } tempicp = Mbuf_Create(temp,ICMP,8); icp = mtod(tempicp,struct icmp *); icp->icmp_cksum = 0; icp->icmp_cksum = Checksum(icp,ip->ip_len-20); uint8* temp1; strToBuf((char *)icp,temp1,8); uint8 j; for(j = 0;j<8;j++){ *tempicmphead = *temp1; temp1++; tempicmphead++; } struct mbuf* tempbuf; tempbuf = Mbuf_Create(Segment,ICMP_IP,length); (void) IP_Output(tempbuf,ip->ip_dst,1); } //计算校验和函数 uint16 Checksum(struct icmp* icmp,uint8 len){ uint32 sum = 0; while(len>1){ sum += *((uint16*)icmp)++; if(sum&0x80000000){ sum = (sum&0xFFFF)+(sum>>16); } len -=2; } if(len) sum += (uint16)*(uint8 *)icmp; while(sum>>16){ sum = (sum & 0xffff)+(sum>>16); } return(~sum); } //Ping函数 uint32 Ping(uint8* addr,uint8 len){ struct mbuf *m; struct icmp *tempicp; uint8 datalen = 30; uint8 packsize; uint8 Ping_addr[4]; uint8 i; uint8* temp; if(i=0;i<len;i++){ Ping_addr[i] = *(addr+i); } tempicp->icmp_type = ICMP_ECHO; tempicp->icmp_code = 0; tempicp->icmp_cksum = 0; tempicp->icmp_seq = 0; tempicp->icmp_id = 0; packsize = 8+datalen; //strToBuf((char *)tempicp,temp,8); tempicp->icmp_cksum = Checksum(tempicp,packsize); strToBuf((char *)tempicp,temp,packsize); m = Mbuf_Create(temp,ICMP,packsize); (void) IP_Output(m,Ping_addr,1); //获取系统时间 Time_EchoSend = OSTimeGet(); OSTimeDlyHMSM(0,0,5,0); if(Time_EchoReplyRecv){ RTT = (Time_EchoReplyRecv -