1 | // SPDX-License-Identifier: GPL-2.0-or-later |
2 | /* |
3 | * Distributed Switch Architecture loopback driver |
4 | * |
5 | * Copyright (C) 2016, Florian Fainelli <f.fainelli@gmail.com> |
6 | */ |
7 | |
8 | #include <linux/platform_device.h> |
9 | #include <linux/netdevice.h> |
10 | #include <linux/phy.h> |
11 | #include <linux/phy_fixed.h> |
12 | #include <linux/export.h> |
13 | #include <linux/ethtool.h> |
14 | #include <linux/workqueue.h> |
15 | #include <linux/module.h> |
16 | #include <linux/if_bridge.h> |
17 | #include <linux/dsa/loop.h> |
18 | #include <net/dsa.h> |
19 | |
20 | #include "dsa_loop.h" |
21 | |
22 | static struct dsa_loop_mib_entry dsa_loop_mibs[] = { |
23 | [DSA_LOOP_PHY_READ_OK] = { "phy_read_ok" , }, |
24 | [DSA_LOOP_PHY_READ_ERR] = { .name: "phy_read_err" , }, |
25 | [DSA_LOOP_PHY_WRITE_OK] = { .name: "phy_write_ok" , }, |
26 | [DSA_LOOP_PHY_WRITE_ERR] = { .name: "phy_write_err" , }, |
27 | }; |
28 | |
29 | static struct phy_device *phydevs[PHY_MAX_ADDR]; |
30 | |
31 | enum dsa_loop_devlink_resource_id { |
32 | DSA_LOOP_DEVLINK_PARAM_ID_VTU, |
33 | }; |
34 | |
35 | static u64 dsa_loop_devlink_vtu_get(void *priv) |
36 | { |
37 | struct dsa_loop_priv *ps = priv; |
38 | unsigned int i, count = 0; |
39 | struct dsa_loop_vlan *vl; |
40 | |
41 | for (i = 0; i < ARRAY_SIZE(ps->vlans); i++) { |
42 | vl = &ps->vlans[i]; |
43 | if (vl->members) |
44 | count++; |
45 | } |
46 | |
47 | return count; |
48 | } |
49 | |
50 | static int dsa_loop_setup_devlink_resources(struct dsa_switch *ds) |
51 | { |
52 | struct devlink_resource_size_params size_params; |
53 | struct dsa_loop_priv *ps = ds->priv; |
54 | int err; |
55 | |
56 | devlink_resource_size_params_init(size_params: &size_params, ARRAY_SIZE(ps->vlans), |
57 | ARRAY_SIZE(ps->vlans), |
58 | size_granularity: 1, unit: DEVLINK_RESOURCE_UNIT_ENTRY); |
59 | |
60 | err = dsa_devlink_resource_register(ds, resource_name: "VTU" , ARRAY_SIZE(ps->vlans), |
61 | resource_id: DSA_LOOP_DEVLINK_PARAM_ID_VTU, |
62 | DEVLINK_RESOURCE_ID_PARENT_TOP, |
63 | size_params: &size_params); |
64 | if (err) |
65 | goto out; |
66 | |
67 | dsa_devlink_resource_occ_get_register(ds, |
68 | resource_id: DSA_LOOP_DEVLINK_PARAM_ID_VTU, |
69 | occ_get: dsa_loop_devlink_vtu_get, occ_get_priv: ps); |
70 | |
71 | return 0; |
72 | |
73 | out: |
74 | dsa_devlink_resources_unregister(ds); |
75 | return err; |
76 | } |
77 | |
78 | static enum dsa_tag_protocol dsa_loop_get_protocol(struct dsa_switch *ds, |
79 | int port, |
80 | enum dsa_tag_protocol mp) |
81 | { |
82 | dev_dbg(ds->dev, "%s: port: %d\n" , __func__, port); |
83 | |
84 | return DSA_TAG_PROTO_NONE; |
85 | } |
86 | |
87 | static int dsa_loop_setup(struct dsa_switch *ds) |
88 | { |
89 | struct dsa_loop_priv *ps = ds->priv; |
90 | unsigned int i; |
91 | |
92 | for (i = 0; i < ds->num_ports; i++) |
93 | memcpy(ps->ports[i].mib, dsa_loop_mibs, |
94 | sizeof(dsa_loop_mibs)); |
95 | |
96 | dev_dbg(ds->dev, "%s\n" , __func__); |
97 | |
98 | return dsa_loop_setup_devlink_resources(ds); |
99 | } |
100 | |
101 | static void dsa_loop_teardown(struct dsa_switch *ds) |
102 | { |
103 | dsa_devlink_resources_unregister(ds); |
104 | } |
105 | |
106 | static int dsa_loop_get_sset_count(struct dsa_switch *ds, int port, int sset) |
107 | { |
108 | if (sset != ETH_SS_STATS && sset != ETH_SS_PHY_STATS) |
109 | return 0; |
110 | |
111 | return __DSA_LOOP_CNT_MAX; |
112 | } |
113 | |
114 | static void dsa_loop_get_strings(struct dsa_switch *ds, int port, |
115 | u32 stringset, uint8_t *data) |
116 | { |
117 | struct dsa_loop_priv *ps = ds->priv; |
118 | unsigned int i; |
119 | |
120 | if (stringset != ETH_SS_STATS && stringset != ETH_SS_PHY_STATS) |
121 | return; |
122 | |
123 | for (i = 0; i < __DSA_LOOP_CNT_MAX; i++) |
124 | memcpy(data + i * ETH_GSTRING_LEN, |
125 | ps->ports[port].mib[i].name, ETH_GSTRING_LEN); |
126 | } |
127 | |
128 | static void dsa_loop_get_ethtool_stats(struct dsa_switch *ds, int port, |
129 | uint64_t *data) |
130 | { |
131 | struct dsa_loop_priv *ps = ds->priv; |
132 | unsigned int i; |
133 | |
134 | for (i = 0; i < __DSA_LOOP_CNT_MAX; i++) |
135 | data[i] = ps->ports[port].mib[i].val; |
136 | } |
137 | |
138 | static int dsa_loop_phy_read(struct dsa_switch *ds, int port, int regnum) |
139 | { |
140 | struct dsa_loop_priv *ps = ds->priv; |
141 | struct mii_bus *bus = ps->bus; |
142 | int ret; |
143 | |
144 | ret = mdiobus_read_nested(bus, addr: ps->port_base + port, regnum); |
145 | if (ret < 0) |
146 | ps->ports[port].mib[DSA_LOOP_PHY_READ_ERR].val++; |
147 | else |
148 | ps->ports[port].mib[DSA_LOOP_PHY_READ_OK].val++; |
149 | |
150 | return ret; |
151 | } |
152 | |
153 | static int dsa_loop_phy_write(struct dsa_switch *ds, int port, |
154 | int regnum, u16 value) |
155 | { |
156 | struct dsa_loop_priv *ps = ds->priv; |
157 | struct mii_bus *bus = ps->bus; |
158 | int ret; |
159 | |
160 | ret = mdiobus_write_nested(bus, addr: ps->port_base + port, regnum, val: value); |
161 | if (ret < 0) |
162 | ps->ports[port].mib[DSA_LOOP_PHY_WRITE_ERR].val++; |
163 | else |
164 | ps->ports[port].mib[DSA_LOOP_PHY_WRITE_OK].val++; |
165 | |
166 | return ret; |
167 | } |
168 | |
169 | static int dsa_loop_port_bridge_join(struct dsa_switch *ds, int port, |
170 | struct dsa_bridge bridge, |
171 | bool *tx_fwd_offload, |
172 | struct netlink_ext_ack *extack) |
173 | { |
174 | dev_dbg(ds->dev, "%s: port: %d, bridge: %s\n" , |
175 | __func__, port, bridge.dev->name); |
176 | |
177 | return 0; |
178 | } |
179 | |
180 | static void dsa_loop_port_bridge_leave(struct dsa_switch *ds, int port, |
181 | struct dsa_bridge bridge) |
182 | { |
183 | dev_dbg(ds->dev, "%s: port: %d, bridge: %s\n" , |
184 | __func__, port, bridge.dev->name); |
185 | } |
186 | |
187 | static void dsa_loop_port_stp_state_set(struct dsa_switch *ds, int port, |
188 | u8 state) |
189 | { |
190 | dev_dbg(ds->dev, "%s: port: %d, state: %d\n" , |
191 | __func__, port, state); |
192 | } |
193 | |
194 | static int dsa_loop_port_vlan_filtering(struct dsa_switch *ds, int port, |
195 | bool vlan_filtering, |
196 | struct netlink_ext_ack *extack) |
197 | { |
198 | dev_dbg(ds->dev, "%s: port: %d, vlan_filtering: %d\n" , |
199 | __func__, port, vlan_filtering); |
200 | |
201 | return 0; |
202 | } |
203 | |
204 | static int dsa_loop_port_vlan_add(struct dsa_switch *ds, int port, |
205 | const struct switchdev_obj_port_vlan *vlan, |
206 | struct netlink_ext_ack *extack) |
207 | { |
208 | bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED; |
209 | bool pvid = vlan->flags & BRIDGE_VLAN_INFO_PVID; |
210 | struct dsa_loop_priv *ps = ds->priv; |
211 | struct mii_bus *bus = ps->bus; |
212 | struct dsa_loop_vlan *vl; |
213 | |
214 | if (vlan->vid >= ARRAY_SIZE(ps->vlans)) |
215 | return -ERANGE; |
216 | |
217 | /* Just do a sleeping operation to make lockdep checks effective */ |
218 | mdiobus_read(bus, addr: ps->port_base + port, MII_BMSR); |
219 | |
220 | vl = &ps->vlans[vlan->vid]; |
221 | |
222 | vl->members |= BIT(port); |
223 | if (untagged) |
224 | vl->untagged |= BIT(port); |
225 | else |
226 | vl->untagged &= ~BIT(port); |
227 | |
228 | dev_dbg(ds->dev, "%s: port: %d vlan: %d, %stagged, pvid: %d\n" , |
229 | __func__, port, vlan->vid, untagged ? "un" : "" , pvid); |
230 | |
231 | if (pvid) |
232 | ps->ports[port].pvid = vlan->vid; |
233 | |
234 | return 0; |
235 | } |
236 | |
237 | static int dsa_loop_port_vlan_del(struct dsa_switch *ds, int port, |
238 | const struct switchdev_obj_port_vlan *vlan) |
239 | { |
240 | bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED; |
241 | struct dsa_loop_priv *ps = ds->priv; |
242 | u16 pvid = ps->ports[port].pvid; |
243 | struct mii_bus *bus = ps->bus; |
244 | struct dsa_loop_vlan *vl; |
245 | |
246 | /* Just do a sleeping operation to make lockdep checks effective */ |
247 | mdiobus_read(bus, addr: ps->port_base + port, MII_BMSR); |
248 | |
249 | vl = &ps->vlans[vlan->vid]; |
250 | |
251 | vl->members &= ~BIT(port); |
252 | if (untagged) |
253 | vl->untagged &= ~BIT(port); |
254 | |
255 | if (pvid == vlan->vid) |
256 | pvid = 1; |
257 | |
258 | dev_dbg(ds->dev, "%s: port: %d vlan: %d, %stagged, pvid: %d\n" , |
259 | __func__, port, vlan->vid, untagged ? "un" : "" , pvid); |
260 | ps->ports[port].pvid = pvid; |
261 | |
262 | return 0; |
263 | } |
264 | |
265 | static int dsa_loop_port_change_mtu(struct dsa_switch *ds, int port, |
266 | int new_mtu) |
267 | { |
268 | struct dsa_loop_priv *priv = ds->priv; |
269 | |
270 | priv->ports[port].mtu = new_mtu; |
271 | |
272 | return 0; |
273 | } |
274 | |
275 | static int dsa_loop_port_max_mtu(struct dsa_switch *ds, int port) |
276 | { |
277 | return ETH_MAX_MTU; |
278 | } |
279 | |
280 | static void dsa_loop_phylink_get_caps(struct dsa_switch *dsa, int port, |
281 | struct phylink_config *config) |
282 | { |
283 | bitmap_fill(dst: config->supported_interfaces, nbits: PHY_INTERFACE_MODE_MAX); |
284 | __clear_bit(PHY_INTERFACE_MODE_NA, config->supported_interfaces); |
285 | config->mac_capabilities = ~0; |
286 | } |
287 | |
288 | static const struct dsa_switch_ops dsa_loop_driver = { |
289 | .get_tag_protocol = dsa_loop_get_protocol, |
290 | .setup = dsa_loop_setup, |
291 | .teardown = dsa_loop_teardown, |
292 | .get_strings = dsa_loop_get_strings, |
293 | .get_ethtool_stats = dsa_loop_get_ethtool_stats, |
294 | .get_sset_count = dsa_loop_get_sset_count, |
295 | .get_ethtool_phy_stats = dsa_loop_get_ethtool_stats, |
296 | .phy_read = dsa_loop_phy_read, |
297 | .phy_write = dsa_loop_phy_write, |
298 | .port_bridge_join = dsa_loop_port_bridge_join, |
299 | .port_bridge_leave = dsa_loop_port_bridge_leave, |
300 | .port_stp_state_set = dsa_loop_port_stp_state_set, |
301 | .port_vlan_filtering = dsa_loop_port_vlan_filtering, |
302 | .port_vlan_add = dsa_loop_port_vlan_add, |
303 | .port_vlan_del = dsa_loop_port_vlan_del, |
304 | .port_change_mtu = dsa_loop_port_change_mtu, |
305 | .port_max_mtu = dsa_loop_port_max_mtu, |
306 | .phylink_get_caps = dsa_loop_phylink_get_caps, |
307 | }; |
308 | |
309 | static int dsa_loop_drv_probe(struct mdio_device *mdiodev) |
310 | { |
311 | struct dsa_loop_pdata *pdata = mdiodev->dev.platform_data; |
312 | struct dsa_loop_priv *ps; |
313 | struct dsa_switch *ds; |
314 | int ret; |
315 | |
316 | if (!pdata) |
317 | return -ENODEV; |
318 | |
319 | ds = devm_kzalloc(dev: &mdiodev->dev, size: sizeof(*ds), GFP_KERNEL); |
320 | if (!ds) |
321 | return -ENOMEM; |
322 | |
323 | ds->dev = &mdiodev->dev; |
324 | ds->num_ports = DSA_LOOP_NUM_PORTS; |
325 | |
326 | ps = devm_kzalloc(dev: &mdiodev->dev, size: sizeof(*ps), GFP_KERNEL); |
327 | if (!ps) |
328 | return -ENOMEM; |
329 | |
330 | ps->netdev = dev_get_by_name(net: &init_net, name: pdata->netdev); |
331 | if (!ps->netdev) |
332 | return -EPROBE_DEFER; |
333 | |
334 | pdata->cd.netdev[DSA_LOOP_CPU_PORT] = &ps->netdev->dev; |
335 | |
336 | ds->dev = &mdiodev->dev; |
337 | ds->ops = &dsa_loop_driver; |
338 | ds->priv = ps; |
339 | ps->bus = mdiodev->bus; |
340 | |
341 | dev_set_drvdata(dev: &mdiodev->dev, data: ds); |
342 | |
343 | ret = dsa_register_switch(ds); |
344 | if (!ret) |
345 | dev_info(&mdiodev->dev, "%s: 0x%0x\n" , |
346 | pdata->name, pdata->enabled_ports); |
347 | |
348 | return ret; |
349 | } |
350 | |
351 | static void dsa_loop_drv_remove(struct mdio_device *mdiodev) |
352 | { |
353 | struct dsa_switch *ds = dev_get_drvdata(dev: &mdiodev->dev); |
354 | struct dsa_loop_priv *ps; |
355 | |
356 | if (!ds) |
357 | return; |
358 | |
359 | ps = ds->priv; |
360 | |
361 | dsa_unregister_switch(ds); |
362 | dev_put(dev: ps->netdev); |
363 | } |
364 | |
365 | static void dsa_loop_drv_shutdown(struct mdio_device *mdiodev) |
366 | { |
367 | struct dsa_switch *ds = dev_get_drvdata(dev: &mdiodev->dev); |
368 | |
369 | if (!ds) |
370 | return; |
371 | |
372 | dsa_switch_shutdown(ds); |
373 | |
374 | dev_set_drvdata(dev: &mdiodev->dev, NULL); |
375 | } |
376 | |
377 | static struct mdio_driver dsa_loop_drv = { |
378 | .mdiodrv.driver = { |
379 | .name = "dsa-loop" , |
380 | }, |
381 | .probe = dsa_loop_drv_probe, |
382 | .remove = dsa_loop_drv_remove, |
383 | .shutdown = dsa_loop_drv_shutdown, |
384 | }; |
385 | |
386 | #define NUM_FIXED_PHYS (DSA_LOOP_NUM_PORTS - 2) |
387 | |
388 | static void dsa_loop_phydevs_unregister(void) |
389 | { |
390 | unsigned int i; |
391 | |
392 | for (i = 0; i < NUM_FIXED_PHYS; i++) |
393 | if (!IS_ERR(ptr: phydevs[i])) { |
394 | fixed_phy_unregister(phydev: phydevs[i]); |
395 | phy_device_free(phydev: phydevs[i]); |
396 | } |
397 | } |
398 | |
399 | static int __init dsa_loop_init(void) |
400 | { |
401 | struct fixed_phy_status status = { |
402 | .link = 1, |
403 | .speed = SPEED_100, |
404 | .duplex = DUPLEX_FULL, |
405 | }; |
406 | unsigned int i, ret; |
407 | |
408 | for (i = 0; i < NUM_FIXED_PHYS; i++) |
409 | phydevs[i] = fixed_phy_register(PHY_POLL, status: &status, NULL); |
410 | |
411 | ret = mdio_driver_register(drv: &dsa_loop_drv); |
412 | if (ret) |
413 | dsa_loop_phydevs_unregister(); |
414 | |
415 | return ret; |
416 | } |
417 | module_init(dsa_loop_init); |
418 | |
419 | static void __exit dsa_loop_exit(void) |
420 | { |
421 | mdio_driver_unregister(drv: &dsa_loop_drv); |
422 | dsa_loop_phydevs_unregister(); |
423 | } |
424 | module_exit(dsa_loop_exit); |
425 | |
426 | MODULE_SOFTDEP("pre: dsa_loop_bdinfo" ); |
427 | MODULE_LICENSE("GPL" ); |
428 | MODULE_AUTHOR("Florian Fainelli" ); |
429 | MODULE_DESCRIPTION("DSA loopback driver" ); |
430 | |