• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
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]	= { "phy_read_err", },
25 	[DSA_LOOP_PHY_WRITE_OK] = { "phy_write_ok", },
26 	[DSA_LOOP_PHY_WRITE_ERR] = { "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 
dsa_loop_devlink_vtu_get(void * priv)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 
dsa_loop_setup_devlink_resources(struct dsa_switch * ds)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, ARRAY_SIZE(ps->vlans),
57 					  ARRAY_SIZE(ps->vlans),
58 					  1, DEVLINK_RESOURCE_UNIT_ENTRY);
59 
60 	err = dsa_devlink_resource_register(ds, "VTU", ARRAY_SIZE(ps->vlans),
61 					    DSA_LOOP_DEVLINK_PARAM_ID_VTU,
62 					    DEVLINK_RESOURCE_ID_PARENT_TOP,
63 					    &size_params);
64 	if (err)
65 		goto out;
66 
67 	dsa_devlink_resource_occ_get_register(ds,
68 					      DSA_LOOP_DEVLINK_PARAM_ID_VTU,
69 					      dsa_loop_devlink_vtu_get, ps);
70 
71 	return 0;
72 
73 out:
74 	dsa_devlink_resources_unregister(ds);
75 	return err;
76 }
77 
dsa_loop_get_protocol(struct dsa_switch * ds,int port,enum dsa_tag_protocol mp)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 
dsa_loop_setup(struct dsa_switch * ds)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 
dsa_loop_teardown(struct dsa_switch * ds)101 static void dsa_loop_teardown(struct dsa_switch *ds)
102 {
103 	dsa_devlink_resources_unregister(ds);
104 }
105 
dsa_loop_get_sset_count(struct dsa_switch * ds,int port,int sset)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 
dsa_loop_get_strings(struct dsa_switch * ds,int port,u32 stringset,uint8_t * data)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 
dsa_loop_get_ethtool_stats(struct dsa_switch * ds,int port,uint64_t * data)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 
dsa_loop_phy_read(struct dsa_switch * ds,int port,int regnum)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, 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 
dsa_loop_phy_write(struct dsa_switch * ds,int port,int regnum,u16 value)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, ps->port_base + port, regnum, 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 
dsa_loop_port_bridge_join(struct dsa_switch * ds,int port,struct net_device * bridge)169 static int dsa_loop_port_bridge_join(struct dsa_switch *ds, int port,
170 				     struct net_device *bridge)
171 {
172 	dev_dbg(ds->dev, "%s: port: %d, bridge: %s\n",
173 		__func__, port, bridge->name);
174 
175 	return 0;
176 }
177 
dsa_loop_port_bridge_leave(struct dsa_switch * ds,int port,struct net_device * bridge)178 static void dsa_loop_port_bridge_leave(struct dsa_switch *ds, int port,
179 				       struct net_device *bridge)
180 {
181 	dev_dbg(ds->dev, "%s: port: %d, bridge: %s\n",
182 		__func__, port, bridge->name);
183 }
184 
dsa_loop_port_stp_state_set(struct dsa_switch * ds,int port,u8 state)185 static void dsa_loop_port_stp_state_set(struct dsa_switch *ds, int port,
186 					u8 state)
187 {
188 	dev_dbg(ds->dev, "%s: port: %d, state: %d\n",
189 		__func__, port, state);
190 }
191 
dsa_loop_port_vlan_filtering(struct dsa_switch * ds,int port,bool vlan_filtering,struct switchdev_trans * trans)192 static int dsa_loop_port_vlan_filtering(struct dsa_switch *ds, int port,
193 					bool vlan_filtering,
194 					struct switchdev_trans *trans)
195 {
196 	dev_dbg(ds->dev, "%s: port: %d, vlan_filtering: %d\n",
197 		__func__, port, vlan_filtering);
198 
199 	return 0;
200 }
201 
202 static int
dsa_loop_port_vlan_prepare(struct dsa_switch * ds,int port,const struct switchdev_obj_port_vlan * vlan)203 dsa_loop_port_vlan_prepare(struct dsa_switch *ds, int port,
204 			   const struct switchdev_obj_port_vlan *vlan)
205 {
206 	struct dsa_loop_priv *ps = ds->priv;
207 	struct mii_bus *bus = ps->bus;
208 
209 	dev_dbg(ds->dev, "%s: port: %d, vlan: %d-%d",
210 		__func__, port, vlan->vid_begin, vlan->vid_end);
211 
212 	/* Just do a sleeping operation to make lockdep checks effective */
213 	mdiobus_read(bus, ps->port_base + port, MII_BMSR);
214 
215 	if (vlan->vid_end > ARRAY_SIZE(ps->vlans))
216 		return -ERANGE;
217 
218 	return 0;
219 }
220 
dsa_loop_port_vlan_add(struct dsa_switch * ds,int port,const struct switchdev_obj_port_vlan * vlan)221 static void dsa_loop_port_vlan_add(struct dsa_switch *ds, int port,
222 				   const struct switchdev_obj_port_vlan *vlan)
223 {
224 	bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
225 	bool pvid = vlan->flags & BRIDGE_VLAN_INFO_PVID;
226 	struct dsa_loop_priv *ps = ds->priv;
227 	struct mii_bus *bus = ps->bus;
228 	struct dsa_loop_vlan *vl;
229 	u16 vid;
230 
231 	/* Just do a sleeping operation to make lockdep checks effective */
232 	mdiobus_read(bus, ps->port_base + port, MII_BMSR);
233 
234 	for (vid = vlan->vid_begin; vid <= vlan->vid_end; ++vid) {
235 		vl = &ps->vlans[vid];
236 
237 		vl->members |= BIT(port);
238 		if (untagged)
239 			vl->untagged |= BIT(port);
240 		else
241 			vl->untagged &= ~BIT(port);
242 
243 		dev_dbg(ds->dev, "%s: port: %d vlan: %d, %stagged, pvid: %d\n",
244 			__func__, port, vid, untagged ? "un" : "", pvid);
245 	}
246 
247 	if (pvid)
248 		ps->ports[port].pvid = vid;
249 }
250 
dsa_loop_port_vlan_del(struct dsa_switch * ds,int port,const struct switchdev_obj_port_vlan * vlan)251 static int dsa_loop_port_vlan_del(struct dsa_switch *ds, int port,
252 				  const struct switchdev_obj_port_vlan *vlan)
253 {
254 	bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
255 	struct dsa_loop_priv *ps = ds->priv;
256 	struct mii_bus *bus = ps->bus;
257 	struct dsa_loop_vlan *vl;
258 	u16 vid, pvid = ps->ports[port].pvid;
259 
260 	/* Just do a sleeping operation to make lockdep checks effective */
261 	mdiobus_read(bus, ps->port_base + port, MII_BMSR);
262 
263 	for (vid = vlan->vid_begin; vid <= vlan->vid_end; ++vid) {
264 		vl = &ps->vlans[vid];
265 
266 		vl->members &= ~BIT(port);
267 		if (untagged)
268 			vl->untagged &= ~BIT(port);
269 
270 		if (pvid == vid)
271 			pvid = 1;
272 
273 		dev_dbg(ds->dev, "%s: port: %d vlan: %d, %stagged, pvid: %d\n",
274 			__func__, port, vid, untagged ? "un" : "", pvid);
275 	}
276 	ps->ports[port].pvid = pvid;
277 
278 	return 0;
279 }
280 
dsa_loop_port_change_mtu(struct dsa_switch * ds,int port,int new_mtu)281 static int dsa_loop_port_change_mtu(struct dsa_switch *ds, int port,
282 				    int new_mtu)
283 {
284 	struct dsa_loop_priv *priv = ds->priv;
285 
286 	priv->ports[port].mtu = new_mtu;
287 
288 	return 0;
289 }
290 
dsa_loop_port_max_mtu(struct dsa_switch * ds,int port)291 static int dsa_loop_port_max_mtu(struct dsa_switch *ds, int port)
292 {
293 	return ETH_MAX_MTU;
294 }
295 
296 static const struct dsa_switch_ops dsa_loop_driver = {
297 	.get_tag_protocol	= dsa_loop_get_protocol,
298 	.setup			= dsa_loop_setup,
299 	.teardown		= dsa_loop_teardown,
300 	.get_strings		= dsa_loop_get_strings,
301 	.get_ethtool_stats	= dsa_loop_get_ethtool_stats,
302 	.get_sset_count		= dsa_loop_get_sset_count,
303 	.get_ethtool_phy_stats	= dsa_loop_get_ethtool_stats,
304 	.phy_read		= dsa_loop_phy_read,
305 	.phy_write		= dsa_loop_phy_write,
306 	.port_bridge_join	= dsa_loop_port_bridge_join,
307 	.port_bridge_leave	= dsa_loop_port_bridge_leave,
308 	.port_stp_state_set	= dsa_loop_port_stp_state_set,
309 	.port_vlan_filtering	= dsa_loop_port_vlan_filtering,
310 	.port_vlan_prepare	= dsa_loop_port_vlan_prepare,
311 	.port_vlan_add		= dsa_loop_port_vlan_add,
312 	.port_vlan_del		= dsa_loop_port_vlan_del,
313 	.port_change_mtu	= dsa_loop_port_change_mtu,
314 	.port_max_mtu		= dsa_loop_port_max_mtu,
315 };
316 
dsa_loop_drv_probe(struct mdio_device * mdiodev)317 static int dsa_loop_drv_probe(struct mdio_device *mdiodev)
318 {
319 	struct dsa_loop_pdata *pdata = mdiodev->dev.platform_data;
320 	struct dsa_loop_priv *ps;
321 	struct dsa_switch *ds;
322 	int ret;
323 
324 	if (!pdata)
325 		return -ENODEV;
326 
327 	ds = devm_kzalloc(&mdiodev->dev, sizeof(*ds), GFP_KERNEL);
328 	if (!ds)
329 		return -ENOMEM;
330 
331 	ds->dev = &mdiodev->dev;
332 	ds->num_ports = DSA_LOOP_NUM_PORTS;
333 
334 	ps = devm_kzalloc(&mdiodev->dev, sizeof(*ps), GFP_KERNEL);
335 	if (!ps)
336 		return -ENOMEM;
337 
338 	ps->netdev = dev_get_by_name(&init_net, pdata->netdev);
339 	if (!ps->netdev)
340 		return -EPROBE_DEFER;
341 
342 	pdata->cd.netdev[DSA_LOOP_CPU_PORT] = &ps->netdev->dev;
343 
344 	ds->dev = &mdiodev->dev;
345 	ds->ops = &dsa_loop_driver;
346 	ds->priv = ps;
347 	ds->configure_vlan_while_not_filtering = true;
348 	ps->bus = mdiodev->bus;
349 
350 	dev_set_drvdata(&mdiodev->dev, ds);
351 
352 	ret = dsa_register_switch(ds);
353 	if (!ret)
354 		dev_info(&mdiodev->dev, "%s: 0x%0x\n",
355 			 pdata->name, pdata->enabled_ports);
356 
357 	return ret;
358 }
359 
dsa_loop_drv_remove(struct mdio_device * mdiodev)360 static void dsa_loop_drv_remove(struct mdio_device *mdiodev)
361 {
362 	struct dsa_switch *ds = dev_get_drvdata(&mdiodev->dev);
363 	struct dsa_loop_priv *ps = ds->priv;
364 
365 	dsa_unregister_switch(ds);
366 	dev_put(ps->netdev);
367 }
368 
369 static struct mdio_driver dsa_loop_drv = {
370 	.mdiodrv.driver	= {
371 		.name	= "dsa-loop",
372 	},
373 	.probe	= dsa_loop_drv_probe,
374 	.remove	= dsa_loop_drv_remove,
375 };
376 
377 #define NUM_FIXED_PHYS	(DSA_LOOP_NUM_PORTS - 2)
378 
dsa_loop_phydevs_unregister(void)379 static void dsa_loop_phydevs_unregister(void)
380 {
381 	unsigned int i;
382 
383 	for (i = 0; i < NUM_FIXED_PHYS; i++)
384 		if (!IS_ERR(phydevs[i])) {
385 			fixed_phy_unregister(phydevs[i]);
386 			phy_device_free(phydevs[i]);
387 		}
388 }
389 
dsa_loop_init(void)390 static int __init dsa_loop_init(void)
391 {
392 	struct fixed_phy_status status = {
393 		.link = 1,
394 		.speed = SPEED_100,
395 		.duplex = DUPLEX_FULL,
396 	};
397 	unsigned int i, ret;
398 
399 	for (i = 0; i < NUM_FIXED_PHYS; i++)
400 		phydevs[i] = fixed_phy_register(PHY_POLL, &status, NULL);
401 
402 	ret = mdio_driver_register(&dsa_loop_drv);
403 	if (ret)
404 		dsa_loop_phydevs_unregister();
405 
406 	return ret;
407 }
408 module_init(dsa_loop_init);
409 
dsa_loop_exit(void)410 static void __exit dsa_loop_exit(void)
411 {
412 	mdio_driver_unregister(&dsa_loop_drv);
413 	dsa_loop_phydevs_unregister();
414 }
415 module_exit(dsa_loop_exit);
416 
417 MODULE_SOFTDEP("pre: dsa_loop_bdinfo");
418 MODULE_LICENSE("GPL");
419 MODULE_AUTHOR("Florian Fainelli");
420 MODULE_DESCRIPTION("DSA loopback driver");
421