[u-boot.git] / drivers / net / sandbox.c

// SPDX-License-Identifier: GPL-2.0
/*
 * Copyright (c) 2015 National Instruments
 *
 * (C) Copyright 2015
 * Joe Hershberger <[email protected]>
 */

#include <common.h>
#include <dm.h>
#include <log.h>
#include <malloc.h>
#include <net.h>
#include <asm/eth.h>
#include <asm/test.h>

DECLARE_GLOBAL_DATA_PTR;

static bool skip_timeout;

/*
 * sandbox_eth_disable_response()
 *
 * index - The alias index (also DM seq number)
 * disable - If non-zero, ignore sent packets and don't send mock response
 */
void sandbox_eth_disable_response(int index, bool disable)
{
	struct udevice *dev;
	struct eth_sandbox_priv *priv;
	int ret;

	ret = uclass_get_device(UCLASS_ETH, index, &dev);
	if (ret)
		return;

	priv = dev_get_priv(dev);
	priv->disabled = disable;
}

/*
 * sandbox_eth_skip_timeout()
 *
 * When the first packet read is attempted, fast-forward time
 */
void sandbox_eth_skip_timeout(void)
{
	skip_timeout = true;
}

/*
 * sandbox_eth_arp_req_to_reply()
 *
 * Check for an arp request to be sent. If so, inject a reply
 *
 * returns 0 if injected, -EAGAIN if not
 */
int sandbox_eth_arp_req_to_reply(struct udevice *dev, void *packet,
				 unsigned int len)
{
	struct eth_sandbox_priv *priv = dev_get_priv(dev);
	struct ethernet_hdr *eth = packet;
	struct arp_hdr *arp;
	struct ethernet_hdr *eth_recv;
	struct arp_hdr *arp_recv;

	if (ntohs(eth->et_protlen) != PROT_ARP)
		return -EAGAIN;

	arp = packet + ETHER_HDR_SIZE;

	if (ntohs(arp->ar_op) != ARPOP_REQUEST)
		return -EAGAIN;

	/* Don't allow the buffer to overrun */
	if (priv->recv_packets >= PKTBUFSRX)
		return 0;

	/* store this as the assumed IP of the fake host */
	priv->fake_host_ipaddr = net_read_ip(&arp->ar_tpa);

	/* Formulate a fake response */
	eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets];
	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);

	arp_recv = (void *)eth_recv + 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);
	net_write_ip(&arp_recv->ar_spa, priv->fake_host_ipaddr);
	memcpy(&arp_recv->ar_tha, &arp->ar_sha, ARP_HLEN);
	net_copy_ip(&arp_recv->ar_tpa, &arp->ar_spa);

	priv->recv_packet_length[priv->recv_packets] =
		ETHER_HDR_SIZE + ARP_HDR_SIZE;
	++priv->recv_packets;

	return 0;
}

/*
 * sandbox_eth_ping_req_to_reply()
 *
 * Check for a ping request to be sent. If so, inject a reply
 *
 * returns 0 if injected, -EAGAIN if not
 */
int sandbox_eth_ping_req_to_reply(struct udevice *dev, void *packet,
				  unsigned int len)
{
	struct eth_sandbox_priv *priv = dev_get_priv(dev);
	struct ethernet_hdr *eth = packet;
	struct ip_udp_hdr *ip;
	struct icmp_hdr *icmp;
	struct ethernet_hdr *eth_recv;
	struct ip_udp_hdr *ipr;
	struct icmp_hdr *icmpr;

	if (ntohs(eth->et_protlen) != PROT_IP)
		return -EAGAIN;

	ip = packet + ETHER_HDR_SIZE;

	if (ip->ip_p != IPPROTO_ICMP)
		return -EAGAIN;

	icmp = (struct icmp_hdr *)&ip->udp_src;

	if (icmp->type != ICMP_ECHO_REQUEST)
		return -EAGAIN;

	/* Don't allow the buffer to overrun */
	if (priv->recv_packets >= PKTBUFSRX)
		return 0;

	/* reply to the ping */
	eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets];
	memcpy(eth_recv, packet, len);
	ipr = (void *)eth_recv + ETHER_HDR_SIZE;
	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;
	net_copy_ip((void *)&ipr->ip_dst, &ip->ip_src);
	net_write_ip((void *)&ipr->ip_src, priv->fake_host_ipaddr);
	ipr->ip_sum = compute_ip_checksum(ipr, IP_HDR_SIZE);

	icmpr->type = ICMP_ECHO_REPLY;
	icmpr->checksum = 0;
	icmpr->checksum = compute_ip_checksum(icmpr, ICMP_HDR_SIZE);

	priv->recv_packet_length[priv->recv_packets] = len;
	++priv->recv_packets;

	return 0;
}

/*
 * sandbox_eth_recv_arp_req()
 *
 * Inject an ARP request for this target
 *
 * returns 0 if injected, -EOVERFLOW if not
 */
int sandbox_eth_recv_arp_req(struct udevice *dev)
{
	struct eth_sandbox_priv *priv = dev_get_priv(dev);
	struct ethernet_hdr *eth_recv;
	struct arp_hdr *arp_recv;

	/* Don't allow the buffer to overrun */
	if (priv->recv_packets >= PKTBUFSRX)
		return -EOVERFLOW;

	/* Formulate a fake request */
	eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets];
	memcpy(eth_recv->et_dest, net_bcast_ethaddr, ARP_HLEN);
	memcpy(eth_recv->et_src, priv->fake_host_hwaddr, ARP_HLEN);
	eth_recv->et_protlen = htons(PROT_ARP);

	arp_recv = (void *)eth_recv + 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_REQUEST);
	memcpy(&arp_recv->ar_sha, priv->fake_host_hwaddr, ARP_HLEN);
	net_write_ip(&arp_recv->ar_spa, priv->fake_host_ipaddr);
	memcpy(&arp_recv->ar_tha, net_null_ethaddr, ARP_HLEN);
	net_write_ip(&arp_recv->ar_tpa, net_ip);

	priv->recv_packet_length[priv->recv_packets] =
		ETHER_HDR_SIZE + ARP_HDR_SIZE;
	++priv->recv_packets;

	return 0;
}

/*
 * sandbox_eth_recv_ping_req()
 *
 * Inject a ping request for this target
 *
 * returns 0 if injected, -EOVERFLOW if not
 */
int sandbox_eth_recv_ping_req(struct udevice *dev)
{
	struct eth_sandbox_priv *priv = dev_get_priv(dev);
	struct ethernet_hdr *eth_recv;
	struct ip_udp_hdr *ipr;
	struct icmp_hdr *icmpr;

	/* Don't allow the buffer to overrun */
	if (priv->recv_packets >= PKTBUFSRX)
		return -EOVERFLOW;

	/* Formulate a fake ping */
	eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets];

	memcpy(eth_recv->et_dest, net_ethaddr, ARP_HLEN);
	memcpy(eth_recv->et_src, priv->fake_host_hwaddr, ARP_HLEN);
	eth_recv->et_protlen = htons(PROT_IP);

	ipr = (void *)eth_recv + ETHER_HDR_SIZE;
	ipr->ip_hl_v = 0x45;
	ipr->ip_len = htons(IP_ICMP_HDR_SIZE);
	ipr->ip_off = htons(IP_FLAGS_DFRAG);
	ipr->ip_p = IPPROTO_ICMP;
	ipr->ip_sum = 0;
	net_write_ip(&ipr->ip_src, priv->fake_host_ipaddr);
	net_write_ip(&ipr->ip_dst, net_ip);
	ipr->ip_sum = compute_ip_checksum(ipr, IP_HDR_SIZE);

	icmpr = (struct icmp_hdr *)&ipr->udp_src;

	icmpr->type = ICMP_ECHO_REQUEST;
	icmpr->code = 0;
	icmpr->checksum = 0;
	icmpr->un.echo.id = 0;
	icmpr->un.echo.sequence = htons(1);
	icmpr->checksum = compute_ip_checksum(icmpr, ICMP_HDR_SIZE);

	priv->recv_packet_length[priv->recv_packets] =
		ETHER_HDR_SIZE + IP_ICMP_HDR_SIZE;
	++priv->recv_packets;

	return 0;
}

/*
 * sb_default_handler()
 *
 * perform typical responses to simple ping
 *
 * dev - device pointer
 * pkt - "sent" packet buffer
 * len - length of packet
 */
static int sb_default_handler(struct udevice *dev, void *packet,
			      unsigned int len)
{
	if (!sandbox_eth_arp_req_to_reply(dev, packet, len))
		return 0;
	if (!sandbox_eth_ping_req_to_reply(dev, packet, len))
		return 0;

	return 0;
}

/*
 * sandbox_eth_set_tx_handler()
 *
 * Set a custom response to a packet being sent through the sandbox eth test
 *	driver
 *
 * index - interface to set the handler for
 * handler - The func ptr to call on send. If NULL, set to default handler
 */
void sandbox_eth_set_tx_handler(int index, sandbox_eth_tx_hand_f *handler)
{
	struct udevice *dev;
	struct eth_sandbox_priv *priv;
	int ret;

	ret = uclass_get_device(UCLASS_ETH, index, &dev);
	if (ret)
		return;

	priv = dev_get_priv(dev);
	if (handler)
		priv->tx_handler = handler;
	else
		priv->tx_handler = sb_default_handler;
}

/*
 * Set priv ptr
 *
 * priv - priv void ptr to store in the device
 */
void sandbox_eth_set_priv(int index, void *priv)
{
	struct udevice *dev;
	struct eth_sandbox_priv *dev_priv;
	int ret;

	ret = uclass_get_device(UCLASS_ETH, index, &dev);
	if (ret)
		return;

	dev_priv = dev_get_priv(dev);

	dev_priv->priv = priv;
}

static int sb_eth_start(struct udevice *dev)
{
	struct eth_sandbox_priv *priv = dev_get_priv(dev);

	debug("eth_sandbox: Start\n");

	priv->recv_packets = 0;
	for (int i = 0; i < PKTBUFSRX; i++) {
		priv->recv_packet_buffer[i] = net_rx_packets[i];
		priv->recv_packet_length[i] = 0;
	}

	return 0;
}

static int sb_eth_send(struct udevice *dev, void *packet, int length)
{
	struct eth_sandbox_priv *priv = dev_get_priv(dev);

	debug("eth_sandbox: Send packet %d\n", length);

	if (priv->disabled)
		return 0;

	return priv->tx_handler(dev, packet, length);
}

static int sb_eth_recv(struct udevice *dev, int flags, uchar **packetp)
{
	struct eth_sandbox_priv *priv = dev_get_priv(dev);

	if (skip_timeout) {
		timer_test_add_offset(11000UL);
		skip_timeout = false;
	}

	if (priv->recv_packets) {
		int lcl_recv_packet_length = priv->recv_packet_length[0];

		debug("eth_sandbox: received packet[%d], %d waiting\n",
		      lcl_recv_packet_length, priv->recv_packets - 1);
		*packetp = priv->recv_packet_buffer[0];
		return lcl_recv_packet_length;
	}
	return 0;
}

static int sb_eth_free_pkt(struct udevice *dev, uchar *packet, int length)
{
	struct eth_sandbox_priv *priv = dev_get_priv(dev);
	int i;

	if (!priv->recv_packets)
		return 0;

	--priv->recv_packets;
	for (i = 0; i < priv->recv_packets; i++) {
		priv->recv_packet_length[i] = priv->recv_packet_length[i + 1];
		memcpy(priv->recv_packet_buffer[i],
		       priv->recv_packet_buffer[i + 1],
		       priv->recv_packet_length[i + 1]);
	}
	priv->recv_packet_length[priv->recv_packets] = 0;

	return 0;
}

static void sb_eth_stop(struct udevice *dev)
{
	debug("eth_sandbox: Stop\n");
}

static int sb_eth_write_hwaddr(struct udevice *dev)
{
	struct eth_pdata *pdata = dev_get_plat(dev);

	debug("eth_sandbox %s: Write HW ADDR - %pM\n", dev->name,
	      pdata->enetaddr);
	return 0;
}

static const struct eth_ops sb_eth_ops = {
	.start			= sb_eth_start,
	.send			= sb_eth_send,
	.recv			= sb_eth_recv,
	.free_pkt		= sb_eth_free_pkt,
	.stop			= sb_eth_stop,
	.write_hwaddr		= sb_eth_write_hwaddr,
};

static int sb_eth_remove(struct udevice *dev)
{
	return 0;
}

static int sb_eth_of_to_plat(struct udevice *dev)
{
	struct eth_pdata *pdata = dev_get_plat(dev);
	struct eth_sandbox_priv *priv = dev_get_priv(dev);
	const u8 *mac;

	pdata->iobase = dev_read_addr(dev);

	mac = dev_read_u8_array_ptr(dev, "fake-host-hwaddr", ARP_HLEN);
	if (!mac) {
		printf("'fake-host-hwaddr' is missing from the DT\n");
		return -EINVAL;
	}
	memcpy(priv->fake_host_hwaddr, mac, ARP_HLEN);
	priv->disabled = false;
	priv->tx_handler = sb_default_handler;

	return 0;
}

static const struct udevice_id sb_eth_ids[] = {
	{ .compatible = "sandbox,eth" },
	{ }
};

U_BOOT_DRIVER(eth_sandbox) = {
	.name	= "eth_sandbox",
	.id	= UCLASS_ETH,
	.of_match = sb_eth_ids,
	.of_to_plat = sb_eth_of_to_plat,
	.remove	= sb_eth_remove,
	.ops	= &sb_eth_ops,
	.priv_auto	= sizeof(struct eth_sandbox_priv),
	.plat_auto	= sizeof(struct eth_pdata),
};
Commit	Line	Data
83d290c5	1	// SPDX-License-Identifier: GPL-2.0
3ea143ab JH	2	/*
	3	* Copyright (c) 2015 National Instruments
	4	*
	5	* (C) Copyright 2015
	6	* Joe Hershberger <[email protected]>
3ea143ab JH	7	*/
	8
	9	#include <common.h>
	10	#include <dm.h>
f7ae49fc	11	#include <log.h>
3ea143ab JH	12	#include <malloc.h>
3ea143ab JH	13	#include <net.h>
c7eb733d	14	#include <asm/eth.h>
6f2707c6	15	#include <asm/test.h>
3ea143ab JH	16
	17	DECLARE_GLOBAL_DATA_PTR;
	18
6f2707c6	19	static bool skip_timeout;
2eede1f3 JH	20
	21	/*
	22	* sandbox_eth_disable_response()
	23	*
	24	* index - The alias index (also DM seq number)
	25	* disable - If non-zero, ignore sent packets and don't send mock response
	26	*/
	27	void sandbox_eth_disable_response(int index, bool disable)
	28	{
e4ab9a65 JH	29	struct udevice *dev;
	30	struct eth_sandbox_priv *priv;
	31	int ret;
	32
	33	ret = uclass_get_device(UCLASS_ETH, index, &dev);
	34	if (ret)
	35	return;
	36
	37	priv = dev_get_priv(dev);
	38	priv->disabled = disable;
2eede1f3 JH	39	}
2eede1f3 JH	40
6f2707c6 JH	41	/*
	42	* sandbox_eth_skip_timeout()
	43	*
	44	* When the first packet read is attempted, fast-forward time
	45	*/
	46	void sandbox_eth_skip_timeout(void)
	47	{
	48	skip_timeout = true;
	49	}
	50
e95bb161 JH	51	/*
	52	* sandbox_eth_arp_req_to_reply()
	53	*
	54	* Check for an arp request to be sent. If so, inject a reply
	55	*
	56	* returns 0 if injected, -EAGAIN if not
	57	*/
	58	int sandbox_eth_arp_req_to_reply(struct udevice dev, void packet,
	59	unsigned int len)
	60	{
	61	struct eth_sandbox_priv *priv = dev_get_priv(dev);
	62	struct ethernet_hdr *eth = packet;
	63	struct arp_hdr *arp;
	64	struct ethernet_hdr *eth_recv;
	65	struct arp_hdr *arp_recv;
	66
	67	if (ntohs(eth->et_protlen) != PROT_ARP)
	68	return -EAGAIN;
	69
	70	arp = packet + ETHER_HDR_SIZE;
	71
	72	if (ntohs(arp->ar_op) != ARPOP_REQUEST)
	73	return -EAGAIN;
	74
c67a4207 JH	75	/* Don't allow the buffer to overrun */
	76	if (priv->recv_packets >= PKTBUFSRX)
	77	return 0;
	78
e95bb161 JH	79	/* store this as the assumed IP of the fake host */
	80	priv->fake_host_ipaddr = net_read_ip(&arp->ar_tpa);
	81
	82	/* Formulate a fake response */
c67a4207	83	eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets];
e95bb161 JH	84	memcpy(eth_recv->et_dest, eth->et_src, ARP_HLEN);
	85	memcpy(eth_recv->et_src, priv->fake_host_hwaddr, ARP_HLEN);
	86	eth_recv->et_protlen = htons(PROT_ARP);
	87
	88	arp_recv = (void *)eth_recv + ETHER_HDR_SIZE;
	89	arp_recv->ar_hrd = htons(ARP_ETHER);
	90	arp_recv->ar_pro = htons(PROT_IP);
	91	arp_recv->ar_hln = ARP_HLEN;
	92	arp_recv->ar_pln = ARP_PLEN;
	93	arp_recv->ar_op = htons(ARPOP_REPLY);
	94	memcpy(&arp_recv->ar_sha, priv->fake_host_hwaddr, ARP_HLEN);
	95	net_write_ip(&arp_recv->ar_spa, priv->fake_host_ipaddr);
	96	memcpy(&arp_recv->ar_tha, &arp->ar_sha, ARP_HLEN);
	97	net_copy_ip(&arp_recv->ar_tpa, &arp->ar_spa);
	98
c67a4207 JH	99	priv->recv_packet_length[priv->recv_packets] =
	100	ETHER_HDR_SIZE + ARP_HDR_SIZE;
	101	++priv->recv_packets;
e95bb161 JH	102
	103	return 0;
	104	}
	105
	106	/*
	107	* sandbox_eth_ping_req_to_reply()
	108	*
	109	* Check for a ping request to be sent. If so, inject a reply
	110	*
	111	* returns 0 if injected, -EAGAIN if not
	112	*/
	113	int sandbox_eth_ping_req_to_reply(struct udevice dev, void packet,
	114	unsigned int len)
	115	{
	116	struct eth_sandbox_priv *priv = dev_get_priv(dev);
	117	struct ethernet_hdr *eth = packet;
	118	struct ip_udp_hdr *ip;
	119	struct icmp_hdr *icmp;
	120	struct ethernet_hdr *eth_recv;
	121	struct ip_udp_hdr *ipr;
	122	struct icmp_hdr *icmpr;
	123
	124	if (ntohs(eth->et_protlen) != PROT_IP)
	125	return -EAGAIN;
	126
	127	ip = packet + ETHER_HDR_SIZE;
	128
	129	if (ip->ip_p != IPPROTO_ICMP)
	130	return -EAGAIN;
	131
	132	icmp = (struct icmp_hdr *)&ip->udp_src;
	133
	134	if (icmp->type != ICMP_ECHO_REQUEST)
	135	return -EAGAIN;
	136
c67a4207 JH	137	/* Don't allow the buffer to overrun */
	138	if (priv->recv_packets >= PKTBUFSRX)
	139	return 0;
	140
e95bb161	141	/* reply to the ping */
c67a4207	142	eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets];
e95bb161 JH	143	memcpy(eth_recv, packet, len);
	144	ipr = (void *)eth_recv + ETHER_HDR_SIZE;
	145	icmpr = (struct icmp_hdr *)&ipr->udp_src;
	146	memcpy(eth_recv->et_dest, eth->et_src, ARP_HLEN);
	147	memcpy(eth_recv->et_src, priv->fake_host_hwaddr, ARP_HLEN);
	148	ipr->ip_sum = 0;
	149	ipr->ip_off = 0;
	150	net_copy_ip((void *)&ipr->ip_dst, &ip->ip_src);
	151	net_write_ip((void *)&ipr->ip_src, priv->fake_host_ipaddr);
	152	ipr->ip_sum = compute_ip_checksum(ipr, IP_HDR_SIZE);
	153
	154	icmpr->type = ICMP_ECHO_REPLY;
	155	icmpr->checksum = 0;
	156	icmpr->checksum = compute_ip_checksum(icmpr, ICMP_HDR_SIZE);
	157
c67a4207 JH	158	priv->recv_packet_length[priv->recv_packets] = len;
c67a4207 JH	159	++priv->recv_packets;
e95bb161 JH	160
	161	return 0;
	162	}
	163
45988dae JH	164	/*
	165	* sandbox_eth_recv_arp_req()
	166	*
	167	* Inject an ARP request for this target
	168	*
	169	* returns 0 if injected, -EOVERFLOW if not
	170	*/
	171	int sandbox_eth_recv_arp_req(struct udevice *dev)
	172	{
	173	struct eth_sandbox_priv *priv = dev_get_priv(dev);
	174	struct ethernet_hdr *eth_recv;
	175	struct arp_hdr *arp_recv;
	176
	177	/* Don't allow the buffer to overrun */
	178	if (priv->recv_packets >= PKTBUFSRX)
	179	return -EOVERFLOW;
	180
	181	/* Formulate a fake request */
	182	eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets];
	183	memcpy(eth_recv->et_dest, net_bcast_ethaddr, ARP_HLEN);
	184	memcpy(eth_recv->et_src, priv->fake_host_hwaddr, ARP_HLEN);
	185	eth_recv->et_protlen = htons(PROT_ARP);
	186
	187	arp_recv = (void *)eth_recv + ETHER_HDR_SIZE;
	188	arp_recv->ar_hrd = htons(ARP_ETHER);
	189	arp_recv->ar_pro = htons(PROT_IP);
	190	arp_recv->ar_hln = ARP_HLEN;
	191	arp_recv->ar_pln = ARP_PLEN;
	192	arp_recv->ar_op = htons(ARPOP_REQUEST);
	193	memcpy(&arp_recv->ar_sha, priv->fake_host_hwaddr, ARP_HLEN);
	194	net_write_ip(&arp_recv->ar_spa, priv->fake_host_ipaddr);
	195	memcpy(&arp_recv->ar_tha, net_null_ethaddr, ARP_HLEN);
	196	net_write_ip(&arp_recv->ar_tpa, net_ip);
	197
	198	priv->recv_packet_length[priv->recv_packets] =
	199	ETHER_HDR_SIZE + ARP_HDR_SIZE;
	200	++priv->recv_packets;
	201
	202	return 0;
	203	}
	204
72ff0042 JH	205	/*
	206	* sandbox_eth_recv_ping_req()
	207	*
	208	* Inject a ping request for this target
	209	*
	210	* returns 0 if injected, -EOVERFLOW if not
	211	*/
	212	int sandbox_eth_recv_ping_req(struct udevice *dev)
	213	{
	214	struct eth_sandbox_priv *priv = dev_get_priv(dev);
	215	struct ethernet_hdr *eth_recv;
	216	struct ip_udp_hdr *ipr;
	217	struct icmp_hdr *icmpr;
	218
	219	/* Don't allow the buffer to overrun */
	220	if (priv->recv_packets >= PKTBUFSRX)
	221	return -EOVERFLOW;
	222
	223	/* Formulate a fake ping */
	224	eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets];
	225
	226	memcpy(eth_recv->et_dest, net_ethaddr, ARP_HLEN);
	227	memcpy(eth_recv->et_src, priv->fake_host_hwaddr, ARP_HLEN);
	228	eth_recv->et_protlen = htons(PROT_IP);
	229
	230	ipr = (void *)eth_recv + ETHER_HDR_SIZE;
	231	ipr->ip_hl_v = 0x45;
	232	ipr->ip_len = htons(IP_ICMP_HDR_SIZE);
	233	ipr->ip_off = htons(IP_FLAGS_DFRAG);
	234	ipr->ip_p = IPPROTO_ICMP;
	235	ipr->ip_sum = 0;
	236	net_write_ip(&ipr->ip_src, priv->fake_host_ipaddr);
	237	net_write_ip(&ipr->ip_dst, net_ip);
	238	ipr->ip_sum = compute_ip_checksum(ipr, IP_HDR_SIZE);
	239
	240	icmpr = (struct icmp_hdr *)&ipr->udp_src;
	241
	242	icmpr->type = ICMP_ECHO_REQUEST;
	243	icmpr->code = 0;
	244	icmpr->checksum = 0;
	245	icmpr->un.echo.id = 0;
	246	icmpr->un.echo.sequence = htons(1);
	247	icmpr->checksum = compute_ip_checksum(icmpr, ICMP_HDR_SIZE);
	248
	249	priv->recv_packet_length[priv->recv_packets] =
	250	ETHER_HDR_SIZE + IP_ICMP_HDR_SIZE;
	251	++priv->recv_packets;
	252
	253	return 0;
	254	}
	255
c7eb733d JH	256	/*
	257	* sb_default_handler()
	258	*
	259	* perform typical responses to simple ping
	260	*
	261	* dev - device pointer
	262	* pkt - "sent" packet buffer
	263	* len - length of packet
	264	*/
	265	static int sb_default_handler(struct udevice dev, void packet,
	266	unsigned int len)
	267	{
	268	if (!sandbox_eth_arp_req_to_reply(dev, packet, len))
	269	return 0;
	270	if (!sandbox_eth_ping_req_to_reply(dev, packet, len))
	271	return 0;
	272
	273	return 0;
	274	}
	275
	276	/*
	277	* sandbox_eth_set_tx_handler()
	278	*
	279	* Set a custom response to a packet being sent through the sandbox eth test
	280	* driver
	281	*
	282	* index - interface to set the handler for
	283	* handler - The func ptr to call on send. If NULL, set to default handler
	284	*/
	285	void sandbox_eth_set_tx_handler(int index, sandbox_eth_tx_hand_f *handler)
	286	{
	287	struct udevice *dev;
	288	struct eth_sandbox_priv *priv;
	289	int ret;
	290
	291	ret = uclass_get_device(UCLASS_ETH, index, &dev);
	292	if (ret)
	293	return;
	294
	295	priv = dev_get_priv(dev);
	296	if (handler)
	297	priv->tx_handler = handler;
	298	else
	299	priv->tx_handler = sb_default_handler;
	300	}
	301
9cbe5972 JH	302	/*
	303	* Set priv ptr
	304	*
	305	* priv - priv void ptr to store in the device
	306	*/
	307	void sandbox_eth_set_priv(int index, void *priv)
	308	{
	309	struct udevice *dev;
	310	struct eth_sandbox_priv *dev_priv;
	311	int ret;
	312
	313	ret = uclass_get_device(UCLASS_ETH, index, &dev);
	314	if (ret)
	315	return;
	316
	317	dev_priv = dev_get_priv(dev);
	318
	319	dev_priv->priv = priv;
	320	}
	321
3ea143ab JH	322	static int sb_eth_start(struct udevice *dev)
3ea143ab JH	323	{
d87a457b JH	324	struct eth_sandbox_priv *priv = dev_get_priv(dev);
d87a457b JH	325
3ea143ab JH	326	debug("eth_sandbox: Start\n");
3ea143ab JH	327
c67a4207 JH	328	priv->recv_packets = 0;
	329	for (int i = 0; i < PKTBUFSRX; i++) {
	330	priv->recv_packet_buffer[i] = net_rx_packets[i];
	331	priv->recv_packet_length[i] = 0;
	332	}
b32dd183	333
3ea143ab JH	334	return 0;
	335	}
	336
	337	static int sb_eth_send(struct udevice dev, void packet, int length)
	338	{
d87a457b	339	struct eth_sandbox_priv *priv = dev_get_priv(dev);
d87a457b	340
3ea143ab JH	341	debug("eth_sandbox: Send packet %d\n", length);
3ea143ab JH	342
e4ab9a65	343	if (priv->disabled)
2eede1f3 JH	344	return 0;
2eede1f3 JH	345
c7eb733d	346	return priv->tx_handler(dev, packet, length);
3ea143ab JH	347	}
3ea143ab JH	348
a1ca92ea	349	static int sb_eth_recv(struct udevice dev, int flags, uchar *packetp)
3ea143ab	350	{
d87a457b JH	351	struct eth_sandbox_priv *priv = dev_get_priv(dev);
d87a457b JH	352
6f2707c6	353	if (skip_timeout) {
d0a9b82b	354	timer_test_add_offset(11000UL);
6f2707c6 JH	355	skip_timeout = false;
	356	}
	357
c67a4207 JH	358	if (priv->recv_packets) {
c67a4207 JH	359	int lcl_recv_packet_length = priv->recv_packet_length[0];
d87a457b	360
c67a4207 JH	361	debug("eth_sandbox: received packet[%d], %d waiting\n",
	362	lcl_recv_packet_length, priv->recv_packets - 1);
	363	*packetp = priv->recv_packet_buffer[0];
d87a457b JH	364	return lcl_recv_packet_length;
d87a457b JH	365	}
3ea143ab JH	366	return 0;
	367	}
	368
c67a4207 JH	369	static int sb_eth_free_pkt(struct udevice dev, uchar packet, int length)
	370	{
	371	struct eth_sandbox_priv *priv = dev_get_priv(dev);
	372	int i;
	373
	374	if (!priv->recv_packets)
	375	return 0;
	376
	377	--priv->recv_packets;
	378	for (i = 0; i < priv->recv_packets; i++) {
	379	priv->recv_packet_length[i] = priv->recv_packet_length[i + 1];
	380	memcpy(priv->recv_packet_buffer[i],
	381	priv->recv_packet_buffer[i + 1],
	382	priv->recv_packet_length[i + 1]);
	383	}
	384	priv->recv_packet_length[priv->recv_packets] = 0;
	385
	386	return 0;
	387	}
	388
3ea143ab JH	389	static void sb_eth_stop(struct udevice *dev)
	390	{
	391	debug("eth_sandbox: Stop\n");
	392	}
	393
	394	static int sb_eth_write_hwaddr(struct udevice *dev)
	395	{
c69cda25	396	struct eth_pdata *pdata = dev_get_plat(dev);
3ea143ab JH	397
	398	debug("eth_sandbox %s: Write HW ADDR - %pM\n", dev->name,
	399	pdata->enetaddr);
	400	return 0;
	401	}
	402
	403	static const struct eth_ops sb_eth_ops = {
	404	.start = sb_eth_start,
	405	.send = sb_eth_send,
	406	.recv = sb_eth_recv,
c67a4207	407	.free_pkt = sb_eth_free_pkt,
3ea143ab JH	408	.stop = sb_eth_stop,
	409	.write_hwaddr = sb_eth_write_hwaddr,
	410	};
	411
	412	static int sb_eth_remove(struct udevice *dev)
	413	{
	414	return 0;
	415	}
	416
d1998a9f	417	static int sb_eth_of_to_plat(struct udevice *dev)
3ea143ab	418	{
c69cda25	419	struct eth_pdata *pdata = dev_get_plat(dev);
b32dd183 JH	420	struct eth_sandbox_priv *priv = dev_get_priv(dev);
	421	const u8 *mac;
	422
	423	pdata->iobase = dev_read_addr(dev);
	424
	425	mac = dev_read_u8_array_ptr(dev, "fake-host-hwaddr", ARP_HLEN);
	426	if (!mac) {
	427	printf("'fake-host-hwaddr' is missing from the DT\n");
	428	return -EINVAL;
	429	}
	430	memcpy(priv->fake_host_hwaddr, mac, ARP_HLEN);
e4ab9a65	431	priv->disabled = false;
c7eb733d	432	priv->tx_handler = sb_default_handler;
3ea143ab	433
3ea143ab JH	434	return 0;
	435	}
	436
	437	static const struct udevice_id sb_eth_ids[] = {
	438	{ .compatible = "sandbox,eth" },
	439	{ }
	440	};
	441
	442	U_BOOT_DRIVER(eth_sandbox) = {
	443	.name = "eth_sandbox",
	444	.id = UCLASS_ETH,
	445	.of_match = sb_eth_ids,
d1998a9f	446	.of_to_plat = sb_eth_of_to_plat,
3ea143ab JH	447	.remove = sb_eth_remove,
3ea143ab JH	448	.ops = &sb_eth_ops,
41575d8e	449	.priv_auto = sizeof(struct eth_sandbox_priv),
caa4daa2	450	.plat_auto = sizeof(struct eth_pdata),
3ea143ab	451	};