xref: /openbmc/linux/drivers/net/ethernet/mscc/ocelot.c (revision ae40e94f)
1 // SPDX-License-Identifier: (GPL-2.0 OR MIT)
2 /*
3  * Microsemi Ocelot Switch driver
4  *
5  * Copyright (c) 2017 Microsemi Corporation
6  */
7 #include <linux/etherdevice.h>
8 #include <linux/ethtool.h>
9 #include <linux/if_bridge.h>
10 #include <linux/if_ether.h>
11 #include <linux/if_vlan.h>
12 #include <linux/interrupt.h>
13 #include <linux/kernel.h>
14 #include <linux/module.h>
15 #include <linux/netdevice.h>
16 #include <linux/phy.h>
17 #include <linux/skbuff.h>
18 #include <linux/iopoll.h>
19 #include <net/arp.h>
20 #include <net/netevent.h>
21 #include <net/rtnetlink.h>
22 #include <net/switchdev.h>
23 
24 #include "ocelot.h"
25 
26 #define TABLE_UPDATE_SLEEP_US 10
27 #define TABLE_UPDATE_TIMEOUT_US 100000
28 
29 /* MAC table entry types.
30  * ENTRYTYPE_NORMAL is subject to aging.
31  * ENTRYTYPE_LOCKED is not subject to aging.
32  * ENTRYTYPE_MACv4 is not subject to aging. For IPv4 multicast.
33  * ENTRYTYPE_MACv6 is not subject to aging. For IPv6 multicast.
34  */
35 enum macaccess_entry_type {
36 	ENTRYTYPE_NORMAL = 0,
37 	ENTRYTYPE_LOCKED,
38 	ENTRYTYPE_MACv4,
39 	ENTRYTYPE_MACv6,
40 };
41 
42 struct ocelot_mact_entry {
43 	u8 mac[ETH_ALEN];
44 	u16 vid;
45 	enum macaccess_entry_type type;
46 };
47 
48 static inline u32 ocelot_mact_read_macaccess(struct ocelot *ocelot)
49 {
50 	return ocelot_read(ocelot, ANA_TABLES_MACACCESS);
51 }
52 
53 static inline int ocelot_mact_wait_for_completion(struct ocelot *ocelot)
54 {
55 	u32 val;
56 
57 	return readx_poll_timeout(ocelot_mact_read_macaccess,
58 		ocelot, val,
59 		(val & ANA_TABLES_MACACCESS_MAC_TABLE_CMD_M) ==
60 		MACACCESS_CMD_IDLE,
61 		TABLE_UPDATE_SLEEP_US, TABLE_UPDATE_TIMEOUT_US);
62 }
63 
64 static void ocelot_mact_select(struct ocelot *ocelot,
65 			       const unsigned char mac[ETH_ALEN],
66 			       unsigned int vid)
67 {
68 	u32 macl = 0, mach = 0;
69 
70 	/* Set the MAC address to handle and the vlan associated in a format
71 	 * understood by the hardware.
72 	 */
73 	mach |= vid    << 16;
74 	mach |= mac[0] << 8;
75 	mach |= mac[1] << 0;
76 	macl |= mac[2] << 24;
77 	macl |= mac[3] << 16;
78 	macl |= mac[4] << 8;
79 	macl |= mac[5] << 0;
80 
81 	ocelot_write(ocelot, macl, ANA_TABLES_MACLDATA);
82 	ocelot_write(ocelot, mach, ANA_TABLES_MACHDATA);
83 
84 }
85 
86 static int ocelot_mact_learn(struct ocelot *ocelot, int port,
87 			     const unsigned char mac[ETH_ALEN],
88 			     unsigned int vid,
89 			     enum macaccess_entry_type type)
90 {
91 	ocelot_mact_select(ocelot, mac, vid);
92 
93 	/* Issue a write command */
94 	ocelot_write(ocelot, ANA_TABLES_MACACCESS_VALID |
95 			     ANA_TABLES_MACACCESS_DEST_IDX(port) |
96 			     ANA_TABLES_MACACCESS_ENTRYTYPE(type) |
97 			     ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_LEARN),
98 			     ANA_TABLES_MACACCESS);
99 
100 	return ocelot_mact_wait_for_completion(ocelot);
101 }
102 
103 static int ocelot_mact_forget(struct ocelot *ocelot,
104 			      const unsigned char mac[ETH_ALEN],
105 			      unsigned int vid)
106 {
107 	ocelot_mact_select(ocelot, mac, vid);
108 
109 	/* Issue a forget command */
110 	ocelot_write(ocelot,
111 		     ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_FORGET),
112 		     ANA_TABLES_MACACCESS);
113 
114 	return ocelot_mact_wait_for_completion(ocelot);
115 }
116 
117 static void ocelot_mact_init(struct ocelot *ocelot)
118 {
119 	/* Configure the learning mode entries attributes:
120 	 * - Do not copy the frame to the CPU extraction queues.
121 	 * - Use the vlan and mac_cpoy for dmac lookup.
122 	 */
123 	ocelot_rmw(ocelot, 0,
124 		   ANA_AGENCTRL_LEARN_CPU_COPY | ANA_AGENCTRL_IGNORE_DMAC_FLAGS
125 		   | ANA_AGENCTRL_LEARN_FWD_KILL
126 		   | ANA_AGENCTRL_LEARN_IGNORE_VLAN,
127 		   ANA_AGENCTRL);
128 
129 	/* Clear the MAC table */
130 	ocelot_write(ocelot, MACACCESS_CMD_INIT, ANA_TABLES_MACACCESS);
131 }
132 
133 static inline u32 ocelot_vlant_read_vlanaccess(struct ocelot *ocelot)
134 {
135 	return ocelot_read(ocelot, ANA_TABLES_VLANACCESS);
136 }
137 
138 static inline int ocelot_vlant_wait_for_completion(struct ocelot *ocelot)
139 {
140 	u32 val;
141 
142 	return readx_poll_timeout(ocelot_vlant_read_vlanaccess,
143 		ocelot,
144 		val,
145 		(val & ANA_TABLES_VLANACCESS_VLAN_TBL_CMD_M) ==
146 		ANA_TABLES_VLANACCESS_CMD_IDLE,
147 		TABLE_UPDATE_SLEEP_US, TABLE_UPDATE_TIMEOUT_US);
148 }
149 
150 static int ocelot_vlant_set_mask(struct ocelot *ocelot, u16 vid, u32 mask)
151 {
152 	/* Select the VID to configure */
153 	ocelot_write(ocelot, ANA_TABLES_VLANTIDX_V_INDEX(vid),
154 		     ANA_TABLES_VLANTIDX);
155 	/* Set the vlan port members mask and issue a write command */
156 	ocelot_write(ocelot, ANA_TABLES_VLANACCESS_VLAN_PORT_MASK(mask) |
157 			     ANA_TABLES_VLANACCESS_CMD_WRITE,
158 		     ANA_TABLES_VLANACCESS);
159 
160 	return ocelot_vlant_wait_for_completion(ocelot);
161 }
162 
163 static void ocelot_vlan_mode(struct ocelot_port *port,
164 			     netdev_features_t features)
165 {
166 	struct ocelot *ocelot = port->ocelot;
167 	u8 p = port->chip_port;
168 	u32 val;
169 
170 	/* Filtering */
171 	val = ocelot_read(ocelot, ANA_VLANMASK);
172 	if (features & NETIF_F_HW_VLAN_CTAG_FILTER)
173 		val |= BIT(p);
174 	else
175 		val &= ~BIT(p);
176 	ocelot_write(ocelot, val, ANA_VLANMASK);
177 }
178 
179 static void ocelot_vlan_port_apply(struct ocelot *ocelot,
180 				   struct ocelot_port *port)
181 {
182 	u32 val;
183 
184 	/* Ingress clasification (ANA_PORT_VLAN_CFG) */
185 	/* Default vlan to clasify for untagged frames (may be zero) */
186 	val = ANA_PORT_VLAN_CFG_VLAN_VID(port->pvid);
187 	if (port->vlan_aware)
188 		val |= ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
189 		       ANA_PORT_VLAN_CFG_VLAN_POP_CNT(1);
190 
191 	ocelot_rmw_gix(ocelot, val,
192 		       ANA_PORT_VLAN_CFG_VLAN_VID_M |
193 		       ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
194 		       ANA_PORT_VLAN_CFG_VLAN_POP_CNT_M,
195 		       ANA_PORT_VLAN_CFG, port->chip_port);
196 
197 	/* Drop frames with multicast source address */
198 	val = ANA_PORT_DROP_CFG_DROP_MC_SMAC_ENA;
199 	if (port->vlan_aware && !port->vid)
200 		/* If port is vlan-aware and tagged, drop untagged and priority
201 		 * tagged frames.
202 		 */
203 		val |= ANA_PORT_DROP_CFG_DROP_UNTAGGED_ENA |
204 		       ANA_PORT_DROP_CFG_DROP_PRIO_S_TAGGED_ENA |
205 		       ANA_PORT_DROP_CFG_DROP_PRIO_C_TAGGED_ENA;
206 	ocelot_write_gix(ocelot, val, ANA_PORT_DROP_CFG, port->chip_port);
207 
208 	/* Egress configuration (REW_TAG_CFG): VLAN tag type to 8021Q. */
209 	val = REW_TAG_CFG_TAG_TPID_CFG(0);
210 
211 	if (port->vlan_aware) {
212 		if (port->vid)
213 			/* Tag all frames except when VID == DEFAULT_VLAN */
214 			val |= REW_TAG_CFG_TAG_CFG(1);
215 		else
216 			/* Tag all frames */
217 			val |= REW_TAG_CFG_TAG_CFG(3);
218 	}
219 	ocelot_rmw_gix(ocelot, val,
220 		       REW_TAG_CFG_TAG_TPID_CFG_M |
221 		       REW_TAG_CFG_TAG_CFG_M,
222 		       REW_TAG_CFG, port->chip_port);
223 
224 	/* Set default VLAN and tag type to 8021Q. */
225 	val = REW_PORT_VLAN_CFG_PORT_TPID(ETH_P_8021Q) |
226 	      REW_PORT_VLAN_CFG_PORT_VID(port->vid);
227 	ocelot_rmw_gix(ocelot, val,
228 		       REW_PORT_VLAN_CFG_PORT_TPID_M |
229 		       REW_PORT_VLAN_CFG_PORT_VID_M,
230 		       REW_PORT_VLAN_CFG, port->chip_port);
231 }
232 
233 static int ocelot_vlan_vid_add(struct net_device *dev, u16 vid, bool pvid,
234 			       bool untagged)
235 {
236 	struct ocelot_port *port = netdev_priv(dev);
237 	struct ocelot *ocelot = port->ocelot;
238 	int ret;
239 
240 	/* Add the port MAC address to with the right VLAN information */
241 	ocelot_mact_learn(ocelot, PGID_CPU, dev->dev_addr, vid,
242 			  ENTRYTYPE_LOCKED);
243 
244 	/* Make the port a member of the VLAN */
245 	ocelot->vlan_mask[vid] |= BIT(port->chip_port);
246 	ret = ocelot_vlant_set_mask(ocelot, vid, ocelot->vlan_mask[vid]);
247 	if (ret)
248 		return ret;
249 
250 	/* Default ingress vlan classification */
251 	if (pvid)
252 		port->pvid = vid;
253 
254 	/* Untagged egress vlan clasification */
255 	if (untagged)
256 		port->vid = vid;
257 
258 	ocelot_vlan_port_apply(ocelot, port);
259 
260 	return 0;
261 }
262 
263 static int ocelot_vlan_vid_del(struct net_device *dev, u16 vid)
264 {
265 	struct ocelot_port *port = netdev_priv(dev);
266 	struct ocelot *ocelot = port->ocelot;
267 	int ret;
268 
269 	/* 8021q removes VID 0 on module unload for all interfaces
270 	 * with VLAN filtering feature. We need to keep it to receive
271 	 * untagged traffic.
272 	 */
273 	if (vid == 0)
274 		return 0;
275 
276 	/* Del the port MAC address to with the right VLAN information */
277 	ocelot_mact_forget(ocelot, dev->dev_addr, vid);
278 
279 	/* Stop the port from being a member of the vlan */
280 	ocelot->vlan_mask[vid] &= ~BIT(port->chip_port);
281 	ret = ocelot_vlant_set_mask(ocelot, vid, ocelot->vlan_mask[vid]);
282 	if (ret)
283 		return ret;
284 
285 	/* Ingress */
286 	if (port->pvid == vid)
287 		port->pvid = 0;
288 
289 	/* Egress */
290 	if (port->vid == vid)
291 		port->vid = 0;
292 
293 	ocelot_vlan_port_apply(ocelot, port);
294 
295 	return 0;
296 }
297 
298 static void ocelot_vlan_init(struct ocelot *ocelot)
299 {
300 	u16 port, vid;
301 
302 	/* Clear VLAN table, by default all ports are members of all VLANs */
303 	ocelot_write(ocelot, ANA_TABLES_VLANACCESS_CMD_INIT,
304 		     ANA_TABLES_VLANACCESS);
305 	ocelot_vlant_wait_for_completion(ocelot);
306 
307 	/* Configure the port VLAN memberships */
308 	for (vid = 1; vid < VLAN_N_VID; vid++) {
309 		ocelot->vlan_mask[vid] = 0;
310 		ocelot_vlant_set_mask(ocelot, vid, ocelot->vlan_mask[vid]);
311 	}
312 
313 	/* Because VLAN filtering is enabled, we need VID 0 to get untagged
314 	 * traffic.  It is added automatically if 8021q module is loaded, but
315 	 * we can't rely on it since module may be not loaded.
316 	 */
317 	ocelot->vlan_mask[0] = GENMASK(ocelot->num_phys_ports - 1, 0);
318 	ocelot_vlant_set_mask(ocelot, 0, ocelot->vlan_mask[0]);
319 
320 	/* Configure the CPU port to be VLAN aware */
321 	ocelot_write_gix(ocelot, ANA_PORT_VLAN_CFG_VLAN_VID(0) |
322 				 ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
323 				 ANA_PORT_VLAN_CFG_VLAN_POP_CNT(1),
324 			 ANA_PORT_VLAN_CFG, ocelot->num_phys_ports);
325 
326 	/* Set vlan ingress filter mask to all ports but the CPU port by
327 	 * default.
328 	 */
329 	ocelot_write(ocelot, GENMASK(9, 0), ANA_VLANMASK);
330 
331 	for (port = 0; port < ocelot->num_phys_ports; port++) {
332 		ocelot_write_gix(ocelot, 0, REW_PORT_VLAN_CFG, port);
333 		ocelot_write_gix(ocelot, 0, REW_TAG_CFG, port);
334 	}
335 }
336 
337 /* Watermark encode
338  * Bit 8:   Unit; 0:1, 1:16
339  * Bit 7-0: Value to be multiplied with unit
340  */
341 static u16 ocelot_wm_enc(u16 value)
342 {
343 	if (value >= BIT(8))
344 		return BIT(8) | (value / 16);
345 
346 	return value;
347 }
348 
349 static void ocelot_port_adjust_link(struct net_device *dev)
350 {
351 	struct ocelot_port *port = netdev_priv(dev);
352 	struct ocelot *ocelot = port->ocelot;
353 	u8 p = port->chip_port;
354 	int speed, atop_wm, mode = 0;
355 
356 	switch (dev->phydev->speed) {
357 	case SPEED_10:
358 		speed = OCELOT_SPEED_10;
359 		break;
360 	case SPEED_100:
361 		speed = OCELOT_SPEED_100;
362 		break;
363 	case SPEED_1000:
364 		speed = OCELOT_SPEED_1000;
365 		mode = DEV_MAC_MODE_CFG_GIGA_MODE_ENA;
366 		break;
367 	case SPEED_2500:
368 		speed = OCELOT_SPEED_2500;
369 		mode = DEV_MAC_MODE_CFG_GIGA_MODE_ENA;
370 		break;
371 	default:
372 		netdev_err(dev, "Unsupported PHY speed: %d\n",
373 			   dev->phydev->speed);
374 		return;
375 	}
376 
377 	phy_print_status(dev->phydev);
378 
379 	if (!dev->phydev->link)
380 		return;
381 
382 	/* Only full duplex supported for now */
383 	ocelot_port_writel(port, DEV_MAC_MODE_CFG_FDX_ENA |
384 			   mode, DEV_MAC_MODE_CFG);
385 
386 	/* Set MAC IFG Gaps
387 	 * FDX: TX_IFG = 5, RX_IFG1 = RX_IFG2 = 0
388 	 * !FDX: TX_IFG = 5, RX_IFG1 = RX_IFG2 = 5
389 	 */
390 	ocelot_port_writel(port, DEV_MAC_IFG_CFG_TX_IFG(5), DEV_MAC_IFG_CFG);
391 
392 	/* Load seed (0) and set MAC HDX late collision  */
393 	ocelot_port_writel(port, DEV_MAC_HDX_CFG_LATE_COL_POS(67) |
394 			   DEV_MAC_HDX_CFG_SEED_LOAD,
395 			   DEV_MAC_HDX_CFG);
396 	mdelay(1);
397 	ocelot_port_writel(port, DEV_MAC_HDX_CFG_LATE_COL_POS(67),
398 			   DEV_MAC_HDX_CFG);
399 
400 	/* Disable HDX fast control */
401 	ocelot_port_writel(port, DEV_PORT_MISC_HDX_FAST_DIS, DEV_PORT_MISC);
402 
403 	/* SGMII only for now */
404 	ocelot_port_writel(port, PCS1G_MODE_CFG_SGMII_MODE_ENA, PCS1G_MODE_CFG);
405 	ocelot_port_writel(port, PCS1G_SD_CFG_SD_SEL, PCS1G_SD_CFG);
406 
407 	/* Enable PCS */
408 	ocelot_port_writel(port, PCS1G_CFG_PCS_ENA, PCS1G_CFG);
409 
410 	/* No aneg on SGMII */
411 	ocelot_port_writel(port, 0, PCS1G_ANEG_CFG);
412 
413 	/* No loopback */
414 	ocelot_port_writel(port, 0, PCS1G_LB_CFG);
415 
416 	/* Set Max Length and maximum tags allowed */
417 	ocelot_port_writel(port, VLAN_ETH_FRAME_LEN, DEV_MAC_MAXLEN_CFG);
418 	ocelot_port_writel(port, DEV_MAC_TAGS_CFG_TAG_ID(ETH_P_8021AD) |
419 			   DEV_MAC_TAGS_CFG_VLAN_AWR_ENA |
420 			   DEV_MAC_TAGS_CFG_VLAN_LEN_AWR_ENA,
421 			   DEV_MAC_TAGS_CFG);
422 
423 	/* Enable MAC module */
424 	ocelot_port_writel(port, DEV_MAC_ENA_CFG_RX_ENA |
425 			   DEV_MAC_ENA_CFG_TX_ENA, DEV_MAC_ENA_CFG);
426 
427 	/* Take MAC, Port, Phy (intern) and PCS (SGMII/Serdes) clock out of
428 	 * reset */
429 	ocelot_port_writel(port, DEV_CLOCK_CFG_LINK_SPEED(speed),
430 			   DEV_CLOCK_CFG);
431 
432 	/* Set SMAC of Pause frame (00:00:00:00:00:00) */
433 	ocelot_port_writel(port, 0, DEV_MAC_FC_MAC_HIGH_CFG);
434 	ocelot_port_writel(port, 0, DEV_MAC_FC_MAC_LOW_CFG);
435 
436 	/* No PFC */
437 	ocelot_write_gix(ocelot, ANA_PFC_PFC_CFG_FC_LINK_SPEED(speed),
438 			 ANA_PFC_PFC_CFG, p);
439 
440 	/* Set Pause WM hysteresis
441 	 * 152 = 6 * VLAN_ETH_FRAME_LEN / OCELOT_BUFFER_CELL_SZ
442 	 * 101 = 4 * VLAN_ETH_FRAME_LEN / OCELOT_BUFFER_CELL_SZ
443 	 */
444 	ocelot_write_rix(ocelot, SYS_PAUSE_CFG_PAUSE_ENA |
445 			 SYS_PAUSE_CFG_PAUSE_STOP(101) |
446 			 SYS_PAUSE_CFG_PAUSE_START(152), SYS_PAUSE_CFG, p);
447 
448 	/* Core: Enable port for frame transfer */
449 	ocelot_write_rix(ocelot, QSYS_SWITCH_PORT_MODE_INGRESS_DROP_MODE |
450 			 QSYS_SWITCH_PORT_MODE_SCH_NEXT_CFG(1) |
451 			 QSYS_SWITCH_PORT_MODE_PORT_ENA,
452 			 QSYS_SWITCH_PORT_MODE, p);
453 
454 	/* Flow control */
455 	ocelot_write_rix(ocelot, SYS_MAC_FC_CFG_PAUSE_VAL_CFG(0xffff) |
456 			 SYS_MAC_FC_CFG_RX_FC_ENA | SYS_MAC_FC_CFG_TX_FC_ENA |
457 			 SYS_MAC_FC_CFG_ZERO_PAUSE_ENA |
458 			 SYS_MAC_FC_CFG_FC_LATENCY_CFG(0x7) |
459 			 SYS_MAC_FC_CFG_FC_LINK_SPEED(speed),
460 			 SYS_MAC_FC_CFG, p);
461 	ocelot_write_rix(ocelot, 0, ANA_POL_FLOWC, p);
462 
463 	/* Tail dropping watermark */
464 	atop_wm = (ocelot->shared_queue_sz - 9 * VLAN_ETH_FRAME_LEN) / OCELOT_BUFFER_CELL_SZ;
465 	ocelot_write_rix(ocelot, ocelot_wm_enc(9 * VLAN_ETH_FRAME_LEN),
466 			 SYS_ATOP, p);
467 	ocelot_write(ocelot, ocelot_wm_enc(atop_wm), SYS_ATOP_TOT_CFG);
468 }
469 
470 static int ocelot_port_open(struct net_device *dev)
471 {
472 	struct ocelot_port *port = netdev_priv(dev);
473 	struct ocelot *ocelot = port->ocelot;
474 	int err;
475 
476 	/* Enable receiving frames on the port, and activate auto-learning of
477 	 * MAC addresses.
478 	 */
479 	ocelot_write_gix(ocelot, ANA_PORT_PORT_CFG_LEARNAUTO |
480 			 ANA_PORT_PORT_CFG_RECV_ENA |
481 			 ANA_PORT_PORT_CFG_PORTID_VAL(port->chip_port),
482 			 ANA_PORT_PORT_CFG, port->chip_port);
483 
484 	if (port->serdes) {
485 		err = phy_set_mode_ext(port->serdes, PHY_MODE_ETHERNET,
486 				       port->phy_mode);
487 		if (err) {
488 			netdev_err(dev, "Could not set mode of SerDes\n");
489 			return err;
490 		}
491 	}
492 
493 	err = phy_connect_direct(dev, port->phy, &ocelot_port_adjust_link,
494 				 port->phy_mode);
495 	if (err) {
496 		netdev_err(dev, "Could not attach to PHY\n");
497 		return err;
498 	}
499 
500 	dev->phydev = port->phy;
501 
502 	phy_attached_info(port->phy);
503 	phy_start(port->phy);
504 	return 0;
505 }
506 
507 static int ocelot_port_stop(struct net_device *dev)
508 {
509 	struct ocelot_port *port = netdev_priv(dev);
510 
511 	phy_disconnect(port->phy);
512 
513 	dev->phydev = NULL;
514 
515 	ocelot_port_writel(port, 0, DEV_MAC_ENA_CFG);
516 	ocelot_rmw_rix(port->ocelot, 0, QSYS_SWITCH_PORT_MODE_PORT_ENA,
517 			 QSYS_SWITCH_PORT_MODE, port->chip_port);
518 	return 0;
519 }
520 
521 /* Generate the IFH for frame injection
522  *
523  * The IFH is a 128bit-value
524  * bit 127: bypass the analyzer processing
525  * bit 56-67: destination mask
526  * bit 28-29: pop_cnt: 3 disables all rewriting of the frame
527  * bit 20-27: cpu extraction queue mask
528  * bit 16: tag type 0: C-tag, 1: S-tag
529  * bit 0-11: VID
530  */
531 static int ocelot_gen_ifh(u32 *ifh, struct frame_info *info)
532 {
533 	ifh[0] = IFH_INJ_BYPASS;
534 	ifh[1] = (0xf00 & info->port) >> 8;
535 	ifh[2] = (0xff & info->port) << 24;
536 	ifh[3] = (info->tag_type << 16) | info->vid;
537 
538 	return 0;
539 }
540 
541 static int ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
542 {
543 	struct ocelot_port *port = netdev_priv(dev);
544 	struct ocelot *ocelot = port->ocelot;
545 	u32 val, ifh[IFH_LEN];
546 	struct frame_info info = {};
547 	u8 grp = 0; /* Send everything on CPU group 0 */
548 	unsigned int i, count, last;
549 
550 	val = ocelot_read(ocelot, QS_INJ_STATUS);
551 	if (!(val & QS_INJ_STATUS_FIFO_RDY(BIT(grp))) ||
552 	    (val & QS_INJ_STATUS_WMARK_REACHED(BIT(grp))))
553 		return NETDEV_TX_BUSY;
554 
555 	ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
556 			 QS_INJ_CTRL_SOF, QS_INJ_CTRL, grp);
557 
558 	info.port = BIT(port->chip_port);
559 	info.tag_type = IFH_TAG_TYPE_C;
560 	info.vid = skb_vlan_tag_get(skb);
561 	ocelot_gen_ifh(ifh, &info);
562 
563 	for (i = 0; i < IFH_LEN; i++)
564 		ocelot_write_rix(ocelot, (__force u32)cpu_to_be32(ifh[i]),
565 				 QS_INJ_WR, grp);
566 
567 	count = (skb->len + 3) / 4;
568 	last = skb->len % 4;
569 	for (i = 0; i < count; i++) {
570 		ocelot_write_rix(ocelot, ((u32 *)skb->data)[i], QS_INJ_WR, grp);
571 	}
572 
573 	/* Add padding */
574 	while (i < (OCELOT_BUFFER_CELL_SZ / 4)) {
575 		ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
576 		i++;
577 	}
578 
579 	/* Indicate EOF and valid bytes in last word */
580 	ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
581 			 QS_INJ_CTRL_VLD_BYTES(skb->len < OCELOT_BUFFER_CELL_SZ ? 0 : last) |
582 			 QS_INJ_CTRL_EOF,
583 			 QS_INJ_CTRL, grp);
584 
585 	/* Add dummy CRC */
586 	ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
587 	skb_tx_timestamp(skb);
588 
589 	dev->stats.tx_packets++;
590 	dev->stats.tx_bytes += skb->len;
591 	dev_kfree_skb_any(skb);
592 
593 	return NETDEV_TX_OK;
594 }
595 
596 static void ocelot_mact_mc_reset(struct ocelot_port *port)
597 {
598 	struct ocelot *ocelot = port->ocelot;
599 	struct netdev_hw_addr *ha, *n;
600 
601 	/* Free and forget all the MAC addresses stored in the port private mc
602 	 * list. These are mc addresses that were previously added by calling
603 	 * ocelot_mact_mc_add().
604 	 */
605 	list_for_each_entry_safe(ha, n, &port->mc, list) {
606 		ocelot_mact_forget(ocelot, ha->addr, port->pvid);
607 		list_del(&ha->list);
608 		kfree(ha);
609 	}
610 }
611 
612 static int ocelot_mact_mc_add(struct ocelot_port *port,
613 			      struct netdev_hw_addr *hw_addr)
614 {
615 	struct ocelot *ocelot = port->ocelot;
616 	struct netdev_hw_addr *ha = kzalloc(sizeof(*ha), GFP_KERNEL);
617 
618 	if (!ha)
619 		return -ENOMEM;
620 
621 	memcpy(ha, hw_addr, sizeof(*ha));
622 	list_add_tail(&ha->list, &port->mc);
623 
624 	ocelot_mact_learn(ocelot, PGID_CPU, ha->addr, port->pvid,
625 			  ENTRYTYPE_LOCKED);
626 
627 	return 0;
628 }
629 
630 static void ocelot_set_rx_mode(struct net_device *dev)
631 {
632 	struct ocelot_port *port = netdev_priv(dev);
633 	struct ocelot *ocelot = port->ocelot;
634 	struct netdev_hw_addr *ha;
635 	int i;
636 	u32 val;
637 
638 	/* This doesn't handle promiscuous mode because the bridge core is
639 	 * setting IFF_PROMISC on all slave interfaces and all frames would be
640 	 * forwarded to the CPU port.
641 	 */
642 	val = GENMASK(ocelot->num_phys_ports - 1, 0);
643 	for (i = ocelot->num_phys_ports + 1; i < PGID_CPU; i++)
644 		ocelot_write_rix(ocelot, val, ANA_PGID_PGID, i);
645 
646 	/* Handle the device multicast addresses. First remove all the
647 	 * previously installed addresses and then add the latest ones to the
648 	 * mac table.
649 	 */
650 	ocelot_mact_mc_reset(port);
651 	netdev_for_each_mc_addr(ha, dev)
652 		ocelot_mact_mc_add(port, ha);
653 }
654 
655 static int ocelot_port_get_phys_port_name(struct net_device *dev,
656 					  char *buf, size_t len)
657 {
658 	struct ocelot_port *port = netdev_priv(dev);
659 	int ret;
660 
661 	ret = snprintf(buf, len, "p%d", port->chip_port);
662 	if (ret >= len)
663 		return -EINVAL;
664 
665 	return 0;
666 }
667 
668 static int ocelot_port_set_mac_address(struct net_device *dev, void *p)
669 {
670 	struct ocelot_port *port = netdev_priv(dev);
671 	struct ocelot *ocelot = port->ocelot;
672 	const struct sockaddr *addr = p;
673 
674 	/* Learn the new net device MAC address in the mac table. */
675 	ocelot_mact_learn(ocelot, PGID_CPU, addr->sa_data, port->pvid,
676 			  ENTRYTYPE_LOCKED);
677 	/* Then forget the previous one. */
678 	ocelot_mact_forget(ocelot, dev->dev_addr, port->pvid);
679 
680 	ether_addr_copy(dev->dev_addr, addr->sa_data);
681 	return 0;
682 }
683 
684 static void ocelot_get_stats64(struct net_device *dev,
685 			       struct rtnl_link_stats64 *stats)
686 {
687 	struct ocelot_port *port = netdev_priv(dev);
688 	struct ocelot *ocelot = port->ocelot;
689 
690 	/* Configure the port to read the stats from */
691 	ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(port->chip_port),
692 		     SYS_STAT_CFG);
693 
694 	/* Get Rx stats */
695 	stats->rx_bytes = ocelot_read(ocelot, SYS_COUNT_RX_OCTETS);
696 	stats->rx_packets = ocelot_read(ocelot, SYS_COUNT_RX_SHORTS) +
697 			    ocelot_read(ocelot, SYS_COUNT_RX_FRAGMENTS) +
698 			    ocelot_read(ocelot, SYS_COUNT_RX_JABBERS) +
699 			    ocelot_read(ocelot, SYS_COUNT_RX_LONGS) +
700 			    ocelot_read(ocelot, SYS_COUNT_RX_64) +
701 			    ocelot_read(ocelot, SYS_COUNT_RX_65_127) +
702 			    ocelot_read(ocelot, SYS_COUNT_RX_128_255) +
703 			    ocelot_read(ocelot, SYS_COUNT_RX_256_1023) +
704 			    ocelot_read(ocelot, SYS_COUNT_RX_1024_1526) +
705 			    ocelot_read(ocelot, SYS_COUNT_RX_1527_MAX);
706 	stats->multicast = ocelot_read(ocelot, SYS_COUNT_RX_MULTICAST);
707 	stats->rx_dropped = dev->stats.rx_dropped;
708 
709 	/* Get Tx stats */
710 	stats->tx_bytes = ocelot_read(ocelot, SYS_COUNT_TX_OCTETS);
711 	stats->tx_packets = ocelot_read(ocelot, SYS_COUNT_TX_64) +
712 			    ocelot_read(ocelot, SYS_COUNT_TX_65_127) +
713 			    ocelot_read(ocelot, SYS_COUNT_TX_128_511) +
714 			    ocelot_read(ocelot, SYS_COUNT_TX_512_1023) +
715 			    ocelot_read(ocelot, SYS_COUNT_TX_1024_1526) +
716 			    ocelot_read(ocelot, SYS_COUNT_TX_1527_MAX);
717 	stats->tx_dropped = ocelot_read(ocelot, SYS_COUNT_TX_DROPS) +
718 			    ocelot_read(ocelot, SYS_COUNT_TX_AGING);
719 	stats->collisions = ocelot_read(ocelot, SYS_COUNT_TX_COLLISION);
720 }
721 
722 static int ocelot_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
723 			  struct net_device *dev, const unsigned char *addr,
724 			  u16 vid, u16 flags,
725 			  struct netlink_ext_ack *extack)
726 {
727 	struct ocelot_port *port = netdev_priv(dev);
728 	struct ocelot *ocelot = port->ocelot;
729 
730 	if (!vid) {
731 		if (!port->vlan_aware)
732 			/* If the bridge is not VLAN aware and no VID was
733 			 * provided, set it to pvid to ensure the MAC entry
734 			 * matches incoming untagged packets
735 			 */
736 			vid = port->pvid;
737 		else
738 			/* If the bridge is VLAN aware a VID must be provided as
739 			 * otherwise the learnt entry wouldn't match any frame.
740 			 */
741 			return -EINVAL;
742 	}
743 
744 	return ocelot_mact_learn(ocelot, port->chip_port, addr, vid,
745 				 ENTRYTYPE_LOCKED);
746 }
747 
748 static int ocelot_fdb_del(struct ndmsg *ndm, struct nlattr *tb[],
749 			  struct net_device *dev,
750 			  const unsigned char *addr, u16 vid)
751 {
752 	struct ocelot_port *port = netdev_priv(dev);
753 	struct ocelot *ocelot = port->ocelot;
754 
755 	return ocelot_mact_forget(ocelot, addr, vid);
756 }
757 
758 struct ocelot_dump_ctx {
759 	struct net_device *dev;
760 	struct sk_buff *skb;
761 	struct netlink_callback *cb;
762 	int idx;
763 };
764 
765 static int ocelot_fdb_do_dump(struct ocelot_mact_entry *entry,
766 			      struct ocelot_dump_ctx *dump)
767 {
768 	u32 portid = NETLINK_CB(dump->cb->skb).portid;
769 	u32 seq = dump->cb->nlh->nlmsg_seq;
770 	struct nlmsghdr *nlh;
771 	struct ndmsg *ndm;
772 
773 	if (dump->idx < dump->cb->args[2])
774 		goto skip;
775 
776 	nlh = nlmsg_put(dump->skb, portid, seq, RTM_NEWNEIGH,
777 			sizeof(*ndm), NLM_F_MULTI);
778 	if (!nlh)
779 		return -EMSGSIZE;
780 
781 	ndm = nlmsg_data(nlh);
782 	ndm->ndm_family  = AF_BRIDGE;
783 	ndm->ndm_pad1    = 0;
784 	ndm->ndm_pad2    = 0;
785 	ndm->ndm_flags   = NTF_SELF;
786 	ndm->ndm_type    = 0;
787 	ndm->ndm_ifindex = dump->dev->ifindex;
788 	ndm->ndm_state   = NUD_REACHABLE;
789 
790 	if (nla_put(dump->skb, NDA_LLADDR, ETH_ALEN, entry->mac))
791 		goto nla_put_failure;
792 
793 	if (entry->vid && nla_put_u16(dump->skb, NDA_VLAN, entry->vid))
794 		goto nla_put_failure;
795 
796 	nlmsg_end(dump->skb, nlh);
797 
798 skip:
799 	dump->idx++;
800 	return 0;
801 
802 nla_put_failure:
803 	nlmsg_cancel(dump->skb, nlh);
804 	return -EMSGSIZE;
805 }
806 
807 static inline int ocelot_mact_read(struct ocelot_port *port, int row, int col,
808 				   struct ocelot_mact_entry *entry)
809 {
810 	struct ocelot *ocelot = port->ocelot;
811 	char mac[ETH_ALEN];
812 	u32 val, dst, macl, mach;
813 
814 	/* Set row and column to read from */
815 	ocelot_field_write(ocelot, ANA_TABLES_MACTINDX_M_INDEX, row);
816 	ocelot_field_write(ocelot, ANA_TABLES_MACTINDX_BUCKET, col);
817 
818 	/* Issue a read command */
819 	ocelot_write(ocelot,
820 		     ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_READ),
821 		     ANA_TABLES_MACACCESS);
822 
823 	if (ocelot_mact_wait_for_completion(ocelot))
824 		return -ETIMEDOUT;
825 
826 	/* Read the entry flags */
827 	val = ocelot_read(ocelot, ANA_TABLES_MACACCESS);
828 	if (!(val & ANA_TABLES_MACACCESS_VALID))
829 		return -EINVAL;
830 
831 	/* If the entry read has another port configured as its destination,
832 	 * do not report it.
833 	 */
834 	dst = (val & ANA_TABLES_MACACCESS_DEST_IDX_M) >> 3;
835 	if (dst != port->chip_port)
836 		return -EINVAL;
837 
838 	/* Get the entry's MAC address and VLAN id */
839 	macl = ocelot_read(ocelot, ANA_TABLES_MACLDATA);
840 	mach = ocelot_read(ocelot, ANA_TABLES_MACHDATA);
841 
842 	mac[0] = (mach >> 8)  & 0xff;
843 	mac[1] = (mach >> 0)  & 0xff;
844 	mac[2] = (macl >> 24) & 0xff;
845 	mac[3] = (macl >> 16) & 0xff;
846 	mac[4] = (macl >> 8)  & 0xff;
847 	mac[5] = (macl >> 0)  & 0xff;
848 
849 	entry->vid = (mach >> 16) & 0xfff;
850 	ether_addr_copy(entry->mac, mac);
851 
852 	return 0;
853 }
854 
855 static int ocelot_fdb_dump(struct sk_buff *skb, struct netlink_callback *cb,
856 			   struct net_device *dev,
857 			   struct net_device *filter_dev, int *idx)
858 {
859 	struct ocelot_port *port = netdev_priv(dev);
860 	int i, j, ret = 0;
861 	struct ocelot_dump_ctx dump = {
862 		.dev = dev,
863 		.skb = skb,
864 		.cb = cb,
865 		.idx = *idx,
866 	};
867 
868 	struct ocelot_mact_entry entry;
869 
870 	/* Loop through all the mac tables entries. There are 1024 rows of 4
871 	 * entries.
872 	 */
873 	for (i = 0; i < 1024; i++) {
874 		for (j = 0; j < 4; j++) {
875 			ret = ocelot_mact_read(port, i, j, &entry);
876 			/* If the entry is invalid (wrong port, invalid...),
877 			 * skip it.
878 			 */
879 			if (ret == -EINVAL)
880 				continue;
881 			else if (ret)
882 				goto end;
883 
884 			ret = ocelot_fdb_do_dump(&entry, &dump);
885 			if (ret)
886 				goto end;
887 		}
888 	}
889 
890 end:
891 	*idx = dump.idx;
892 	return ret;
893 }
894 
895 static int ocelot_vlan_rx_add_vid(struct net_device *dev, __be16 proto,
896 				  u16 vid)
897 {
898 	return ocelot_vlan_vid_add(dev, vid, false, true);
899 }
900 
901 static int ocelot_vlan_rx_kill_vid(struct net_device *dev, __be16 proto,
902 				   u16 vid)
903 {
904 	return ocelot_vlan_vid_del(dev, vid);
905 }
906 
907 static int ocelot_set_features(struct net_device *dev,
908 			       netdev_features_t features)
909 {
910 	struct ocelot_port *port = netdev_priv(dev);
911 	netdev_features_t changed = dev->features ^ features;
912 
913 	if (changed & NETIF_F_HW_VLAN_CTAG_FILTER)
914 		ocelot_vlan_mode(port, features);
915 
916 	return 0;
917 }
918 
919 static int ocelot_get_port_parent_id(struct net_device *dev,
920 				     struct netdev_phys_item_id *ppid)
921 {
922 	struct ocelot_port *ocelot_port = netdev_priv(dev);
923 	struct ocelot *ocelot = ocelot_port->ocelot;
924 
925 	ppid->id_len = sizeof(ocelot->base_mac);
926 	memcpy(&ppid->id, &ocelot->base_mac, ppid->id_len);
927 
928 	return 0;
929 }
930 
931 static const struct net_device_ops ocelot_port_netdev_ops = {
932 	.ndo_open			= ocelot_port_open,
933 	.ndo_stop			= ocelot_port_stop,
934 	.ndo_start_xmit			= ocelot_port_xmit,
935 	.ndo_set_rx_mode		= ocelot_set_rx_mode,
936 	.ndo_get_phys_port_name		= ocelot_port_get_phys_port_name,
937 	.ndo_set_mac_address		= ocelot_port_set_mac_address,
938 	.ndo_get_stats64		= ocelot_get_stats64,
939 	.ndo_fdb_add			= ocelot_fdb_add,
940 	.ndo_fdb_del			= ocelot_fdb_del,
941 	.ndo_fdb_dump			= ocelot_fdb_dump,
942 	.ndo_vlan_rx_add_vid		= ocelot_vlan_rx_add_vid,
943 	.ndo_vlan_rx_kill_vid		= ocelot_vlan_rx_kill_vid,
944 	.ndo_set_features		= ocelot_set_features,
945 	.ndo_get_port_parent_id		= ocelot_get_port_parent_id,
946 };
947 
948 static void ocelot_get_strings(struct net_device *netdev, u32 sset, u8 *data)
949 {
950 	struct ocelot_port *port = netdev_priv(netdev);
951 	struct ocelot *ocelot = port->ocelot;
952 	int i;
953 
954 	if (sset != ETH_SS_STATS)
955 		return;
956 
957 	for (i = 0; i < ocelot->num_stats; i++)
958 		memcpy(data + i * ETH_GSTRING_LEN, ocelot->stats_layout[i].name,
959 		       ETH_GSTRING_LEN);
960 }
961 
962 static void ocelot_check_stats(struct work_struct *work)
963 {
964 	struct delayed_work *del_work = to_delayed_work(work);
965 	struct ocelot *ocelot = container_of(del_work, struct ocelot, stats_work);
966 	int i, j;
967 
968 	mutex_lock(&ocelot->stats_lock);
969 
970 	for (i = 0; i < ocelot->num_phys_ports; i++) {
971 		/* Configure the port to read the stats from */
972 		ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(i), SYS_STAT_CFG);
973 
974 		for (j = 0; j < ocelot->num_stats; j++) {
975 			u32 val;
976 			unsigned int idx = i * ocelot->num_stats + j;
977 
978 			val = ocelot_read_rix(ocelot, SYS_COUNT_RX_OCTETS,
979 					      ocelot->stats_layout[j].offset);
980 
981 			if (val < (ocelot->stats[idx] & U32_MAX))
982 				ocelot->stats[idx] += (u64)1 << 32;
983 
984 			ocelot->stats[idx] = (ocelot->stats[idx] &
985 					      ~(u64)U32_MAX) + val;
986 		}
987 	}
988 
989 	cancel_delayed_work(&ocelot->stats_work);
990 	queue_delayed_work(ocelot->stats_queue, &ocelot->stats_work,
991 			   OCELOT_STATS_CHECK_DELAY);
992 
993 	mutex_unlock(&ocelot->stats_lock);
994 }
995 
996 static void ocelot_get_ethtool_stats(struct net_device *dev,
997 				     struct ethtool_stats *stats, u64 *data)
998 {
999 	struct ocelot_port *port = netdev_priv(dev);
1000 	struct ocelot *ocelot = port->ocelot;
1001 	int i;
1002 
1003 	/* check and update now */
1004 	ocelot_check_stats(&ocelot->stats_work.work);
1005 
1006 	/* Copy all counters */
1007 	for (i = 0; i < ocelot->num_stats; i++)
1008 		*data++ = ocelot->stats[port->chip_port * ocelot->num_stats + i];
1009 }
1010 
1011 static int ocelot_get_sset_count(struct net_device *dev, int sset)
1012 {
1013 	struct ocelot_port *port = netdev_priv(dev);
1014 	struct ocelot *ocelot = port->ocelot;
1015 
1016 	if (sset != ETH_SS_STATS)
1017 		return -EOPNOTSUPP;
1018 	return ocelot->num_stats;
1019 }
1020 
1021 static const struct ethtool_ops ocelot_ethtool_ops = {
1022 	.get_strings		= ocelot_get_strings,
1023 	.get_ethtool_stats	= ocelot_get_ethtool_stats,
1024 	.get_sset_count		= ocelot_get_sset_count,
1025 	.get_link_ksettings	= phy_ethtool_get_link_ksettings,
1026 	.set_link_ksettings	= phy_ethtool_set_link_ksettings,
1027 };
1028 
1029 static int ocelot_port_attr_stp_state_set(struct ocelot_port *ocelot_port,
1030 					  struct switchdev_trans *trans,
1031 					  u8 state)
1032 {
1033 	struct ocelot *ocelot = ocelot_port->ocelot;
1034 	u32 port_cfg;
1035 	int port, i;
1036 
1037 	if (switchdev_trans_ph_prepare(trans))
1038 		return 0;
1039 
1040 	if (!(BIT(ocelot_port->chip_port) & ocelot->bridge_mask))
1041 		return 0;
1042 
1043 	port_cfg = ocelot_read_gix(ocelot, ANA_PORT_PORT_CFG,
1044 				   ocelot_port->chip_port);
1045 
1046 	switch (state) {
1047 	case BR_STATE_FORWARDING:
1048 		ocelot->bridge_fwd_mask |= BIT(ocelot_port->chip_port);
1049 		/* Fallthrough */
1050 	case BR_STATE_LEARNING:
1051 		port_cfg |= ANA_PORT_PORT_CFG_LEARN_ENA;
1052 		break;
1053 
1054 	default:
1055 		port_cfg &= ~ANA_PORT_PORT_CFG_LEARN_ENA;
1056 		ocelot->bridge_fwd_mask &= ~BIT(ocelot_port->chip_port);
1057 		break;
1058 	}
1059 
1060 	ocelot_write_gix(ocelot, port_cfg, ANA_PORT_PORT_CFG,
1061 			 ocelot_port->chip_port);
1062 
1063 	/* Apply FWD mask. The loop is needed to add/remove the current port as
1064 	 * a source for the other ports.
1065 	 */
1066 	for (port = 0; port < ocelot->num_phys_ports; port++) {
1067 		if (ocelot->bridge_fwd_mask & BIT(port)) {
1068 			unsigned long mask = ocelot->bridge_fwd_mask & ~BIT(port);
1069 
1070 			for (i = 0; i < ocelot->num_phys_ports; i++) {
1071 				unsigned long bond_mask = ocelot->lags[i];
1072 
1073 				if (!bond_mask)
1074 					continue;
1075 
1076 				if (bond_mask & BIT(port)) {
1077 					mask &= ~bond_mask;
1078 					break;
1079 				}
1080 			}
1081 
1082 			ocelot_write_rix(ocelot,
1083 					 BIT(ocelot->num_phys_ports) | mask,
1084 					 ANA_PGID_PGID, PGID_SRC + port);
1085 		} else {
1086 			/* Only the CPU port, this is compatible with link
1087 			 * aggregation.
1088 			 */
1089 			ocelot_write_rix(ocelot,
1090 					 BIT(ocelot->num_phys_ports),
1091 					 ANA_PGID_PGID, PGID_SRC + port);
1092 		}
1093 	}
1094 
1095 	return 0;
1096 }
1097 
1098 static void ocelot_port_attr_ageing_set(struct ocelot_port *ocelot_port,
1099 					unsigned long ageing_clock_t)
1100 {
1101 	struct ocelot *ocelot = ocelot_port->ocelot;
1102 	unsigned long ageing_jiffies = clock_t_to_jiffies(ageing_clock_t);
1103 	u32 ageing_time = jiffies_to_msecs(ageing_jiffies) / 1000;
1104 
1105 	ocelot_write(ocelot, ANA_AUTOAGE_AGE_PERIOD(ageing_time / 2),
1106 		     ANA_AUTOAGE);
1107 }
1108 
1109 static void ocelot_port_attr_mc_set(struct ocelot_port *port, bool mc)
1110 {
1111 	struct ocelot *ocelot = port->ocelot;
1112 	u32 val = ocelot_read_gix(ocelot, ANA_PORT_CPU_FWD_CFG,
1113 				  port->chip_port);
1114 
1115 	if (mc)
1116 		val |= ANA_PORT_CPU_FWD_CFG_CPU_IGMP_REDIR_ENA |
1117 		       ANA_PORT_CPU_FWD_CFG_CPU_MLD_REDIR_ENA |
1118 		       ANA_PORT_CPU_FWD_CFG_CPU_IPMC_CTRL_COPY_ENA;
1119 	else
1120 		val &= ~(ANA_PORT_CPU_FWD_CFG_CPU_IGMP_REDIR_ENA |
1121 			 ANA_PORT_CPU_FWD_CFG_CPU_MLD_REDIR_ENA |
1122 			 ANA_PORT_CPU_FWD_CFG_CPU_IPMC_CTRL_COPY_ENA);
1123 
1124 	ocelot_write_gix(ocelot, val, ANA_PORT_CPU_FWD_CFG, port->chip_port);
1125 }
1126 
1127 static int ocelot_port_attr_set(struct net_device *dev,
1128 				const struct switchdev_attr *attr,
1129 				struct switchdev_trans *trans)
1130 {
1131 	struct ocelot_port *ocelot_port = netdev_priv(dev);
1132 	int err = 0;
1133 
1134 	switch (attr->id) {
1135 	case SWITCHDEV_ATTR_ID_PORT_STP_STATE:
1136 		ocelot_port_attr_stp_state_set(ocelot_port, trans,
1137 					       attr->u.stp_state);
1138 		break;
1139 	case SWITCHDEV_ATTR_ID_BRIDGE_AGEING_TIME:
1140 		ocelot_port_attr_ageing_set(ocelot_port, attr->u.ageing_time);
1141 		break;
1142 	case SWITCHDEV_ATTR_ID_BRIDGE_VLAN_FILTERING:
1143 		ocelot_port->vlan_aware = attr->u.vlan_filtering;
1144 		ocelot_vlan_port_apply(ocelot_port->ocelot, ocelot_port);
1145 		break;
1146 	case SWITCHDEV_ATTR_ID_BRIDGE_MC_DISABLED:
1147 		ocelot_port_attr_mc_set(ocelot_port, !attr->u.mc_disabled);
1148 		break;
1149 	default:
1150 		err = -EOPNOTSUPP;
1151 		break;
1152 	}
1153 
1154 	return err;
1155 }
1156 
1157 static int ocelot_port_obj_add_vlan(struct net_device *dev,
1158 				    const struct switchdev_obj_port_vlan *vlan,
1159 				    struct switchdev_trans *trans)
1160 {
1161 	int ret;
1162 	u16 vid;
1163 
1164 	for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
1165 		ret = ocelot_vlan_vid_add(dev, vid,
1166 					  vlan->flags & BRIDGE_VLAN_INFO_PVID,
1167 					  vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED);
1168 		if (ret)
1169 			return ret;
1170 	}
1171 
1172 	return 0;
1173 }
1174 
1175 static int ocelot_port_vlan_del_vlan(struct net_device *dev,
1176 				     const struct switchdev_obj_port_vlan *vlan)
1177 {
1178 	int ret;
1179 	u16 vid;
1180 
1181 	for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
1182 		ret = ocelot_vlan_vid_del(dev, vid);
1183 
1184 		if (ret)
1185 			return ret;
1186 	}
1187 
1188 	return 0;
1189 }
1190 
1191 static struct ocelot_multicast *ocelot_multicast_get(struct ocelot *ocelot,
1192 						     const unsigned char *addr,
1193 						     u16 vid)
1194 {
1195 	struct ocelot_multicast *mc;
1196 
1197 	list_for_each_entry(mc, &ocelot->multicast, list) {
1198 		if (ether_addr_equal(mc->addr, addr) && mc->vid == vid)
1199 			return mc;
1200 	}
1201 
1202 	return NULL;
1203 }
1204 
1205 static int ocelot_port_obj_add_mdb(struct net_device *dev,
1206 				   const struct switchdev_obj_port_mdb *mdb,
1207 				   struct switchdev_trans *trans)
1208 {
1209 	struct ocelot_port *port = netdev_priv(dev);
1210 	struct ocelot *ocelot = port->ocelot;
1211 	struct ocelot_multicast *mc;
1212 	unsigned char addr[ETH_ALEN];
1213 	u16 vid = mdb->vid;
1214 	bool new = false;
1215 
1216 	if (!vid)
1217 		vid = port->pvid;
1218 
1219 	mc = ocelot_multicast_get(ocelot, mdb->addr, vid);
1220 	if (!mc) {
1221 		mc = devm_kzalloc(ocelot->dev, sizeof(*mc), GFP_KERNEL);
1222 		if (!mc)
1223 			return -ENOMEM;
1224 
1225 		memcpy(mc->addr, mdb->addr, ETH_ALEN);
1226 		mc->vid = vid;
1227 
1228 		list_add_tail(&mc->list, &ocelot->multicast);
1229 		new = true;
1230 	}
1231 
1232 	memcpy(addr, mc->addr, ETH_ALEN);
1233 	addr[0] = 0;
1234 
1235 	if (!new) {
1236 		addr[2] = mc->ports << 0;
1237 		addr[1] = mc->ports << 8;
1238 		ocelot_mact_forget(ocelot, addr, vid);
1239 	}
1240 
1241 	mc->ports |= BIT(port->chip_port);
1242 	addr[2] = mc->ports << 0;
1243 	addr[1] = mc->ports << 8;
1244 
1245 	return ocelot_mact_learn(ocelot, 0, addr, vid, ENTRYTYPE_MACv4);
1246 }
1247 
1248 static int ocelot_port_obj_del_mdb(struct net_device *dev,
1249 				   const struct switchdev_obj_port_mdb *mdb)
1250 {
1251 	struct ocelot_port *port = netdev_priv(dev);
1252 	struct ocelot *ocelot = port->ocelot;
1253 	struct ocelot_multicast *mc;
1254 	unsigned char addr[ETH_ALEN];
1255 	u16 vid = mdb->vid;
1256 
1257 	if (!vid)
1258 		vid = port->pvid;
1259 
1260 	mc = ocelot_multicast_get(ocelot, mdb->addr, vid);
1261 	if (!mc)
1262 		return -ENOENT;
1263 
1264 	memcpy(addr, mc->addr, ETH_ALEN);
1265 	addr[2] = mc->ports << 0;
1266 	addr[1] = mc->ports << 8;
1267 	addr[0] = 0;
1268 	ocelot_mact_forget(ocelot, addr, vid);
1269 
1270 	mc->ports &= ~BIT(port->chip_port);
1271 	if (!mc->ports) {
1272 		list_del(&mc->list);
1273 		devm_kfree(ocelot->dev, mc);
1274 		return 0;
1275 	}
1276 
1277 	addr[2] = mc->ports << 0;
1278 	addr[1] = mc->ports << 8;
1279 
1280 	return ocelot_mact_learn(ocelot, 0, addr, vid, ENTRYTYPE_MACv4);
1281 }
1282 
1283 static int ocelot_port_obj_add(struct net_device *dev,
1284 			       const struct switchdev_obj *obj,
1285 			       struct switchdev_trans *trans,
1286 			       struct netlink_ext_ack *extack)
1287 {
1288 	int ret = 0;
1289 
1290 	switch (obj->id) {
1291 	case SWITCHDEV_OBJ_ID_PORT_VLAN:
1292 		ret = ocelot_port_obj_add_vlan(dev,
1293 					       SWITCHDEV_OBJ_PORT_VLAN(obj),
1294 					       trans);
1295 		break;
1296 	case SWITCHDEV_OBJ_ID_PORT_MDB:
1297 		ret = ocelot_port_obj_add_mdb(dev, SWITCHDEV_OBJ_PORT_MDB(obj),
1298 					      trans);
1299 		break;
1300 	default:
1301 		return -EOPNOTSUPP;
1302 	}
1303 
1304 	return ret;
1305 }
1306 
1307 static int ocelot_port_obj_del(struct net_device *dev,
1308 			       const struct switchdev_obj *obj)
1309 {
1310 	int ret = 0;
1311 
1312 	switch (obj->id) {
1313 	case SWITCHDEV_OBJ_ID_PORT_VLAN:
1314 		ret = ocelot_port_vlan_del_vlan(dev,
1315 						SWITCHDEV_OBJ_PORT_VLAN(obj));
1316 		break;
1317 	case SWITCHDEV_OBJ_ID_PORT_MDB:
1318 		ret = ocelot_port_obj_del_mdb(dev, SWITCHDEV_OBJ_PORT_MDB(obj));
1319 		break;
1320 	default:
1321 		return -EOPNOTSUPP;
1322 	}
1323 
1324 	return ret;
1325 }
1326 
1327 static int ocelot_port_bridge_join(struct ocelot_port *ocelot_port,
1328 				   struct net_device *bridge)
1329 {
1330 	struct ocelot *ocelot = ocelot_port->ocelot;
1331 
1332 	if (!ocelot->bridge_mask) {
1333 		ocelot->hw_bridge_dev = bridge;
1334 	} else {
1335 		if (ocelot->hw_bridge_dev != bridge)
1336 			/* This is adding the port to a second bridge, this is
1337 			 * unsupported */
1338 			return -ENODEV;
1339 	}
1340 
1341 	ocelot->bridge_mask |= BIT(ocelot_port->chip_port);
1342 
1343 	return 0;
1344 }
1345 
1346 static void ocelot_port_bridge_leave(struct ocelot_port *ocelot_port,
1347 				     struct net_device *bridge)
1348 {
1349 	struct ocelot *ocelot = ocelot_port->ocelot;
1350 
1351 	ocelot->bridge_mask &= ~BIT(ocelot_port->chip_port);
1352 
1353 	if (!ocelot->bridge_mask)
1354 		ocelot->hw_bridge_dev = NULL;
1355 
1356 	/* Clear bridge vlan settings before calling ocelot_vlan_port_apply */
1357 	ocelot_port->vlan_aware = 0;
1358 	ocelot_port->pvid = 0;
1359 	ocelot_port->vid = 0;
1360 }
1361 
1362 static void ocelot_set_aggr_pgids(struct ocelot *ocelot)
1363 {
1364 	int i, port, lag;
1365 
1366 	/* Reset destination and aggregation PGIDS */
1367 	for (port = 0; port < ocelot->num_phys_ports; port++)
1368 		ocelot_write_rix(ocelot, BIT(port), ANA_PGID_PGID, port);
1369 
1370 	for (i = PGID_AGGR; i < PGID_SRC; i++)
1371 		ocelot_write_rix(ocelot, GENMASK(ocelot->num_phys_ports - 1, 0),
1372 				 ANA_PGID_PGID, i);
1373 
1374 	/* Now, set PGIDs for each LAG */
1375 	for (lag = 0; lag < ocelot->num_phys_ports; lag++) {
1376 		unsigned long bond_mask;
1377 		int aggr_count = 0;
1378 		u8 aggr_idx[16];
1379 
1380 		bond_mask = ocelot->lags[lag];
1381 		if (!bond_mask)
1382 			continue;
1383 
1384 		for_each_set_bit(port, &bond_mask, ocelot->num_phys_ports) {
1385 			// Destination mask
1386 			ocelot_write_rix(ocelot, bond_mask,
1387 					 ANA_PGID_PGID, port);
1388 			aggr_idx[aggr_count] = port;
1389 			aggr_count++;
1390 		}
1391 
1392 		for (i = PGID_AGGR; i < PGID_SRC; i++) {
1393 			u32 ac;
1394 
1395 			ac = ocelot_read_rix(ocelot, ANA_PGID_PGID, i);
1396 			ac &= ~bond_mask;
1397 			ac |= BIT(aggr_idx[i % aggr_count]);
1398 			ocelot_write_rix(ocelot, ac, ANA_PGID_PGID, i);
1399 		}
1400 	}
1401 }
1402 
1403 static void ocelot_setup_lag(struct ocelot *ocelot, int lag)
1404 {
1405 	unsigned long bond_mask = ocelot->lags[lag];
1406 	unsigned int p;
1407 
1408 	for_each_set_bit(p, &bond_mask, ocelot->num_phys_ports) {
1409 		u32 port_cfg = ocelot_read_gix(ocelot, ANA_PORT_PORT_CFG, p);
1410 
1411 		port_cfg &= ~ANA_PORT_PORT_CFG_PORTID_VAL_M;
1412 
1413 		/* Use lag port as logical port for port i */
1414 		ocelot_write_gix(ocelot, port_cfg |
1415 				 ANA_PORT_PORT_CFG_PORTID_VAL(lag),
1416 				 ANA_PORT_PORT_CFG, p);
1417 	}
1418 }
1419 
1420 static int ocelot_port_lag_join(struct ocelot_port *ocelot_port,
1421 				struct net_device *bond)
1422 {
1423 	struct ocelot *ocelot = ocelot_port->ocelot;
1424 	int p = ocelot_port->chip_port;
1425 	int lag, lp;
1426 	struct net_device *ndev;
1427 	u32 bond_mask = 0;
1428 
1429 	rcu_read_lock();
1430 	for_each_netdev_in_bond_rcu(bond, ndev) {
1431 		struct ocelot_port *port = netdev_priv(ndev);
1432 
1433 		bond_mask |= BIT(port->chip_port);
1434 	}
1435 	rcu_read_unlock();
1436 
1437 	lp = __ffs(bond_mask);
1438 
1439 	/* If the new port is the lowest one, use it as the logical port from
1440 	 * now on
1441 	 */
1442 	if (p == lp) {
1443 		lag = p;
1444 		ocelot->lags[p] = bond_mask;
1445 		bond_mask &= ~BIT(p);
1446 		if (bond_mask) {
1447 			lp = __ffs(bond_mask);
1448 			ocelot->lags[lp] = 0;
1449 		}
1450 	} else {
1451 		lag = lp;
1452 		ocelot->lags[lp] |= BIT(p);
1453 	}
1454 
1455 	ocelot_setup_lag(ocelot, lag);
1456 	ocelot_set_aggr_pgids(ocelot);
1457 
1458 	return 0;
1459 }
1460 
1461 static void ocelot_port_lag_leave(struct ocelot_port *ocelot_port,
1462 				  struct net_device *bond)
1463 {
1464 	struct ocelot *ocelot = ocelot_port->ocelot;
1465 	int p = ocelot_port->chip_port;
1466 	u32 port_cfg;
1467 	int i;
1468 
1469 	/* Remove port from any lag */
1470 	for (i = 0; i < ocelot->num_phys_ports; i++)
1471 		ocelot->lags[i] &= ~BIT(ocelot_port->chip_port);
1472 
1473 	/* if it was the logical port of the lag, move the lag config to the
1474 	 * next port
1475 	 */
1476 	if (ocelot->lags[p]) {
1477 		int n = __ffs(ocelot->lags[p]);
1478 
1479 		ocelot->lags[n] = ocelot->lags[p];
1480 		ocelot->lags[p] = 0;
1481 
1482 		ocelot_setup_lag(ocelot, n);
1483 	}
1484 
1485 	port_cfg = ocelot_read_gix(ocelot, ANA_PORT_PORT_CFG, p);
1486 	port_cfg &= ~ANA_PORT_PORT_CFG_PORTID_VAL_M;
1487 	ocelot_write_gix(ocelot, port_cfg | ANA_PORT_PORT_CFG_PORTID_VAL(p),
1488 			 ANA_PORT_PORT_CFG, p);
1489 
1490 	ocelot_set_aggr_pgids(ocelot);
1491 }
1492 
1493 /* Checks if the net_device instance given to us originate from our driver. */
1494 static bool ocelot_netdevice_dev_check(const struct net_device *dev)
1495 {
1496 	return dev->netdev_ops == &ocelot_port_netdev_ops;
1497 }
1498 
1499 static int ocelot_netdevice_port_event(struct net_device *dev,
1500 				       unsigned long event,
1501 				       struct netdev_notifier_changeupper_info *info)
1502 {
1503 	struct ocelot_port *ocelot_port = netdev_priv(dev);
1504 	int err = 0;
1505 
1506 	if (!ocelot_netdevice_dev_check(dev))
1507 		return 0;
1508 
1509 	switch (event) {
1510 	case NETDEV_CHANGEUPPER:
1511 		if (netif_is_bridge_master(info->upper_dev)) {
1512 			if (info->linking)
1513 				err = ocelot_port_bridge_join(ocelot_port,
1514 							      info->upper_dev);
1515 			else
1516 				ocelot_port_bridge_leave(ocelot_port,
1517 							 info->upper_dev);
1518 
1519 			ocelot_vlan_port_apply(ocelot_port->ocelot,
1520 					       ocelot_port);
1521 		}
1522 		if (netif_is_lag_master(info->upper_dev)) {
1523 			if (info->linking)
1524 				err = ocelot_port_lag_join(ocelot_port,
1525 							   info->upper_dev);
1526 			else
1527 				ocelot_port_lag_leave(ocelot_port,
1528 						      info->upper_dev);
1529 		}
1530 		break;
1531 	default:
1532 		break;
1533 	}
1534 
1535 	return err;
1536 }
1537 
1538 static int ocelot_netdevice_event(struct notifier_block *unused,
1539 				  unsigned long event, void *ptr)
1540 {
1541 	struct netdev_notifier_changeupper_info *info = ptr;
1542 	struct net_device *dev = netdev_notifier_info_to_dev(ptr);
1543 	int ret = 0;
1544 
1545 	if (event == NETDEV_PRECHANGEUPPER &&
1546 	    netif_is_lag_master(info->upper_dev)) {
1547 		struct netdev_lag_upper_info *lag_upper_info = info->upper_info;
1548 		struct netlink_ext_ack *extack;
1549 
1550 		if (lag_upper_info->tx_type != NETDEV_LAG_TX_TYPE_HASH) {
1551 			extack = netdev_notifier_info_to_extack(&info->info);
1552 			NL_SET_ERR_MSG_MOD(extack, "LAG device using unsupported Tx type");
1553 
1554 			ret = -EINVAL;
1555 			goto notify;
1556 		}
1557 	}
1558 
1559 	if (netif_is_lag_master(dev)) {
1560 		struct net_device *slave;
1561 		struct list_head *iter;
1562 
1563 		netdev_for_each_lower_dev(dev, slave, iter) {
1564 			ret = ocelot_netdevice_port_event(slave, event, info);
1565 			if (ret)
1566 				goto notify;
1567 		}
1568 	} else {
1569 		ret = ocelot_netdevice_port_event(dev, event, info);
1570 	}
1571 
1572 notify:
1573 	return notifier_from_errno(ret);
1574 }
1575 
1576 struct notifier_block ocelot_netdevice_nb __read_mostly = {
1577 	.notifier_call = ocelot_netdevice_event,
1578 };
1579 EXPORT_SYMBOL(ocelot_netdevice_nb);
1580 
1581 static int ocelot_switchdev_event(struct notifier_block *unused,
1582 				  unsigned long event, void *ptr)
1583 {
1584 	struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
1585 	int err;
1586 
1587 	switch (event) {
1588 	case SWITCHDEV_PORT_ATTR_SET:
1589 		err = switchdev_handle_port_attr_set(dev, ptr,
1590 						     ocelot_netdevice_dev_check,
1591 						     ocelot_port_attr_set);
1592 		return notifier_from_errno(err);
1593 	}
1594 
1595 	return NOTIFY_DONE;
1596 }
1597 
1598 struct notifier_block ocelot_switchdev_nb __read_mostly = {
1599 	.notifier_call = ocelot_switchdev_event,
1600 };
1601 EXPORT_SYMBOL(ocelot_switchdev_nb);
1602 
1603 static int ocelot_switchdev_blocking_event(struct notifier_block *unused,
1604 					   unsigned long event, void *ptr)
1605 {
1606 	struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
1607 	int err;
1608 
1609 	switch (event) {
1610 		/* Blocking events. */
1611 	case SWITCHDEV_PORT_OBJ_ADD:
1612 		err = switchdev_handle_port_obj_add(dev, ptr,
1613 						    ocelot_netdevice_dev_check,
1614 						    ocelot_port_obj_add);
1615 		return notifier_from_errno(err);
1616 	case SWITCHDEV_PORT_OBJ_DEL:
1617 		err = switchdev_handle_port_obj_del(dev, ptr,
1618 						    ocelot_netdevice_dev_check,
1619 						    ocelot_port_obj_del);
1620 		return notifier_from_errno(err);
1621 	case SWITCHDEV_PORT_ATTR_SET:
1622 		err = switchdev_handle_port_attr_set(dev, ptr,
1623 						     ocelot_netdevice_dev_check,
1624 						     ocelot_port_attr_set);
1625 		return notifier_from_errno(err);
1626 	}
1627 
1628 	return NOTIFY_DONE;
1629 }
1630 
1631 struct notifier_block ocelot_switchdev_blocking_nb __read_mostly = {
1632 	.notifier_call = ocelot_switchdev_blocking_event,
1633 };
1634 EXPORT_SYMBOL(ocelot_switchdev_blocking_nb);
1635 
1636 int ocelot_probe_port(struct ocelot *ocelot, u8 port,
1637 		      void __iomem *regs,
1638 		      struct phy_device *phy)
1639 {
1640 	struct ocelot_port *ocelot_port;
1641 	struct net_device *dev;
1642 	int err;
1643 
1644 	dev = alloc_etherdev(sizeof(struct ocelot_port));
1645 	if (!dev)
1646 		return -ENOMEM;
1647 	SET_NETDEV_DEV(dev, ocelot->dev);
1648 	ocelot_port = netdev_priv(dev);
1649 	ocelot_port->dev = dev;
1650 	ocelot_port->ocelot = ocelot;
1651 	ocelot_port->regs = regs;
1652 	ocelot_port->chip_port = port;
1653 	ocelot_port->phy = phy;
1654 	INIT_LIST_HEAD(&ocelot_port->mc);
1655 	ocelot->ports[port] = ocelot_port;
1656 
1657 	dev->netdev_ops = &ocelot_port_netdev_ops;
1658 	dev->ethtool_ops = &ocelot_ethtool_ops;
1659 
1660 	dev->hw_features |= NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_RXFCS;
1661 	dev->features |= NETIF_F_HW_VLAN_CTAG_FILTER;
1662 
1663 	memcpy(dev->dev_addr, ocelot->base_mac, ETH_ALEN);
1664 	dev->dev_addr[ETH_ALEN - 1] += port;
1665 	ocelot_mact_learn(ocelot, PGID_CPU, dev->dev_addr, ocelot_port->pvid,
1666 			  ENTRYTYPE_LOCKED);
1667 
1668 	err = register_netdev(dev);
1669 	if (err) {
1670 		dev_err(ocelot->dev, "register_netdev failed\n");
1671 		goto err_register_netdev;
1672 	}
1673 
1674 	/* Basic L2 initialization */
1675 	ocelot_vlan_port_apply(ocelot, ocelot_port);
1676 
1677 	return 0;
1678 
1679 err_register_netdev:
1680 	free_netdev(dev);
1681 	return err;
1682 }
1683 EXPORT_SYMBOL(ocelot_probe_port);
1684 
1685 int ocelot_init(struct ocelot *ocelot)
1686 {
1687 	u32 port;
1688 	int i, cpu = ocelot->num_phys_ports;
1689 	char queue_name[32];
1690 
1691 	ocelot->lags = devm_kcalloc(ocelot->dev, ocelot->num_phys_ports,
1692 				    sizeof(u32), GFP_KERNEL);
1693 	if (!ocelot->lags)
1694 		return -ENOMEM;
1695 
1696 	ocelot->stats = devm_kcalloc(ocelot->dev,
1697 				     ocelot->num_phys_ports * ocelot->num_stats,
1698 				     sizeof(u64), GFP_KERNEL);
1699 	if (!ocelot->stats)
1700 		return -ENOMEM;
1701 
1702 	mutex_init(&ocelot->stats_lock);
1703 	snprintf(queue_name, sizeof(queue_name), "%s-stats",
1704 		 dev_name(ocelot->dev));
1705 	ocelot->stats_queue = create_singlethread_workqueue(queue_name);
1706 	if (!ocelot->stats_queue)
1707 		return -ENOMEM;
1708 
1709 	ocelot_mact_init(ocelot);
1710 	ocelot_vlan_init(ocelot);
1711 
1712 	for (port = 0; port < ocelot->num_phys_ports; port++) {
1713 		/* Clear all counters (5 groups) */
1714 		ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(port) |
1715 				     SYS_STAT_CFG_STAT_CLEAR_SHOT(0x7f),
1716 			     SYS_STAT_CFG);
1717 	}
1718 
1719 	/* Only use S-Tag */
1720 	ocelot_write(ocelot, ETH_P_8021AD, SYS_VLAN_ETYPE_CFG);
1721 
1722 	/* Aggregation mode */
1723 	ocelot_write(ocelot, ANA_AGGR_CFG_AC_SMAC_ENA |
1724 			     ANA_AGGR_CFG_AC_DMAC_ENA |
1725 			     ANA_AGGR_CFG_AC_IP4_SIPDIP_ENA |
1726 			     ANA_AGGR_CFG_AC_IP4_TCPUDP_ENA, ANA_AGGR_CFG);
1727 
1728 	/* Set MAC age time to default value. The entry is aged after
1729 	 * 2*AGE_PERIOD
1730 	 */
1731 	ocelot_write(ocelot,
1732 		     ANA_AUTOAGE_AGE_PERIOD(BR_DEFAULT_AGEING_TIME / 2 / HZ),
1733 		     ANA_AUTOAGE);
1734 
1735 	/* Disable learning for frames discarded by VLAN ingress filtering */
1736 	regmap_field_write(ocelot->regfields[ANA_ADVLEARN_VLAN_CHK], 1);
1737 
1738 	/* Setup frame ageing - fixed value "2 sec" - in 6.5 us units */
1739 	ocelot_write(ocelot, SYS_FRM_AGING_AGE_TX_ENA |
1740 		     SYS_FRM_AGING_MAX_AGE(307692), SYS_FRM_AGING);
1741 
1742 	/* Setup flooding PGIDs */
1743 	ocelot_write_rix(ocelot, ANA_FLOODING_FLD_MULTICAST(PGID_MC) |
1744 			 ANA_FLOODING_FLD_BROADCAST(PGID_MC) |
1745 			 ANA_FLOODING_FLD_UNICAST(PGID_UC),
1746 			 ANA_FLOODING, 0);
1747 	ocelot_write(ocelot, ANA_FLOODING_IPMC_FLD_MC6_DATA(PGID_MCIPV6) |
1748 		     ANA_FLOODING_IPMC_FLD_MC6_CTRL(PGID_MC) |
1749 		     ANA_FLOODING_IPMC_FLD_MC4_DATA(PGID_MCIPV4) |
1750 		     ANA_FLOODING_IPMC_FLD_MC4_CTRL(PGID_MC),
1751 		     ANA_FLOODING_IPMC);
1752 
1753 	for (port = 0; port < ocelot->num_phys_ports; port++) {
1754 		/* Transmit the frame to the local port. */
1755 		ocelot_write_rix(ocelot, BIT(port), ANA_PGID_PGID, port);
1756 		/* Do not forward BPDU frames to the front ports. */
1757 		ocelot_write_gix(ocelot,
1758 				 ANA_PORT_CPU_FWD_BPDU_CFG_BPDU_REDIR_ENA(0xffff),
1759 				 ANA_PORT_CPU_FWD_BPDU_CFG,
1760 				 port);
1761 		/* Ensure bridging is disabled */
1762 		ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_SRC + port);
1763 	}
1764 
1765 	/* Configure and enable the CPU port. */
1766 	ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, cpu);
1767 	ocelot_write_rix(ocelot, BIT(cpu), ANA_PGID_PGID, PGID_CPU);
1768 	ocelot_write_gix(ocelot, ANA_PORT_PORT_CFG_RECV_ENA |
1769 			 ANA_PORT_PORT_CFG_PORTID_VAL(cpu),
1770 			 ANA_PORT_PORT_CFG, cpu);
1771 
1772 	/* Allow broadcast MAC frames. */
1773 	for (i = ocelot->num_phys_ports + 1; i < PGID_CPU; i++) {
1774 		u32 val = ANA_PGID_PGID_PGID(GENMASK(ocelot->num_phys_ports - 1, 0));
1775 
1776 		ocelot_write_rix(ocelot, val, ANA_PGID_PGID, i);
1777 	}
1778 	ocelot_write_rix(ocelot,
1779 			 ANA_PGID_PGID_PGID(GENMASK(ocelot->num_phys_ports, 0)),
1780 			 ANA_PGID_PGID, PGID_MC);
1781 	ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_MCIPV4);
1782 	ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_MCIPV6);
1783 
1784 	/* CPU port Injection/Extraction configuration */
1785 	ocelot_write_rix(ocelot, QSYS_SWITCH_PORT_MODE_INGRESS_DROP_MODE |
1786 			 QSYS_SWITCH_PORT_MODE_SCH_NEXT_CFG(1) |
1787 			 QSYS_SWITCH_PORT_MODE_PORT_ENA,
1788 			 QSYS_SWITCH_PORT_MODE, cpu);
1789 	ocelot_write_rix(ocelot, SYS_PORT_MODE_INCL_XTR_HDR(1) |
1790 			 SYS_PORT_MODE_INCL_INJ_HDR(1), SYS_PORT_MODE, cpu);
1791 	/* Allow manual injection via DEVCPU_QS registers, and byte swap these
1792 	 * registers endianness.
1793 	 */
1794 	ocelot_write_rix(ocelot, QS_INJ_GRP_CFG_BYTE_SWAP |
1795 			 QS_INJ_GRP_CFG_MODE(1), QS_INJ_GRP_CFG, 0);
1796 	ocelot_write_rix(ocelot, QS_XTR_GRP_CFG_BYTE_SWAP |
1797 			 QS_XTR_GRP_CFG_MODE(1), QS_XTR_GRP_CFG, 0);
1798 	ocelot_write(ocelot, ANA_CPUQ_CFG_CPUQ_MIRROR(2) |
1799 		     ANA_CPUQ_CFG_CPUQ_LRN(2) |
1800 		     ANA_CPUQ_CFG_CPUQ_MAC_COPY(2) |
1801 		     ANA_CPUQ_CFG_CPUQ_SRC_COPY(2) |
1802 		     ANA_CPUQ_CFG_CPUQ_LOCKED_PORTMOVE(2) |
1803 		     ANA_CPUQ_CFG_CPUQ_ALLBRIDGE(6) |
1804 		     ANA_CPUQ_CFG_CPUQ_IPMC_CTRL(6) |
1805 		     ANA_CPUQ_CFG_CPUQ_IGMP(6) |
1806 		     ANA_CPUQ_CFG_CPUQ_MLD(6), ANA_CPUQ_CFG);
1807 	for (i = 0; i < 16; i++)
1808 		ocelot_write_rix(ocelot, ANA_CPUQ_8021_CFG_CPUQ_GARP_VAL(6) |
1809 				 ANA_CPUQ_8021_CFG_CPUQ_BPDU_VAL(6),
1810 				 ANA_CPUQ_8021_CFG, i);
1811 
1812 	INIT_DELAYED_WORK(&ocelot->stats_work, ocelot_check_stats);
1813 	queue_delayed_work(ocelot->stats_queue, &ocelot->stats_work,
1814 			   OCELOT_STATS_CHECK_DELAY);
1815 	return 0;
1816 }
1817 EXPORT_SYMBOL(ocelot_init);
1818 
1819 void ocelot_deinit(struct ocelot *ocelot)
1820 {
1821 	destroy_workqueue(ocelot->stats_queue);
1822 	mutex_destroy(&ocelot->stats_lock);
1823 }
1824 EXPORT_SYMBOL(ocelot_deinit);
1825 
1826 MODULE_LICENSE("Dual MIT/GPL");
1827