~hc/RK356X_SDK_RELEASE.git

..	..	@@ -1,13 +1,9 @@
	1	+// SPDX-License-Identifier: GPL-2.0-or-later
1	2	/*
2	3	* net/dsa/dsa2.c - Hardware switch handling, binding version 2
3	4	* Copyright (c) 2008-2009 Marvell Semiconductor
4	5	* Copyright (c) 2013 Florian Fainelli <florian@openwrt.org>
5	6	* Copyright (c) 2016 Andrew Lunn <andrew@lunn.ch>
6		- *
7		- * This program is free software; you can redistribute it and/or modify
8		- * it under the terms of the GNU General Public License as published by
9		- * the Free Software Foundation; either version 2 of the License, or
10		- * (at your option) any later version.
11	7	*/
12	8
13	9	#include <linux/device.h>
..	..	@@ -18,14 +14,33 @@
18	14	#include <linux/rtnetlink.h>
19	15	#include <linux/of.h>
20	16	#include <linux/of_net.h>
	17	+#include <net/devlink.h>
21	18
22	19	#include "dsa_priv.h"
23	20
24		-static LIST_HEAD(dsa_tree_list);
25	21	static DEFINE_MUTEX(dsa2_mutex);
	22	+LIST_HEAD(dsa_tree_list);
26	23
27		-static const struct devlink_ops dsa_devlink_ops = {
28		-};
	24	+struct dsa_switch *dsa_switch_find(int tree_index, int sw_index)
	25	+{
	26	+ struct dsa_switch_tree *dst;
	27	+ struct dsa_port *dp;
	28	+
	29	+ list_for_each_entry(dst, &dsa_tree_list, list) {
	30	+ if (dst->index != tree_index)
	31	+ continue;
	32	+
	33	+ list_for_each_entry(dp, &dst->ports, list) {
	34	+ if (dp->ds->index != sw_index)
	35	+ continue;
	36	+
	37	+ return dp->ds;
	38	+ }
	39	+ }
	40	+
	41	+ return NULL;
	42	+}
	43	+EXPORT_SYMBOL_GPL(dsa_switch_find);
29	44
30	45	static struct dsa_switch_tree *dsa_tree_find(int index)
31	46	{
..	..	@@ -47,6 +62,10 @@
47	62	return NULL;
48	63
49	64	dst->index = index;
	65	+
	66	+ INIT_LIST_HEAD(&dst->rtable);
	67	+
	68	+ INIT_LIST_HEAD(&dst->ports);
50	69
51	70	INIT_LIST_HEAD(&dst->list);
52	71	list_add_tail(&dst->list, &dsa_tree_list);
..	..	@@ -114,24 +133,39 @@
114	133	static struct dsa_port dsa_tree_find_port_by_node(struct dsa_switch_tree dst,
115	134	struct device_node *dn)
116	135	{
117		- struct dsa_switch *ds;
118	136	struct dsa_port *dp;
119		- int device, port;
120	137
121		- for (device = 0; device < DSA_MAX_SWITCHES; device++) {
122		- ds = dst->ds[device];
123		- if (!ds)
124		- continue;
125		-
126		- for (port = 0; port < ds->num_ports; port++) {
127		- dp = &ds->ports[port];
128		-
129		- if (dp->dn == dn)
130		- return dp;
131		- }
132		- }
	138	+ list_for_each_entry(dp, &dst->ports, list)
	139	+ if (dp->dn == dn)
	140	+ return dp;
133	141
134	142	return NULL;
	143	+}
	144	+
	145	+static struct dsa_link dsa_link_touch(struct dsa_port dp,
	146	+ struct dsa_port *link_dp)
	147	+{
	148	+ struct dsa_switch *ds = dp->ds;
	149	+ struct dsa_switch_tree *dst;
	150	+ struct dsa_link *dl;
	151	+
	152	+ dst = ds->dst;
	153	+
	154	+ list_for_each_entry(dl, &dst->rtable, list)
	155	+ if (dl->dp == dp && dl->link_dp == link_dp)
	156	+ return dl;
	157	+
	158	+ dl = kzalloc(sizeof(*dl), GFP_KERNEL);
	159	+ if (!dl)
	160	+ return NULL;
	161	+
	162	+ dl->dp = dp;
	163	+ dl->link_dp = link_dp;
	164	+
	165	+ INIT_LIST_HEAD(&dl->list);
	166	+ list_add_tail(&dl->list, &dst->rtable);
	167	+
	168	+ return dl;
135	169	}
136	170
137	171	static bool dsa_port_setup_routing_table(struct dsa_port *dp)
..	..	@@ -141,6 +175,7 @@
141	175	struct device_node *dn = dp->dn;
142	176	struct of_phandle_iterator it;
143	177	struct dsa_port *link_dp;
	178	+ struct dsa_link *dl;
144	179	int err;
145	180
146	181	of_for_each_phandle(&it, err, dn, "link", NULL, 0) {
..	..	@@ -150,24 +185,22 @@
150	185	return false;
151	186	}
152	187
153		- ds->rtable[link_dp->ds->index] = dp->index;
	188	+ dl = dsa_link_touch(dp, link_dp);
	189	+ if (!dl) {
	190	+ of_node_put(it.node);
	191	+ return false;
	192	+ }
154	193	}
155	194
156	195	return true;
157	196	}
158	197
159		-static bool dsa_switch_setup_routing_table(struct dsa_switch *ds)
	198	+static bool dsa_tree_setup_routing_table(struct dsa_switch_tree *dst)
160	199	{
161	200	bool complete = true;
162	201	struct dsa_port *dp;
163		- int i;
164	202
165		- for (i = 0; i < DSA_MAX_SWITCHES; i++)
166		- ds->rtable[i] = DSA_RTABLE_NONE;
167		-
168		- for (i = 0; i < ds->num_ports; i++) {
169		- dp = &ds->ports[i];
170		-
	203	+ list_for_each_entry(dp, &dst->ports, list) {
171	204	if (dsa_port_is_dsa(dp)) {
172	205	complete = dsa_port_setup_routing_table(dp);
173	206	if (!complete)
..	..	@@ -178,156 +211,165 @@
178	211	return complete;
179	212	}
180	213
181		-static bool dsa_tree_setup_routing_table(struct dsa_switch_tree *dst)
182		-{
183		- struct dsa_switch *ds;
184		- bool complete = true;
185		- int device;
186		-
187		- for (device = 0; device < DSA_MAX_SWITCHES; device++) {
188		- ds = dst->ds[device];
189		- if (!ds)
190		- continue;
191		-
192		- complete = dsa_switch_setup_routing_table(ds);
193		- if (!complete)
194		- break;
195		- }
196		-
197		- return complete;
198		-}
199		-
200	214	static struct dsa_port dsa_tree_find_first_cpu(struct dsa_switch_tree dst)
201	215	{
202		- struct dsa_switch *ds;
203	216	struct dsa_port *dp;
204		- int device, port;
205	217
206		- for (device = 0; device < DSA_MAX_SWITCHES; device++) {
207		- ds = dst->ds[device];
208		- if (!ds)
209		- continue;
210		-
211		- for (port = 0; port < ds->num_ports; port++) {
212		- dp = &ds->ports[port];
213		-
214		- if (dsa_port_is_cpu(dp))
215		- return dp;
216		- }
217		- }
	218	+ list_for_each_entry(dp, &dst->ports, list)
	219	+ if (dsa_port_is_cpu(dp))
	220	+ return dp;
218	221
219	222	return NULL;
220	223	}
221	224
222	225	static int dsa_tree_setup_default_cpu(struct dsa_switch_tree *dst)
223	226	{
224		- struct dsa_switch *ds;
225		- struct dsa_port *dp;
226		- int device, port;
	227	+ struct dsa_port cpu_dp, dp;
227	228
228		- /* DSA currently only supports a single CPU port */
229		- dst->cpu_dp = dsa_tree_find_first_cpu(dst);
230		- if (!dst->cpu_dp) {
231		- pr_warn("Tree has no master device\n");
	229	+ cpu_dp = dsa_tree_find_first_cpu(dst);
	230	+ if (!cpu_dp) {
	231	+ pr_err("DSA: tree %d has no CPU port\n", dst->index);
232	232	return -EINVAL;
233	233	}
234	234
235	235	/* Assign the default CPU port to all ports of the fabric */
236		- for (device = 0; device < DSA_MAX_SWITCHES; device++) {
237		- ds = dst->ds[device];
238		- if (!ds)
239		- continue;
240		-
241		- for (port = 0; port < ds->num_ports; port++) {
242		- dp = &ds->ports[port];
243		-
244		- if (dsa_port_is_user(dp) \|\| dsa_port_is_dsa(dp))
245		- dp->cpu_dp = dst->cpu_dp;
246		- }
247		- }
	236	+ list_for_each_entry(dp, &dst->ports, list)
	237	+ if (dsa_port_is_user(dp) \|\| dsa_port_is_dsa(dp))
	238	+ dp->cpu_dp = cpu_dp;
248	239
249	240	return 0;
250	241	}
251	242
252	243	static void dsa_tree_teardown_default_cpu(struct dsa_switch_tree *dst)
253	244	{
254		- /* DSA currently only supports a single CPU port */
255		- dst->cpu_dp = NULL;
	245	+ struct dsa_port *dp;
	246	+
	247	+ list_for_each_entry(dp, &dst->ports, list)
	248	+ if (dsa_port_is_user(dp) \|\| dsa_port_is_dsa(dp))
	249	+ dp->cpu_dp = NULL;
256	250	}
257	251
258	252	static int dsa_port_setup(struct dsa_port *dp)
259	253	{
260		- struct dsa_switch *ds = dp->ds;
	254	+ struct devlink_port *dlp = &dp->devlink_port;
	255	+ bool dsa_port_link_registered = false;
	256	+ bool dsa_port_enabled = false;
261	257	int err = 0;
262	258
263		- memset(&dp->devlink_port, 0, sizeof(dp->devlink_port));
264		- dp->mac = of_get_mac_address(dp->dn);
265		-
266		- if (dp->type != DSA_PORT_TYPE_UNUSED)
267		- err = devlink_port_register(ds->devlink, &dp->devlink_port,
268		- dp->index);
269		- if (err)
270		- return err;
	259	+ if (dp->setup)
	260	+ return 0;
271	261
272	262	switch (dp->type) {
273	263	case DSA_PORT_TYPE_UNUSED:
	264	+ dsa_port_disable(dp);
274	265	break;
275	266	case DSA_PORT_TYPE_CPU:
276		- /* dp->index is used now as port_number. However
277		- * CPU ports should have separate numbering
278		- * independent from front panel port numbers.
279		- */
280		- devlink_port_attrs_set(&dp->devlink_port,
281		- DEVLINK_PORT_FLAVOUR_CPU,
282		- dp->index, false, 0);
283	267	err = dsa_port_link_register_of(dp);
284		- if (err) {
285		- dev_err(ds->dev, "failed to setup link for port %d.%d\n",
286		- ds->index, dp->index);
287		- return err;
288		- }
	268	+ if (err)
	269	+ break;
	270	+ dsa_port_link_registered = true;
	271	+
	272	+ err = dsa_port_enable(dp, NULL);
	273	+ if (err)
	274	+ break;
	275	+ dsa_port_enabled = true;
	276	+
289	277	break;
290	278	case DSA_PORT_TYPE_DSA:
291		- /* dp->index is used now as port_number. However
292		- * DSA ports should have separate numbering
293		- * independent from front panel port numbers.
294		- */
295		- devlink_port_attrs_set(&dp->devlink_port,
296		- DEVLINK_PORT_FLAVOUR_DSA,
297		- dp->index, false, 0);
298	279	err = dsa_port_link_register_of(dp);
299		- if (err) {
300		- dev_err(ds->dev, "failed to setup link for port %d.%d\n",
301		- ds->index, dp->index);
302		- return err;
303		- }
	280	+ if (err)
	281	+ break;
	282	+ dsa_port_link_registered = true;
	283	+
	284	+ err = dsa_port_enable(dp, NULL);
	285	+ if (err)
	286	+ break;
	287	+ dsa_port_enabled = true;
	288	+
304	289	break;
305	290	case DSA_PORT_TYPE_USER:
306		- devlink_port_attrs_set(&dp->devlink_port,
307		- DEVLINK_PORT_FLAVOUR_PHYSICAL,
308		- dp->index, false, 0);
	291	+ dp->mac = of_get_mac_address(dp->dn);
309	292	err = dsa_slave_create(dp);
310	293	if (err)
311		- dev_err(ds->dev, "failed to create slave for port %d.%d\n",
312		- ds->index, dp->index);
313		- else
314		- devlink_port_type_eth_set(&dp->devlink_port, dp->slave);
	294	+ break;
	295	+
	296	+ devlink_port_type_eth_set(dlp, dp->slave);
315	297	break;
316	298	}
	299	+
	300	+ if (err && dsa_port_enabled)
	301	+ dsa_port_disable(dp);
	302	+ if (err && dsa_port_link_registered)
	303	+ dsa_port_link_unregister_of(dp);
	304	+ if (err)
	305	+ return err;
	306	+
	307	+ dp->setup = true;
317	308
318	309	return 0;
319	310	}
320	311
	312	+static int dsa_port_devlink_setup(struct dsa_port *dp)
	313	+{
	314	+ struct devlink_port *dlp = &dp->devlink_port;
	315	+ struct dsa_switch_tree *dst = dp->ds->dst;
	316	+ struct devlink_port_attrs attrs = {};
	317	+ struct devlink *dl = dp->ds->devlink;
	318	+ const unsigned char *id;
	319	+ unsigned char len;
	320	+ int err;
	321	+
	322	+ id = (const unsigned char *)&dst->index;
	323	+ len = sizeof(dst->index);
	324	+
	325	+ attrs.phys.port_number = dp->index;
	326	+ memcpy(attrs.switch_id.id, id, len);
	327	+ attrs.switch_id.id_len = len;
	328	+ memset(dlp, 0, sizeof(*dlp));
	329	+
	330	+ switch (dp->type) {
	331	+ case DSA_PORT_TYPE_UNUSED:
	332	+ attrs.flavour = DEVLINK_PORT_FLAVOUR_UNUSED;
	333	+ break;
	334	+ case DSA_PORT_TYPE_CPU:
	335	+ attrs.flavour = DEVLINK_PORT_FLAVOUR_CPU;
	336	+ break;
	337	+ case DSA_PORT_TYPE_DSA:
	338	+ attrs.flavour = DEVLINK_PORT_FLAVOUR_DSA;
	339	+ break;
	340	+ case DSA_PORT_TYPE_USER:
	341	+ attrs.flavour = DEVLINK_PORT_FLAVOUR_PHYSICAL;
	342	+ break;
	343	+ }
	344	+
	345	+ devlink_port_attrs_set(dlp, &attrs);
	346	+ err = devlink_port_register(dl, dlp, dp->index);
	347	+
	348	+ if (!err)
	349	+ dp->devlink_port_setup = true;
	350	+
	351	+ return err;
	352	+}
	353	+
321	354	static void dsa_port_teardown(struct dsa_port *dp)
322	355	{
323		- if (dp->type != DSA_PORT_TYPE_UNUSED)
324		- devlink_port_unregister(&dp->devlink_port);
	356	+ struct devlink_port *dlp = &dp->devlink_port;
	357	+
	358	+ if (!dp->setup)
	359	+ return;
	360	+
	361	+ devlink_port_type_clear(dlp);
325	362
326	363	switch (dp->type) {
327	364	case DSA_PORT_TYPE_UNUSED:
328	365	break;
329	366	case DSA_PORT_TYPE_CPU:
	367	+ dsa_port_disable(dp);
	368	+ dsa_tag_driver_put(dp->tag_ops);
	369	+ dsa_port_link_unregister_of(dp);
	370	+ break;
330	371	case DSA_PORT_TYPE_DSA:
	372	+ dsa_port_disable(dp);
331	373	dsa_port_link_unregister_of(dp);
332	374	break;
333	375	case DSA_PORT_TYPE_USER:
..	..	@@ -337,11 +379,43 @@
337	379	}
338	380	break;
339	381	}
	382	+
	383	+ dp->setup = false;
340	384	}
	385	+
	386	+static void dsa_port_devlink_teardown(struct dsa_port *dp)
	387	+{
	388	+ struct devlink_port *dlp = &dp->devlink_port;
	389	+
	390	+ if (dp->devlink_port_setup)
	391	+ devlink_port_unregister(dlp);
	392	+ dp->devlink_port_setup = false;
	393	+}
	394	+
	395	+static int dsa_devlink_info_get(struct devlink *dl,
	396	+ struct devlink_info_req *req,
	397	+ struct netlink_ext_ack *extack)
	398	+{
	399	+ struct dsa_switch *ds = dsa_devlink_to_ds(dl);
	400	+
	401	+ if (ds->ops->devlink_info_get)
	402	+ return ds->ops->devlink_info_get(ds, req, extack);
	403	+
	404	+ return -EOPNOTSUPP;
	405	+}
	406	+
	407	+static const struct devlink_ops dsa_devlink_ops = {
	408	+ .info_get = dsa_devlink_info_get,
	409	+};
341	410
342	411	static int dsa_switch_setup(struct dsa_switch *ds)
343	412	{
	413	+ struct dsa_devlink_priv *dl_priv;
	414	+ struct dsa_port *dp;
344	415	int err;
	416	+
	417	+ if (ds->setup)
	418	+ return 0;
345	419
346	420	/* Initialize ds->phys_mii_mask before registering the slave MDIO bus
347	421	* driver and before ops->setup() has run, since the switch drivers and
..	..	@@ -353,72 +427,159 @@
353	427	/* Add the switch to devlink before calling setup, so that setup can
354	428	* add dpipe tables
355	429	*/
356		- ds->devlink = devlink_alloc(&dsa_devlink_ops, 0);
	430	+ ds->devlink = devlink_alloc(&dsa_devlink_ops, sizeof(*dl_priv));
357	431	if (!ds->devlink)
358	432	return -ENOMEM;
	433	+ dl_priv = devlink_priv(ds->devlink);
	434	+ dl_priv->ds = ds;
359	435
360	436	err = devlink_register(ds->devlink, ds->dev);
361	437	if (err)
362		- return err;
	438	+ goto free_devlink;
363	439
364		- err = ds->ops->setup(ds);
365		- if (err < 0)
366		- return err;
	440	+ /* Setup devlink port instances now, so that the switch
	441	+ * setup() can register regions etc, against the ports
	442	+ */
	443	+ list_for_each_entry(dp, &ds->dst->ports, list) {
	444	+ if (dp->ds == ds) {
	445	+ err = dsa_port_devlink_setup(dp);
	446	+ if (err)
	447	+ goto unregister_devlink_ports;
	448	+ }
	449	+ }
367	450
368	451	err = dsa_switch_register_notifier(ds);
369	452	if (err)
370		- return err;
	453	+ goto unregister_devlink_ports;
	454	+
	455	+ err = ds->ops->setup(ds);
	456	+ if (err < 0)
	457	+ goto unregister_notifier;
	458	+
	459	+ devlink_params_publish(ds->devlink);
371	460
372	461	if (!ds->slave_mii_bus && ds->ops->phy_read) {
373		- ds->slave_mii_bus = devm_mdiobus_alloc(ds->dev);
374		- if (!ds->slave_mii_bus)
375		- return -ENOMEM;
	462	+ ds->slave_mii_bus = mdiobus_alloc();
	463	+ if (!ds->slave_mii_bus) {
	464	+ err = -ENOMEM;
	465	+ goto teardown;
	466	+ }
376	467
377	468	dsa_slave_mii_bus_init(ds);
378	469
379	470	err = mdiobus_register(ds->slave_mii_bus);
380	471	if (err < 0)
381		- return err;
	472	+ goto free_slave_mii_bus;
382	473	}
383	474
	475	+ ds->setup = true;
	476	+
384	477	return 0;
	478	+
	479	+free_slave_mii_bus:
	480	+ if (ds->slave_mii_bus && ds->ops->phy_read)
	481	+ mdiobus_free(ds->slave_mii_bus);
	482	+teardown:
	483	+ if (ds->ops->teardown)
	484	+ ds->ops->teardown(ds);
	485	+unregister_notifier:
	486	+ dsa_switch_unregister_notifier(ds);
	487	+unregister_devlink_ports:
	488	+ list_for_each_entry(dp, &ds->dst->ports, list)
	489	+ if (dp->ds == ds)
	490	+ dsa_port_devlink_teardown(dp);
	491	+ devlink_unregister(ds->devlink);
	492	+free_devlink:
	493	+ devlink_free(ds->devlink);
	494	+ ds->devlink = NULL;
	495	+
	496	+ return err;
385	497	}
386	498
387	499	static void dsa_switch_teardown(struct dsa_switch *ds)
388	500	{
389		- if (ds->slave_mii_bus && ds->ops->phy_read)
	501	+ struct dsa_port *dp;
	502	+
	503	+ if (!ds->setup)
	504	+ return;
	505	+
	506	+ if (ds->slave_mii_bus && ds->ops->phy_read) {
390	507	mdiobus_unregister(ds->slave_mii_bus);
	508	+ mdiobus_free(ds->slave_mii_bus);
	509	+ ds->slave_mii_bus = NULL;
	510	+ }
391	511
392	512	dsa_switch_unregister_notifier(ds);
393	513
	514	+ if (ds->ops->teardown)
	515	+ ds->ops->teardown(ds);
	516	+
394	517	if (ds->devlink) {
	518	+ list_for_each_entry(dp, &ds->dst->ports, list)
	519	+ if (dp->ds == ds)
	520	+ dsa_port_devlink_teardown(dp);
395	521	devlink_unregister(ds->devlink);
396	522	devlink_free(ds->devlink);
397	523	ds->devlink = NULL;
398	524	}
399	525
	526	+ ds->setup = false;
400	527	}
401	528
402	529	static int dsa_tree_setup_switches(struct dsa_switch_tree *dst)
403	530	{
404		- struct dsa_switch *ds;
405	531	struct dsa_port *dp;
406		- int device, port;
407	532	int err;
408	533
409		- for (device = 0; device < DSA_MAX_SWITCHES; device++) {
410		- ds = dst->ds[device];
411		- if (!ds)
412		- continue;
413		-
414		- err = dsa_switch_setup(ds);
	534	+ list_for_each_entry(dp, &dst->ports, list) {
	535	+ err = dsa_switch_setup(dp->ds);
415	536	if (err)
	537	+ goto teardown;
	538	+ }
	539	+
	540	+ list_for_each_entry(dp, &dst->ports, list) {
	541	+ err = dsa_port_setup(dp);
	542	+ if (err) {
	543	+ dsa_port_devlink_teardown(dp);
	544	+ dp->type = DSA_PORT_TYPE_UNUSED;
	545	+ err = dsa_port_devlink_setup(dp);
	546	+ if (err)
	547	+ goto teardown;
416	548	continue;
	549	+ }
	550	+ }
417	551
418		- for (port = 0; port < ds->num_ports; port++) {
419		- dp = &ds->ports[port];
	552	+ return 0;
420	553
421		- err = dsa_port_setup(dp);
	554	+teardown:
	555	+ list_for_each_entry(dp, &dst->ports, list)
	556	+ dsa_port_teardown(dp);
	557	+
	558	+ list_for_each_entry(dp, &dst->ports, list)
	559	+ dsa_switch_teardown(dp->ds);
	560	+
	561	+ return err;
	562	+}
	563	+
	564	+static void dsa_tree_teardown_switches(struct dsa_switch_tree *dst)
	565	+{
	566	+ struct dsa_port *dp;
	567	+
	568	+ list_for_each_entry(dp, &dst->ports, list)
	569	+ dsa_port_teardown(dp);
	570	+
	571	+ list_for_each_entry(dp, &dst->ports, list)
	572	+ dsa_switch_teardown(dp->ds);
	573	+}
	574	+
	575	+static int dsa_tree_setup_master(struct dsa_switch_tree *dst)
	576	+{
	577	+ struct dsa_port *dp;
	578	+ int err;
	579	+
	580	+ list_for_each_entry(dp, &dst->ports, list) {
	581	+ if (dsa_port_is_cpu(dp)) {
	582	+ err = dsa_master_setup(dp->master, dp);
422	583	if (err)
423	584	return err;
424	585	}
..	..	@@ -427,42 +588,13 @@
427	588	return 0;
428	589	}
429	590
430		-static void dsa_tree_teardown_switches(struct dsa_switch_tree *dst)
431		-{
432		- struct dsa_switch *ds;
433		- struct dsa_port *dp;
434		- int device, port;
435		-
436		- for (device = 0; device < DSA_MAX_SWITCHES; device++) {
437		- ds = dst->ds[device];
438		- if (!ds)
439		- continue;
440		-
441		- for (port = 0; port < ds->num_ports; port++) {
442		- dp = &ds->ports[port];
443		-
444		- dsa_port_teardown(dp);
445		- }
446		-
447		- dsa_switch_teardown(ds);
448		- }
449		-}
450		-
451		-static int dsa_tree_setup_master(struct dsa_switch_tree *dst)
452		-{
453		- struct dsa_port *cpu_dp = dst->cpu_dp;
454		- struct net_device *master = cpu_dp->master;
455		-
456		- /* DSA currently supports a single pair of CPU port and master device */
457		- return dsa_master_setup(master, cpu_dp);
458		-}
459		-
460	591	static void dsa_tree_teardown_master(struct dsa_switch_tree *dst)
461	592	{
462		- struct dsa_port *cpu_dp = dst->cpu_dp;
463		- struct net_device *master = cpu_dp->master;
	593	+ struct dsa_port *dp;
464	594
465		- return dsa_master_teardown(master);
	595	+ list_for_each_entry(dp, &dst->ports, list)
	596	+ if (dsa_port_is_cpu(dp))
	597	+ dsa_master_teardown(dp->master);
466	598	}
467	599
468	600	static int dsa_tree_setup(struct dsa_switch_tree *dst)
..	..	@@ -486,21 +618,30 @@
486	618
487	619	err = dsa_tree_setup_switches(dst);
488	620	if (err)
489		- return err;
	621	+ goto teardown_default_cpu;
490	622
491	623	err = dsa_tree_setup_master(dst);
492	624	if (err)
493		- return err;
	625	+ goto teardown_switches;
494	626
495	627	dst->setup = true;
496	628
497	629	pr_info("DSA: tree %d setup\n", dst->index);
498	630
499	631	return 0;
	632	+
	633	+teardown_switches:
	634	+ dsa_tree_teardown_switches(dst);
	635	+teardown_default_cpu:
	636	+ dsa_tree_teardown_default_cpu(dst);
	637	+
	638	+ return err;
500	639	}
501	640
502	641	static void dsa_tree_teardown(struct dsa_switch_tree *dst)
503	642	{
	643	+ struct dsa_link dl, next;
	644	+
504	645	if (!dst->setup)
505	646	return;
506	647
..	..	@@ -510,37 +651,36 @@
510	651
511	652	dsa_tree_teardown_default_cpu(dst);
512	653
	654	+ list_for_each_entry_safe(dl, next, &dst->rtable, list) {
	655	+ list_del(&dl->list);
	656	+ kfree(dl);
	657	+ }
	658	+
513	659	pr_info("DSA: tree %d torn down\n", dst->index);
514	660
515	661	dst->setup = false;
516	662	}
517	663
518		-static void dsa_tree_remove_switch(struct dsa_switch_tree *dst,
519		- unsigned int index)
	664	+static struct dsa_port dsa_port_touch(struct dsa_switch ds, int index)
520	665	{
521		- dsa_tree_teardown(dst);
	666	+ struct dsa_switch_tree *dst = ds->dst;
	667	+ struct dsa_port *dp;
522	668
523		- dst->ds[index] = NULL;
524		- dsa_tree_put(dst);
525		-}
	669	+ list_for_each_entry(dp, &dst->ports, list)
	670	+ if (dp->ds == ds && dp->index == index)
	671	+ return dp;
526	672
527		-static int dsa_tree_add_switch(struct dsa_switch_tree *dst,
528		- struct dsa_switch *ds)
529		-{
530		- unsigned int index = ds->index;
531		- int err;
	673	+ dp = kzalloc(sizeof(*dp), GFP_KERNEL);
	674	+ if (!dp)
	675	+ return NULL;
532	676
533		- if (dst->ds[index])
534		- return -EBUSY;
	677	+ dp->ds = ds;
	678	+ dp->index = index;
535	679
536		- dsa_tree_get(dst);
537		- dst->ds[index] = ds;
	680	+ INIT_LIST_HEAD(&dp->list);
	681	+ list_add_tail(&dp->list, &dst->ports);
538	682
539		- err = dsa_tree_setup(dst);
540		- if (err)
541		- dsa_tree_remove_switch(dst, index);
542		-
543		- return err;
	683	+ return dp;
544	684	}
545	685
546	686	static int dsa_port_parse_user(struct dsa_port dp, const char name)
..	..	@@ -561,6 +701,32 @@
561	701	return 0;
562	702	}
563	703
	704	+static enum dsa_tag_protocol dsa_get_tag_protocol(struct dsa_port *dp,
	705	+ struct net_device *master)
	706	+{
	707	+ enum dsa_tag_protocol tag_protocol = DSA_TAG_PROTO_NONE;
	708	+ struct dsa_switch mds, ds = dp->ds;
	709	+ unsigned int mdp_upstream;
	710	+ struct dsa_port *mdp;
	711	+
	712	+ /* It is possible to stack DSA switches onto one another when that
	713	+ * happens the switch driver may want to know if its tagging protocol
	714	+ * is going to work in such a configuration.
	715	+ */
	716	+ if (dsa_slave_dev_check(master)) {
	717	+ mdp = dsa_slave_to_port(master);
	718	+ mds = mdp->ds;
	719	+ mdp_upstream = dsa_upstream_port(mds, mdp->index);
	720	+ tag_protocol = mds->ops->get_tag_protocol(mds, mdp_upstream,
	721	+ DSA_TAG_PROTO_NONE);
	722	+ }
	723	+
	724	+ /* If the master device is not itself a DSA slave in a disjoint DSA
	725	+ * tree, then return immediately.
	726	+ */
	727	+ return ds->ops->get_tag_protocol(ds, dp->index, tag_protocol);
	728	+}
	729	+
564	730	static int dsa_port_parse_cpu(struct dsa_port dp, struct net_device master)
565	731	{
566	732	struct dsa_switch *ds = dp->ds;
..	..	@@ -568,17 +734,21 @@
568	734	const struct dsa_device_ops *tag_ops;
569	735	enum dsa_tag_protocol tag_protocol;
570	736
571		- tag_protocol = ds->ops->get_tag_protocol(ds, dp->index);
572		- tag_ops = dsa_resolve_tag_protocol(tag_protocol);
	737	+ tag_protocol = dsa_get_tag_protocol(dp, master);
	738	+ tag_ops = dsa_tag_driver_get(tag_protocol);
573	739	if (IS_ERR(tag_ops)) {
	740	+ if (PTR_ERR(tag_ops) == -ENOPROTOOPT)
	741	+ return -EPROBE_DEFER;
574	742	dev_warn(ds->dev, "No tagger for this switch\n");
	743	+ dp->master = NULL;
575	744	return PTR_ERR(tag_ops);
576	745	}
577	746
	747	+ dp->master = master;
578	748	dp->type = DSA_PORT_TYPE_CPU;
	749	+ dp->filter = tag_ops->filter;
579	750	dp->rcv = tag_ops->rcv;
580	751	dp->tag_ops = tag_ops;
581		- dp->master = master;
582	752	dp->dst = dst;
583	753
584	754	return 0;
..	..	@@ -596,6 +766,7 @@
596	766	struct net_device *master;
597	767
598	768	master = of_find_net_device_by_node(ethernet);
	769	+ of_node_put(ethernet);
599	770	if (!master)
600	771	return -EPROBE_DEFER;
601	772
..	..	@@ -613,31 +784,39 @@
613	784	{
614	785	struct device_node ports, port;
615	786	struct dsa_port *dp;
	787	+ int err = 0;
616	788	u32 reg;
617		- int err;
618	789
619	790	ports = of_get_child_by_name(dn, "ports");
620	791	if (!ports) {
621		- dev_err(ds->dev, "no ports child node found\n");
622		- return -EINVAL;
	792	+ /* The second possibility is "ethernet-ports" */
	793	+ ports = of_get_child_by_name(dn, "ethernet-ports");
	794	+ if (!ports) {
	795	+ dev_err(ds->dev, "no ports child node found\n");
	796	+ return -EINVAL;
	797	+ }
623	798	}
624	799
625	800	for_each_available_child_of_node(ports, port) {
626	801	err = of_property_read_u32(port, "reg", &reg);
627	802	if (err)
628		- return err;
	803	+ goto out_put_node;
629	804
630		- if (reg >= ds->num_ports)
631		- return -EINVAL;
	805	+ if (reg >= ds->num_ports) {
	806	+ err = -EINVAL;
	807	+ goto out_put_node;
	808	+ }
632	809
633		- dp = &ds->ports[reg];
	810	+ dp = dsa_to_port(ds, reg);
634	811
635	812	err = dsa_port_parse_of(dp, port);
636	813	if (err)
637		- return err;
	814	+ goto out_put_node;
638	815	}
639	816
640		- return 0;
	817	+out_put_node:
	818	+ of_node_put(ports);
	819	+ return err;
641	820	}
642	821
643	822	static int dsa_switch_parse_member_of(struct dsa_switch *ds,
..	..	@@ -652,12 +831,24 @@
652	831	return sz;
653	832
654	833	ds->index = m[1];
655		- if (ds->index >= DSA_MAX_SWITCHES)
656		- return -EINVAL;
657	834
658	835	ds->dst = dsa_tree_touch(m[0]);
659	836	if (!ds->dst)
660	837	return -ENOMEM;
	838	+
	839	+ return 0;
	840	+}
	841	+
	842	+static int dsa_switch_touch_ports(struct dsa_switch *ds)
	843	+{
	844	+ struct dsa_port *dp;
	845	+ int port;
	846	+
	847	+ for (port = 0; port < ds->num_ports; port++) {
	848	+ dp = dsa_port_touch(ds, port);
	849	+ if (!dp)
	850	+ return -ENOMEM;
	851	+ }
661	852
662	853	return 0;
663	854	}
..	..	@@ -667,6 +858,10 @@
667	858	int err;
668	859
669	860	err = dsa_switch_parse_member_of(ds, dn);
	861	+ if (err)
	862	+ return err;
	863	+
	864	+ err = dsa_switch_touch_ports(ds);
670	865	if (err)
671	866	return err;
672	867
..	..	@@ -707,7 +902,7 @@
707	902	for (i = 0; i < DSA_MAX_PORTS; i++) {
708	903	name = cd->port_names[i];
709	904	dev = cd->netdev[i];
710		- dp = &ds->ports[i];
	905	+ dp = dsa_to_port(ds, i);
711	906
712	907	if (!name)
713	908	continue;
..	..	@@ -727,6 +922,8 @@
727	922
728	923	static int dsa_switch_parse(struct dsa_switch ds, struct dsa_chip_data cd)
729	924	{
	925	+ int err;
	926	+
730	927	ds->cd = cd;
731	928
732	929	/* We don't support interconnected switches nor multiple trees via
..	..	@@ -737,70 +934,67 @@
737	934	if (!ds->dst)
738	935	return -ENOMEM;
739	936
	937	+ err = dsa_switch_touch_ports(ds);
	938	+ if (err)
	939	+ return err;
	940	+
740	941	return dsa_switch_parse_ports(ds, cd);
741	942	}
742	943
743		-static int dsa_switch_add(struct dsa_switch *ds)
	944	+static void dsa_switch_release_ports(struct dsa_switch *ds)
744	945	{
745	946	struct dsa_switch_tree *dst = ds->dst;
	947	+ struct dsa_port dp, next;
746	948
747		- return dsa_tree_add_switch(dst, ds);
	949	+ list_for_each_entry_safe(dp, next, &dst->ports, list) {
	950	+ if (dp->ds != ds)
	951	+ continue;
	952	+ list_del(&dp->list);
	953	+ kfree(dp);
	954	+ }
748	955	}
749	956
750	957	static int dsa_switch_probe(struct dsa_switch *ds)
751	958	{
752		- struct dsa_chip_data *pdata = ds->dev->platform_data;
753		- struct device_node *np = ds->dev->of_node;
	959	+ struct dsa_switch_tree *dst;
	960	+ struct dsa_chip_data *pdata;
	961	+ struct device_node *np;
754	962	int err;
755	963
756		- if (np)
	964	+ if (!ds->dev)
	965	+ return -ENODEV;
	966	+
	967	+ pdata = ds->dev->platform_data;
	968	+ np = ds->dev->of_node;
	969	+
	970	+ if (!ds->num_ports)
	971	+ return -EINVAL;
	972	+
	973	+ if (np) {
757	974	err = dsa_switch_parse_of(ds, np);
758		- else if (pdata)
	975	+ if (err)
	976	+ dsa_switch_release_ports(ds);
	977	+ } else if (pdata) {
759	978	err = dsa_switch_parse(ds, pdata);
760		- else
	979	+ if (err)
	980	+ dsa_switch_release_ports(ds);
	981	+ } else {
761	982	err = -ENODEV;
	983	+ }
762	984
763	985	if (err)
764	986	return err;
765	987
766		- return dsa_switch_add(ds);
767		-}
768		-
769		-struct dsa_switch dsa_switch_alloc(struct device dev, size_t n)
770		-{
771		- size_t size = sizeof(struct dsa_switch) + n * sizeof(struct dsa_port);
772		- struct dsa_switch *ds;
773		- int i;
774		-
775		- ds = devm_kzalloc(dev, size, GFP_KERNEL);
776		- if (!ds)
777		- return NULL;
778		-
779		- /* We avoid allocating memory outside dsa_switch
780		- * if it is not needed.
781		- */
782		- if (n <= sizeof(ds->_bitmap) * 8) {
783		- ds->bitmap = &ds->_bitmap;
784		- } else {
785		- ds->bitmap = devm_kcalloc(dev,
786		- BITS_TO_LONGS(n),
787		- sizeof(unsigned long),
788		- GFP_KERNEL);
789		- if (unlikely(!ds->bitmap))
790		- return NULL;
	988	+ dst = ds->dst;
	989	+ dsa_tree_get(dst);
	990	+ err = dsa_tree_setup(dst);
	991	+ if (err) {
	992	+ dsa_switch_release_ports(ds);
	993	+ dsa_tree_put(dst);
791	994	}
792	995
793		- ds->dev = dev;
794		- ds->num_ports = n;
795		-
796		- for (i = 0; i < ds->num_ports; ++i) {
797		- ds->ports[i].index = i;
798		- ds->ports[i].ds = ds;
799		- }
800		-
801		- return ds;
	996	+ return err;
802	997	}
803		-EXPORT_SYMBOL_GPL(dsa_switch_alloc);
804	998
805	999	int dsa_register_switch(struct dsa_switch *ds)
806	1000	{
..	..	@@ -818,9 +1012,10 @@
818	1012	static void dsa_switch_remove(struct dsa_switch *ds)
819	1013	{
820	1014	struct dsa_switch_tree *dst = ds->dst;
821		- unsigned int index = ds->index;
822	1015
823		- dsa_tree_remove_switch(dst, index);
	1016	+ dsa_tree_teardown(dst);
	1017	+ dsa_switch_release_ports(ds);
	1018	+ dsa_tree_put(dst);
824	1019	}
825	1020
826	1021	void dsa_unregister_switch(struct dsa_switch *ds)