net.c

来自「LUBBOCK板的BLOB」· C语言 代码 · 共 741 行 · 第 1/2 页

	if(tftp_state) return ;
	else tftp_state = 1;

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

	eh = (struct etherhdr*) (out->buf);
	ip = (struct iphdr*) (eh + 1);
	uh = (struct udphdr*) (ip + 1);

	printf("downloading %s to 0x%8x\n", image_name, image_addr);

	/* reset */
	last_block_num = 0;
	our_tftp_port =0;


	/* tftp header */
	p = th = (unsigned char*)(uh+ 1);
	*(unsigned short *) p = htons(RRQ);
	p += 2;

	strcpy(p, image_name);
	p += strlen(image_name) + 1;

	strcpy(p, mode);
	p += sizeof(mode);

	pktlen = p - th;

	/* udp header */
	our_tftp_port = 1024 + ( TimerGetTime() % 3072);
	uh->uh_dport = htons(69);
	uh->uh_sport = htons(our_tftp_port);
	uh->uh_sum = 0;
	uh->uh_len = htons(pktlen + sizeof(struct udphdr) );

	/* ip header */
	ip->ip_v = IPVERSION;
	ip->ip_tos = 0; 
	ip->ip_hl = sizeof(struct iphdr) >> 2;
	ip->ip_id = htons(ip_id++);
	ip->ip_off = htons(0x4000); /* no frag */
	ip->ip_p = IPPROTO_UDP;
	ip->ip_len = htons( pktlen + sizeof(struct udphdr) + sizeof(struct iphdr));
	ip->ip_ttl = MAXTTL;
	memcpy(&ip->ip_dst, svr_ip_addr,sizeof(struct in_addr));
	memcpy(&ip->ip_src, ip_addr, sizeof(struct in_addr));
	ip->ip_sum = 0;
	ip->ip_sum = in_chksum((unsigned char*)ip, sizeof(struct iphdr));

	/* ether header */
	memcpy(eh->eh_shost, eth_mac_addr, ETH_ALEN);
	memcpy(eh->eh_dhost, svr_mac_addr, ETH_ALEN);
	eh->eh_proto = htons(ETHPROTO_IP);

	out->len = pktlen + sizeof(struct udphdr) + sizeof(struct iphdr)+ sizeof(struct etherhdr);

	/* xmit */
	eth_xmit(out); 
}

static void tftp_handler()
{
	unsigned char *p, *th;
	unsigned short opcode, blocknum, expected;
	int pktlen, datalen;
	struct etherhdr *eh, *ehout;
	struct iphdr *ip, *ipout;
	struct udphdr *uh, *uhout;

	struct mybuf* out;

	/* Not in tftp session */
	if(!tftp_state) return;

	/* reset timeout count */
	tftp_timeout = 0;
	
	out = bget(1);
	if(!out) return;
	
	eh = (struct etherhdr*) (in.buf);
	ip = (struct iphdr*) (eh + 1);
	uh = (struct udphdr*) ( (unsigned char*) ip + (ip->ip_hl << 2) );

	ehout = (struct etherhdr*)(out->buf);
	ipout = (struct iphdr*) ( ehout + 1);
	uhout = (struct udphdr*) (ipout + 1);

	p = th = (unsigned char*)uh + sizeof(struct udphdr);

	pktlen = ntohs(uh->uh_len) - sizeof(struct udphdr) ;


	if( pktlen < 2) return;

	opcode = ntohs(*(unsigned short*)p);
	p += 2;
	switch(opcode) {
	case RRQ:
	case WRQ:
		/* we are client ignore */
		break;
	case DATA:
		datalen = pktlen - 4;
		blocknum = ntohs(*(unsigned short*)p);
		p += 2;

		expected = (last_block_num + 1) % (2<<16);

		if ( last_block_num == blocknum ) {
			DBPRINT("same block\n");
		} 
		else if ( blocknum == expected )  {
			last_block_num = blocknum;
			/* store data */
			memcpy((unsigned char*)image_addr, p, datalen);
			image_addr += datalen; 
		}
		else {
			DBPRINT("TFTP : Expected block %d, got block %d\n", expected , blocknum);
			/* ASK FOR IT */
			blocknum = last_block_num;
		}

		th = (unsigned char*) (uhout + 1);
		*(unsigned short*) th = htons(ACK);
		*((unsigned short*) th + 1) = htons(blocknum);

		uhout->uh_dport = uh->uh_sport;
		uhout->uh_sport = htons(our_tftp_port);
		uhout->uh_len = htons(4 + sizeof(struct udphdr));
		uhout->uh_sum = 0;

		ipout->ip_v = IPVERSION;
		ipout->ip_tos = 0; 
		ipout->ip_hl = sizeof(struct iphdr) >> 2;
		ipout->ip_id = htons(ip_id++);
		ipout->ip_off = htons(0x4000); /* no frag */
		ipout->ip_p = IPPROTO_UDP;
		ipout->ip_len = htons( 4 + sizeof(struct udphdr) + sizeof(struct iphdr));
		ipout->ip_ttl = MAXTTL;
		memcpy(&ipout->ip_dst, svr_ip_addr,sizeof(struct in_addr));
		memcpy(&ipout->ip_src, ip_addr, sizeof(struct in_addr));
		ipout->ip_sum = 0;
		ipout->ip_sum = in_chksum((unsigned char*)ipout, sizeof(struct iphdr));

		memcpy(ehout->eh_shost, eth_mac_addr, ETH_ALEN);
		memcpy(ehout->eh_dhost, svr_mac_addr, ETH_ALEN);
		ehout->eh_proto = htons(ETHPROTO_IP);

		out->len = sizeof(struct iphdr) + sizeof(struct etherhdr) + sizeof(struct udphdr) + 4;
		/* ack it */
		eth_xmit(out);

		if (datalen < 512) {
			/* reset tftp state */
			tftp_state++;
			printf("done\n");
		}
		else {
			//printf(".");
			//if( 0 == (blocknum % 80) ) printf("\n");
		}
		break;
	case ERROR:
		tftp_state ++;
		printf("tftp error, aborted\n");

		break;
	}
}

void net_reset()
{
	out1.len = 0;
	out2.len = 0;
	in.len = 0;
}

void net_rx() 
{
	struct etherhdr *eh;
	int proto;

	eh = (struct etherhdr*)in.buf;

	proto = ntohs(eh->eh_proto);

	switch(proto) {
	case ETHPROTO_IP:
		ip_input();
		break;
	case ETHPROTO_ARP:
		arp_input() ;
		break;
	default:
		DBPRINT("Unsupported packet : 0x%x\n", proto);
		break;
	}

}

int cmd_tftp(int argc, char* argv[])
{
	int try = 3;
	unsigned long timeout = TICKS_PER_SECOND;

	if(argc < 3) {
		printf("usage:\n \ttftp file-name ram-addr\n");
		return 0;
	}

	strcpy(image_name, argv[1]);
	strtou32(argv[2], (u32*)&image_addr);

	/* reset count */
	arp_state = 0;
	tftp_state = 0;
	tftp_timeout = 0;

	while(!arp_state) {
		timeout = TICKS_PER_SECOND/10;
		arp_request();
		while(timeout && !arp_state) {
			eth_rx();
			timeout -- ;
		}

		if(arp_state) break;
		if( !(try--) ) {
			printf("TFTP : ARP timeout.\n");
			break;
		}
	}

	while(tftp_state < 2) {
		tftp_timeout ++ ;
		if(tftp_timeout > TICKS_PER_SECOND) {
			printf("TFTP : transfer timeout\n");
			break;
		}

		eth_rx();
	}

	/* reset count */
	arp_state = 0;
	tftp_state = 0;
	tftp_timeout = 0;

	return 0;
}

static char flashtftphelp[] = "tftp file-name ram-addr\n" ;
__commandlist( cmd_tftp, "tftp", flashtftphelp );


static int str2mac(unsigned char* str, unsigned char* mac)
{
	int i=0;
	unsigned char *p=str;
	unsigned char t[6];
	int addr;
	
	while(i < 6) {
		addr = 0;
		while( *p && (*p != ':') ) {
			if( (*p >= '0') && (*p <= '9') ) {
				addr = addr * 16 + (*p - '0');
				p++;
			}
			else if( (*p >= 'a') && (*p <= 'f') ) {
				addr = addr * 16 + (*p - 'a');
				p++;
			}
			else if ( (*p >= 'A') && (*p <= 'F') ) {
				addr = addr * 16 + (*p - 'A');
			       	p++;
			}
			else return -1; /* invalid */
		}

		if (addr < 255) t[i] = (addr&0xFF);
		else break;

		i++;

		if(*p) p++;
		else break;
	}

	if( i!=6 )  return -1;

	memcpy(mac, t, sizeof(t));

	return 0;
}

static int str2ip(unsigned char* str, unsigned char* ip)
{
	int i=0;
	unsigned char *p=str;
	unsigned char t[4];
	int addr;
	
	while(i < 4) {
		addr = 0;
		while( *p && (*p != '.') ) {
			if( (*p >= '0') && (*p <= '9') ) {
				addr = addr * 10 + (*p - '0');
			       	p++;
			}
			else return -1; /* invalid */
		}

		if (addr < 255) t[i] = (addr&0xFF);
		else break;

		i++;

		if(*p) p++;
		else break;

	}

	if( i!=4 )  return -1;

	memcpy(ip, t, sizeof(t));

	return 0;
}

static int cmd_ifconfig(int argc, char* argv[])
{
	if(argc == 1) {
		printf("Mac addr\t: %02x.%02x.%02x.%02x.%02x.%02x\n",
			eth_mac_addr[0],eth_mac_addr[1],eth_mac_addr[2],
			eth_mac_addr[3],eth_mac_addr[4],eth_mac_addr[5]);
		printf("Our IP addr\t: %d.%d.%d.%d\n", ip_addr[0], ip_addr[1], ip_addr[2], ip_addr[3]);
		printf("Server IP addr\t: %d.%d.%d.%d\n", svr_ip_addr[0],
				svr_ip_addr[1], svr_ip_addr[2], svr_ip_addr[3]);
		return 0;

	}else if(argc < 3) {
		printf("usage:\n \t ifconfig [ip|mac|server] addr\n");
		return 0;
	}

	if(!strncmp(argv[1], "ip", 6)) {
		return str2ip(argv[2],ip_addr);
	}

	if(!strncmp(argv[1],"mac", 6)) {
		return str2mac(argv[2],eth_mac_addr);
	}

	if(!strncmp(argv[1],"server", 6)) {
		return str2ip(argv[2],svr_ip_addr);

	}
	return 0;

}

static char flashifconfighelp[] = "ifconfig [ip|mac|server] addr\n" ;
__commandlist( cmd_ifconfig, "ifconfig", flashifconfighelp );