xref: /openbmc/linux/drivers/net/dsa/ocelot/felix.c (revision 78c5335b)
1 // SPDX-License-Identifier: GPL-2.0
2 /* Copyright 2019-2021 NXP
3  *
4  * This is an umbrella module for all network switches that are
5  * register-compatible with Ocelot and that perform I/O to their host CPU
6  * through an NPI (Node Processor Interface) Ethernet port.
7  */
8 #include <uapi/linux/if_bridge.h>
9 #include <soc/mscc/ocelot_vcap.h>
10 #include <soc/mscc/ocelot_qsys.h>
11 #include <soc/mscc/ocelot_sys.h>
12 #include <soc/mscc/ocelot_dev.h>
13 #include <soc/mscc/ocelot_ana.h>
14 #include <soc/mscc/ocelot_ptp.h>
15 #include <soc/mscc/ocelot.h>
16 #include <linux/dsa/8021q.h>
17 #include <linux/dsa/ocelot.h>
18 #include <linux/platform_device.h>
19 #include <linux/ptp_classify.h>
20 #include <linux/module.h>
21 #include <linux/of_net.h>
22 #include <linux/pci.h>
23 #include <linux/of.h>
24 #include <linux/pcs-lynx.h>
25 #include <net/pkt_sched.h>
26 #include <net/dsa.h>
27 #include "felix.h"
28 
29 static int felix_tag_8021q_rxvlan_add(struct felix *felix, int port, u16 vid,
30 				      bool pvid, bool untagged)
31 {
32 	struct ocelot_vcap_filter *outer_tagging_rule;
33 	struct ocelot *ocelot = &felix->ocelot;
34 	struct dsa_switch *ds = felix->ds;
35 	int key_length, upstream, err;
36 
37 	/* We don't need to install the rxvlan into the other ports' filtering
38 	 * tables, because we're just pushing the rxvlan when sending towards
39 	 * the CPU
40 	 */
41 	if (!pvid)
42 		return 0;
43 
44 	key_length = ocelot->vcap[VCAP_ES0].keys[VCAP_ES0_IGR_PORT].length;
45 	upstream = dsa_upstream_port(ds, port);
46 
47 	outer_tagging_rule = kzalloc(sizeof(struct ocelot_vcap_filter),
48 				     GFP_KERNEL);
49 	if (!outer_tagging_rule)
50 		return -ENOMEM;
51 
52 	outer_tagging_rule->key_type = OCELOT_VCAP_KEY_ANY;
53 	outer_tagging_rule->prio = 1;
54 	outer_tagging_rule->id.cookie = port;
55 	outer_tagging_rule->id.tc_offload = false;
56 	outer_tagging_rule->block_id = VCAP_ES0;
57 	outer_tagging_rule->type = OCELOT_VCAP_FILTER_OFFLOAD;
58 	outer_tagging_rule->lookup = 0;
59 	outer_tagging_rule->ingress_port.value = port;
60 	outer_tagging_rule->ingress_port.mask = GENMASK(key_length - 1, 0);
61 	outer_tagging_rule->egress_port.value = upstream;
62 	outer_tagging_rule->egress_port.mask = GENMASK(key_length - 1, 0);
63 	outer_tagging_rule->action.push_outer_tag = OCELOT_ES0_TAG;
64 	outer_tagging_rule->action.tag_a_tpid_sel = OCELOT_TAG_TPID_SEL_8021AD;
65 	outer_tagging_rule->action.tag_a_vid_sel = 1;
66 	outer_tagging_rule->action.vid_a_val = vid;
67 
68 	err = ocelot_vcap_filter_add(ocelot, outer_tagging_rule, NULL);
69 	if (err)
70 		kfree(outer_tagging_rule);
71 
72 	return err;
73 }
74 
75 static int felix_tag_8021q_txvlan_add(struct felix *felix, int port, u16 vid,
76 				      bool pvid, bool untagged)
77 {
78 	struct ocelot_vcap_filter *untagging_rule, *redirect_rule;
79 	struct ocelot *ocelot = &felix->ocelot;
80 	struct dsa_switch *ds = felix->ds;
81 	int upstream, err;
82 
83 	/* tag_8021q.c assumes we are implementing this via port VLAN
84 	 * membership, which we aren't. So we don't need to add any VCAP filter
85 	 * for the CPU port.
86 	 */
87 	if (ocelot->ports[port]->is_dsa_8021q_cpu)
88 		return 0;
89 
90 	untagging_rule = kzalloc(sizeof(struct ocelot_vcap_filter), GFP_KERNEL);
91 	if (!untagging_rule)
92 		return -ENOMEM;
93 
94 	redirect_rule = kzalloc(sizeof(struct ocelot_vcap_filter), GFP_KERNEL);
95 	if (!redirect_rule) {
96 		kfree(untagging_rule);
97 		return -ENOMEM;
98 	}
99 
100 	upstream = dsa_upstream_port(ds, port);
101 
102 	untagging_rule->key_type = OCELOT_VCAP_KEY_ANY;
103 	untagging_rule->ingress_port_mask = BIT(upstream);
104 	untagging_rule->vlan.vid.value = vid;
105 	untagging_rule->vlan.vid.mask = VLAN_VID_MASK;
106 	untagging_rule->prio = 1;
107 	untagging_rule->id.cookie = port;
108 	untagging_rule->id.tc_offload = false;
109 	untagging_rule->block_id = VCAP_IS1;
110 	untagging_rule->type = OCELOT_VCAP_FILTER_OFFLOAD;
111 	untagging_rule->lookup = 0;
112 	untagging_rule->action.vlan_pop_cnt_ena = true;
113 	untagging_rule->action.vlan_pop_cnt = 1;
114 	untagging_rule->action.pag_override_mask = 0xff;
115 	untagging_rule->action.pag_val = port;
116 
117 	err = ocelot_vcap_filter_add(ocelot, untagging_rule, NULL);
118 	if (err) {
119 		kfree(untagging_rule);
120 		kfree(redirect_rule);
121 		return err;
122 	}
123 
124 	redirect_rule->key_type = OCELOT_VCAP_KEY_ANY;
125 	redirect_rule->ingress_port_mask = BIT(upstream);
126 	redirect_rule->pag = port;
127 	redirect_rule->prio = 1;
128 	redirect_rule->id.cookie = port;
129 	redirect_rule->id.tc_offload = false;
130 	redirect_rule->block_id = VCAP_IS2;
131 	redirect_rule->type = OCELOT_VCAP_FILTER_OFFLOAD;
132 	redirect_rule->lookup = 0;
133 	redirect_rule->action.mask_mode = OCELOT_MASK_MODE_REDIRECT;
134 	redirect_rule->action.port_mask = BIT(port);
135 
136 	err = ocelot_vcap_filter_add(ocelot, redirect_rule, NULL);
137 	if (err) {
138 		ocelot_vcap_filter_del(ocelot, untagging_rule);
139 		kfree(redirect_rule);
140 		return err;
141 	}
142 
143 	return 0;
144 }
145 
146 static int felix_tag_8021q_vlan_add(struct dsa_switch *ds, int port, u16 vid,
147 				    u16 flags)
148 {
149 	bool untagged = flags & BRIDGE_VLAN_INFO_UNTAGGED;
150 	bool pvid = flags & BRIDGE_VLAN_INFO_PVID;
151 	struct ocelot *ocelot = ds->priv;
152 
153 	if (vid_is_dsa_8021q_rxvlan(vid))
154 		return felix_tag_8021q_rxvlan_add(ocelot_to_felix(ocelot),
155 						  port, vid, pvid, untagged);
156 
157 	if (vid_is_dsa_8021q_txvlan(vid))
158 		return felix_tag_8021q_txvlan_add(ocelot_to_felix(ocelot),
159 						  port, vid, pvid, untagged);
160 
161 	return 0;
162 }
163 
164 static int felix_tag_8021q_rxvlan_del(struct felix *felix, int port, u16 vid)
165 {
166 	struct ocelot_vcap_filter *outer_tagging_rule;
167 	struct ocelot_vcap_block *block_vcap_es0;
168 	struct ocelot *ocelot = &felix->ocelot;
169 
170 	block_vcap_es0 = &ocelot->block[VCAP_ES0];
171 
172 	outer_tagging_rule = ocelot_vcap_block_find_filter_by_id(block_vcap_es0,
173 								 port, false);
174 	/* In rxvlan_add, we had the "if (!pvid) return 0" logic to avoid
175 	 * installing outer tagging ES0 rules where they weren't needed.
176 	 * But in rxvlan_del, the API doesn't give us the "flags" anymore,
177 	 * so that forces us to be slightly sloppy here, and just assume that
178 	 * if we didn't find an outer_tagging_rule it means that there was
179 	 * none in the first place, i.e. rxvlan_del is called on a non-pvid
180 	 * port. This is most probably true though.
181 	 */
182 	if (!outer_tagging_rule)
183 		return 0;
184 
185 	return ocelot_vcap_filter_del(ocelot, outer_tagging_rule);
186 }
187 
188 static int felix_tag_8021q_txvlan_del(struct felix *felix, int port, u16 vid)
189 {
190 	struct ocelot_vcap_filter *untagging_rule, *redirect_rule;
191 	struct ocelot_vcap_block *block_vcap_is1;
192 	struct ocelot_vcap_block *block_vcap_is2;
193 	struct ocelot *ocelot = &felix->ocelot;
194 	int err;
195 
196 	if (ocelot->ports[port]->is_dsa_8021q_cpu)
197 		return 0;
198 
199 	block_vcap_is1 = &ocelot->block[VCAP_IS1];
200 	block_vcap_is2 = &ocelot->block[VCAP_IS2];
201 
202 	untagging_rule = ocelot_vcap_block_find_filter_by_id(block_vcap_is1,
203 							     port, false);
204 	if (!untagging_rule)
205 		return 0;
206 
207 	err = ocelot_vcap_filter_del(ocelot, untagging_rule);
208 	if (err)
209 		return err;
210 
211 	redirect_rule = ocelot_vcap_block_find_filter_by_id(block_vcap_is2,
212 							    port, false);
213 	if (!redirect_rule)
214 		return 0;
215 
216 	return ocelot_vcap_filter_del(ocelot, redirect_rule);
217 }
218 
219 static int felix_tag_8021q_vlan_del(struct dsa_switch *ds, int port, u16 vid)
220 {
221 	struct ocelot *ocelot = ds->priv;
222 
223 	if (vid_is_dsa_8021q_rxvlan(vid))
224 		return felix_tag_8021q_rxvlan_del(ocelot_to_felix(ocelot),
225 						  port, vid);
226 
227 	if (vid_is_dsa_8021q_txvlan(vid))
228 		return felix_tag_8021q_txvlan_del(ocelot_to_felix(ocelot),
229 						  port, vid);
230 
231 	return 0;
232 }
233 
234 /* Alternatively to using the NPI functionality, that same hardware MAC
235  * connected internally to the enetc or fman DSA master can be configured to
236  * use the software-defined tag_8021q frame format. As far as the hardware is
237  * concerned, it thinks it is a "dumb switch" - the queues of the CPU port
238  * module are now disconnected from it, but can still be accessed through
239  * register-based MMIO.
240  */
241 static void felix_8021q_cpu_port_init(struct ocelot *ocelot, int port)
242 {
243 	ocelot->ports[port]->is_dsa_8021q_cpu = true;
244 	ocelot->npi = -1;
245 
246 	/* Overwrite PGID_CPU with the non-tagging port */
247 	ocelot_write_rix(ocelot, BIT(port), ANA_PGID_PGID, PGID_CPU);
248 
249 	ocelot_apply_bridge_fwd_mask(ocelot);
250 }
251 
252 static void felix_8021q_cpu_port_deinit(struct ocelot *ocelot, int port)
253 {
254 	ocelot->ports[port]->is_dsa_8021q_cpu = false;
255 
256 	/* Restore PGID_CPU */
257 	ocelot_write_rix(ocelot, BIT(ocelot->num_phys_ports), ANA_PGID_PGID,
258 			 PGID_CPU);
259 
260 	ocelot_apply_bridge_fwd_mask(ocelot);
261 }
262 
263 /* Set up a VCAP IS2 rule for delivering PTP frames to the CPU port module.
264  * If the quirk_no_xtr_irq is in place, then also copy those PTP frames to the
265  * tag_8021q CPU port.
266  */
267 static int felix_setup_mmio_filtering(struct felix *felix)
268 {
269 	unsigned long user_ports = dsa_user_ports(felix->ds);
270 	struct ocelot_vcap_filter *redirect_rule;
271 	struct ocelot_vcap_filter *tagging_rule;
272 	struct ocelot *ocelot = &felix->ocelot;
273 	struct dsa_switch *ds = felix->ds;
274 	int cpu = -1, port, ret;
275 
276 	tagging_rule = kzalloc(sizeof(struct ocelot_vcap_filter), GFP_KERNEL);
277 	if (!tagging_rule)
278 		return -ENOMEM;
279 
280 	redirect_rule = kzalloc(sizeof(struct ocelot_vcap_filter), GFP_KERNEL);
281 	if (!redirect_rule) {
282 		kfree(tagging_rule);
283 		return -ENOMEM;
284 	}
285 
286 	for (port = 0; port < ocelot->num_phys_ports; port++) {
287 		if (dsa_is_cpu_port(ds, port)) {
288 			cpu = port;
289 			break;
290 		}
291 	}
292 
293 	if (cpu < 0) {
294 		kfree(tagging_rule);
295 		kfree(redirect_rule);
296 		return -EINVAL;
297 	}
298 
299 	tagging_rule->key_type = OCELOT_VCAP_KEY_ETYPE;
300 	*(__be16 *)tagging_rule->key.etype.etype.value = htons(ETH_P_1588);
301 	*(__be16 *)tagging_rule->key.etype.etype.mask = htons(0xffff);
302 	tagging_rule->ingress_port_mask = user_ports;
303 	tagging_rule->prio = 1;
304 	tagging_rule->id.cookie = ocelot->num_phys_ports;
305 	tagging_rule->id.tc_offload = false;
306 	tagging_rule->block_id = VCAP_IS1;
307 	tagging_rule->type = OCELOT_VCAP_FILTER_OFFLOAD;
308 	tagging_rule->lookup = 0;
309 	tagging_rule->action.pag_override_mask = 0xff;
310 	tagging_rule->action.pag_val = ocelot->num_phys_ports;
311 
312 	ret = ocelot_vcap_filter_add(ocelot, tagging_rule, NULL);
313 	if (ret) {
314 		kfree(tagging_rule);
315 		kfree(redirect_rule);
316 		return ret;
317 	}
318 
319 	redirect_rule->key_type = OCELOT_VCAP_KEY_ANY;
320 	redirect_rule->ingress_port_mask = user_ports;
321 	redirect_rule->pag = ocelot->num_phys_ports;
322 	redirect_rule->prio = 1;
323 	redirect_rule->id.cookie = ocelot->num_phys_ports;
324 	redirect_rule->id.tc_offload = false;
325 	redirect_rule->block_id = VCAP_IS2;
326 	redirect_rule->type = OCELOT_VCAP_FILTER_OFFLOAD;
327 	redirect_rule->lookup = 0;
328 	redirect_rule->action.cpu_copy_ena = true;
329 	if (felix->info->quirk_no_xtr_irq) {
330 		/* Redirect to the tag_8021q CPU but also copy PTP packets to
331 		 * the CPU port module
332 		 */
333 		redirect_rule->action.mask_mode = OCELOT_MASK_MODE_REDIRECT;
334 		redirect_rule->action.port_mask = BIT(cpu);
335 	} else {
336 		/* Trap PTP packets only to the CPU port module (which is
337 		 * redirected to the NPI port)
338 		 */
339 		redirect_rule->action.mask_mode = OCELOT_MASK_MODE_PERMIT_DENY;
340 		redirect_rule->action.port_mask = 0;
341 	}
342 
343 	ret = ocelot_vcap_filter_add(ocelot, redirect_rule, NULL);
344 	if (ret) {
345 		ocelot_vcap_filter_del(ocelot, tagging_rule);
346 		kfree(redirect_rule);
347 		return ret;
348 	}
349 
350 	/* The ownership of the CPU port module's queues might have just been
351 	 * transferred to the tag_8021q tagger from the NPI-based tagger.
352 	 * So there might still be all sorts of crap in the queues. On the
353 	 * other hand, the MMIO-based matching of PTP frames is very brittle,
354 	 * so we need to be careful that there are no extra frames to be
355 	 * dequeued over MMIO, since we would never know to discard them.
356 	 */
357 	ocelot_drain_cpu_queue(ocelot, 0);
358 
359 	return 0;
360 }
361 
362 static int felix_teardown_mmio_filtering(struct felix *felix)
363 {
364 	struct ocelot_vcap_filter *tagging_rule, *redirect_rule;
365 	struct ocelot_vcap_block *block_vcap_is1;
366 	struct ocelot_vcap_block *block_vcap_is2;
367 	struct ocelot *ocelot = &felix->ocelot;
368 	int err;
369 
370 	block_vcap_is1 = &ocelot->block[VCAP_IS1];
371 	block_vcap_is2 = &ocelot->block[VCAP_IS2];
372 
373 	tagging_rule = ocelot_vcap_block_find_filter_by_id(block_vcap_is1,
374 							   ocelot->num_phys_ports,
375 							   false);
376 	if (!tagging_rule)
377 		return -ENOENT;
378 
379 	err = ocelot_vcap_filter_del(ocelot, tagging_rule);
380 	if (err)
381 		return err;
382 
383 	redirect_rule = ocelot_vcap_block_find_filter_by_id(block_vcap_is2,
384 							    ocelot->num_phys_ports,
385 							    false);
386 	if (!redirect_rule)
387 		return -ENOENT;
388 
389 	return ocelot_vcap_filter_del(ocelot, redirect_rule);
390 }
391 
392 static int felix_setup_tag_8021q(struct dsa_switch *ds, int cpu)
393 {
394 	struct ocelot *ocelot = ds->priv;
395 	struct felix *felix = ocelot_to_felix(ocelot);
396 	unsigned long cpu_flood;
397 	int port, err;
398 
399 	felix_8021q_cpu_port_init(ocelot, cpu);
400 
401 	for (port = 0; port < ds->num_ports; port++) {
402 		if (dsa_is_unused_port(ds, port))
403 			continue;
404 
405 		/* This overwrites ocelot_init():
406 		 * Do not forward BPDU frames to the CPU port module,
407 		 * for 2 reasons:
408 		 * - When these packets are injected from the tag_8021q
409 		 *   CPU port, we want them to go out, not loop back
410 		 *   into the system.
411 		 * - STP traffic ingressing on a user port should go to
412 		 *   the tag_8021q CPU port, not to the hardware CPU
413 		 *   port module.
414 		 */
415 		ocelot_write_gix(ocelot,
416 				 ANA_PORT_CPU_FWD_BPDU_CFG_BPDU_REDIR_ENA(0),
417 				 ANA_PORT_CPU_FWD_BPDU_CFG, port);
418 	}
419 
420 	/* In tag_8021q mode, the CPU port module is unused, except for PTP
421 	 * frames. So we want to disable flooding of any kind to the CPU port
422 	 * module, since packets going there will end in a black hole.
423 	 */
424 	cpu_flood = ANA_PGID_PGID_PGID(BIT(ocelot->num_phys_ports));
425 	ocelot_rmw_rix(ocelot, 0, cpu_flood, ANA_PGID_PGID, PGID_UC);
426 	ocelot_rmw_rix(ocelot, 0, cpu_flood, ANA_PGID_PGID, PGID_MC);
427 	ocelot_rmw_rix(ocelot, 0, cpu_flood, ANA_PGID_PGID, PGID_BC);
428 
429 	err = dsa_tag_8021q_register(ds, htons(ETH_P_8021AD));
430 	if (err)
431 		return err;
432 
433 	err = felix_setup_mmio_filtering(felix);
434 	if (err)
435 		goto out_tag_8021q_unregister;
436 
437 	return 0;
438 
439 out_tag_8021q_unregister:
440 	dsa_tag_8021q_unregister(ds);
441 	return err;
442 }
443 
444 static void felix_teardown_tag_8021q(struct dsa_switch *ds, int cpu)
445 {
446 	struct ocelot *ocelot = ds->priv;
447 	struct felix *felix = ocelot_to_felix(ocelot);
448 	int err, port;
449 
450 	err = felix_teardown_mmio_filtering(felix);
451 	if (err)
452 		dev_err(ds->dev, "felix_teardown_mmio_filtering returned %d",
453 			err);
454 
455 	dsa_tag_8021q_unregister(ds);
456 
457 	for (port = 0; port < ds->num_ports; port++) {
458 		if (dsa_is_unused_port(ds, port))
459 			continue;
460 
461 		/* Restore the logic from ocelot_init:
462 		 * do not forward BPDU frames to the front ports.
463 		 */
464 		ocelot_write_gix(ocelot,
465 				 ANA_PORT_CPU_FWD_BPDU_CFG_BPDU_REDIR_ENA(0xffff),
466 				 ANA_PORT_CPU_FWD_BPDU_CFG,
467 				 port);
468 	}
469 
470 	felix_8021q_cpu_port_deinit(ocelot, cpu);
471 }
472 
473 /* The CPU port module is connected to the Node Processor Interface (NPI). This
474  * is the mode through which frames can be injected from and extracted to an
475  * external CPU, over Ethernet. In NXP SoCs, the "external CPU" is the ARM CPU
476  * running Linux, and this forms a DSA setup together with the enetc or fman
477  * DSA master.
478  */
479 static void felix_npi_port_init(struct ocelot *ocelot, int port)
480 {
481 	ocelot->npi = port;
482 
483 	ocelot_write(ocelot, QSYS_EXT_CPU_CFG_EXT_CPUQ_MSK_M |
484 		     QSYS_EXT_CPU_CFG_EXT_CPU_PORT(port),
485 		     QSYS_EXT_CPU_CFG);
486 
487 	/* NPI port Injection/Extraction configuration */
488 	ocelot_fields_write(ocelot, port, SYS_PORT_MODE_INCL_XTR_HDR,
489 			    ocelot->npi_xtr_prefix);
490 	ocelot_fields_write(ocelot, port, SYS_PORT_MODE_INCL_INJ_HDR,
491 			    ocelot->npi_inj_prefix);
492 
493 	/* Disable transmission of pause frames */
494 	ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_ENA, 0);
495 }
496 
497 static void felix_npi_port_deinit(struct ocelot *ocelot, int port)
498 {
499 	/* Restore hardware defaults */
500 	int unused_port = ocelot->num_phys_ports + 2;
501 
502 	ocelot->npi = -1;
503 
504 	ocelot_write(ocelot, QSYS_EXT_CPU_CFG_EXT_CPU_PORT(unused_port),
505 		     QSYS_EXT_CPU_CFG);
506 
507 	ocelot_fields_write(ocelot, port, SYS_PORT_MODE_INCL_XTR_HDR,
508 			    OCELOT_TAG_PREFIX_DISABLED);
509 	ocelot_fields_write(ocelot, port, SYS_PORT_MODE_INCL_INJ_HDR,
510 			    OCELOT_TAG_PREFIX_DISABLED);
511 
512 	/* Enable transmission of pause frames */
513 	ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_ENA, 1);
514 }
515 
516 static int felix_setup_tag_npi(struct dsa_switch *ds, int cpu)
517 {
518 	struct ocelot *ocelot = ds->priv;
519 	unsigned long cpu_flood;
520 
521 	felix_npi_port_init(ocelot, cpu);
522 
523 	/* Include the CPU port module (and indirectly, the NPI port)
524 	 * in the forwarding mask for unknown unicast - the hardware
525 	 * default value for ANA_FLOODING_FLD_UNICAST excludes
526 	 * BIT(ocelot->num_phys_ports), and so does ocelot_init,
527 	 * since Ocelot relies on whitelisting MAC addresses towards
528 	 * PGID_CPU.
529 	 * We do this because DSA does not yet perform RX filtering,
530 	 * and the NPI port does not perform source address learning,
531 	 * so traffic sent to Linux is effectively unknown from the
532 	 * switch's perspective.
533 	 */
534 	cpu_flood = ANA_PGID_PGID_PGID(BIT(ocelot->num_phys_ports));
535 	ocelot_rmw_rix(ocelot, cpu_flood, cpu_flood, ANA_PGID_PGID, PGID_UC);
536 	ocelot_rmw_rix(ocelot, cpu_flood, cpu_flood, ANA_PGID_PGID, PGID_MC);
537 	ocelot_rmw_rix(ocelot, cpu_flood, cpu_flood, ANA_PGID_PGID, PGID_BC);
538 
539 	return 0;
540 }
541 
542 static void felix_teardown_tag_npi(struct dsa_switch *ds, int cpu)
543 {
544 	struct ocelot *ocelot = ds->priv;
545 
546 	felix_npi_port_deinit(ocelot, cpu);
547 }
548 
549 static int felix_set_tag_protocol(struct dsa_switch *ds, int cpu,
550 				  enum dsa_tag_protocol proto)
551 {
552 	int err;
553 
554 	switch (proto) {
555 	case DSA_TAG_PROTO_SEVILLE:
556 	case DSA_TAG_PROTO_OCELOT:
557 		err = felix_setup_tag_npi(ds, cpu);
558 		break;
559 	case DSA_TAG_PROTO_OCELOT_8021Q:
560 		err = felix_setup_tag_8021q(ds, cpu);
561 		break;
562 	default:
563 		err = -EPROTONOSUPPORT;
564 	}
565 
566 	return err;
567 }
568 
569 static void felix_del_tag_protocol(struct dsa_switch *ds, int cpu,
570 				   enum dsa_tag_protocol proto)
571 {
572 	switch (proto) {
573 	case DSA_TAG_PROTO_SEVILLE:
574 	case DSA_TAG_PROTO_OCELOT:
575 		felix_teardown_tag_npi(ds, cpu);
576 		break;
577 	case DSA_TAG_PROTO_OCELOT_8021Q:
578 		felix_teardown_tag_8021q(ds, cpu);
579 		break;
580 	default:
581 		break;
582 	}
583 }
584 
585 /* This always leaves the switch in a consistent state, because although the
586  * tag_8021q setup can fail, the NPI setup can't. So either the change is made,
587  * or the restoration is guaranteed to work.
588  */
589 static int felix_change_tag_protocol(struct dsa_switch *ds, int cpu,
590 				     enum dsa_tag_protocol proto)
591 {
592 	struct ocelot *ocelot = ds->priv;
593 	struct felix *felix = ocelot_to_felix(ocelot);
594 	enum dsa_tag_protocol old_proto = felix->tag_proto;
595 	int err;
596 
597 	if (proto != DSA_TAG_PROTO_SEVILLE &&
598 	    proto != DSA_TAG_PROTO_OCELOT &&
599 	    proto != DSA_TAG_PROTO_OCELOT_8021Q)
600 		return -EPROTONOSUPPORT;
601 
602 	felix_del_tag_protocol(ds, cpu, old_proto);
603 
604 	err = felix_set_tag_protocol(ds, cpu, proto);
605 	if (err) {
606 		felix_set_tag_protocol(ds, cpu, old_proto);
607 		return err;
608 	}
609 
610 	felix->tag_proto = proto;
611 
612 	return 0;
613 }
614 
615 static enum dsa_tag_protocol felix_get_tag_protocol(struct dsa_switch *ds,
616 						    int port,
617 						    enum dsa_tag_protocol mp)
618 {
619 	struct ocelot *ocelot = ds->priv;
620 	struct felix *felix = ocelot_to_felix(ocelot);
621 
622 	return felix->tag_proto;
623 }
624 
625 static int felix_set_ageing_time(struct dsa_switch *ds,
626 				 unsigned int ageing_time)
627 {
628 	struct ocelot *ocelot = ds->priv;
629 
630 	ocelot_set_ageing_time(ocelot, ageing_time);
631 
632 	return 0;
633 }
634 
635 static int felix_fdb_dump(struct dsa_switch *ds, int port,
636 			  dsa_fdb_dump_cb_t *cb, void *data)
637 {
638 	struct ocelot *ocelot = ds->priv;
639 
640 	return ocelot_fdb_dump(ocelot, port, cb, data);
641 }
642 
643 static int felix_fdb_add(struct dsa_switch *ds, int port,
644 			 const unsigned char *addr, u16 vid)
645 {
646 	struct ocelot *ocelot = ds->priv;
647 
648 	return ocelot_fdb_add(ocelot, port, addr, vid);
649 }
650 
651 static int felix_fdb_del(struct dsa_switch *ds, int port,
652 			 const unsigned char *addr, u16 vid)
653 {
654 	struct ocelot *ocelot = ds->priv;
655 
656 	return ocelot_fdb_del(ocelot, port, addr, vid);
657 }
658 
659 static int felix_mdb_add(struct dsa_switch *ds, int port,
660 			 const struct switchdev_obj_port_mdb *mdb)
661 {
662 	struct ocelot *ocelot = ds->priv;
663 
664 	return ocelot_port_mdb_add(ocelot, port, mdb);
665 }
666 
667 static int felix_mdb_del(struct dsa_switch *ds, int port,
668 			 const struct switchdev_obj_port_mdb *mdb)
669 {
670 	struct ocelot *ocelot = ds->priv;
671 
672 	return ocelot_port_mdb_del(ocelot, port, mdb);
673 }
674 
675 static void felix_bridge_stp_state_set(struct dsa_switch *ds, int port,
676 				       u8 state)
677 {
678 	struct ocelot *ocelot = ds->priv;
679 
680 	return ocelot_bridge_stp_state_set(ocelot, port, state);
681 }
682 
683 static int felix_pre_bridge_flags(struct dsa_switch *ds, int port,
684 				  struct switchdev_brport_flags val,
685 				  struct netlink_ext_ack *extack)
686 {
687 	struct ocelot *ocelot = ds->priv;
688 
689 	return ocelot_port_pre_bridge_flags(ocelot, port, val);
690 }
691 
692 static int felix_bridge_flags(struct dsa_switch *ds, int port,
693 			      struct switchdev_brport_flags val,
694 			      struct netlink_ext_ack *extack)
695 {
696 	struct ocelot *ocelot = ds->priv;
697 
698 	ocelot_port_bridge_flags(ocelot, port, val);
699 
700 	return 0;
701 }
702 
703 static int felix_bridge_join(struct dsa_switch *ds, int port,
704 			     struct net_device *br)
705 {
706 	struct ocelot *ocelot = ds->priv;
707 
708 	ocelot_port_bridge_join(ocelot, port, br);
709 
710 	return 0;
711 }
712 
713 static void felix_bridge_leave(struct dsa_switch *ds, int port,
714 			       struct net_device *br)
715 {
716 	struct ocelot *ocelot = ds->priv;
717 
718 	ocelot_port_bridge_leave(ocelot, port, br);
719 }
720 
721 static int felix_lag_join(struct dsa_switch *ds, int port,
722 			  struct net_device *bond,
723 			  struct netdev_lag_upper_info *info)
724 {
725 	struct ocelot *ocelot = ds->priv;
726 
727 	return ocelot_port_lag_join(ocelot, port, bond, info);
728 }
729 
730 static int felix_lag_leave(struct dsa_switch *ds, int port,
731 			   struct net_device *bond)
732 {
733 	struct ocelot *ocelot = ds->priv;
734 
735 	ocelot_port_lag_leave(ocelot, port, bond);
736 
737 	return 0;
738 }
739 
740 static int felix_lag_change(struct dsa_switch *ds, int port)
741 {
742 	struct dsa_port *dp = dsa_to_port(ds, port);
743 	struct ocelot *ocelot = ds->priv;
744 
745 	ocelot_port_lag_change(ocelot, port, dp->lag_tx_enabled);
746 
747 	return 0;
748 }
749 
750 static int felix_vlan_prepare(struct dsa_switch *ds, int port,
751 			      const struct switchdev_obj_port_vlan *vlan,
752 			      struct netlink_ext_ack *extack)
753 {
754 	struct ocelot *ocelot = ds->priv;
755 	u16 flags = vlan->flags;
756 
757 	/* Ocelot switches copy frames as-is to the CPU, so the flags:
758 	 * egress-untagged or not, pvid or not, make no difference. This
759 	 * behavior is already better than what DSA just tries to approximate
760 	 * when it installs the VLAN with the same flags on the CPU port.
761 	 * Just accept any configuration, and don't let ocelot deny installing
762 	 * multiple native VLANs on the NPI port, because the switch doesn't
763 	 * look at the port tag settings towards the NPI interface anyway.
764 	 */
765 	if (port == ocelot->npi)
766 		return 0;
767 
768 	return ocelot_vlan_prepare(ocelot, port, vlan->vid,
769 				   flags & BRIDGE_VLAN_INFO_PVID,
770 				   flags & BRIDGE_VLAN_INFO_UNTAGGED,
771 				   extack);
772 }
773 
774 static int felix_vlan_filtering(struct dsa_switch *ds, int port, bool enabled,
775 				struct netlink_ext_ack *extack)
776 {
777 	struct ocelot *ocelot = ds->priv;
778 
779 	return ocelot_port_vlan_filtering(ocelot, port, enabled, extack);
780 }
781 
782 static int felix_vlan_add(struct dsa_switch *ds, int port,
783 			  const struct switchdev_obj_port_vlan *vlan,
784 			  struct netlink_ext_ack *extack)
785 {
786 	struct ocelot *ocelot = ds->priv;
787 	u16 flags = vlan->flags;
788 	int err;
789 
790 	err = felix_vlan_prepare(ds, port, vlan, extack);
791 	if (err)
792 		return err;
793 
794 	return ocelot_vlan_add(ocelot, port, vlan->vid,
795 			       flags & BRIDGE_VLAN_INFO_PVID,
796 			       flags & BRIDGE_VLAN_INFO_UNTAGGED);
797 }
798 
799 static int felix_vlan_del(struct dsa_switch *ds, int port,
800 			  const struct switchdev_obj_port_vlan *vlan)
801 {
802 	struct ocelot *ocelot = ds->priv;
803 
804 	return ocelot_vlan_del(ocelot, port, vlan->vid);
805 }
806 
807 static void felix_phylink_validate(struct dsa_switch *ds, int port,
808 				   unsigned long *supported,
809 				   struct phylink_link_state *state)
810 {
811 	struct ocelot *ocelot = ds->priv;
812 	struct felix *felix = ocelot_to_felix(ocelot);
813 
814 	if (felix->info->phylink_validate)
815 		felix->info->phylink_validate(ocelot, port, supported, state);
816 }
817 
818 static void felix_phylink_mac_config(struct dsa_switch *ds, int port,
819 				     unsigned int link_an_mode,
820 				     const struct phylink_link_state *state)
821 {
822 	struct ocelot *ocelot = ds->priv;
823 	struct felix *felix = ocelot_to_felix(ocelot);
824 	struct dsa_port *dp = dsa_to_port(ds, port);
825 
826 	if (felix->pcs[port])
827 		phylink_set_pcs(dp->pl, &felix->pcs[port]->pcs);
828 }
829 
830 static void felix_phylink_mac_link_down(struct dsa_switch *ds, int port,
831 					unsigned int link_an_mode,
832 					phy_interface_t interface)
833 {
834 	struct ocelot *ocelot = ds->priv;
835 
836 	ocelot_phylink_mac_link_down(ocelot, port, link_an_mode, interface,
837 				     FELIX_MAC_QUIRKS);
838 }
839 
840 static void felix_phylink_mac_link_up(struct dsa_switch *ds, int port,
841 				      unsigned int link_an_mode,
842 				      phy_interface_t interface,
843 				      struct phy_device *phydev,
844 				      int speed, int duplex,
845 				      bool tx_pause, bool rx_pause)
846 {
847 	struct ocelot *ocelot = ds->priv;
848 	struct felix *felix = ocelot_to_felix(ocelot);
849 
850 	ocelot_phylink_mac_link_up(ocelot, port, phydev, link_an_mode,
851 				   interface, speed, duplex, tx_pause, rx_pause,
852 				   FELIX_MAC_QUIRKS);
853 
854 	if (felix->info->port_sched_speed_set)
855 		felix->info->port_sched_speed_set(ocelot, port, speed);
856 }
857 
858 static void felix_port_qos_map_init(struct ocelot *ocelot, int port)
859 {
860 	int i;
861 
862 	ocelot_rmw_gix(ocelot,
863 		       ANA_PORT_QOS_CFG_QOS_PCP_ENA,
864 		       ANA_PORT_QOS_CFG_QOS_PCP_ENA,
865 		       ANA_PORT_QOS_CFG,
866 		       port);
867 
868 	for (i = 0; i < OCELOT_NUM_TC * 2; i++) {
869 		ocelot_rmw_ix(ocelot,
870 			      (ANA_PORT_PCP_DEI_MAP_DP_PCP_DEI_VAL & i) |
871 			      ANA_PORT_PCP_DEI_MAP_QOS_PCP_DEI_VAL(i),
872 			      ANA_PORT_PCP_DEI_MAP_DP_PCP_DEI_VAL |
873 			      ANA_PORT_PCP_DEI_MAP_QOS_PCP_DEI_VAL_M,
874 			      ANA_PORT_PCP_DEI_MAP,
875 			      port, i);
876 	}
877 }
878 
879 static void felix_get_strings(struct dsa_switch *ds, int port,
880 			      u32 stringset, u8 *data)
881 {
882 	struct ocelot *ocelot = ds->priv;
883 
884 	return ocelot_get_strings(ocelot, port, stringset, data);
885 }
886 
887 static void felix_get_ethtool_stats(struct dsa_switch *ds, int port, u64 *data)
888 {
889 	struct ocelot *ocelot = ds->priv;
890 
891 	ocelot_get_ethtool_stats(ocelot, port, data);
892 }
893 
894 static int felix_get_sset_count(struct dsa_switch *ds, int port, int sset)
895 {
896 	struct ocelot *ocelot = ds->priv;
897 
898 	return ocelot_get_sset_count(ocelot, port, sset);
899 }
900 
901 static int felix_get_ts_info(struct dsa_switch *ds, int port,
902 			     struct ethtool_ts_info *info)
903 {
904 	struct ocelot *ocelot = ds->priv;
905 
906 	return ocelot_get_ts_info(ocelot, port, info);
907 }
908 
909 static int felix_parse_ports_node(struct felix *felix,
910 				  struct device_node *ports_node,
911 				  phy_interface_t *port_phy_modes)
912 {
913 	struct ocelot *ocelot = &felix->ocelot;
914 	struct device *dev = felix->ocelot.dev;
915 	struct device_node *child;
916 
917 	for_each_available_child_of_node(ports_node, child) {
918 		phy_interface_t phy_mode;
919 		u32 port;
920 		int err;
921 
922 		/* Get switch port number from DT */
923 		if (of_property_read_u32(child, "reg", &port) < 0) {
924 			dev_err(dev, "Port number not defined in device tree "
925 				"(property \"reg\")\n");
926 			of_node_put(child);
927 			return -ENODEV;
928 		}
929 
930 		/* Get PHY mode from DT */
931 		err = of_get_phy_mode(child, &phy_mode);
932 		if (err) {
933 			dev_err(dev, "Failed to read phy-mode or "
934 				"phy-interface-type property for port %d\n",
935 				port);
936 			of_node_put(child);
937 			return -ENODEV;
938 		}
939 
940 		err = felix->info->prevalidate_phy_mode(ocelot, port, phy_mode);
941 		if (err < 0) {
942 			dev_err(dev, "Unsupported PHY mode %s on port %d\n",
943 				phy_modes(phy_mode), port);
944 			of_node_put(child);
945 			return err;
946 		}
947 
948 		port_phy_modes[port] = phy_mode;
949 	}
950 
951 	return 0;
952 }
953 
954 static int felix_parse_dt(struct felix *felix, phy_interface_t *port_phy_modes)
955 {
956 	struct device *dev = felix->ocelot.dev;
957 	struct device_node *switch_node;
958 	struct device_node *ports_node;
959 	int err;
960 
961 	switch_node = dev->of_node;
962 
963 	ports_node = of_get_child_by_name(switch_node, "ports");
964 	if (!ports_node)
965 		ports_node = of_get_child_by_name(switch_node, "ethernet-ports");
966 	if (!ports_node) {
967 		dev_err(dev, "Incorrect bindings: absent \"ports\" or \"ethernet-ports\" node\n");
968 		return -ENODEV;
969 	}
970 
971 	err = felix_parse_ports_node(felix, ports_node, port_phy_modes);
972 	of_node_put(ports_node);
973 
974 	return err;
975 }
976 
977 static int felix_init_structs(struct felix *felix, int num_phys_ports)
978 {
979 	struct ocelot *ocelot = &felix->ocelot;
980 	phy_interface_t *port_phy_modes;
981 	struct resource res;
982 	int port, i, err;
983 
984 	ocelot->num_phys_ports = num_phys_ports;
985 	ocelot->ports = devm_kcalloc(ocelot->dev, num_phys_ports,
986 				     sizeof(struct ocelot_port *), GFP_KERNEL);
987 	if (!ocelot->ports)
988 		return -ENOMEM;
989 
990 	ocelot->map		= felix->info->map;
991 	ocelot->stats_layout	= felix->info->stats_layout;
992 	ocelot->num_stats	= felix->info->num_stats;
993 	ocelot->num_mact_rows	= felix->info->num_mact_rows;
994 	ocelot->vcap		= felix->info->vcap;
995 	ocelot->ops		= felix->info->ops;
996 	ocelot->npi_inj_prefix	= OCELOT_TAG_PREFIX_SHORT;
997 	ocelot->npi_xtr_prefix	= OCELOT_TAG_PREFIX_SHORT;
998 	ocelot->devlink		= felix->ds->devlink;
999 
1000 	port_phy_modes = kcalloc(num_phys_ports, sizeof(phy_interface_t),
1001 				 GFP_KERNEL);
1002 	if (!port_phy_modes)
1003 		return -ENOMEM;
1004 
1005 	err = felix_parse_dt(felix, port_phy_modes);
1006 	if (err) {
1007 		kfree(port_phy_modes);
1008 		return err;
1009 	}
1010 
1011 	for (i = 0; i < TARGET_MAX; i++) {
1012 		struct regmap *target;
1013 
1014 		if (!felix->info->target_io_res[i].name)
1015 			continue;
1016 
1017 		memcpy(&res, &felix->info->target_io_res[i], sizeof(res));
1018 		res.flags = IORESOURCE_MEM;
1019 		res.start += felix->switch_base;
1020 		res.end += felix->switch_base;
1021 
1022 		target = ocelot_regmap_init(ocelot, &res);
1023 		if (IS_ERR(target)) {
1024 			dev_err(ocelot->dev,
1025 				"Failed to map device memory space\n");
1026 			kfree(port_phy_modes);
1027 			return PTR_ERR(target);
1028 		}
1029 
1030 		ocelot->targets[i] = target;
1031 	}
1032 
1033 	err = ocelot_regfields_init(ocelot, felix->info->regfields);
1034 	if (err) {
1035 		dev_err(ocelot->dev, "failed to init reg fields map\n");
1036 		kfree(port_phy_modes);
1037 		return err;
1038 	}
1039 
1040 	for (port = 0; port < num_phys_ports; port++) {
1041 		struct ocelot_port *ocelot_port;
1042 		struct regmap *target;
1043 
1044 		ocelot_port = devm_kzalloc(ocelot->dev,
1045 					   sizeof(struct ocelot_port),
1046 					   GFP_KERNEL);
1047 		if (!ocelot_port) {
1048 			dev_err(ocelot->dev,
1049 				"failed to allocate port memory\n");
1050 			kfree(port_phy_modes);
1051 			return -ENOMEM;
1052 		}
1053 
1054 		memcpy(&res, &felix->info->port_io_res[port], sizeof(res));
1055 		res.flags = IORESOURCE_MEM;
1056 		res.start += felix->switch_base;
1057 		res.end += felix->switch_base;
1058 
1059 		target = ocelot_regmap_init(ocelot, &res);
1060 		if (IS_ERR(target)) {
1061 			dev_err(ocelot->dev,
1062 				"Failed to map memory space for port %d\n",
1063 				port);
1064 			kfree(port_phy_modes);
1065 			return PTR_ERR(target);
1066 		}
1067 
1068 		ocelot_port->phy_mode = port_phy_modes[port];
1069 		ocelot_port->ocelot = ocelot;
1070 		ocelot_port->target = target;
1071 		ocelot->ports[port] = ocelot_port;
1072 	}
1073 
1074 	kfree(port_phy_modes);
1075 
1076 	if (felix->info->mdio_bus_alloc) {
1077 		err = felix->info->mdio_bus_alloc(ocelot);
1078 		if (err < 0)
1079 			return err;
1080 	}
1081 
1082 	return 0;
1083 }
1084 
1085 static void ocelot_port_purge_txtstamp_skb(struct ocelot *ocelot, int port,
1086 					   struct sk_buff *skb)
1087 {
1088 	struct ocelot_port *ocelot_port = ocelot->ports[port];
1089 	struct sk_buff *clone = OCELOT_SKB_CB(skb)->clone;
1090 	struct sk_buff *skb_match = NULL, *skb_tmp;
1091 	unsigned long flags;
1092 
1093 	if (!clone)
1094 		return;
1095 
1096 	spin_lock_irqsave(&ocelot_port->tx_skbs.lock, flags);
1097 
1098 	skb_queue_walk_safe(&ocelot_port->tx_skbs, skb, skb_tmp) {
1099 		if (skb != clone)
1100 			continue;
1101 		__skb_unlink(skb, &ocelot_port->tx_skbs);
1102 		skb_match = skb;
1103 		break;
1104 	}
1105 
1106 	spin_unlock_irqrestore(&ocelot_port->tx_skbs.lock, flags);
1107 
1108 	WARN_ONCE(!skb_match,
1109 		  "Could not find skb clone in TX timestamping list\n");
1110 }
1111 
1112 #define work_to_xmit_work(w) \
1113 		container_of((w), struct felix_deferred_xmit_work, work)
1114 
1115 static void felix_port_deferred_xmit(struct kthread_work *work)
1116 {
1117 	struct felix_deferred_xmit_work *xmit_work = work_to_xmit_work(work);
1118 	struct dsa_switch *ds = xmit_work->dp->ds;
1119 	struct sk_buff *skb = xmit_work->skb;
1120 	u32 rew_op = ocelot_ptp_rew_op(skb);
1121 	struct ocelot *ocelot = ds->priv;
1122 	int port = xmit_work->dp->index;
1123 	int retries = 10;
1124 
1125 	do {
1126 		if (ocelot_can_inject(ocelot, 0))
1127 			break;
1128 
1129 		cpu_relax();
1130 	} while (--retries);
1131 
1132 	if (!retries) {
1133 		dev_err(ocelot->dev, "port %d failed to inject skb\n",
1134 			port);
1135 		ocelot_port_purge_txtstamp_skb(ocelot, port, skb);
1136 		kfree_skb(skb);
1137 		return;
1138 	}
1139 
1140 	ocelot_port_inject_frame(ocelot, port, 0, rew_op, skb);
1141 
1142 	consume_skb(skb);
1143 	kfree(xmit_work);
1144 }
1145 
1146 static int felix_port_setup_tagger_data(struct dsa_switch *ds, int port)
1147 {
1148 	struct dsa_port *dp = dsa_to_port(ds, port);
1149 	struct ocelot *ocelot = ds->priv;
1150 	struct felix *felix = ocelot_to_felix(ocelot);
1151 	struct felix_port *felix_port;
1152 
1153 	if (!dsa_port_is_user(dp))
1154 		return 0;
1155 
1156 	felix_port = kzalloc(sizeof(*felix_port), GFP_KERNEL);
1157 	if (!felix_port)
1158 		return -ENOMEM;
1159 
1160 	felix_port->xmit_worker = felix->xmit_worker;
1161 	felix_port->xmit_work_fn = felix_port_deferred_xmit;
1162 
1163 	dp->priv = felix_port;
1164 
1165 	return 0;
1166 }
1167 
1168 static void felix_port_teardown_tagger_data(struct dsa_switch *ds, int port)
1169 {
1170 	struct dsa_port *dp = dsa_to_port(ds, port);
1171 	struct felix_port *felix_port = dp->priv;
1172 
1173 	if (!felix_port)
1174 		return;
1175 
1176 	dp->priv = NULL;
1177 	kfree(felix_port);
1178 }
1179 
1180 /* Hardware initialization done here so that we can allocate structures with
1181  * devm without fear of dsa_register_switch returning -EPROBE_DEFER and causing
1182  * us to allocate structures twice (leak memory) and map PCI memory twice
1183  * (which will not work).
1184  */
1185 static int felix_setup(struct dsa_switch *ds)
1186 {
1187 	struct ocelot *ocelot = ds->priv;
1188 	struct felix *felix = ocelot_to_felix(ocelot);
1189 	int port, err;
1190 
1191 	err = felix_init_structs(felix, ds->num_ports);
1192 	if (err)
1193 		return err;
1194 
1195 	err = ocelot_init(ocelot);
1196 	if (err)
1197 		goto out_mdiobus_free;
1198 
1199 	if (ocelot->ptp) {
1200 		err = ocelot_init_timestamp(ocelot, felix->info->ptp_caps);
1201 		if (err) {
1202 			dev_err(ocelot->dev,
1203 				"Timestamp initialization failed\n");
1204 			ocelot->ptp = 0;
1205 		}
1206 	}
1207 
1208 	felix->xmit_worker = kthread_create_worker(0, "felix_xmit");
1209 	if (IS_ERR(felix->xmit_worker)) {
1210 		err = PTR_ERR(felix->xmit_worker);
1211 		goto out_deinit_timestamp;
1212 	}
1213 
1214 	for (port = 0; port < ds->num_ports; port++) {
1215 		if (dsa_is_unused_port(ds, port))
1216 			continue;
1217 
1218 		ocelot_init_port(ocelot, port);
1219 
1220 		/* Set the default QoS Classification based on PCP and DEI
1221 		 * bits of vlan tag.
1222 		 */
1223 		felix_port_qos_map_init(ocelot, port);
1224 
1225 		err = felix_port_setup_tagger_data(ds, port);
1226 		if (err) {
1227 			dev_err(ds->dev,
1228 				"port %d failed to set up tagger data: %pe\n",
1229 				port, ERR_PTR(err));
1230 			goto out_deinit_ports;
1231 		}
1232 	}
1233 
1234 	err = ocelot_devlink_sb_register(ocelot);
1235 	if (err)
1236 		goto out_deinit_ports;
1237 
1238 	for (port = 0; port < ds->num_ports; port++) {
1239 		if (!dsa_is_cpu_port(ds, port))
1240 			continue;
1241 
1242 		/* The initial tag protocol is NPI which always returns 0, so
1243 		 * there's no real point in checking for errors.
1244 		 */
1245 		felix_set_tag_protocol(ds, port, felix->tag_proto);
1246 		break;
1247 	}
1248 
1249 	ds->mtu_enforcement_ingress = true;
1250 	ds->assisted_learning_on_cpu_port = true;
1251 
1252 	return 0;
1253 
1254 out_deinit_ports:
1255 	for (port = 0; port < ocelot->num_phys_ports; port++) {
1256 		if (dsa_is_unused_port(ds, port))
1257 			continue;
1258 
1259 		felix_port_teardown_tagger_data(ds, port);
1260 		ocelot_deinit_port(ocelot, port);
1261 	}
1262 
1263 	kthread_destroy_worker(felix->xmit_worker);
1264 
1265 out_deinit_timestamp:
1266 	ocelot_deinit_timestamp(ocelot);
1267 	ocelot_deinit(ocelot);
1268 
1269 out_mdiobus_free:
1270 	if (felix->info->mdio_bus_free)
1271 		felix->info->mdio_bus_free(ocelot);
1272 
1273 	return err;
1274 }
1275 
1276 static void felix_teardown(struct dsa_switch *ds)
1277 {
1278 	struct ocelot *ocelot = ds->priv;
1279 	struct felix *felix = ocelot_to_felix(ocelot);
1280 	int port;
1281 
1282 	for (port = 0; port < ds->num_ports; port++) {
1283 		if (!dsa_is_cpu_port(ds, port))
1284 			continue;
1285 
1286 		felix_del_tag_protocol(ds, port, felix->tag_proto);
1287 		break;
1288 	}
1289 
1290 	for (port = 0; port < ocelot->num_phys_ports; port++) {
1291 		if (dsa_is_unused_port(ds, port))
1292 			continue;
1293 
1294 		felix_port_teardown_tagger_data(ds, port);
1295 		ocelot_deinit_port(ocelot, port);
1296 	}
1297 
1298 	kthread_destroy_worker(felix->xmit_worker);
1299 
1300 	ocelot_devlink_sb_unregister(ocelot);
1301 	ocelot_deinit_timestamp(ocelot);
1302 	ocelot_deinit(ocelot);
1303 
1304 	if (felix->info->mdio_bus_free)
1305 		felix->info->mdio_bus_free(ocelot);
1306 }
1307 
1308 static int felix_hwtstamp_get(struct dsa_switch *ds, int port,
1309 			      struct ifreq *ifr)
1310 {
1311 	struct ocelot *ocelot = ds->priv;
1312 
1313 	return ocelot_hwstamp_get(ocelot, port, ifr);
1314 }
1315 
1316 static int felix_hwtstamp_set(struct dsa_switch *ds, int port,
1317 			      struct ifreq *ifr)
1318 {
1319 	struct ocelot *ocelot = ds->priv;
1320 
1321 	return ocelot_hwstamp_set(ocelot, port, ifr);
1322 }
1323 
1324 static bool felix_check_xtr_pkt(struct ocelot *ocelot, unsigned int ptp_type)
1325 {
1326 	struct felix *felix = ocelot_to_felix(ocelot);
1327 	int err, grp = 0;
1328 
1329 	if (felix->tag_proto != DSA_TAG_PROTO_OCELOT_8021Q)
1330 		return false;
1331 
1332 	if (!felix->info->quirk_no_xtr_irq)
1333 		return false;
1334 
1335 	if (ptp_type == PTP_CLASS_NONE)
1336 		return false;
1337 
1338 	while (ocelot_read(ocelot, QS_XTR_DATA_PRESENT) & BIT(grp)) {
1339 		struct sk_buff *skb;
1340 		unsigned int type;
1341 
1342 		err = ocelot_xtr_poll_frame(ocelot, grp, &skb);
1343 		if (err)
1344 			goto out;
1345 
1346 		/* We trap to the CPU port module all PTP frames, but
1347 		 * felix_rxtstamp() only gets called for event frames.
1348 		 * So we need to avoid sending duplicate general
1349 		 * message frames by running a second BPF classifier
1350 		 * here and dropping those.
1351 		 */
1352 		__skb_push(skb, ETH_HLEN);
1353 
1354 		type = ptp_classify_raw(skb);
1355 
1356 		__skb_pull(skb, ETH_HLEN);
1357 
1358 		if (type == PTP_CLASS_NONE) {
1359 			kfree_skb(skb);
1360 			continue;
1361 		}
1362 
1363 		netif_rx(skb);
1364 	}
1365 
1366 out:
1367 	if (err < 0)
1368 		ocelot_drain_cpu_queue(ocelot, 0);
1369 
1370 	return true;
1371 }
1372 
1373 static bool felix_rxtstamp(struct dsa_switch *ds, int port,
1374 			   struct sk_buff *skb, unsigned int type)
1375 {
1376 	u32 tstamp_lo = OCELOT_SKB_CB(skb)->tstamp_lo;
1377 	struct skb_shared_hwtstamps *shhwtstamps;
1378 	struct ocelot *ocelot = ds->priv;
1379 	struct timespec64 ts;
1380 	u32 tstamp_hi;
1381 	u64 tstamp;
1382 
1383 	/* If the "no XTR IRQ" workaround is in use, tell DSA to defer this skb
1384 	 * for RX timestamping. Then free it, and poll for its copy through
1385 	 * MMIO in the CPU port module, and inject that into the stack from
1386 	 * ocelot_xtr_poll().
1387 	 */
1388 	if (felix_check_xtr_pkt(ocelot, type)) {
1389 		kfree_skb(skb);
1390 		return true;
1391 	}
1392 
1393 	ocelot_ptp_gettime64(&ocelot->ptp_info, &ts);
1394 	tstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
1395 
1396 	tstamp_hi = tstamp >> 32;
1397 	if ((tstamp & 0xffffffff) < tstamp_lo)
1398 		tstamp_hi--;
1399 
1400 	tstamp = ((u64)tstamp_hi << 32) | tstamp_lo;
1401 
1402 	shhwtstamps = skb_hwtstamps(skb);
1403 	memset(shhwtstamps, 0, sizeof(struct skb_shared_hwtstamps));
1404 	shhwtstamps->hwtstamp = tstamp;
1405 	return false;
1406 }
1407 
1408 static void felix_txtstamp(struct dsa_switch *ds, int port,
1409 			   struct sk_buff *skb)
1410 {
1411 	struct ocelot *ocelot = ds->priv;
1412 	struct sk_buff *clone = NULL;
1413 
1414 	if (!ocelot->ptp)
1415 		return;
1416 
1417 	if (ocelot_port_txtstamp_request(ocelot, port, skb, &clone)) {
1418 		dev_err_ratelimited(ds->dev,
1419 				    "port %d delivering skb without TX timestamp\n",
1420 				    port);
1421 		return;
1422 	}
1423 
1424 	if (clone)
1425 		OCELOT_SKB_CB(skb)->clone = clone;
1426 }
1427 
1428 static int felix_change_mtu(struct dsa_switch *ds, int port, int new_mtu)
1429 {
1430 	struct ocelot *ocelot = ds->priv;
1431 
1432 	ocelot_port_set_maxlen(ocelot, port, new_mtu);
1433 
1434 	return 0;
1435 }
1436 
1437 static int felix_get_max_mtu(struct dsa_switch *ds, int port)
1438 {
1439 	struct ocelot *ocelot = ds->priv;
1440 
1441 	return ocelot_get_max_mtu(ocelot, port);
1442 }
1443 
1444 static int felix_cls_flower_add(struct dsa_switch *ds, int port,
1445 				struct flow_cls_offload *cls, bool ingress)
1446 {
1447 	struct ocelot *ocelot = ds->priv;
1448 
1449 	return ocelot_cls_flower_replace(ocelot, port, cls, ingress);
1450 }
1451 
1452 static int felix_cls_flower_del(struct dsa_switch *ds, int port,
1453 				struct flow_cls_offload *cls, bool ingress)
1454 {
1455 	struct ocelot *ocelot = ds->priv;
1456 
1457 	return ocelot_cls_flower_destroy(ocelot, port, cls, ingress);
1458 }
1459 
1460 static int felix_cls_flower_stats(struct dsa_switch *ds, int port,
1461 				  struct flow_cls_offload *cls, bool ingress)
1462 {
1463 	struct ocelot *ocelot = ds->priv;
1464 
1465 	return ocelot_cls_flower_stats(ocelot, port, cls, ingress);
1466 }
1467 
1468 static int felix_port_policer_add(struct dsa_switch *ds, int port,
1469 				  struct dsa_mall_policer_tc_entry *policer)
1470 {
1471 	struct ocelot *ocelot = ds->priv;
1472 	struct ocelot_policer pol = {
1473 		.rate = div_u64(policer->rate_bytes_per_sec, 1000) * 8,
1474 		.burst = policer->burst,
1475 	};
1476 
1477 	return ocelot_port_policer_add(ocelot, port, &pol);
1478 }
1479 
1480 static void felix_port_policer_del(struct dsa_switch *ds, int port)
1481 {
1482 	struct ocelot *ocelot = ds->priv;
1483 
1484 	ocelot_port_policer_del(ocelot, port);
1485 }
1486 
1487 static int felix_port_setup_tc(struct dsa_switch *ds, int port,
1488 			       enum tc_setup_type type,
1489 			       void *type_data)
1490 {
1491 	struct ocelot *ocelot = ds->priv;
1492 	struct felix *felix = ocelot_to_felix(ocelot);
1493 
1494 	if (felix->info->port_setup_tc)
1495 		return felix->info->port_setup_tc(ds, port, type, type_data);
1496 	else
1497 		return -EOPNOTSUPP;
1498 }
1499 
1500 static int felix_sb_pool_get(struct dsa_switch *ds, unsigned int sb_index,
1501 			     u16 pool_index,
1502 			     struct devlink_sb_pool_info *pool_info)
1503 {
1504 	struct ocelot *ocelot = ds->priv;
1505 
1506 	return ocelot_sb_pool_get(ocelot, sb_index, pool_index, pool_info);
1507 }
1508 
1509 static int felix_sb_pool_set(struct dsa_switch *ds, unsigned int sb_index,
1510 			     u16 pool_index, u32 size,
1511 			     enum devlink_sb_threshold_type threshold_type,
1512 			     struct netlink_ext_ack *extack)
1513 {
1514 	struct ocelot *ocelot = ds->priv;
1515 
1516 	return ocelot_sb_pool_set(ocelot, sb_index, pool_index, size,
1517 				  threshold_type, extack);
1518 }
1519 
1520 static int felix_sb_port_pool_get(struct dsa_switch *ds, int port,
1521 				  unsigned int sb_index, u16 pool_index,
1522 				  u32 *p_threshold)
1523 {
1524 	struct ocelot *ocelot = ds->priv;
1525 
1526 	return ocelot_sb_port_pool_get(ocelot, port, sb_index, pool_index,
1527 				       p_threshold);
1528 }
1529 
1530 static int felix_sb_port_pool_set(struct dsa_switch *ds, int port,
1531 				  unsigned int sb_index, u16 pool_index,
1532 				  u32 threshold, struct netlink_ext_ack *extack)
1533 {
1534 	struct ocelot *ocelot = ds->priv;
1535 
1536 	return ocelot_sb_port_pool_set(ocelot, port, sb_index, pool_index,
1537 				       threshold, extack);
1538 }
1539 
1540 static int felix_sb_tc_pool_bind_get(struct dsa_switch *ds, int port,
1541 				     unsigned int sb_index, u16 tc_index,
1542 				     enum devlink_sb_pool_type pool_type,
1543 				     u16 *p_pool_index, u32 *p_threshold)
1544 {
1545 	struct ocelot *ocelot = ds->priv;
1546 
1547 	return ocelot_sb_tc_pool_bind_get(ocelot, port, sb_index, tc_index,
1548 					  pool_type, p_pool_index,
1549 					  p_threshold);
1550 }
1551 
1552 static int felix_sb_tc_pool_bind_set(struct dsa_switch *ds, int port,
1553 				     unsigned int sb_index, u16 tc_index,
1554 				     enum devlink_sb_pool_type pool_type,
1555 				     u16 pool_index, u32 threshold,
1556 				     struct netlink_ext_ack *extack)
1557 {
1558 	struct ocelot *ocelot = ds->priv;
1559 
1560 	return ocelot_sb_tc_pool_bind_set(ocelot, port, sb_index, tc_index,
1561 					  pool_type, pool_index, threshold,
1562 					  extack);
1563 }
1564 
1565 static int felix_sb_occ_snapshot(struct dsa_switch *ds,
1566 				 unsigned int sb_index)
1567 {
1568 	struct ocelot *ocelot = ds->priv;
1569 
1570 	return ocelot_sb_occ_snapshot(ocelot, sb_index);
1571 }
1572 
1573 static int felix_sb_occ_max_clear(struct dsa_switch *ds,
1574 				  unsigned int sb_index)
1575 {
1576 	struct ocelot *ocelot = ds->priv;
1577 
1578 	return ocelot_sb_occ_max_clear(ocelot, sb_index);
1579 }
1580 
1581 static int felix_sb_occ_port_pool_get(struct dsa_switch *ds, int port,
1582 				      unsigned int sb_index, u16 pool_index,
1583 				      u32 *p_cur, u32 *p_max)
1584 {
1585 	struct ocelot *ocelot = ds->priv;
1586 
1587 	return ocelot_sb_occ_port_pool_get(ocelot, port, sb_index, pool_index,
1588 					   p_cur, p_max);
1589 }
1590 
1591 static int felix_sb_occ_tc_port_bind_get(struct dsa_switch *ds, int port,
1592 					 unsigned int sb_index, u16 tc_index,
1593 					 enum devlink_sb_pool_type pool_type,
1594 					 u32 *p_cur, u32 *p_max)
1595 {
1596 	struct ocelot *ocelot = ds->priv;
1597 
1598 	return ocelot_sb_occ_tc_port_bind_get(ocelot, port, sb_index, tc_index,
1599 					      pool_type, p_cur, p_max);
1600 }
1601 
1602 static int felix_mrp_add(struct dsa_switch *ds, int port,
1603 			 const struct switchdev_obj_mrp *mrp)
1604 {
1605 	struct ocelot *ocelot = ds->priv;
1606 
1607 	return ocelot_mrp_add(ocelot, port, mrp);
1608 }
1609 
1610 static int felix_mrp_del(struct dsa_switch *ds, int port,
1611 			 const struct switchdev_obj_mrp *mrp)
1612 {
1613 	struct ocelot *ocelot = ds->priv;
1614 
1615 	return ocelot_mrp_add(ocelot, port, mrp);
1616 }
1617 
1618 static int
1619 felix_mrp_add_ring_role(struct dsa_switch *ds, int port,
1620 			const struct switchdev_obj_ring_role_mrp *mrp)
1621 {
1622 	struct ocelot *ocelot = ds->priv;
1623 
1624 	return ocelot_mrp_add_ring_role(ocelot, port, mrp);
1625 }
1626 
1627 static int
1628 felix_mrp_del_ring_role(struct dsa_switch *ds, int port,
1629 			const struct switchdev_obj_ring_role_mrp *mrp)
1630 {
1631 	struct ocelot *ocelot = ds->priv;
1632 
1633 	return ocelot_mrp_del_ring_role(ocelot, port, mrp);
1634 }
1635 
1636 const struct dsa_switch_ops felix_switch_ops = {
1637 	.get_tag_protocol		= felix_get_tag_protocol,
1638 	.change_tag_protocol		= felix_change_tag_protocol,
1639 	.setup				= felix_setup,
1640 	.teardown			= felix_teardown,
1641 	.set_ageing_time		= felix_set_ageing_time,
1642 	.get_strings			= felix_get_strings,
1643 	.get_ethtool_stats		= felix_get_ethtool_stats,
1644 	.get_sset_count			= felix_get_sset_count,
1645 	.get_ts_info			= felix_get_ts_info,
1646 	.phylink_validate		= felix_phylink_validate,
1647 	.phylink_mac_config		= felix_phylink_mac_config,
1648 	.phylink_mac_link_down		= felix_phylink_mac_link_down,
1649 	.phylink_mac_link_up		= felix_phylink_mac_link_up,
1650 	.port_fdb_dump			= felix_fdb_dump,
1651 	.port_fdb_add			= felix_fdb_add,
1652 	.port_fdb_del			= felix_fdb_del,
1653 	.port_mdb_add			= felix_mdb_add,
1654 	.port_mdb_del			= felix_mdb_del,
1655 	.port_pre_bridge_flags		= felix_pre_bridge_flags,
1656 	.port_bridge_flags		= felix_bridge_flags,
1657 	.port_bridge_join		= felix_bridge_join,
1658 	.port_bridge_leave		= felix_bridge_leave,
1659 	.port_lag_join			= felix_lag_join,
1660 	.port_lag_leave			= felix_lag_leave,
1661 	.port_lag_change		= felix_lag_change,
1662 	.port_stp_state_set		= felix_bridge_stp_state_set,
1663 	.port_vlan_filtering		= felix_vlan_filtering,
1664 	.port_vlan_add			= felix_vlan_add,
1665 	.port_vlan_del			= felix_vlan_del,
1666 	.port_hwtstamp_get		= felix_hwtstamp_get,
1667 	.port_hwtstamp_set		= felix_hwtstamp_set,
1668 	.port_rxtstamp			= felix_rxtstamp,
1669 	.port_txtstamp			= felix_txtstamp,
1670 	.port_change_mtu		= felix_change_mtu,
1671 	.port_max_mtu			= felix_get_max_mtu,
1672 	.port_policer_add		= felix_port_policer_add,
1673 	.port_policer_del		= felix_port_policer_del,
1674 	.cls_flower_add			= felix_cls_flower_add,
1675 	.cls_flower_del			= felix_cls_flower_del,
1676 	.cls_flower_stats		= felix_cls_flower_stats,
1677 	.port_setup_tc			= felix_port_setup_tc,
1678 	.devlink_sb_pool_get		= felix_sb_pool_get,
1679 	.devlink_sb_pool_set		= felix_sb_pool_set,
1680 	.devlink_sb_port_pool_get	= felix_sb_port_pool_get,
1681 	.devlink_sb_port_pool_set	= felix_sb_port_pool_set,
1682 	.devlink_sb_tc_pool_bind_get	= felix_sb_tc_pool_bind_get,
1683 	.devlink_sb_tc_pool_bind_set	= felix_sb_tc_pool_bind_set,
1684 	.devlink_sb_occ_snapshot	= felix_sb_occ_snapshot,
1685 	.devlink_sb_occ_max_clear	= felix_sb_occ_max_clear,
1686 	.devlink_sb_occ_port_pool_get	= felix_sb_occ_port_pool_get,
1687 	.devlink_sb_occ_tc_port_bind_get= felix_sb_occ_tc_port_bind_get,
1688 	.port_mrp_add			= felix_mrp_add,
1689 	.port_mrp_del			= felix_mrp_del,
1690 	.port_mrp_add_ring_role		= felix_mrp_add_ring_role,
1691 	.port_mrp_del_ring_role		= felix_mrp_del_ring_role,
1692 	.tag_8021q_vlan_add		= felix_tag_8021q_vlan_add,
1693 	.tag_8021q_vlan_del		= felix_tag_8021q_vlan_del,
1694 };
1695 
1696 struct net_device *felix_port_to_netdev(struct ocelot *ocelot, int port)
1697 {
1698 	struct felix *felix = ocelot_to_felix(ocelot);
1699 	struct dsa_switch *ds = felix->ds;
1700 
1701 	if (!dsa_is_user_port(ds, port))
1702 		return NULL;
1703 
1704 	return dsa_to_port(ds, port)->slave;
1705 }
1706 
1707 int felix_netdev_to_port(struct net_device *dev)
1708 {
1709 	struct dsa_port *dp;
1710 
1711 	dp = dsa_port_from_netdev(dev);
1712 	if (IS_ERR(dp))
1713 		return -EINVAL;
1714 
1715 	return dp->index;
1716 }
1717