www.pudn.com > uCOSII上实现的tcpip.rar > zicmp.c

#include "include/zicmp.h"


int icmp_input(znetif_t *pnetif, zbuffer_t *pbuffer)
{
	ip_header_t	*pipheader;
	icmp_echo_header_t *picmp_echo_header;
	eth_header_t *pethheader;

	ipaddr_t tmpipaddr;
	u8_t	i;
	u16_t	hlen;
	u8_t	type;

	
	pipheader =(ip_header_t *) (( u8_t*)pbuffer->pdata + ETH_HEAD_LEN);	/*remenber buffer is whole PACKET*/
	pethheader = (eth_header_t *) (( u8_t*)pbuffer->pdata );
	
	hlen = IPH_HL(pipheader) * 4 + ETH_HEAD_LEN;			/*must jump ETH and IP */
	
	type = *(u8_t *)((u8_t *)pbuffer->pdata + hlen );		/*move to the ICMP header*/
	
	switch(type)
	{
	case ICMP_ECHO:
			if ( pbuffer->tot_len <  ( sizeof(ip_header_t) + sizeof( icmp_echo_header_t) ) )
				return -1;
			picmp_echo_header = (icmp_echo_header_t *)((u8_t *)pbuffer->pdata + hlen );

			/*change src and dest ip*/
			tmpipaddr = pipheader->src_ipaddr;
		    pipheader->src_ipaddr = pipheader->dest_ipaddr;
		    pipheader->dest_ipaddr = tmpipaddr;

			/*change ICMP type*/
			ICMPH_TYPE_SET(picmp_echo_header, ICMP_ER);

			/*change ICMP checksum*/
			if( picmp_echo_header->chksum >= htons(0xffff - (ICMP_ECHO << 8))) 
			{
				picmp_echo_header->chksum += htons(ICMP_ECHO << 8) + 1;
			}
			else
			{
				picmp_echo_header->chksum += htons(ICMP_ECHO << 8);
			}
			/*setup ETH addr */
			for ( i=0 ; i < 6 ; i++)
			{
				pethheader->dest_hwaddr.addr[i] = pethheader->src_hwaddr.addr[i];
				pethheader->src_hwaddr.addr[i] = pnetif->hwaddr.addr[i];
			}
		
			pnetif->netif_tx(pnetif, pbuffer);			
			zbuffer_delete(pbuffer);
			return 0;
			break;
	}
	zbuffer_delete(pbuffer);
	return -1;
}

void icmp_dest_unreach(znetif_t *pnetif, zbuffer_t * pbuffer, enum icmp_dur_type t)
{
	ip_header_t	* pipheader;
	u16_t	offset;
	u8_t	i;
	icmp_dur_header_t	*pdur;
	eth_header_t		*pethheader;
	ipaddr_t		tmpipaddr;

					
	pethheader = (eth_header_t *)(pbuffer->pdata);
	pipheader =(ip_header_t *) (( u8_t*)pbuffer->pdata + ETH_HEAD_LEN);	/*remenber buffer is whole PACKET*/
	offset = IPH_HL(pipheader) * 4 + ETH_HEAD_LEN;
	pdur = (icmp_dur_header_t *)((u8_t *)pbuffer->pdata + offset);


	pbuffer = zbuffer_adjust(pbuffer, 0, (offset + 8 + IP_HEAD_LEN + 8 - pbuffer->tot_len) );
	
	if (pbuffer == NULL )
		return;

	for ( i = 0; i < 8 + IP_HEAD_LEN; i++)
	{
		*(u8_t *)((u8_t *)pbuffer->pdata + offset + i + 8) = *(u8_t *)((u8_t *)pbuffer->pdata + ETH_HEAD_LEN  + i);
	}
	
	for ( i = 0; i < 4; i++)
	{
		*(u8_t *)((u8_t *)pbuffer->pdata + offset + i + 4) = 0x00;
	}
	ICMPH_TYPE_SET(pdur, ICMP_DUR);
	ICMPH_CODE_SET(pdur, t);
	pdur->chksum = 0;
	pdur->chksum = inet_chksum(pdur, 8 + IP_HEAD_LEN + 8);

	/*change src and dest ip and proto and checksum*/
	tmpipaddr = pipheader->src_ipaddr;
	pipheader->src_ipaddr = pipheader->dest_ipaddr;
	pipheader->dest_ipaddr = tmpipaddr;
	pipheader->_len = offset + 8 + IP_HEAD_LEN + 8 - ETH_HEAD_LEN;	/*updata length*/
	pipheader->_ttl_proto &= (0xff00 &  IP_PROTO_ICMP);
	pipheader->_chksum = 0;
	pipheader->_chksum = inet_chksum(pipheader, IPH_HL(pipheader) * 4);

	for ( i=0 ; i < 6 ; i++)
	{
		pethheader->dest_hwaddr.addr[i] = pethheader->src_hwaddr.addr[i];
		pethheader->src_hwaddr.addr[i] = pnetif->hwaddr.addr[i];
	}
	
	pnetif->netif_tx(pnetif, pbuffer);
	zbuffer_delete(pbuffer);

}