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