1 // SPDX-License-Identifier: GPL-2.0+
2 
3 #include <linux/module.h>
4 #include <linux/if_bridge.h>
5 #include <linux/if_vlan.h>
6 #include <linux/iopoll.h>
7 #include <linux/ip.h>
8 #include <linux/of_platform.h>
9 #include <linux/of_net.h>
10 #include <linux/packing.h>
11 #include <linux/phy/phy.h>
12 #include <linux/reset.h>
13 #include <net/addrconf.h>
14 
15 #include "lan966x_main.h"
16 
17 #define XTR_EOF_0			0x00000080U
18 #define XTR_EOF_1			0x01000080U
19 #define XTR_EOF_2			0x02000080U
20 #define XTR_EOF_3			0x03000080U
21 #define XTR_PRUNED			0x04000080U
22 #define XTR_ABORT			0x05000080U
23 #define XTR_ESCAPE			0x06000080U
24 #define XTR_NOT_READY			0x07000080U
25 #define XTR_VALID_BYTES(x)		(4 - (((x) >> 24) & 3))
26 
27 #define IO_RANGES 2
28 
29 static const struct of_device_id lan966x_match[] = {
30 	{ .compatible = "microchip,lan966x-switch" },
31 	{ }
32 };
33 MODULE_DEVICE_TABLE(of, lan966x_match);
34 
35 struct lan966x_main_io_resource {
36 	enum lan966x_target id;
37 	phys_addr_t offset;
38 	int range;
39 };
40 
41 static const struct lan966x_main_io_resource lan966x_main_iomap[] =  {
42 	{ TARGET_CPU,                   0xc0000, 0 }, /* 0xe00c0000 */
43 	{ TARGET_FDMA,                  0xc0400, 0 }, /* 0xe00c0400 */
44 	{ TARGET_ORG,                         0, 1 }, /* 0xe2000000 */
45 	{ TARGET_GCB,                    0x4000, 1 }, /* 0xe2004000 */
46 	{ TARGET_QS,                     0x8000, 1 }, /* 0xe2008000 */
47 	{ TARGET_PTP,                    0xc000, 1 }, /* 0xe200c000 */
48 	{ TARGET_CHIP_TOP,              0x10000, 1 }, /* 0xe2010000 */
49 	{ TARGET_REW,                   0x14000, 1 }, /* 0xe2014000 */
50 	{ TARGET_SYS,                   0x28000, 1 }, /* 0xe2028000 */
51 	{ TARGET_DEV,                   0x34000, 1 }, /* 0xe2034000 */
52 	{ TARGET_DEV +  1,              0x38000, 1 }, /* 0xe2038000 */
53 	{ TARGET_DEV +  2,              0x3c000, 1 }, /* 0xe203c000 */
54 	{ TARGET_DEV +  3,              0x40000, 1 }, /* 0xe2040000 */
55 	{ TARGET_DEV +  4,              0x44000, 1 }, /* 0xe2044000 */
56 	{ TARGET_DEV +  5,              0x48000, 1 }, /* 0xe2048000 */
57 	{ TARGET_DEV +  6,              0x4c000, 1 }, /* 0xe204c000 */
58 	{ TARGET_DEV +  7,              0x50000, 1 }, /* 0xe2050000 */
59 	{ TARGET_QSYS,                 0x100000, 1 }, /* 0xe2100000 */
60 	{ TARGET_AFI,                  0x120000, 1 }, /* 0xe2120000 */
61 	{ TARGET_ANA,                  0x140000, 1 }, /* 0xe2140000 */
62 };
63 
64 static int lan966x_create_targets(struct platform_device *pdev,
65 				  struct lan966x *lan966x)
66 {
67 	struct resource *iores[IO_RANGES];
68 	void __iomem *begin[IO_RANGES];
69 	int idx;
70 
71 	/* Initially map the entire range and after that update each target to
72 	 * point inside the region at the correct offset. It is possible that
73 	 * other devices access the same region so don't add any checks about
74 	 * this.
75 	 */
76 	for (idx = 0; idx < IO_RANGES; idx++) {
77 		iores[idx] = platform_get_resource(pdev, IORESOURCE_MEM,
78 						   idx);
79 		if (!iores[idx]) {
80 			dev_err(&pdev->dev, "Invalid resource\n");
81 			return -EINVAL;
82 		}
83 
84 		begin[idx] = devm_ioremap(&pdev->dev,
85 					  iores[idx]->start,
86 					  resource_size(iores[idx]));
87 		if (!begin[idx]) {
88 			dev_err(&pdev->dev, "Unable to get registers: %s\n",
89 				iores[idx]->name);
90 			return -ENOMEM;
91 		}
92 	}
93 
94 	for (idx = 0; idx < ARRAY_SIZE(lan966x_main_iomap); idx++) {
95 		const struct lan966x_main_io_resource *iomap =
96 			&lan966x_main_iomap[idx];
97 
98 		lan966x->regs[iomap->id] = begin[iomap->range] + iomap->offset;
99 	}
100 
101 	return 0;
102 }
103 
104 static bool lan966x_port_unique_address(struct net_device *dev)
105 {
106 	struct lan966x_port *port = netdev_priv(dev);
107 	struct lan966x *lan966x = port->lan966x;
108 	int p;
109 
110 	for (p = 0; p < lan966x->num_phys_ports; ++p) {
111 		port = lan966x->ports[p];
112 		if (!port || port->dev == dev)
113 			continue;
114 
115 		if (ether_addr_equal(dev->dev_addr, port->dev->dev_addr))
116 			return false;
117 	}
118 
119 	return true;
120 }
121 
122 static int lan966x_port_set_mac_address(struct net_device *dev, void *p)
123 {
124 	struct lan966x_port *port = netdev_priv(dev);
125 	struct lan966x *lan966x = port->lan966x;
126 	const struct sockaddr *addr = p;
127 	int ret;
128 
129 	if (ether_addr_equal(addr->sa_data, dev->dev_addr))
130 		return 0;
131 
132 	/* Learn the new net device MAC address in the mac table. */
133 	ret = lan966x_mac_cpu_learn(lan966x, addr->sa_data, HOST_PVID);
134 	if (ret)
135 		return ret;
136 
137 	/* If there is another port with the same address as the dev, then don't
138 	 * delete it from the MAC table
139 	 */
140 	if (!lan966x_port_unique_address(dev))
141 		goto out;
142 
143 	/* Then forget the previous one. */
144 	ret = lan966x_mac_cpu_forget(lan966x, dev->dev_addr, HOST_PVID);
145 	if (ret)
146 		return ret;
147 
148 out:
149 	eth_hw_addr_set(dev, addr->sa_data);
150 	return ret;
151 }
152 
153 static int lan966x_port_get_phys_port_name(struct net_device *dev,
154 					   char *buf, size_t len)
155 {
156 	struct lan966x_port *port = netdev_priv(dev);
157 	int ret;
158 
159 	ret = snprintf(buf, len, "p%d", port->chip_port);
160 	if (ret >= len)
161 		return -EINVAL;
162 
163 	return 0;
164 }
165 
166 static int lan966x_port_open(struct net_device *dev)
167 {
168 	struct lan966x_port *port = netdev_priv(dev);
169 	struct lan966x *lan966x = port->lan966x;
170 	int err;
171 
172 	/* Enable receiving frames on the port, and activate auto-learning of
173 	 * MAC addresses.
174 	 */
175 	lan_rmw(ANA_PORT_CFG_LEARNAUTO_SET(1) |
176 		ANA_PORT_CFG_RECV_ENA_SET(1) |
177 		ANA_PORT_CFG_PORTID_VAL_SET(port->chip_port),
178 		ANA_PORT_CFG_LEARNAUTO |
179 		ANA_PORT_CFG_RECV_ENA |
180 		ANA_PORT_CFG_PORTID_VAL,
181 		lan966x, ANA_PORT_CFG(port->chip_port));
182 
183 	err = phylink_fwnode_phy_connect(port->phylink, port->fwnode, 0);
184 	if (err) {
185 		netdev_err(dev, "Could not attach to PHY\n");
186 		return err;
187 	}
188 
189 	phylink_start(port->phylink);
190 
191 	return 0;
192 }
193 
194 static int lan966x_port_stop(struct net_device *dev)
195 {
196 	struct lan966x_port *port = netdev_priv(dev);
197 
198 	lan966x_port_config_down(port);
199 	phylink_stop(port->phylink);
200 	phylink_disconnect_phy(port->phylink);
201 
202 	return 0;
203 }
204 
205 static int lan966x_port_inj_status(struct lan966x *lan966x)
206 {
207 	return lan_rd(lan966x, QS_INJ_STATUS);
208 }
209 
210 static int lan966x_port_inj_ready(struct lan966x *lan966x, u8 grp)
211 {
212 	u32 val;
213 
214 	if (lan_rd(lan966x, QS_INJ_STATUS) & QS_INJ_STATUS_FIFO_RDY_SET(BIT(grp)))
215 		return 0;
216 
217 	return readx_poll_timeout_atomic(lan966x_port_inj_status, lan966x, val,
218 					 QS_INJ_STATUS_FIFO_RDY_GET(val) & BIT(grp),
219 					 READL_SLEEP_US, READL_TIMEOUT_US);
220 }
221 
222 static int lan966x_port_ifh_xmit(struct sk_buff *skb,
223 				 __be32 *ifh,
224 				 struct net_device *dev)
225 {
226 	struct lan966x_port *port = netdev_priv(dev);
227 	struct lan966x *lan966x = port->lan966x;
228 	u32 i, count, last;
229 	u8 grp = 0;
230 	u32 val;
231 	int err;
232 
233 	val = lan_rd(lan966x, QS_INJ_STATUS);
234 	if (!(QS_INJ_STATUS_FIFO_RDY_GET(val) & BIT(grp)) ||
235 	    (QS_INJ_STATUS_WMARK_REACHED_GET(val) & BIT(grp)))
236 		goto err;
237 
238 	/* Write start of frame */
239 	lan_wr(QS_INJ_CTRL_GAP_SIZE_SET(1) |
240 	       QS_INJ_CTRL_SOF_SET(1),
241 	       lan966x, QS_INJ_CTRL(grp));
242 
243 	/* Write IFH header */
244 	for (i = 0; i < IFH_LEN; ++i) {
245 		/* Wait until the fifo is ready */
246 		err = lan966x_port_inj_ready(lan966x, grp);
247 		if (err)
248 			goto err;
249 
250 		lan_wr((__force u32)ifh[i], lan966x, QS_INJ_WR(grp));
251 	}
252 
253 	/* Write frame */
254 	count = DIV_ROUND_UP(skb->len, 4);
255 	last = skb->len % 4;
256 	for (i = 0; i < count; ++i) {
257 		/* Wait until the fifo is ready */
258 		err = lan966x_port_inj_ready(lan966x, grp);
259 		if (err)
260 			goto err;
261 
262 		lan_wr(((u32 *)skb->data)[i], lan966x, QS_INJ_WR(grp));
263 	}
264 
265 	/* Add padding */
266 	while (i < (LAN966X_BUFFER_MIN_SZ / 4)) {
267 		/* Wait until the fifo is ready */
268 		err = lan966x_port_inj_ready(lan966x, grp);
269 		if (err)
270 			goto err;
271 
272 		lan_wr(0, lan966x, QS_INJ_WR(grp));
273 		++i;
274 	}
275 
276 	/* Inidcate EOF and valid bytes in the last word */
277 	lan_wr(QS_INJ_CTRL_GAP_SIZE_SET(1) |
278 	       QS_INJ_CTRL_VLD_BYTES_SET(skb->len < LAN966X_BUFFER_MIN_SZ ?
279 				     0 : last) |
280 	       QS_INJ_CTRL_EOF_SET(1),
281 	       lan966x, QS_INJ_CTRL(grp));
282 
283 	/* Add dummy CRC */
284 	lan_wr(0, lan966x, QS_INJ_WR(grp));
285 	skb_tx_timestamp(skb);
286 
287 	dev->stats.tx_packets++;
288 	dev->stats.tx_bytes += skb->len;
289 
290 	if (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP &&
291 	    LAN966X_SKB_CB(skb)->rew_op == IFH_REW_OP_TWO_STEP_PTP)
292 		return NETDEV_TX_OK;
293 
294 	dev_consume_skb_any(skb);
295 	return NETDEV_TX_OK;
296 
297 err:
298 	if (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP &&
299 	    LAN966X_SKB_CB(skb)->rew_op == IFH_REW_OP_TWO_STEP_PTP)
300 		lan966x_ptp_txtstamp_release(port, skb);
301 
302 	return NETDEV_TX_BUSY;
303 }
304 
305 static void lan966x_ifh_set_bypass(void *ifh, u64 bypass)
306 {
307 	packing(ifh, &bypass, IFH_POS_BYPASS + IFH_WID_BYPASS - 1,
308 		IFH_POS_BYPASS, IFH_LEN * 4, PACK, 0);
309 }
310 
311 static void lan966x_ifh_set_port(void *ifh, u64 bypass)
312 {
313 	packing(ifh, &bypass, IFH_POS_DSTS + IFH_WID_DSTS - 1,
314 		IFH_POS_DSTS, IFH_LEN * 4, PACK, 0);
315 }
316 
317 static void lan966x_ifh_set_qos_class(void *ifh, u64 bypass)
318 {
319 	packing(ifh, &bypass, IFH_POS_QOS_CLASS + IFH_WID_QOS_CLASS - 1,
320 		IFH_POS_QOS_CLASS, IFH_LEN * 4, PACK, 0);
321 }
322 
323 static void lan966x_ifh_set_ipv(void *ifh, u64 bypass)
324 {
325 	packing(ifh, &bypass, IFH_POS_IPV + IFH_WID_IPV - 1,
326 		IFH_POS_IPV, IFH_LEN * 4, PACK, 0);
327 }
328 
329 static void lan966x_ifh_set_vid(void *ifh, u64 vid)
330 {
331 	packing(ifh, &vid, IFH_POS_TCI + IFH_WID_TCI - 1,
332 		IFH_POS_TCI, IFH_LEN * 4, PACK, 0);
333 }
334 
335 static void lan966x_ifh_set_rew_op(void *ifh, u64 rew_op)
336 {
337 	packing(ifh, &rew_op, IFH_POS_REW_CMD + IFH_WID_REW_CMD - 1,
338 		IFH_POS_REW_CMD, IFH_LEN * 4, PACK, 0);
339 }
340 
341 static void lan966x_ifh_set_timestamp(void *ifh, u64 timestamp)
342 {
343 	packing(ifh, &timestamp, IFH_POS_TIMESTAMP + IFH_WID_TIMESTAMP - 1,
344 		IFH_POS_TIMESTAMP, IFH_LEN * 4, PACK, 0);
345 }
346 
347 static netdev_tx_t lan966x_port_xmit(struct sk_buff *skb,
348 				     struct net_device *dev)
349 {
350 	struct lan966x_port *port = netdev_priv(dev);
351 	struct lan966x *lan966x = port->lan966x;
352 	__be32 ifh[IFH_LEN];
353 	int err;
354 
355 	memset(ifh, 0x0, sizeof(__be32) * IFH_LEN);
356 
357 	lan966x_ifh_set_bypass(ifh, 1);
358 	lan966x_ifh_set_port(ifh, BIT_ULL(port->chip_port));
359 	lan966x_ifh_set_qos_class(ifh, skb->priority >= 7 ? 0x7 : skb->priority);
360 	lan966x_ifh_set_ipv(ifh, skb->priority >= 7 ? 0x7 : skb->priority);
361 	lan966x_ifh_set_vid(ifh, skb_vlan_tag_get(skb));
362 
363 	if (port->lan966x->ptp && skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) {
364 		err = lan966x_ptp_txtstamp_request(port, skb);
365 		if (err)
366 			return err;
367 
368 		lan966x_ifh_set_rew_op(ifh, LAN966X_SKB_CB(skb)->rew_op);
369 		lan966x_ifh_set_timestamp(ifh, LAN966X_SKB_CB(skb)->ts_id);
370 	}
371 
372 	spin_lock(&lan966x->tx_lock);
373 	if (port->lan966x->fdma)
374 		err = lan966x_fdma_xmit(skb, ifh, dev);
375 	else
376 		err = lan966x_port_ifh_xmit(skb, ifh, dev);
377 	spin_unlock(&lan966x->tx_lock);
378 
379 	return err;
380 }
381 
382 static int lan966x_port_change_mtu(struct net_device *dev, int new_mtu)
383 {
384 	struct lan966x_port *port = netdev_priv(dev);
385 	struct lan966x *lan966x = port->lan966x;
386 	int old_mtu = dev->mtu;
387 	int err;
388 
389 	lan_wr(DEV_MAC_MAXLEN_CFG_MAX_LEN_SET(LAN966X_HW_MTU(new_mtu)),
390 	       lan966x, DEV_MAC_MAXLEN_CFG(port->chip_port));
391 	dev->mtu = new_mtu;
392 
393 	if (!lan966x->fdma)
394 		return 0;
395 
396 	err = lan966x_fdma_change_mtu(lan966x);
397 	if (err) {
398 		lan_wr(DEV_MAC_MAXLEN_CFG_MAX_LEN_SET(LAN966X_HW_MTU(old_mtu)),
399 		       lan966x, DEV_MAC_MAXLEN_CFG(port->chip_port));
400 		dev->mtu = old_mtu;
401 	}
402 
403 	return err;
404 }
405 
406 static int lan966x_mc_unsync(struct net_device *dev, const unsigned char *addr)
407 {
408 	struct lan966x_port *port = netdev_priv(dev);
409 	struct lan966x *lan966x = port->lan966x;
410 
411 	return lan966x_mac_forget(lan966x, addr, HOST_PVID, ENTRYTYPE_LOCKED);
412 }
413 
414 static int lan966x_mc_sync(struct net_device *dev, const unsigned char *addr)
415 {
416 	struct lan966x_port *port = netdev_priv(dev);
417 	struct lan966x *lan966x = port->lan966x;
418 
419 	return lan966x_mac_cpu_learn(lan966x, addr, HOST_PVID);
420 }
421 
422 static void lan966x_port_set_rx_mode(struct net_device *dev)
423 {
424 	__dev_mc_sync(dev, lan966x_mc_sync, lan966x_mc_unsync);
425 }
426 
427 static int lan966x_port_get_parent_id(struct net_device *dev,
428 				      struct netdev_phys_item_id *ppid)
429 {
430 	struct lan966x_port *port = netdev_priv(dev);
431 	struct lan966x *lan966x = port->lan966x;
432 
433 	ppid->id_len = sizeof(lan966x->base_mac);
434 	memcpy(&ppid->id, &lan966x->base_mac, ppid->id_len);
435 
436 	return 0;
437 }
438 
439 static int lan966x_port_ioctl(struct net_device *dev, struct ifreq *ifr,
440 			      int cmd)
441 {
442 	struct lan966x_port *port = netdev_priv(dev);
443 
444 	if (!phy_has_hwtstamp(dev->phydev) && port->lan966x->ptp) {
445 		switch (cmd) {
446 		case SIOCSHWTSTAMP:
447 			return lan966x_ptp_hwtstamp_set(port, ifr);
448 		case SIOCGHWTSTAMP:
449 			return lan966x_ptp_hwtstamp_get(port, ifr);
450 		}
451 	}
452 
453 	if (!dev->phydev)
454 		return -ENODEV;
455 
456 	return phy_mii_ioctl(dev->phydev, ifr, cmd);
457 }
458 
459 static const struct net_device_ops lan966x_port_netdev_ops = {
460 	.ndo_open			= lan966x_port_open,
461 	.ndo_stop			= lan966x_port_stop,
462 	.ndo_start_xmit			= lan966x_port_xmit,
463 	.ndo_change_mtu			= lan966x_port_change_mtu,
464 	.ndo_set_rx_mode		= lan966x_port_set_rx_mode,
465 	.ndo_get_phys_port_name		= lan966x_port_get_phys_port_name,
466 	.ndo_get_stats64		= lan966x_stats_get,
467 	.ndo_set_mac_address		= lan966x_port_set_mac_address,
468 	.ndo_get_port_parent_id		= lan966x_port_get_parent_id,
469 	.ndo_eth_ioctl			= lan966x_port_ioctl,
470 	.ndo_setup_tc			= lan966x_tc_setup,
471 	.ndo_bpf			= lan966x_xdp,
472 };
473 
474 bool lan966x_netdevice_check(const struct net_device *dev)
475 {
476 	return dev->netdev_ops == &lan966x_port_netdev_ops;
477 }
478 
479 bool lan966x_hw_offload(struct lan966x *lan966x, u32 port, struct sk_buff *skb)
480 {
481 	u32 val;
482 
483 	/* The IGMP and MLD frames are not forward by the HW if
484 	 * multicast snooping is enabled, therefor don't mark as
485 	 * offload to allow the SW to forward the frames accordingly.
486 	 */
487 	val = lan_rd(lan966x, ANA_CPU_FWD_CFG(port));
488 	if (!(val & (ANA_CPU_FWD_CFG_IGMP_REDIR_ENA |
489 		     ANA_CPU_FWD_CFG_MLD_REDIR_ENA)))
490 		return true;
491 
492 	if (eth_type_vlan(skb->protocol)) {
493 		skb = skb_vlan_untag(skb);
494 		if (unlikely(!skb))
495 			return false;
496 	}
497 
498 	if (skb->protocol == htons(ETH_P_IP) &&
499 	    ip_hdr(skb)->protocol == IPPROTO_IGMP)
500 		return false;
501 
502 	if (IS_ENABLED(CONFIG_IPV6) &&
503 	    skb->protocol == htons(ETH_P_IPV6) &&
504 	    ipv6_addr_is_multicast(&ipv6_hdr(skb)->daddr) &&
505 	    !ipv6_mc_check_mld(skb))
506 		return false;
507 
508 	return true;
509 }
510 
511 static int lan966x_port_xtr_status(struct lan966x *lan966x, u8 grp)
512 {
513 	return lan_rd(lan966x, QS_XTR_RD(grp));
514 }
515 
516 static int lan966x_port_xtr_ready(struct lan966x *lan966x, u8 grp)
517 {
518 	u32 val;
519 
520 	return read_poll_timeout(lan966x_port_xtr_status, val,
521 				 val != XTR_NOT_READY,
522 				 READL_SLEEP_US, READL_TIMEOUT_US, false,
523 				 lan966x, grp);
524 }
525 
526 static int lan966x_rx_frame_word(struct lan966x *lan966x, u8 grp, u32 *rval)
527 {
528 	u32 bytes_valid;
529 	u32 val;
530 	int err;
531 
532 	val = lan_rd(lan966x, QS_XTR_RD(grp));
533 	if (val == XTR_NOT_READY) {
534 		err = lan966x_port_xtr_ready(lan966x, grp);
535 		if (err)
536 			return -EIO;
537 	}
538 
539 	switch (val) {
540 	case XTR_ABORT:
541 		return -EIO;
542 	case XTR_EOF_0:
543 	case XTR_EOF_1:
544 	case XTR_EOF_2:
545 	case XTR_EOF_3:
546 	case XTR_PRUNED:
547 		bytes_valid = XTR_VALID_BYTES(val);
548 		val = lan_rd(lan966x, QS_XTR_RD(grp));
549 		if (val == XTR_ESCAPE)
550 			*rval = lan_rd(lan966x, QS_XTR_RD(grp));
551 		else
552 			*rval = val;
553 
554 		return bytes_valid;
555 	case XTR_ESCAPE:
556 		*rval = lan_rd(lan966x, QS_XTR_RD(grp));
557 
558 		return 4;
559 	default:
560 		*rval = val;
561 
562 		return 4;
563 	}
564 }
565 
566 void lan966x_ifh_get_src_port(void *ifh, u64 *src_port)
567 {
568 	packing(ifh, src_port, IFH_POS_SRCPORT + IFH_WID_SRCPORT - 1,
569 		IFH_POS_SRCPORT, IFH_LEN * 4, UNPACK, 0);
570 }
571 
572 static void lan966x_ifh_get_len(void *ifh, u64 *len)
573 {
574 	packing(ifh, len, IFH_POS_LEN + IFH_WID_LEN - 1,
575 		IFH_POS_LEN, IFH_LEN * 4, UNPACK, 0);
576 }
577 
578 void lan966x_ifh_get_timestamp(void *ifh, u64 *timestamp)
579 {
580 	packing(ifh, timestamp, IFH_POS_TIMESTAMP + IFH_WID_TIMESTAMP - 1,
581 		IFH_POS_TIMESTAMP, IFH_LEN * 4, UNPACK, 0);
582 }
583 
584 static irqreturn_t lan966x_xtr_irq_handler(int irq, void *args)
585 {
586 	struct lan966x *lan966x = args;
587 	int i, grp = 0, err = 0;
588 
589 	if (!(lan_rd(lan966x, QS_XTR_DATA_PRESENT) & BIT(grp)))
590 		return IRQ_NONE;
591 
592 	do {
593 		u64 src_port, len, timestamp;
594 		struct net_device *dev;
595 		struct sk_buff *skb;
596 		int sz = 0, buf_len;
597 		u32 ifh[IFH_LEN];
598 		u32 *buf;
599 		u32 val;
600 
601 		for (i = 0; i < IFH_LEN; i++) {
602 			err = lan966x_rx_frame_word(lan966x, grp, &ifh[i]);
603 			if (err != 4)
604 				goto recover;
605 		}
606 
607 		err = 0;
608 
609 		lan966x_ifh_get_src_port(ifh, &src_port);
610 		lan966x_ifh_get_len(ifh, &len);
611 		lan966x_ifh_get_timestamp(ifh, &timestamp);
612 
613 		WARN_ON(src_port >= lan966x->num_phys_ports);
614 
615 		dev = lan966x->ports[src_port]->dev;
616 		skb = netdev_alloc_skb(dev, len);
617 		if (unlikely(!skb)) {
618 			netdev_err(dev, "Unable to allocate sk_buff\n");
619 			err = -ENOMEM;
620 			break;
621 		}
622 		buf_len = len - ETH_FCS_LEN;
623 		buf = (u32 *)skb_put(skb, buf_len);
624 
625 		len = 0;
626 		do {
627 			sz = lan966x_rx_frame_word(lan966x, grp, &val);
628 			if (sz < 0) {
629 				kfree_skb(skb);
630 				goto recover;
631 			}
632 
633 			*buf++ = val;
634 			len += sz;
635 		} while (len < buf_len);
636 
637 		/* Read the FCS */
638 		sz = lan966x_rx_frame_word(lan966x, grp, &val);
639 		if (sz < 0) {
640 			kfree_skb(skb);
641 			goto recover;
642 		}
643 
644 		/* Update the statistics if part of the FCS was read before */
645 		len -= ETH_FCS_LEN - sz;
646 
647 		if (unlikely(dev->features & NETIF_F_RXFCS)) {
648 			buf = (u32 *)skb_put(skb, ETH_FCS_LEN);
649 			*buf = val;
650 		}
651 
652 		lan966x_ptp_rxtstamp(lan966x, skb, timestamp);
653 		skb->protocol = eth_type_trans(skb, dev);
654 
655 		if (lan966x->bridge_mask & BIT(src_port)) {
656 			skb->offload_fwd_mark = 1;
657 
658 			skb_reset_network_header(skb);
659 			if (!lan966x_hw_offload(lan966x, src_port, skb))
660 				skb->offload_fwd_mark = 0;
661 		}
662 
663 		if (!skb_defer_rx_timestamp(skb))
664 			netif_rx(skb);
665 
666 		dev->stats.rx_bytes += len;
667 		dev->stats.rx_packets++;
668 
669 recover:
670 		if (sz < 0 || err)
671 			lan_rd(lan966x, QS_XTR_RD(grp));
672 
673 	} while (lan_rd(lan966x, QS_XTR_DATA_PRESENT) & BIT(grp));
674 
675 	return IRQ_HANDLED;
676 }
677 
678 static irqreturn_t lan966x_ana_irq_handler(int irq, void *args)
679 {
680 	struct lan966x *lan966x = args;
681 
682 	return lan966x_mac_irq_handler(lan966x);
683 }
684 
685 static void lan966x_cleanup_ports(struct lan966x *lan966x)
686 {
687 	struct lan966x_port *port;
688 	int p;
689 
690 	for (p = 0; p < lan966x->num_phys_ports; p++) {
691 		port = lan966x->ports[p];
692 		if (!port)
693 			continue;
694 
695 		if (port->dev)
696 			unregister_netdev(port->dev);
697 
698 		lan966x_xdp_port_deinit(port);
699 		if (lan966x->fdma && lan966x->fdma_ndev == port->dev)
700 			lan966x_fdma_netdev_deinit(lan966x, port->dev);
701 
702 		if (port->phylink) {
703 			rtnl_lock();
704 			lan966x_port_stop(port->dev);
705 			rtnl_unlock();
706 			phylink_destroy(port->phylink);
707 			port->phylink = NULL;
708 		}
709 
710 		if (port->fwnode)
711 			fwnode_handle_put(port->fwnode);
712 	}
713 
714 	disable_irq(lan966x->xtr_irq);
715 	lan966x->xtr_irq = -ENXIO;
716 
717 	if (lan966x->ana_irq > 0) {
718 		disable_irq(lan966x->ana_irq);
719 		lan966x->ana_irq = -ENXIO;
720 	}
721 
722 	if (lan966x->fdma)
723 		devm_free_irq(lan966x->dev, lan966x->fdma_irq, lan966x);
724 
725 	if (lan966x->ptp_irq > 0)
726 		devm_free_irq(lan966x->dev, lan966x->ptp_irq, lan966x);
727 
728 	if (lan966x->ptp_ext_irq > 0)
729 		devm_free_irq(lan966x->dev, lan966x->ptp_ext_irq, lan966x);
730 }
731 
732 static int lan966x_probe_port(struct lan966x *lan966x, u32 p,
733 			      phy_interface_t phy_mode,
734 			      struct fwnode_handle *portnp)
735 {
736 	struct lan966x_port *port;
737 	struct phylink *phylink;
738 	struct net_device *dev;
739 	int err;
740 
741 	if (p >= lan966x->num_phys_ports)
742 		return -EINVAL;
743 
744 	dev = devm_alloc_etherdev_mqs(lan966x->dev,
745 				      sizeof(struct lan966x_port),
746 				      NUM_PRIO_QUEUES, 1);
747 	if (!dev)
748 		return -ENOMEM;
749 
750 	SET_NETDEV_DEV(dev, lan966x->dev);
751 	port = netdev_priv(dev);
752 	port->dev = dev;
753 	port->lan966x = lan966x;
754 	port->chip_port = p;
755 	lan966x->ports[p] = port;
756 
757 	dev->max_mtu = ETH_MAX_MTU;
758 
759 	dev->netdev_ops = &lan966x_port_netdev_ops;
760 	dev->ethtool_ops = &lan966x_ethtool_ops;
761 	dev->features |= NETIF_F_HW_VLAN_CTAG_TX |
762 			 NETIF_F_HW_VLAN_STAG_TX |
763 			 NETIF_F_HW_TC;
764 	dev->hw_features |= NETIF_F_HW_TC;
765 	dev->needed_headroom = IFH_LEN_BYTES;
766 
767 	eth_hw_addr_gen(dev, lan966x->base_mac, p + 1);
768 
769 	lan966x_mac_learn(lan966x, PGID_CPU, dev->dev_addr, HOST_PVID,
770 			  ENTRYTYPE_LOCKED);
771 
772 	port->phylink_config.dev = &port->dev->dev;
773 	port->phylink_config.type = PHYLINK_NETDEV;
774 	port->phylink_pcs.poll = true;
775 	port->phylink_pcs.ops = &lan966x_phylink_pcs_ops;
776 
777 	port->phylink_config.mac_capabilities = MAC_ASYM_PAUSE | MAC_SYM_PAUSE |
778 		MAC_10 | MAC_100 | MAC_1000FD | MAC_2500FD;
779 
780 	phy_interface_set_rgmii(port->phylink_config.supported_interfaces);
781 	__set_bit(PHY_INTERFACE_MODE_MII,
782 		  port->phylink_config.supported_interfaces);
783 	__set_bit(PHY_INTERFACE_MODE_GMII,
784 		  port->phylink_config.supported_interfaces);
785 	__set_bit(PHY_INTERFACE_MODE_SGMII,
786 		  port->phylink_config.supported_interfaces);
787 	__set_bit(PHY_INTERFACE_MODE_QSGMII,
788 		  port->phylink_config.supported_interfaces);
789 	__set_bit(PHY_INTERFACE_MODE_QUSGMII,
790 		  port->phylink_config.supported_interfaces);
791 	__set_bit(PHY_INTERFACE_MODE_1000BASEX,
792 		  port->phylink_config.supported_interfaces);
793 	__set_bit(PHY_INTERFACE_MODE_2500BASEX,
794 		  port->phylink_config.supported_interfaces);
795 
796 	phylink = phylink_create(&port->phylink_config,
797 				 portnp,
798 				 phy_mode,
799 				 &lan966x_phylink_mac_ops);
800 	if (IS_ERR(phylink)) {
801 		port->dev = NULL;
802 		return PTR_ERR(phylink);
803 	}
804 
805 	port->phylink = phylink;
806 
807 	err = register_netdev(dev);
808 	if (err) {
809 		dev_err(lan966x->dev, "register_netdev failed\n");
810 		return err;
811 	}
812 
813 	lan966x_vlan_port_set_vlan_aware(port, 0);
814 	lan966x_vlan_port_set_vid(port, HOST_PVID, false, false);
815 	lan966x_vlan_port_apply(port);
816 
817 	return 0;
818 }
819 
820 static void lan966x_init(struct lan966x *lan966x)
821 {
822 	u32 p, i;
823 
824 	/* MAC table initialization */
825 	lan966x_mac_init(lan966x);
826 
827 	lan966x_vlan_init(lan966x);
828 
829 	/* Flush queues */
830 	lan_wr(lan_rd(lan966x, QS_XTR_FLUSH) |
831 	       GENMASK(1, 0),
832 	       lan966x, QS_XTR_FLUSH);
833 
834 	/* Allow to drain */
835 	mdelay(1);
836 
837 	/* All Queues normal */
838 	lan_wr(lan_rd(lan966x, QS_XTR_FLUSH) &
839 	       ~(GENMASK(1, 0)),
840 	       lan966x, QS_XTR_FLUSH);
841 
842 	/* Set MAC age time to default value, the entry is aged after
843 	 * 2 * AGE_PERIOD
844 	 */
845 	lan_wr(ANA_AUTOAGE_AGE_PERIOD_SET(BR_DEFAULT_AGEING_TIME / 2 / HZ),
846 	       lan966x, ANA_AUTOAGE);
847 
848 	/* Disable learning for frames discarded by VLAN ingress filtering */
849 	lan_rmw(ANA_ADVLEARN_VLAN_CHK_SET(1),
850 		ANA_ADVLEARN_VLAN_CHK,
851 		lan966x, ANA_ADVLEARN);
852 
853 	/* Setup frame ageing - "2 sec" - The unit is 6.5 us on lan966x */
854 	lan_wr(SYS_FRM_AGING_AGE_TX_ENA_SET(1) |
855 	       (20000000 / 65),
856 	       lan966x,  SYS_FRM_AGING);
857 
858 	/* Map the 8 CPU extraction queues to CPU port */
859 	lan_wr(0, lan966x, QSYS_CPU_GROUP_MAP);
860 
861 	/* Do byte-swap and expect status after last data word
862 	 * Extraction: Mode: manual extraction) | Byte_swap
863 	 */
864 	lan_wr(QS_XTR_GRP_CFG_MODE_SET(lan966x->fdma ? 2 : 1) |
865 	       QS_XTR_GRP_CFG_BYTE_SWAP_SET(1),
866 	       lan966x, QS_XTR_GRP_CFG(0));
867 
868 	/* Injection: Mode: manual injection | Byte_swap */
869 	lan_wr(QS_INJ_GRP_CFG_MODE_SET(lan966x->fdma ? 2 : 1) |
870 	       QS_INJ_GRP_CFG_BYTE_SWAP_SET(1),
871 	       lan966x, QS_INJ_GRP_CFG(0));
872 
873 	lan_rmw(QS_INJ_CTRL_GAP_SIZE_SET(0),
874 		QS_INJ_CTRL_GAP_SIZE,
875 		lan966x, QS_INJ_CTRL(0));
876 
877 	/* Enable IFH insertion/parsing on CPU ports */
878 	lan_wr(SYS_PORT_MODE_INCL_INJ_HDR_SET(1) |
879 	       SYS_PORT_MODE_INCL_XTR_HDR_SET(1),
880 	       lan966x, SYS_PORT_MODE(CPU_PORT));
881 
882 	/* Setup flooding PGIDs */
883 	lan_wr(ANA_FLOODING_IPMC_FLD_MC4_DATA_SET(PGID_MCIPV4) |
884 	       ANA_FLOODING_IPMC_FLD_MC4_CTRL_SET(PGID_MC) |
885 	       ANA_FLOODING_IPMC_FLD_MC6_DATA_SET(PGID_MCIPV6) |
886 	       ANA_FLOODING_IPMC_FLD_MC6_CTRL_SET(PGID_MC),
887 	       lan966x, ANA_FLOODING_IPMC);
888 
889 	/* There are 8 priorities */
890 	for (i = 0; i < 8; ++i)
891 		lan_rmw(ANA_FLOODING_FLD_MULTICAST_SET(PGID_MC) |
892 			ANA_FLOODING_FLD_UNICAST_SET(PGID_UC) |
893 			ANA_FLOODING_FLD_BROADCAST_SET(PGID_BC),
894 			ANA_FLOODING_FLD_MULTICAST |
895 			ANA_FLOODING_FLD_UNICAST |
896 			ANA_FLOODING_FLD_BROADCAST,
897 			lan966x, ANA_FLOODING(i));
898 
899 	for (i = 0; i < PGID_ENTRIES; ++i)
900 		/* Set all the entries to obey VLAN_VLAN */
901 		lan_rmw(ANA_PGID_CFG_OBEY_VLAN_SET(1),
902 			ANA_PGID_CFG_OBEY_VLAN,
903 			lan966x, ANA_PGID_CFG(i));
904 
905 	for (p = 0; p < lan966x->num_phys_ports; p++) {
906 		/* Disable bridging by default */
907 		lan_rmw(ANA_PGID_PGID_SET(0x0),
908 			ANA_PGID_PGID,
909 			lan966x, ANA_PGID(p + PGID_SRC));
910 
911 		/* Do not forward BPDU frames to the front ports and copy them
912 		 * to CPU
913 		 */
914 		lan_wr(0xffff, lan966x, ANA_CPU_FWD_BPDU_CFG(p));
915 	}
916 
917 	/* Set source buffer size for each priority and each port to 1500 bytes */
918 	for (i = 0; i <= QSYS_Q_RSRV; ++i) {
919 		lan_wr(1500 / 64, lan966x, QSYS_RES_CFG(i));
920 		lan_wr(1500 / 64, lan966x, QSYS_RES_CFG(512 + i));
921 	}
922 
923 	/* Enable switching to/from cpu port */
924 	lan_wr(QSYS_SW_PORT_MODE_PORT_ENA_SET(1) |
925 	       QSYS_SW_PORT_MODE_SCH_NEXT_CFG_SET(1) |
926 	       QSYS_SW_PORT_MODE_INGRESS_DROP_MODE_SET(1),
927 	       lan966x,  QSYS_SW_PORT_MODE(CPU_PORT));
928 
929 	/* Configure and enable the CPU port */
930 	lan_rmw(ANA_PGID_PGID_SET(0),
931 		ANA_PGID_PGID,
932 		lan966x, ANA_PGID(CPU_PORT));
933 	lan_rmw(ANA_PGID_PGID_SET(BIT(CPU_PORT)),
934 		ANA_PGID_PGID,
935 		lan966x, ANA_PGID(PGID_CPU));
936 
937 	/* Multicast to all other ports */
938 	lan_rmw(GENMASK(lan966x->num_phys_ports - 1, 0),
939 		ANA_PGID_PGID,
940 		lan966x, ANA_PGID(PGID_MC));
941 
942 	/* This will be controlled by mrouter ports */
943 	lan_rmw(GENMASK(lan966x->num_phys_ports - 1, 0),
944 		ANA_PGID_PGID,
945 		lan966x, ANA_PGID(PGID_MCIPV4));
946 
947 	lan_rmw(GENMASK(lan966x->num_phys_ports - 1, 0),
948 		ANA_PGID_PGID,
949 		lan966x, ANA_PGID(PGID_MCIPV6));
950 
951 	/* Unicast to all other ports */
952 	lan_rmw(GENMASK(lan966x->num_phys_ports - 1, 0),
953 		ANA_PGID_PGID,
954 		lan966x, ANA_PGID(PGID_UC));
955 
956 	/* Broadcast to the CPU port and to other ports */
957 	lan_rmw(ANA_PGID_PGID_SET(BIT(CPU_PORT) | GENMASK(lan966x->num_phys_ports - 1, 0)),
958 		ANA_PGID_PGID,
959 		lan966x, ANA_PGID(PGID_BC));
960 
961 	lan_wr(REW_PORT_CFG_NO_REWRITE_SET(1),
962 	       lan966x, REW_PORT_CFG(CPU_PORT));
963 
964 	lan_rmw(ANA_ANAINTR_INTR_ENA_SET(1),
965 		ANA_ANAINTR_INTR_ENA,
966 		lan966x, ANA_ANAINTR);
967 
968 	spin_lock_init(&lan966x->tx_lock);
969 
970 	lan966x_taprio_init(lan966x);
971 }
972 
973 static int lan966x_ram_init(struct lan966x *lan966x)
974 {
975 	return lan_rd(lan966x, SYS_RAM_INIT);
976 }
977 
978 static int lan966x_reset_switch(struct lan966x *lan966x)
979 {
980 	struct reset_control *switch_reset;
981 	int val = 0;
982 	int ret;
983 
984 	switch_reset = devm_reset_control_get_optional_shared(lan966x->dev,
985 							      "switch");
986 	if (IS_ERR(switch_reset))
987 		return dev_err_probe(lan966x->dev, PTR_ERR(switch_reset),
988 				     "Could not obtain switch reset");
989 
990 	reset_control_reset(switch_reset);
991 
992 	lan_wr(SYS_RESET_CFG_CORE_ENA_SET(0), lan966x, SYS_RESET_CFG);
993 	lan_wr(SYS_RAM_INIT_RAM_INIT_SET(1), lan966x, SYS_RAM_INIT);
994 	ret = readx_poll_timeout(lan966x_ram_init, lan966x,
995 				 val, (val & BIT(1)) == 0, READL_SLEEP_US,
996 				 READL_TIMEOUT_US);
997 	if (ret)
998 		return ret;
999 
1000 	lan_wr(SYS_RESET_CFG_CORE_ENA_SET(1), lan966x, SYS_RESET_CFG);
1001 
1002 	return 0;
1003 }
1004 
1005 static int lan966x_probe(struct platform_device *pdev)
1006 {
1007 	struct fwnode_handle *ports, *portnp;
1008 	struct lan966x *lan966x;
1009 	u8 mac_addr[ETH_ALEN];
1010 	int err;
1011 
1012 	lan966x = devm_kzalloc(&pdev->dev, sizeof(*lan966x), GFP_KERNEL);
1013 	if (!lan966x)
1014 		return -ENOMEM;
1015 
1016 	platform_set_drvdata(pdev, lan966x);
1017 	lan966x->dev = &pdev->dev;
1018 
1019 	if (!device_get_mac_address(&pdev->dev, mac_addr)) {
1020 		ether_addr_copy(lan966x->base_mac, mac_addr);
1021 	} else {
1022 		pr_info("MAC addr was not set, use random MAC\n");
1023 		eth_random_addr(lan966x->base_mac);
1024 		lan966x->base_mac[5] &= 0xf0;
1025 	}
1026 
1027 	ports = device_get_named_child_node(&pdev->dev, "ethernet-ports");
1028 	if (!ports)
1029 		return dev_err_probe(&pdev->dev, -ENODEV,
1030 				     "no ethernet-ports child found\n");
1031 
1032 	err = lan966x_create_targets(pdev, lan966x);
1033 	if (err)
1034 		return dev_err_probe(&pdev->dev, err,
1035 				     "Failed to create targets");
1036 
1037 	err = lan966x_reset_switch(lan966x);
1038 	if (err)
1039 		return dev_err_probe(&pdev->dev, err, "Reset failed");
1040 
1041 	lan966x->num_phys_ports = NUM_PHYS_PORTS;
1042 	lan966x->ports = devm_kcalloc(&pdev->dev, lan966x->num_phys_ports,
1043 				      sizeof(struct lan966x_port *),
1044 				      GFP_KERNEL);
1045 	if (!lan966x->ports)
1046 		return -ENOMEM;
1047 
1048 	/* There QS system has 32KB of memory */
1049 	lan966x->shared_queue_sz = LAN966X_BUFFER_MEMORY;
1050 
1051 	/* set irq */
1052 	lan966x->xtr_irq = platform_get_irq_byname(pdev, "xtr");
1053 	if (lan966x->xtr_irq <= 0)
1054 		return -EINVAL;
1055 
1056 	err = devm_request_threaded_irq(&pdev->dev, lan966x->xtr_irq, NULL,
1057 					lan966x_xtr_irq_handler, IRQF_ONESHOT,
1058 					"frame extraction", lan966x);
1059 	if (err) {
1060 		pr_err("Unable to use xtr irq");
1061 		return -ENODEV;
1062 	}
1063 
1064 	lan966x->ana_irq = platform_get_irq_byname(pdev, "ana");
1065 	if (lan966x->ana_irq > 0) {
1066 		err = devm_request_threaded_irq(&pdev->dev, lan966x->ana_irq, NULL,
1067 						lan966x_ana_irq_handler, IRQF_ONESHOT,
1068 						"ana irq", lan966x);
1069 		if (err)
1070 			return dev_err_probe(&pdev->dev, err, "Unable to use ana irq");
1071 	}
1072 
1073 	lan966x->ptp_irq = platform_get_irq_byname(pdev, "ptp");
1074 	if (lan966x->ptp_irq > 0) {
1075 		err = devm_request_threaded_irq(&pdev->dev, lan966x->ptp_irq, NULL,
1076 						lan966x_ptp_irq_handler, IRQF_ONESHOT,
1077 						"ptp irq", lan966x);
1078 		if (err)
1079 			return dev_err_probe(&pdev->dev, err, "Unable to use ptp irq");
1080 
1081 		lan966x->ptp = 1;
1082 	}
1083 
1084 	lan966x->fdma_irq = platform_get_irq_byname(pdev, "fdma");
1085 	if (lan966x->fdma_irq > 0) {
1086 		err = devm_request_irq(&pdev->dev, lan966x->fdma_irq,
1087 				       lan966x_fdma_irq_handler, 0,
1088 				       "fdma irq", lan966x);
1089 		if (err)
1090 			return dev_err_probe(&pdev->dev, err, "Unable to use fdma irq");
1091 
1092 		lan966x->fdma = true;
1093 	}
1094 
1095 	if (lan966x->ptp) {
1096 		lan966x->ptp_ext_irq = platform_get_irq_byname(pdev, "ptp-ext");
1097 		if (lan966x->ptp_ext_irq > 0) {
1098 			err = devm_request_threaded_irq(&pdev->dev,
1099 							lan966x->ptp_ext_irq, NULL,
1100 							lan966x_ptp_ext_irq_handler,
1101 							IRQF_ONESHOT,
1102 							"ptp-ext irq", lan966x);
1103 			if (err)
1104 				return dev_err_probe(&pdev->dev, err,
1105 						     "Unable to use ptp-ext irq");
1106 		}
1107 	}
1108 
1109 	/* init switch */
1110 	lan966x_init(lan966x);
1111 	lan966x_stats_init(lan966x);
1112 
1113 	/* go over the child nodes */
1114 	fwnode_for_each_available_child_node(ports, portnp) {
1115 		phy_interface_t phy_mode;
1116 		struct phy *serdes;
1117 		u32 p;
1118 
1119 		if (fwnode_property_read_u32(portnp, "reg", &p))
1120 			continue;
1121 
1122 		phy_mode = fwnode_get_phy_mode(portnp);
1123 		err = lan966x_probe_port(lan966x, p, phy_mode, portnp);
1124 		if (err)
1125 			goto cleanup_ports;
1126 
1127 		/* Read needed configuration */
1128 		lan966x->ports[p]->config.portmode = phy_mode;
1129 		lan966x->ports[p]->fwnode = fwnode_handle_get(portnp);
1130 
1131 		serdes = devm_of_phy_get(lan966x->dev, to_of_node(portnp), NULL);
1132 		if (PTR_ERR(serdes) == -ENODEV)
1133 			serdes = NULL;
1134 		if (IS_ERR(serdes)) {
1135 			err = PTR_ERR(serdes);
1136 			goto cleanup_ports;
1137 		}
1138 		lan966x->ports[p]->serdes = serdes;
1139 
1140 		lan966x_port_init(lan966x->ports[p]);
1141 		err = lan966x_xdp_port_init(lan966x->ports[p]);
1142 		if (err)
1143 			goto cleanup_ports;
1144 	}
1145 
1146 	lan966x_mdb_init(lan966x);
1147 	err = lan966x_fdb_init(lan966x);
1148 	if (err)
1149 		goto cleanup_ports;
1150 
1151 	err = lan966x_ptp_init(lan966x);
1152 	if (err)
1153 		goto cleanup_fdb;
1154 
1155 	err = lan966x_fdma_init(lan966x);
1156 	if (err)
1157 		goto cleanup_ptp;
1158 
1159 	return 0;
1160 
1161 cleanup_ptp:
1162 	lan966x_ptp_deinit(lan966x);
1163 
1164 cleanup_fdb:
1165 	lan966x_fdb_deinit(lan966x);
1166 
1167 cleanup_ports:
1168 	fwnode_handle_put(portnp);
1169 
1170 	lan966x_cleanup_ports(lan966x);
1171 
1172 	cancel_delayed_work_sync(&lan966x->stats_work);
1173 	destroy_workqueue(lan966x->stats_queue);
1174 	mutex_destroy(&lan966x->stats_lock);
1175 
1176 	return err;
1177 }
1178 
1179 static int lan966x_remove(struct platform_device *pdev)
1180 {
1181 	struct lan966x *lan966x = platform_get_drvdata(pdev);
1182 
1183 	lan966x_taprio_deinit(lan966x);
1184 	lan966x_fdma_deinit(lan966x);
1185 	lan966x_cleanup_ports(lan966x);
1186 
1187 	cancel_delayed_work_sync(&lan966x->stats_work);
1188 	destroy_workqueue(lan966x->stats_queue);
1189 	mutex_destroy(&lan966x->stats_lock);
1190 
1191 	lan966x_mac_purge_entries(lan966x);
1192 	lan966x_mdb_deinit(lan966x);
1193 	lan966x_fdb_deinit(lan966x);
1194 	lan966x_ptp_deinit(lan966x);
1195 
1196 	return 0;
1197 }
1198 
1199 static struct platform_driver lan966x_driver = {
1200 	.probe = lan966x_probe,
1201 	.remove = lan966x_remove,
1202 	.driver = {
1203 		.name = "lan966x-switch",
1204 		.of_match_table = lan966x_match,
1205 	},
1206 };
1207 
1208 static int __init lan966x_switch_driver_init(void)
1209 {
1210 	int ret;
1211 
1212 	lan966x_register_notifier_blocks();
1213 
1214 	ret = platform_driver_register(&lan966x_driver);
1215 	if (ret)
1216 		goto err;
1217 
1218 	return 0;
1219 
1220 err:
1221 	lan966x_unregister_notifier_blocks();
1222 	return ret;
1223 }
1224 
1225 static void __exit lan966x_switch_driver_exit(void)
1226 {
1227 	platform_driver_unregister(&lan966x_driver);
1228 	lan966x_unregister_notifier_blocks();
1229 }
1230 
1231 module_init(lan966x_switch_driver_init);
1232 module_exit(lan966x_switch_driver_exit);
1233 
1234 MODULE_DESCRIPTION("Microchip LAN966X switch driver");
1235 MODULE_AUTHOR("Horatiu Vultur <horatiu.vultur@microchip.com>");
1236 MODULE_LICENSE("Dual MIT/GPL");
1237