xref: /openbmc/linux/drivers/net/dsa/dsa_loop.c (revision ae40e94f)
1 /*
2  * Distributed Switch Architecture loopback driver
3  *
4  * Copyright (C) 2016, Florian Fainelli <f.fainelli@gmail.com>
5  *
6  * This program is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
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 
25 struct dsa_loop_vlan {
26 	u16 members;
27 	u16 untagged;
28 };
29 
30 struct dsa_loop_mib_entry {
31 	char name[ETH_GSTRING_LEN];
32 	unsigned long val;
33 };
34 
35 enum 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 
43 static 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 
50 struct dsa_loop_port {
51 	struct dsa_loop_mib_entry mib[__DSA_LOOP_CNT_MAX];
52 };
53 
54 #define DSA_LOOP_VLANS	5
55 
56 struct 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 
65 static struct phy_device *phydevs[PHY_MAX_ADDR];
66 
67 static 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 
75 static 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 
89 static 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 
97 static 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 
111 static 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 
121 static 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 
136 static 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 
152 static 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 
161 static 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 
168 static 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 
175 static 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 
184 static int
185 dsa_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 	/* Just do a sleeping operation to make lockdep checks effective */
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 
203 static 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 	/* Just do a sleeping operation to make lockdep checks effective */
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 
233 static 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 	/* Just do a sleeping operation to make lockdep checks effective */
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 
263 static 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 
281 static 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 
317 static 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 
326 static 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 
336 static 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, NULL);
347 
348 	return mdio_driver_register(&dsa_loop_drv);
349 }
350 module_init(dsa_loop_init);
351 
352 static 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 }
361 module_exit(dsa_loop_exit);
362 
363 MODULE_LICENSE("GPL");
364 MODULE_AUTHOR("Florian Fainelli");
365 MODULE_DESCRIPTION("DSA loopback driver");
366