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 <linux/iopoll.h>
  19 #include <net/arp.h>
  20 #include <net/netevent.h>
  21 #include <net/rtnetlink.h>
  22 #include <net/switchdev.h>
  23
  24 #include "ocelot.h"
  25 #include "ocelot_ace.h"
  26
  27 #define TABLE_UPDATE_SLEEP_US 10
  28 #define TABLE_UPDATE_TIMEOUT_US 100000
  29
  30 /* MAC table entry types.
  31  * ENTRYTYPE_NORMAL is subject to aging.
  32  * ENTRYTYPE_LOCKED is not subject to aging.
  33  * ENTRYTYPE_MACv4 is not subject to aging. For IPv4 multicast.
  34  * ENTRYTYPE_MACv6 is not subject to aging. For IPv6 multicast.
  35  */
  36 enum macaccess_entry_type {
  37         ENTRYTYPE_NORMAL = 0,
  38         ENTRYTYPE_LOCKED,
  39         ENTRYTYPE_MACv4,
  40         ENTRYTYPE_MACv6,
  41 };
  42
  43 struct ocelot_mact_entry {
  44         u8 mac[ETH_ALEN];
  45         u16 vid;
  46         enum macaccess_entry_type type;
  47 };
  48
  49 static inline u32 ocelot_mact_read_macaccess(struct ocelot *ocelot)
  50 {
  51         return ocelot_read(ocelot, ANA_TABLES_MACACCESS);
  52 }
  53
  54 static inline int ocelot_mact_wait_for_completion(struct ocelot *ocelot)
  55 {
  56         u32 val;
  57
  58         return readx_poll_timeout(ocelot_mact_read_macaccess,
  59                 ocelot, val,
  60                 (val & ANA_TABLES_MACACCESS_MAC_TABLE_CMD_M) ==
  61                 MACACCESS_CMD_IDLE,
  62                 TABLE_UPDATE_SLEEP_US, TABLE_UPDATE_TIMEOUT_US);
  63 }
  64
  65 static void ocelot_mact_select(struct ocelot *ocelot,
  66                                const unsigned char mac[ETH_ALEN],
  67                                unsigned int vid)
  68 {
  69         u32 macl = 0, mach = 0;
  70
  71         /* Set the MAC address to handle and the vlan associated in a format
  72          * understood by the hardware.
  73          */
  74         mach |= vid    << 16;
  75         mach |= mac[0] << 8;
  76         mach |= mac[1] << 0;
  77         macl |= mac[2] << 24;
  78         macl |= mac[3] << 16;
  79         macl |= mac[4] << 8;
  80         macl |= mac[5] << 0;
  81
  82         ocelot_write(ocelot, macl, ANA_TABLES_MACLDATA);
  83         ocelot_write(ocelot, mach, ANA_TABLES_MACHDATA);
  84
  85 }
  86
  87 static int ocelot_mact_learn(struct ocelot *ocelot, int port,
  88                              const unsigned char mac[ETH_ALEN],
  89                              unsigned int vid,
  90                              enum macaccess_entry_type type)
  91 {
  92         ocelot_mact_select(ocelot, mac, vid);
  93
  94         /* Issue a write command */
  95         ocelot_write(ocelot, ANA_TABLES_MACACCESS_VALID |
  96                              ANA_TABLES_MACACCESS_DEST_IDX(port) |
  97                              ANA_TABLES_MACACCESS_ENTRYTYPE(type) |
  98                              ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_LEARN),
  99                              ANA_TABLES_MACACCESS);
 100
 101         return ocelot_mact_wait_for_completion(ocelot);
 102 }
 103
 104 static int ocelot_mact_forget(struct ocelot *ocelot,
 105                               const unsigned char mac[ETH_ALEN],
 106                               unsigned int vid)
 107 {
 108         ocelot_mact_select(ocelot, mac, vid);
 109
 110         /* Issue a forget command */
 111         ocelot_write(ocelot,
 112                      ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_FORGET),
 113                      ANA_TABLES_MACACCESS);
 114
 115         return ocelot_mact_wait_for_completion(ocelot);
 116 }
 117
 118 static void ocelot_mact_init(struct ocelot *ocelot)
 119 {
 120         /* Configure the learning mode entries attributes:
 121          * - Do not copy the frame to the CPU extraction queues.
 122          * - Use the vlan and mac_cpoy for dmac lookup.
 123          */
 124         ocelot_rmw(ocelot, 0,
 125                    ANA_AGENCTRL_LEARN_CPU_COPY | ANA_AGENCTRL_IGNORE_DMAC_FLAGS
 126                    | ANA_AGENCTRL_LEARN_FWD_KILL
 127                    | ANA_AGENCTRL_LEARN_IGNORE_VLAN,
 128                    ANA_AGENCTRL);
 129
 130         /* Clear the MAC table */
 131         ocelot_write(ocelot, MACACCESS_CMD_INIT, ANA_TABLES_MACACCESS);
 132 }
 133
 134 static void ocelot_vcap_enable(struct ocelot *ocelot, struct ocelot_port *port)
 135 {
 136         ocelot_write_gix(ocelot, ANA_PORT_VCAP_S2_CFG_S2_ENA |
 137                          ANA_PORT_VCAP_S2_CFG_S2_IP6_CFG(0xa),
 138                          ANA_PORT_VCAP_S2_CFG, port->chip_port);
 139 }
 140
 141 static inline u32 ocelot_vlant_read_vlanaccess(struct ocelot *ocelot)
 142 {
 143         return ocelot_read(ocelot, ANA_TABLES_VLANACCESS);
 144 }
 145
 146 static inline int ocelot_vlant_wait_for_completion(struct ocelot *ocelot)
 147 {
 148         u32 val;
 149
 150         return readx_poll_timeout(ocelot_vlant_read_vlanaccess,
 151                 ocelot,
 152                 val,
 153                 (val & ANA_TABLES_VLANACCESS_VLAN_TBL_CMD_M) ==
 154                 ANA_TABLES_VLANACCESS_CMD_IDLE,
 155                 TABLE_UPDATE_SLEEP_US, TABLE_UPDATE_TIMEOUT_US);
 156 }
 157
 158 static int ocelot_vlant_set_mask(struct ocelot *ocelot, u16 vid, u32 mask)
 159 {
 160         /* Select the VID to configure */
 161         ocelot_write(ocelot, ANA_TABLES_VLANTIDX_V_INDEX(vid),
 162                      ANA_TABLES_VLANTIDX);
 163         /* Set the vlan port members mask and issue a write command */
 164         ocelot_write(ocelot, ANA_TABLES_VLANACCESS_VLAN_PORT_MASK(mask) |
 165                              ANA_TABLES_VLANACCESS_CMD_WRITE,
 166                      ANA_TABLES_VLANACCESS);
 167
 168         return ocelot_vlant_wait_for_completion(ocelot);
 169 }
 170
 171 static void ocelot_vlan_mode(struct ocelot_port *port,
 172                              netdev_features_t features)
 173 {
 174         struct ocelot *ocelot = port->ocelot;
 175         u8 p = port->chip_port;
 176         u32 val;
 177
 178         /* Filtering */
 179         val = ocelot_read(ocelot, ANA_VLANMASK);
 180         if (features & NETIF_F_HW_VLAN_CTAG_FILTER)
 181                 val |= BIT(p);
 182         else
 183                 val &= ~BIT(p);
 184         ocelot_write(ocelot, val, ANA_VLANMASK);
 185 }
 186
 187 static void ocelot_vlan_port_apply(struct ocelot *ocelot,
 188                                    struct ocelot_port *port)
 189 {
 190         u32 val;
 191
 192         /* Ingress clasification (ANA_PORT_VLAN_CFG) */
 193         /* Default vlan to clasify for untagged frames (may be zero) */
 194         val = ANA_PORT_VLAN_CFG_VLAN_VID(port->pvid);
 195         if (port->vlan_aware)
 196                 val |= ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
 197                        ANA_PORT_VLAN_CFG_VLAN_POP_CNT(1);
 198
 199         ocelot_rmw_gix(ocelot, val,
 200                        ANA_PORT_VLAN_CFG_VLAN_VID_M |
 201                        ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
 202                        ANA_PORT_VLAN_CFG_VLAN_POP_CNT_M,
 203                        ANA_PORT_VLAN_CFG, port->chip_port);
 204
 205         /* Drop frames with multicast source address */
 206         val = ANA_PORT_DROP_CFG_DROP_MC_SMAC_ENA;
 207         if (port->vlan_aware && !port->vid)
 208                 /* If port is vlan-aware and tagged, drop untagged and priority
 209                  * tagged frames.
 210                  */
 211                 val |= ANA_PORT_DROP_CFG_DROP_UNTAGGED_ENA |
 212                        ANA_PORT_DROP_CFG_DROP_PRIO_S_TAGGED_ENA |
 213                        ANA_PORT_DROP_CFG_DROP_PRIO_C_TAGGED_ENA;
 214         ocelot_write_gix(ocelot, val, ANA_PORT_DROP_CFG, port->chip_port);
 215
 216         /* Egress configuration (REW_TAG_CFG): VLAN tag type to 8021Q. */
 217         val = REW_TAG_CFG_TAG_TPID_CFG(0);
 218
 219         if (port->vlan_aware) {
 220                 if (port->vid)
 221                         /* Tag all frames except when VID == DEFAULT_VLAN */
 222                         val |= REW_TAG_CFG_TAG_CFG(1);
 223                 else
 224                         /* Tag all frames */
 225                         val |= REW_TAG_CFG_TAG_CFG(3);
 226         }
 227         ocelot_rmw_gix(ocelot, val,
 228                        REW_TAG_CFG_TAG_TPID_CFG_M |
 229                        REW_TAG_CFG_TAG_CFG_M,
 230                        REW_TAG_CFG, port->chip_port);
 231
 232         /* Set default VLAN and tag type to 8021Q. */
 233         val = REW_PORT_VLAN_CFG_PORT_TPID(ETH_P_8021Q) |
 234               REW_PORT_VLAN_CFG_PORT_VID(port->vid);
 235         ocelot_rmw_gix(ocelot, val,
 236                        REW_PORT_VLAN_CFG_PORT_TPID_M |
 237                        REW_PORT_VLAN_CFG_PORT_VID_M,
 238                        REW_PORT_VLAN_CFG, port->chip_port);
 239 }
 240
 241 static int ocelot_vlan_vid_add(struct net_device *dev, u16 vid, bool pvid,
 242                                bool untagged)
 243 {
 244         struct ocelot_port *port = netdev_priv(dev);
 245         struct ocelot *ocelot = port->ocelot;
 246         int ret;
 247
 248         /* Add the port MAC address to with the right VLAN information */
 249         ocelot_mact_learn(ocelot, PGID_CPU, dev->dev_addr, vid,
 250                           ENTRYTYPE_LOCKED);
 251
 252         /* Make the port a member of the VLAN */
 253         ocelot->vlan_mask[vid] |= BIT(port->chip_port);
 254         ret = ocelot_vlant_set_mask(ocelot, vid, ocelot->vlan_mask[vid]);
 255         if (ret)
 256                 return ret;
 257
 258         /* Default ingress vlan classification */
 259         if (pvid)
 260                 port->pvid = vid;
 261
 262         /* Untagged egress vlan clasification */
 263         if (untagged && port->vid != vid) {
 264                 if (port->vid) {
 265                         dev_err(ocelot->dev,
 266                                 "Port already has a native VLAN: %d\n",
 267                                 port->vid);
 268                         return -EBUSY;
 269                 }
 270                 port->vid = vid;
 271         }
 272
 273         ocelot_vlan_port_apply(ocelot, port);
 274
 275         return 0;
 276 }
 277
 278 static int ocelot_vlan_vid_del(struct net_device *dev, u16 vid)
 279 {
 280         struct ocelot_port *port = netdev_priv(dev);
 281         struct ocelot *ocelot = port->ocelot;
 282         int ret;
 283
 284         /* 8021q removes VID 0 on module unload for all interfaces
 285          * with VLAN filtering feature. We need to keep it to receive
 286          * untagged traffic.
 287          */
 288         if (vid == 0)
 289                 return 0;
 290
 291         /* Del the port MAC address to with the right VLAN information */
 292         ocelot_mact_forget(ocelot, dev->dev_addr, vid);
 293
 294         /* Stop the port from being a member of the vlan */
 295         ocelot->vlan_mask[vid] &= ~BIT(port->chip_port);
 296         ret = ocelot_vlant_set_mask(ocelot, vid, ocelot->vlan_mask[vid]);
 297         if (ret)
 298                 return ret;
 299
 300         /* Ingress */
 301         if (port->pvid == vid)
 302                 port->pvid = 0;
 303
 304         /* Egress */
 305         if (port->vid == vid)
 306                 port->vid = 0;
 307
 308         ocelot_vlan_port_apply(ocelot, port);
 309
 310         return 0;
 311 }
 312
 313 static void ocelot_vlan_init(struct ocelot *ocelot)
 314 {
 315         u16 port, vid;
 316
 317         /* Clear VLAN table, by default all ports are members of all VLANs */
 318         ocelot_write(ocelot, ANA_TABLES_VLANACCESS_CMD_INIT,
 319                      ANA_TABLES_VLANACCESS);
 320         ocelot_vlant_wait_for_completion(ocelot);
 321
 322         /* Configure the port VLAN memberships */
 323         for (vid = 1; vid < VLAN_N_VID; vid++) {
 324                 ocelot->vlan_mask[vid] = 0;
 325                 ocelot_vlant_set_mask(ocelot, vid, ocelot->vlan_mask[vid]);
 326         }
 327
 328         /* Because VLAN filtering is enabled, we need VID 0 to get untagged
 329          * traffic.  It is added automatically if 8021q module is loaded, but
 330          * we can't rely on it since module may be not loaded.
 331          */
 332         ocelot->vlan_mask[0] = GENMASK(ocelot->num_phys_ports - 1, 0);
 333         ocelot_vlant_set_mask(ocelot, 0, ocelot->vlan_mask[0]);
 334
 335         /* Configure the CPU port to be VLAN aware */
 336         ocelot_write_gix(ocelot, ANA_PORT_VLAN_CFG_VLAN_VID(0) |
 337                                  ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
 338                                  ANA_PORT_VLAN_CFG_VLAN_POP_CNT(1),
 339                          ANA_PORT_VLAN_CFG, ocelot->num_phys_ports);
 340
 341         /* Set vlan ingress filter mask to all ports but the CPU port by
 342          * default.
 343          */
 344         ocelot_write(ocelot, GENMASK(9, 0), ANA_VLANMASK);
 345
 346         for (port = 0; port < ocelot->num_phys_ports; port++) {
 347                 ocelot_write_gix(ocelot, 0, REW_PORT_VLAN_CFG, port);
 348                 ocelot_write_gix(ocelot, 0, REW_TAG_CFG, port);
 349         }
 350 }
 351
 352 /* Watermark encode
 353  * Bit 8:   Unit; 0:1, 1:16
 354  * Bit 7-0: Value to be multiplied with unit
 355  */
 356 static u16 ocelot_wm_enc(u16 value)
 357 {
 358         if (value >= BIT(8))
 359                 return BIT(8) | (value / 16);
 360
 361         return value;
 362 }
 363
 364 static void ocelot_port_adjust_link(struct net_device *dev)
 365 {
 366         struct ocelot_port *port = netdev_priv(dev);
 367         struct ocelot *ocelot = port->ocelot;
 368         u8 p = port->chip_port;
 369         int speed, atop_wm, mode = 0;
 370
 371         switch (dev->phydev->speed) {
 372         case SPEED_10:
 373                 speed = OCELOT_SPEED_10;
 374                 break;
 375         case SPEED_100:
 376                 speed = OCELOT_SPEED_100;
 377                 break;
 378         case SPEED_1000:
 379                 speed = OCELOT_SPEED_1000;
 380                 mode = DEV_MAC_MODE_CFG_GIGA_MODE_ENA;
 381                 break;
 382         case SPEED_2500:
 383                 speed = OCELOT_SPEED_2500;
 384                 mode = DEV_MAC_MODE_CFG_GIGA_MODE_ENA;
 385                 break;
 386         default:
 387                 netdev_err(dev, "Unsupported PHY speed: %d\n",
 388                            dev->phydev->speed);
 389                 return;
 390         }
 391
 392         phy_print_status(dev->phydev);
 393
 394         if (!dev->phydev->link)
 395                 return;
 396
 397         /* Only full duplex supported for now */
 398         ocelot_port_writel(port, DEV_MAC_MODE_CFG_FDX_ENA |
 399                            mode, DEV_MAC_MODE_CFG);
 400
 401         /* Set MAC IFG Gaps
 402          * FDX: TX_IFG = 5, RX_IFG1 = RX_IFG2 = 0
 403          * !FDX: TX_IFG = 5, RX_IFG1 = RX_IFG2 = 5
 404          */
 405         ocelot_port_writel(port, DEV_MAC_IFG_CFG_TX_IFG(5), DEV_MAC_IFG_CFG);
 406
 407         /* Load seed (0) and set MAC HDX late collision  */
 408         ocelot_port_writel(port, DEV_MAC_HDX_CFG_LATE_COL_POS(67) |
 409                            DEV_MAC_HDX_CFG_SEED_LOAD,
 410                            DEV_MAC_HDX_CFG);
 411         mdelay(1);
 412         ocelot_port_writel(port, DEV_MAC_HDX_CFG_LATE_COL_POS(67),
 413                            DEV_MAC_HDX_CFG);
 414
 415         /* Disable HDX fast control */
 416         ocelot_port_writel(port, DEV_PORT_MISC_HDX_FAST_DIS, DEV_PORT_MISC);
 417
 418         /* SGMII only for now */
 419         ocelot_port_writel(port, PCS1G_MODE_CFG_SGMII_MODE_ENA, PCS1G_MODE_CFG);
 420         ocelot_port_writel(port, PCS1G_SD_CFG_SD_SEL, PCS1G_SD_CFG);
 421
 422         /* Enable PCS */
 423         ocelot_port_writel(port, PCS1G_CFG_PCS_ENA, PCS1G_CFG);
 424
 425         /* No aneg on SGMII */
 426         ocelot_port_writel(port, 0, PCS1G_ANEG_CFG);
 427
 428         /* No loopback */
 429         ocelot_port_writel(port, 0, PCS1G_LB_CFG);
 430
 431         /* Set Max Length and maximum tags allowed */
 432         ocelot_port_writel(port, VLAN_ETH_FRAME_LEN, DEV_MAC_MAXLEN_CFG);
 433         ocelot_port_writel(port, DEV_MAC_TAGS_CFG_TAG_ID(ETH_P_8021AD) |
 434                            DEV_MAC_TAGS_CFG_VLAN_AWR_ENA |
 435                            DEV_MAC_TAGS_CFG_VLAN_LEN_AWR_ENA,
 436                            DEV_MAC_TAGS_CFG);
 437
 438         /* Enable MAC module */
 439         ocelot_port_writel(port, DEV_MAC_ENA_CFG_RX_ENA |
 440                            DEV_MAC_ENA_CFG_TX_ENA, DEV_MAC_ENA_CFG);
 441
 442         /* Take MAC, Port, Phy (intern) and PCS (SGMII/Serdes) clock out of
 443          * reset */
 444         ocelot_port_writel(port, DEV_CLOCK_CFG_LINK_SPEED(speed),
 445                            DEV_CLOCK_CFG);
 446
 447         /* Set SMAC of Pause frame (00:00:00:00:00:00) */
 448         ocelot_port_writel(port, 0, DEV_MAC_FC_MAC_HIGH_CFG);
 449         ocelot_port_writel(port, 0, DEV_MAC_FC_MAC_LOW_CFG);
 450
 451         /* No PFC */
 452         ocelot_write_gix(ocelot, ANA_PFC_PFC_CFG_FC_LINK_SPEED(speed),
 453                          ANA_PFC_PFC_CFG, p);
 454
 455         /* Set Pause WM hysteresis
 456          * 152 = 6 * VLAN_ETH_FRAME_LEN / OCELOT_BUFFER_CELL_SZ
 457          * 101 = 4 * VLAN_ETH_FRAME_LEN / OCELOT_BUFFER_CELL_SZ
 458          */
 459         ocelot_write_rix(ocelot, SYS_PAUSE_CFG_PAUSE_ENA |
 460                          SYS_PAUSE_CFG_PAUSE_STOP(101) |
 461                          SYS_PAUSE_CFG_PAUSE_START(152), SYS_PAUSE_CFG, p);
 462
 463         /* Core: Enable port for frame transfer */
 464         ocelot_write_rix(ocelot, QSYS_SWITCH_PORT_MODE_INGRESS_DROP_MODE |
 465                          QSYS_SWITCH_PORT_MODE_SCH_NEXT_CFG(1) |
 466                          QSYS_SWITCH_PORT_MODE_PORT_ENA,
 467                          QSYS_SWITCH_PORT_MODE, p);
 468
 469         /* Flow control */
 470         ocelot_write_rix(ocelot, SYS_MAC_FC_CFG_PAUSE_VAL_CFG(0xffff) |
 471                          SYS_MAC_FC_CFG_RX_FC_ENA | SYS_MAC_FC_CFG_TX_FC_ENA |
 472                          SYS_MAC_FC_CFG_ZERO_PAUSE_ENA |
 473                          SYS_MAC_FC_CFG_FC_LATENCY_CFG(0x7) |
 474                          SYS_MAC_FC_CFG_FC_LINK_SPEED(speed),
 475                          SYS_MAC_FC_CFG, p);
 476         ocelot_write_rix(ocelot, 0, ANA_POL_FLOWC, p);
 477
 478         /* Tail dropping watermark */
 479         atop_wm = (ocelot->shared_queue_sz - 9 * VLAN_ETH_FRAME_LEN) / OCELOT_BUFFER_CELL_SZ;
 480         ocelot_write_rix(ocelot, ocelot_wm_enc(9 * VLAN_ETH_FRAME_LEN),
 481                          SYS_ATOP, p);
 482         ocelot_write(ocelot, ocelot_wm_enc(atop_wm), SYS_ATOP_TOT_CFG);
 483 }
 484
 485 static int ocelot_port_open(struct net_device *dev)
 486 {
 487         struct ocelot_port *port = netdev_priv(dev);
 488         struct ocelot *ocelot = port->ocelot;
 489         int err;
 490
 491         /* Enable receiving frames on the port, and activate auto-learning of
 492          * MAC addresses.
 493          */
 494         ocelot_write_gix(ocelot, ANA_PORT_PORT_CFG_LEARNAUTO |
 495                          ANA_PORT_PORT_CFG_RECV_ENA |
 496                          ANA_PORT_PORT_CFG_PORTID_VAL(port->chip_port),
 497                          ANA_PORT_PORT_CFG, port->chip_port);
 498
 499         if (port->serdes) {
 500                 err = phy_set_mode_ext(port->serdes, PHY_MODE_ETHERNET,
 501                                        port->phy_mode);
 502                 if (err) {
 503                         netdev_err(dev, "Could not set mode of SerDes\n");
 504                         return err;
 505                 }
 506         }
 507
 508         err = phy_connect_direct(dev, port->phy, &ocelot_port_adjust_link,
 509                                  port->phy_mode);
 510         if (err) {
 511                 netdev_err(dev, "Could not attach to PHY\n");
 512                 return err;
 513         }
 514
 515         dev->phydev = port->phy;
 516
 517         phy_attached_info(port->phy);
 518         phy_start(port->phy);
 519         return 0;
 520 }
 521
 522 static int ocelot_port_stop(struct net_device *dev)
 523 {
 524         struct ocelot_port *port = netdev_priv(dev);
 525
 526         phy_disconnect(port->phy);
 527
 528         dev->phydev = NULL;
 529
 530         ocelot_port_writel(port, 0, DEV_MAC_ENA_CFG);
 531         ocelot_rmw_rix(port->ocelot, 0, QSYS_SWITCH_PORT_MODE_PORT_ENA,
 532                          QSYS_SWITCH_PORT_MODE, port->chip_port);
 533         return 0;
 534 }
 535
 536 /* Generate the IFH for frame injection
 537  *
 538  * The IFH is a 128bit-value
 539  * bit 127: bypass the analyzer processing
 540  * bit 56-67: destination mask
 541  * bit 28-29: pop_cnt: 3 disables all rewriting of the frame
 542  * bit 20-27: cpu extraction queue mask
 543  * bit 16: tag type 0: C-tag, 1: S-tag
 544  * bit 0-11: VID
 545  */
 546 static int ocelot_gen_ifh(u32 *ifh, struct frame_info *info)
 547 {
 548         ifh[0] = IFH_INJ_BYPASS;
 549         ifh[1] = (0xf00 & info->port) >> 8;
 550         ifh[2] = (0xff & info->port) << 24;
 551         ifh[3] = (info->tag_type << 16) | info->vid;
 552
 553         return 0;
 554 }
 555
 556 static int ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
 557 {
 558         struct ocelot_port *port = netdev_priv(dev);
 559         struct ocelot *ocelot = port->ocelot;
 560         u32 val, ifh[IFH_LEN];
 561         struct frame_info info = {};
 562         u8 grp = 0; /* Send everything on CPU group 0 */
 563         unsigned int i, count, last;
 564
 565         val = ocelot_read(ocelot, QS_INJ_STATUS);
 566         if (!(val & QS_INJ_STATUS_FIFO_RDY(BIT(grp))) ||
 567             (val & QS_INJ_STATUS_WMARK_REACHED(BIT(grp))))
 568                 return NETDEV_TX_BUSY;
 569
 570         ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
 571                          QS_INJ_CTRL_SOF, QS_INJ_CTRL, grp);
 572
 573         info.port = BIT(port->chip_port);
 574         info.tag_type = IFH_TAG_TYPE_C;
 575         info.vid = skb_vlan_tag_get(skb);
 576         ocelot_gen_ifh(ifh, &info);
 577
 578         for (i = 0; i < IFH_LEN; i++)
 579                 ocelot_write_rix(ocelot, (__force u32)cpu_to_be32(ifh[i]),
 580                                  QS_INJ_WR, grp);
 581
 582         count = (skb->len + 3) / 4;
 583         last = skb->len % 4;
 584         for (i = 0; i < count; i++) {
 585                 ocelot_write_rix(ocelot, ((u32 *)skb->data)[i], QS_INJ_WR, grp);
 586         }
 587
 588         /* Add padding */
 589         while (i < (OCELOT_BUFFER_CELL_SZ / 4)) {
 590                 ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
 591                 i++;
 592         }
 593
 594         /* Indicate EOF and valid bytes in last word */
 595         ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
 596                          QS_INJ_CTRL_VLD_BYTES(skb->len < OCELOT_BUFFER_CELL_SZ ? 0 : last) |
 597                          QS_INJ_CTRL_EOF,
 598                          QS_INJ_CTRL, grp);
 599
 600         /* Add dummy CRC */
 601         ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
 602         skb_tx_timestamp(skb);
 603
 604         dev->stats.tx_packets++;
 605         dev->stats.tx_bytes += skb->len;
 606         dev_kfree_skb_any(skb);
 607
 608         return NETDEV_TX_OK;
 609 }
 610
 611 static int ocelot_mc_unsync(struct net_device *dev, const unsigned char *addr)
 612 {
 613         struct ocelot_port *port = netdev_priv(dev);
 614
 615         return ocelot_mact_forget(port->ocelot, addr, port->pvid);
 616 }
 617
 618 static int ocelot_mc_sync(struct net_device *dev, const unsigned char *addr)
 619 {
 620         struct ocelot_port *port = netdev_priv(dev);
 621
 622         return ocelot_mact_learn(port->ocelot, PGID_CPU, addr, port->pvid,
 623                                  ENTRYTYPE_LOCKED);
 624 }
 625
 626 static void ocelot_set_rx_mode(struct net_device *dev)
 627 {
 628         struct ocelot_port *port = netdev_priv(dev);
 629         struct ocelot *ocelot = port->ocelot;
 630         int i;
 631         u32 val;
 632
 633         /* This doesn't handle promiscuous mode because the bridge core is
 634          * setting IFF_PROMISC on all slave interfaces and all frames would be
 635          * forwarded to the CPU port.
 636          */
 637         val = GENMASK(ocelot->num_phys_ports - 1, 0);
 638         for (i = ocelot->num_phys_ports + 1; i < PGID_CPU; i++)
 639                 ocelot_write_rix(ocelot, val, ANA_PGID_PGID, i);
 640
 641         __dev_mc_sync(dev, ocelot_mc_sync, ocelot_mc_unsync);
 642 }
 643
 644 static int ocelot_port_get_phys_port_name(struct net_device *dev,
 645                                           char *buf, size_t len)
 646 {
 647         struct ocelot_port *port = netdev_priv(dev);
 648         int ret;
 649
 650         ret = snprintf(buf, len, "p%d", port->chip_port);
 651         if (ret >= len)
 652                 return -EINVAL;
 653
 654         return 0;
 655 }
 656
 657 static int ocelot_port_set_mac_address(struct net_device *dev, void *p)
 658 {
 659         struct ocelot_port *port = netdev_priv(dev);
 660         struct ocelot *ocelot = port->ocelot;
 661         const struct sockaddr *addr = p;
 662
 663         /* Learn the new net device MAC address in the mac table. */
 664         ocelot_mact_learn(ocelot, PGID_CPU, addr->sa_data, port->pvid,
 665                           ENTRYTYPE_LOCKED);
 666         /* Then forget the previous one. */
 667         ocelot_mact_forget(ocelot, dev->dev_addr, port->pvid);
 668
 669         ether_addr_copy(dev->dev_addr, addr->sa_data);
 670         return 0;
 671 }
 672
 673 static void ocelot_get_stats64(struct net_device *dev,
 674                                struct rtnl_link_stats64 *stats)
 675 {
 676         struct ocelot_port *port = netdev_priv(dev);
 677         struct ocelot *ocelot = port->ocelot;
 678
 679         /* Configure the port to read the stats from */
 680         ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(port->chip_port),
 681                      SYS_STAT_CFG);
 682
 683         /* Get Rx stats */
 684         stats->rx_bytes = ocelot_read(ocelot, SYS_COUNT_RX_OCTETS);
 685         stats->rx_packets = ocelot_read(ocelot, SYS_COUNT_RX_SHORTS) +
 686                             ocelot_read(ocelot, SYS_COUNT_RX_FRAGMENTS) +
 687                             ocelot_read(ocelot, SYS_COUNT_RX_JABBERS) +
 688                             ocelot_read(ocelot, SYS_COUNT_RX_LONGS) +
 689                             ocelot_read(ocelot, SYS_COUNT_RX_64) +
 690                             ocelot_read(ocelot, SYS_COUNT_RX_65_127) +
 691                             ocelot_read(ocelot, SYS_COUNT_RX_128_255) +
 692                             ocelot_read(ocelot, SYS_COUNT_RX_256_1023) +
 693                             ocelot_read(ocelot, SYS_COUNT_RX_1024_1526) +
 694                             ocelot_read(ocelot, SYS_COUNT_RX_1527_MAX);
 695         stats->multicast = ocelot_read(ocelot, SYS_COUNT_RX_MULTICAST);
 696         stats->rx_dropped = dev->stats.rx_dropped;
 697
 698         /* Get Tx stats */
 699         stats->tx_bytes = ocelot_read(ocelot, SYS_COUNT_TX_OCTETS);
 700         stats->tx_packets = ocelot_read(ocelot, SYS_COUNT_TX_64) +
 701                             ocelot_read(ocelot, SYS_COUNT_TX_65_127) +
 702                             ocelot_read(ocelot, SYS_COUNT_TX_128_511) +
 703                             ocelot_read(ocelot, SYS_COUNT_TX_512_1023) +
 704                             ocelot_read(ocelot, SYS_COUNT_TX_1024_1526) +
 705                             ocelot_read(ocelot, SYS_COUNT_TX_1527_MAX);
 706         stats->tx_dropped = ocelot_read(ocelot, SYS_COUNT_TX_DROPS) +
 707                             ocelot_read(ocelot, SYS_COUNT_TX_AGING);
 708         stats->collisions = ocelot_read(ocelot, SYS_COUNT_TX_COLLISION);
 709 }
 710
 711 static int ocelot_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
 712                           struct net_device *dev, const unsigned char *addr,
 713                           u16 vid, u16 flags,
 714                           struct netlink_ext_ack *extack)
 715 {
 716         struct ocelot_port *port = netdev_priv(dev);
 717         struct ocelot *ocelot = port->ocelot;
 718
 719         if (!vid) {
 720                 if (!port->vlan_aware)
 721                         /* If the bridge is not VLAN aware and no VID was
 722                          * provided, set it to pvid to ensure the MAC entry
 723                          * matches incoming untagged packets
 724                          */
 725                         vid = port->pvid;
 726                 else
 727                         /* If the bridge is VLAN aware a VID must be provided as
 728                          * otherwise the learnt entry wouldn't match any frame.
 729                          */
 730                         return -EINVAL;
 731         }
 732
 733         return ocelot_mact_learn(ocelot, port->chip_port, addr, vid,
 734                                  ENTRYTYPE_LOCKED);
 735 }
 736
 737 static int ocelot_fdb_del(struct ndmsg *ndm, struct nlattr *tb[],
 738                           struct net_device *dev,
 739                           const unsigned char *addr, u16 vid)
 740 {
 741         struct ocelot_port *port = netdev_priv(dev);
 742         struct ocelot *ocelot = port->ocelot;
 743
 744         return ocelot_mact_forget(ocelot, addr, vid);
 745 }
 746
 747 struct ocelot_dump_ctx {
 748         struct net_device *dev;
 749         struct sk_buff *skb;
 750         struct netlink_callback *cb;
 751         int idx;
 752 };
 753
 754 static int ocelot_fdb_do_dump(struct ocelot_mact_entry *entry,
 755                               struct ocelot_dump_ctx *dump)
 756 {
 757         u32 portid = NETLINK_CB(dump->cb->skb).portid;
 758         u32 seq = dump->cb->nlh->nlmsg_seq;
 759         struct nlmsghdr *nlh;
 760         struct ndmsg *ndm;
 761
 762         if (dump->idx < dump->cb->args[2])
 763                 goto skip;
 764
 765         nlh = nlmsg_put(dump->skb, portid, seq, RTM_NEWNEIGH,
 766                         sizeof(*ndm), NLM_F_MULTI);
 767         if (!nlh)
 768                 return -EMSGSIZE;
 769
 770         ndm = nlmsg_data(nlh);
 771         ndm->ndm_family  = AF_BRIDGE;
 772         ndm->ndm_pad1    = 0;
 773         ndm->ndm_pad2    = 0;
 774         ndm->ndm_flags   = NTF_SELF;
 775         ndm->ndm_type    = 0;
 776         ndm->ndm_ifindex = dump->dev->ifindex;
 777         ndm->ndm_state   = NUD_REACHABLE;
 778
 779         if (nla_put(dump->skb, NDA_LLADDR, ETH_ALEN, entry->mac))
 780                 goto nla_put_failure;
 781
 782         if (entry->vid && nla_put_u16(dump->skb, NDA_VLAN, entry->vid))
 783                 goto nla_put_failure;
 784
 785         nlmsg_end(dump->skb, nlh);
 786
 787 skip:
 788         dump->idx++;
 789         return 0;
 790
 791 nla_put_failure:
 792         nlmsg_cancel(dump->skb, nlh);
 793         return -EMSGSIZE;
 794 }
 795
 796 static inline int ocelot_mact_read(struct ocelot_port *port, int row, int col,
 797                                    struct ocelot_mact_entry *entry)
 798 {
 799         struct ocelot *ocelot = port->ocelot;
 800         char mac[ETH_ALEN];
 801         u32 val, dst, macl, mach;
 802
 803         /* Set row and column to read from */
 804         ocelot_field_write(ocelot, ANA_TABLES_MACTINDX_M_INDEX, row);
 805         ocelot_field_write(ocelot, ANA_TABLES_MACTINDX_BUCKET, col);
 806
 807         /* Issue a read command */
 808         ocelot_write(ocelot,
 809                      ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_READ),
 810                      ANA_TABLES_MACACCESS);
 811
 812         if (ocelot_mact_wait_for_completion(ocelot))
 813                 return -ETIMEDOUT;
 814
 815         /* Read the entry flags */
 816         val = ocelot_read(ocelot, ANA_TABLES_MACACCESS);
 817         if (!(val & ANA_TABLES_MACACCESS_VALID))
 818                 return -EINVAL;
 819
 820         /* If the entry read has another port configured as its destination,
 821          * do not report it.
 822          */
 823         dst = (val & ANA_TABLES_MACACCESS_DEST_IDX_M) >> 3;
 824         if (dst != port->chip_port)
 825                 return -EINVAL;
 826
 827         /* Get the entry's MAC address and VLAN id */
 828         macl = ocelot_read(ocelot, ANA_TABLES_MACLDATA);
 829         mach = ocelot_read(ocelot, ANA_TABLES_MACHDATA);
 830
 831         mac[0] = (mach >> 8)  & 0xff;
 832         mac[1] = (mach >> 0)  & 0xff;
 833         mac[2] = (macl >> 24) & 0xff;
 834         mac[3] = (macl >> 16) & 0xff;
 835         mac[4] = (macl >> 8)  & 0xff;
 836         mac[5] = (macl >> 0)  & 0xff;
 837
 838         entry->vid = (mach >> 16) & 0xfff;
 839         ether_addr_copy(entry->mac, mac);
 840
 841         return 0;
 842 }
 843
 844 static int ocelot_fdb_dump(struct sk_buff *skb, struct netlink_callback *cb,
 845                            struct net_device *dev,
 846                            struct net_device *filter_dev, int *idx)
 847 {
 848         struct ocelot_port *port = netdev_priv(dev);
 849         int i, j, ret = 0;
 850         struct ocelot_dump_ctx dump = {
 851                 .dev = dev,
 852                 .skb = skb,
 853                 .cb = cb,
 854                 .idx = *idx,
 855         };
 856
 857         struct ocelot_mact_entry entry;
 858
 859         /* Loop through all the mac tables entries. There are 1024 rows of 4
 860          * entries.
 861          */
 862         for (i = 0; i < 1024; i++) {
 863                 for (j = 0; j < 4; j++) {
 864                         ret = ocelot_mact_read(port, i, j, &entry);
 865                         /* If the entry is invalid (wrong port, invalid...),
 866                          * skip it.
 867                          */
 868                         if (ret == -EINVAL)
 869                                 continue;
 870                         else if (ret)
 871                                 goto end;
 872
 873                         ret = ocelot_fdb_do_dump(&entry, &dump);
 874                         if (ret)
 875                                 goto end;
 876                 }
 877         }
 878
 879 end:
 880         *idx = dump.idx;
 881         return ret;
 882 }
 883
 884 static int ocelot_vlan_rx_add_vid(struct net_device *dev, __be16 proto,
 885                                   u16 vid)
 886 {
 887         return ocelot_vlan_vid_add(dev, vid, false, false);
 888 }
 889
 890 static int ocelot_vlan_rx_kill_vid(struct net_device *dev, __be16 proto,
 891                                    u16 vid)
 892 {
 893         return ocelot_vlan_vid_del(dev, vid);
 894 }
 895
 896 static int ocelot_set_features(struct net_device *dev,
 897                                netdev_features_t features)
 898 {
 899         struct ocelot_port *port = netdev_priv(dev);
 900         netdev_features_t changed = dev->features ^ features;
 901
 902         if ((dev->features & NETIF_F_HW_TC) > (features & NETIF_F_HW_TC) &&
 903             port->tc.offload_cnt) {
 904                 netdev_err(dev,
 905                            "Cannot disable HW TC offload while offloads active\n");
 906                 return -EBUSY;
 907         }
 908
 909         if (changed & NETIF_F_HW_VLAN_CTAG_FILTER)
 910                 ocelot_vlan_mode(port, features);
 911
 912         return 0;
 913 }
 914
 915 static int ocelot_get_port_parent_id(struct net_device *dev,
 916                                      struct netdev_phys_item_id *ppid)
 917 {
 918         struct ocelot_port *ocelot_port = netdev_priv(dev);
 919         struct ocelot *ocelot = ocelot_port->ocelot;
 920
 921         ppid->id_len = sizeof(ocelot->base_mac);
 922         memcpy(&ppid->id, &ocelot->base_mac, ppid->id_len);
 923
 924         return 0;
 925 }
 926
 927 static const struct net_device_ops ocelot_port_netdev_ops = {
 928         .ndo_open                       = ocelot_port_open,
 929         .ndo_stop                       = ocelot_port_stop,
 930         .ndo_start_xmit                 = ocelot_port_xmit,
 931         .ndo_set_rx_mode                = ocelot_set_rx_mode,
 932         .ndo_get_phys_port_name         = ocelot_port_get_phys_port_name,
 933         .ndo_set_mac_address            = ocelot_port_set_mac_address,
 934         .ndo_get_stats64                = ocelot_get_stats64,
 935         .ndo_fdb_add                    = ocelot_fdb_add,
 936         .ndo_fdb_del                    = ocelot_fdb_del,
 937         .ndo_fdb_dump                   = ocelot_fdb_dump,
 938         .ndo_vlan_rx_add_vid            = ocelot_vlan_rx_add_vid,
 939         .ndo_vlan_rx_kill_vid           = ocelot_vlan_rx_kill_vid,
 940         .ndo_set_features               = ocelot_set_features,
 941         .ndo_get_port_parent_id         = ocelot_get_port_parent_id,
 942         .ndo_setup_tc                   = ocelot_setup_tc,
 943 };
 944
 945 static void ocelot_get_strings(struct net_device *netdev, u32 sset, u8 *data)
 946 {
 947         struct ocelot_port *port = netdev_priv(netdev);
 948         struct ocelot *ocelot = port->ocelot;
 949         int i;
 950
 951         if (sset != ETH_SS_STATS)
 952                 return;
 953
 954         for (i = 0; i < ocelot->num_stats; i++)
 955                 memcpy(data + i * ETH_GSTRING_LEN, ocelot->stats_layout[i].name,
 956                        ETH_GSTRING_LEN);
 957 }
 958
 959 static void ocelot_update_stats(struct ocelot *ocelot)
 960 {
 961         int i, j;
 962
 963         mutex_lock(&ocelot->stats_lock);
 964
 965         for (i = 0; i < ocelot->num_phys_ports; i++) {
 966                 /* Configure the port to read the stats from */
 967                 ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(i), SYS_STAT_CFG);
 968
 969                 for (j = 0; j < ocelot->num_stats; j++) {
 970                         u32 val;
 971                         unsigned int idx = i * ocelot->num_stats + j;
 972
 973                         val = ocelot_read_rix(ocelot, SYS_COUNT_RX_OCTETS,
 974                                               ocelot->stats_layout[j].offset);
 975
 976                         if (val < (ocelot->stats[idx] & U32_MAX))
 977                                 ocelot->stats[idx] += (u64)1 << 32;
 978
 979                         ocelot->stats[idx] = (ocelot->stats[idx] &
 980                                               ~(u64)U32_MAX) + val;
 981                 }
 982         }
 983
 984         mutex_unlock(&ocelot->stats_lock);
 985 }
 986
 987 static void ocelot_check_stats_work(struct work_struct *work)
 988 {
 989         struct delayed_work *del_work = to_delayed_work(work);
 990         struct ocelot *ocelot = container_of(del_work, struct ocelot,
 991                                              stats_work);
 992
 993         ocelot_update_stats(ocelot);
 994
 995         queue_delayed_work(ocelot->stats_queue, &ocelot->stats_work,
 996                            OCELOT_STATS_CHECK_DELAY);
 997 }
 998
 999 static void ocelot_get_ethtool_stats(struct net_device *dev,
1000                                      struct ethtool_stats *stats, u64 *data)
1001 {
1002         struct ocelot_port *port = netdev_priv(dev);
1003         struct ocelot *ocelot = port->ocelot;
1004         int i;
1005
1006         /* check and update now */
1007         ocelot_update_stats(ocelot);
1008
1009         /* Copy all counters */
1010         for (i = 0; i < ocelot->num_stats; i++)
1011                 *data++ = ocelot->stats[port->chip_port * ocelot->num_stats + i];
1012 }
1013
1014 static int ocelot_get_sset_count(struct net_device *dev, int sset)
1015 {
1016         struct ocelot_port *port = netdev_priv(dev);
1017         struct ocelot *ocelot = port->ocelot;
1018
1019         if (sset != ETH_SS_STATS)
1020                 return -EOPNOTSUPP;
1021         return ocelot->num_stats;
1022 }
1023
1024 static const struct ethtool_ops ocelot_ethtool_ops = {
1025         .get_strings            = ocelot_get_strings,
1026         .get_ethtool_stats      = ocelot_get_ethtool_stats,
1027         .get_sset_count         = ocelot_get_sset_count,
1028         .get_link_ksettings     = phy_ethtool_get_link_ksettings,
1029         .set_link_ksettings     = phy_ethtool_set_link_ksettings,
1030 };
1031
1032 static int ocelot_port_attr_stp_state_set(struct ocelot_port *ocelot_port,
1033                                           struct switchdev_trans *trans,
1034                                           u8 state)
1035 {
1036         struct ocelot *ocelot = ocelot_port->ocelot;
1037         u32 port_cfg;
1038         int port, i;
1039
1040         if (switchdev_trans_ph_prepare(trans))
1041                 return 0;
1042
1043         if (!(BIT(ocelot_port->chip_port) & ocelot->bridge_mask))
1044                 return 0;
1045
1046         port_cfg = ocelot_read_gix(ocelot, ANA_PORT_PORT_CFG,
1047                                    ocelot_port->chip_port);
1048
1049         switch (state) {
1050         case BR_STATE_FORWARDING:
1051                 ocelot->bridge_fwd_mask |= BIT(ocelot_port->chip_port);
1052                 /* Fallthrough */
1053         case BR_STATE_LEARNING:
1054                 port_cfg |= ANA_PORT_PORT_CFG_LEARN_ENA;
1055                 break;
1056
1057         default:
1058                 port_cfg &= ~ANA_PORT_PORT_CFG_LEARN_ENA;
1059                 ocelot->bridge_fwd_mask &= ~BIT(ocelot_port->chip_port);
1060                 break;
1061         }
1062
1063         ocelot_write_gix(ocelot, port_cfg, ANA_PORT_PORT_CFG,
1064                          ocelot_port->chip_port);
1065
1066         /* Apply FWD mask. The loop is needed to add/remove the current port as
1067          * a source for the other ports.
1068          */
1069         for (port = 0; port < ocelot->num_phys_ports; port++) {
1070                 if (ocelot->bridge_fwd_mask & BIT(port)) {
1071                         unsigned long mask = ocelot->bridge_fwd_mask & ~BIT(port);
1072
1073                         for (i = 0; i < ocelot->num_phys_ports; i++) {
1074                                 unsigned long bond_mask = ocelot->lags[i];
1075
1076                                 if (!bond_mask)
1077                                         continue;
1078
1079                                 if (bond_mask & BIT(port)) {
1080                                         mask &= ~bond_mask;
1081                                         break;
1082                                 }
1083                         }
1084
1085                         ocelot_write_rix(ocelot,
1086                                          BIT(ocelot->num_phys_ports) | mask,
1087                                          ANA_PGID_PGID, PGID_SRC + port);
1088                 } else {
1089                         /* Only the CPU port, this is compatible with link
1090                          * aggregation.
1091                          */
1092                         ocelot_write_rix(ocelot,
1093                                          BIT(ocelot->num_phys_ports),
1094                                          ANA_PGID_PGID, PGID_SRC + port);
1095                 }
1096         }
1097
1098         return 0;
1099 }
1100
1101 static void ocelot_port_attr_ageing_set(struct ocelot_port *ocelot_port,
1102                                         unsigned long ageing_clock_t)
1103 {
1104         struct ocelot *ocelot = ocelot_port->ocelot;
1105         unsigned long ageing_jiffies = clock_t_to_jiffies(ageing_clock_t);
1106         u32 ageing_time = jiffies_to_msecs(ageing_jiffies) / 1000;
1107
1108         ocelot_write(ocelot, ANA_AUTOAGE_AGE_PERIOD(ageing_time / 2),
1109                      ANA_AUTOAGE);
1110 }
1111
1112 static void ocelot_port_attr_mc_set(struct ocelot_port *port, bool mc)
1113 {
1114         struct ocelot *ocelot = port->ocelot;
1115         u32 val = ocelot_read_gix(ocelot, ANA_PORT_CPU_FWD_CFG,
1116                                   port->chip_port);
1117
1118         if (mc)
1119                 val |= ANA_PORT_CPU_FWD_CFG_CPU_IGMP_REDIR_ENA |
1120                        ANA_PORT_CPU_FWD_CFG_CPU_MLD_REDIR_ENA |
1121                        ANA_PORT_CPU_FWD_CFG_CPU_IPMC_CTRL_COPY_ENA;
1122         else
1123                 val &= ~(ANA_PORT_CPU_FWD_CFG_CPU_IGMP_REDIR_ENA |
1124                          ANA_PORT_CPU_FWD_CFG_CPU_MLD_REDIR_ENA |
1125                          ANA_PORT_CPU_FWD_CFG_CPU_IPMC_CTRL_COPY_ENA);
1126
1127         ocelot_write_gix(ocelot, val, ANA_PORT_CPU_FWD_CFG, port->chip_port);
1128 }
1129
1130 static int ocelot_port_attr_set(struct net_device *dev,
1131                                 const struct switchdev_attr *attr,
1132                                 struct switchdev_trans *trans)
1133 {
1134         struct ocelot_port *ocelot_port = netdev_priv(dev);
1135         int err = 0;
1136
1137         switch (attr->id) {
1138         case SWITCHDEV_ATTR_ID_PORT_STP_STATE:
1139                 ocelot_port_attr_stp_state_set(ocelot_port, trans,
1140                                                attr->u.stp_state);
1141                 break;
1142         case SWITCHDEV_ATTR_ID_BRIDGE_AGEING_TIME:
1143                 ocelot_port_attr_ageing_set(ocelot_port, attr->u.ageing_time);
1144                 break;
1145         case SWITCHDEV_ATTR_ID_BRIDGE_VLAN_FILTERING:
1146                 ocelot_port->vlan_aware = attr->u.vlan_filtering;
1147                 ocelot_vlan_port_apply(ocelot_port->ocelot, ocelot_port);
1148                 break;
1149         case SWITCHDEV_ATTR_ID_BRIDGE_MC_DISABLED:
1150                 ocelot_port_attr_mc_set(ocelot_port, !attr->u.mc_disabled);
1151                 break;
1152         default:
1153                 err = -EOPNOTSUPP;
1154                 break;
1155         }
1156
1157         return err;
1158 }
1159
1160 static int ocelot_port_obj_add_vlan(struct net_device *dev,
1161                                     const struct switchdev_obj_port_vlan *vlan,
1162                                     struct switchdev_trans *trans)
1163 {
1164         int ret;
1165         u16 vid;
1166
1167         for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
1168                 ret = ocelot_vlan_vid_add(dev, vid,
1169                                           vlan->flags & BRIDGE_VLAN_INFO_PVID,
1170                                           vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED);
1171                 if (ret)
1172                         return ret;
1173         }
1174
1175         return 0;
1176 }
1177
1178 static int ocelot_port_vlan_del_vlan(struct net_device *dev,
1179                                      const struct switchdev_obj_port_vlan *vlan)
1180 {
1181         int ret;
1182         u16 vid;
1183
1184         for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
1185                 ret = ocelot_vlan_vid_del(dev, vid);
1186
1187                 if (ret)
1188                         return ret;
1189         }
1190
1191         return 0;
1192 }
1193
1194 static struct ocelot_multicast *ocelot_multicast_get(struct ocelot *ocelot,
1195                                                      const unsigned char *addr,
1196                                                      u16 vid)
1197 {
1198         struct ocelot_multicast *mc;
1199
1200         list_for_each_entry(mc, &ocelot->multicast, list) {
1201                 if (ether_addr_equal(mc->addr, addr) && mc->vid == vid)
1202                         return mc;
1203         }
1204
1205         return NULL;
1206 }
1207
1208 static int ocelot_port_obj_add_mdb(struct net_device *dev,
1209                                    const struct switchdev_obj_port_mdb *mdb,
1210                                    struct switchdev_trans *trans)
1211 {
1212         struct ocelot_port *port = netdev_priv(dev);
1213         struct ocelot *ocelot = port->ocelot;
1214         struct ocelot_multicast *mc;
1215         unsigned char addr[ETH_ALEN];
1216         u16 vid = mdb->vid;
1217         bool new = false;
1218
1219         if (!vid)
1220                 vid = port->pvid;
1221
1222         mc = ocelot_multicast_get(ocelot, mdb->addr, vid);
1223         if (!mc) {
1224                 mc = devm_kzalloc(ocelot->dev, sizeof(*mc), GFP_KERNEL);
1225                 if (!mc)
1226                         return -ENOMEM;
1227
1228                 memcpy(mc->addr, mdb->addr, ETH_ALEN);
1229                 mc->vid = vid;
1230
1231                 list_add_tail(&mc->list, &ocelot->multicast);
1232                 new = true;
1233         }
1234
1235         memcpy(addr, mc->addr, ETH_ALEN);
1236         addr[0] = 0;
1237
1238         if (!new) {
1239                 addr[2] = mc->ports << 0;
1240                 addr[1] = mc->ports << 8;
1241                 ocelot_mact_forget(ocelot, addr, vid);
1242         }
1243
1244         mc->ports |= BIT(port->chip_port);
1245         addr[2] = mc->ports << 0;
1246         addr[1] = mc->ports << 8;
1247
1248         return ocelot_mact_learn(ocelot, 0, addr, vid, ENTRYTYPE_MACv4);
1249 }
1250
1251 static int ocelot_port_obj_del_mdb(struct net_device *dev,
1252                                    const struct switchdev_obj_port_mdb *mdb)
1253 {
1254         struct ocelot_port *port = netdev_priv(dev);
1255         struct ocelot *ocelot = port->ocelot;
1256         struct ocelot_multicast *mc;
1257         unsigned char addr[ETH_ALEN];
1258         u16 vid = mdb->vid;
1259
1260         if (!vid)
1261                 vid = port->pvid;
1262
1263         mc = ocelot_multicast_get(ocelot, mdb->addr, vid);
1264         if (!mc)
1265                 return -ENOENT;
1266
1267         memcpy(addr, mc->addr, ETH_ALEN);
1268         addr[2] = mc->ports << 0;
1269         addr[1] = mc->ports << 8;
1270         addr[0] = 0;
1271         ocelot_mact_forget(ocelot, addr, vid);
1272
1273         mc->ports &= ~BIT(port->chip_port);
1274         if (!mc->ports) {
1275                 list_del(&mc->list);
1276                 devm_kfree(ocelot->dev, mc);
1277                 return 0;
1278         }
1279
1280         addr[2] = mc->ports << 0;
1281         addr[1] = mc->ports << 8;
1282
1283         return ocelot_mact_learn(ocelot, 0, addr, vid, ENTRYTYPE_MACv4);
1284 }
1285
1286 static int ocelot_port_obj_add(struct net_device *dev,
1287                                const struct switchdev_obj *obj,
1288                                struct switchdev_trans *trans,
1289                                struct netlink_ext_ack *extack)
1290 {
1291         int ret = 0;
1292
1293         switch (obj->id) {
1294         case SWITCHDEV_OBJ_ID_PORT_VLAN:
1295                 ret = ocelot_port_obj_add_vlan(dev,
1296                                                SWITCHDEV_OBJ_PORT_VLAN(obj),
1297                                                trans);
1298                 break;
1299         case SWITCHDEV_OBJ_ID_PORT_MDB:
1300                 ret = ocelot_port_obj_add_mdb(dev, SWITCHDEV_OBJ_PORT_MDB(obj),
1301                                               trans);
1302                 break;
1303         default:
1304                 return -EOPNOTSUPP;
1305         }
1306
1307         return ret;
1308 }
1309
1310 static int ocelot_port_obj_del(struct net_device *dev,
1311                                const struct switchdev_obj *obj)
1312 {
1313         int ret = 0;
1314
1315         switch (obj->id) {
1316         case SWITCHDEV_OBJ_ID_PORT_VLAN:
1317                 ret = ocelot_port_vlan_del_vlan(dev,
1318                                                 SWITCHDEV_OBJ_PORT_VLAN(obj));
1319                 break;
1320         case SWITCHDEV_OBJ_ID_PORT_MDB:
1321                 ret = ocelot_port_obj_del_mdb(dev, SWITCHDEV_OBJ_PORT_MDB(obj));
1322                 break;
1323         default:
1324                 return -EOPNOTSUPP;
1325         }
1326
1327         return ret;
1328 }
1329
1330 static int ocelot_port_bridge_join(struct ocelot_port *ocelot_port,
1331                                    struct net_device *bridge)
1332 {
1333         struct ocelot *ocelot = ocelot_port->ocelot;
1334
1335         if (!ocelot->bridge_mask) {
1336                 ocelot->hw_bridge_dev = bridge;
1337         } else {
1338                 if (ocelot->hw_bridge_dev != bridge)
1339                         /* This is adding the port to a second bridge, this is
1340                          * unsupported */
1341                         return -ENODEV;
1342         }
1343
1344         ocelot->bridge_mask |= BIT(ocelot_port->chip_port);
1345
1346         return 0;
1347 }
1348
1349 static void ocelot_port_bridge_leave(struct ocelot_port *ocelot_port,
1350                                      struct net_device *bridge)
1351 {
1352         struct ocelot *ocelot = ocelot_port->ocelot;
1353
1354         ocelot->bridge_mask &= ~BIT(ocelot_port->chip_port);
1355
1356         if (!ocelot->bridge_mask)
1357                 ocelot->hw_bridge_dev = NULL;
1358
1359         /* Clear bridge vlan settings before calling ocelot_vlan_port_apply */
1360         ocelot_port->vlan_aware = 0;
1361         ocelot_port->pvid = 0;
1362         ocelot_port->vid = 0;
1363 }
1364
1365 static void ocelot_set_aggr_pgids(struct ocelot *ocelot)
1366 {
1367         int i, port, lag;
1368
1369         /* Reset destination and aggregation PGIDS */
1370         for (port = 0; port < ocelot->num_phys_ports; port++)
1371                 ocelot_write_rix(ocelot, BIT(port), ANA_PGID_PGID, port);
1372
1373         for (i = PGID_AGGR; i < PGID_SRC; i++)
1374                 ocelot_write_rix(ocelot, GENMASK(ocelot->num_phys_ports - 1, 0),
1375                                  ANA_PGID_PGID, i);
1376
1377         /* Now, set PGIDs for each LAG */
1378         for (lag = 0; lag < ocelot->num_phys_ports; lag++) {
1379                 unsigned long bond_mask;
1380                 int aggr_count = 0;
1381                 u8 aggr_idx[16];
1382
1383                 bond_mask = ocelot->lags[lag];
1384                 if (!bond_mask)
1385                         continue;
1386
1387                 for_each_set_bit(port, &bond_mask, ocelot->num_phys_ports) {
1388                         // Destination mask
1389                         ocelot_write_rix(ocelot, bond_mask,
1390                                          ANA_PGID_PGID, port);
1391                         aggr_idx[aggr_count] = port;
1392                         aggr_count++;
1393                 }
1394
1395                 for (i = PGID_AGGR; i < PGID_SRC; i++) {
1396                         u32 ac;
1397
1398                         ac = ocelot_read_rix(ocelot, ANA_PGID_PGID, i);
1399                         ac &= ~bond_mask;
1400                         ac |= BIT(aggr_idx[i % aggr_count]);
1401                         ocelot_write_rix(ocelot, ac, ANA_PGID_PGID, i);
1402                 }
1403         }
1404 }
1405
1406 static void ocelot_setup_lag(struct ocelot *ocelot, int lag)
1407 {
1408         unsigned long bond_mask = ocelot->lags[lag];
1409         unsigned int p;
1410
1411         for_each_set_bit(p, &bond_mask, ocelot->num_phys_ports) {
1412                 u32 port_cfg = ocelot_read_gix(ocelot, ANA_PORT_PORT_CFG, p);
1413
1414                 port_cfg &= ~ANA_PORT_PORT_CFG_PORTID_VAL_M;
1415
1416                 /* Use lag port as logical port for port i */
1417                 ocelot_write_gix(ocelot, port_cfg |
1418                                  ANA_PORT_PORT_CFG_PORTID_VAL(lag),
1419                                  ANA_PORT_PORT_CFG, p);
1420         }
1421 }
1422
1423 static int ocelot_port_lag_join(struct ocelot_port *ocelot_port,
1424                                 struct net_device *bond)
1425 {
1426         struct ocelot *ocelot = ocelot_port->ocelot;
1427         int p = ocelot_port->chip_port;
1428         int lag, lp;
1429         struct net_device *ndev;
1430         u32 bond_mask = 0;
1431
1432         rcu_read_lock();
1433         for_each_netdev_in_bond_rcu(bond, ndev) {
1434                 struct ocelot_port *port = netdev_priv(ndev);
1435
1436                 bond_mask |= BIT(port->chip_port);
1437         }
1438         rcu_read_unlock();
1439
1440         lp = __ffs(bond_mask);
1441
1442         /* If the new port is the lowest one, use it as the logical port from
1443          * now on
1444          */
1445         if (p == lp) {
1446                 lag = p;
1447                 ocelot->lags[p] = bond_mask;
1448                 bond_mask &= ~BIT(p);
1449                 if (bond_mask) {
1450                         lp = __ffs(bond_mask);
1451                         ocelot->lags[lp] = 0;
1452                 }
1453         } else {
1454                 lag = lp;
1455                 ocelot->lags[lp] |= BIT(p);
1456         }
1457
1458         ocelot_setup_lag(ocelot, lag);
1459         ocelot_set_aggr_pgids(ocelot);
1460
1461         return 0;
1462 }
1463
1464 static void ocelot_port_lag_leave(struct ocelot_port *ocelot_port,
1465                                   struct net_device *bond)
1466 {
1467         struct ocelot *ocelot = ocelot_port->ocelot;
1468         int p = ocelot_port->chip_port;
1469         u32 port_cfg;
1470         int i;
1471
1472         /* Remove port from any lag */
1473         for (i = 0; i < ocelot->num_phys_ports; i++)
1474                 ocelot->lags[i] &= ~BIT(ocelot_port->chip_port);
1475
1476         /* if it was the logical port of the lag, move the lag config to the
1477          * next port
1478          */
1479         if (ocelot->lags[p]) {
1480                 int n = __ffs(ocelot->lags[p]);
1481
1482                 ocelot->lags[n] = ocelot->lags[p];
1483                 ocelot->lags[p] = 0;
1484
1485                 ocelot_setup_lag(ocelot, n);
1486         }
1487
1488         port_cfg = ocelot_read_gix(ocelot, ANA_PORT_PORT_CFG, p);
1489         port_cfg &= ~ANA_PORT_PORT_CFG_PORTID_VAL_M;
1490         ocelot_write_gix(ocelot, port_cfg | ANA_PORT_PORT_CFG_PORTID_VAL(p),
1491                          ANA_PORT_PORT_CFG, p);
1492
1493         ocelot_set_aggr_pgids(ocelot);
1494 }
1495
1496 /* Checks if the net_device instance given to us originate from our driver. */
1497 static bool ocelot_netdevice_dev_check(const struct net_device *dev)
1498 {
1499         return dev->netdev_ops == &ocelot_port_netdev_ops;
1500 }
1501
1502 static int ocelot_netdevice_port_event(struct net_device *dev,
1503                                        unsigned long event,
1504                                        struct netdev_notifier_changeupper_info *info)
1505 {
1506         struct ocelot_port *ocelot_port = netdev_priv(dev);
1507         int err = 0;
1508
1509         switch (event) {
1510         case NETDEV_CHANGEUPPER:
1511                 if (netif_is_bridge_master(info->upper_dev)) {
1512                         if (info->linking)
1513                                 err = ocelot_port_bridge_join(ocelot_port,
1514                                                               info->upper_dev);
1515                         else
1516                                 ocelot_port_bridge_leave(ocelot_port,
1517                                                          info->upper_dev);
1518
1519                         ocelot_vlan_port_apply(ocelot_port->ocelot,
1520                                                ocelot_port);
1521                 }
1522                 if (netif_is_lag_master(info->upper_dev)) {
1523                         if (info->linking)
1524                                 err = ocelot_port_lag_join(ocelot_port,
1525                                                            info->upper_dev);
1526                         else
1527                                 ocelot_port_lag_leave(ocelot_port,
1528                                                       info->upper_dev);
1529                 }
1530                 break;
1531         default:
1532                 break;
1533         }
1534
1535         return err;
1536 }
1537
1538 static int ocelot_netdevice_event(struct notifier_block *unused,
1539                                   unsigned long event, void *ptr)
1540 {
1541         struct netdev_notifier_changeupper_info *info = ptr;
1542         struct net_device *dev = netdev_notifier_info_to_dev(ptr);
1543         int ret = 0;
1544
1545         if (!ocelot_netdevice_dev_check(dev))
1546                 return 0;
1547
1548         if (event == NETDEV_PRECHANGEUPPER &&
1549             netif_is_lag_master(info->upper_dev)) {
1550                 struct netdev_lag_upper_info *lag_upper_info = info->upper_info;
1551                 struct netlink_ext_ack *extack;
1552
1553                 if (lag_upper_info &&
1554                     lag_upper_info->tx_type != NETDEV_LAG_TX_TYPE_HASH) {
1555                         extack = netdev_notifier_info_to_extack(&info->info);
1556                         NL_SET_ERR_MSG_MOD(extack, "LAG device using unsupported Tx type");
1557
1558                         ret = -EINVAL;
1559                         goto notify;
1560                 }
1561         }
1562
1563         if (netif_is_lag_master(dev)) {
1564                 struct net_device *slave;
1565                 struct list_head *iter;
1566
1567                 netdev_for_each_lower_dev(dev, slave, iter) {
1568                         ret = ocelot_netdevice_port_event(slave, event, info);
1569                         if (ret)
1570                                 goto notify;
1571                 }
1572         } else {
1573                 ret = ocelot_netdevice_port_event(dev, event, info);
1574         }
1575
1576 notify:
1577         return notifier_from_errno(ret);
1578 }
1579
1580 struct notifier_block ocelot_netdevice_nb __read_mostly = {
1581         .notifier_call = ocelot_netdevice_event,
1582 };
1583 EXPORT_SYMBOL(ocelot_netdevice_nb);
1584
1585 static int ocelot_switchdev_event(struct notifier_block *unused,
1586                                   unsigned long event, void *ptr)
1587 {
1588         struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
1589         int err;
1590
1591         switch (event) {
1592         case SWITCHDEV_PORT_ATTR_SET:
1593                 err = switchdev_handle_port_attr_set(dev, ptr,
1594                                                      ocelot_netdevice_dev_check,
1595                                                      ocelot_port_attr_set);
1596                 return notifier_from_errno(err);
1597         }
1598
1599         return NOTIFY_DONE;
1600 }
1601
1602 struct notifier_block ocelot_switchdev_nb __read_mostly = {
1603         .notifier_call = ocelot_switchdev_event,
1604 };
1605 EXPORT_SYMBOL(ocelot_switchdev_nb);
1606
1607 static int ocelot_switchdev_blocking_event(struct notifier_block *unused,
1608                                            unsigned long event, void *ptr)
1609 {
1610         struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
1611         int err;
1612
1613         switch (event) {
1614                 /* Blocking events. */
1615         case SWITCHDEV_PORT_OBJ_ADD:
1616                 err = switchdev_handle_port_obj_add(dev, ptr,
1617                                                     ocelot_netdevice_dev_check,
1618                                                     ocelot_port_obj_add);
1619                 return notifier_from_errno(err);
1620         case SWITCHDEV_PORT_OBJ_DEL:
1621                 err = switchdev_handle_port_obj_del(dev, ptr,
1622                                                     ocelot_netdevice_dev_check,
1623                                                     ocelot_port_obj_del);
1624                 return notifier_from_errno(err);
1625         case SWITCHDEV_PORT_ATTR_SET:
1626                 err = switchdev_handle_port_attr_set(dev, ptr,
1627                                                      ocelot_netdevice_dev_check,
1628                                                      ocelot_port_attr_set);
1629                 return notifier_from_errno(err);
1630         }
1631
1632         return NOTIFY_DONE;
1633 }
1634
1635 struct notifier_block ocelot_switchdev_blocking_nb __read_mostly = {
1636         .notifier_call = ocelot_switchdev_blocking_event,
1637 };
1638 EXPORT_SYMBOL(ocelot_switchdev_blocking_nb);
1639
1640 int ocelot_probe_port(struct ocelot *ocelot, u8 port,
1641                       void __iomem *regs,
1642                       struct phy_device *phy)
1643 {
1644         struct ocelot_port *ocelot_port;
1645         struct net_device *dev;
1646         int err;
1647
1648         dev = alloc_etherdev(sizeof(struct ocelot_port));
1649         if (!dev)
1650                 return -ENOMEM;
1651         SET_NETDEV_DEV(dev, ocelot->dev);
1652         ocelot_port = netdev_priv(dev);
1653         ocelot_port->dev = dev;
1654         ocelot_port->ocelot = ocelot;
1655         ocelot_port->regs = regs;
1656         ocelot_port->chip_port = port;
1657         ocelot_port->phy = phy;
1658         ocelot->ports[port] = ocelot_port;
1659
1660         dev->netdev_ops = &ocelot_port_netdev_ops;
1661         dev->ethtool_ops = &ocelot_ethtool_ops;
1662
1663         dev->hw_features |= NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_RXFCS |
1664                 NETIF_F_HW_TC;
1665         dev->features |= NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_HW_TC;
1666
1667         memcpy(dev->dev_addr, ocelot->base_mac, ETH_ALEN);
1668         dev->dev_addr[ETH_ALEN - 1] += port;
1669         ocelot_mact_learn(ocelot, PGID_CPU, dev->dev_addr, ocelot_port->pvid,
1670                           ENTRYTYPE_LOCKED);
1671
1672         err = register_netdev(dev);
1673         if (err) {
1674                 dev_err(ocelot->dev, "register_netdev failed\n");
1675                 goto err_register_netdev;
1676         }
1677
1678         /* Basic L2 initialization */
1679         ocelot_vlan_port_apply(ocelot, ocelot_port);
1680
1681         /* Enable vcap lookups */
1682         ocelot_vcap_enable(ocelot, ocelot_port);
1683
1684         return 0;
1685
1686 err_register_netdev:
1687         free_netdev(dev);
1688         return err;
1689 }
1690 EXPORT_SYMBOL(ocelot_probe_port);
1691
1692 int ocelot_init(struct ocelot *ocelot)
1693 {
1694         u32 port;
1695         int i, cpu = ocelot->num_phys_ports;
1696         char queue_name[32];
1697
1698         ocelot->lags = devm_kcalloc(ocelot->dev, ocelot->num_phys_ports,
1699                                     sizeof(u32), GFP_KERNEL);
1700         if (!ocelot->lags)
1701                 return -ENOMEM;
1702
1703         ocelot->stats = devm_kcalloc(ocelot->dev,
1704                                      ocelot->num_phys_ports * ocelot->num_stats,
1705                                      sizeof(u64), GFP_KERNEL);
1706         if (!ocelot->stats)
1707                 return -ENOMEM;
1708
1709         mutex_init(&ocelot->stats_lock);
1710         snprintf(queue_name, sizeof(queue_name), "%s-stats",
1711                  dev_name(ocelot->dev));
1712         ocelot->stats_queue = create_singlethread_workqueue(queue_name);
1713         if (!ocelot->stats_queue)
1714                 return -ENOMEM;
1715
1716         ocelot_mact_init(ocelot);
1717         ocelot_vlan_init(ocelot);
1718         ocelot_ace_init(ocelot);
1719
1720         for (port = 0; port < ocelot->num_phys_ports; port++) {
1721                 /* Clear all counters (5 groups) */
1722                 ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(port) |
1723                                      SYS_STAT_CFG_STAT_CLEAR_SHOT(0x7f),
1724                              SYS_STAT_CFG);
1725         }
1726
1727         /* Only use S-Tag */
1728         ocelot_write(ocelot, ETH_P_8021AD, SYS_VLAN_ETYPE_CFG);
1729
1730         /* Aggregation mode */
1731         ocelot_write(ocelot, ANA_AGGR_CFG_AC_SMAC_ENA |
1732                              ANA_AGGR_CFG_AC_DMAC_ENA |
1733                              ANA_AGGR_CFG_AC_IP4_SIPDIP_ENA |
1734                              ANA_AGGR_CFG_AC_IP4_TCPUDP_ENA, ANA_AGGR_CFG);
1735
1736         /* Set MAC age time to default value. The entry is aged after
1737          * 2*AGE_PERIOD
1738          */
1739         ocelot_write(ocelot,
1740                      ANA_AUTOAGE_AGE_PERIOD(BR_DEFAULT_AGEING_TIME / 2 / HZ),
1741                      ANA_AUTOAGE);
1742
1743         /* Disable learning for frames discarded by VLAN ingress filtering */
1744         regmap_field_write(ocelot->regfields[ANA_ADVLEARN_VLAN_CHK], 1);
1745
1746         /* Setup frame ageing - fixed value "2 sec" - in 6.5 us units */
1747         ocelot_write(ocelot, SYS_FRM_AGING_AGE_TX_ENA |
1748                      SYS_FRM_AGING_MAX_AGE(307692), SYS_FRM_AGING);
1749
1750         /* Setup flooding PGIDs */
1751         ocelot_write_rix(ocelot, ANA_FLOODING_FLD_MULTICAST(PGID_MC) |
1752                          ANA_FLOODING_FLD_BROADCAST(PGID_MC) |
1753                          ANA_FLOODING_FLD_UNICAST(PGID_UC),
1754                          ANA_FLOODING, 0);
1755         ocelot_write(ocelot, ANA_FLOODING_IPMC_FLD_MC6_DATA(PGID_MCIPV6) |
1756                      ANA_FLOODING_IPMC_FLD_MC6_CTRL(PGID_MC) |
1757                      ANA_FLOODING_IPMC_FLD_MC4_DATA(PGID_MCIPV4) |
1758                      ANA_FLOODING_IPMC_FLD_MC4_CTRL(PGID_MC),
1759                      ANA_FLOODING_IPMC);
1760
1761         for (port = 0; port < ocelot->num_phys_ports; port++) {
1762                 /* Transmit the frame to the local port. */
1763                 ocelot_write_rix(ocelot, BIT(port), ANA_PGID_PGID, port);
1764                 /* Do not forward BPDU frames to the front ports. */
1765                 ocelot_write_gix(ocelot,
1766                                  ANA_PORT_CPU_FWD_BPDU_CFG_BPDU_REDIR_ENA(0xffff),
1767                                  ANA_PORT_CPU_FWD_BPDU_CFG,
1768                                  port);
1769                 /* Ensure bridging is disabled */
1770                 ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_SRC + port);
1771         }
1772
1773         /* Configure and enable the CPU port. */
1774         ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, cpu);
1775         ocelot_write_rix(ocelot, BIT(cpu), ANA_PGID_PGID, PGID_CPU);
1776         ocelot_write_gix(ocelot, ANA_PORT_PORT_CFG_RECV_ENA |
1777                          ANA_PORT_PORT_CFG_PORTID_VAL(cpu),
1778                          ANA_PORT_PORT_CFG, cpu);
1779
1780         /* Allow broadcast MAC frames. */
1781         for (i = ocelot->num_phys_ports + 1; i < PGID_CPU; i++) {
1782                 u32 val = ANA_PGID_PGID_PGID(GENMASK(ocelot->num_phys_ports - 1, 0));
1783
1784                 ocelot_write_rix(ocelot, val, ANA_PGID_PGID, i);
1785         }
1786         ocelot_write_rix(ocelot,
1787                          ANA_PGID_PGID_PGID(GENMASK(ocelot->num_phys_ports, 0)),
1788                          ANA_PGID_PGID, PGID_MC);
1789         ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_MCIPV4);
1790         ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_MCIPV6);
1791
1792         /* CPU port Injection/Extraction configuration */
1793         ocelot_write_rix(ocelot, QSYS_SWITCH_PORT_MODE_INGRESS_DROP_MODE |
1794                          QSYS_SWITCH_PORT_MODE_SCH_NEXT_CFG(1) |
1795                          QSYS_SWITCH_PORT_MODE_PORT_ENA,
1796                          QSYS_SWITCH_PORT_MODE, cpu);
1797         ocelot_write_rix(ocelot, SYS_PORT_MODE_INCL_XTR_HDR(1) |
1798                          SYS_PORT_MODE_INCL_INJ_HDR(1), SYS_PORT_MODE, cpu);
1799         /* Allow manual injection via DEVCPU_QS registers, and byte swap these
1800          * registers endianness.
1801          */
1802         ocelot_write_rix(ocelot, QS_INJ_GRP_CFG_BYTE_SWAP |
1803                          QS_INJ_GRP_CFG_MODE(1), QS_INJ_GRP_CFG, 0);
1804         ocelot_write_rix(ocelot, QS_XTR_GRP_CFG_BYTE_SWAP |
1805                          QS_XTR_GRP_CFG_MODE(1), QS_XTR_GRP_CFG, 0);
1806         ocelot_write(ocelot, ANA_CPUQ_CFG_CPUQ_MIRROR(2) |
1807                      ANA_CPUQ_CFG_CPUQ_LRN(2) |
1808                      ANA_CPUQ_CFG_CPUQ_MAC_COPY(2) |
1809                      ANA_CPUQ_CFG_CPUQ_SRC_COPY(2) |
1810                      ANA_CPUQ_CFG_CPUQ_LOCKED_PORTMOVE(2) |
1811                      ANA_CPUQ_CFG_CPUQ_ALLBRIDGE(6) |
1812                      ANA_CPUQ_CFG_CPUQ_IPMC_CTRL(6) |
1813                      ANA_CPUQ_CFG_CPUQ_IGMP(6) |
1814                      ANA_CPUQ_CFG_CPUQ_MLD(6), ANA_CPUQ_CFG);
1815         for (i = 0; i < 16; i++)
1816                 ocelot_write_rix(ocelot, ANA_CPUQ_8021_CFG_CPUQ_GARP_VAL(6) |
1817                                  ANA_CPUQ_8021_CFG_CPUQ_BPDU_VAL(6),
1818                                  ANA_CPUQ_8021_CFG, i);
1819
1820         INIT_DELAYED_WORK(&ocelot->stats_work, ocelot_check_stats_work);
1821         queue_delayed_work(ocelot->stats_queue, &ocelot->stats_work,
1822                            OCELOT_STATS_CHECK_DELAY);
1823         return 0;
1824 }
1825 EXPORT_SYMBOL(ocelot_init);
1826
1827 void ocelot_deinit(struct ocelot *ocelot)
1828 {
1829         cancel_delayed_work(&ocelot->stats_work);
1830         destroy_workqueue(ocelot->stats_queue);
1831         mutex_destroy(&ocelot->stats_lock);
1832         ocelot_ace_deinit();
1833 }
1834 EXPORT_SYMBOL(ocelot_deinit);
1835
1836 MODULE_LICENSE("Dual MIT/GPL");