1// SPDX-License-Identifier: GPL-2.0+
2/*
3 * Copyright 2019-2021 NXP
4 */
5
6#include <asm/eth.h>
7#include <net/dsa.h>
8#include <net.h>
9
10#define DSA_SANDBOX_MAGIC	0x00415344
11#define DSA_SANDBOX_TAG_LEN	sizeof(struct dsa_sandbox_tag)
12
13struct dsa_sandbox_priv {
14	struct eth_sandbox_priv *master_priv;
15	int port_en_mask;
16};
17
18struct dsa_sandbox_tag {
19	u32 magic;
20	u32 port;
21};
22
23static bool sb_dsa_port_enabled(struct udevice *dev, int port)
24{
25	struct dsa_sandbox_priv *priv = dev_get_priv(dev);
26
27	return priv->port_en_mask & BIT(port);
28}
29
30static bool sb_dsa_master_enabled(struct udevice *dev)
31{
32	struct dsa_sandbox_priv *priv = dev_get_priv(dev);
33
34	return !priv->master_priv->disabled;
35}
36
37static int dsa_sandbox_port_enable(struct udevice *dev, int port,
38				   struct phy_device *phy)
39{
40	struct dsa_sandbox_priv *priv = dev_get_priv(dev);
41
42	if (!sb_dsa_master_enabled(dev))
43		return -EFAULT;
44
45	priv->port_en_mask |= BIT(port);
46
47	return 0;
48}
49
50static void dsa_sandbox_port_disable(struct udevice *dev, int port,
51				     struct phy_device *phy)
52{
53	struct dsa_sandbox_priv *priv = dev_get_priv(dev);
54
55	priv->port_en_mask &= ~BIT(port);
56}
57
58static int dsa_sandbox_xmit(struct udevice *dev, int port, void *packet,
59			    int length)
60{
61	struct dsa_sandbox_tag *tag = packet;
62
63	if (!sb_dsa_master_enabled(dev))
64		return -EFAULT;
65
66	if (!sb_dsa_port_enabled(dev, port))
67		return -EFAULT;
68
69	tag->magic = DSA_SANDBOX_MAGIC;
70	tag->port = port;
71
72	return 0;
73}
74
75static int dsa_sandbox_rcv(struct udevice *dev, int *port, void *packet,
76			   int length)
77{
78	struct dsa_sandbox_tag *tag = packet;
79
80	if (!sb_dsa_master_enabled(dev))
81		return -EFAULT;
82
83	if (tag->magic != DSA_SANDBOX_MAGIC)
84		return -EFAULT;
85
86	*port = tag->port;
87	if (!sb_dsa_port_enabled(dev, tag->port))
88		return -EFAULT;
89
90	return 0;
91}
92
93static const struct dsa_ops dsa_sandbox_ops = {
94	.port_enable = dsa_sandbox_port_enable,
95	.port_disable = dsa_sandbox_port_disable,
96	.xmit = dsa_sandbox_xmit,
97	.rcv = dsa_sandbox_rcv,
98};
99
100static int sb_dsa_handler(struct udevice *dev, void *packet,
101			  unsigned int len)
102{
103	struct eth_sandbox_priv *master_priv;
104	struct dsa_sandbox_tag *tag = packet;
105	struct udevice *dsa_dev;
106	u32 port_index;
107	void *rx_buf;
108	int i;
109
110	/* this emulates the switch hw and the network side */
111	if (tag->magic != DSA_SANDBOX_MAGIC)
112		return -EFAULT;
113
114	port_index = tag->port;
115	master_priv = dev_get_priv(dev);
116	dsa_dev = master_priv->priv;
117	if (!sb_dsa_port_enabled(dsa_dev, port_index))
118		return -EFAULT;
119
120	packet += DSA_SANDBOX_TAG_LEN;
121	len -= DSA_SANDBOX_TAG_LEN;
122
123	if (!sandbox_eth_arp_req_to_reply(dev, packet, len))
124		goto dsa_tagging;
125	if (!sandbox_eth_ping_req_to_reply(dev, packet, len))
126		goto dsa_tagging;
127
128	return 0;
129
130dsa_tagging:
131	master_priv->recv_packets--;
132	i = master_priv->recv_packets;
133	rx_buf = master_priv->recv_packet_buffer[i];
134	len = master_priv->recv_packet_length[i];
135	memmove(rx_buf + DSA_SANDBOX_TAG_LEN, rx_buf, len);
136
137	tag = rx_buf;
138	tag->magic = DSA_SANDBOX_MAGIC;
139	tag->port = port_index;
140	len += DSA_SANDBOX_TAG_LEN;
141	master_priv->recv_packet_length[i] = len;
142	master_priv->recv_packets++;
143
144	return 0;
145}
146
147static int dsa_sandbox_probe(struct udevice *dev)
148{
149	struct dsa_sandbox_priv *priv = dev_get_priv(dev);
150	struct udevice *master = dsa_get_master(dev);
151	struct eth_sandbox_priv *master_priv;
152
153	if (!master)
154		return -ENODEV;
155
156	dsa_set_tagging(dev, DSA_SANDBOX_TAG_LEN, 0);
157
158	master_priv = dev_get_priv(master);
159	master_priv->priv = dev;
160	master_priv->tx_handler = sb_dsa_handler;
161
162	priv->master_priv = master_priv;
163
164	return 0;
165}
166
167static const struct udevice_id dsa_sandbox_ids[] = {
168	{ .compatible = "sandbox,dsa" },
169	{ }
170};
171
172U_BOOT_DRIVER(dsa_sandbox) = {
173	.name		= "dsa_sandbox",
174	.id		= UCLASS_DSA,
175	.of_match	= dsa_sandbox_ids,
176	.probe		= dsa_sandbox_probe,
177	.ops		= &dsa_sandbox_ops,
178	.priv_auto	= sizeof(struct dsa_sandbox_priv),
179};
180