net_ipv4.c

bootloader源代码

				ipaddr_to_str(g_arptable[i].ipaddr), 
				cp[0], cp[1], cp[2], cp[3], cp[4], cp[5]);
		}
	}
}

// 
// ARP protocol
//
// Refer to RFC 826 "An Ethernet Address Resolution Protocol"
// 

// send ARP packet : request or reply
// opcode : ARPOP_REQUEST or ARPOP_REPLY
// ipaddr : address to reply
// node : hardware address to reply
int arp_transmit(int opcode, in_addr_t ipaddr, unsigned char *node)
{
	arprequest_t arpreq;

	// fill ARP header 
	arpreq.ar_hrd = htons(ARPHRD_ETHER);
	arpreq.ar_pro = htons(ETH_P_IP);
	arpreq.ar_hln = ETH_ALEN;
	arpreq.ar_pln = 4;
	arpreq.ar_op = htons(opcode);

	// fill ARP body
	memcpy(arpreq.ar_sha, g_arptable[ARP_CLIENT].node, ETH_ALEN);
	arpreq.ar_sip = htonl(g_arptable[ARP_CLIENT].ipaddr);
	
	if (node)
		memcpy(arpreq.ar_tha, node, ETH_ALEN);
	else
		memset(arpreq.ar_tha, 0, ETH_ALEN);
	arpreq.ar_tip = htonl(ipaddr);

	// send ARP request (broadcast) or reply packet
	if (opcode == ARPOP_REQUEST)
		return eth_transmit(g_eth_broadcast, ETH_P_ARP, sizeof(arpreq), &arpreq);
	else
		return eth_transmit(node, ETH_P_ARP, sizeof(arpreq), &arpreq);
}

arptable_t *arp_dorequest(unsigned long ipaddr)
{
	int retry;
	arptable_t *arp = arp_lookup(ipaddr);
	struct sk_buff *skb;

	// look for available ARP slot
	if (arp == NULL) 
		if ((arp = arp_alloc()) == NULL)
			return NULL;

	// send ARP request packet and wait for reply
	for (retry = 1; retry <= MAX_ARP_RETRIES; retry++) {
		arp_transmit(ARPOP_REQUEST, ipaddr, NULL);
		skb = eth_receive(WAIT_ARP_REPLY, TIMEOUT);

		if (skb) {
			if (skb->arp->ar_op == ARPOP_REPLY && skb->arp->ar_sip == ipaddr) {
				arp_setaddr(arp, ipaddr, INADDR_ANY, skb->arp->ar_sha);
				skb_free(skb);
				break;
			}
		} else if (retry < MAX_ARP_RETRIES)
			PrintUart("ARP request timeout. Retry.\r\n", -1);
		else {
			PrintUart("ARP request failed.\r\n", -1);
			return NULL;
		}
	}
	
	return arp;
}

//
// send a IP packet
//
// Refer to RFC 791 "Internet Protocol" about IP header
//

int ip_transmit(in_addr_t destip, int protocol, int len, const void *buf)
{
	struct iphdr *ip;

	len += sizeof(struct iphdr);

	// fill in the IP header
	ip = (struct iphdr *) buf;
	ip->version = 0x04;
	ip->ihl = 0x05;
	ip->tos = 0;
	ip->tot_len = htons(len);
	ip->id = 0;
	ip->frag_off = 0;
	ip->ttl = 60;
	ip->protocol = (unsigned char) protocol;
	ip->check = 0;
	ip->saddr = htonl(g_arptable[ARP_CLIENT].ipaddr);
	ip->daddr = htonl(destip);
	ip->check = ipv4_calc_checksum(ip, sizeof(struct iphdr));

	// transmit the packet
	if (destip == INADDR_BROADCAST) {
		eth_transmit(g_eth_broadcast, ETH_P_IP, len, buf);
	} else {
		arptable_t *arp;
		in_addr_t netmask = g_arptable[ARP_CLIENT].netmask;

		if (ipv4_ipaddr_valid(g_arptable[ARP_CLIENT].ipaddr)) {
			if ((destip & netmask) != (g_arptable[ARP_CLIENT].ipaddr & netmask)) {
				// not in the same class
				// send packet over gateway
				if (!ipv4_ipaddr_valid(g_arptable[ARP_GATEWAY].ipaddr)) {
					// now the gateway is not specified
					return 1;
				}
				destip = g_arptable[ARP_GATEWAY].ipaddr;
			}
		}

		if ((arp = arp_lookup(destip)) == NULL) 
			if ((arp = arp_dorequest(destip)) == NULL) 
				return 1;

		eth_transmit(arp->node, ETH_P_IP, len, buf);
	}

	return 0;
}

unsigned int ipv4_calc_sum(const void *ip, int len)
{
	uint16_t *_ip = (uint16_t *) ip;
	unsigned int sum = 0;
	
	for (len >>= 1; len > 0; --len) 
		sum += *(_ip++);

	return sum;
}

unsigned short ipv4_calc_checksum(const void *ip, int len)
{
	unsigned int sum = ipv4_calc_sum(ip, len);
	
	while (sum >> 16)
		sum = (sum & 0xffff) + (sum >> 16);

	return ((~sum) & 0x0000FFFF);
}

//
// send a ping ICMP packet
// 

int icmp_ping(in_addr_t destip, int count)
{
	static unsigned short s_id = 0, s_seq = 0;
	static unsigned char s_pingmsg[] = "FLYDUCK, SIGMADESIGNS";

	int i, nreply;
	icmpip_t packet;
	struct sk_buff *skb;

	for (i = 0, nreply = 0; i < count; ++i) {
		packet.icmp.type = ICMP_ECHO;
		packet.icmp.code = 0;
		packet.icmp.checksum = 0;
		packet.icmp.un.echo.id = htons(++s_id);
		packet.icmp.un.echo.sequence = htons(++s_seq);

		memset(packet.icmp.data, ' ', MAX_ICMP_DATA);
		memcpy(packet.icmp.data, s_pingmsg, sizeof s_pingmsg);
	
		packet.icmp.checksum = ipv4_calc_checksum(&packet.icmp, sizeof packet.icmp);

		if (ip_transmit(destip, IPPROTO_ICMP, sizeof packet.icmp, &packet) != 0)
			break;

		if ((skb = eth_receive(WAIT_ICMP_REPLY, TIMEOUT)) != NULL) {
			PrintFormat("Reply from %s : icmp_seq = %d\n", 
				ipaddr_to_str(skb->ip->saddr), skb->icmp->un.echo.sequence);
			++nreply;
		} else {
			PrintFormat("Unreachable to %s : icmp_seq = %d\n",
				ipaddr_to_str(destip), s_seq);
		}

		skb_free(skb);
	}

	return nreply;
}

//
// send a UDP datagram
//
// destip : destination IP address
// srcsock : source port number
// destsock : destination socket number
// len : size of UDP data (exept IP, UDP header)
// buf : packet buffer
// 
// buffer is consisted of 
//   struct iphdr ip;
//   struct udphdr udp;
//   protocol specific body;
//
// return 0 : success
// return non-zero : error
//
// Refer to RFC 768 "User Datagram Protocol" about header and checksum
//

int udp_transmit(in_addr_t destip, unsigned int srcsock, unsigned int destsock, int len, const void *buf)
{
	struct udphdr *udp;

	len += sizeof(struct udphdr);

	// fill in the UDP header
	udp = (struct udphdr *)((char *)buf + sizeof(struct iphdr));
	udp->src = htons(srcsock);
	udp->dest = htons(destsock);
	udp->len = htons(len);
	udp->chksum = 0;

	udp->chksum = ipv4_calc_tcpudp_checksum(udp, len, g_arptable[ARP_CLIENT].ipaddr, destip);
	
	return ip_transmit(destip, IPPROTO_UDP, len, buf);
}

unsigned short ipv4_calc_tcpudp_checksum(const void *data, int len, in_addr_t src, in_addr_t dest)
{
	unsigned int sum;
	tcpudp_pseudohdr_t phdr;

	phdr.src = htonl(src);
	phdr.dest = htonl(dest);
	phdr.zero = 0;
	phdr.protocol = IPPROTO_UDP;
	phdr.length = htons(len);

	sum = ipv4_calc_sum(data, len);
	sum += ipv4_calc_sum(&phdr, sizeof phdr);

	while (sum >> 16)
		sum = (sum & 0xffff) + (sum >> 16);
		
	return ((~sum) & 0x0000FFFF);
}

struct sk_buff *udp_receive(in_addr_t fromip, unsigned int srcsock, unsigned int destsock, int timeout)
{
	struct sk_buff *skb;
	unsigned int startticks = timer_getticks();
   
	while ((skb = eth_receive(WAIT_UDP, timeout)) != NULL) {
		int match = 1;

		// PrintFormat("from = %08x, %08x, src = %04x %04x, des = %04x %04x\n",
		// 	fromip, skb->ip->saddr, srcsock, skb->udp->src, destsock, skb->udp->dest);
		
		if (fromip) 
			if (skb->ip->saddr != fromip)
				match = 0;
		if (srcsock)
			if (skb->udp->src != srcsock)
				match = 0;
		if (destsock)
			if (skb->udp->dest != destsock)
				match = 0;

		if (match)
			return skb;

		skb_free(skb);

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

	return NULL;
}

//
// Miscellaneous
//

enum {
	DUMP_PACKET_DEFAULT,
	DUMP_PACKET_BOOTP,
	DUMP_PACKET_TFTP,
};

void dump_memory(void *memptr, unsigned int addr, int num, int unit);

static char *ipv4_ethproto_str(int proto)
{
	switch (proto) {
	case ETH_P_LOOP : 
		return "LOOP";
	case ETH_P_IP : 
		return "IP";
	case ETH_P_ARP : 
		return "ARP";
	case ETH_P_RARP :
		return "RARP";
	default : 
		return "Unknown";
	}
}

static char *ipv4_ipproto_str(int proto)
{
	switch (proto) {
	case IPPROTO_IP :
		return "IP";
	case IPPROTO_ICMP :
		return "ICMP";
	case IPPROTO_IGMP :
		return "IGMP";
	case IPPROTO_TCP :
		return "TCP";
	case IPPROTO_UDP :
		return "UDP";
	case IPPROTO_RAW :
		return "RAW";
	default :
		return "Other";
	}
}

static char *ipv4_ipport_str(int port)
{
	static struct {
		int port;
		char *name;
	} s_ipport_str_list[] = {
		{ IPPORT_ECHO, "echo" },
		{ IPPORT_FTP, "ftp" },
		{ IPPORT_TELNET, "telnet" },
		{ IPPORT_SMTP, "smtp" },
		{ IPPORT_NAMESERVER, "nameserver" },
		{ IPPORT_BOOTP_SERVER, "bootp server" },
		{ IPPORT_BOOTP_CLIENT, "bootp client" },
		{ IPPORT_TFTP, "tftp" },
		{ IPPORT_SUNRPC, "RPC" },
		{ 0, "Other" },
	};

	int i;

	for (i = 0; s_ipport_str_list[i].port != 0 && s_ipport_str_list[i].port != port; ++i)
		;

	return s_ipport_str_list[i].name;
}

void ipv4_dump_packet(struct sk_buff *skb, int type, int need_parsing)
{
	int len;

	if (need_parsing)
		ipv4_parsepacket(skb);

	PrintFormat("Ethernet : %d (0x%x)\n", skb->len, skb->len);
	PrintFormat("  Target   : %02X:%02X:%02X:%02X:%02X:%02X\n",
		skb->eth->h_dest[0], skb->eth->h_dest[1], 
		skb->eth->h_dest[2], skb->eth->h_dest[3], 
		skb->eth->h_dest[4], skb->eth->h_dest[5]);
	PrintFormat("  Source   : %02X:%02X:%02X:%02X:%02X:%02X\n",
		skb->eth->h_source[0], skb->eth->h_source[1], 
		skb->eth->h_source[2], skb->eth->h_source[3], 
		skb->eth->h_source[4], skb->eth->h_source[5]);
	PrintFormat("  Protocol : %04x (%s)\n", 
		skb->eth->h_proto,
		ipv4_ethproto_str(skb->eth->h_proto));
	PrintFormat("  data len : %x (%d)\n", skb->ethdata_len, skb->ethdata_len);

	switch (skb->eth->h_proto) {
	case ETH_P_IP : 
		PrintFormat("IP :\n");
		PrintFormat("  Version + IHL : %02x\n", skb->ethdata[0]);
		PrintFormat("  TOS           : %02x\n", skb->ip->tos);
		PrintFormat("  Length        : %04x\n", skb->ip->tot_len);
		PrintFormat("  ID            : %04x\n", skb->ip->id);
		PrintFormat("  Frag          : %04x\n", skb->ip->frag_off);
		PrintFormat("  TTL           : %02x\n", skb->ip->ttl);
		PrintFormat("  Protocol      : %02x (%s)\n", skb->ip->protocol, ipv4_ipproto_str(skb->ip->protocol));
		PrintFormat("  Checksum      : %04x\n", skb->ip->check);
		PrintFormat("  Source        : %s (%08x)\n", ipaddr_to_str(skb->ip->saddr), skb->ip->saddr);
		PrintFormat("  Target        : %s (%08x)\n", ipaddr_to_str(skb->ip->daddr), skb->ip->daddr);
		PrintFormat("  data len      : %x (%d)\n", skb->ipdata_len, skb->ipdata_len);

		switch (skb->ip->protocol) {
		case IPPROTO_IP : 
			PrintFormat("Raw IP packet\n");
			break;
		case IPPROTO_ICMP :
			PrintFormat("ICMP :\n");
			PrintFormat("  Type : %02x\n", skb->icmp->type);
			PrintFormat("  Code : %02x\n", skb->icmp->code);
			PrintFormat("  Checksum : %04x\n", skb->icmp->checksum);
			len = skb->ipdata_len - 4;
			PrintFormat("  data len : %x (%d)\n", len, len);
			PrintFormat("ICMP data :\n");
			dump_memory(skb->ipdata, 0, len, 1);
			break;
		case IPPROTO_UDP :
			PrintFormat("UDP :\n");
			PrintFormat("  Source   : %04x (%d) (%s)\n", skb->udp->src, skb->udp->src, ipv4_ipport_str(skb->udp->src));
			PrintFormat("  Destin   : %04x (%d) (%s)\n", skb->udp->dest, skb->udp->dest, ipv4_ipport_str(skb->udp->dest));
			PrintFormat("  Length   : %04x\n", skb->udp->len);
			PrintFormat("  Checksum : %04x\n", skb->udp->chksum);
			PrintFormat("  data len : %x (%d)\n", skb->udpdata_len, skb->udpdata_len);

			switch (type) {
			case DUMP_PACKET_DEFAULT :
				PrintFormat("UDP data :\n");
				dump_memory(skb->udpdata, 0, skb->udpdata_len, 1);
				break;
			case DUMP_PACKET_BOOTP :
				bootp_dump_packet(skb, need_parsing);
				break;
			case DUMP_PACKET_TFTP :
				tftp_dump_packet(skb, need_parsing);
				break;
			}
			break;
		}	
		break;

	case ETH_P_ARP :
		PrintFormat("ARP :\n");
		PrintFormat("  HWADDR type   : %04x\n", skb->arp->ar_hrd);
		PrintFormat("  Protocol type : %04x\n", skb->arp->ar_pro);
		PrintFormat("  HWADDR len    : %02x\n", skb->arp->ar_hln);
		PrintFormat("  Address len   : %02x\n", skb->arp->ar_pln);
		PrintFormat("  Opcode        : %04x\n", skb->arp->ar_op);
		PrintFormat("  Sender HWADDR : %02X:%02X:%02X:%02X:%02X:%02X\n",
			skb->arp->ar_sha[0], skb->arp->ar_sha[1], 
			skb->arp->ar_sha[2], skb->arp->ar_sha[3], 
			skb->arp->ar_sha[4], skb->arp->ar_sha[5]);
		PrintFormat("  Sender IPADDR : %s (%08x)\n", ipaddr_to_str(skb->arp->ar_sip), skb->arp->ar_sip);
		PrintFormat("  Target HWADDR : %02X:%02X:%02X:%02X:%02X:%02X\n",
			skb->arp->ar_tha[0], skb->arp->ar_tha[1], 
			skb->arp->ar_tha[2], skb->arp->ar_tha[3], 
			skb->arp->ar_tha[4], skb->arp->ar_tha[5]);
		PrintFormat("  Target IPADDR : %s (%08x)\n", ipaddr_to_str(skb->arp->ar_tip), skb->arp->ar_tip);
		break;

	case ETH_P_RARP :
		break;
	}
}