1 // SPDX-License-Identifier: GPL-2.0+
2 /*
3 * Copyright 2019-2021 NXP Semiconductors
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
13 struct dsa_sandbox_priv {
14 struct eth_sandbox_priv *master_priv;
15 int port_en_mask;
16 };
17
18 struct dsa_sandbox_tag {
19 u32 magic;
20 u32 port;
21 };
22
sb_dsa_port_enabled(struct udevice * dev,int port)23 static 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
sb_dsa_master_enabled(struct udevice * dev)30 static 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
dsa_sandbox_port_enable(struct udevice * dev,int port,struct phy_device * phy)37 static 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
dsa_sandbox_port_disable(struct udevice * dev,int port,struct phy_device * phy)50 static 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
dsa_sandbox_xmit(struct udevice * dev,int port,void * packet,int length)58 static 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
dsa_sandbox_rcv(struct udevice * dev,int * port,void * packet,int length)75 static 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
93 static 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
sb_dsa_handler(struct udevice * dev,void * packet,unsigned int len)100 static 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
130 dsa_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
dsa_sandbox_probe(struct udevice * dev)147 static 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
167 static const struct udevice_id dsa_sandbox_ids[] = {
168 { .compatible = "sandbox,dsa" },
169 { }
170 };
171
172 U_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