[U-Boot] [RFC PATCH v2 7/8] net: Add ARP and PING response to sandbox driver

[Joe Hershberger]

The sandbox driver will now generate response traffic to exercise the
ping command even when no network exists.  This allows the basic data
pathways of the DM to be tested.

Signed-off-by: Joe Hershberger <joe.hershberger at ni.com>
---

Changes in v2:
-Change printfs to debug in sandbox driver
-Move static data to priv
-Move fake hwaddr to the device tree

 arch/sandbox/dts/sandbox.dts |  1 +
 drivers/net/sandbox.c        | 95 ++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 96 insertions(+)

diff --git a/arch/sandbox/dts/sandbox.dts b/arch/sandbox/dts/sandbox.dts
index 502eb3d..ba635e8 100644
--- a/arch/sandbox/dts/sandbox.dts
+++ b/arch/sandbox/dts/sandbox.dts
@@ -186,5 +186,6 @@
 	eth at 10002000 {
 		compatible = "sandbox,eth";
 		reg = <0x10002000 0x1000>;
+		fake-host-hwaddr = <0x00 0x00 0x66 0x44 0x22 0x00>;
 	};
 };
diff --git a/drivers/net/sandbox.c b/drivers/net/sandbox.c
index 3b7c3ac..79a2fd5 100644
--- a/drivers/net/sandbox.c
+++ b/drivers/net/sandbox.c
@@ -15,16 +15,102 @@
 
 DECLARE_GLOBAL_DATA_PTR;
 
+struct eth_sandbox_priv {
+	uchar fake_host_hwaddr[ARP_HLEN];
+	IPaddr_t fake_host_ipaddr;
+	uchar recv_packet_buffer[PKTSIZE];
+	int recv_packet_length;
+};
+
 int sb_eth_init(struct udevice *dev, bd_t *bis)
 {
 	debug("eth_sandbox: Init\n");
 
+	struct eth_sandbox_priv *priv = dev->priv;
+	u32 int_array[ARP_HLEN];
+	int i;
+
+	fdtdec_get_int_array(gd->fdt_blob, dev->of_offset, "fake-host-hwaddr",
+			     int_array, ARP_HLEN);
+	for (i = 0; i < ARP_HLEN; i++)
+		priv->fake_host_hwaddr[i] = (uchar)int_array[i];
+
 	return 0;
 }
 
 int sb_eth_send(struct udevice *dev, void *packet, int length)
 {
 	debug("eth_sandbox: Send packet %d\n", length);
+	struct eth_sandbox_priv *priv = dev->priv;
+
+	struct ethernet_hdr *eth = packet;
+	if (ntohs(eth->et_protlen) == PROT_ARP) {
+		struct arp_hdr *arp = packet + ETHER_HDR_SIZE;
+		if (ntohs(arp->ar_op) == ARPOP_REQUEST) {
+			/* store this as the assumed IP of the fake host */
+			priv->fake_host_ipaddr = NetReadIP(&arp->ar_tpa);
+			/* Formulate a fake response */
+			struct ethernet_hdr *eth_recv =
+				(void *)priv->recv_packet_buffer;
+			memcpy(eth_recv->et_dest, eth->et_src, ARP_HLEN);
+			memcpy(eth_recv->et_src, priv->fake_host_hwaddr,
+			       ARP_HLEN);
+			eth_recv->et_protlen = htons(PROT_ARP);
+
+			struct arp_hdr *arp_recv =
+				(void *)priv->recv_packet_buffer +
+				ETHER_HDR_SIZE;
+			arp_recv->ar_hrd = htons(ARP_ETHER);
+			arp_recv->ar_pro = htons(PROT_IP);
+			arp_recv->ar_hln = ARP_HLEN;
+			arp_recv->ar_pln = ARP_PLEN;
+			arp_recv->ar_op = htons(ARPOP_REPLY);
+			memcpy(&arp_recv->ar_sha, priv->fake_host_hwaddr,
+			       ARP_HLEN);
+			NetWriteIP(&arp_recv->ar_spa, priv->fake_host_ipaddr);
+			memcpy(&arp_recv->ar_tha, &arp->ar_sha, ARP_HLEN);
+			NetCopyIP(&arp_recv->ar_tpa, &arp->ar_spa);
+
+			priv->recv_packet_length = ETHER_HDR_SIZE +
+				ARP_HDR_SIZE;
+		}
+	} else if (ntohs(eth->et_protlen) == PROT_IP) {
+		struct ip_udp_hdr *ip = packet + ETHER_HDR_SIZE;
+		if (ip->ip_p == IPPROTO_ICMP) {
+			struct icmp_hdr *icmp = (struct icmp_hdr *)&ip->udp_src;
+			if (icmp->type == ICMP_ECHO_REQUEST) {
+				/* reply to the ping */
+				memcpy(priv->recv_packet_buffer, packet,
+				       length);
+				struct ethernet_hdr *eth_recv =
+					(void *)priv->recv_packet_buffer;
+				struct ip_udp_hdr *ipr =
+					(void *)priv->recv_packet_buffer +
+					ETHER_HDR_SIZE;
+				struct icmp_hdr *icmpr =
+					(struct icmp_hdr *)&ipr->udp_src;
+				memcpy(eth_recv->et_dest, eth->et_src,
+				       ARP_HLEN);
+				memcpy(eth_recv->et_src, priv->fake_host_hwaddr,
+				       ARP_HLEN);
+				ipr->ip_sum = 0;
+				ipr->ip_off = 0;
+				NetCopyIP((void *)&ipr->ip_dst, &ip->ip_src);
+				NetWriteIP((void *)&ipr->ip_src,
+					   priv->fake_host_ipaddr);
+				ipr->ip_sum = ~NetCksum((uchar *)ipr,
+					IP_HDR_SIZE >> 1);
+
+				icmpr->type = ICMP_ECHO_REPLY;
+				icmpr->checksum = 0;
+				icmpr->checksum = ~NetCksum((uchar *)icmpr,
+					(length - ETHER_HDR_SIZE -
+						IP_HDR_SIZE) >> 1);
+
+				priv->recv_packet_length = length;
+			}
+		}
+	}
 
 	return 0;
 #endif
@@ -32,6 +118,15 @@ int sb_eth_send(struct udevice *dev, void *packet, int length)
 
 int sb_eth_recv(struct udevice *dev)
 {
+	struct eth_sandbox_priv *priv = dev->priv;
+	if (priv->recv_packet_length) {
+		int lcl_recv_packet_length = priv->recv_packet_length;
+		debug("eth_sandbox: received packet %d\n",
+		      priv->recv_packet_length);
+		priv->recv_packet_length = 0;
+		NetReceive((void *)priv->recv_packet_buffer,
+			   lcl_recv_packet_length);
+	}
 	return 0;
 }
 
-- 
1.7.11.5