1
2
3
4
5
6
7
8
9
10
11
12#include <linux/platform_device.h>
13#include <linux/netdevice.h>
14#include <linux/phy.h>
15#include <linux/phy_fixed.h>
16#include <linux/export.h>
17#include <linux/ethtool.h>
18#include <linux/workqueue.h>
19#include <linux/module.h>
20#include <linux/if_bridge.h>
21#include <net/dsa.h>
22
23#include "dsa_loop.h"
24
25struct dsa_loop_vlan {
26 u16 members;
27 u16 untagged;
28};
29
30struct dsa_loop_mib_entry {
31 char name[ETH_GSTRING_LEN];
32 unsigned long val;
33};
34
35enum dsa_loop_mib_counters {
36 DSA_LOOP_PHY_READ_OK,
37 DSA_LOOP_PHY_READ_ERR,
38 DSA_LOOP_PHY_WRITE_OK,
39 DSA_LOOP_PHY_WRITE_ERR,
40 __DSA_LOOP_CNT_MAX,
41};
42
43static struct dsa_loop_mib_entry dsa_loop_mibs[] = {
44 [DSA_LOOP_PHY_READ_OK] = { "phy_read_ok", },
45 [DSA_LOOP_PHY_READ_ERR] = { "phy_read_err", },
46 [DSA_LOOP_PHY_WRITE_OK] = { "phy_write_ok", },
47 [DSA_LOOP_PHY_WRITE_ERR] = { "phy_write_err", },
48};
49
50struct dsa_loop_port {
51 struct dsa_loop_mib_entry mib[__DSA_LOOP_CNT_MAX];
52};
53
54#define DSA_LOOP_VLANS 5
55
56struct dsa_loop_priv {
57 struct mii_bus *bus;
58 unsigned int port_base;
59 struct dsa_loop_vlan vlans[DSA_LOOP_VLANS];
60 struct net_device *netdev;
61 struct dsa_loop_port ports[DSA_MAX_PORTS];
62 u16 pvid;
63};
64
65static struct phy_device *phydevs[PHY_MAX_ADDR];
66
67static enum dsa_tag_protocol dsa_loop_get_protocol(struct dsa_switch *ds,
68 int port)
69{
70 dev_dbg(ds->dev, "%s: port: %d\n", __func__, port);
71
72 return DSA_TAG_PROTO_NONE;
73}
74
75static int dsa_loop_setup(struct dsa_switch *ds)
76{
77 struct dsa_loop_priv *ps = ds->priv;
78 unsigned int i;
79
80 for (i = 0; i < ds->num_ports; i++)
81 memcpy(ps->ports[i].mib, dsa_loop_mibs,
82 sizeof(dsa_loop_mibs));
83
84 dev_dbg(ds->dev, "%s\n", __func__);
85
86 return 0;
87}
88
89static int dsa_loop_get_sset_count(struct dsa_switch *ds, int port, int sset)
90{
91 if (sset != ETH_SS_STATS && sset != ETH_SS_PHY_STATS)
92 return 0;
93
94 return __DSA_LOOP_CNT_MAX;
95}
96
97static void dsa_loop_get_strings(struct dsa_switch *ds, int port,
98 u32 stringset, uint8_t *data)
99{
100 struct dsa_loop_priv *ps = ds->priv;
101 unsigned int i;
102
103 if (stringset != ETH_SS_STATS && stringset != ETH_SS_PHY_STATS)
104 return;
105
106 for (i = 0; i < __DSA_LOOP_CNT_MAX; i++)
107 memcpy(data + i * ETH_GSTRING_LEN,
108 ps->ports[port].mib[i].name, ETH_GSTRING_LEN);
109}
110
111static void dsa_loop_get_ethtool_stats(struct dsa_switch *ds, int port,
112 uint64_t *data)
113{
114 struct dsa_loop_priv *ps = ds->priv;
115 unsigned int i;
116
117 for (i = 0; i < __DSA_LOOP_CNT_MAX; i++)
118 data[i] = ps->ports[port].mib[i].val;
119}
120
121static int dsa_loop_phy_read(struct dsa_switch *ds, int port, int regnum)
122{
123 struct dsa_loop_priv *ps = ds->priv;
124 struct mii_bus *bus = ps->bus;
125 int ret;
126
127 ret = mdiobus_read_nested(bus, ps->port_base + port, regnum);
128 if (ret < 0)
129 ps->ports[port].mib[DSA_LOOP_PHY_READ_ERR].val++;
130 else
131 ps->ports[port].mib[DSA_LOOP_PHY_READ_OK].val++;
132
133 return ret;
134}
135
136static int dsa_loop_phy_write(struct dsa_switch *ds, int port,
137 int regnum, u16 value)
138{
139 struct dsa_loop_priv *ps = ds->priv;
140 struct mii_bus *bus = ps->bus;
141 int ret;
142
143 ret = mdiobus_write_nested(bus, ps->port_base + port, regnum, value);
144 if (ret < 0)
145 ps->ports[port].mib[DSA_LOOP_PHY_WRITE_ERR].val++;
146 else
147 ps->ports[port].mib[DSA_LOOP_PHY_WRITE_OK].val++;
148
149 return ret;
150}
151
152static int dsa_loop_port_bridge_join(struct dsa_switch *ds, int port,
153 struct net_device *bridge)
154{
155 dev_dbg(ds->dev, "%s: port: %d, bridge: %s\n",
156 __func__, port, bridge->name);
157
158 return 0;
159}
160
161static void dsa_loop_port_bridge_leave(struct dsa_switch *ds, int port,
162 struct net_device *bridge)
163{
164 dev_dbg(ds->dev, "%s: port: %d, bridge: %s\n",
165 __func__, port, bridge->name);
166}
167
168static void dsa_loop_port_stp_state_set(struct dsa_switch *ds, int port,
169 u8 state)
170{
171 dev_dbg(ds->dev, "%s: port: %d, state: %d\n",
172 __func__, port, state);
173}
174
175static int dsa_loop_port_vlan_filtering(struct dsa_switch *ds, int port,
176 bool vlan_filtering)
177{
178 dev_dbg(ds->dev, "%s: port: %d, vlan_filtering: %d\n",
179 __func__, port, vlan_filtering);
180
181 return 0;
182}
183
184static int
185dsa_loop_port_vlan_prepare(struct dsa_switch *ds, int port,
186 const struct switchdev_obj_port_vlan *vlan)
187{
188 struct dsa_loop_priv *ps = ds->priv;
189 struct mii_bus *bus = ps->bus;
190
191 dev_dbg(ds->dev, "%s: port: %d, vlan: %d-%d",
192 __func__, port, vlan->vid_begin, vlan->vid_end);
193
194
195 mdiobus_read(bus, ps->port_base + port, MII_BMSR);
196
197 if (vlan->vid_end > DSA_LOOP_VLANS)
198 return -ERANGE;
199
200 return 0;
201}
202
203static void dsa_loop_port_vlan_add(struct dsa_switch *ds, int port,
204 const struct switchdev_obj_port_vlan *vlan)
205{
206 bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
207 bool pvid = vlan->flags & BRIDGE_VLAN_INFO_PVID;
208 struct dsa_loop_priv *ps = ds->priv;
209 struct mii_bus *bus = ps->bus;
210 struct dsa_loop_vlan *vl;
211 u16 vid;
212
213
214 mdiobus_read(bus, ps->port_base + port, MII_BMSR);
215
216 for (vid = vlan->vid_begin; vid <= vlan->vid_end; ++vid) {
217 vl = &ps->vlans[vid];
218
219 vl->members |= BIT(port);
220 if (untagged)
221 vl->untagged |= BIT(port);
222 else
223 vl->untagged &= ~BIT(port);
224
225 dev_dbg(ds->dev, "%s: port: %d vlan: %d, %stagged, pvid: %d\n",
226 __func__, port, vid, untagged ? "un" : "", pvid);
227 }
228
229 if (pvid)
230 ps->pvid = vid;
231}
232
233static int dsa_loop_port_vlan_del(struct dsa_switch *ds, int port,
234 const struct switchdev_obj_port_vlan *vlan)
235{
236 bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
237 struct dsa_loop_priv *ps = ds->priv;
238 struct mii_bus *bus = ps->bus;
239 struct dsa_loop_vlan *vl;
240 u16 vid, pvid = ps->pvid;
241
242
243 mdiobus_read(bus, ps->port_base + port, MII_BMSR);
244
245 for (vid = vlan->vid_begin; vid <= vlan->vid_end; ++vid) {
246 vl = &ps->vlans[vid];
247
248 vl->members &= ~BIT(port);
249 if (untagged)
250 vl->untagged &= ~BIT(port);
251
252 if (pvid == vid)
253 pvid = 1;
254
255 dev_dbg(ds->dev, "%s: port: %d vlan: %d, %stagged, pvid: %d\n",
256 __func__, port, vid, untagged ? "un" : "", pvid);
257 }
258 ps->pvid = pvid;
259
260 return 0;
261}
262
263static const struct dsa_switch_ops dsa_loop_driver = {
264 .get_tag_protocol = dsa_loop_get_protocol,
265 .setup = dsa_loop_setup,
266 .get_strings = dsa_loop_get_strings,
267 .get_ethtool_stats = dsa_loop_get_ethtool_stats,
268 .get_sset_count = dsa_loop_get_sset_count,
269 .get_ethtool_phy_stats = dsa_loop_get_ethtool_stats,
270 .phy_read = dsa_loop_phy_read,
271 .phy_write = dsa_loop_phy_write,
272 .port_bridge_join = dsa_loop_port_bridge_join,
273 .port_bridge_leave = dsa_loop_port_bridge_leave,
274 .port_stp_state_set = dsa_loop_port_stp_state_set,
275 .port_vlan_filtering = dsa_loop_port_vlan_filtering,
276 .port_vlan_prepare = dsa_loop_port_vlan_prepare,
277 .port_vlan_add = dsa_loop_port_vlan_add,
278 .port_vlan_del = dsa_loop_port_vlan_del,
279};
280
281static int dsa_loop_drv_probe(struct mdio_device *mdiodev)
282{
283 struct dsa_loop_pdata *pdata = mdiodev->dev.platform_data;
284 struct dsa_loop_priv *ps;
285 struct dsa_switch *ds;
286
287 if (!pdata)
288 return -ENODEV;
289
290 dev_info(&mdiodev->dev, "%s: 0x%0x\n",
291 pdata->name, pdata->enabled_ports);
292
293 ds = dsa_switch_alloc(&mdiodev->dev, DSA_MAX_PORTS);
294 if (!ds)
295 return -ENOMEM;
296
297 ps = devm_kzalloc(&mdiodev->dev, sizeof(*ps), GFP_KERNEL);
298 if (!ps)
299 return -ENOMEM;
300
301 ps->netdev = dev_get_by_name(&init_net, pdata->netdev);
302 if (!ps->netdev)
303 return -EPROBE_DEFER;
304
305 pdata->cd.netdev[DSA_LOOP_CPU_PORT] = &ps->netdev->dev;
306
307 ds->dev = &mdiodev->dev;
308 ds->ops = &dsa_loop_driver;
309 ds->priv = ps;
310 ps->bus = mdiodev->bus;
311
312 dev_set_drvdata(&mdiodev->dev, ds);
313
314 return dsa_register_switch(ds);
315}
316
317static void dsa_loop_drv_remove(struct mdio_device *mdiodev)
318{
319 struct dsa_switch *ds = dev_get_drvdata(&mdiodev->dev);
320 struct dsa_loop_priv *ps = ds->priv;
321
322 dsa_unregister_switch(ds);
323 dev_put(ps->netdev);
324}
325
326static struct mdio_driver dsa_loop_drv = {
327 .mdiodrv.driver = {
328 .name = "dsa-loop",
329 },
330 .probe = dsa_loop_drv_probe,
331 .remove = dsa_loop_drv_remove,
332};
333
334#define NUM_FIXED_PHYS (DSA_LOOP_NUM_PORTS - 2)
335
336static int __init dsa_loop_init(void)
337{
338 struct fixed_phy_status status = {
339 .link = 1,
340 .speed = SPEED_100,
341 .duplex = DUPLEX_FULL,
342 };
343 unsigned int i;
344
345 for (i = 0; i < NUM_FIXED_PHYS; i++)
346 phydevs[i] = fixed_phy_register(PHY_POLL, &status, -1, NULL);
347
348 return mdio_driver_register(&dsa_loop_drv);
349}
350module_init(dsa_loop_init);
351
352static void __exit dsa_loop_exit(void)
353{
354 unsigned int i;
355
356 mdio_driver_unregister(&dsa_loop_drv);
357 for (i = 0; i < NUM_FIXED_PHYS; i++)
358 if (!IS_ERR(phydevs[i]))
359 fixed_phy_unregister(phydevs[i]);
360}
361module_exit(dsa_loop_exit);
362
363MODULE_LICENSE("GPL");
364MODULE_AUTHOR("Florian Fainelli");
365MODULE_DESCRIPTION("DSA loopback driver");
366
