drivers/net/ethernet/microchip/lan966x/lan966x_main.c

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