NetReceive函数－－精简的网络数据包解析代码，支持802，VLAN

void NetReceive(volatile uchar * inpkt, int len) { Ethernet_t *et; IP_t *ip; ARP_t *arp; IPaddr_t tmp; int x; uchar *pkt; #if defined(CONFIG_CMD_CDP) int iscdp; #endif ushort cti = 0, vlanid = VLAN_NONE, myvlanid, mynvlanid; debug("packet received/n"); NetRxPacket = inpkt; NetRxPacketLen = len; et = (Ethernet_t *)inpkt; /* too small packet? */ if (len < ETHER_HDR_SIZE) return; #ifdef CONFIG_API if (push_packet) { (*push_packet)(inpkt, len); return; } #endif #if defined(CONFIG_CMD_CDP) /* keep track if packet is CDP */ iscdp = memcmp(et->et_dest, NetCDPAddr, 6) == 0; #endif myvlanid = ntohs(NetOurVLAN); if (myvlanid == (ushort)-1) myvlanid = VLAN_NONE; mynvlanid = ntohs(NetOurNativeVLAN); if (mynvlanid == (ushort)-1) mynvlanid = VLAN_NONE; x = ntohs(et->et_protlen); debug("packet received/n"); //x 可以是VLAN标志，或是802包的长度，或是包类型 if (x < 1514) { /* * Got a 802 packet. Check the other protocol field. */ x = ntohs(et->et_prot); ip = (IP_t *)(inpkt + E802_HDR_SIZE); len -= E802_HDR_SIZE; } else if (x != PROT_VLAN) { /* normal packet */ ip = (IP_t *)(inpkt + ETHER_HDR_SIZE); len -= ETHER_HDR_SIZE; } else { /* VLAN packet */ VLAN_Ethernet_t *vet = (VLAN_Ethernet_t *)et; debug("VLAN packet received/n"); /* too small packet? */ if (len < VLAN_ETHER_HDR_SIZE) return; /* if no VLAN active */ if ((ntohs(NetOurVLAN) & VLAN_IDMASK) == VLAN_NONE #if defined(CONFIG_CMD_CDP) && iscdp == 0 #endif ) return; cti = ntohs(vet->vet_tag); vlanid = cti & VLAN_IDMASK; x = ntohs(vet->vet_type); ip = (IP_t *)(inpkt + VLAN_ETHER_HDR_SIZE); len -= VLAN_ETHER_HDR_SIZE; } debug("Receive from protocol 0x%x/n", x); #if defined(CONFIG_CMD_CDP) if (iscdp) { CDPHandler((uchar *)ip, len); return; } #endif if ((myvlanid & VLAN_IDMASK) != VLAN_NONE) { if (vlanid == VLAN_NONE) vlanid = (mynvlanid & VLAN_IDMASK); /* not matched? */ if (vlanid != (myvlanid & VLAN_IDMASK)) return; } switch (x) { case PROT_ARP: /* * We have to deal with two types of ARP packets: * - REQUEST packets will be answered by sending our * IP address - if we know it. * - REPLY packates are expected only after we asked * for the TFTP server's or the gateway's ethernet * address; so if we receive such a packet, we set * the server ethernet address */ debug("Got ARP/n"); arp = (ARP_t *)ip; if (len < ARP_HDR_SIZE) { printf("bad length %d < %d/n", len, ARP_HDR_SIZE); return; } if (ntohs(arp->ar_hrd) != ARP_ETHER) { return; } if (ntohs(arp->ar_pro) != PROT_IP) { return; } if (arp->ar_hln != 6) { return; } if (arp->ar_pln != 4) { return; } if (NetOurIP == 0) { return; } if (NetReadIP(&arp->ar_data[16]) != NetOurIP) { return; } switch (ntohs(arp->ar_op)) { case ARPOP_REQUEST: /* reply with our IP address */ debug("Got ARP REQUEST, return our IP/n"); pkt = (uchar *)et; pkt += NetSetEther(pkt, et->et_src, PROT_ARP); arp->ar_op = htons(ARPOP_REPLY); memcpy (&arp->ar_data[10], &arp->ar_data[0], 6); NetCopyIP(&arp->ar_data[16], &arp->ar_data[6]); memcpy (&arp->ar_data[ 0], NetOurEther, 6); NetCopyIP(&arp->ar_data[ 6], &NetOurIP); (void) eth_send((uchar *)et, (pkt - (uchar *)et) + ARP_HDR_SIZE); return; case ARPOP_REPLY: /* arp reply */ /* are we waiting for a reply */ if (!NetArpWaitPacketIP || !NetArpWaitPacketMAC) break; #ifdef CONFIG_KEEP_SERVERADDR if (NetServerIP == NetArpWaitPacketIP) { char buf[20]; sprintf(buf, "%pM", arp->ar_data); setenv("serveraddr", buf); } #endif debug("Got ARP REPLY, set server/gtwy eth addr (%pM)/n", arp->ar_data); tmp = NetReadIP(&arp->ar_data[6]); /* matched waiting packet's address */ if (tmp == NetArpWaitReplyIP) { debug("Got it/n"); /* save address for later use */ memcpy(NetArpWaitPacketMAC, &arp->ar_data[0], 6); #ifdef CONFIG_NETCONSOLE (*packetHandler)(0,0,0,0); #endif /* modify header, and transmit it */ memcpy(((Ethernet_t *)NetArpWaitTxPacket)->et_dest, NetArpWaitPacketMAC, 6); (void) eth_send(NetArpWaitTxPacket, NetArpWaitTxPacketSize); /* no arp request pending now */ NetArpWaitPacketIP = 0; NetArpWaitTxPacketSize = 0; NetArpWaitPacketMAC = NULL; } return; default: debug("Unexpected ARP opcode 0x%x/n", ntohs(arp->ar_op)); return; } break; case PROT_RARP: debug("Got RARP/n"); arp = (ARP_t *)ip; if (len < ARP_HDR_SIZE) { printf("bad length %d < %d/n", len, ARP_HDR_SIZE); return; } if ((ntohs(arp->ar_op) != RARPOP_REPLY) || (ntohs(arp->ar_hrd) != ARP_ETHER) || (ntohs(arp->ar_pro) != PROT_IP) || (arp->ar_hln != 6) || (arp->ar_pln != 4)) { puts ("invalid RARP header/n"); } else { NetCopyIP(&NetOurIP, &arp->ar_data[16]); if (NetServerIP == 0) NetCopyIP(&NetServerIP, &arp->ar_data[ 6]); memcpy (NetServerEther, &arp->ar_data[ 0], 6); (*packetHandler)(0,0,0,0); } break; case PROT_IP: debug("Got IP/n"); /* Before we start poking the header, make sure it is there */ if (len < IP_HDR_SIZE) { debug("len bad %d < %lu/n", len, (ulong)IP_HDR_SIZE); return; } /* Check the packet length */ if (len < ntohs(ip->ip_len)) { printf("len bad %d < %d/n", len, ntohs(ip->ip_len)); return; } len = ntohs(ip->ip_len); debug("len=%d, v=%02x/n", len, ip->ip_hl_v & 0xff); /* Can't deal with anything except IPv4 */ if ((ip->ip_hl_v & 0xf0) != 0x40) { return; } /* Can't deal with IP options (headers != 20 bytes) */ if ((ip->ip_hl_v & 0x0f) > 0x05) { return; } /* Check the Checksum of the header */ if (!NetCksumOk((uchar *)ip, IP_HDR_SIZE_NO_UDP / 2)) { puts ("checksum bad/n"); return; } /* If it is not for us, ignore it */ tmp = NetReadIP(&ip->ip_dst); if (NetOurIP && tmp != NetOurIP && tmp != 0xFFFFFFFF) { #ifdef CONFIG_MCAST_TFTP if (Mcast_addr != tmp) #endif return; } /* * The function returns the unchanged packet if it's not * a fragment, and either the complete packet or NULL if * it is a fragment (if !CONFIG_IP_DEFRAG, it returns NULL) */ if (!(ip = NetDefragment(ip, &len))) return; /* * watch for ICMP host redirects * * There is no real handler code (yet). We just watch * for ICMP host redirect messages. In case anybody * sees these messages: please contact me * ([email protected]), or - even better - send me the * necessary fixes :-) * * Note: in all cases where I have seen this so far * it was a problem with the router configuration, * for instance when a router was configured in the * BOOTP reply, but the TFTP server was on the same * subnet. So this is probably a warning that your * configuration might be wrong. But I'm not really * sure if there aren't any other situations. */ if (ip->ip_p == IPPROTO_ICMP) { ICMP_t *icmph = (ICMP_t *)&(ip->udp_src); switch (icmph->type) { case ICMP_REDIRECT: if (icmph->code != ICMP_REDIR_HOST) return; printf (" ICMP Host Redirect to %pI4 ", &icmph->un.gateway); return; #if defined(CONFIG_CMD_PING) case ICMP_ECHO_REPLY: /* * IP header OK. Pass the packet to the current handler. */ /* XXX point to ip packet */ (*packetHandler)((uchar *)ip, 0, 0, 0); return; case ICMP_ECHO_REQUEST: debug("Got ICMP ECHO REQUEST, return %d bytes /n", ETHER_HDR_SIZE + len); memcpy (&et->et_dest[0], &et->et_src[0], 6); memcpy (&et->et_src[ 0], NetOurEther, 6); ip->ip_sum = 0; ip->ip_off = 0; NetCopyIP((void*)&ip->ip_dst, &ip->ip_src); NetCopyIP((void*)&ip->ip_src, &NetOurIP); ip->ip_sum = ~NetCksum((uchar *)ip, IP_HDR_SIZE_NO_UDP >> 1); icmph->type = ICMP_ECHO_REPLY; icmph->checksum = 0; icmph->checksum = ~NetCksum((uchar *)icmph, (len - IP_HDR_SIZE_NO_UDP) >> 1); (void) eth_send((uchar *)et, ETHER_HDR_SIZE + len); return; #endif default: return; } } else if (ip->ip_p != IPPROTO_UDP) { /* Only UDP packets */ return; } #ifdef CONFIG_UDP_CHECKSUM if (ip->udp_xsum != 0) { ulong xsum; ushort *sumptr; ushort sumlen; xsum = ip->ip_p; xsum += (ntohs(ip->udp_len)); xsum += (ntohl(ip->ip_src) >> 16) & 0x0000ffff; xsum += (ntohl(ip->ip_src) >> 0) & 0x0000ffff; xsum += (ntohl(ip->ip_dst) >> 16) & 0x0000ffff; xsum += (ntohl(ip->ip_dst) >> 0) & 0x0000ffff; sumlen = ntohs(ip->udp_len); sumptr = (ushort *) &(ip->udp_src); while (sumlen > 1) { ushort sumdata; sumdata = *sumptr++; xsum += ntohs(sumdata); sumlen -= 2; } if (sumlen > 0) { ushort sumdata; sumdata = *(unsigned char *) sumptr; sumdata = (sumdata << 8) & 0xff00; xsum += sumdata; } while ((xsum >> 16) != 0) { xsum = (xsum & 0x0000ffff) + ((xsum >> 16) & 0x0000ffff); } if ((xsum != 0x00000000) && (xsum != 0x0000ffff)) { printf(" UDP wrong checksum %08lx %08x/n", xsum, ntohs(ip->udp_xsum)); return; } } #endif #ifdef CONFIG_NETCONSOLE nc_input_packet((uchar *)ip +IP_HDR_SIZE, ntohs(ip->udp_dst), ntohs(ip->udp_src), ntohs(ip->udp_len) - 8); #endif /* * IP header OK. Pass the packet to the current handler. */ (*packetHandler)((uchar *)ip +IP_HDR_SIZE, ntohs(ip->udp_dst), ntohs(ip->udp_src), ntohs(ip->udp_len) - 8); break; } }