net.c

LUBBOCK板的BLOB

/*
 *  net.c : Simple network layer with TFTP/ICMP builtin
 *
 *  Copyright (c) 2003, Intel Corporation ([e-mail])
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License version 2 as
 *  published by the Free Software Foundation.
 */

#ifdef HAVE_CONFIG_H
# include <blob/config.h>
#endif

#include <blob/arch.h>
#include <blob/command.h>
#include <blob/serial.h>
#include <blob/util.h>
#include <blob/time.h>

#include <net.h>

#ifdef DEBUG
#define DBPRINT(args...) printf(args)
#else
#define DBPRINT(args...)
#endif

static struct mybuf in = {0} ,out1 = {0}, out2 = {0};
static unsigned char broadcast_mac_addr[6] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
static unsigned char broadcast_ip_addr[4] = {0xff, 0xff, 0xff, 0xff};

//static unsigned char mac_addr[6] = {0x0, 0x0, 0x0, 0x0, 0xf, 0xe};
static unsigned char ip_addr[4] = {192, 1, 1, 2};

static unsigned char svr_mac_addr[6];
static unsigned char svr_ip_addr[4] = {192,1,1,1};
static unsigned short our_tftp_port = 0;

static char image_name[256];
static unsigned int image_addr;

static int last_block_num;
static unsigned short ip_id=0; 

static unsigned long tftp_timeout = 0;
static int tftp_state = 0;
static int arp_state = 0;

#define MIN(a,b) ((a)<(b) ? (a): (b))

static unsigned short in_chksum(unsigned char *p, int len);
static void arp_input();
static void ip_input();
static void icmp_input();
static void udp_input();
static void arp_request();
static void tftp_handler();
static void do_tftp();

/* 
 * Very simple buffer management.
 *
 * io = 0 : request for input buffer
 * io = 1 : request for output buffer
 *
 * set buffer length to 0 to free the buffer.
 *
 */
struct mybuf * bget(int io) 
{
	if( io == 1) {	
		if(out1.len == 0) return &out1;
		if(out2.len == 0) return &out2;
	}else return &in;

	printf("%s:can't get buffer\n",__FUNCTION__);
	return 0;
}

static unsigned short in_chksum(unsigned char *p, int len)
{
	unsigned long sum=0;
	
	while(len > 1) {
		sum += *((unsigned short*)p)++;
		if( sum & 0x80000000 ) 
			sum = (sum & 0xFFFF) + (sum >> 16);
		len -= 2;
	}

	if(len) 
		sum += (unsigned short) *(unsigned char*) p;

	while(sum >> 16) 
		sum = (sum & 0xFFFF) + (sum >> 16);

	return ~sum;
}



/* ARP zone */

static void arp_input()
{
	int op;
	struct etherhdr *eh, *ehout;
	struct ether_arp *ea, *eaout ;
	struct mybuf *out;

	out = bget(1);
	if(!out) return ;

	eh = (struct etherhdr*) (in.buf);
	ea = (struct ether_arp*)(eh + 1 );

	ehout = (struct etherhdr*) (out->buf);
	eaout = (struct ether_arp*) (ehout + 1);

	/* sanity check */
	if ( ntohs(ea->ea_hdr.ar_hrd) != ARPHDR_ETHER ) {
		DBPRINT("check 1\n");
	       	return;
	}
	if ( ntohs(ea->ea_hdr.ar_pro) != ETHPROTO_IP ) {
		DBPRINT("check 2\n");
		return;
	}
	if ( ea->ea_hdr.ar_hln != 6) {
		DBPRINT("check 3\n");
		return;
	}

	if ( ea->ea_hdr.ar_pln != 4) {
		DBPRINT("check 4\n");
		return;
	}

	/* matching ip */

	op = ntohs(ea->ea_hdr.ar_op);

	switch(op) {
	case ARP_REQUEST:
		*ehout = *eh;
		*eaout = *ea;

		if ( !memcmp(ea->arp_spa, svr_ip_addr, 4) ) {
			memcpy(svr_mac_addr, ea->arp_sha, 6);
		}

		memcpy(ehout->eh_dhost, eh->eh_shost, 6);
		memcpy(ehout->eh_shost, eth_mac_addr, 6);

		eaout->ea_hdr.ar_op = htons(ARP_REPLY);

		memcpy(eaout->arp_tha, ea->arp_sha, 6);
		memcpy(eaout->arp_tpa, ea->arp_spa, 4);

		memcpy(eaout->arp_sha, eth_mac_addr, 6);
		memcpy(eaout->arp_spa, ip_addr, 4);

		out->len = sizeof(struct etherhdr) + sizeof(struct ether_arp);

		/* send it */
		eth_xmit(out);

		break;
	case ARP_REPLY:
		if( (!memcmp(eh->eh_dhost, eth_mac_addr, 6)) && ( !memcmp(ea->arp_spa, svr_ip_addr, 4) ) ) {
			arp_state = 1;
			memcpy(svr_mac_addr, ea->arp_sha, 6);
			do_tftp();
		}
		break;
	case ARP_REVREQUEST:
		break;
	case ARP_REVREPLY:
		break;
	}
}

static void arp_request()
{	
	struct etherhdr *eh;
	struct ether_arp *ea;	
	struct mybuf *out;

	out = bget(1);
	if(!out) return;

	eh = (struct etherhdr *) (out->buf);
	ea = (struct ether_arp *) (eh + 1 );

	memcpy(eh->eh_dhost, broadcast_mac_addr, 6);
	memcpy(eh->eh_shost, eth_mac_addr, 6);
	eh->eh_proto = htons( ETHPROTO_ARP);
	
	ea->ea_hdr.ar_hrd = htons(ARPHDR_ETHER);
	ea->ea_hdr.ar_pro = htons(ETHPROTO_IP);
	ea->ea_hdr.ar_hln = 6;
	ea->ea_hdr.ar_pln = 4;
	ea->ea_hdr.ar_op = htons(ARP_REQUEST);

	memcpy(ea->arp_sha, eth_mac_addr, 6);
	memcpy(ea->arp_spa, ip_addr, 4);
	//memcpy(ea->.tha, 6);
	memcpy(ea->arp_tpa, svr_ip_addr, 4);

	out->len = sizeof(struct etherhdr) + sizeof(struct ether_arp);

	eth_xmit(out);
}

/* IP zone */
static void ip_input()
{
	int hlen;

	struct iphdr *ip = (struct iphdr*) (in.buf + 14 );

	if (ip->ip_v != IPVERSION) {
		printf("%s:ip version not matacing\n", __FUNCTION__);
		return;
	}

	hlen =  ip->ip_hl << 2 ;

	if( hlen < sizeof(struct iphdr) ) {
		printf("%s:hlen < iphdr\n", __FUNCTION__);
		return;
	}

	ip->ip_sum = in_chksum((unsigned char*)ip, hlen);
       	if(ip->ip_sum) {
		printf("%s:chksum error\n",__FUNCTION__);
		return;
	}
	
	if(ntohs(ip->ip_len) < hlen) {
		printf("%s:ip_len < hlen: 0x%x, 0x%x\n",__FUNCTION__,ip->ip_len, hlen);
		return;
	}

	if( ntohs(ip->ip_off) & IP_OFFMASK ) {
		printf("%s:can't handler fragment\n", __FUNCTION__);
		return;
	}

	if( memcmp(&ip->ip_dst, ip_addr, 4) && memcmp(&ip->ip_dst,broadcast_ip_addr,4) ) {
		/* packet not for us, ingore */
		DBPRINT("noise, not for us\n");
		return;
	}

	switch(ip->ip_p) {
	case IPPROTO_ICMP:
		icmp_input();
		break;
	case IPPROTO_UDP:
		udp_input();
		break;
	default:
		DBPRINT("Unsupported IP packet\n");
		break;
	}
	return; 
}

/* ICMP zone */
static void icmp_input()
{
	struct in_addr t;
	struct icmphdr *icmp;
	struct mybuf *out;

	struct etherhdr *eh = (struct etherhdr*)(in.buf);
	struct iphdr *ip = (struct iphdr *) (in.buf + sizeof(struct etherhdr));
	int hlen = ip->ip_hl << 2;
	int icmplen = ntohs(ip->ip_len) - hlen;

	if( icmplen < ICMP_MINLEN ) {
		printf("%s: .. < ICMP_MINLEN\n", __FUNCTION__);
		return;
	}

	icmp = (struct icmphdr *) (in.buf + sizeof(struct etherhdr) + hlen);

	switch(icmp->type) {
	case ICMP_ECHO:
		out = bget(1);
		if(!out) return;

		icmp->type = ICMP_ECHOREPLY;
		icmp->cksum = 0;
		icmp->cksum = in_chksum((unsigned char*)icmp, icmplen);
		memcpy(out->buf + sizeof(struct etherhdr) + hlen, 
				icmp, icmplen);

		memcpy(&t, &ip->ip_dst, sizeof(struct in_addr));
		memcpy(&ip->ip_dst, &ip->ip_src, sizeof(struct in_addr));
		memcpy(&ip->ip_src, &t, sizeof(struct in_addr)); 

		ip->ip_ttl = MAXTTL;
		ip->ip_hl = sizeof(struct iphdr) >> 2; /* discard the options */
		ip->ip_len = htons(icmplen + sizeof(struct iphdr) );
		ip->ip_sum = 0;
		ip->ip_sum = in_chksum((unsigned char*)ip, sizeof(struct iphdr));
		memcpy(out->buf + sizeof(struct etherhdr), ip, sizeof(struct iphdr));

		memcpy(eh->eh_dhost, eh->eh_shost, 6);
		memcpy(eh->eh_shost, eth_mac_addr, 6);
		memcpy(out->buf, eh, sizeof(struct etherhdr) );

		out->len = icmplen + sizeof(struct iphdr) + sizeof( struct etherhdr);
		eth_xmit(out);

		break;

	case ICMP_ECHOREPLY:
		break;

	case ICMP_UNREACH:
		break;
	}

	return;

}

/* TFTP zone */
static void udp_input()
{
	struct etherhdr *eh;
	struct iphdr *ip;
	struct udphdr *uh;


	eh = (struct etherhdr*) (in.buf);
	ip = (struct iphdr *) (eh + 1);
	uh = (struct udphdr *) ( (unsigned char*)ip + (ip->ip_hl << 2));

	/* check-sum */

	/* Not for us? */
	if( ntohs(uh->uh_dport) != our_tftp_port ) return;

	/* ok, tftp handler */
	tftp_handler();
}


#define	RRQ	01				/* read request */
#define	WRQ	02				/* write request */
#define	DATA	03				/* data packet */
#define	ACK	04				/* acknowledgement */
#define	ERROR	05				/* error code */


static void do_tftp()
{
	struct mybuf* out;
	char *p , *th ;
	char mode[]="octet";
	int pktlen = 0;
	struct etherhdr *eh;
	struct iphdr *ip;
	struct udphdr *uh;

	/* in progress... */