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
lan966x_create_targets(struct platform_device * pdev,struct lan966x * lan966x)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
lan966x_port_unique_address(struct net_device * dev)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
lan966x_port_set_mac_address(struct net_device * dev,void * p)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
lan966x_port_get_phys_port_name(struct net_device * dev,char * buf,size_t len)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
lan966x_port_open(struct net_device * dev)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
lan966x_port_stop(struct net_device * dev)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
lan966x_port_inj_status(struct lan966x * lan966x)208 static int lan966x_port_inj_status(struct lan966x *lan966x)
209 {
210 return lan_rd(lan966x, QS_INJ_STATUS);
211 }
212
lan966x_port_inj_ready(struct lan966x * lan966x,u8 grp)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
lan966x_port_ifh_xmit(struct sk_buff * skb,__be32 * ifh,struct net_device * dev)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
lan966x_ifh_set_bypass(void * ifh,u64 bypass)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
lan966x_ifh_set_port(void * ifh,u64 bypass)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
lan966x_ifh_set_qos_class(void * ifh,u64 bypass)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
lan966x_ifh_set_ipv(void * ifh,u64 bypass)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
lan966x_ifh_set_vid(void * ifh,u64 vid)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
lan966x_ifh_set_rew_op(void * ifh,u64 rew_op)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
lan966x_ifh_set_timestamp(void * ifh,u64 timestamp)344 static void lan966x_ifh_set_timestamp(void *ifh, u64 timestamp)
345 {
346 packing(ifh, ×tamp, IFH_POS_TIMESTAMP + IFH_WID_TIMESTAMP - 1,
347 IFH_POS_TIMESTAMP, IFH_LEN * 4, PACK, 0);
348 }
349
lan966x_port_xmit(struct sk_buff * skb,struct net_device * dev)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
lan966x_port_change_mtu(struct net_device * dev,int new_mtu)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
lan966x_mc_unsync(struct net_device * dev,const unsigned char * addr)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
lan966x_mc_sync(struct net_device * dev,const unsigned char * addr)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
lan966x_port_set_rx_mode(struct net_device * dev)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
lan966x_port_get_parent_id(struct net_device * dev,struct netdev_phys_item_id * ppid)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
lan966x_port_ioctl(struct net_device * dev,struct ifreq * ifr,int cmd)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
lan966x_netdevice_check(const struct net_device * dev)493 bool lan966x_netdevice_check(const struct net_device *dev)
494 {
495 return dev->netdev_ops == &lan966x_port_netdev_ops;
496 }
497
lan966x_hw_offload(struct lan966x * lan966x,u32 port,struct sk_buff * skb)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
lan966x_port_xtr_status(struct lan966x * lan966x,u8 grp)530 static int lan966x_port_xtr_status(struct lan966x *lan966x, u8 grp)
531 {
532 return lan_rd(lan966x, QS_XTR_RD(grp));
533 }
534
lan966x_port_xtr_ready(struct lan966x * lan966x,u8 grp)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
lan966x_rx_frame_word(struct lan966x * lan966x,u8 grp,u32 * rval)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
lan966x_ifh_get_src_port(void * ifh,u64 * src_port)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
lan966x_ifh_get_len(void * ifh,u64 * len)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
lan966x_ifh_get_timestamp(void * ifh,u64 * timestamp)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
lan966x_xtr_irq_handler(int irq,void * args)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, ×tamp);
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
lan966x_ana_irq_handler(int irq,void * args)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
lan966x_cleanup_ports(struct lan966x * lan966x)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
lan966x_probe_port(struct lan966x * lan966x,u32 p,phy_interface_t phy_mode,struct fwnode_handle * portnp)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
lan966x_init(struct lan966x * lan966x)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
lan966x_ram_init(struct lan966x * lan966x)997 static int lan966x_ram_init(struct lan966x *lan966x)
998 {
999 return lan_rd(lan966x, SYS_RAM_INIT);
1000 }
1001
lan966x_reset_switch(struct lan966x * lan966x)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
lan966x_probe(struct platform_device * pdev)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
lan966x_remove(struct platform_device * pdev)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
lan966x_switch_driver_init(void)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
lan966x_switch_driver_exit(void)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