ip.c

基于Freescale的MX21处理器的bootloader程序

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "net.h"
#include "io.h"

IPaddr_t NetConvIP(char * str)
{
	IPaddr_t ip = 0;
	char *head = str;
	char *tail = str;
	while(*tail != 0){
		if (*tail == '.'){
			*tail = 0;
			ip <<= 8;
			ip += atoi(head);
			*tail = '.';
			head = tail + 1;
		}		
		tail++;
	}
	ip <<= 8;
	ip += atoi(head);
	return ip;
}

void NetWriteIP(volatile char * to, IPaddr_t ip)
{
	int	i;

	for (i = 0; i < 4; i++) 
	{
		*to++ = ip >> 24;
	    	ip <<= 8;
	}
}

IPaddr_t NetReadIP(volatile char * from)
{
	IPaddr_t ip;
	int i;

	ip = 0;
	for (i = 0; i < 4; i++) 
		ip = (ip << 8) | *from++;

	return ip;
}

void NetCopyIP(volatile char * to, volatile char *from)
{
	int i;
	for (i = 0; i < 4; i++) 
		*to++ = *from++;
}

void NetSetIP(volatile char * xip, IPaddr_t dest, int dport, int sport, int len)
{
	volatile IP_t *ip = (IP_t *)xip;

	/*
	 *	If the data is an odd number of bytes, zero the
	 *	byte after the last byte so that the checksum
	 *	will work.
	 */
	if (len & 1)
		xip[IP_HDR_SIZE + len] = 0;

	/*
	 *	Construct an IP and UDP header.
			(need to set no fragment bit - XXX)
	 */
	ip->ip_hl_v  = 0x45;		/* IP_HDR_SIZE / 4 (not including UDP) */
	ip->ip_tos   = 0;
	ip->ip_len   = SWAP16(IP_HDR_SIZE + len);
	ip->ip_id    = SWAP16(NetIPID++);
	ip->ip_off   = SWAP16c(0x4000);	/* No fragmentation */
	ip->ip_ttl   = 255;
	ip->ip_p     = 17;		/* UDP */
	ip->ip_sum   = 0;
	NetWriteIP((char*)&ip->ip_src, NetOurIP);
	NetWriteIP((char*)&ip->ip_dst, dest);
	ip->udp_src  = SWAP16(sport);
	ip->udp_dst  = SWAP16(dport);
	ip->udp_len  = SWAP16(8 + len);
	ip->udp_xsum = 0;
	ip->ip_sum   = ~ NetCksum((char *)ip, IP_HDR_SIZE_NO_UDP / 2);
}

void NetSetIP_NoUDP(volatile char * xip, IPaddr_t dest, int len)
{
	volatile IP_t *ip = (IP_t *)xip;

	/*
	 *	If the data is an odd number of bytes, zero the
	 *	byte after the last byte so that the checksum
	 *	will work.
	 */
	if (len & 1)
		xip[IP_HDR_SIZE + len] = 0;

	/*
	 *	Construct an IP and UDP header.
			(need to set no fragment bit - XXX)
	 */
	ip->ip_hl_v  = 0x45;		/* IP_HDR_SIZE / 4 (not including UDP) */
	ip->ip_tos   = 0;
	ip->ip_len   = SWAP16(IP_HDR_SIZE + len);
	ip->ip_id    = SWAP16(NetIPID++);
	ip->ip_off   = SWAP16c(0x4000);	/* No fragmentation */
	ip->ip_ttl   = 255;
	ip->ip_p     = 17;		/* UDP */
	ip->ip_sum   = 0;
	NetWriteIP((char*)&ip->ip_src, NetOurIP);
	NetWriteIP((char*)&ip->ip_dst, dest);
	ip->ip_sum   = ~ NetCksum((char *)ip, IP_HDR_SIZE_NO_UDP / 2);
}


void ip_to_string (IPaddr_t x, char *s)
{
    sprintf(s,"%d.%d.%d.%d",(int)((x >> 24) & 0xff),(int)((x >> 16) & 0xff),\
	(int)((x >>  8) & 0xff),(int)((x >>  0) & 0xff));
}

void print_IPaddr (IPaddr_t x)
{
    char tmp[12];

    ip_to_string(x, tmp);

    mputs(tmp);
}
 
void icmp_echo_reply(volatile char * pkt)
{
	Ethernet_t *et;
	IP_t	*ip;
	IPaddr_t ip_swap;
	ICMP_t  *hdr;
	unsigned short len;

	et = (Ethernet_t *)pkt;
	ip = (IP_t *)(pkt + ETHER_HDR_SIZE);
	hdr = (ICMP_t *)(pkt + ETHER_HDR_SIZE + IP_HDR_SIZE_NO_UDP);
	len = SWAP16(ip->ip_len);

	/* Adust icmp packet */
	hdr->type = ICMP_ECHO_REPLY;
	hdr->checksum = 0;
	hdr->checksum = ~ NetCksum((char *)hdr, (len - IP_HDR_SIZE_NO_UDP)/2);

	{
		char buf[IP_HDR_SIZE_NO_UDP];
		char *src,*des;
		IP_t *iphead = (IP_t *)buf;

		src = (char *)ip;
		des = (char *)buf;
		memcpy(des,src,IP_HDR_SIZE_NO_UDP);

		/* Adust ip packet */
		iphead->ip_id    = SWAP16(NetIPID++);
		iphead->ip_sum   = 0;
		ip_swap = iphead->ip_src;
		iphead->ip_src = iphead->ip_dst;
		iphead->ip_dst = ip_swap;
		iphead->ip_sum   = ~ NetCksum((char *)(buf), IP_HDR_SIZE_NO_UDP / 2);

		des = (char *)ip;
		src = (char *)buf;
		memcpy(des,(char *)buf,IP_HDR_SIZE_NO_UDP);
	}
	/* Adust ethernet packet */
	NetCopyEther((char *)(et->et_dest) , (char *)(et->et_src));
	NetCopyEther((char *)(et->et_src) , NetOurEther);
	NetSendPacket(pkt, ETHER_HDR_SIZE+len);
}