drivers/net/ethernet/mscc/ocelot.c

   1 // SPDX-License-Identifier: (GPL-2.0 OR MIT)
   2 /*
   3  * Microsemi Ocelot Switch driver
   4  *
   5  * Copyright (c) 2017 Microsemi Corporation
   6  */
   7 #include <linux/etherdevice.h>
   8 #include <linux/ethtool.h>
   9 #include <linux/if_bridge.h>
  10 #include <linux/if_ether.h>
  11 #include <linux/if_vlan.h>
  12 #include <linux/interrupt.h>
  13 #include <linux/kernel.h>
  14 #include <linux/module.h>
  15 #include <linux/netdevice.h>
  16 #include <linux/phy.h>
  17 #include <linux/skbuff.h>
  18 #include <net/arp.h>
  19 #include <net/netevent.h>
  20 #include <net/rtnetlink.h>
  21 #include <net/switchdev.h>
  22
  23 #include "ocelot.h"
  24
  25 /* MAC table entry types.
  26  * ENTRYTYPE_NORMAL is subject to aging.
  27  * ENTRYTYPE_LOCKED is not subject to aging.
  28  * ENTRYTYPE_MACv4 is not subject to aging. For IPv4 multicast.
  29  * ENTRYTYPE_MACv6 is not subject to aging. For IPv6 multicast.
  30  */
  31 enum macaccess_entry_type {
  32         ENTRYTYPE_NORMAL = 0,
  33         ENTRYTYPE_LOCKED,
  34         ENTRYTYPE_MACv4,
  35         ENTRYTYPE_MACv6,
  36 };
  37
  38 struct ocelot_mact_entry {
  39         u8 mac[ETH_ALEN];
  40         u16 vid;
  41         enum macaccess_entry_type type;
  42 };
  43
  44 static inline int ocelot_mact_wait_for_completion(struct ocelot *ocelot)
  45 {
  46         unsigned int val, timeout = 10;
  47
  48         /* Wait for the issued mac table command to be completed, or timeout.
  49          * When the command read from  ANA_TABLES_MACACCESS is
  50          * MACACCESS_CMD_IDLE, the issued command completed successfully.
  51          */
  52         do {
  53                 val = ocelot_read(ocelot, ANA_TABLES_MACACCESS);
  54                 val &= ANA_TABLES_MACACCESS_MAC_TABLE_CMD_M;
  55         } while (val != MACACCESS_CMD_IDLE && timeout--);
  56
  57         if (!timeout)
  58                 return -ETIMEDOUT;
  59
  60         return 0;
  61 }
  62
  63 static void ocelot_mact_select(struct ocelot *ocelot,
  64                                const unsigned char mac[ETH_ALEN],
  65                                unsigned int vid)
  66 {
  67         u32 macl = 0, mach = 0;
  68
  69         /* Set the MAC address to handle and the vlan associated in a format
  70          * understood by the hardware.
  71          */
  72         mach |= vid    << 16;
  73         mach |= mac[0] << 8;
  74         mach |= mac[1] << 0;
  75         macl |= mac[2] << 24;
  76         macl |= mac[3] << 16;
  77         macl |= mac[4] << 8;
  78         macl |= mac[5] << 0;
  79
  80         ocelot_write(ocelot, macl, ANA_TABLES_MACLDATA);
  81         ocelot_write(ocelot, mach, ANA_TABLES_MACHDATA);
  82
  83 }
  84
  85 static int ocelot_mact_learn(struct ocelot *ocelot, int port,
  86                              const unsigned char mac[ETH_ALEN],
  87                              unsigned int vid,
  88                              enum macaccess_entry_type type)
  89 {
  90         ocelot_mact_select(ocelot, mac, vid);
  91
  92         /* Issue a write command */
  93         ocelot_write(ocelot, ANA_TABLES_MACACCESS_VALID |
  94                              ANA_TABLES_MACACCESS_DEST_IDX(port) |
  95                              ANA_TABLES_MACACCESS_ENTRYTYPE(type) |
  96                              ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_LEARN),
  97                              ANA_TABLES_MACACCESS);
  98
  99         return ocelot_mact_wait_for_completion(ocelot);
 100 }
 101
 102 static int ocelot_mact_forget(struct ocelot *ocelot,
 103                               const unsigned char mac[ETH_ALEN],
 104                               unsigned int vid)
 105 {
 106         ocelot_mact_select(ocelot, mac, vid);
 107
 108         /* Issue a forget command */
 109         ocelot_write(ocelot,
 110                      ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_FORGET),
 111                      ANA_TABLES_MACACCESS);
 112
 113         return ocelot_mact_wait_for_completion(ocelot);
 114 }
 115
 116 static void ocelot_mact_init(struct ocelot *ocelot)
 117 {
 118         /* Configure the learning mode entries attributes:
 119          * - Do not copy the frame to the CPU extraction queues.
 120          * - Use the vlan and mac_cpoy for dmac lookup.
 121          */
 122         ocelot_rmw(ocelot, 0,
 123                    ANA_AGENCTRL_LEARN_CPU_COPY | ANA_AGENCTRL_IGNORE_DMAC_FLAGS
 124                    | ANA_AGENCTRL_LEARN_FWD_KILL
 125                    | ANA_AGENCTRL_LEARN_IGNORE_VLAN,
 126                    ANA_AGENCTRL);
 127
 128         /* Clear the MAC table */
 129         ocelot_write(ocelot, MACACCESS_CMD_INIT, ANA_TABLES_MACACCESS);
 130 }
 131
 132 static inline int ocelot_vlant_wait_for_completion(struct ocelot *ocelot)
 133 {
 134         unsigned int val, timeout = 10;
 135
 136         /* Wait for the issued mac table command to be completed, or timeout.
 137          * When the command read from ANA_TABLES_MACACCESS is
 138          * MACACCESS_CMD_IDLE, the issued command completed successfully.
 139          */
 140         do {
 141                 val = ocelot_read(ocelot, ANA_TABLES_VLANACCESS);
 142                 val &= ANA_TABLES_VLANACCESS_VLAN_TBL_CMD_M;
 143         } while (val != ANA_TABLES_VLANACCESS_CMD_IDLE && timeout--);
 144
 145         if (!timeout)
 146                 return -ETIMEDOUT;
 147
 148         return 0;
 149 }
 150
 151 static void ocelot_vlan_init(struct ocelot *ocelot)
 152 {
 153         /* Clear VLAN table, by default all ports are members of all VLANs */
 154         ocelot_write(ocelot, ANA_TABLES_VLANACCESS_CMD_INIT,
 155                      ANA_TABLES_VLANACCESS);
 156         ocelot_vlant_wait_for_completion(ocelot);
 157 }
 158
 159 /* Watermark encode
 160  * Bit 8:   Unit; 0:1, 1:16
 161  * Bit 7-0: Value to be multiplied with unit
 162  */
 163 static u16 ocelot_wm_enc(u16 value)
 164 {
 165         if (value >= BIT(8))
 166                 return BIT(8) | (value / 16);
 167
 168         return value;
 169 }
 170
 171 static void ocelot_port_adjust_link(struct net_device *dev)
 172 {
 173         struct ocelot_port *port = netdev_priv(dev);
 174         struct ocelot *ocelot = port->ocelot;
 175         u8 p = port->chip_port;
 176         int speed, atop_wm, mode = 0;
 177
 178         switch (dev->phydev->speed) {
 179         case SPEED_10:
 180                 speed = OCELOT_SPEED_10;
 181                 break;
 182         case SPEED_100:
 183                 speed = OCELOT_SPEED_100;
 184                 break;
 185         case SPEED_1000:
 186                 speed = OCELOT_SPEED_1000;
 187                 mode = DEV_MAC_MODE_CFG_GIGA_MODE_ENA;
 188                 break;
 189         case SPEED_2500:
 190                 speed = OCELOT_SPEED_2500;
 191                 mode = DEV_MAC_MODE_CFG_GIGA_MODE_ENA;
 192                 break;
 193         default:
 194                 netdev_err(dev, "Unsupported PHY speed: %d\n",
 195                            dev->phydev->speed);
 196                 return;
 197         }
 198
 199         phy_print_status(dev->phydev);
 200
 201         if (!dev->phydev->link)
 202                 return;
 203
 204         /* Only full duplex supported for now */
 205         ocelot_port_writel(port, DEV_MAC_MODE_CFG_FDX_ENA |
 206                            mode, DEV_MAC_MODE_CFG);
 207
 208         /* Set MAC IFG Gaps
 209          * FDX: TX_IFG = 5, RX_IFG1 = RX_IFG2 = 0
 210          * !FDX: TX_IFG = 5, RX_IFG1 = RX_IFG2 = 5
 211          */
 212         ocelot_port_writel(port, DEV_MAC_IFG_CFG_TX_IFG(5), DEV_MAC_IFG_CFG);
 213
 214         /* Load seed (0) and set MAC HDX late collision  */
 215         ocelot_port_writel(port, DEV_MAC_HDX_CFG_LATE_COL_POS(67) |
 216                            DEV_MAC_HDX_CFG_SEED_LOAD,
 217                            DEV_MAC_HDX_CFG);
 218         mdelay(1);
 219         ocelot_port_writel(port, DEV_MAC_HDX_CFG_LATE_COL_POS(67),
 220                            DEV_MAC_HDX_CFG);
 221
 222         /* Disable HDX fast control */
 223         ocelot_port_writel(port, DEV_PORT_MISC_HDX_FAST_DIS, DEV_PORT_MISC);
 224
 225         /* SGMII only for now */
 226         ocelot_port_writel(port, PCS1G_MODE_CFG_SGMII_MODE_ENA, PCS1G_MODE_CFG);
 227         ocelot_port_writel(port, PCS1G_SD_CFG_SD_SEL, PCS1G_SD_CFG);
 228
 229         /* Enable PCS */
 230         ocelot_port_writel(port, PCS1G_CFG_PCS_ENA, PCS1G_CFG);
 231
 232         /* No aneg on SGMII */
 233         ocelot_port_writel(port, 0, PCS1G_ANEG_CFG);
 234
 235         /* No loopback */
 236         ocelot_port_writel(port, 0, PCS1G_LB_CFG);
 237
 238         /* Set Max Length and maximum tags allowed */
 239         ocelot_port_writel(port, VLAN_ETH_FRAME_LEN, DEV_MAC_MAXLEN_CFG);
 240         ocelot_port_writel(port, DEV_MAC_TAGS_CFG_TAG_ID(ETH_P_8021AD) |
 241                            DEV_MAC_TAGS_CFG_VLAN_AWR_ENA |
 242                            DEV_MAC_TAGS_CFG_VLAN_LEN_AWR_ENA,
 243                            DEV_MAC_TAGS_CFG);
 244
 245         /* Enable MAC module */
 246         ocelot_port_writel(port, DEV_MAC_ENA_CFG_RX_ENA |
 247                            DEV_MAC_ENA_CFG_TX_ENA, DEV_MAC_ENA_CFG);
 248
 249         /* Take MAC, Port, Phy (intern) and PCS (SGMII/Serdes) clock out of
 250          * reset */
 251         ocelot_port_writel(port, DEV_CLOCK_CFG_LINK_SPEED(speed),
 252                            DEV_CLOCK_CFG);
 253
 254         /* Set SMAC of Pause frame (00:00:00:00:00:00) */
 255         ocelot_port_writel(port, 0, DEV_MAC_FC_MAC_HIGH_CFG);
 256         ocelot_port_writel(port, 0, DEV_MAC_FC_MAC_LOW_CFG);
 257
 258         /* No PFC */
 259         ocelot_write_gix(ocelot, ANA_PFC_PFC_CFG_FC_LINK_SPEED(speed),
 260                          ANA_PFC_PFC_CFG, p);
 261
 262         /* Set Pause WM hysteresis
 263          * 152 = 6 * VLAN_ETH_FRAME_LEN / OCELOT_BUFFER_CELL_SZ
 264          * 101 = 4 * VLAN_ETH_FRAME_LEN / OCELOT_BUFFER_CELL_SZ
 265          */
 266         ocelot_write_rix(ocelot, SYS_PAUSE_CFG_PAUSE_ENA |
 267                          SYS_PAUSE_CFG_PAUSE_STOP(101) |
 268                          SYS_PAUSE_CFG_PAUSE_START(152), SYS_PAUSE_CFG, p);
 269
 270         /* Core: Enable port for frame transfer */
 271         ocelot_write_rix(ocelot, QSYS_SWITCH_PORT_MODE_INGRESS_DROP_MODE |
 272                          QSYS_SWITCH_PORT_MODE_SCH_NEXT_CFG(1) |
 273                          QSYS_SWITCH_PORT_MODE_PORT_ENA,
 274                          QSYS_SWITCH_PORT_MODE, p);
 275
 276         /* Flow control */
 277         ocelot_write_rix(ocelot, SYS_MAC_FC_CFG_PAUSE_VAL_CFG(0xffff) |
 278                          SYS_MAC_FC_CFG_RX_FC_ENA | SYS_MAC_FC_CFG_TX_FC_ENA |
 279                          SYS_MAC_FC_CFG_ZERO_PAUSE_ENA |
 280                          SYS_MAC_FC_CFG_FC_LATENCY_CFG(0x7) |
 281                          SYS_MAC_FC_CFG_FC_LINK_SPEED(speed),
 282                          SYS_MAC_FC_CFG, p);
 283         ocelot_write_rix(ocelot, 0, ANA_POL_FLOWC, p);
 284
 285         /* Tail dropping watermark */
 286         atop_wm = (ocelot->shared_queue_sz - 9 * VLAN_ETH_FRAME_LEN) / OCELOT_BUFFER_CELL_SZ;
 287         ocelot_write_rix(ocelot, ocelot_wm_enc(9 * VLAN_ETH_FRAME_LEN),
 288                          SYS_ATOP, p);
 289         ocelot_write(ocelot, ocelot_wm_enc(atop_wm), SYS_ATOP_TOT_CFG);
 290 }
 291
 292 static int ocelot_port_open(struct net_device *dev)
 293 {
 294         struct ocelot_port *port = netdev_priv(dev);
 295         struct ocelot *ocelot = port->ocelot;
 296         int err;
 297
 298         /* Enable receiving frames on the port, and activate auto-learning of
 299          * MAC addresses.
 300          */
 301         ocelot_write_gix(ocelot, ANA_PORT_PORT_CFG_LEARNAUTO |
 302                          ANA_PORT_PORT_CFG_RECV_ENA |
 303                          ANA_PORT_PORT_CFG_PORTID_VAL(port->chip_port),
 304                          ANA_PORT_PORT_CFG, port->chip_port);
 305
 306         err = phy_connect_direct(dev, port->phy, &ocelot_port_adjust_link,
 307                                  PHY_INTERFACE_MODE_NA);
 308         if (err) {
 309                 netdev_err(dev, "Could not attach to PHY\n");
 310                 return err;
 311         }
 312
 313         dev->phydev = port->phy;
 314
 315         phy_attached_info(port->phy);
 316         phy_start(port->phy);
 317         return 0;
 318 }
 319
 320 static int ocelot_port_stop(struct net_device *dev)
 321 {
 322         struct ocelot_port *port = netdev_priv(dev);
 323
 324         phy_disconnect(port->phy);
 325
 326         dev->phydev = NULL;
 327
 328         ocelot_port_writel(port, 0, DEV_MAC_ENA_CFG);
 329         ocelot_rmw_rix(port->ocelot, 0, QSYS_SWITCH_PORT_MODE_PORT_ENA,
 330                          QSYS_SWITCH_PORT_MODE, port->chip_port);
 331         return 0;
 332 }
 333
 334 /* Generate the IFH for frame injection
 335  *
 336  * The IFH is a 128bit-value
 337  * bit 127: bypass the analyzer processing
 338  * bit 56-67: destination mask
 339  * bit 28-29: pop_cnt: 3 disables all rewriting of the frame
 340  * bit 20-27: cpu extraction queue mask
 341  * bit 16: tag type 0: C-tag, 1: S-tag
 342  * bit 0-11: VID
 343  */
 344 static int ocelot_gen_ifh(u32 *ifh, struct frame_info *info)
 345 {
 346         ifh[0] = IFH_INJ_BYPASS;
 347         ifh[1] = (0xf00 & info->port) >> 8;
 348         ifh[2] = (0xff & info->port) << 24;
 349         ifh[3] = (info->tag_type << 16) | info->vid;
 350
 351         return 0;
 352 }
 353
 354 static int ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
 355 {
 356         struct ocelot_port *port = netdev_priv(dev);
 357         struct ocelot *ocelot = port->ocelot;
 358         u32 val, ifh[IFH_LEN];
 359         struct frame_info info = {};
 360         u8 grp = 0; /* Send everything on CPU group 0 */
 361         unsigned int i, count, last;
 362
 363         val = ocelot_read(ocelot, QS_INJ_STATUS);
 364         if (!(val & QS_INJ_STATUS_FIFO_RDY(BIT(grp))) ||
 365             (val & QS_INJ_STATUS_WMARK_REACHED(BIT(grp))))
 366                 return NETDEV_TX_BUSY;
 367
 368         ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
 369                          QS_INJ_CTRL_SOF, QS_INJ_CTRL, grp);
 370
 371         info.port = BIT(port->chip_port);
 372         info.tag_type = IFH_TAG_TYPE_C;
 373         info.vid = skb_vlan_tag_get(skb);
 374         ocelot_gen_ifh(ifh, &info);
 375
 376         for (i = 0; i < IFH_LEN; i++)
 377                 ocelot_write_rix(ocelot, (__force u32)cpu_to_be32(ifh[i]),
 378                                  QS_INJ_WR, grp);
 379
 380         count = (skb->len + 3) / 4;
 381         last = skb->len % 4;
 382         for (i = 0; i < count; i++) {
 383                 ocelot_write_rix(ocelot, ((u32 *)skb->data)[i], QS_INJ_WR, grp);
 384         }
 385
 386         /* Add padding */
 387         while (i < (OCELOT_BUFFER_CELL_SZ / 4)) {
 388                 ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
 389                 i++;
 390         }
 391
 392         /* Indicate EOF and valid bytes in last word */
 393         ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
 394                          QS_INJ_CTRL_VLD_BYTES(skb->len < OCELOT_BUFFER_CELL_SZ ? 0 : last) |
 395                          QS_INJ_CTRL_EOF,
 396                          QS_INJ_CTRL, grp);
 397
 398         /* Add dummy CRC */
 399         ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
 400         skb_tx_timestamp(skb);
 401
 402         dev->stats.tx_packets++;
 403         dev->stats.tx_bytes += skb->len;
 404         dev_kfree_skb_any(skb);
 405
 406         return NETDEV_TX_OK;
 407 }
 408
 409 static void ocelot_mact_mc_reset(struct ocelot_port *port)
 410 {
 411         struct ocelot *ocelot = port->ocelot;
 412         struct netdev_hw_addr *ha, *n;
 413
 414         /* Free and forget all the MAC addresses stored in the port private mc
 415          * list. These are mc addresses that were previously added by calling
 416          * ocelot_mact_mc_add().
 417          */
 418         list_for_each_entry_safe(ha, n, &port->mc, list) {
 419                 ocelot_mact_forget(ocelot, ha->addr, port->pvid);
 420                 list_del(&ha->list);
 421                 kfree(ha);
 422         }
 423 }
 424
 425 static int ocelot_mact_mc_add(struct ocelot_port *port,
 426                               struct netdev_hw_addr *hw_addr)
 427 {
 428         struct ocelot *ocelot = port->ocelot;
 429         struct netdev_hw_addr *ha = kzalloc(sizeof(*ha), GFP_KERNEL);
 430
 431         if (!ha)
 432                 return -ENOMEM;
 433
 434         memcpy(ha, hw_addr, sizeof(*ha));
 435         list_add_tail(&ha->list, &port->mc);
 436
 437         ocelot_mact_learn(ocelot, PGID_CPU, ha->addr, port->pvid,
 438                           ENTRYTYPE_LOCKED);
 439
 440         return 0;
 441 }
 442
 443 static void ocelot_set_rx_mode(struct net_device *dev)
 444 {
 445         struct ocelot_port *port = netdev_priv(dev);
 446         struct ocelot *ocelot = port->ocelot;
 447         struct netdev_hw_addr *ha;
 448         int i;
 449         u32 val;
 450
 451         /* This doesn't handle promiscuous mode because the bridge core is
 452          * setting IFF_PROMISC on all slave interfaces and all frames would be
 453          * forwarded to the CPU port.
 454          */
 455         val = GENMASK(ocelot->num_phys_ports - 1, 0);
 456         for (i = ocelot->num_phys_ports + 1; i < PGID_CPU; i++)
 457                 ocelot_write_rix(ocelot, val, ANA_PGID_PGID, i);
 458
 459         /* Handle the device multicast addresses. First remove all the
 460          * previously installed addresses and then add the latest ones to the
 461          * mac table.
 462          */
 463         ocelot_mact_mc_reset(port);
 464         netdev_for_each_mc_addr(ha, dev)
 465                 ocelot_mact_mc_add(port, ha);
 466 }
 467
 468 static int ocelot_port_get_phys_port_name(struct net_device *dev,
 469                                           char *buf, size_t len)
 470 {
 471         struct ocelot_port *port = netdev_priv(dev);
 472         int ret;
 473
 474         ret = snprintf(buf, len, "p%d", port->chip_port);
 475         if (ret >= len)
 476                 return -EINVAL;
 477
 478         return 0;
 479 }
 480
 481 static int ocelot_port_set_mac_address(struct net_device *dev, void *p)
 482 {
 483         struct ocelot_port *port = netdev_priv(dev);
 484         struct ocelot *ocelot = port->ocelot;
 485         const struct sockaddr *addr = p;
 486
 487         /* Learn the new net device MAC address in the mac table. */
 488         ocelot_mact_learn(ocelot, PGID_CPU, addr->sa_data, port->pvid,
 489                           ENTRYTYPE_LOCKED);
 490         /* Then forget the previous one. */
 491         ocelot_mact_forget(ocelot, dev->dev_addr, port->pvid);
 492
 493         ether_addr_copy(dev->dev_addr, addr->sa_data);
 494         return 0;
 495 }
 496
 497 static void ocelot_get_stats64(struct net_device *dev,
 498                                struct rtnl_link_stats64 *stats)
 499 {
 500         struct ocelot_port *port = netdev_priv(dev);
 501         struct ocelot *ocelot = port->ocelot;
 502
 503         /* Configure the port to read the stats from */
 504         ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(port->chip_port),
 505                      SYS_STAT_CFG);
 506
 507         /* Get Rx stats */
 508         stats->rx_bytes = ocelot_read(ocelot, SYS_COUNT_RX_OCTETS);
 509         stats->rx_packets = ocelot_read(ocelot, SYS_COUNT_RX_SHORTS) +
 510                             ocelot_read(ocelot, SYS_COUNT_RX_FRAGMENTS) +
 511                             ocelot_read(ocelot, SYS_COUNT_RX_JABBERS) +
 512                             ocelot_read(ocelot, SYS_COUNT_RX_LONGS) +
 513                             ocelot_read(ocelot, SYS_COUNT_RX_64) +
 514                             ocelot_read(ocelot, SYS_COUNT_RX_65_127) +
 515                             ocelot_read(ocelot, SYS_COUNT_RX_128_255) +
 516                             ocelot_read(ocelot, SYS_COUNT_RX_256_1023) +
 517                             ocelot_read(ocelot, SYS_COUNT_RX_1024_1526) +
 518                             ocelot_read(ocelot, SYS_COUNT_RX_1527_MAX);
 519         stats->multicast = ocelot_read(ocelot, SYS_COUNT_RX_MULTICAST);
 520         stats->rx_dropped = dev->stats.rx_dropped;
 521
 522         /* Get Tx stats */
 523         stats->tx_bytes = ocelot_read(ocelot, SYS_COUNT_TX_OCTETS);
 524         stats->tx_packets = ocelot_read(ocelot, SYS_COUNT_TX_64) +
 525                             ocelot_read(ocelot, SYS_COUNT_TX_65_127) +
 526                             ocelot_read(ocelot, SYS_COUNT_TX_128_511) +
 527                             ocelot_read(ocelot, SYS_COUNT_TX_512_1023) +
 528                             ocelot_read(ocelot, SYS_COUNT_TX_1024_1526) +
 529                             ocelot_read(ocelot, SYS_COUNT_TX_1527_MAX);
 530         stats->tx_dropped = ocelot_read(ocelot, SYS_COUNT_TX_DROPS) +
 531                             ocelot_read(ocelot, SYS_COUNT_TX_AGING);
 532         stats->collisions = ocelot_read(ocelot, SYS_COUNT_TX_COLLISION);
 533 }
 534
 535 static int ocelot_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
 536                           struct net_device *dev, const unsigned char *addr,
 537                           u16 vid, u16 flags)
 538 {
 539         struct ocelot_port *port = netdev_priv(dev);
 540         struct ocelot *ocelot = port->ocelot;
 541
 542         return ocelot_mact_learn(ocelot, port->chip_port, addr, vid,
 543                                  ENTRYTYPE_NORMAL);
 544 }
 545
 546 static int ocelot_fdb_del(struct ndmsg *ndm, struct nlattr *tb[],
 547                           struct net_device *dev,
 548                           const unsigned char *addr, u16 vid)
 549 {
 550         struct ocelot_port *port = netdev_priv(dev);
 551         struct ocelot *ocelot = port->ocelot;
 552
 553         return ocelot_mact_forget(ocelot, addr, vid);
 554 }
 555
 556 struct ocelot_dump_ctx {
 557         struct net_device *dev;
 558         struct sk_buff *skb;
 559         struct netlink_callback *cb;
 560         int idx;
 561 };
 562
 563 static int ocelot_fdb_do_dump(struct ocelot_mact_entry *entry,
 564                               struct ocelot_dump_ctx *dump)
 565 {
 566         u32 portid = NETLINK_CB(dump->cb->skb).portid;
 567         u32 seq = dump->cb->nlh->nlmsg_seq;
 568         struct nlmsghdr *nlh;
 569         struct ndmsg *ndm;
 570
 571         if (dump->idx < dump->cb->args[2])
 572                 goto skip;
 573
 574         nlh = nlmsg_put(dump->skb, portid, seq, RTM_NEWNEIGH,
 575                         sizeof(*ndm), NLM_F_MULTI);
 576         if (!nlh)
 577                 return -EMSGSIZE;
 578
 579         ndm = nlmsg_data(nlh);
 580         ndm->ndm_family  = AF_BRIDGE;
 581         ndm->ndm_pad1    = 0;
 582         ndm->ndm_pad2    = 0;
 583         ndm->ndm_flags   = NTF_SELF;
 584         ndm->ndm_type    = 0;
 585         ndm->ndm_ifindex = dump->dev->ifindex;
 586         ndm->ndm_state   = NUD_REACHABLE;
 587
 588         if (nla_put(dump->skb, NDA_LLADDR, ETH_ALEN, entry->mac))
 589                 goto nla_put_failure;
 590
 591         if (entry->vid && nla_put_u16(dump->skb, NDA_VLAN, entry->vid))
 592                 goto nla_put_failure;
 593
 594         nlmsg_end(dump->skb, nlh);
 595
 596 skip:
 597         dump->idx++;
 598         return 0;
 599
 600 nla_put_failure:
 601         nlmsg_cancel(dump->skb, nlh);
 602         return -EMSGSIZE;
 603 }
 604
 605 static inline int ocelot_mact_read(struct ocelot_port *port, int row, int col,
 606                                    struct ocelot_mact_entry *entry)
 607 {
 608         struct ocelot *ocelot = port->ocelot;
 609         char mac[ETH_ALEN];
 610         u32 val, dst, macl, mach;
 611
 612         /* Set row and column to read from */
 613         ocelot_field_write(ocelot, ANA_TABLES_MACTINDX_M_INDEX, row);
 614         ocelot_field_write(ocelot, ANA_TABLES_MACTINDX_BUCKET, col);
 615
 616         /* Issue a read command */
 617         ocelot_write(ocelot,
 618                      ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_READ),
 619                      ANA_TABLES_MACACCESS);
 620
 621         if (ocelot_mact_wait_for_completion(ocelot))
 622                 return -ETIMEDOUT;
 623
 624         /* Read the entry flags */
 625         val = ocelot_read(ocelot, ANA_TABLES_MACACCESS);
 626         if (!(val & ANA_TABLES_MACACCESS_VALID))
 627                 return -EINVAL;
 628
 629         /* If the entry read has another port configured as its destination,
 630          * do not report it.
 631          */
 632         dst = (val & ANA_TABLES_MACACCESS_DEST_IDX_M) >> 3;
 633         if (dst != port->chip_port)
 634                 return -EINVAL;
 635
 636         /* Get the entry's MAC address and VLAN id */
 637         macl = ocelot_read(ocelot, ANA_TABLES_MACLDATA);
 638         mach = ocelot_read(ocelot, ANA_TABLES_MACHDATA);
 639
 640         mac[0] = (mach >> 8)  & 0xff;
 641         mac[1] = (mach >> 0)  & 0xff;
 642         mac[2] = (macl >> 24) & 0xff;
 643         mac[3] = (macl >> 16) & 0xff;
 644         mac[4] = (macl >> 8)  & 0xff;
 645         mac[5] = (macl >> 0)  & 0xff;
 646
 647         entry->vid = (mach >> 16) & 0xfff;
 648         ether_addr_copy(entry->mac, mac);
 649
 650         return 0;
 651 }
 652
 653 static int ocelot_fdb_dump(struct sk_buff *skb, struct netlink_callback *cb,
 654                            struct net_device *dev,
 655                            struct net_device *filter_dev, int *idx)
 656 {
 657         struct ocelot_port *port = netdev_priv(dev);
 658         int i, j, ret = 0;
 659         struct ocelot_dump_ctx dump = {
 660                 .dev = dev,
 661                 .skb = skb,
 662                 .cb = cb,
 663                 .idx = *idx,
 664         };
 665
 666         struct ocelot_mact_entry entry;
 667
 668         /* Loop through all the mac tables entries. There are 1024 rows of 4
 669          * entries.
 670          */
 671         for (i = 0; i < 1024; i++) {
 672                 for (j = 0; j < 4; j++) {
 673                         ret = ocelot_mact_read(port, i, j, &entry);
 674                         /* If the entry is invalid (wrong port, invalid...),
 675                          * skip it.
 676                          */
 677                         if (ret == -EINVAL)
 678                                 continue;
 679                         else if (ret)
 680                                 goto end;
 681
 682                         ret = ocelot_fdb_do_dump(&entry, &dump);
 683                         if (ret)
 684                                 goto end;
 685                 }
 686         }
 687
 688 end:
 689         *idx = dump.idx;
 690         return ret;
 691 }
 692
 693 static const struct net_device_ops ocelot_port_netdev_ops = {
 694         .ndo_open                       = ocelot_port_open,
 695         .ndo_stop                       = ocelot_port_stop,
 696         .ndo_start_xmit                 = ocelot_port_xmit,
 697         .ndo_set_rx_mode                = ocelot_set_rx_mode,
 698         .ndo_get_phys_port_name         = ocelot_port_get_phys_port_name,
 699         .ndo_set_mac_address            = ocelot_port_set_mac_address,
 700         .ndo_get_stats64                = ocelot_get_stats64,
 701         .ndo_fdb_add                    = ocelot_fdb_add,
 702         .ndo_fdb_del                    = ocelot_fdb_del,
 703         .ndo_fdb_dump                   = ocelot_fdb_dump,
 704 };
 705
 706 static void ocelot_get_strings(struct net_device *netdev, u32 sset, u8 *data)
 707 {
 708         struct ocelot_port *port = netdev_priv(netdev);
 709         struct ocelot *ocelot = port->ocelot;
 710         int i;
 711
 712         if (sset != ETH_SS_STATS)
 713                 return;
 714
 715         for (i = 0; i < ocelot->num_stats; i++)
 716                 memcpy(data + i * ETH_GSTRING_LEN, ocelot->stats_layout[i].name,
 717                        ETH_GSTRING_LEN);
 718 }
 719
 720 static void ocelot_check_stats(struct work_struct *work)
 721 {
 722         struct delayed_work *del_work = to_delayed_work(work);
 723         struct ocelot *ocelot = container_of(del_work, struct ocelot, stats_work);
 724         int i, j;
 725
 726         mutex_lock(&ocelot->stats_lock);
 727
 728         for (i = 0; i < ocelot->num_phys_ports; i++) {
 729                 /* Configure the port to read the stats from */
 730                 ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(i), SYS_STAT_CFG);
 731
 732                 for (j = 0; j < ocelot->num_stats; j++) {
 733                         u32 val;
 734                         unsigned int idx = i * ocelot->num_stats + j;
 735
 736                         val = ocelot_read_rix(ocelot, SYS_COUNT_RX_OCTETS,
 737                                               ocelot->stats_layout[j].offset);
 738
 739                         if (val < (ocelot->stats[idx] & U32_MAX))
 740                                 ocelot->stats[idx] += (u64)1 << 32;
 741
 742                         ocelot->stats[idx] = (ocelot->stats[idx] &
 743                                               ~(u64)U32_MAX) + val;
 744                 }
 745         }
 746
 747         cancel_delayed_work(&ocelot->stats_work);
 748         queue_delayed_work(ocelot->stats_queue, &ocelot->stats_work,
 749                            OCELOT_STATS_CHECK_DELAY);
 750
 751         mutex_unlock(&ocelot->stats_lock);
 752 }
 753
 754 static void ocelot_get_ethtool_stats(struct net_device *dev,
 755                                      struct ethtool_stats *stats, u64 *data)
 756 {
 757         struct ocelot_port *port = netdev_priv(dev);
 758         struct ocelot *ocelot = port->ocelot;
 759         int i;
 760
 761         /* check and update now */
 762         ocelot_check_stats(&ocelot->stats_work.work);
 763
 764         /* Copy all counters */
 765         for (i = 0; i < ocelot->num_stats; i++)
 766                 *data++ = ocelot->stats[port->chip_port * ocelot->num_stats + i];
 767 }
 768
 769 static int ocelot_get_sset_count(struct net_device *dev, int sset)
 770 {
 771         struct ocelot_port *port = netdev_priv(dev);
 772         struct ocelot *ocelot = port->ocelot;
 773
 774         if (sset != ETH_SS_STATS)
 775                 return -EOPNOTSUPP;
 776         return ocelot->num_stats;
 777 }
 778
 779 static const struct ethtool_ops ocelot_ethtool_ops = {
 780         .get_strings            = ocelot_get_strings,
 781         .get_ethtool_stats      = ocelot_get_ethtool_stats,
 782         .get_sset_count         = ocelot_get_sset_count,
 783         .get_link_ksettings     = phy_ethtool_get_link_ksettings,
 784         .set_link_ksettings     = phy_ethtool_set_link_ksettings,
 785 };
 786
 787 static int ocelot_port_attr_get(struct net_device *dev,
 788                                 struct switchdev_attr *attr)
 789 {
 790         struct ocelot_port *ocelot_port = netdev_priv(dev);
 791         struct ocelot *ocelot = ocelot_port->ocelot;
 792
 793         switch (attr->id) {
 794         case SWITCHDEV_ATTR_ID_PORT_PARENT_ID:
 795                 attr->u.ppid.id_len = sizeof(ocelot->base_mac);
 796                 memcpy(&attr->u.ppid.id, &ocelot->base_mac,
 797                        attr->u.ppid.id_len);
 798                 break;
 799         default:
 800                 return -EOPNOTSUPP;
 801         }
 802
 803         return 0;
 804 }
 805
 806 static int ocelot_port_attr_stp_state_set(struct ocelot_port *ocelot_port,
 807                                           struct switchdev_trans *trans,
 808                                           u8 state)
 809 {
 810         struct ocelot *ocelot = ocelot_port->ocelot;
 811         u32 port_cfg;
 812         int port, i;
 813
 814         if (switchdev_trans_ph_prepare(trans))
 815                 return 0;
 816
 817         if (!(BIT(ocelot_port->chip_port) & ocelot->bridge_mask))
 818                 return 0;
 819
 820         port_cfg = ocelot_read_gix(ocelot, ANA_PORT_PORT_CFG,
 821                                    ocelot_port->chip_port);
 822
 823         switch (state) {
 824         case BR_STATE_FORWARDING:
 825                 ocelot->bridge_fwd_mask |= BIT(ocelot_port->chip_port);
 826                 /* Fallthrough */
 827         case BR_STATE_LEARNING:
 828                 port_cfg |= ANA_PORT_PORT_CFG_LEARN_ENA;
 829                 break;
 830
 831         default:
 832                 port_cfg &= ~ANA_PORT_PORT_CFG_LEARN_ENA;
 833                 ocelot->bridge_fwd_mask &= ~BIT(ocelot_port->chip_port);
 834                 break;
 835         }
 836
 837         ocelot_write_gix(ocelot, port_cfg, ANA_PORT_PORT_CFG,
 838                          ocelot_port->chip_port);
 839
 840         /* Apply FWD mask. The loop is needed to add/remove the current port as
 841          * a source for the other ports.
 842          */
 843         for (port = 0; port < ocelot->num_phys_ports; port++) {
 844                 if (ocelot->bridge_fwd_mask & BIT(port)) {
 845                         unsigned long mask = ocelot->bridge_fwd_mask & ~BIT(port);
 846
 847                         for (i = 0; i < ocelot->num_phys_ports; i++) {
 848                                 unsigned long bond_mask = ocelot->lags[i];
 849
 850                                 if (!bond_mask)
 851                                         continue;
 852
 853                                 if (bond_mask & BIT(port)) {
 854                                         mask &= ~bond_mask;
 855                                         break;
 856                                 }
 857                         }
 858
 859                         ocelot_write_rix(ocelot,
 860                                          BIT(ocelot->num_phys_ports) | mask,
 861                                          ANA_PGID_PGID, PGID_SRC + port);
 862                 } else {
 863                         /* Only the CPU port, this is compatible with link
 864                          * aggregation.
 865                          */
 866                         ocelot_write_rix(ocelot,
 867                                          BIT(ocelot->num_phys_ports),
 868                                          ANA_PGID_PGID, PGID_SRC + port);
 869                 }
 870         }
 871
 872         return 0;
 873 }
 874
 875 static void ocelot_port_attr_ageing_set(struct ocelot_port *ocelot_port,
 876                                         unsigned long ageing_clock_t)
 877 {
 878         struct ocelot *ocelot = ocelot_port->ocelot;
 879         unsigned long ageing_jiffies = clock_t_to_jiffies(ageing_clock_t);
 880         u32 ageing_time = jiffies_to_msecs(ageing_jiffies) / 1000;
 881
 882         ocelot_write(ocelot, ANA_AUTOAGE_AGE_PERIOD(ageing_time / 2),
 883                      ANA_AUTOAGE);
 884 }
 885
 886 static void ocelot_port_attr_mc_set(struct ocelot_port *port, bool mc)
 887 {
 888         struct ocelot *ocelot = port->ocelot;
 889         u32 val = ocelot_read_gix(ocelot, ANA_PORT_CPU_FWD_CFG,
 890                                   port->chip_port);
 891
 892         if (mc)
 893                 val |= ANA_PORT_CPU_FWD_CFG_CPU_IGMP_REDIR_ENA |
 894                        ANA_PORT_CPU_FWD_CFG_CPU_MLD_REDIR_ENA |
 895                        ANA_PORT_CPU_FWD_CFG_CPU_IPMC_CTRL_COPY_ENA;
 896         else
 897                 val &= ~(ANA_PORT_CPU_FWD_CFG_CPU_IGMP_REDIR_ENA |
 898                          ANA_PORT_CPU_FWD_CFG_CPU_MLD_REDIR_ENA |
 899                          ANA_PORT_CPU_FWD_CFG_CPU_IPMC_CTRL_COPY_ENA);
 900
 901         ocelot_write_gix(ocelot, val, ANA_PORT_CPU_FWD_CFG, port->chip_port);
 902 }
 903
 904 static int ocelot_port_attr_set(struct net_device *dev,
 905                                 const struct switchdev_attr *attr,
 906                                 struct switchdev_trans *trans)
 907 {
 908         struct ocelot_port *ocelot_port = netdev_priv(dev);
 909         int err = 0;
 910
 911         switch (attr->id) {
 912         case SWITCHDEV_ATTR_ID_PORT_STP_STATE:
 913                 ocelot_port_attr_stp_state_set(ocelot_port, trans,
 914                                                attr->u.stp_state);
 915                 break;
 916         case SWITCHDEV_ATTR_ID_BRIDGE_AGEING_TIME:
 917                 ocelot_port_attr_ageing_set(ocelot_port, attr->u.ageing_time);
 918                 break;
 919         case SWITCHDEV_ATTR_ID_BRIDGE_MC_DISABLED:
 920                 ocelot_port_attr_mc_set(ocelot_port, !attr->u.mc_disabled);
 921                 break;
 922         default:
 923                 err = -EOPNOTSUPP;
 924                 break;
 925         }
 926
 927         return err;
 928 }
 929
 930 static struct ocelot_multicast *ocelot_multicast_get(struct ocelot *ocelot,
 931                                                      const unsigned char *addr,
 932                                                      u16 vid)
 933 {
 934         struct ocelot_multicast *mc;
 935
 936         list_for_each_entry(mc, &ocelot->multicast, list) {
 937                 if (ether_addr_equal(mc->addr, addr) && mc->vid == vid)
 938                         return mc;
 939         }
 940
 941         return NULL;
 942 }
 943
 944 static int ocelot_port_obj_add_mdb(struct net_device *dev,
 945                                    const struct switchdev_obj_port_mdb *mdb,
 946                                    struct switchdev_trans *trans)
 947 {
 948         struct ocelot_port *port = netdev_priv(dev);
 949         struct ocelot *ocelot = port->ocelot;
 950         struct ocelot_multicast *mc;
 951         unsigned char addr[ETH_ALEN];
 952         u16 vid = mdb->vid;
 953         bool new = false;
 954
 955         if (!vid)
 956                 vid = 1;
 957
 958         mc = ocelot_multicast_get(ocelot, mdb->addr, vid);
 959         if (!mc) {
 960                 mc = devm_kzalloc(ocelot->dev, sizeof(*mc), GFP_KERNEL);
 961                 if (!mc)
 962                         return -ENOMEM;
 963
 964                 memcpy(mc->addr, mdb->addr, ETH_ALEN);
 965                 mc->vid = vid;
 966
 967                 list_add_tail(&mc->list, &ocelot->multicast);
 968                 new = true;
 969         }
 970
 971         memcpy(addr, mc->addr, ETH_ALEN);
 972         addr[0] = 0;
 973
 974         if (!new) {
 975                 addr[2] = mc->ports << 0;
 976                 addr[1] = mc->ports << 8;
 977                 ocelot_mact_forget(ocelot, addr, vid);
 978         }
 979
 980         mc->ports |= BIT(port->chip_port);
 981         addr[2] = mc->ports << 0;
 982         addr[1] = mc->ports << 8;
 983
 984         return ocelot_mact_learn(ocelot, 0, addr, vid, ENTRYTYPE_MACv4);
 985 }
 986
 987 static int ocelot_port_obj_del_mdb(struct net_device *dev,
 988                                    const struct switchdev_obj_port_mdb *mdb)
 989 {
 990         struct ocelot_port *port = netdev_priv(dev);
 991         struct ocelot *ocelot = port->ocelot;
 992         struct ocelot_multicast *mc;
 993         unsigned char addr[ETH_ALEN];
 994         u16 vid = mdb->vid;
 995
 996         if (!vid)
 997                 vid = 1;
 998
 999         mc = ocelot_multicast_get(ocelot, mdb->addr, vid);
1000         if (!mc)
1001                 return -ENOENT;
1002
1003         memcpy(addr, mc->addr, ETH_ALEN);
1004         addr[2] = mc->ports << 0;
1005         addr[1] = mc->ports << 8;
1006         addr[0] = 0;
1007         ocelot_mact_forget(ocelot, addr, vid);
1008
1009         mc->ports &= ~BIT(port->chip_port);
1010         if (!mc->ports) {
1011                 list_del(&mc->list);
1012                 devm_kfree(ocelot->dev, mc);
1013                 return 0;
1014         }
1015
1016         addr[2] = mc->ports << 0;
1017         addr[1] = mc->ports << 8;
1018
1019         return ocelot_mact_learn(ocelot, 0, addr, vid, ENTRYTYPE_MACv4);
1020 }
1021
1022 static int ocelot_port_obj_add(struct net_device *dev,
1023                                const struct switchdev_obj *obj,
1024                                struct switchdev_trans *trans)
1025 {
1026         int ret = 0;
1027
1028         switch (obj->id) {
1029         case SWITCHDEV_OBJ_ID_PORT_MDB:
1030                 ret = ocelot_port_obj_add_mdb(dev, SWITCHDEV_OBJ_PORT_MDB(obj),
1031                                               trans);
1032                 break;
1033         default:
1034                 return -EOPNOTSUPP;
1035         }
1036
1037         return ret;
1038 }
1039
1040 static int ocelot_port_obj_del(struct net_device *dev,
1041                                const struct switchdev_obj *obj)
1042 {
1043         int ret = 0;
1044
1045         switch (obj->id) {
1046         case SWITCHDEV_OBJ_ID_PORT_MDB:
1047                 ret = ocelot_port_obj_del_mdb(dev, SWITCHDEV_OBJ_PORT_MDB(obj));
1048                 break;
1049         default:
1050                 return -EOPNOTSUPP;
1051         }
1052
1053         return ret;
1054 }
1055
1056 static const struct switchdev_ops ocelot_port_switchdev_ops = {
1057         .switchdev_port_attr_get        = ocelot_port_attr_get,
1058         .switchdev_port_attr_set        = ocelot_port_attr_set,
1059         .switchdev_port_obj_add         = ocelot_port_obj_add,
1060         .switchdev_port_obj_del         = ocelot_port_obj_del,
1061 };
1062
1063 static int ocelot_port_bridge_join(struct ocelot_port *ocelot_port,
1064                                    struct net_device *bridge)
1065 {
1066         struct ocelot *ocelot = ocelot_port->ocelot;
1067
1068         if (!ocelot->bridge_mask) {
1069                 ocelot->hw_bridge_dev = bridge;
1070         } else {
1071                 if (ocelot->hw_bridge_dev != bridge)
1072                         /* This is adding the port to a second bridge, this is
1073                          * unsupported */
1074                         return -ENODEV;
1075         }
1076
1077         ocelot->bridge_mask |= BIT(ocelot_port->chip_port);
1078
1079         return 0;
1080 }
1081
1082 static void ocelot_port_bridge_leave(struct ocelot_port *ocelot_port,
1083                                      struct net_device *bridge)
1084 {
1085         struct ocelot *ocelot = ocelot_port->ocelot;
1086
1087         ocelot->bridge_mask &= ~BIT(ocelot_port->chip_port);
1088
1089         if (!ocelot->bridge_mask)
1090                 ocelot->hw_bridge_dev = NULL;
1091 }
1092
1093 static void ocelot_set_aggr_pgids(struct ocelot *ocelot)
1094 {
1095         int i, port, lag;
1096
1097         /* Reset destination and aggregation PGIDS */
1098         for (port = 0; port < ocelot->num_phys_ports; port++)
1099                 ocelot_write_rix(ocelot, BIT(port), ANA_PGID_PGID, port);
1100
1101         for (i = PGID_AGGR; i < PGID_SRC; i++)
1102                 ocelot_write_rix(ocelot, GENMASK(ocelot->num_phys_ports - 1, 0),
1103                                  ANA_PGID_PGID, i);
1104
1105         /* Now, set PGIDs for each LAG */
1106         for (lag = 0; lag < ocelot->num_phys_ports; lag++) {
1107                 unsigned long bond_mask;
1108                 int aggr_count = 0;
1109                 u8 aggr_idx[16];
1110
1111                 bond_mask = ocelot->lags[lag];
1112                 if (!bond_mask)
1113                         continue;
1114
1115                 for_each_set_bit(port, &bond_mask, ocelot->num_phys_ports) {
1116                         // Destination mask
1117                         ocelot_write_rix(ocelot, bond_mask,
1118                                          ANA_PGID_PGID, port);
1119                         aggr_idx[aggr_count] = port;
1120                         aggr_count++;
1121                 }
1122
1123                 for (i = PGID_AGGR; i < PGID_SRC; i++) {
1124                         u32 ac;
1125
1126                         ac = ocelot_read_rix(ocelot, ANA_PGID_PGID, i);
1127                         ac &= ~bond_mask;
1128                         ac |= BIT(aggr_idx[i % aggr_count]);
1129                         ocelot_write_rix(ocelot, ac, ANA_PGID_PGID, i);
1130                 }
1131         }
1132 }
1133
1134 static void ocelot_setup_lag(struct ocelot *ocelot, int lag)
1135 {
1136         unsigned long bond_mask = ocelot->lags[lag];
1137         unsigned int p;
1138
1139         for_each_set_bit(p, &bond_mask, ocelot->num_phys_ports) {
1140                 u32 port_cfg = ocelot_read_gix(ocelot, ANA_PORT_PORT_CFG, p);
1141
1142                 port_cfg &= ~ANA_PORT_PORT_CFG_PORTID_VAL_M;
1143
1144                 /* Use lag port as logical port for port i */
1145                 ocelot_write_gix(ocelot, port_cfg |
1146                                  ANA_PORT_PORT_CFG_PORTID_VAL(lag),
1147                                  ANA_PORT_PORT_CFG, p);
1148         }
1149 }
1150
1151 static int ocelot_port_lag_join(struct ocelot_port *ocelot_port,
1152                                 struct net_device *bond)
1153 {
1154         struct ocelot *ocelot = ocelot_port->ocelot;
1155         int p = ocelot_port->chip_port;
1156         int lag, lp;
1157         struct net_device *ndev;
1158         u32 bond_mask = 0;
1159
1160         rcu_read_lock();
1161         for_each_netdev_in_bond_rcu(bond, ndev) {
1162                 struct ocelot_port *port = netdev_priv(ndev);
1163
1164                 bond_mask |= BIT(port->chip_port);
1165         }
1166         rcu_read_unlock();
1167
1168         lp = __ffs(bond_mask);
1169
1170         /* If the new port is the lowest one, use it as the logical port from
1171          * now on
1172          */
1173         if (p == lp) {
1174                 lag = p;
1175                 ocelot->lags[p] = bond_mask;
1176                 bond_mask &= ~BIT(p);
1177                 if (bond_mask) {
1178                         lp = __ffs(bond_mask);
1179                         ocelot->lags[lp] = 0;
1180                 }
1181         } else {
1182                 lag = lp;
1183                 ocelot->lags[lp] |= BIT(p);
1184         }
1185
1186         ocelot_setup_lag(ocelot, lag);
1187         ocelot_set_aggr_pgids(ocelot);
1188
1189         return 0;
1190 }
1191
1192 static void ocelot_port_lag_leave(struct ocelot_port *ocelot_port,
1193                                   struct net_device *bond)
1194 {
1195         struct ocelot *ocelot = ocelot_port->ocelot;
1196         int p = ocelot_port->chip_port;
1197         u32 port_cfg;
1198         int i;
1199
1200         /* Remove port from any lag */
1201         for (i = 0; i < ocelot->num_phys_ports; i++)
1202                 ocelot->lags[i] &= ~BIT(ocelot_port->chip_port);
1203
1204         /* if it was the logical port of the lag, move the lag config to the
1205          * next port
1206          */
1207         if (ocelot->lags[p]) {
1208                 int n = __ffs(ocelot->lags[p]);
1209
1210                 ocelot->lags[n] = ocelot->lags[p];
1211                 ocelot->lags[p] = 0;
1212
1213                 ocelot_setup_lag(ocelot, n);
1214         }
1215
1216         port_cfg = ocelot_read_gix(ocelot, ANA_PORT_PORT_CFG, p);
1217         port_cfg &= ~ANA_PORT_PORT_CFG_PORTID_VAL_M;
1218         ocelot_write_gix(ocelot, port_cfg | ANA_PORT_PORT_CFG_PORTID_VAL(p),
1219                          ANA_PORT_PORT_CFG, p);
1220
1221         ocelot_set_aggr_pgids(ocelot);
1222 }
1223
1224 /* Checks if the net_device instance given to us originate from our driver. */
1225 static bool ocelot_netdevice_dev_check(const struct net_device *dev)
1226 {
1227         return dev->netdev_ops == &ocelot_port_netdev_ops;
1228 }
1229
1230 static int ocelot_netdevice_port_event(struct net_device *dev,
1231                                        unsigned long event,
1232                                        struct netdev_notifier_changeupper_info *info)
1233 {
1234         struct ocelot_port *ocelot_port = netdev_priv(dev);
1235         int err = 0;
1236
1237         if (!ocelot_netdevice_dev_check(dev))
1238                 return 0;
1239
1240         switch (event) {
1241         case NETDEV_CHANGEUPPER:
1242                 if (netif_is_bridge_master(info->upper_dev)) {
1243                         if (info->linking)
1244                                 err = ocelot_port_bridge_join(ocelot_port,
1245                                                               info->upper_dev);
1246                         else
1247                                 ocelot_port_bridge_leave(ocelot_port,
1248                                                          info->upper_dev);
1249                 }
1250                 if (netif_is_lag_master(info->upper_dev)) {
1251                         if (info->linking)
1252                                 err = ocelot_port_lag_join(ocelot_port,
1253                                                            info->upper_dev);
1254                         else
1255                                 ocelot_port_lag_leave(ocelot_port,
1256                                                       info->upper_dev);
1257                 }
1258                 break;
1259         default:
1260                 break;
1261         }
1262
1263         return err;
1264 }
1265
1266 static int ocelot_netdevice_event(struct notifier_block *unused,
1267                                   unsigned long event, void *ptr)
1268 {
1269         struct netdev_notifier_changeupper_info *info = ptr;
1270         struct net_device *dev = netdev_notifier_info_to_dev(ptr);
1271         int ret = 0;
1272
1273         if (event == NETDEV_PRECHANGEUPPER &&
1274             netif_is_lag_master(info->upper_dev)) {
1275                 struct netdev_lag_upper_info *lag_upper_info = info->upper_info;
1276                 struct netlink_ext_ack *extack;
1277
1278                 if (lag_upper_info->tx_type != NETDEV_LAG_TX_TYPE_HASH) {
1279                         extack = netdev_notifier_info_to_extack(&info->info);
1280                         NL_SET_ERR_MSG_MOD(extack, "LAG device using unsupported Tx type");
1281
1282                         ret = -EINVAL;
1283                         goto notify;
1284                 }
1285         }
1286
1287         if (netif_is_lag_master(dev)) {
1288                 struct net_device *slave;
1289                 struct list_head *iter;
1290
1291                 netdev_for_each_lower_dev(dev, slave, iter) {
1292                         ret = ocelot_netdevice_port_event(slave, event, info);
1293                         if (ret)
1294                                 goto notify;
1295                 }
1296         } else {
1297                 ret = ocelot_netdevice_port_event(dev, event, info);
1298         }
1299
1300 notify:
1301         return notifier_from_errno(ret);
1302 }
1303
1304 struct notifier_block ocelot_netdevice_nb __read_mostly = {
1305         .notifier_call = ocelot_netdevice_event,
1306 };
1307 EXPORT_SYMBOL(ocelot_netdevice_nb);
1308
1309 int ocelot_probe_port(struct ocelot *ocelot, u8 port,
1310                       void __iomem *regs,
1311                       struct phy_device *phy)
1312 {
1313         struct ocelot_port *ocelot_port;
1314         struct net_device *dev;
1315         int err;
1316
1317         dev = alloc_etherdev(sizeof(struct ocelot_port));
1318         if (!dev)
1319                 return -ENOMEM;
1320         SET_NETDEV_DEV(dev, ocelot->dev);
1321         ocelot_port = netdev_priv(dev);
1322         ocelot_port->dev = dev;
1323         ocelot_port->ocelot = ocelot;
1324         ocelot_port->regs = regs;
1325         ocelot_port->chip_port = port;
1326         ocelot_port->phy = phy;
1327         INIT_LIST_HEAD(&ocelot_port->mc);
1328         ocelot->ports[port] = ocelot_port;
1329
1330         dev->netdev_ops = &ocelot_port_netdev_ops;
1331         dev->ethtool_ops = &ocelot_ethtool_ops;
1332         dev->switchdev_ops = &ocelot_port_switchdev_ops;
1333
1334         memcpy(dev->dev_addr, ocelot->base_mac, ETH_ALEN);
1335         dev->dev_addr[ETH_ALEN - 1] += port;
1336         ocelot_mact_learn(ocelot, PGID_CPU, dev->dev_addr, ocelot_port->pvid,
1337                           ENTRYTYPE_LOCKED);
1338
1339         err = register_netdev(dev);
1340         if (err) {
1341                 dev_err(ocelot->dev, "register_netdev failed\n");
1342                 goto err_register_netdev;
1343         }
1344
1345         return 0;
1346
1347 err_register_netdev:
1348         free_netdev(dev);
1349         return err;
1350 }
1351 EXPORT_SYMBOL(ocelot_probe_port);
1352
1353 int ocelot_init(struct ocelot *ocelot)
1354 {
1355         u32 port;
1356         int i, cpu = ocelot->num_phys_ports;
1357         char queue_name[32];
1358
1359         ocelot->lags = devm_kcalloc(ocelot->dev, ocelot->num_phys_ports,
1360                                     sizeof(u32), GFP_KERNEL);
1361         if (!ocelot->lags)
1362                 return -ENOMEM;
1363
1364         ocelot->stats = devm_kcalloc(ocelot->dev,
1365                                      ocelot->num_phys_ports * ocelot->num_stats,
1366                                      sizeof(u64), GFP_KERNEL);
1367         if (!ocelot->stats)
1368                 return -ENOMEM;
1369
1370         mutex_init(&ocelot->stats_lock);
1371         snprintf(queue_name, sizeof(queue_name), "%s-stats",
1372                  dev_name(ocelot->dev));
1373         ocelot->stats_queue = create_singlethread_workqueue(queue_name);
1374         if (!ocelot->stats_queue)
1375                 return -ENOMEM;
1376
1377         ocelot_mact_init(ocelot);
1378         ocelot_vlan_init(ocelot);
1379
1380         for (port = 0; port < ocelot->num_phys_ports; port++) {
1381                 /* Clear all counters (5 groups) */
1382                 ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(port) |
1383                                      SYS_STAT_CFG_STAT_CLEAR_SHOT(0x7f),
1384                              SYS_STAT_CFG);
1385         }
1386
1387         /* Only use S-Tag */
1388         ocelot_write(ocelot, ETH_P_8021AD, SYS_VLAN_ETYPE_CFG);
1389
1390         /* Aggregation mode */
1391         ocelot_write(ocelot, ANA_AGGR_CFG_AC_SMAC_ENA |
1392                              ANA_AGGR_CFG_AC_DMAC_ENA |
1393                              ANA_AGGR_CFG_AC_IP4_SIPDIP_ENA |
1394                              ANA_AGGR_CFG_AC_IP4_TCPUDP_ENA, ANA_AGGR_CFG);
1395
1396         /* Set MAC age time to default value. The entry is aged after
1397          * 2*AGE_PERIOD
1398          */
1399         ocelot_write(ocelot,
1400                      ANA_AUTOAGE_AGE_PERIOD(BR_DEFAULT_AGEING_TIME / 2 / HZ),
1401                      ANA_AUTOAGE);
1402
1403         /* Disable learning for frames discarded by VLAN ingress filtering */
1404         regmap_field_write(ocelot->regfields[ANA_ADVLEARN_VLAN_CHK], 1);
1405
1406         /* Setup frame ageing - fixed value "2 sec" - in 6.5 us units */
1407         ocelot_write(ocelot, SYS_FRM_AGING_AGE_TX_ENA |
1408                      SYS_FRM_AGING_MAX_AGE(307692), SYS_FRM_AGING);
1409
1410         /* Setup flooding PGIDs */
1411         ocelot_write_rix(ocelot, ANA_FLOODING_FLD_MULTICAST(PGID_MC) |
1412                          ANA_FLOODING_FLD_BROADCAST(PGID_MC) |
1413                          ANA_FLOODING_FLD_UNICAST(PGID_UC),
1414                          ANA_FLOODING, 0);
1415         ocelot_write(ocelot, ANA_FLOODING_IPMC_FLD_MC6_DATA(PGID_MCIPV6) |
1416                      ANA_FLOODING_IPMC_FLD_MC6_CTRL(PGID_MC) |
1417                      ANA_FLOODING_IPMC_FLD_MC4_DATA(PGID_MCIPV4) |
1418                      ANA_FLOODING_IPMC_FLD_MC4_CTRL(PGID_MC),
1419                      ANA_FLOODING_IPMC);
1420
1421         for (port = 0; port < ocelot->num_phys_ports; port++) {
1422                 /* Transmit the frame to the local port. */
1423                 ocelot_write_rix(ocelot, BIT(port), ANA_PGID_PGID, port);
1424                 /* Do not forward BPDU frames to the front ports. */
1425                 ocelot_write_gix(ocelot,
1426                                  ANA_PORT_CPU_FWD_BPDU_CFG_BPDU_REDIR_ENA(0xffff),
1427                                  ANA_PORT_CPU_FWD_BPDU_CFG,
1428                                  port);
1429                 /* Ensure bridging is disabled */
1430                 ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_SRC + port);
1431         }
1432
1433         /* Configure and enable the CPU port. */
1434         ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, cpu);
1435         ocelot_write_rix(ocelot, BIT(cpu), ANA_PGID_PGID, PGID_CPU);
1436         ocelot_write_gix(ocelot, ANA_PORT_PORT_CFG_RECV_ENA |
1437                          ANA_PORT_PORT_CFG_PORTID_VAL(cpu),
1438                          ANA_PORT_PORT_CFG, cpu);
1439
1440         /* Allow broadcast MAC frames. */
1441         for (i = ocelot->num_phys_ports + 1; i < PGID_CPU; i++) {
1442                 u32 val = ANA_PGID_PGID_PGID(GENMASK(ocelot->num_phys_ports - 1, 0));
1443
1444                 ocelot_write_rix(ocelot, val, ANA_PGID_PGID, i);
1445         }
1446         ocelot_write_rix(ocelot,
1447                          ANA_PGID_PGID_PGID(GENMASK(ocelot->num_phys_ports, 0)),
1448                          ANA_PGID_PGID, PGID_MC);
1449         ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_MCIPV4);
1450         ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_MCIPV6);
1451
1452         /* CPU port Injection/Extraction configuration */
1453         ocelot_write_rix(ocelot, QSYS_SWITCH_PORT_MODE_INGRESS_DROP_MODE |
1454                          QSYS_SWITCH_PORT_MODE_SCH_NEXT_CFG(1) |
1455                          QSYS_SWITCH_PORT_MODE_PORT_ENA,
1456                          QSYS_SWITCH_PORT_MODE, cpu);
1457         ocelot_write_rix(ocelot, SYS_PORT_MODE_INCL_XTR_HDR(1) |
1458                          SYS_PORT_MODE_INCL_INJ_HDR(1), SYS_PORT_MODE, cpu);
1459         /* Allow manual injection via DEVCPU_QS registers, and byte swap these
1460          * registers endianness.
1461          */
1462         ocelot_write_rix(ocelot, QS_INJ_GRP_CFG_BYTE_SWAP |
1463                          QS_INJ_GRP_CFG_MODE(1), QS_INJ_GRP_CFG, 0);
1464         ocelot_write_rix(ocelot, QS_XTR_GRP_CFG_BYTE_SWAP |
1465                          QS_XTR_GRP_CFG_MODE(1), QS_XTR_GRP_CFG, 0);
1466         ocelot_write(ocelot, ANA_CPUQ_CFG_CPUQ_MIRROR(2) |
1467                      ANA_CPUQ_CFG_CPUQ_LRN(2) |
1468                      ANA_CPUQ_CFG_CPUQ_MAC_COPY(2) |
1469                      ANA_CPUQ_CFG_CPUQ_SRC_COPY(2) |
1470                      ANA_CPUQ_CFG_CPUQ_LOCKED_PORTMOVE(2) |
1471                      ANA_CPUQ_CFG_CPUQ_ALLBRIDGE(6) |
1472                      ANA_CPUQ_CFG_CPUQ_IPMC_CTRL(6) |
1473                      ANA_CPUQ_CFG_CPUQ_IGMP(6) |
1474                      ANA_CPUQ_CFG_CPUQ_MLD(6), ANA_CPUQ_CFG);
1475         for (i = 0; i < 16; i++)
1476                 ocelot_write_rix(ocelot, ANA_CPUQ_8021_CFG_CPUQ_GARP_VAL(6) |
1477                                  ANA_CPUQ_8021_CFG_CPUQ_BPDU_VAL(6),
1478                                  ANA_CPUQ_8021_CFG, i);
1479
1480         INIT_DELAYED_WORK(&ocelot->stats_work, ocelot_check_stats);
1481         queue_delayed_work(ocelot->stats_queue, &ocelot->stats_work,
1482                            OCELOT_STATS_CHECK_DELAY);
1483         return 0;
1484 }
1485 EXPORT_SYMBOL(ocelot_init);
1486
1487 void ocelot_deinit(struct ocelot *ocelot)
1488 {
1489         destroy_workqueue(ocelot->stats_queue);
1490         mutex_destroy(&ocelot->stats_lock);
1491 }
1492 EXPORT_SYMBOL(ocelot_deinit);
1493
1494 MODULE_LICENSE("Dual MIT/GPL");