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