net_ipv4.c

bootloader源代码

/*****************************************
 Copyright (c) 2002
 Sigma Designs, Inc. All Rights Reserved
 Proprietary and Confidential
 *****************************************/

/* This file is part of the boot loader */

/*
 * net_ipv4.c
 *
 * UDP/IP protocol implementation
 */

#include "util.h"
#include "vsprintf.h"
#include "uart.h"
#include "timer.h"
#include "net.h"
#include "net_ipv4.h"

#if 0
#define DPRINTF(x...)		PrintFormat(x)
#else
#define DPRINTF(x...)
#endif

//
// global variables
//

// ethernet 
static unsigned char g_eth_broadcast[] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };

// ARP
arptable_t g_arptable[MAX_ARPTABLE];

//
// Init
// 

int ipv4_init(void)
{
	memset(&g_arptable, 0, sizeof g_arptable);

	return 0;
}

void ipv4_setup_arptable(in_addr_t ipaddr, in_addr_t netmask, unsigned char *node, in_addr_t gateway, in_addr_t dns)
{
	arp_setaddr_index(ARP_CLIENT, ipaddr, netmask, node);
	arp_setaddr_index(ARP_GATEWAY, gateway, INADDR_ANY, NULL);
	arp_setaddr_index(ARP_DNS, dns, INADDR_ANY, NULL);
}

//
// TCP/IP Basic
//

// return 
//   0 : A class
//   1 : B class
//   2 : C class
//   < 0 : invalid
int ipv4_get_ipaddr_class(in_addr_t ipaddr)
{
	if (IN_CLASSA(ipaddr))
		return 0;
	else if (IN_CLASSB(ipaddr))
		return 1;
	else if (IN_CLASSC(ipaddr))
		return 2;
	else
		return -1;
}

// check IP address class and return appropriate default network mask address
// return INADDR_INVALID if the address is not valid
in_addr_t ipv4_get_default_netmask(in_addr_t ipaddr)
{
	switch (ipv4_get_ipaddr_class(ipaddr)) {
	case 0 : return IN_CLASSA_NET;
	case 1 : return IN_CLASSB_NET;
	case 2 : return IN_CLASSC_NET;
	default : return INADDR_INVALID;
	}
}

// check if the IP address is valid
// IP address should not be zero or broadcast address in the subclass
int ipv4_ipaddr_valid(in_addr_t ipaddr)
{
	int netmask;

	// zero of broadcast
	if (ipaddr == INADDR_ANY || ipaddr == INADDR_BROADCAST)
		return 0;
	
	// check if the address is in the valid region
	if (ipv4_get_ipaddr_class(ipaddr) < 0)
		return 0;
	
	// check if the address is zero or broadcast in the subclass
	netmask = ipv4_get_default_netmask(ipaddr);
	ipaddr &= ~netmask;
	if (ipaddr == 0 || ipaddr == (~netmask))
		return 0;

	return 1;
}

// assign not assigned user port
int ipv4_alloc_port(void)
{
	static int s_lastport = IPPORT_RESERVED;

	if (++s_lastport == IPPORT_USERRESERVED)
		s_lastport = IPPORT_RESERVED;

	return s_lastport;
}

//
// Packet processing
//

// decide whether to accept the packet or discard it. 
// called before decapsulating the packet
int ipv4_check_for_me(struct sk_buff *skb)
{
	DPRINTF("RX : %d, To = %02x:%02x:%02x:%02x:%02x:%02x, From = %02x:%02x:%02x:%02x:%02x:%02x, Proto = %02x%02x\n", 
		skb->len, 
		skb->data[0], skb->data[1], skb->data[2], skb->data[3], skb->data[4], skb->data[5],
		skb->data[6], skb->data[7], skb->data[8], skb->data[9], skb->data[10], skb->data[11],
		skb->data[12], skb->data[13]
		);
		
	// if the destination is my MAC address, it's for me
	if (memcmp(skb->data, g_arptable[ARP_CLIENT].node, ETH_ALEN) == 0)
		return 1;

	// accept just a few of broadcast message
	// 1. BOOTP/DHCP response
	// 2. ARP request
	if (memcmp(skb->data, g_eth_broadcast, ETH_ALEN) == 0) {
		struct ethhdr *eth;
		struct iphdr *ip;
		struct udphdr *udp;
		struct arprequest *arp;

		eth = (struct ethhdr *) skb->data;

		switch (ntohs(eth->h_proto)) {
		case ETH_P_IP :
			ip = (struct iphdr *) (skb->data + sizeof(struct ethhdr));
			
			switch (ip->protocol) {
			case IPPROTO_UDP : 
				udp = (struct udphdr *) (skb->data + sizeof(struct ethhdr) + sizeof(struct iphdr));
				if (ntohs(udp->dest) == IPPORT_BOOTP_CLIENT)
					return 1;
				break;
			}
			break;

		case ETH_P_ARP :
			arp = (struct arprequest *) (skb->data + sizeof(struct ethhdr));
			PrintFormat("ARP for %d, %s\n", ntohs(arp->ar_op), ipaddr_to_str(ntohl(arp->ar_tip)));
			if (ntohl(arp->ar_tip) == g_arptable[ARP_CLIENT].ipaddr) 
				return 1;
			break;
		}
	}

	return 0;
}

// parse the ethernet packet
// fix the byte order
int ipv4_parsepacket(struct sk_buff *skb)
{
	int len = skb->len;

	// parse ethernet header
	skb->eth = (struct ethhdr *) skb->data;
	skb->eth->h_proto = ntohs(skb->eth->h_proto);

	skb->ethdata = skb->data + ETH_HLEN;
	len -= ETH_HLEN;
	skb->ethdata_len = len;
	
	// parse ethernet data
	skb->arp = NULL;
	skb->ip = NULL;
	skb->ipdata = NULL;
	skb->icmp = NULL;
	skb->udp = NULL;
	skb->udpdata = NULL;
	skb->bootp = NULL;
	skb->tftp = NULL;

	// parse each protocol header (especially byte order)
	switch (skb->eth->h_proto) {
	case ETH_P_IP : 
		DPRINTF("IP : ");
		skb->ip = (struct iphdr *) skb->ethdata;

		if (ipv4_calc_checksum(skb->ip, sizeof(struct iphdr)) != 0) {
			PrintUart("IP packet checksum error.\r\n", -1);
			return 1;
		}

		skb->ip->tot_len = ntohs(skb->ip->tot_len);
		skb->ip->id = ntohs(skb->ip->id);
		skb->ip->frag_off = ntohs(skb->ip->frag_off);
		skb->ip->saddr = ntohl(skb->ip->saddr);
		skb->ip->daddr = ntohl(skb->ip->daddr);

		skb->ipdata = skb->ethdata + sizeof(struct iphdr);
		if (len > skb->ip->tot_len)
			len = skb->ip->tot_len;
		len -= sizeof(struct iphdr);
		skb->ipdata_len = len;

		switch (skb->ip->protocol) {
		case IPPROTO_IP :
			DPRINTF("IP\n");
			break;
		case IPPROTO_ICMP :
			DPRINTF("ICMP\n");
			skb->icmp = (struct icmphdr *) skb->ipdata;

			if (ipv4_calc_checksum(skb->icmp, len) != 0) {
				PrintUart("ICMP packet checksum error.\r\n", -1);
				return 1;
			}

			if (skb->icmp->type == ICMP_ECHOREPLY || skb->icmp->type == ICMP_ECHO) {
				skb->icmp->un.echo.id = ntohs(skb->icmp->un.echo.id);
				skb->icmp->un.echo.sequence = ntohs(skb->icmp->un.echo.sequence);
			}
			break;
		case IPPROTO_UDP :
			DPRINTF("UDP\n");
			skb->udp = (struct udphdr *) skb->ipdata;

			if (ipv4_calc_tcpudp_checksum(skb->udp, len, skb->ip->saddr, skb->ip->daddr) != 0) {
				PrintUart("UDP checksum error.\r\n", -1);
				return 1;
			} 

			skb->udp->src = ntohs(skb->udp->src);
			skb->udp->dest = ntohs(skb->udp->dest);
			skb->udp->len = ntohs(skb->udp->len);

			if (skb->udp->len != len) {
				PrintUart("UDP packet size mismatch\r\n", -1);
				return 1;
			}

			skb->udpdata = skb->ipdata + sizeof(struct udphdr);
			len -= sizeof(struct udphdr);
			skb->udpdata_len = len;
		}
		break;
		
	case ETH_P_ARP :
		skb->arp = (struct arprequest *) skb->ethdata;
		skb->arp->ar_hrd = ntohs(skb->arp->ar_hrd);
		skb->arp->ar_pro = ntohs(skb->arp->ar_pro);
		skb->arp->ar_op = ntohs(skb->arp->ar_op);
		skb->arp->ar_sip = ntohl(skb->arp->ar_sip);
		skb->arp->ar_tip = ntohl(skb->arp->ar_tip);
		DPRINTF("ARP : %d, from = %s, to = %s\n", skb->arp->ar_op, 
			ipaddr_to_str(skb->arp->ar_sip),
			ipaddr_to_str(skb->arp->ar_tip));
		break;

	case ETH_P_RARP : 
		DPRINTF("RARP\n");
		break;
	default : 
		return 1;
	}

	return 0;
}

int ipv4_processpacket(struct sk_buff *skb)
{
	switch (skb->eth->h_proto) {
	case ETH_P_IP :
		if (skb->icmp && skb->icmp->type == ICMP_ECHO) {
			// reply to echo request
			int len = skb->ipdata_len;
			skb->icmp->type = ICMP_ECHOREPLY;
			skb->icmp->checksum = 0;
			skb->icmp->checksum = ipv4_calc_checksum(skb->icmp, sizeof(struct icmphdr));
			len += sizeof(struct iphdr);
			ip_transmit(skb->ip->saddr, IPPROTO_ICMP, len, skb->ip);
		}
		break;
	case ETH_P_ARP :
		// reply to ARP request
		if (skb->arp->ar_op == ARPOP_REQUEST) {
			arptable_t *arp;
			in_addr_t sipaddr, tipaddr;

			// Add or update ARP table
			tipaddr = skb->arp->ar_tip;
			if (tipaddr == g_arptable[ARP_CLIENT].ipaddr) {
				sipaddr = skb->arp->ar_sip;
	 			if ((arp = arp_lookup(sipaddr)) == NULL)
					arp = arp_alloc();
				if (arp) 
					arp_setaddr(arp, sipaddr, INADDR_ANY, skb->arp->ar_sha);

				// transmit ARP reply packet
				DPRINTF("ARP reply sent\r\n", -1);
				arp_transmit(ARPOP_REPLY, sipaddr, skb->arp->ar_sha);
			}
		}
		break;
	case ETH_P_RARP :
		break;
	}

	return 0;
}

//
// Ethernet
//

// send ethernet packet to specified ethernet address
// dest : ethernet address
// type : ethernet protocol ID (ETH_P_xxx)
int eth_transmit(unsigned char *dest, uint16_t type, unsigned int len, const void *buf)
{
	unsigned char txbuf[ETH_BUF_LEN];
	ethhdr_t *pethhdr = (ethhdr_t *) txbuf;
	struct sk_buff skb;

	// fill ethernet header
	memcpy(pethhdr->h_dest, dest, ETH_ALEN);	// Destination address
	memcpy(pethhdr->h_source, g_arptable[ARP_CLIENT].node, ETH_ALEN);	// Source address
	pethhdr->h_proto = htons(type);				// Frame type
	memcpy(txbuf + ETH_HLEN, buf, len);			// Frame data

	// fill sk_buff structure
	skb.data = txbuf;
	skb.len = len + ETH_HLEN;
	skb.valid = 1;

	// send it!
	return net_sendpacket(&skb);
}

struct sk_buff *eth_receive(int type, int timeout)
{
	struct sk_buff *skb;
	unsigned int startticks = timer_getticks();
   
	while ((skb = net_getpacket(timeout)) != NULL) {
		if (ipv4_parsepacket(skb) == 0) {
			switch (type) {
			case WAIT_NOTHING : 
				break;
			case WAIT_ARP_REPLY : 
				if (skb->arp && skb->arp->ar_op == ARPOP_REPLY)
					return skb;
				break;
			case WAIT_ICMP_REPLY : 
				if (skb->icmp && skb->icmp->type == ICMP_ECHOREPLY)
					return skb;
				break;
			case WAIT_UDP : 
				if (skb->udp)
					return skb;
				break;
			}

			ipv4_processpacket(skb);
		}

		skb_free(skb);

		if (timer_timeout(startticks, timeout))
			break;
	}

	return NULL;
}

//
// ARP table
//

arptable_t *arp_lookup(unsigned long ipaddr)
{
	int i;

	for (i = 0; i < MAX_ARPTABLE; ++i)
		if (g_arptable[i].ipaddr == ipaddr)
			return g_arptable + i;

	return NULL;
}

arptable_t *arp_alloc(void)
{
	int i;

	for (i = ARP_USER; i < MAX_ARPTABLE; ++i)
		if (g_arptable[i].ipaddr == INADDR_ANY)
			break;
	if (i == MAX_ARPTABLE)
		return NULL;

	return g_arptable + i;
}

int arp_valid(arptable_t *arp)
{
	int i;

	if (!ipv4_ipaddr_valid(arp->ipaddr))
		return 0;

	for (i = 0; i < ETH_ALEN; ++i)
		if (arp->node[i] != 0)
			return 1;

	return 0;
}

void arp_setaddr(arptable_t *arp, in_addr_t ipaddr, in_addr_t netmask, unsigned char *node)
{
	arp->ipaddr = ipaddr;
	arp->netmask = (netmask == INADDR_ANY) ? ipv4_get_default_netmask(ipaddr) : netmask;
	if (node)
		memcpy(arp->node, node, ETH_ALEN);
	else
		memset(arp->node, 0, ETH_ALEN);
}

void arp_setaddr_index(int index, in_addr_t ipaddr, in_addr_t netmask, unsigned char *node)
{
	arp_setaddr(g_arptable + index, ipaddr, netmask, node);
}

void arp_listtable(arptable_t *arp)
{
	int i;
	unsigned char *cp;

	for (i = 0; i < MAX_ARPTABLE; ++i) {
		if ((arp == NULL || arp == g_arptable + i) && arp_valid(g_arptable + i)) {
			cp = g_arptable[i].node;
			PrintFormat("%-20s %02X:%02X:%02X:%02X:%02X:%02X\n",