[U-Boot] [PATCH v2 10/18] net: Add NetUpdateEther() to handle ARP and Ping replies

[Joe Hershberger]

When the network is VLAN or SNAP, NetUpdateEther() 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 at ni.com>
Cc: Joe Hershberger <joe.hershberger at gmail.com>
Cc: Simon Glass <sjg at chromium.org>
Cc: Mike Frysinger <vapier at gentoo.org>
---
 include/net.h |    1 +
 net/arp.c     |    2 +-
 net/net.c     |   23 +++++++++++++++++++++++
 net/ping.c    |   10 +++++-----
 4 files changed, 30 insertions(+), 6 deletions(-)

diff --git a/include/net.h b/include/net.h
index 796d59f..33570c7 100644
--- a/include/net.h
+++ b/include/net.h
@@ -443,6 +443,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 90bcc09..8d5ac20 100644
--- 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 cac9406..0a35858 100644
--- a/net/net.c
+++ b/net/net.c
@@ -1279,6 +1279,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 6cdcdf0..09935b3 100644
--- 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;*/
-- 
1.6.0.2