network.c

pxa270 的bootloader源码

#include <setup.h>
#include <stdio.h>
#include <ctype.h>
#include <string.h>
#include <time.h>
#include <command.h>
#include <network.h>
#include <netdev.h>


static uint16 ip_id;

extern bool bootp_recv(void *packet, int len);
extern bool tftp_recv(int port, void *packet, int len);

void *find_mac_addr(uint32 ip);

static int set_arp_header(void *packet, int type, uint32 dip, uint32 sip, void *dmac, void *smac);
static void eth_recv_packet(void *packet, int len);
static void arp_recv_packet(void *packet, int len);
static void ip_recv_packet(void *packet, int len);
static void udp_recv_packet(void *packet, int len);

static void send_icmp_echo_request(uint32 dip, uint16 id, uint16 seq);
static void icmp_recv_packet(void *packet, int len);

static uint16 chksum(void *ptr, int len);

bool net_send_packet(void *packet, int len){
	return eth_send(packet, len);
}

bool net_recv_poll(void){
	return eth_recv_poll();
}

void netif_rx(void *packet, int len){
	eth_recv_packet(packet, len);
	return;
}

void net_set_haddr(const void *haddr){
	eth_set_haddr(haddr);
	return;
}

static uint16 chksum(void *buff, int len){
	ulong sum = 0;
	uint16 *p = buff;
	len = len / 2;
	while (len-- > 0)
		sum += *p++;
	sum = (sum & 0xFFFF) + (sum >> 16);
	sum = (sum & 0xFFFF) + (sum >> 16);
	return ~sum;
}

// dmac : destination mac address
int set_ether_header(void *packet, const void *dmac, uint16 prot){
	struct ethhdr *eth = (struct ethhdr *)packet - 1;
	memcpy(eth->et_dest, dmac, 6);
	memcpy(eth->et_src, setup->myhaddr, 6);
	eth->et_protlen = htons(prot);
	return sizeof(struct ethhdr);
}

int set_ip_header(void *packet, uint32 sip, uint32 dip, int prot, int len){
	struct iphdr *ip = (struct iphdr *)packet - 1;

	// 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) ((uchar *)packet)[len] = 0;

	ip->ip_hl_v   = 0x45;								// 泅犁 ip version 4, IP_HDR_SIZE / 4 (not including UDP)
	ip->ip_tos    = 0;									// type of service. 泅犁 network俊辑 瘤盔救窃. 亲惑 0.
	ip->ip_len    = htons(sizeof(struct iphdr) + len);	// total length of ip packet.
	ip->ip_id     = htons(ip_id); ip_id++;				// identifier.
	ip->ip_off    = htons(0x0000);						// No fragmentation.
	ip->ip_ttl    = 64;									// time to live.
	ip->ip_p      = prot;
	ip->ip_chksum = 0;
	memcpy((char *)&(ip->ip_src), &(sip), 4);
	memcpy((char *)&(ip->ip_dest), &(dip), 4);
	ip->ip_chksum = chksum(ip, sizeof(struct iphdr));
	return sizeof(struct iphdr);
}

static void eth_recv_packet(void *packet, int len){
	uint16 prot;
	struct ethhdr *eth = packet;
	void (*upper_recv)(void *packet, int len);

	if (memcmp(eth->et_dest, setup->myhaddr, 6)) return;

	prot = ntohs(eth->et_protlen);
	if (prot == PROT_ARP){
		upper_recv = arp_recv_packet;
	} else if (prot == PROT_IP){
		upper_recv = ip_recv_packet;
	} else return;
	upper_recv(eth+1, len - sizeof(struct ethhdr));
	return;
}	

static void ip_recv_packet(void *packet, int len){
	int prot;
	struct iphdr *ip = packet;
	void (*upper_recv)(void *packet, int len);

       //  printf("ip_recv_packet() enter here \n");

	if (len < sizeof(struct iphdr)){printf("1\n"); return;}
	//if (len < ntohs(ip->ip_len)) {printf("2\n"); return;}

	if ((ip->ip_hl_v & 0xf0) != 0x40) {printf("3\n"); return;}					// 泅犁 ip version篮 4.
	if (ip->ip_off & htons(0x1fff)) {printf("5\n");return;}						// can't deal fragments.
	if (chksum(ip, sizeof(struct iphdr))) {printf("6\n");return;}				// checksum
	if (setup->myipaddr && (ip->ip_dest != setup->myipaddr)) {printf("7\n"); return;}

        // printf("ip_recv_packet() enter here again\n ");

	prot = ip->ip_p;
	if (prot == IP_P_UDP)
		upper_recv = udp_recv_packet;
	else if (prot == IP_P_ICMP)
		upper_recv = icmp_recv_packet;
	else return;

	upper_recv(ip+1, ntohs(ip->ip_len) - sizeof(struct iphdr));
	return;
}

/* udp */
int set_udp_header(void *packet, uint16 dport, uint16 sport, int len){
	struct udphdr *udp = (struct udphdr *)packet - 1;
	udp->udp_dest    = htons(dport);
	udp->udp_src     = htons(sport);
	udp->udp_len     = htons(sizeof(struct udphdr) + len);
	udp->udp_chksum  = 0;
	return sizeof(struct udphdr);
}

static void udp_recv_packet(void *packet, int len){
	int port;
	struct udphdr *udp = packet;

      //  printf("udp_recv_packet() enter here \n");
	if (len < sizeof(struct udphdr)) return;
	if (len < ntohs(udp->udp_len)) return;

	port = ntohs(udp->udp_dest);
	if (port == PORT_BOOTP){
		bootp_recv(udp+1, len - sizeof(struct udphdr));
	} else if (port == PORT_TFTP_CLIENT){
		tftp_recv(ntohs(udp->udp_src), udp+1, ntohs(udp->udp_len) - sizeof(struct udphdr));
	}
	return;
}

/* arp */
struct arp_cache {
	bool exist;
	uint32 ip;
	uchar haddr[8];
	time_t last;
};

static struct arp_cache arp_cache;

static int set_arp_header(void *packet, int type, uint32 dip, uint32 sip, void *dmac, void *smac){
	uchar *s;
	struct arphdr *arp = packet;

	arp->ar_hrd = htons(HWT_ETHER);			// format of hardware address
	arp->ar_pro = htons(ETH_P_IP);			// format of protocol address
	arp->ar_hln = 0x06;						// length of hardware address
	arp->ar_pln = 0x04;						// length of protocol address
	arp->ar_op = htons(type);

	s = (uchar *)(arp + 1);
	memcpy(s, smac, 6);						// sender ethernet address (sha)
	s += 6;
	memcpy(s, &sip, 4);						// sender ip address (spa)
	s += 4;
	memcpy(s, dmac, 6);						// target ethernet address (tha)
	s += 6;
	memcpy(s, &dip, 4);						// target ip address (tpa)
	s += 4;
	return sizeof(struct arphdr) + 2 * (6 + 4);	
}

static bool send_arp_request_packet(uint32 ip){
	int n, len;
	uchar pktbuff[1024], *packet;
	uchar broadcast[] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
	packet = pktbuff + 100;

	len = set_arp_header(packet, ARPOP_REQUEST, ip, setup->myipaddr, "\0\0\0\0\0\0", setup->myhaddr);
	n = set_ether_header(packet, broadcast, PROT_ARP);
	packet -= n; len += n;
	return net_send_packet(packet, len);
}

static void arp_recv_packet(void *packet, int len){
	int n, txsize;
	uchar pktbuff[1024], *txpkt = pktbuff + 100;
	struct arphdr *arp = packet;

	if (len < ARP_HDR_SIZE) return;
	if (ntohs(arp->ar_hrd) != ARP_ETHER) return;
	if (ntohs(arp->ar_pro) != PROT_IP) return;
	if (ntohs(arp->ar_hln != 6)) return;
	if (ntohs(arp->ar_pln != 4)) return;

	if (ntohs(arp->ar_op) == ARPOP_REQUEST){  //  printf("\narp_recv_packet() ARPOP_REQUEST 1\n");
		if (setup->myipaddr==0 || memcmp(&(arp->ar_data[16]), &setup->myipaddr, 4)) return;

		txsize = set_arp_header(txpkt, ARPOP_REPLY, *(uint32 *)(arp->ar_data + 6), setup->myipaddr, arp->ar_data, setup->myhaddr);
		n = set_ether_header(txpkt, arp->ar_data , PROT_ARP);
		txpkt -= n; txsize += n;
		net_send_packet(txpkt, txsize);
	} else if (ntohs(arp->ar_op) == ARPOP_REPLY){ //  printf("\narp_recv_packet() ARPOP_REPLY 2\n" );
		time_t now;
		struct arp_cache *ap = &arp_cache;
	//	if (ap->exist) return;
		if (memcmp(&ap->ip, arp->ar_data+6, 4)) return;
		ap->exist = true;
		ap->last = time(&now);
		memcpy(ap->haddr, arp->ar_data, 6);
	}
	return;
}

/* icmp */
static uint16 icmp_id;
static uint32 icmp_dip;
static void icmp_dummy_handler(uint32 dip, uint16 seq){ return; }
static void (*icmp_handler)(uint32 dip, uint16 seq) = icmp_dummy_handler;

void ping(uint32 ip, void (*handler)(uint32 dip, uint16 seq)){
	int i, id, seq;
	clock_t timeout;
	seq = 0; id = clock();

	for (i=0; i < 5; i++){
		icmp_handler = handler;
		timeout = clock() + CLOCKS_PER_SEC;
		send_icmp_echo_request(ip, id, seq++);
		while (clock() < timeout)
			net_recv_poll();
	}
	icmp_handler = icmp_dummy_handler;
	return;
}

static int set_icmp_header(void *packet, uint8 type, uint16 id, uint16 seq){
	struct icmphdr *icmp = packet;

	icmp->type = type;
	icmp->code = 0;
	icmp->chksum = 0;
	icmp->id = htols(id);
	icmp->seq = htols(seq);
	icmp->chksum = chksum(packet, sizeof(struct icmphdr));
	return sizeof(struct icmphdr);
}

static void icmp_recv_packet(void *packet, int len){
	struct icmphdr *icmp = packet;

	if (icmp->type != ICMP_ECHO_REPLY) return;
	if (icmp->code != 0) return;
	if (chksum(icmp, sizeof(struct icmphdr))) return;
	if (icmp->id != htols(icmp_id)) return;

	icmp_handler(icmp_dip, htols(icmp->seq));
	icmp_handler = icmp_dummy_handler;
	return;
}

static void send_icmp_echo_request(uint32 dip, uint16 id, uint16 seq){
	int n, len;
	uchar pktbuff[1024], *packet;
	uchar *dmac;

	dmac = find_mac_addr(dip);

	icmp_id = id;
	icmp_dip = dip;

	packet = pktbuff + 100;
	len = set_icmp_header(packet, ICMP_ECHO_REQUEST, id, seq);
	n = set_ip_header(packet, setup->myipaddr, dip, IP_P_ICMP, len);
	packet -= n; len += n;
	n = set_ether_header(packet, dmac, PROT_IP);
	packet -= n; len += n;
	net_send_packet(packet, len);
	return;
}


static inline void *check_arp_cache(uint32 ip){
	time_t now;
	struct arp_cache *ap = &arp_cache;

	if (!ap->exist) goto failed;					/* not exist */
	if (time(&now) > ap->last + 5) goto failed;		/* expired */
	if (ip != ap->ip) goto failed;					/* no match */
	ap->last = time(&now);
	return ap->haddr;
failed :
	return 0;
}

void *find_mac_addr(uint32 ip){
	//time_t start, now;
	void *mac;
	clock_t timeout;
	struct arp_cache *ap = &arp_cache;

	mac = check_arp_cache(ip);
	if (mac) return mac;

	ap->exist = false;
	ap->ip = ip;
	timeout = clock() + CLOCKS_PER_SEC ;    //modify by xian ray.
	send_arp_request_packet(ip);
	while (clock() < timeout){
		net_recv_poll();
		if (ap->exist && (ap->ip == ip))
                     { // printf("\n\nap fuzhi chenggong ,return now\n" );
			return  ap->haddr;
                     }
	}
	return 0;
}

uint32 inet_addr(const char *ip){
	int i, ndots;
	char ch;
	bool valid;
	uint32 retval, val;

	i = val = retval = ndots = 0;
	valid = false;
	while ((ch = ip[i++])){
		if (isdigit(ch)){
			val = (val * 10) + (ch & 0x0F);	// '0' -> 0.
			valid = true;
		} else if (ch == '.'){
			if (val > 0xFF) return 0;
			retval = (retval << 8) | val;
			val = 0;
			ndots++;
			valid = false;
		} else return 0;
	}
	if (!valid || ndots != 3) return 0;
	retval = (retval << 8) | val;
	return htonl(retval);
}

uint32 delay_aton(const char *delaytime)
{
	uint32 ret, tmp, count;
	
	ret = 0;
	count = strlen(delaytime);
	
	if (count < 1 || count > 2)
		return DELAYTIME;

	while (count--){
		tmp = *delaytime;
		if (!isdigit(tmp)) return DELAYTIME;
		if (count == 0)
			ret = ret + tmp - '0';
		else 
			ret = (tmp - '0') * 10;
		delaytime++;
	}

	return ret;
}

char *inet_ntoa(uint32 ip){
	uchar *s;
	static char buff[16];

	s = (uchar *)&ip;
	sprintf(buff, "%d.%d.%d.%d", s[0], s[1], s[2], s[3]);
	return buff;
}

uchar *mac_ntoa(const void *mac){
	int i;
	const uchar *s = mac;
	static char buff[24];
	char *at = buff;
	at += sprintf(at, "%02X", *s++);
	for (i=1; i < 6; i++)
		at += sprintf(at, ":%02X", *s++);
	return buff;
}

void *mac_aton(const uchar *str){
	int ch, pos;
	static uchar buff[8];
	memset(buff, 0, sizeof(buff));

	pos = 0;
	while (*str && pos < 12){
		if (*str == ':'){ str++; continue; }
		ch = *str;
		if (!isxdigit(ch)) return 0;
		ch = isdigit(ch) ? ch - '0' : tolower(ch) - 'a' + 10;
		buff[pos / 2] |= ch << (4 * (1 - (pos % 2)));
		pos++; str++;
	}
	if (*str || pos < 12) return 0;
	return buff;
}