net: Add net_update_ether() to handle ARP and Ping replies

When the network is VLAN or SNAP, net_update_ether() will preserve
the original Ethernet packet header and simply replace the src and
dest MACs and the protocol

Signed-off-by: Joe Hershberger <joe.hershberger@ni.com>
Acked-by: Simon Glass <sjg@chromium.org>

diff --git a/include/net.h b/include/net.h
index 09f040f33d66543095039ce5b924e1c6af4650bf..9564051c56e6f49e14a17bef92ffd55e3c515531 100644 (file)
--- a/include/net.h
+++ b/include/net.h
@@ -449,6 +449,7 @@ extern int  NetEthHdrSize(void);
 
 /* Set ethernet header; returns the size of the header */
 extern int NetSetEther(uchar *, uchar *, uint);
+extern int net_update_ether(struct ethernet_hdr *et, uchar *addr, uint prot);
 
 /* Set IP header */
 extern void net_set_ip_header(uchar *pkt, IPaddr_t dest, IPaddr_t source);

diff --git a/net/arp.c b/net/arp.c
index fcff53602f03e765ee79f0c7ccfb309638ffc3a8..8cc175a7d4393f5cdb5972acf6af81c10b9b4b16 100644 (file)
--- a/net/arp.c
+++ b/net/arp.c
@@ -158,7 +158,7 @@ void ArpReceive(struct ethernet_hdr *et, struct ip_udp_hdr *ip, int len)
                /* reply with our IP address */
                debug("Got ARP REQUEST, return our IP\n");
                pkt = (uchar *)et;
-               eth_hdr_size = NetSetEther(pkt, et->et_src, PROT_ARP);
+               eth_hdr_size = net_update_ether(et, et->et_src, PROT_ARP);
                pkt += eth_hdr_size;
                arp->ar_op = htons(ARPOP_REPLY);
                memcpy(&arp->ar_tha, &arp->ar_sha, ARP_HLEN);

diff --git a/net/net.c b/net/net.c
index 72fafd817aad6d07ecdb8ba0eb6475bb6b7882f9..651f3f73ef2df8ade28dfebafb28ee422c4fb5df 100644 (file)
--- a/net/net.c
+++ b/net/net.c
@@ -1276,6 +1276,29 @@ NetSetEther(uchar *xet, uchar * addr, uint prot)
        }
 }
 
+int net_update_ether(struct ethernet_hdr *et, uchar *addr, uint prot)
+{
+       ushort protlen;
+
+       memcpy(et->et_dest, addr, 6);
+       memcpy(et->et_src, NetOurEther, 6);
+       protlen = ntohs(et->et_protlen);
+       if (protlen == PROT_VLAN) {
+               struct vlan_ethernet_hdr *vet =
+                       (struct vlan_ethernet_hdr *)et;
+               vet->vet_type = htons(prot);
+               return VLAN_ETHER_HDR_SIZE;
+       } else if (protlen > 1514) {
+               et->et_protlen = htons(prot);
+               return ETHER_HDR_SIZE;
+       } else {
+               /* 802.2 + SNAP */
+               struct e802_hdr *et802 = (struct e802_hdr *)et;
+               et802->et_prot = htons(prot);
+               return E802_HDR_SIZE;
+       }
+}
+
 void net_set_ip_header(uchar *pkt, IPaddr_t dest, IPaddr_t source)
 {
        struct ip_udp_hdr *ip = (struct ip_udp_hdr *)pkt;

diff --git a/net/ping.c b/net/ping.c
index 9c690d8c85f1dbb0ff4e0f4a5af1fdb0de8e269a..71246de5ff87965a9474fc0565b31d73c28617d5 100644 (file)
--- a/net/ping.c
+++ b/net/ping.c
@@ -87,6 +87,7 @@ void ping_receive(struct ethernet_hdr *et, struct ip_udp_hdr *ip, int len)
 {
        struct icmp_hdr *icmph = (struct icmp_hdr *)&ip->udp_src;
        IPaddr_t src_ip;
+       int eth_hdr_size;
 
        switch (icmph->type) {
        case ICMP_ECHO_REPLY:
@@ -95,11 +96,10 @@ void ping_receive(struct ethernet_hdr *et, struct ip_udp_hdr *ip, int len)
                        net_set_state(NETLOOP_SUCCESS);
                return;
        case ICMP_ECHO_REQUEST:
-               debug("Got ICMP ECHO REQUEST, return "
-                       "%d bytes\n", ETHER_HDR_SIZE + len);
+               eth_hdr_size = net_update_ether(et, et->et_src, PROT_IP);
 
-               memcpy(&et->et_dest[0], &et->et_src[0], 6);
-               memcpy(&et->et_src[0], NetOurEther, 6);
+               debug("Got ICMP ECHO REQUEST, return "
+                       "%d bytes\n", eth_hdr_size + len);
 
                ip->ip_sum = 0;
                ip->ip_off = 0;
@@ -112,7 +112,7 @@ void ping_receive(struct ethernet_hdr *et, struct ip_udp_hdr *ip, int len)
                icmph->checksum = 0;
                icmph->checksum = ~NetCksum((uchar *)icmph,
                        (len - IP_HDR_SIZE) >> 1);
-               NetSendPacket((uchar *)et, ETHER_HDR_SIZE + len);
+               NetSendPacket((uchar *)et, eth_hdr_size + len);
                return;
 /*     default:
                return;*/