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 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 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 	.ndo_xdp_xmit			= lan966x_xdp_xmit,
473 };
474 
475 bool lan966x_netdevice_check(const struct net_device *dev)
476 {
477 	return dev->netdev_ops == &lan966x_port_netdev_ops;
478 }
479 
480 bool lan966x_hw_offload(struct lan966x *lan966x, u32 port, struct sk_buff *skb)
481 {
482 	u32 val;
483 
484 	/* The IGMP and MLD frames are not forward by the HW if
485 	 * multicast snooping is enabled, therefor don't mark as
486 	 * offload to allow the SW to forward the frames accordingly.
487 	 */
488 	val = lan_rd(lan966x, ANA_CPU_FWD_CFG(port));
489 	if (!(val & (ANA_CPU_FWD_CFG_IGMP_REDIR_ENA |
490 		     ANA_CPU_FWD_CFG_MLD_REDIR_ENA)))
491 		return true;
492 
493 	if (eth_type_vlan(skb->protocol)) {
494 		skb = skb_vlan_untag(skb);
495 		if (unlikely(!skb))
496 			return false;
497 	}
498 
499 	if (skb->protocol == htons(ETH_P_IP) &&
500 	    ip_hdr(skb)->protocol == IPPROTO_IGMP)
501 		return false;
502 
503 	if (IS_ENABLED(CONFIG_IPV6) &&
504 	    skb->protocol == htons(ETH_P_IPV6) &&
505 	    ipv6_addr_is_multicast(&ipv6_hdr(skb)->daddr) &&
506 	    !ipv6_mc_check_mld(skb))
507 		return false;
508 
509 	return true;
510 }
511 
512 static int lan966x_port_xtr_status(struct lan966x *lan966x, u8 grp)
513 {
514 	return lan_rd(lan966x, QS_XTR_RD(grp));
515 }
516 
517 static int lan966x_port_xtr_ready(struct lan966x *lan966x, u8 grp)
518 {
519 	u32 val;
520 
521 	return read_poll_timeout(lan966x_port_xtr_status, val,
522 				 val != XTR_NOT_READY,
523 				 READL_SLEEP_US, READL_TIMEOUT_US, false,
524 				 lan966x, grp);
525 }
526 
527 static int lan966x_rx_frame_word(struct lan966x *lan966x, u8 grp, u32 *rval)
528 {
529 	u32 bytes_valid;
530 	u32 val;
531 	int err;
532 
533 	val = lan_rd(lan966x, QS_XTR_RD(grp));
534 	if (val == XTR_NOT_READY) {
535 		err = lan966x_port_xtr_ready(lan966x, grp);
536 		if (err)
537 			return -EIO;
538 	}
539 
540 	switch (val) {
541 	case XTR_ABORT:
542 		return -EIO;
543 	case XTR_EOF_0:
544 	case XTR_EOF_1:
545 	case XTR_EOF_2:
546 	case XTR_EOF_3:
547 	case XTR_PRUNED:
548 		bytes_valid = XTR_VALID_BYTES(val);
549 		val = lan_rd(lan966x, QS_XTR_RD(grp));
550 		if (val == XTR_ESCAPE)
551 			*rval = lan_rd(lan966x, QS_XTR_RD(grp));
552 		else
553 			*rval = val;
554 
555 		return bytes_valid;
556 	case XTR_ESCAPE:
557 		*rval = lan_rd(lan966x, QS_XTR_RD(grp));
558 
559 		return 4;
560 	default:
561 		*rval = val;
562 
563 		return 4;
564 	}
565 }
566 
567 void lan966x_ifh_get_src_port(void *ifh, u64 *src_port)
568 {
569 	packing(ifh, src_port, IFH_POS_SRCPORT + IFH_WID_SRCPORT - 1,
570 		IFH_POS_SRCPORT, IFH_LEN * 4, UNPACK, 0);
571 }
572 
573 static void lan966x_ifh_get_len(void *ifh, u64 *len)
574 {
575 	packing(ifh, len, IFH_POS_LEN + IFH_WID_LEN - 1,
576 		IFH_POS_LEN, IFH_LEN * 4, UNPACK, 0);
577 }
578 
579 void lan966x_ifh_get_timestamp(void *ifh, u64 *timestamp)
580 {
581 	packing(ifh, timestamp, IFH_POS_TIMESTAMP + IFH_WID_TIMESTAMP - 1,
582 		IFH_POS_TIMESTAMP, IFH_LEN * 4, UNPACK, 0);
583 }
584 
585 static irqreturn_t lan966x_xtr_irq_handler(int irq, void *args)
586 {
587 	struct lan966x *lan966x = args;
588 	int i, grp = 0, err = 0;
589 
590 	if (!(lan_rd(lan966x, QS_XTR_DATA_PRESENT) & BIT(grp)))
591 		return IRQ_NONE;
592 
593 	do {
594 		u64 src_port, len, timestamp;
595 		struct net_device *dev;
596 		struct sk_buff *skb;
597 		int sz = 0, buf_len;
598 		u32 ifh[IFH_LEN];
599 		u32 *buf;
600 		u32 val;
601 
602 		for (i = 0; i < IFH_LEN; i++) {
603 			err = lan966x_rx_frame_word(lan966x, grp, &ifh[i]);
604 			if (err != 4)
605 				goto recover;
606 		}
607 
608 		err = 0;
609 
610 		lan966x_ifh_get_src_port(ifh, &src_port);
611 		lan966x_ifh_get_len(ifh, &len);
612 		lan966x_ifh_get_timestamp(ifh, &timestamp);
613 
614 		WARN_ON(src_port >= lan966x->num_phys_ports);
615 
616 		dev = lan966x->ports[src_port]->dev;
617 		skb = netdev_alloc_skb(dev, len);
618 		if (unlikely(!skb)) {
619 			netdev_err(dev, "Unable to allocate sk_buff\n");
620 			err = -ENOMEM;
621 			break;
622 		}
623 		buf_len = len - ETH_FCS_LEN;
624 		buf = (u32 *)skb_put(skb, buf_len);
625 
626 		len = 0;
627 		do {
628 			sz = lan966x_rx_frame_word(lan966x, grp, &val);
629 			if (sz < 0) {
630 				kfree_skb(skb);
631 				goto recover;
632 			}
633 
634 			*buf++ = val;
635 			len += sz;
636 		} while (len < buf_len);
637 
638 		/* Read the FCS */
639 		sz = lan966x_rx_frame_word(lan966x, grp, &val);
640 		if (sz < 0) {
641 			kfree_skb(skb);
642 			goto recover;
643 		}
644 
645 		/* Update the statistics if part of the FCS was read before */
646 		len -= ETH_FCS_LEN - sz;
647 
648 		if (unlikely(dev->features & NETIF_F_RXFCS)) {
649 			buf = (u32 *)skb_put(skb, ETH_FCS_LEN);
650 			*buf = val;
651 		}
652 
653 		lan966x_ptp_rxtstamp(lan966x, skb, timestamp);
654 		skb->protocol = eth_type_trans(skb, dev);
655 
656 		if (lan966x->bridge_mask & BIT(src_port)) {
657 			skb->offload_fwd_mark = 1;
658 
659 			skb_reset_network_header(skb);
660 			if (!lan966x_hw_offload(lan966x, src_port, skb))
661 				skb->offload_fwd_mark = 0;
662 		}
663 
664 		if (!skb_defer_rx_timestamp(skb))
665 			netif_rx(skb);
666 
667 		dev->stats.rx_bytes += len;
668 		dev->stats.rx_packets++;
669 
670 recover:
671 		if (sz < 0 || err)
672 			lan_rd(lan966x, QS_XTR_RD(grp));
673 
674 	} while (lan_rd(lan966x, QS_XTR_DATA_PRESENT) & BIT(grp));
675 
676 	return IRQ_HANDLED;
677 }
678 
679 static irqreturn_t lan966x_ana_irq_handler(int irq, void *args)
680 {
681 	struct lan966x *lan966x = args;
682 
683 	return lan966x_mac_irq_handler(lan966x);
684 }
685 
686 static void lan966x_cleanup_ports(struct lan966x *lan966x)
687 {
688 	struct lan966x_port *port;
689 	int p;
690 
691 	for (p = 0; p < lan966x->num_phys_ports; p++) {
692 		port = lan966x->ports[p];
693 		if (!port)
694 			continue;
695 
696 		if (port->dev)
697 			unregister_netdev(port->dev);
698 
699 		lan966x_xdp_port_deinit(port);
700 		if (lan966x->fdma && lan966x->fdma_ndev == port->dev)
701 			lan966x_fdma_netdev_deinit(lan966x, port->dev);
702 
703 		if (port->phylink) {
704 			rtnl_lock();
705 			lan966x_port_stop(port->dev);
706 			rtnl_unlock();
707 			phylink_destroy(port->phylink);
708 			port->phylink = NULL;
709 		}
710 
711 		if (port->fwnode)
712 			fwnode_handle_put(port->fwnode);
713 	}
714 
715 	disable_irq(lan966x->xtr_irq);
716 	lan966x->xtr_irq = -ENXIO;
717 
718 	if (lan966x->ana_irq > 0) {
719 		disable_irq(lan966x->ana_irq);
720 		lan966x->ana_irq = -ENXIO;
721 	}
722 
723 	if (lan966x->fdma)
724 		devm_free_irq(lan966x->dev, lan966x->fdma_irq, lan966x);
725 
726 	if (lan966x->ptp_irq > 0)
727 		devm_free_irq(lan966x->dev, lan966x->ptp_irq, lan966x);
728 
729 	if (lan966x->ptp_ext_irq > 0)
730 		devm_free_irq(lan966x->dev, lan966x->ptp_ext_irq, lan966x);
731 }
732 
733 static int lan966x_probe_port(struct lan966x *lan966x, u32 p,
734 			      phy_interface_t phy_mode,
735 			      struct fwnode_handle *portnp)
736 {
737 	struct lan966x_port *port;
738 	struct phylink *phylink;
739 	struct net_device *dev;
740 	int err;
741 
742 	if (p >= lan966x->num_phys_ports)
743 		return -EINVAL;
744 
745 	dev = devm_alloc_etherdev_mqs(lan966x->dev,
746 				      sizeof(struct lan966x_port),
747 				      NUM_PRIO_QUEUES, 1);
748 	if (!dev)
749 		return -ENOMEM;
750 
751 	SET_NETDEV_DEV(dev, lan966x->dev);
752 	port = netdev_priv(dev);
753 	port->dev = dev;
754 	port->lan966x = lan966x;
755 	port->chip_port = p;
756 	lan966x->ports[p] = port;
757 
758 	dev->max_mtu = ETH_MAX_MTU;
759 
760 	dev->netdev_ops = &lan966x_port_netdev_ops;
761 	dev->ethtool_ops = &lan966x_ethtool_ops;
762 	dev->features |= NETIF_F_HW_VLAN_CTAG_TX |
763 			 NETIF_F_HW_VLAN_STAG_TX |
764 			 NETIF_F_HW_TC;
765 	dev->hw_features |= NETIF_F_HW_TC;
766 	dev->needed_headroom = IFH_LEN_BYTES;
767 
768 	eth_hw_addr_gen(dev, lan966x->base_mac, p + 1);
769 
770 	lan966x_mac_learn(lan966x, PGID_CPU, dev->dev_addr, HOST_PVID,
771 			  ENTRYTYPE_LOCKED);
772 
773 	port->phylink_config.dev = &port->dev->dev;
774 	port->phylink_config.type = PHYLINK_NETDEV;
775 	port->phylink_pcs.poll = true;
776 	port->phylink_pcs.ops = &lan966x_phylink_pcs_ops;
777 
778 	port->phylink_config.mac_capabilities = MAC_ASYM_PAUSE | MAC_SYM_PAUSE |
779 		MAC_10 | MAC_100 | MAC_1000FD | MAC_2500FD;
780 
781 	phy_interface_set_rgmii(port->phylink_config.supported_interfaces);
782 	__set_bit(PHY_INTERFACE_MODE_MII,
783 		  port->phylink_config.supported_interfaces);
784 	__set_bit(PHY_INTERFACE_MODE_GMII,
785 		  port->phylink_config.supported_interfaces);
786 	__set_bit(PHY_INTERFACE_MODE_SGMII,
787 		  port->phylink_config.supported_interfaces);
788 	__set_bit(PHY_INTERFACE_MODE_QSGMII,
789 		  port->phylink_config.supported_interfaces);
790 	__set_bit(PHY_INTERFACE_MODE_QUSGMII,
791 		  port->phylink_config.supported_interfaces);
792 	__set_bit(PHY_INTERFACE_MODE_1000BASEX,
793 		  port->phylink_config.supported_interfaces);
794 	__set_bit(PHY_INTERFACE_MODE_2500BASEX,
795 		  port->phylink_config.supported_interfaces);
796 
797 	phylink = phylink_create(&port->phylink_config,
798 				 portnp,
799 				 phy_mode,
800 				 &lan966x_phylink_mac_ops);
801 	if (IS_ERR(phylink)) {
802 		port->dev = NULL;
803 		return PTR_ERR(phylink);
804 	}
805 
806 	port->phylink = phylink;
807 
808 	err = register_netdev(dev);
809 	if (err) {
810 		dev_err(lan966x->dev, "register_netdev failed\n");
811 		return err;
812 	}
813 
814 	lan966x_vlan_port_set_vlan_aware(port, 0);
815 	lan966x_vlan_port_set_vid(port, HOST_PVID, false, false);
816 	lan966x_vlan_port_apply(port);
817 
818 	return 0;
819 }
820 
821 static void lan966x_init(struct lan966x *lan966x)
822 {
823 	u32 p, i;
824 
825 	/* MAC table initialization */
826 	lan966x_mac_init(lan966x);
827 
828 	lan966x_vlan_init(lan966x);
829 
830 	/* Flush queues */
831 	lan_wr(lan_rd(lan966x, QS_XTR_FLUSH) |
832 	       GENMASK(1, 0),
833 	       lan966x, QS_XTR_FLUSH);
834 
835 	/* Allow to drain */
836 	mdelay(1);
837 
838 	/* All Queues normal */
839 	lan_wr(lan_rd(lan966x, QS_XTR_FLUSH) &
840 	       ~(GENMASK(1, 0)),
841 	       lan966x, QS_XTR_FLUSH);
842 
843 	/* Set MAC age time to default value, the entry is aged after
844 	 * 2 * AGE_PERIOD
845 	 */
846 	lan_wr(ANA_AUTOAGE_AGE_PERIOD_SET(BR_DEFAULT_AGEING_TIME / 2 / HZ),
847 	       lan966x, ANA_AUTOAGE);
848 
849 	/* Disable learning for frames discarded by VLAN ingress filtering */
850 	lan_rmw(ANA_ADVLEARN_VLAN_CHK_SET(1),
851 		ANA_ADVLEARN_VLAN_CHK,
852 		lan966x, ANA_ADVLEARN);
853 
854 	/* Setup frame ageing - "2 sec" - The unit is 6.5 us on lan966x */
855 	lan_wr(SYS_FRM_AGING_AGE_TX_ENA_SET(1) |
856 	       (20000000 / 65),
857 	       lan966x,  SYS_FRM_AGING);
858 
859 	/* Map the 8 CPU extraction queues to CPU port */
860 	lan_wr(0, lan966x, QSYS_CPU_GROUP_MAP);
861 
862 	/* Do byte-swap and expect status after last data word
863 	 * Extraction: Mode: manual extraction) | Byte_swap
864 	 */
865 	lan_wr(QS_XTR_GRP_CFG_MODE_SET(lan966x->fdma ? 2 : 1) |
866 	       QS_XTR_GRP_CFG_BYTE_SWAP_SET(1),
867 	       lan966x, QS_XTR_GRP_CFG(0));
868 
869 	/* Injection: Mode: manual injection | Byte_swap */
870 	lan_wr(QS_INJ_GRP_CFG_MODE_SET(lan966x->fdma ? 2 : 1) |
871 	       QS_INJ_GRP_CFG_BYTE_SWAP_SET(1),
872 	       lan966x, QS_INJ_GRP_CFG(0));
873 
874 	lan_rmw(QS_INJ_CTRL_GAP_SIZE_SET(0),
875 		QS_INJ_CTRL_GAP_SIZE,
876 		lan966x, QS_INJ_CTRL(0));
877 
878 	/* Enable IFH insertion/parsing on CPU ports */
879 	lan_wr(SYS_PORT_MODE_INCL_INJ_HDR_SET(1) |
880 	       SYS_PORT_MODE_INCL_XTR_HDR_SET(1),
881 	       lan966x, SYS_PORT_MODE(CPU_PORT));
882 
883 	/* Setup flooding PGIDs */
884 	lan_wr(ANA_FLOODING_IPMC_FLD_MC4_DATA_SET(PGID_MCIPV4) |
885 	       ANA_FLOODING_IPMC_FLD_MC4_CTRL_SET(PGID_MC) |
886 	       ANA_FLOODING_IPMC_FLD_MC6_DATA_SET(PGID_MCIPV6) |
887 	       ANA_FLOODING_IPMC_FLD_MC6_CTRL_SET(PGID_MC),
888 	       lan966x, ANA_FLOODING_IPMC);
889 
890 	/* There are 8 priorities */
891 	for (i = 0; i < 8; ++i)
892 		lan_rmw(ANA_FLOODING_FLD_MULTICAST_SET(PGID_MC) |
893 			ANA_FLOODING_FLD_UNICAST_SET(PGID_UC) |
894 			ANA_FLOODING_FLD_BROADCAST_SET(PGID_BC),
895 			ANA_FLOODING_FLD_MULTICAST |
896 			ANA_FLOODING_FLD_UNICAST |
897 			ANA_FLOODING_FLD_BROADCAST,
898 			lan966x, ANA_FLOODING(i));
899 
900 	for (i = 0; i < PGID_ENTRIES; ++i)
901 		/* Set all the entries to obey VLAN_VLAN */
902 		lan_rmw(ANA_PGID_CFG_OBEY_VLAN_SET(1),
903 			ANA_PGID_CFG_OBEY_VLAN,
904 			lan966x, ANA_PGID_CFG(i));
905 
906 	for (p = 0; p < lan966x->num_phys_ports; p++) {
907 		/* Disable bridging by default */
908 		lan_rmw(ANA_PGID_PGID_SET(0x0),
909 			ANA_PGID_PGID,
910 			lan966x, ANA_PGID(p + PGID_SRC));
911 
912 		/* Do not forward BPDU frames to the front ports and copy them
913 		 * to CPU
914 		 */
915 		lan_wr(0xffff, lan966x, ANA_CPU_FWD_BPDU_CFG(p));
916 	}
917 
918 	/* Set source buffer size for each priority and each port to 1500 bytes */
919 	for (i = 0; i <= QSYS_Q_RSRV; ++i) {
920 		lan_wr(1500 / 64, lan966x, QSYS_RES_CFG(i));
921 		lan_wr(1500 / 64, lan966x, QSYS_RES_CFG(512 + i));
922 	}
923 
924 	/* Enable switching to/from cpu port */
925 	lan_wr(QSYS_SW_PORT_MODE_PORT_ENA_SET(1) |
926 	       QSYS_SW_PORT_MODE_SCH_NEXT_CFG_SET(1) |
927 	       QSYS_SW_PORT_MODE_INGRESS_DROP_MODE_SET(1),
928 	       lan966x,  QSYS_SW_PORT_MODE(CPU_PORT));
929 
930 	/* Configure and enable the CPU port */
931 	lan_rmw(ANA_PGID_PGID_SET(0),
932 		ANA_PGID_PGID,
933 		lan966x, ANA_PGID(CPU_PORT));
934 	lan_rmw(ANA_PGID_PGID_SET(BIT(CPU_PORT)),
935 		ANA_PGID_PGID,
936 		lan966x, ANA_PGID(PGID_CPU));
937 
938 	/* Multicast to all other ports */
939 	lan_rmw(GENMASK(lan966x->num_phys_ports - 1, 0),
940 		ANA_PGID_PGID,
941 		lan966x, ANA_PGID(PGID_MC));
942 
943 	/* This will be controlled by mrouter ports */
944 	lan_rmw(GENMASK(lan966x->num_phys_ports - 1, 0),
945 		ANA_PGID_PGID,
946 		lan966x, ANA_PGID(PGID_MCIPV4));
947 
948 	lan_rmw(GENMASK(lan966x->num_phys_ports - 1, 0),
949 		ANA_PGID_PGID,
950 		lan966x, ANA_PGID(PGID_MCIPV6));
951 
952 	/* Unicast to all other ports */
953 	lan_rmw(GENMASK(lan966x->num_phys_ports - 1, 0),
954 		ANA_PGID_PGID,
955 		lan966x, ANA_PGID(PGID_UC));
956 
957 	/* Broadcast to the CPU port and to other ports */
958 	lan_rmw(ANA_PGID_PGID_SET(BIT(CPU_PORT) | GENMASK(lan966x->num_phys_ports - 1, 0)),
959 		ANA_PGID_PGID,
960 		lan966x, ANA_PGID(PGID_BC));
961 
962 	lan_wr(REW_PORT_CFG_NO_REWRITE_SET(1),
963 	       lan966x, REW_PORT_CFG(CPU_PORT));
964 
965 	lan_rmw(ANA_ANAINTR_INTR_ENA_SET(1),
966 		ANA_ANAINTR_INTR_ENA,
967 		lan966x, ANA_ANAINTR);
968 
969 	spin_lock_init(&lan966x->tx_lock);
970 
971 	lan966x_taprio_init(lan966x);
972 }
973 
974 static int lan966x_ram_init(struct lan966x *lan966x)
975 {
976 	return lan_rd(lan966x, SYS_RAM_INIT);
977 }
978 
979 static int lan966x_reset_switch(struct lan966x *lan966x)
980 {
981 	struct reset_control *switch_reset;
982 	int val = 0;
983 	int ret;
984 
985 	switch_reset = devm_reset_control_get_optional_shared(lan966x->dev,
986 							      "switch");
987 	if (IS_ERR(switch_reset))
988 		return dev_err_probe(lan966x->dev, PTR_ERR(switch_reset),
989 				     "Could not obtain switch reset");
990 
991 	reset_control_reset(switch_reset);
992 
993 	lan_wr(SYS_RESET_CFG_CORE_ENA_SET(0), lan966x, SYS_RESET_CFG);
994 	lan_wr(SYS_RAM_INIT_RAM_INIT_SET(1), lan966x, SYS_RAM_INIT);
995 	ret = readx_poll_timeout(lan966x_ram_init, lan966x,
996 				 val, (val & BIT(1)) == 0, READL_SLEEP_US,
997 				 READL_TIMEOUT_US);
998 	if (ret)
999 		return ret;
1000 
1001 	lan_wr(SYS_RESET_CFG_CORE_ENA_SET(1), lan966x, SYS_RESET_CFG);
1002 
1003 	return 0;
1004 }
1005 
1006 static int lan966x_probe(struct platform_device *pdev)
1007 {
1008 	struct fwnode_handle *ports, *portnp;
1009 	struct lan966x *lan966x;
1010 	u8 mac_addr[ETH_ALEN];
1011 	int err;
1012 
1013 	lan966x = devm_kzalloc(&pdev->dev, sizeof(*lan966x), GFP_KERNEL);
1014 	if (!lan966x)
1015 		return -ENOMEM;
1016 
1017 	platform_set_drvdata(pdev, lan966x);
1018 	lan966x->dev = &pdev->dev;
1019 
1020 	if (!device_get_mac_address(&pdev->dev, mac_addr)) {
1021 		ether_addr_copy(lan966x->base_mac, mac_addr);
1022 	} else {
1023 		pr_info("MAC addr was not set, use random MAC\n");
1024 		eth_random_addr(lan966x->base_mac);
1025 		lan966x->base_mac[5] &= 0xf0;
1026 	}
1027 
1028 	ports = device_get_named_child_node(&pdev->dev, "ethernet-ports");
1029 	if (!ports)
1030 		return dev_err_probe(&pdev->dev, -ENODEV,
1031 				     "no ethernet-ports child found\n");
1032 
1033 	err = lan966x_create_targets(pdev, lan966x);
1034 	if (err)
1035 		return dev_err_probe(&pdev->dev, err,
1036 				     "Failed to create targets");
1037 
1038 	err = lan966x_reset_switch(lan966x);
1039 	if (err)
1040 		return dev_err_probe(&pdev->dev, err, "Reset failed");
1041 
1042 	lan966x->num_phys_ports = NUM_PHYS_PORTS;
1043 	lan966x->ports = devm_kcalloc(&pdev->dev, lan966x->num_phys_ports,
1044 				      sizeof(struct lan966x_port *),
1045 				      GFP_KERNEL);
1046 	if (!lan966x->ports)
1047 		return -ENOMEM;
1048 
1049 	/* There QS system has 32KB of memory */
1050 	lan966x->shared_queue_sz = LAN966X_BUFFER_MEMORY;
1051 
1052 	/* set irq */
1053 	lan966x->xtr_irq = platform_get_irq_byname(pdev, "xtr");
1054 	if (lan966x->xtr_irq <= 0)
1055 		return -EINVAL;
1056 
1057 	err = devm_request_threaded_irq(&pdev->dev, lan966x->xtr_irq, NULL,
1058 					lan966x_xtr_irq_handler, IRQF_ONESHOT,
1059 					"frame extraction", lan966x);
1060 	if (err) {
1061 		pr_err("Unable to use xtr irq");
1062 		return -ENODEV;
1063 	}
1064 
1065 	lan966x->ana_irq = platform_get_irq_byname(pdev, "ana");
1066 	if (lan966x->ana_irq > 0) {
1067 		err = devm_request_threaded_irq(&pdev->dev, lan966x->ana_irq, NULL,
1068 						lan966x_ana_irq_handler, IRQF_ONESHOT,
1069 						"ana irq", lan966x);
1070 		if (err)
1071 			return dev_err_probe(&pdev->dev, err, "Unable to use ana irq");
1072 	}
1073 
1074 	lan966x->ptp_irq = platform_get_irq_byname(pdev, "ptp");
1075 	if (lan966x->ptp_irq > 0) {
1076 		err = devm_request_threaded_irq(&pdev->dev, lan966x->ptp_irq, NULL,
1077 						lan966x_ptp_irq_handler, IRQF_ONESHOT,
1078 						"ptp irq", lan966x);
1079 		if (err)
1080 			return dev_err_probe(&pdev->dev, err, "Unable to use ptp irq");
1081 
1082 		lan966x->ptp = 1;
1083 	}
1084 
1085 	lan966x->fdma_irq = platform_get_irq_byname(pdev, "fdma");
1086 	if (lan966x->fdma_irq > 0) {
1087 		err = devm_request_irq(&pdev->dev, lan966x->fdma_irq,
1088 				       lan966x_fdma_irq_handler, 0,
1089 				       "fdma irq", lan966x);
1090 		if (err)
1091 			return dev_err_probe(&pdev->dev, err, "Unable to use fdma irq");
1092 
1093 		lan966x->fdma = true;
1094 	}
1095 
1096 	if (lan966x->ptp) {
1097 		lan966x->ptp_ext_irq = platform_get_irq_byname(pdev, "ptp-ext");
1098 		if (lan966x->ptp_ext_irq > 0) {
1099 			err = devm_request_threaded_irq(&pdev->dev,
1100 							lan966x->ptp_ext_irq, NULL,
1101 							lan966x_ptp_ext_irq_handler,
1102 							IRQF_ONESHOT,
1103 							"ptp-ext irq", lan966x);
1104 			if (err)
1105 				return dev_err_probe(&pdev->dev, err,
1106 						     "Unable to use ptp-ext irq");
1107 		}
1108 	}
1109 
1110 	/* init switch */
1111 	lan966x_init(lan966x);
1112 	lan966x_stats_init(lan966x);
1113 
1114 	/* go over the child nodes */
1115 	fwnode_for_each_available_child_node(ports, portnp) {
1116 		phy_interface_t phy_mode;
1117 		struct phy *serdes;
1118 		u32 p;
1119 
1120 		if (fwnode_property_read_u32(portnp, "reg", &p))
1121 			continue;
1122 
1123 		phy_mode = fwnode_get_phy_mode(portnp);
1124 		err = lan966x_probe_port(lan966x, p, phy_mode, portnp);
1125 		if (err)
1126 			goto cleanup_ports;
1127 
1128 		/* Read needed configuration */
1129 		lan966x->ports[p]->config.portmode = phy_mode;
1130 		lan966x->ports[p]->fwnode = fwnode_handle_get(portnp);
1131 
1132 		serdes = devm_of_phy_get(lan966x->dev, to_of_node(portnp), NULL);
1133 		if (PTR_ERR(serdes) == -ENODEV)
1134 			serdes = NULL;
1135 		if (IS_ERR(serdes)) {
1136 			err = PTR_ERR(serdes);
1137 			goto cleanup_ports;
1138 		}
1139 		lan966x->ports[p]->serdes = serdes;
1140 
1141 		lan966x_port_init(lan966x->ports[p]);
1142 		err = lan966x_xdp_port_init(lan966x->ports[p]);
1143 		if (err)
1144 			goto cleanup_ports;
1145 	}
1146 
1147 	lan966x_mdb_init(lan966x);
1148 	err = lan966x_fdb_init(lan966x);
1149 	if (err)
1150 		goto cleanup_ports;
1151 
1152 	err = lan966x_ptp_init(lan966x);
1153 	if (err)
1154 		goto cleanup_fdb;
1155 
1156 	err = lan966x_fdma_init(lan966x);
1157 	if (err)
1158 		goto cleanup_ptp;
1159 
1160 	return 0;
1161 
1162 cleanup_ptp:
1163 	lan966x_ptp_deinit(lan966x);
1164 
1165 cleanup_fdb:
1166 	lan966x_fdb_deinit(lan966x);
1167 
1168 cleanup_ports:
1169 	fwnode_handle_put(portnp);
1170 
1171 	lan966x_cleanup_ports(lan966x);
1172 
1173 	cancel_delayed_work_sync(&lan966x->stats_work);
1174 	destroy_workqueue(lan966x->stats_queue);
1175 	mutex_destroy(&lan966x->stats_lock);
1176 
1177 	return err;
1178 }
1179 
1180 static int lan966x_remove(struct platform_device *pdev)
1181 {
1182 	struct lan966x *lan966x = platform_get_drvdata(pdev);
1183 
1184 	lan966x_taprio_deinit(lan966x);
1185 	lan966x_fdma_deinit(lan966x);
1186 	lan966x_cleanup_ports(lan966x);
1187 
1188 	cancel_delayed_work_sync(&lan966x->stats_work);
1189 	destroy_workqueue(lan966x->stats_queue);
1190 	mutex_destroy(&lan966x->stats_lock);
1191 
1192 	lan966x_mac_purge_entries(lan966x);
1193 	lan966x_mdb_deinit(lan966x);
1194 	lan966x_fdb_deinit(lan966x);
1195 	lan966x_ptp_deinit(lan966x);
1196 
1197 	return 0;
1198 }
1199 
1200 static struct platform_driver lan966x_driver = {
1201 	.probe = lan966x_probe,
1202 	.remove = lan966x_remove,
1203 	.driver = {
1204 		.name = "lan966x-switch",
1205 		.of_match_table = lan966x_match,
1206 	},
1207 };
1208 
1209 static int __init lan966x_switch_driver_init(void)
1210 {
1211 	int ret;
1212 
1213 	lan966x_register_notifier_blocks();
1214 
1215 	ret = platform_driver_register(&lan966x_driver);
1216 	if (ret)
1217 		goto err;
1218 
1219 	return 0;
1220 
1221 err:
1222 	lan966x_unregister_notifier_blocks();
1223 	return ret;
1224 }
1225 
1226 static void __exit lan966x_switch_driver_exit(void)
1227 {
1228 	platform_driver_unregister(&lan966x_driver);
1229 	lan966x_unregister_notifier_blocks();
1230 }
1231 
1232 module_init(lan966x_switch_driver_init);
1233 module_exit(lan966x_switch_driver_exit);
1234 
1235 MODULE_DESCRIPTION("Microchip LAN966X switch driver");
1236 MODULE_AUTHOR("Horatiu Vultur <horatiu.vultur@microchip.com>");
1237 MODULE_LICENSE("Dual MIT/GPL");
1238