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/ptp_clock_kernel.h>
  18 #include <linux/skbuff.h>
  19 #include <linux/iopoll.h>
  20 #include <net/arp.h>
  21 #include <net/netevent.h>
  22 #include <net/rtnetlink.h>
  23 #include <net/switchdev.h>
  24 #include <net/dsa.h>
  25
  26 #include "ocelot.h"
  27 #include "ocelot_ace.h"
  28
  29 #define TABLE_UPDATE_SLEEP_US 10
  30 #define TABLE_UPDATE_TIMEOUT_US 100000
  31
  32 /* MAC table entry types.
  33  * ENTRYTYPE_NORMAL is subject to aging.
  34  * ENTRYTYPE_LOCKED is not subject to aging.
  35  * ENTRYTYPE_MACv4 is not subject to aging. For IPv4 multicast.
  36  * ENTRYTYPE_MACv6 is not subject to aging. For IPv6 multicast.
  37  */
  38 enum macaccess_entry_type {
  39         ENTRYTYPE_NORMAL = 0,
  40         ENTRYTYPE_LOCKED,
  41         ENTRYTYPE_MACv4,
  42         ENTRYTYPE_MACv6,
  43 };
  44
  45 struct ocelot_mact_entry {
  46         u8 mac[ETH_ALEN];
  47         u16 vid;
  48         enum macaccess_entry_type type;
  49 };
  50
  51 static inline u32 ocelot_mact_read_macaccess(struct ocelot *ocelot)
  52 {
  53         return ocelot_read(ocelot, ANA_TABLES_MACACCESS);
  54 }
  55
  56 static inline int ocelot_mact_wait_for_completion(struct ocelot *ocelot)
  57 {
  58         u32 val;
  59
  60         return readx_poll_timeout(ocelot_mact_read_macaccess,
  61                 ocelot, val,
  62                 (val & ANA_TABLES_MACACCESS_MAC_TABLE_CMD_M) ==
  63                 MACACCESS_CMD_IDLE,
  64                 TABLE_UPDATE_SLEEP_US, TABLE_UPDATE_TIMEOUT_US);
  65 }
  66
  67 static void ocelot_mact_select(struct ocelot *ocelot,
  68                                const unsigned char mac[ETH_ALEN],
  69                                unsigned int vid)
  70 {
  71         u32 macl = 0, mach = 0;
  72
  73         /* Set the MAC address to handle and the vlan associated in a format
  74          * understood by the hardware.
  75          */
  76         mach |= vid    << 16;
  77         mach |= mac[0] << 8;
  78         mach |= mac[1] << 0;
  79         macl |= mac[2] << 24;
  80         macl |= mac[3] << 16;
  81         macl |= mac[4] << 8;
  82         macl |= mac[5] << 0;
  83
  84         ocelot_write(ocelot, macl, ANA_TABLES_MACLDATA);
  85         ocelot_write(ocelot, mach, ANA_TABLES_MACHDATA);
  86
  87 }
  88
  89 static int ocelot_mact_learn(struct ocelot *ocelot, int port,
  90                              const unsigned char mac[ETH_ALEN],
  91                              unsigned int vid,
  92                              enum macaccess_entry_type type)
  93 {
  94         ocelot_mact_select(ocelot, mac, vid);
  95
  96         /* Issue a write command */
  97         ocelot_write(ocelot, ANA_TABLES_MACACCESS_VALID |
  98                              ANA_TABLES_MACACCESS_DEST_IDX(port) |
  99                              ANA_TABLES_MACACCESS_ENTRYTYPE(type) |
 100                              ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_LEARN),
 101                              ANA_TABLES_MACACCESS);
 102
 103         return ocelot_mact_wait_for_completion(ocelot);
 104 }
 105
 106 static int ocelot_mact_forget(struct ocelot *ocelot,
 107                               const unsigned char mac[ETH_ALEN],
 108                               unsigned int vid)
 109 {
 110         ocelot_mact_select(ocelot, mac, vid);
 111
 112         /* Issue a forget command */
 113         ocelot_write(ocelot,
 114                      ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_FORGET),
 115                      ANA_TABLES_MACACCESS);
 116
 117         return ocelot_mact_wait_for_completion(ocelot);
 118 }
 119
 120 static void ocelot_mact_init(struct ocelot *ocelot)
 121 {
 122         /* Configure the learning mode entries attributes:
 123          * - Do not copy the frame to the CPU extraction queues.
 124          * - Use the vlan and mac_cpoy for dmac lookup.
 125          */
 126         ocelot_rmw(ocelot, 0,
 127                    ANA_AGENCTRL_LEARN_CPU_COPY | ANA_AGENCTRL_IGNORE_DMAC_FLAGS
 128                    | ANA_AGENCTRL_LEARN_FWD_KILL
 129                    | ANA_AGENCTRL_LEARN_IGNORE_VLAN,
 130                    ANA_AGENCTRL);
 131
 132         /* Clear the MAC table */
 133         ocelot_write(ocelot, MACACCESS_CMD_INIT, ANA_TABLES_MACACCESS);
 134 }
 135
 136 static void ocelot_vcap_enable(struct ocelot *ocelot, int port)
 137 {
 138         ocelot_write_gix(ocelot, ANA_PORT_VCAP_S2_CFG_S2_ENA |
 139                          ANA_PORT_VCAP_S2_CFG_S2_IP6_CFG(0xa),
 140                          ANA_PORT_VCAP_S2_CFG, port);
 141 }
 142
 143 static inline u32 ocelot_vlant_read_vlanaccess(struct ocelot *ocelot)
 144 {
 145         return ocelot_read(ocelot, ANA_TABLES_VLANACCESS);
 146 }
 147
 148 static inline int ocelot_vlant_wait_for_completion(struct ocelot *ocelot)
 149 {
 150         u32 val;
 151
 152         return readx_poll_timeout(ocelot_vlant_read_vlanaccess,
 153                 ocelot,
 154                 val,
 155                 (val & ANA_TABLES_VLANACCESS_VLAN_TBL_CMD_M) ==
 156                 ANA_TABLES_VLANACCESS_CMD_IDLE,
 157                 TABLE_UPDATE_SLEEP_US, TABLE_UPDATE_TIMEOUT_US);
 158 }
 159
 160 static int ocelot_vlant_set_mask(struct ocelot *ocelot, u16 vid, u32 mask)
 161 {
 162         /* Select the VID to configure */
 163         ocelot_write(ocelot, ANA_TABLES_VLANTIDX_V_INDEX(vid),
 164                      ANA_TABLES_VLANTIDX);
 165         /* Set the vlan port members mask and issue a write command */
 166         ocelot_write(ocelot, ANA_TABLES_VLANACCESS_VLAN_PORT_MASK(mask) |
 167                              ANA_TABLES_VLANACCESS_CMD_WRITE,
 168                      ANA_TABLES_VLANACCESS);
 169
 170         return ocelot_vlant_wait_for_completion(ocelot);
 171 }
 172
 173 static void ocelot_vlan_mode(struct ocelot *ocelot, int port,
 174                              netdev_features_t features)
 175 {
 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(port);
 182         else
 183                 val &= ~BIT(port);
 184         ocelot_write(ocelot, val, ANA_VLANMASK);
 185 }
 186
 187 static void ocelot_port_vlan_filtering(struct ocelot *ocelot, int port,
 188                                        bool vlan_aware)
 189 {
 190         struct ocelot_port *ocelot_port = ocelot->ports[port];
 191         u32 val;
 192
 193         if (vlan_aware)
 194                 val = ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
 195                       ANA_PORT_VLAN_CFG_VLAN_POP_CNT(1);
 196         else
 197                 val = 0;
 198         ocelot_rmw_gix(ocelot, val,
 199                        ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
 200                        ANA_PORT_VLAN_CFG_VLAN_POP_CNT_M,
 201                        ANA_PORT_VLAN_CFG, port);
 202
 203         if (vlan_aware && !ocelot_port->vid)
 204                 /* If port is vlan-aware and tagged, drop untagged and priority
 205                  * tagged frames.
 206                  */
 207                 val = ANA_PORT_DROP_CFG_DROP_UNTAGGED_ENA |
 208                       ANA_PORT_DROP_CFG_DROP_PRIO_S_TAGGED_ENA |
 209                       ANA_PORT_DROP_CFG_DROP_PRIO_C_TAGGED_ENA;
 210         else
 211                 val = 0;
 212         ocelot_rmw_gix(ocelot, val,
 213                        ANA_PORT_DROP_CFG_DROP_UNTAGGED_ENA |
 214                        ANA_PORT_DROP_CFG_DROP_PRIO_S_TAGGED_ENA |
 215                        ANA_PORT_DROP_CFG_DROP_PRIO_C_TAGGED_ENA,
 216                        ANA_PORT_DROP_CFG, port);
 217
 218         if (vlan_aware) {
 219                 if (ocelot_port->vid)
 220                         /* Tag all frames except when VID == DEFAULT_VLAN */
 221                         val |= REW_TAG_CFG_TAG_CFG(1);
 222                 else
 223                         /* Tag all frames */
 224                         val |= REW_TAG_CFG_TAG_CFG(3);
 225         } else {
 226                 /* Port tagging disabled. */
 227                 val = REW_TAG_CFG_TAG_CFG(0);
 228         }
 229         ocelot_rmw_gix(ocelot, val,
 230                        REW_TAG_CFG_TAG_CFG_M,
 231                        REW_TAG_CFG, port);
 232 }
 233
 234 static int ocelot_port_set_native_vlan(struct ocelot *ocelot, int port,
 235                                        u16 vid)
 236 {
 237         struct ocelot_port *ocelot_port = ocelot->ports[port];
 238
 239         if (ocelot_port->vid != vid) {
 240                 /* Always permit deleting the native VLAN (vid = 0) */
 241                 if (ocelot_port->vid && vid) {
 242                         dev_err(ocelot->dev,
 243                                 "Port already has a native VLAN: %d\n",
 244                                 ocelot_port->vid);
 245                         return -EBUSY;
 246                 }
 247                 ocelot_port->vid = vid;
 248         }
 249
 250         ocelot_rmw_gix(ocelot, REW_PORT_VLAN_CFG_PORT_VID(vid),
 251                        REW_PORT_VLAN_CFG_PORT_VID_M,
 252                        REW_PORT_VLAN_CFG, port);
 253
 254         return 0;
 255 }
 256
 257 /* Default vlan to clasify for untagged frames (may be zero) */
 258 static void ocelot_port_set_pvid(struct ocelot *ocelot, int port, u16 pvid)
 259 {
 260         struct ocelot_port *ocelot_port = ocelot->ports[port];
 261
 262         ocelot_rmw_gix(ocelot,
 263                        ANA_PORT_VLAN_CFG_VLAN_VID(pvid),
 264                        ANA_PORT_VLAN_CFG_VLAN_VID_M,
 265                        ANA_PORT_VLAN_CFG, port);
 266
 267         ocelot_port->pvid = pvid;
 268 }
 269
 270 static int ocelot_vlan_add(struct ocelot *ocelot, int port, u16 vid, bool pvid,
 271                            bool untagged)
 272 {
 273         int ret;
 274
 275         /* Make the port a member of the VLAN */
 276         ocelot->vlan_mask[vid] |= BIT(port);
 277         ret = ocelot_vlant_set_mask(ocelot, vid, ocelot->vlan_mask[vid]);
 278         if (ret)
 279                 return ret;
 280
 281         /* Default ingress vlan classification */
 282         if (pvid)
 283                 ocelot_port_set_pvid(ocelot, port, vid);
 284
 285         /* Untagged egress vlan clasification */
 286         if (untagged) {
 287                 ret = ocelot_port_set_native_vlan(ocelot, port, vid);
 288                 if (ret)
 289                         return ret;
 290         }
 291
 292         return 0;
 293 }
 294
 295 static int ocelot_vlan_vid_add(struct net_device *dev, u16 vid, bool pvid,
 296                                bool untagged)
 297 {
 298         struct ocelot_port_private *priv = netdev_priv(dev);
 299         struct ocelot_port *ocelot_port = &priv->port;
 300         struct ocelot *ocelot = ocelot_port->ocelot;
 301         int port = priv->chip_port;
 302         int ret;
 303
 304         ret = ocelot_vlan_add(ocelot, port, vid, pvid, untagged);
 305         if (ret)
 306                 return ret;
 307
 308         /* Add the port MAC address to with the right VLAN information */
 309         ocelot_mact_learn(ocelot, PGID_CPU, dev->dev_addr, vid,
 310                           ENTRYTYPE_LOCKED);
 311
 312         return 0;
 313 }
 314
 315 static int ocelot_vlan_del(struct ocelot *ocelot, int port, u16 vid)
 316 {
 317         struct ocelot_port *ocelot_port = ocelot->ports[port];
 318         int ret;
 319
 320         /* Stop the port from being a member of the vlan */
 321         ocelot->vlan_mask[vid] &= ~BIT(port);
 322         ret = ocelot_vlant_set_mask(ocelot, vid, ocelot->vlan_mask[vid]);
 323         if (ret)
 324                 return ret;
 325
 326         /* Ingress */
 327         if (ocelot_port->pvid == vid)
 328                 ocelot_port_set_pvid(ocelot, port, 0);
 329
 330         /* Egress */
 331         if (ocelot_port->vid == vid)
 332                 ocelot_port_set_native_vlan(ocelot, port, 0);
 333
 334         return 0;
 335 }
 336
 337 static int ocelot_vlan_vid_del(struct net_device *dev, u16 vid)
 338 {
 339         struct ocelot_port_private *priv = netdev_priv(dev);
 340         struct ocelot *ocelot = priv->port.ocelot;
 341         int port = priv->chip_port;
 342         int ret;
 343
 344         /* 8021q removes VID 0 on module unload for all interfaces
 345          * with VLAN filtering feature. We need to keep it to receive
 346          * untagged traffic.
 347          */
 348         if (vid == 0)
 349                 return 0;
 350
 351         ret = ocelot_vlan_del(ocelot, port, vid);
 352         if (ret)
 353                 return ret;
 354
 355         /* Del the port MAC address to with the right VLAN information */
 356         ocelot_mact_forget(ocelot, dev->dev_addr, vid);
 357
 358         return 0;
 359 }
 360
 361 static void ocelot_vlan_init(struct ocelot *ocelot)
 362 {
 363         u16 port, vid;
 364
 365         /* Clear VLAN table, by default all ports are members of all VLANs */
 366         ocelot_write(ocelot, ANA_TABLES_VLANACCESS_CMD_INIT,
 367                      ANA_TABLES_VLANACCESS);
 368         ocelot_vlant_wait_for_completion(ocelot);
 369
 370         /* Configure the port VLAN memberships */
 371         for (vid = 1; vid < VLAN_N_VID; vid++) {
 372                 ocelot->vlan_mask[vid] = 0;
 373                 ocelot_vlant_set_mask(ocelot, vid, ocelot->vlan_mask[vid]);
 374         }
 375
 376         /* Because VLAN filtering is enabled, we need VID 0 to get untagged
 377          * traffic.  It is added automatically if 8021q module is loaded, but
 378          * we can't rely on it since module may be not loaded.
 379          */
 380         ocelot->vlan_mask[0] = GENMASK(ocelot->num_phys_ports - 1, 0);
 381         ocelot_vlant_set_mask(ocelot, 0, ocelot->vlan_mask[0]);
 382
 383         /* Configure the CPU port to be VLAN aware */
 384         ocelot_write_gix(ocelot, ANA_PORT_VLAN_CFG_VLAN_VID(0) |
 385                                  ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
 386                                  ANA_PORT_VLAN_CFG_VLAN_POP_CNT(1),
 387                          ANA_PORT_VLAN_CFG, ocelot->num_phys_ports);
 388
 389         /* Set vlan ingress filter mask to all ports but the CPU port by
 390          * default.
 391          */
 392         ocelot_write(ocelot, GENMASK(9, 0), ANA_VLANMASK);
 393
 394         for (port = 0; port < ocelot->num_phys_ports; port++) {
 395                 ocelot_write_gix(ocelot, 0, REW_PORT_VLAN_CFG, port);
 396                 ocelot_write_gix(ocelot, 0, REW_TAG_CFG, port);
 397         }
 398 }
 399
 400 /* Watermark encode
 401  * Bit 8:   Unit; 0:1, 1:16
 402  * Bit 7-0: Value to be multiplied with unit
 403  */
 404 static u16 ocelot_wm_enc(u16 value)
 405 {
 406         if (value >= BIT(8))
 407                 return BIT(8) | (value / 16);
 408
 409         return value;
 410 }
 411
 412 static void ocelot_port_adjust_link(struct net_device *dev)
 413 {
 414         struct ocelot_port_private *priv = netdev_priv(dev);
 415         struct ocelot_port *ocelot_port = &priv->port;
 416         struct ocelot *ocelot = ocelot_port->ocelot;
 417         int speed, atop_wm, mode = 0;
 418         u8 port = priv->chip_port;
 419
 420         switch (dev->phydev->speed) {
 421         case SPEED_10:
 422                 speed = OCELOT_SPEED_10;
 423                 break;
 424         case SPEED_100:
 425                 speed = OCELOT_SPEED_100;
 426                 break;
 427         case SPEED_1000:
 428                 speed = OCELOT_SPEED_1000;
 429                 mode = DEV_MAC_MODE_CFG_GIGA_MODE_ENA;
 430                 break;
 431         case SPEED_2500:
 432                 speed = OCELOT_SPEED_2500;
 433                 mode = DEV_MAC_MODE_CFG_GIGA_MODE_ENA;
 434                 break;
 435         default:
 436                 netdev_err(dev, "Unsupported PHY speed: %d\n",
 437                            dev->phydev->speed);
 438                 return;
 439         }
 440
 441         phy_print_status(dev->phydev);
 442
 443         if (!dev->phydev->link)
 444                 return;
 445
 446         /* Only full duplex supported for now */
 447         ocelot_port_writel(ocelot_port, DEV_MAC_MODE_CFG_FDX_ENA |
 448                            mode, DEV_MAC_MODE_CFG);
 449
 450         /* Set MAC IFG Gaps
 451          * FDX: TX_IFG = 5, RX_IFG1 = RX_IFG2 = 0
 452          * !FDX: TX_IFG = 5, RX_IFG1 = RX_IFG2 = 5
 453          */
 454         ocelot_port_writel(ocelot_port, DEV_MAC_IFG_CFG_TX_IFG(5),
 455                            DEV_MAC_IFG_CFG);
 456
 457         /* Load seed (0) and set MAC HDX late collision  */
 458         ocelot_port_writel(ocelot_port, DEV_MAC_HDX_CFG_LATE_COL_POS(67) |
 459                            DEV_MAC_HDX_CFG_SEED_LOAD,
 460                            DEV_MAC_HDX_CFG);
 461         mdelay(1);
 462         ocelot_port_writel(ocelot_port, DEV_MAC_HDX_CFG_LATE_COL_POS(67),
 463                            DEV_MAC_HDX_CFG);
 464
 465         /* Disable HDX fast control */
 466         ocelot_port_writel(ocelot_port, DEV_PORT_MISC_HDX_FAST_DIS,
 467                            DEV_PORT_MISC);
 468
 469         /* SGMII only for now */
 470         ocelot_port_writel(ocelot_port, PCS1G_MODE_CFG_SGMII_MODE_ENA,
 471                            PCS1G_MODE_CFG);
 472         ocelot_port_writel(ocelot_port, PCS1G_SD_CFG_SD_SEL, PCS1G_SD_CFG);
 473
 474         /* Enable PCS */
 475         ocelot_port_writel(ocelot_port, PCS1G_CFG_PCS_ENA, PCS1G_CFG);
 476
 477         /* No aneg on SGMII */
 478         ocelot_port_writel(ocelot_port, 0, PCS1G_ANEG_CFG);
 479
 480         /* No loopback */
 481         ocelot_port_writel(ocelot_port, 0, PCS1G_LB_CFG);
 482
 483         /* Set Max Length and maximum tags allowed */
 484         ocelot_port_writel(ocelot_port, VLAN_ETH_FRAME_LEN,
 485                            DEV_MAC_MAXLEN_CFG);
 486         ocelot_port_writel(ocelot_port, DEV_MAC_TAGS_CFG_TAG_ID(ETH_P_8021AD) |
 487                            DEV_MAC_TAGS_CFG_VLAN_AWR_ENA |
 488                            DEV_MAC_TAGS_CFG_VLAN_LEN_AWR_ENA,
 489                            DEV_MAC_TAGS_CFG);
 490
 491         /* Enable MAC module */
 492         ocelot_port_writel(ocelot_port, DEV_MAC_ENA_CFG_RX_ENA |
 493                            DEV_MAC_ENA_CFG_TX_ENA, DEV_MAC_ENA_CFG);
 494
 495         /* Take MAC, Port, Phy (intern) and PCS (SGMII/Serdes) clock out of
 496          * reset */
 497         ocelot_port_writel(ocelot_port, DEV_CLOCK_CFG_LINK_SPEED(speed),
 498                            DEV_CLOCK_CFG);
 499
 500         /* Set SMAC of Pause frame (00:00:00:00:00:00) */
 501         ocelot_port_writel(ocelot_port, 0, DEV_MAC_FC_MAC_HIGH_CFG);
 502         ocelot_port_writel(ocelot_port, 0, DEV_MAC_FC_MAC_LOW_CFG);
 503
 504         /* No PFC */
 505         ocelot_write_gix(ocelot, ANA_PFC_PFC_CFG_FC_LINK_SPEED(speed),
 506                          ANA_PFC_PFC_CFG, port);
 507
 508         /* Set Pause WM hysteresis
 509          * 152 = 6 * VLAN_ETH_FRAME_LEN / OCELOT_BUFFER_CELL_SZ
 510          * 101 = 4 * VLAN_ETH_FRAME_LEN / OCELOT_BUFFER_CELL_SZ
 511          */
 512         ocelot_write_rix(ocelot, SYS_PAUSE_CFG_PAUSE_ENA |
 513                          SYS_PAUSE_CFG_PAUSE_STOP(101) |
 514                          SYS_PAUSE_CFG_PAUSE_START(152), SYS_PAUSE_CFG, port);
 515
 516         /* Core: Enable port for frame transfer */
 517         ocelot_write_rix(ocelot, QSYS_SWITCH_PORT_MODE_INGRESS_DROP_MODE |
 518                          QSYS_SWITCH_PORT_MODE_SCH_NEXT_CFG(1) |
 519                          QSYS_SWITCH_PORT_MODE_PORT_ENA,
 520                          QSYS_SWITCH_PORT_MODE, port);
 521
 522         /* Flow control */
 523         ocelot_write_rix(ocelot, SYS_MAC_FC_CFG_PAUSE_VAL_CFG(0xffff) |
 524                          SYS_MAC_FC_CFG_RX_FC_ENA | SYS_MAC_FC_CFG_TX_FC_ENA |
 525                          SYS_MAC_FC_CFG_ZERO_PAUSE_ENA |
 526                          SYS_MAC_FC_CFG_FC_LATENCY_CFG(0x7) |
 527                          SYS_MAC_FC_CFG_FC_LINK_SPEED(speed),
 528                          SYS_MAC_FC_CFG, port);
 529         ocelot_write_rix(ocelot, 0, ANA_POL_FLOWC, port);
 530
 531         /* Tail dropping watermark */
 532         atop_wm = (ocelot->shared_queue_sz - 9 * VLAN_ETH_FRAME_LEN) / OCELOT_BUFFER_CELL_SZ;
 533         ocelot_write_rix(ocelot, ocelot_wm_enc(9 * VLAN_ETH_FRAME_LEN),
 534                          SYS_ATOP, port);
 535         ocelot_write(ocelot, ocelot_wm_enc(atop_wm), SYS_ATOP_TOT_CFG);
 536 }
 537
 538 static int ocelot_port_open(struct net_device *dev)
 539 {
 540         struct ocelot_port_private *priv = netdev_priv(dev);
 541         struct ocelot *ocelot = priv->port.ocelot;
 542         int port = priv->chip_port;
 543         int err;
 544
 545         /* Enable receiving frames on the port, and activate auto-learning of
 546          * MAC addresses.
 547          */
 548         ocelot_write_gix(ocelot, ANA_PORT_PORT_CFG_LEARNAUTO |
 549                          ANA_PORT_PORT_CFG_RECV_ENA |
 550                          ANA_PORT_PORT_CFG_PORTID_VAL(port),
 551                          ANA_PORT_PORT_CFG, port);
 552
 553         if (priv->serdes) {
 554                 err = phy_set_mode_ext(priv->serdes, PHY_MODE_ETHERNET,
 555                                        priv->phy_mode);
 556                 if (err) {
 557                         netdev_err(dev, "Could not set mode of SerDes\n");
 558                         return err;
 559                 }
 560         }
 561
 562         err = phy_connect_direct(dev, priv->phy, &ocelot_port_adjust_link,
 563                                  priv->phy_mode);
 564         if (err) {
 565                 netdev_err(dev, "Could not attach to PHY\n");
 566                 return err;
 567         }
 568
 569         dev->phydev = priv->phy;
 570
 571         phy_attached_info(priv->phy);
 572         phy_start(priv->phy);
 573         return 0;
 574 }
 575
 576 static int ocelot_port_stop(struct net_device *dev)
 577 {
 578         struct ocelot_port_private *priv = netdev_priv(dev);
 579         struct ocelot_port *port = &priv->port;
 580
 581         phy_disconnect(priv->phy);
 582
 583         dev->phydev = NULL;
 584
 585         ocelot_port_writel(port, 0, DEV_MAC_ENA_CFG);
 586         ocelot_rmw_rix(port->ocelot, 0, QSYS_SWITCH_PORT_MODE_PORT_ENA,
 587                        QSYS_SWITCH_PORT_MODE, priv->chip_port);
 588         return 0;
 589 }
 590
 591 /* Generate the IFH for frame injection
 592  *
 593  * The IFH is a 128bit-value
 594  * bit 127: bypass the analyzer processing
 595  * bit 56-67: destination mask
 596  * bit 28-29: pop_cnt: 3 disables all rewriting of the frame
 597  * bit 20-27: cpu extraction queue mask
 598  * bit 16: tag type 0: C-tag, 1: S-tag
 599  * bit 0-11: VID
 600  */
 601 static int ocelot_gen_ifh(u32 *ifh, struct frame_info *info)
 602 {
 603         ifh[0] = IFH_INJ_BYPASS | ((0x1ff & info->rew_op) << 21);
 604         ifh[1] = (0xf00 & info->port) >> 8;
 605         ifh[2] = (0xff & info->port) << 24;
 606         ifh[3] = (info->tag_type << 16) | info->vid;
 607
 608         return 0;
 609 }
 610
 611 static int ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
 612 {
 613         struct ocelot_port_private *priv = netdev_priv(dev);
 614         struct skb_shared_info *shinfo = skb_shinfo(skb);
 615         struct ocelot_port *ocelot_port = &priv->port;
 616         struct ocelot *ocelot = ocelot_port->ocelot;
 617         struct frame_info info = {};
 618         u8 grp = 0; /* Send everything on CPU group 0 */
 619         unsigned int i, count, last;
 620         int port = priv->chip_port;
 621         u32 val, ifh[IFH_LEN];
 622
 623         val = ocelot_read(ocelot, QS_INJ_STATUS);
 624         if (!(val & QS_INJ_STATUS_FIFO_RDY(BIT(grp))) ||
 625             (val & QS_INJ_STATUS_WMARK_REACHED(BIT(grp))))
 626                 return NETDEV_TX_BUSY;
 627
 628         ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
 629                          QS_INJ_CTRL_SOF, QS_INJ_CTRL, grp);
 630
 631         info.port = BIT(port);
 632         info.tag_type = IFH_TAG_TYPE_C;
 633         info.vid = skb_vlan_tag_get(skb);
 634
 635         /* Check if timestamping is needed */
 636         if (ocelot->ptp && shinfo->tx_flags & SKBTX_HW_TSTAMP) {
 637                 info.rew_op = ocelot_port->ptp_cmd;
 638                 if (ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP)
 639                         info.rew_op |= (ocelot_port->ts_id  % 4) << 3;
 640         }
 641
 642         ocelot_gen_ifh(ifh, &info);
 643
 644         for (i = 0; i < IFH_LEN; i++)
 645                 ocelot_write_rix(ocelot, (__force u32)cpu_to_be32(ifh[i]),
 646                                  QS_INJ_WR, grp);
 647
 648         count = (skb->len + 3) / 4;
 649         last = skb->len % 4;
 650         for (i = 0; i < count; i++) {
 651                 ocelot_write_rix(ocelot, ((u32 *)skb->data)[i], QS_INJ_WR, grp);
 652         }
 653
 654         /* Add padding */
 655         while (i < (OCELOT_BUFFER_CELL_SZ / 4)) {
 656                 ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
 657                 i++;
 658         }
 659
 660         /* Indicate EOF and valid bytes in last word */
 661         ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
 662                          QS_INJ_CTRL_VLD_BYTES(skb->len < OCELOT_BUFFER_CELL_SZ ? 0 : last) |
 663                          QS_INJ_CTRL_EOF,
 664                          QS_INJ_CTRL, grp);
 665
 666         /* Add dummy CRC */
 667         ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
 668         skb_tx_timestamp(skb);
 669
 670         dev->stats.tx_packets++;
 671         dev->stats.tx_bytes += skb->len;
 672
 673         if (ocelot->ptp && shinfo->tx_flags & SKBTX_HW_TSTAMP &&
 674             ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
 675                 struct ocelot_skb *oskb =
 676                         kzalloc(sizeof(struct ocelot_skb), GFP_ATOMIC);
 677
 678                 if (unlikely(!oskb))
 679                         goto out;
 680
 681                 skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
 682
 683                 oskb->skb = skb;
 684                 oskb->id = ocelot_port->ts_id % 4;
 685                 ocelot_port->ts_id++;
 686
 687                 list_add_tail(&oskb->head, &ocelot_port->skbs);
 688
 689                 return NETDEV_TX_OK;
 690         }
 691
 692 out:
 693         dev_kfree_skb_any(skb);
 694         return NETDEV_TX_OK;
 695 }
 696
 697 void ocelot_get_hwtimestamp(struct ocelot *ocelot, struct timespec64 *ts)
 698 {
 699         unsigned long flags;
 700         u32 val;
 701
 702         spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
 703
 704         /* Read current PTP time to get seconds */
 705         val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
 706
 707         val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
 708         val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE);
 709         ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
 710         ts->tv_sec = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
 711
 712         /* Read packet HW timestamp from FIFO */
 713         val = ocelot_read(ocelot, SYS_PTP_TXSTAMP);
 714         ts->tv_nsec = SYS_PTP_TXSTAMP_PTP_TXSTAMP(val);
 715
 716         /* Sec has incremented since the ts was registered */
 717         if ((ts->tv_sec & 0x1) != !!(val & SYS_PTP_TXSTAMP_PTP_TXSTAMP_SEC))
 718                 ts->tv_sec--;
 719
 720         spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
 721 }
 722 EXPORT_SYMBOL(ocelot_get_hwtimestamp);
 723
 724 static int ocelot_mc_unsync(struct net_device *dev, const unsigned char *addr)
 725 {
 726         struct ocelot_port_private *priv = netdev_priv(dev);
 727         struct ocelot_port *ocelot_port = &priv->port;
 728         struct ocelot *ocelot = ocelot_port->ocelot;
 729
 730         return ocelot_mact_forget(ocelot, addr, ocelot_port->pvid);
 731 }
 732
 733 static int ocelot_mc_sync(struct net_device *dev, const unsigned char *addr)
 734 {
 735         struct ocelot_port_private *priv = netdev_priv(dev);
 736         struct ocelot_port *ocelot_port = &priv->port;
 737         struct ocelot *ocelot = ocelot_port->ocelot;
 738
 739         return ocelot_mact_learn(ocelot, PGID_CPU, addr, ocelot_port->pvid,
 740                                  ENTRYTYPE_LOCKED);
 741 }
 742
 743 static void ocelot_set_rx_mode(struct net_device *dev)
 744 {
 745         struct ocelot_port_private *priv = netdev_priv(dev);
 746         struct ocelot *ocelot = priv->port.ocelot;
 747         u32 val;
 748         int i;
 749
 750         /* This doesn't handle promiscuous mode because the bridge core is
 751          * setting IFF_PROMISC on all slave interfaces and all frames would be
 752          * forwarded to the CPU port.
 753          */
 754         val = GENMASK(ocelot->num_phys_ports - 1, 0);
 755         for (i = ocelot->num_phys_ports + 1; i < PGID_CPU; i++)
 756                 ocelot_write_rix(ocelot, val, ANA_PGID_PGID, i);
 757
 758         __dev_mc_sync(dev, ocelot_mc_sync, ocelot_mc_unsync);
 759 }
 760
 761 static int ocelot_port_get_phys_port_name(struct net_device *dev,
 762                                           char *buf, size_t len)
 763 {
 764         struct ocelot_port_private *priv = netdev_priv(dev);
 765         int port = priv->chip_port;
 766         int ret;
 767
 768         ret = snprintf(buf, len, "p%d", port);
 769         if (ret >= len)
 770                 return -EINVAL;
 771
 772         return 0;
 773 }
 774
 775 static int ocelot_port_set_mac_address(struct net_device *dev, void *p)
 776 {
 777         struct ocelot_port_private *priv = netdev_priv(dev);
 778         struct ocelot_port *ocelot_port = &priv->port;
 779         struct ocelot *ocelot = ocelot_port->ocelot;
 780         const struct sockaddr *addr = p;
 781
 782         /* Learn the new net device MAC address in the mac table. */
 783         ocelot_mact_learn(ocelot, PGID_CPU, addr->sa_data, ocelot_port->pvid,
 784                           ENTRYTYPE_LOCKED);
 785         /* Then forget the previous one. */
 786         ocelot_mact_forget(ocelot, dev->dev_addr, ocelot_port->pvid);
 787
 788         ether_addr_copy(dev->dev_addr, addr->sa_data);
 789         return 0;
 790 }
 791
 792 static void ocelot_get_stats64(struct net_device *dev,
 793                                struct rtnl_link_stats64 *stats)
 794 {
 795         struct ocelot_port_private *priv = netdev_priv(dev);
 796         struct ocelot *ocelot = priv->port.ocelot;
 797         int port = priv->chip_port;
 798
 799         /* Configure the port to read the stats from */
 800         ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(port),
 801                      SYS_STAT_CFG);
 802
 803         /* Get Rx stats */
 804         stats->rx_bytes = ocelot_read(ocelot, SYS_COUNT_RX_OCTETS);
 805         stats->rx_packets = ocelot_read(ocelot, SYS_COUNT_RX_SHORTS) +
 806                             ocelot_read(ocelot, SYS_COUNT_RX_FRAGMENTS) +
 807                             ocelot_read(ocelot, SYS_COUNT_RX_JABBERS) +
 808                             ocelot_read(ocelot, SYS_COUNT_RX_LONGS) +
 809                             ocelot_read(ocelot, SYS_COUNT_RX_64) +
 810                             ocelot_read(ocelot, SYS_COUNT_RX_65_127) +
 811                             ocelot_read(ocelot, SYS_COUNT_RX_128_255) +
 812                             ocelot_read(ocelot, SYS_COUNT_RX_256_1023) +
 813                             ocelot_read(ocelot, SYS_COUNT_RX_1024_1526) +
 814                             ocelot_read(ocelot, SYS_COUNT_RX_1527_MAX);
 815         stats->multicast = ocelot_read(ocelot, SYS_COUNT_RX_MULTICAST);
 816         stats->rx_dropped = dev->stats.rx_dropped;
 817
 818         /* Get Tx stats */
 819         stats->tx_bytes = ocelot_read(ocelot, SYS_COUNT_TX_OCTETS);
 820         stats->tx_packets = ocelot_read(ocelot, SYS_COUNT_TX_64) +
 821                             ocelot_read(ocelot, SYS_COUNT_TX_65_127) +
 822                             ocelot_read(ocelot, SYS_COUNT_TX_128_511) +
 823                             ocelot_read(ocelot, SYS_COUNT_TX_512_1023) +
 824                             ocelot_read(ocelot, SYS_COUNT_TX_1024_1526) +
 825                             ocelot_read(ocelot, SYS_COUNT_TX_1527_MAX);
 826         stats->tx_dropped = ocelot_read(ocelot, SYS_COUNT_TX_DROPS) +
 827                             ocelot_read(ocelot, SYS_COUNT_TX_AGING);
 828         stats->collisions = ocelot_read(ocelot, SYS_COUNT_TX_COLLISION);
 829 }
 830
 831 static int ocelot_fdb_add(struct ocelot *ocelot, int port,
 832                           const unsigned char *addr, u16 vid,
 833                           bool vlan_aware)
 834 {
 835         struct ocelot_port *ocelot_port = ocelot->ports[port];
 836
 837         if (!vid) {
 838                 if (!vlan_aware)
 839                         /* If the bridge is not VLAN aware and no VID was
 840                          * provided, set it to pvid to ensure the MAC entry
 841                          * matches incoming untagged packets
 842                          */
 843                         vid = ocelot_port->pvid;
 844                 else
 845                         /* If the bridge is VLAN aware a VID must be provided as
 846                          * otherwise the learnt entry wouldn't match any frame.
 847                          */
 848                         return -EINVAL;
 849         }
 850
 851         return ocelot_mact_learn(ocelot, port, addr, vid, ENTRYTYPE_LOCKED);
 852 }
 853
 854 static int ocelot_port_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
 855                                struct net_device *dev,
 856                                const unsigned char *addr,
 857                                u16 vid, u16 flags,
 858                                struct netlink_ext_ack *extack)
 859 {
 860         struct ocelot_port_private *priv = netdev_priv(dev);
 861         struct ocelot *ocelot = priv->port.ocelot;
 862         int port = priv->chip_port;
 863
 864         return ocelot_fdb_add(ocelot, port, addr, vid, priv->vlan_aware);
 865 }
 866
 867 static int ocelot_fdb_del(struct ocelot *ocelot, int port,
 868                           const unsigned char *addr, u16 vid)
 869 {
 870         return ocelot_mact_forget(ocelot, addr, vid);
 871 }
 872
 873 static int ocelot_port_fdb_del(struct ndmsg *ndm, struct nlattr *tb[],
 874                                struct net_device *dev,
 875                                const unsigned char *addr, u16 vid)
 876 {
 877         struct ocelot_port_private *priv = netdev_priv(dev);
 878         struct ocelot *ocelot = priv->port.ocelot;
 879         int port = priv->chip_port;
 880
 881         return ocelot_fdb_del(ocelot, port, addr, vid);
 882 }
 883
 884 struct ocelot_dump_ctx {
 885         struct net_device *dev;
 886         struct sk_buff *skb;
 887         struct netlink_callback *cb;
 888         int idx;
 889 };
 890
 891 static int ocelot_port_fdb_do_dump(const unsigned char *addr, u16 vid,
 892                                    bool is_static, void *data)
 893 {
 894         struct ocelot_dump_ctx *dump = data;
 895         u32 portid = NETLINK_CB(dump->cb->skb).portid;
 896         u32 seq = dump->cb->nlh->nlmsg_seq;
 897         struct nlmsghdr *nlh;
 898         struct ndmsg *ndm;
 899
 900         if (dump->idx < dump->cb->args[2])
 901                 goto skip;
 902
 903         nlh = nlmsg_put(dump->skb, portid, seq, RTM_NEWNEIGH,
 904                         sizeof(*ndm), NLM_F_MULTI);
 905         if (!nlh)
 906                 return -EMSGSIZE;
 907
 908         ndm = nlmsg_data(nlh);
 909         ndm->ndm_family  = AF_BRIDGE;
 910         ndm->ndm_pad1    = 0;
 911         ndm->ndm_pad2    = 0;
 912         ndm->ndm_flags   = NTF_SELF;
 913         ndm->ndm_type    = 0;
 914         ndm->ndm_ifindex = dump->dev->ifindex;
 915         ndm->ndm_state   = is_static ? NUD_NOARP : NUD_REACHABLE;
 916
 917         if (nla_put(dump->skb, NDA_LLADDR, ETH_ALEN, addr))
 918                 goto nla_put_failure;
 919
 920         if (vid && nla_put_u16(dump->skb, NDA_VLAN, vid))
 921                 goto nla_put_failure;
 922
 923         nlmsg_end(dump->skb, nlh);
 924
 925 skip:
 926         dump->idx++;
 927         return 0;
 928
 929 nla_put_failure:
 930         nlmsg_cancel(dump->skb, nlh);
 931         return -EMSGSIZE;
 932 }
 933
 934 static int ocelot_mact_read(struct ocelot *ocelot, int port, int row, int col,
 935                             struct ocelot_mact_entry *entry)
 936 {
 937         u32 val, dst, macl, mach;
 938         char mac[ETH_ALEN];
 939
 940         /* Set row and column to read from */
 941         ocelot_field_write(ocelot, ANA_TABLES_MACTINDX_M_INDEX, row);
 942         ocelot_field_write(ocelot, ANA_TABLES_MACTINDX_BUCKET, col);
 943
 944         /* Issue a read command */
 945         ocelot_write(ocelot,
 946                      ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_READ),
 947                      ANA_TABLES_MACACCESS);
 948
 949         if (ocelot_mact_wait_for_completion(ocelot))
 950                 return -ETIMEDOUT;
 951
 952         /* Read the entry flags */
 953         val = ocelot_read(ocelot, ANA_TABLES_MACACCESS);
 954         if (!(val & ANA_TABLES_MACACCESS_VALID))
 955                 return -EINVAL;
 956
 957         /* If the entry read has another port configured as its destination,
 958          * do not report it.
 959          */
 960         dst = (val & ANA_TABLES_MACACCESS_DEST_IDX_M) >> 3;
 961         if (dst != port)
 962                 return -EINVAL;
 963
 964         /* Get the entry's MAC address and VLAN id */
 965         macl = ocelot_read(ocelot, ANA_TABLES_MACLDATA);
 966         mach = ocelot_read(ocelot, ANA_TABLES_MACHDATA);
 967
 968         mac[0] = (mach >> 8)  & 0xff;
 969         mac[1] = (mach >> 0)  & 0xff;
 970         mac[2] = (macl >> 24) & 0xff;
 971         mac[3] = (macl >> 16) & 0xff;
 972         mac[4] = (macl >> 8)  & 0xff;
 973         mac[5] = (macl >> 0)  & 0xff;
 974
 975         entry->vid = (mach >> 16) & 0xfff;
 976         ether_addr_copy(entry->mac, mac);
 977
 978         return 0;
 979 }
 980
 981 static int ocelot_fdb_dump(struct ocelot *ocelot, int port,
 982                            dsa_fdb_dump_cb_t *cb, void *data)
 983 {
 984         int i, j;
 985
 986         /* Loop through all the mac tables entries. There are 1024 rows of 4
 987          * entries.
 988          */
 989         for (i = 0; i < 1024; i++) {
 990                 for (j = 0; j < 4; j++) {
 991                         struct ocelot_mact_entry entry;
 992                         bool is_static;
 993                         int ret;
 994
 995                         ret = ocelot_mact_read(ocelot, port, i, j, &entry);
 996                         /* If the entry is invalid (wrong port, invalid...),
 997                          * skip it.
 998                          */
 999                         if (ret == -EINVAL)
1000                                 continue;
1001                         else if (ret)
1002                                 return ret;
1003
1004                         is_static = (entry.type == ENTRYTYPE_LOCKED);
1005
1006                         ret = cb(entry.mac, entry.vid, is_static, data);
1007                         if (ret)
1008                                 return ret;
1009                 }
1010         }
1011
1012         return 0;
1013 }
1014
1015 static int ocelot_port_fdb_dump(struct sk_buff *skb,
1016                                 struct netlink_callback *cb,
1017                                 struct net_device *dev,
1018                                 struct net_device *filter_dev, int *idx)
1019 {
1020         struct ocelot_port_private *priv = netdev_priv(dev);
1021         struct ocelot *ocelot = priv->port.ocelot;
1022         struct ocelot_dump_ctx dump = {
1023                 .dev = dev,
1024                 .skb = skb,
1025                 .cb = cb,
1026                 .idx = *idx,
1027         };
1028         int port = priv->chip_port;
1029         int ret;
1030
1031         ret = ocelot_fdb_dump(ocelot, port, ocelot_port_fdb_do_dump, &dump);
1032
1033         *idx = dump.idx;
1034
1035         return ret;
1036 }
1037
1038 static int ocelot_vlan_rx_add_vid(struct net_device *dev, __be16 proto,
1039                                   u16 vid)
1040 {
1041         return ocelot_vlan_vid_add(dev, vid, false, false);
1042 }
1043
1044 static int ocelot_vlan_rx_kill_vid(struct net_device *dev, __be16 proto,
1045                                    u16 vid)
1046 {
1047         return ocelot_vlan_vid_del(dev, vid);
1048 }
1049
1050 static int ocelot_set_features(struct net_device *dev,
1051                                netdev_features_t features)
1052 {
1053         netdev_features_t changed = dev->features ^ features;
1054         struct ocelot_port_private *priv = netdev_priv(dev);
1055         struct ocelot *ocelot = priv->port.ocelot;
1056         int port = priv->chip_port;
1057
1058         if ((dev->features & NETIF_F_HW_TC) > (features & NETIF_F_HW_TC) &&
1059             priv->tc.offload_cnt) {
1060                 netdev_err(dev,
1061                            "Cannot disable HW TC offload while offloads active\n");
1062                 return -EBUSY;
1063         }
1064
1065         if (changed & NETIF_F_HW_VLAN_CTAG_FILTER)
1066                 ocelot_vlan_mode(ocelot, port, features);
1067
1068         return 0;
1069 }
1070
1071 static int ocelot_get_port_parent_id(struct net_device *dev,
1072                                      struct netdev_phys_item_id *ppid)
1073 {
1074         struct ocelot_port_private *priv = netdev_priv(dev);
1075         struct ocelot *ocelot = priv->port.ocelot;
1076
1077         ppid->id_len = sizeof(ocelot->base_mac);
1078         memcpy(&ppid->id, &ocelot->base_mac, ppid->id_len);
1079
1080         return 0;
1081 }
1082
1083 static int ocelot_hwstamp_get(struct ocelot *ocelot, int port,
1084                               struct ifreq *ifr)
1085 {
1086         return copy_to_user(ifr->ifr_data, &ocelot->hwtstamp_config,
1087                             sizeof(ocelot->hwtstamp_config)) ? -EFAULT : 0;
1088 }
1089
1090 static int ocelot_hwstamp_set(struct ocelot *ocelot, int port,
1091                               struct ifreq *ifr)
1092 {
1093         struct ocelot_port *ocelot_port = ocelot->ports[port];
1094         struct hwtstamp_config cfg;
1095
1096         if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
1097                 return -EFAULT;
1098
1099         /* reserved for future extensions */
1100         if (cfg.flags)
1101                 return -EINVAL;
1102
1103         /* Tx type sanity check */
1104         switch (cfg.tx_type) {
1105         case HWTSTAMP_TX_ON:
1106                 ocelot_port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
1107                 break;
1108         case HWTSTAMP_TX_ONESTEP_SYNC:
1109                 /* IFH_REW_OP_ONE_STEP_PTP updates the correctional field, we
1110                  * need to update the origin time.
1111                  */
1112                 ocelot_port->ptp_cmd = IFH_REW_OP_ORIGIN_PTP;
1113                 break;
1114         case HWTSTAMP_TX_OFF:
1115                 ocelot_port->ptp_cmd = 0;
1116                 break;
1117         default:
1118                 return -ERANGE;
1119         }
1120
1121         mutex_lock(&ocelot->ptp_lock);
1122
1123         switch (cfg.rx_filter) {
1124         case HWTSTAMP_FILTER_NONE:
1125                 break;
1126         case HWTSTAMP_FILTER_ALL:
1127         case HWTSTAMP_FILTER_SOME:
1128         case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
1129         case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
1130         case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
1131         case HWTSTAMP_FILTER_NTP_ALL:
1132         case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
1133         case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
1134         case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
1135         case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
1136         case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
1137         case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
1138         case HWTSTAMP_FILTER_PTP_V2_EVENT:
1139         case HWTSTAMP_FILTER_PTP_V2_SYNC:
1140         case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
1141                 cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
1142                 break;
1143         default:
1144                 mutex_unlock(&ocelot->ptp_lock);
1145                 return -ERANGE;
1146         }
1147
1148         /* Commit back the result & save it */
1149         memcpy(&ocelot->hwtstamp_config, &cfg, sizeof(cfg));
1150         mutex_unlock(&ocelot->ptp_lock);
1151
1152         return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
1153 }
1154
1155 static int ocelot_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
1156 {
1157         struct ocelot_port_private *priv = netdev_priv(dev);
1158         struct ocelot *ocelot = priv->port.ocelot;
1159         int port = priv->chip_port;
1160
1161         /* The function is only used for PTP operations for now */
1162         if (!ocelot->ptp)
1163                 return -EOPNOTSUPP;
1164
1165         switch (cmd) {
1166         case SIOCSHWTSTAMP:
1167                 return ocelot_hwstamp_set(ocelot, port, ifr);
1168         case SIOCGHWTSTAMP:
1169                 return ocelot_hwstamp_get(ocelot, port, ifr);
1170         default:
1171                 return -EOPNOTSUPP;
1172         }
1173 }
1174
1175 static const struct net_device_ops ocelot_port_netdev_ops = {
1176         .ndo_open                       = ocelot_port_open,
1177         .ndo_stop                       = ocelot_port_stop,
1178         .ndo_start_xmit                 = ocelot_port_xmit,
1179         .ndo_set_rx_mode                = ocelot_set_rx_mode,
1180         .ndo_get_phys_port_name         = ocelot_port_get_phys_port_name,
1181         .ndo_set_mac_address            = ocelot_port_set_mac_address,
1182         .ndo_get_stats64                = ocelot_get_stats64,
1183         .ndo_fdb_add                    = ocelot_port_fdb_add,
1184         .ndo_fdb_del                    = ocelot_port_fdb_del,
1185         .ndo_fdb_dump                   = ocelot_port_fdb_dump,
1186         .ndo_vlan_rx_add_vid            = ocelot_vlan_rx_add_vid,
1187         .ndo_vlan_rx_kill_vid           = ocelot_vlan_rx_kill_vid,
1188         .ndo_set_features               = ocelot_set_features,
1189         .ndo_get_port_parent_id         = ocelot_get_port_parent_id,
1190         .ndo_setup_tc                   = ocelot_setup_tc,
1191         .ndo_do_ioctl                   = ocelot_ioctl,
1192 };
1193
1194 static void ocelot_get_strings(struct ocelot *ocelot, int port, u32 sset,
1195                                u8 *data)
1196 {
1197         int i;
1198
1199         if (sset != ETH_SS_STATS)
1200                 return;
1201
1202         for (i = 0; i < ocelot->num_stats; i++)
1203                 memcpy(data + i * ETH_GSTRING_LEN, ocelot->stats_layout[i].name,
1204                        ETH_GSTRING_LEN);
1205 }
1206
1207 static void ocelot_port_get_strings(struct net_device *netdev, u32 sset,
1208                                     u8 *data)
1209 {
1210         struct ocelot_port_private *priv = netdev_priv(netdev);
1211         struct ocelot *ocelot = priv->port.ocelot;
1212         int port = priv->chip_port;
1213
1214         ocelot_get_strings(ocelot, port, sset, data);
1215 }
1216
1217 static void ocelot_update_stats(struct ocelot *ocelot)
1218 {
1219         int i, j;
1220
1221         mutex_lock(&ocelot->stats_lock);
1222
1223         for (i = 0; i < ocelot->num_phys_ports; i++) {
1224                 /* Configure the port to read the stats from */
1225                 ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(i), SYS_STAT_CFG);
1226
1227                 for (j = 0; j < ocelot->num_stats; j++) {
1228                         u32 val;
1229                         unsigned int idx = i * ocelot->num_stats + j;
1230
1231                         val = ocelot_read_rix(ocelot, SYS_COUNT_RX_OCTETS,
1232                                               ocelot->stats_layout[j].offset);
1233
1234                         if (val < (ocelot->stats[idx] & U32_MAX))
1235                                 ocelot->stats[idx] += (u64)1 << 32;
1236
1237                         ocelot->stats[idx] = (ocelot->stats[idx] &
1238                                               ~(u64)U32_MAX) + val;
1239                 }
1240         }
1241
1242         mutex_unlock(&ocelot->stats_lock);
1243 }
1244
1245 static void ocelot_check_stats_work(struct work_struct *work)
1246 {
1247         struct delayed_work *del_work = to_delayed_work(work);
1248         struct ocelot *ocelot = container_of(del_work, struct ocelot,
1249                                              stats_work);
1250
1251         ocelot_update_stats(ocelot);
1252
1253         queue_delayed_work(ocelot->stats_queue, &ocelot->stats_work,
1254                            OCELOT_STATS_CHECK_DELAY);
1255 }
1256
1257 static void ocelot_get_ethtool_stats(struct ocelot *ocelot, int port, u64 *data)
1258 {
1259         int i;
1260
1261         /* check and update now */
1262         ocelot_update_stats(ocelot);
1263
1264         /* Copy all counters */
1265         for (i = 0; i < ocelot->num_stats; i++)
1266                 *data++ = ocelot->stats[port * ocelot->num_stats + i];
1267 }
1268
1269 static void ocelot_port_get_ethtool_stats(struct net_device *dev,
1270                                           struct ethtool_stats *stats,
1271                                           u64 *data)
1272 {
1273         struct ocelot_port_private *priv = netdev_priv(dev);
1274         struct ocelot *ocelot = priv->port.ocelot;
1275         int port = priv->chip_port;
1276
1277         ocelot_get_ethtool_stats(ocelot, port, data);
1278 }
1279
1280 static int ocelot_get_sset_count(struct ocelot *ocelot, int port, int sset)
1281 {
1282         if (sset != ETH_SS_STATS)
1283                 return -EOPNOTSUPP;
1284
1285         return ocelot->num_stats;
1286 }
1287
1288 static int ocelot_port_get_sset_count(struct net_device *dev, int sset)
1289 {
1290         struct ocelot_port_private *priv = netdev_priv(dev);
1291         struct ocelot *ocelot = priv->port.ocelot;
1292         int port = priv->chip_port;
1293
1294         return ocelot_get_sset_count(ocelot, port, sset);
1295 }
1296
1297 static int ocelot_get_ts_info(struct ocelot *ocelot, int port,
1298                               struct ethtool_ts_info *info)
1299 {
1300         info->phc_index = ocelot->ptp_clock ?
1301                           ptp_clock_index(ocelot->ptp_clock) : -1;
1302         info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE |
1303                                  SOF_TIMESTAMPING_RX_SOFTWARE |
1304                                  SOF_TIMESTAMPING_SOFTWARE |
1305                                  SOF_TIMESTAMPING_TX_HARDWARE |
1306                                  SOF_TIMESTAMPING_RX_HARDWARE |
1307                                  SOF_TIMESTAMPING_RAW_HARDWARE;
1308         info->tx_types = BIT(HWTSTAMP_TX_OFF) | BIT(HWTSTAMP_TX_ON) |
1309                          BIT(HWTSTAMP_TX_ONESTEP_SYNC);
1310         info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) | BIT(HWTSTAMP_FILTER_ALL);
1311
1312         return 0;
1313 }
1314
1315 static int ocelot_port_get_ts_info(struct net_device *dev,
1316                                    struct ethtool_ts_info *info)
1317 {
1318         struct ocelot_port_private *priv = netdev_priv(dev);
1319         struct ocelot *ocelot = priv->port.ocelot;
1320         int port = priv->chip_port;
1321
1322         if (!ocelot->ptp)
1323                 return ethtool_op_get_ts_info(dev, info);
1324
1325         return ocelot_get_ts_info(ocelot, port, info);
1326 }
1327
1328 static const struct ethtool_ops ocelot_ethtool_ops = {
1329         .get_strings            = ocelot_port_get_strings,
1330         .get_ethtool_stats      = ocelot_port_get_ethtool_stats,
1331         .get_sset_count         = ocelot_port_get_sset_count,
1332         .get_link_ksettings     = phy_ethtool_get_link_ksettings,
1333         .set_link_ksettings     = phy_ethtool_set_link_ksettings,
1334         .get_ts_info            = ocelot_port_get_ts_info,
1335 };
1336
1337 static void ocelot_bridge_stp_state_set(struct ocelot *ocelot, int port,
1338                                         u8 state)
1339 {
1340         u32 port_cfg;
1341         int p, i;
1342
1343         if (!(BIT(port) & ocelot->bridge_mask))
1344                 return;
1345
1346         port_cfg = ocelot_read_gix(ocelot, ANA_PORT_PORT_CFG, port);
1347
1348         switch (state) {
1349         case BR_STATE_FORWARDING:
1350                 ocelot->bridge_fwd_mask |= BIT(port);
1351                 /* Fallthrough */
1352         case BR_STATE_LEARNING:
1353                 port_cfg |= ANA_PORT_PORT_CFG_LEARN_ENA;
1354                 break;
1355
1356         default:
1357                 port_cfg &= ~ANA_PORT_PORT_CFG_LEARN_ENA;
1358                 ocelot->bridge_fwd_mask &= ~BIT(port);
1359                 break;
1360         }
1361
1362         ocelot_write_gix(ocelot, port_cfg, ANA_PORT_PORT_CFG, port);
1363
1364         /* Apply FWD mask. The loop is needed to add/remove the current port as
1365          * a source for the other ports.
1366          */
1367         for (p = 0; p < ocelot->num_phys_ports; p++) {
1368                 if (ocelot->bridge_fwd_mask & BIT(p)) {
1369                         unsigned long mask = ocelot->bridge_fwd_mask & ~BIT(p);
1370
1371                         for (i = 0; i < ocelot->num_phys_ports; i++) {
1372                                 unsigned long bond_mask = ocelot->lags[i];
1373
1374                                 if (!bond_mask)
1375                                         continue;
1376
1377                                 if (bond_mask & BIT(p)) {
1378                                         mask &= ~bond_mask;
1379                                         break;
1380                                 }
1381                         }
1382
1383                         ocelot_write_rix(ocelot,
1384                                          BIT(ocelot->num_phys_ports) | mask,
1385                                          ANA_PGID_PGID, PGID_SRC + p);
1386                 } else {
1387                         /* Only the CPU port, this is compatible with link
1388                          * aggregation.
1389                          */
1390                         ocelot_write_rix(ocelot,
1391                                          BIT(ocelot->num_phys_ports),
1392                                          ANA_PGID_PGID, PGID_SRC + p);
1393                 }
1394         }
1395 }
1396
1397 static void ocelot_port_attr_stp_state_set(struct ocelot *ocelot, int port,
1398                                            struct switchdev_trans *trans,
1399                                            u8 state)
1400 {
1401         if (switchdev_trans_ph_prepare(trans))
1402                 return;
1403
1404         ocelot_bridge_stp_state_set(ocelot, port, state);
1405 }
1406
1407 static void ocelot_set_ageing_time(struct ocelot *ocelot, unsigned int msecs)
1408 {
1409         ocelot_write(ocelot, ANA_AUTOAGE_AGE_PERIOD(msecs / 2),
1410                      ANA_AUTOAGE);
1411 }
1412
1413 static void ocelot_port_attr_ageing_set(struct ocelot *ocelot, int port,
1414                                         unsigned long ageing_clock_t)
1415 {
1416         unsigned long ageing_jiffies = clock_t_to_jiffies(ageing_clock_t);
1417         u32 ageing_time = jiffies_to_msecs(ageing_jiffies) / 1000;
1418
1419         ocelot_set_ageing_time(ocelot, ageing_time);
1420 }
1421
1422 static void ocelot_port_attr_mc_set(struct ocelot *ocelot, int port, bool mc)
1423 {
1424         u32 cpu_fwd_mcast = ANA_PORT_CPU_FWD_CFG_CPU_IGMP_REDIR_ENA |
1425                             ANA_PORT_CPU_FWD_CFG_CPU_MLD_REDIR_ENA |
1426                             ANA_PORT_CPU_FWD_CFG_CPU_IPMC_CTRL_COPY_ENA;
1427         u32 val = 0;
1428
1429         if (mc)
1430                 val = cpu_fwd_mcast;
1431
1432         ocelot_rmw_gix(ocelot, val, cpu_fwd_mcast,
1433                        ANA_PORT_CPU_FWD_CFG, port);
1434 }
1435
1436 static int ocelot_port_attr_set(struct net_device *dev,
1437                                 const struct switchdev_attr *attr,
1438                                 struct switchdev_trans *trans)
1439 {
1440         struct ocelot_port_private *priv = netdev_priv(dev);
1441         struct ocelot *ocelot = priv->port.ocelot;
1442         int port = priv->chip_port;
1443         int err = 0;
1444
1445         switch (attr->id) {
1446         case SWITCHDEV_ATTR_ID_PORT_STP_STATE:
1447                 ocelot_port_attr_stp_state_set(ocelot, port, trans,
1448                                                attr->u.stp_state);
1449                 break;
1450         case SWITCHDEV_ATTR_ID_BRIDGE_AGEING_TIME:
1451                 ocelot_port_attr_ageing_set(ocelot, port, attr->u.ageing_time);
1452                 break;
1453         case SWITCHDEV_ATTR_ID_BRIDGE_VLAN_FILTERING:
1454                 priv->vlan_aware = attr->u.vlan_filtering;
1455                 ocelot_port_vlan_filtering(ocelot, port, priv->vlan_aware);
1456                 break;
1457         case SWITCHDEV_ATTR_ID_BRIDGE_MC_DISABLED:
1458                 ocelot_port_attr_mc_set(ocelot, port, !attr->u.mc_disabled);
1459                 break;
1460         default:
1461                 err = -EOPNOTSUPP;
1462                 break;
1463         }
1464
1465         return err;
1466 }
1467
1468 static int ocelot_port_obj_add_vlan(struct net_device *dev,
1469                                     const struct switchdev_obj_port_vlan *vlan,
1470                                     struct switchdev_trans *trans)
1471 {
1472         int ret;
1473         u16 vid;
1474
1475         for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
1476                 ret = ocelot_vlan_vid_add(dev, vid,
1477                                           vlan->flags & BRIDGE_VLAN_INFO_PVID,
1478                                           vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED);
1479                 if (ret)
1480                         return ret;
1481         }
1482
1483         return 0;
1484 }
1485
1486 static int ocelot_port_vlan_del_vlan(struct net_device *dev,
1487                                      const struct switchdev_obj_port_vlan *vlan)
1488 {
1489         int ret;
1490         u16 vid;
1491
1492         for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
1493                 ret = ocelot_vlan_vid_del(dev, vid);
1494
1495                 if (ret)
1496                         return ret;
1497         }
1498
1499         return 0;
1500 }
1501
1502 static struct ocelot_multicast *ocelot_multicast_get(struct ocelot *ocelot,
1503                                                      const unsigned char *addr,
1504                                                      u16 vid)
1505 {
1506         struct ocelot_multicast *mc;
1507
1508         list_for_each_entry(mc, &ocelot->multicast, list) {
1509                 if (ether_addr_equal(mc->addr, addr) && mc->vid == vid)
1510                         return mc;
1511         }
1512
1513         return NULL;
1514 }
1515
1516 static int ocelot_port_obj_add_mdb(struct net_device *dev,
1517                                    const struct switchdev_obj_port_mdb *mdb,
1518                                    struct switchdev_trans *trans)
1519 {
1520         struct ocelot_port_private *priv = netdev_priv(dev);
1521         struct ocelot_port *ocelot_port = &priv->port;
1522         struct ocelot *ocelot = ocelot_port->ocelot;
1523         unsigned char addr[ETH_ALEN];
1524         struct ocelot_multicast *mc;
1525         int port = priv->chip_port;
1526         u16 vid = mdb->vid;
1527         bool new = false;
1528
1529         if (!vid)
1530                 vid = ocelot_port->pvid;
1531
1532         mc = ocelot_multicast_get(ocelot, mdb->addr, vid);
1533         if (!mc) {
1534                 mc = devm_kzalloc(ocelot->dev, sizeof(*mc), GFP_KERNEL);
1535                 if (!mc)
1536                         return -ENOMEM;
1537
1538                 memcpy(mc->addr, mdb->addr, ETH_ALEN);
1539                 mc->vid = vid;
1540
1541                 list_add_tail(&mc->list, &ocelot->multicast);
1542                 new = true;
1543         }
1544
1545         memcpy(addr, mc->addr, ETH_ALEN);
1546         addr[0] = 0;
1547
1548         if (!new) {
1549                 addr[2] = mc->ports << 0;
1550                 addr[1] = mc->ports << 8;
1551                 ocelot_mact_forget(ocelot, addr, vid);
1552         }
1553
1554         mc->ports |= BIT(port);
1555         addr[2] = mc->ports << 0;
1556         addr[1] = mc->ports << 8;
1557
1558         return ocelot_mact_learn(ocelot, 0, addr, vid, ENTRYTYPE_MACv4);
1559 }
1560
1561 static int ocelot_port_obj_del_mdb(struct net_device *dev,
1562                                    const struct switchdev_obj_port_mdb *mdb)
1563 {
1564         struct ocelot_port_private *priv = netdev_priv(dev);
1565         struct ocelot_port *ocelot_port = &priv->port;
1566         struct ocelot *ocelot = ocelot_port->ocelot;
1567         unsigned char addr[ETH_ALEN];
1568         struct ocelot_multicast *mc;
1569         int port = priv->chip_port;
1570         u16 vid = mdb->vid;
1571
1572         if (!vid)
1573                 vid = ocelot_port->pvid;
1574
1575         mc = ocelot_multicast_get(ocelot, mdb->addr, vid);
1576         if (!mc)
1577                 return -ENOENT;
1578
1579         memcpy(addr, mc->addr, ETH_ALEN);
1580         addr[2] = mc->ports << 0;
1581         addr[1] = mc->ports << 8;
1582         addr[0] = 0;
1583         ocelot_mact_forget(ocelot, addr, vid);
1584
1585         mc->ports &= ~BIT(port);
1586         if (!mc->ports) {
1587                 list_del(&mc->list);
1588                 devm_kfree(ocelot->dev, mc);
1589                 return 0;
1590         }
1591
1592         addr[2] = mc->ports << 0;
1593         addr[1] = mc->ports << 8;
1594
1595         return ocelot_mact_learn(ocelot, 0, addr, vid, ENTRYTYPE_MACv4);
1596 }
1597
1598 static int ocelot_port_obj_add(struct net_device *dev,
1599                                const struct switchdev_obj *obj,
1600                                struct switchdev_trans *trans,
1601                                struct netlink_ext_ack *extack)
1602 {
1603         int ret = 0;
1604
1605         switch (obj->id) {
1606         case SWITCHDEV_OBJ_ID_PORT_VLAN:
1607                 ret = ocelot_port_obj_add_vlan(dev,
1608                                                SWITCHDEV_OBJ_PORT_VLAN(obj),
1609                                                trans);
1610                 break;
1611         case SWITCHDEV_OBJ_ID_PORT_MDB:
1612                 ret = ocelot_port_obj_add_mdb(dev, SWITCHDEV_OBJ_PORT_MDB(obj),
1613                                               trans);
1614                 break;
1615         default:
1616                 return -EOPNOTSUPP;
1617         }
1618
1619         return ret;
1620 }
1621
1622 static int ocelot_port_obj_del(struct net_device *dev,
1623                                const struct switchdev_obj *obj)
1624 {
1625         int ret = 0;
1626
1627         switch (obj->id) {
1628         case SWITCHDEV_OBJ_ID_PORT_VLAN:
1629                 ret = ocelot_port_vlan_del_vlan(dev,
1630                                                 SWITCHDEV_OBJ_PORT_VLAN(obj));
1631                 break;
1632         case SWITCHDEV_OBJ_ID_PORT_MDB:
1633                 ret = ocelot_port_obj_del_mdb(dev, SWITCHDEV_OBJ_PORT_MDB(obj));
1634                 break;
1635         default:
1636                 return -EOPNOTSUPP;
1637         }
1638
1639         return ret;
1640 }
1641
1642 static int ocelot_port_bridge_join(struct ocelot *ocelot, int port,
1643                                    struct net_device *bridge)
1644 {
1645         if (!ocelot->bridge_mask) {
1646                 ocelot->hw_bridge_dev = bridge;
1647         } else {
1648                 if (ocelot->hw_bridge_dev != bridge)
1649                         /* This is adding the port to a second bridge, this is
1650                          * unsupported */
1651                         return -ENODEV;
1652         }
1653
1654         ocelot->bridge_mask |= BIT(port);
1655
1656         return 0;
1657 }
1658
1659 static int ocelot_port_bridge_leave(struct ocelot *ocelot, int port,
1660                                     struct net_device *bridge)
1661 {
1662         ocelot->bridge_mask &= ~BIT(port);
1663
1664         if (!ocelot->bridge_mask)
1665                 ocelot->hw_bridge_dev = NULL;
1666
1667         ocelot_port_vlan_filtering(ocelot, port, 0);
1668         ocelot_port_set_pvid(ocelot, port, 0);
1669         return ocelot_port_set_native_vlan(ocelot, port, 0);
1670 }
1671
1672 static void ocelot_set_aggr_pgids(struct ocelot *ocelot)
1673 {
1674         int i, port, lag;
1675
1676         /* Reset destination and aggregation PGIDS */
1677         for (port = 0; port < ocelot->num_phys_ports; port++)
1678                 ocelot_write_rix(ocelot, BIT(port), ANA_PGID_PGID, port);
1679
1680         for (i = PGID_AGGR; i < PGID_SRC; i++)
1681                 ocelot_write_rix(ocelot, GENMASK(ocelot->num_phys_ports - 1, 0),
1682                                  ANA_PGID_PGID, i);
1683
1684         /* Now, set PGIDs for each LAG */
1685         for (lag = 0; lag < ocelot->num_phys_ports; lag++) {
1686                 unsigned long bond_mask;
1687                 int aggr_count = 0;
1688                 u8 aggr_idx[16];
1689
1690                 bond_mask = ocelot->lags[lag];
1691                 if (!bond_mask)
1692                         continue;
1693
1694                 for_each_set_bit(port, &bond_mask, ocelot->num_phys_ports) {
1695                         // Destination mask
1696                         ocelot_write_rix(ocelot, bond_mask,
1697                                          ANA_PGID_PGID, port);
1698                         aggr_idx[aggr_count] = port;
1699                         aggr_count++;
1700                 }
1701
1702                 for (i = PGID_AGGR; i < PGID_SRC; i++) {
1703                         u32 ac;
1704
1705                         ac = ocelot_read_rix(ocelot, ANA_PGID_PGID, i);
1706                         ac &= ~bond_mask;
1707                         ac |= BIT(aggr_idx[i % aggr_count]);
1708                         ocelot_write_rix(ocelot, ac, ANA_PGID_PGID, i);
1709                 }
1710         }
1711 }
1712
1713 static void ocelot_setup_lag(struct ocelot *ocelot, int lag)
1714 {
1715         unsigned long bond_mask = ocelot->lags[lag];
1716         unsigned int p;
1717
1718         for_each_set_bit(p, &bond_mask, ocelot->num_phys_ports) {
1719                 u32 port_cfg = ocelot_read_gix(ocelot, ANA_PORT_PORT_CFG, p);
1720
1721                 port_cfg &= ~ANA_PORT_PORT_CFG_PORTID_VAL_M;
1722
1723                 /* Use lag port as logical port for port i */
1724                 ocelot_write_gix(ocelot, port_cfg |
1725                                  ANA_PORT_PORT_CFG_PORTID_VAL(lag),
1726                                  ANA_PORT_PORT_CFG, p);
1727         }
1728 }
1729
1730 static int ocelot_port_lag_join(struct ocelot *ocelot, int port,
1731                                 struct net_device *bond)
1732 {
1733         struct net_device *ndev;
1734         u32 bond_mask = 0;
1735         int lag, lp;
1736
1737         rcu_read_lock();
1738         for_each_netdev_in_bond_rcu(bond, ndev) {
1739                 struct ocelot_port_private *priv = netdev_priv(ndev);
1740
1741                 bond_mask |= BIT(priv->chip_port);
1742         }
1743         rcu_read_unlock();
1744
1745         lp = __ffs(bond_mask);
1746
1747         /* If the new port is the lowest one, use it as the logical port from
1748          * now on
1749          */
1750         if (port == lp) {
1751                 lag = port;
1752                 ocelot->lags[port] = bond_mask;
1753                 bond_mask &= ~BIT(port);
1754                 if (bond_mask) {
1755                         lp = __ffs(bond_mask);
1756                         ocelot->lags[lp] = 0;
1757                 }
1758         } else {
1759                 lag = lp;
1760                 ocelot->lags[lp] |= BIT(port);
1761         }
1762
1763         ocelot_setup_lag(ocelot, lag);
1764         ocelot_set_aggr_pgids(ocelot);
1765
1766         return 0;
1767 }
1768
1769 static void ocelot_port_lag_leave(struct ocelot *ocelot, int port,
1770                                   struct net_device *bond)
1771 {
1772         u32 port_cfg;
1773         int i;
1774
1775         /* Remove port from any lag */
1776         for (i = 0; i < ocelot->num_phys_ports; i++)
1777                 ocelot->lags[i] &= ~BIT(port);
1778
1779         /* if it was the logical port of the lag, move the lag config to the
1780          * next port
1781          */
1782         if (ocelot->lags[port]) {
1783                 int n = __ffs(ocelot->lags[port]);
1784
1785                 ocelot->lags[n] = ocelot->lags[port];
1786                 ocelot->lags[port] = 0;
1787
1788                 ocelot_setup_lag(ocelot, n);
1789         }
1790
1791         port_cfg = ocelot_read_gix(ocelot, ANA_PORT_PORT_CFG, port);
1792         port_cfg &= ~ANA_PORT_PORT_CFG_PORTID_VAL_M;
1793         ocelot_write_gix(ocelot, port_cfg | ANA_PORT_PORT_CFG_PORTID_VAL(port),
1794                          ANA_PORT_PORT_CFG, port);
1795
1796         ocelot_set_aggr_pgids(ocelot);
1797 }
1798
1799 /* Checks if the net_device instance given to us originate from our driver. */
1800 static bool ocelot_netdevice_dev_check(const struct net_device *dev)
1801 {
1802         return dev->netdev_ops == &ocelot_port_netdev_ops;
1803 }
1804
1805 static int ocelot_netdevice_port_event(struct net_device *dev,
1806                                        unsigned long event,
1807                                        struct netdev_notifier_changeupper_info *info)
1808 {
1809         struct ocelot_port_private *priv = netdev_priv(dev);
1810         struct ocelot_port *ocelot_port = &priv->port;
1811         struct ocelot *ocelot = ocelot_port->ocelot;
1812         int port = priv->chip_port;
1813         int err = 0;
1814
1815         switch (event) {
1816         case NETDEV_CHANGEUPPER:
1817                 if (netif_is_bridge_master(info->upper_dev)) {
1818                         if (info->linking) {
1819                                 err = ocelot_port_bridge_join(ocelot, port,
1820                                                               info->upper_dev);
1821                         } else {
1822                                 err = ocelot_port_bridge_leave(ocelot, port,
1823                                                                info->upper_dev);
1824                                 priv->vlan_aware = false;
1825                         }
1826                 }
1827                 if (netif_is_lag_master(info->upper_dev)) {
1828                         if (info->linking)
1829                                 err = ocelot_port_lag_join(ocelot, port,
1830                                                            info->upper_dev);
1831                         else
1832                                 ocelot_port_lag_leave(ocelot, port,
1833                                                       info->upper_dev);
1834                 }
1835                 break;
1836         default:
1837                 break;
1838         }
1839
1840         return err;
1841 }
1842
1843 static int ocelot_netdevice_event(struct notifier_block *unused,
1844                                   unsigned long event, void *ptr)
1845 {
1846         struct netdev_notifier_changeupper_info *info = ptr;
1847         struct net_device *dev = netdev_notifier_info_to_dev(ptr);
1848         int ret = 0;
1849
1850         if (!ocelot_netdevice_dev_check(dev))
1851                 return 0;
1852
1853         if (event == NETDEV_PRECHANGEUPPER &&
1854             netif_is_lag_master(info->upper_dev)) {
1855                 struct netdev_lag_upper_info *lag_upper_info = info->upper_info;
1856                 struct netlink_ext_ack *extack;
1857
1858                 if (lag_upper_info &&
1859                     lag_upper_info->tx_type != NETDEV_LAG_TX_TYPE_HASH) {
1860                         extack = netdev_notifier_info_to_extack(&info->info);
1861                         NL_SET_ERR_MSG_MOD(extack, "LAG device using unsupported Tx type");
1862
1863                         ret = -EINVAL;
1864                         goto notify;
1865                 }
1866         }
1867
1868         if (netif_is_lag_master(dev)) {
1869                 struct net_device *slave;
1870                 struct list_head *iter;
1871
1872                 netdev_for_each_lower_dev(dev, slave, iter) {
1873                         ret = ocelot_netdevice_port_event(slave, event, info);
1874                         if (ret)
1875                                 goto notify;
1876                 }
1877         } else {
1878                 ret = ocelot_netdevice_port_event(dev, event, info);
1879         }
1880
1881 notify:
1882         return notifier_from_errno(ret);
1883 }
1884
1885 struct notifier_block ocelot_netdevice_nb __read_mostly = {
1886         .notifier_call = ocelot_netdevice_event,
1887 };
1888 EXPORT_SYMBOL(ocelot_netdevice_nb);
1889
1890 static int ocelot_switchdev_event(struct notifier_block *unused,
1891                                   unsigned long event, void *ptr)
1892 {
1893         struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
1894         int err;
1895
1896         switch (event) {
1897         case SWITCHDEV_PORT_ATTR_SET:
1898                 err = switchdev_handle_port_attr_set(dev, ptr,
1899                                                      ocelot_netdevice_dev_check,
1900                                                      ocelot_port_attr_set);
1901                 return notifier_from_errno(err);
1902         }
1903
1904         return NOTIFY_DONE;
1905 }
1906
1907 struct notifier_block ocelot_switchdev_nb __read_mostly = {
1908         .notifier_call = ocelot_switchdev_event,
1909 };
1910 EXPORT_SYMBOL(ocelot_switchdev_nb);
1911
1912 static int ocelot_switchdev_blocking_event(struct notifier_block *unused,
1913                                            unsigned long event, void *ptr)
1914 {
1915         struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
1916         int err;
1917
1918         switch (event) {
1919                 /* Blocking events. */
1920         case SWITCHDEV_PORT_OBJ_ADD:
1921                 err = switchdev_handle_port_obj_add(dev, ptr,
1922                                                     ocelot_netdevice_dev_check,
1923                                                     ocelot_port_obj_add);
1924                 return notifier_from_errno(err);
1925         case SWITCHDEV_PORT_OBJ_DEL:
1926                 err = switchdev_handle_port_obj_del(dev, ptr,
1927                                                     ocelot_netdevice_dev_check,
1928                                                     ocelot_port_obj_del);
1929                 return notifier_from_errno(err);
1930         case SWITCHDEV_PORT_ATTR_SET:
1931                 err = switchdev_handle_port_attr_set(dev, ptr,
1932                                                      ocelot_netdevice_dev_check,
1933                                                      ocelot_port_attr_set);
1934                 return notifier_from_errno(err);
1935         }
1936
1937         return NOTIFY_DONE;
1938 }
1939
1940 struct notifier_block ocelot_switchdev_blocking_nb __read_mostly = {
1941         .notifier_call = ocelot_switchdev_blocking_event,
1942 };
1943 EXPORT_SYMBOL(ocelot_switchdev_blocking_nb);
1944
1945 int ocelot_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts)
1946 {
1947         struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info);
1948         unsigned long flags;
1949         time64_t s;
1950         u32 val;
1951         s64 ns;
1952
1953         spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
1954
1955         val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
1956         val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
1957         val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE);
1958         ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
1959
1960         s = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_MSB, TOD_ACC_PIN) & 0xffff;
1961         s <<= 32;
1962         s += ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
1963         ns = ocelot_read_rix(ocelot, PTP_PIN_TOD_NSEC, TOD_ACC_PIN);
1964
1965         spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
1966
1967         /* Deal with negative values */
1968         if (ns >= 0x3ffffff0 && ns <= 0x3fffffff) {
1969                 s--;
1970                 ns &= 0xf;
1971                 ns += 999999984;
1972         }
1973
1974         set_normalized_timespec64(ts, s, ns);
1975         return 0;
1976 }
1977 EXPORT_SYMBOL(ocelot_ptp_gettime64);
1978
1979 static int ocelot_ptp_settime64(struct ptp_clock_info *ptp,
1980                                 const struct timespec64 *ts)
1981 {
1982         struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info);
1983         unsigned long flags;
1984         u32 val;
1985
1986         spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
1987
1988         val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
1989         val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
1990         val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_IDLE);
1991
1992         ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
1993
1994         ocelot_write_rix(ocelot, lower_32_bits(ts->tv_sec), PTP_PIN_TOD_SEC_LSB,
1995                          TOD_ACC_PIN);
1996         ocelot_write_rix(ocelot, upper_32_bits(ts->tv_sec), PTP_PIN_TOD_SEC_MSB,
1997                          TOD_ACC_PIN);
1998         ocelot_write_rix(ocelot, ts->tv_nsec, PTP_PIN_TOD_NSEC, TOD_ACC_PIN);
1999
2000         val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
2001         val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
2002         val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_LOAD);
2003
2004         ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
2005
2006         spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
2007         return 0;
2008 }
2009
2010 static int ocelot_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
2011 {
2012         if (delta > -(NSEC_PER_SEC / 2) && delta < (NSEC_PER_SEC / 2)) {
2013                 struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info);
2014                 unsigned long flags;
2015                 u32 val;
2016
2017                 spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
2018
2019                 val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
2020                 val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
2021                 val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_IDLE);
2022
2023                 ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
2024
2025                 ocelot_write_rix(ocelot, 0, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
2026                 ocelot_write_rix(ocelot, 0, PTP_PIN_TOD_SEC_MSB, TOD_ACC_PIN);
2027                 ocelot_write_rix(ocelot, delta, PTP_PIN_TOD_NSEC, TOD_ACC_PIN);
2028
2029                 val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
2030                 val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
2031                 val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_DELTA);
2032
2033                 ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
2034
2035                 spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
2036         } else {
2037                 /* Fall back using ocelot_ptp_settime64 which is not exact. */
2038                 struct timespec64 ts;
2039                 u64 now;
2040
2041                 ocelot_ptp_gettime64(ptp, &ts);
2042
2043                 now = ktime_to_ns(timespec64_to_ktime(ts));
2044                 ts = ns_to_timespec64(now + delta);
2045
2046                 ocelot_ptp_settime64(ptp, &ts);
2047         }
2048         return 0;
2049 }
2050
2051 static int ocelot_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
2052 {
2053         struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info);
2054         u32 unit = 0, direction = 0;
2055         unsigned long flags;
2056         u64 adj = 0;
2057
2058         spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
2059
2060         if (!scaled_ppm)
2061                 goto disable_adj;
2062
2063         if (scaled_ppm < 0) {
2064                 direction = PTP_CFG_CLK_ADJ_CFG_DIR;
2065                 scaled_ppm = -scaled_ppm;
2066         }
2067
2068         adj = PSEC_PER_SEC << 16;
2069         do_div(adj, scaled_ppm);
2070         do_div(adj, 1000);
2071
2072         /* If the adjustment value is too large, use ns instead */
2073         if (adj >= (1L << 30)) {
2074                 unit = PTP_CFG_CLK_ADJ_FREQ_NS;
2075                 do_div(adj, 1000);
2076         }
2077
2078         /* Still too big */
2079         if (adj >= (1L << 30))
2080                 goto disable_adj;
2081
2082         ocelot_write(ocelot, unit | adj, PTP_CLK_CFG_ADJ_FREQ);
2083         ocelot_write(ocelot, PTP_CFG_CLK_ADJ_CFG_ENA | direction,
2084                      PTP_CLK_CFG_ADJ_CFG);
2085
2086         spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
2087         return 0;
2088
2089 disable_adj:
2090         ocelot_write(ocelot, 0, PTP_CLK_CFG_ADJ_CFG);
2091
2092         spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
2093         return 0;
2094 }
2095
2096 static struct ptp_clock_info ocelot_ptp_clock_info = {
2097         .owner          = THIS_MODULE,
2098         .name           = "ocelot ptp",
2099         .max_adj        = 0x7fffffff,
2100         .n_alarm        = 0,
2101         .n_ext_ts       = 0,
2102         .n_per_out      = 0,
2103         .n_pins         = 0,
2104         .pps            = 0,
2105         .gettime64      = ocelot_ptp_gettime64,
2106         .settime64      = ocelot_ptp_settime64,
2107         .adjtime        = ocelot_ptp_adjtime,
2108         .adjfine        = ocelot_ptp_adjfine,
2109 };
2110
2111 static int ocelot_init_timestamp(struct ocelot *ocelot)
2112 {
2113         ocelot->ptp_info = ocelot_ptp_clock_info;
2114         ocelot->ptp_clock = ptp_clock_register(&ocelot->ptp_info, ocelot->dev);
2115         if (IS_ERR(ocelot->ptp_clock))
2116                 return PTR_ERR(ocelot->ptp_clock);
2117         /* Check if PHC support is missing at the configuration level */
2118         if (!ocelot->ptp_clock)
2119                 return 0;
2120
2121         ocelot_write(ocelot, SYS_PTP_CFG_PTP_STAMP_WID(30), SYS_PTP_CFG);
2122         ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_LOW);
2123         ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_HIGH);
2124
2125         ocelot_write(ocelot, PTP_CFG_MISC_PTP_EN, PTP_CFG_MISC);
2126
2127         /* There is no device reconfiguration, PTP Rx stamping is always
2128          * enabled.
2129          */
2130         ocelot->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
2131
2132         return 0;
2133 }
2134
2135 int ocelot_probe_port(struct ocelot *ocelot, u8 port,
2136                       void __iomem *regs,
2137                       struct phy_device *phy)
2138 {
2139         struct ocelot_port_private *priv;
2140         struct ocelot_port *ocelot_port;
2141         struct net_device *dev;
2142         u32 val;
2143         int err;
2144
2145         dev = alloc_etherdev(sizeof(struct ocelot_port_private));
2146         if (!dev)
2147                 return -ENOMEM;
2148         SET_NETDEV_DEV(dev, ocelot->dev);
2149         priv = netdev_priv(dev);
2150         priv->dev = dev;
2151         priv->phy = phy;
2152         priv->chip_port = port;
2153         ocelot_port = &priv->port;
2154         ocelot_port->ocelot = ocelot;
2155         ocelot_port->regs = regs;
2156         ocelot->ports[port] = ocelot_port;
2157
2158         dev->netdev_ops = &ocelot_port_netdev_ops;
2159         dev->ethtool_ops = &ocelot_ethtool_ops;
2160
2161         dev->hw_features |= NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_RXFCS |
2162                 NETIF_F_HW_TC;
2163         dev->features |= NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_HW_TC;
2164
2165         memcpy(dev->dev_addr, ocelot->base_mac, ETH_ALEN);
2166         dev->dev_addr[ETH_ALEN - 1] += port;
2167         ocelot_mact_learn(ocelot, PGID_CPU, dev->dev_addr, ocelot_port->pvid,
2168                           ENTRYTYPE_LOCKED);
2169
2170         INIT_LIST_HEAD(&ocelot_port->skbs);
2171
2172         err = register_netdev(dev);
2173         if (err) {
2174                 dev_err(ocelot->dev, "register_netdev failed\n");
2175                 goto err_register_netdev;
2176         }
2177
2178         /* Basic L2 initialization */
2179
2180         /* Drop frames with multicast source address */
2181         val = ANA_PORT_DROP_CFG_DROP_MC_SMAC_ENA;
2182         ocelot_rmw_gix(ocelot, val, val, ANA_PORT_DROP_CFG, port);
2183
2184         /* Set default VLAN and tag type to 8021Q. */
2185         ocelot_rmw_gix(ocelot, REW_PORT_VLAN_CFG_PORT_TPID(ETH_P_8021Q),
2186                        REW_PORT_VLAN_CFG_PORT_TPID_M,
2187                        REW_PORT_VLAN_CFG, port);
2188
2189         /* Enable vcap lookups */
2190         ocelot_vcap_enable(ocelot, port);
2191
2192         return 0;
2193
2194 err_register_netdev:
2195         free_netdev(dev);
2196         return err;
2197 }
2198 EXPORT_SYMBOL(ocelot_probe_port);
2199
2200 int ocelot_init(struct ocelot *ocelot)
2201 {
2202         u32 port;
2203         int i, ret, cpu = ocelot->num_phys_ports;
2204         char queue_name[32];
2205
2206         ocelot->lags = devm_kcalloc(ocelot->dev, ocelot->num_phys_ports,
2207                                     sizeof(u32), GFP_KERNEL);
2208         if (!ocelot->lags)
2209                 return -ENOMEM;
2210
2211         ocelot->stats = devm_kcalloc(ocelot->dev,
2212                                      ocelot->num_phys_ports * ocelot->num_stats,
2213                                      sizeof(u64), GFP_KERNEL);
2214         if (!ocelot->stats)
2215                 return -ENOMEM;
2216
2217         mutex_init(&ocelot->stats_lock);
2218         mutex_init(&ocelot->ptp_lock);
2219         spin_lock_init(&ocelot->ptp_clock_lock);
2220         snprintf(queue_name, sizeof(queue_name), "%s-stats",
2221                  dev_name(ocelot->dev));
2222         ocelot->stats_queue = create_singlethread_workqueue(queue_name);
2223         if (!ocelot->stats_queue)
2224                 return -ENOMEM;
2225
2226         ocelot_mact_init(ocelot);
2227         ocelot_vlan_init(ocelot);
2228         ocelot_ace_init(ocelot);
2229
2230         for (port = 0; port < ocelot->num_phys_ports; port++) {
2231                 /* Clear all counters (5 groups) */
2232                 ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(port) |
2233                                      SYS_STAT_CFG_STAT_CLEAR_SHOT(0x7f),
2234                              SYS_STAT_CFG);
2235         }
2236
2237         /* Only use S-Tag */
2238         ocelot_write(ocelot, ETH_P_8021AD, SYS_VLAN_ETYPE_CFG);
2239
2240         /* Aggregation mode */
2241         ocelot_write(ocelot, ANA_AGGR_CFG_AC_SMAC_ENA |
2242                              ANA_AGGR_CFG_AC_DMAC_ENA |
2243                              ANA_AGGR_CFG_AC_IP4_SIPDIP_ENA |
2244                              ANA_AGGR_CFG_AC_IP4_TCPUDP_ENA, ANA_AGGR_CFG);
2245
2246         /* Set MAC age time to default value. The entry is aged after
2247          * 2*AGE_PERIOD
2248          */
2249         ocelot_write(ocelot,
2250                      ANA_AUTOAGE_AGE_PERIOD(BR_DEFAULT_AGEING_TIME / 2 / HZ),
2251                      ANA_AUTOAGE);
2252
2253         /* Disable learning for frames discarded by VLAN ingress filtering */
2254         regmap_field_write(ocelot->regfields[ANA_ADVLEARN_VLAN_CHK], 1);
2255
2256         /* Setup frame ageing - fixed value "2 sec" - in 6.5 us units */
2257         ocelot_write(ocelot, SYS_FRM_AGING_AGE_TX_ENA |
2258                      SYS_FRM_AGING_MAX_AGE(307692), SYS_FRM_AGING);
2259
2260         /* Setup flooding PGIDs */
2261         ocelot_write_rix(ocelot, ANA_FLOODING_FLD_MULTICAST(PGID_MC) |
2262                          ANA_FLOODING_FLD_BROADCAST(PGID_MC) |
2263                          ANA_FLOODING_FLD_UNICAST(PGID_UC),
2264                          ANA_FLOODING, 0);
2265         ocelot_write(ocelot, ANA_FLOODING_IPMC_FLD_MC6_DATA(PGID_MCIPV6) |
2266                      ANA_FLOODING_IPMC_FLD_MC6_CTRL(PGID_MC) |
2267                      ANA_FLOODING_IPMC_FLD_MC4_DATA(PGID_MCIPV4) |
2268                      ANA_FLOODING_IPMC_FLD_MC4_CTRL(PGID_MC),
2269                      ANA_FLOODING_IPMC);
2270
2271         for (port = 0; port < ocelot->num_phys_ports; port++) {
2272                 /* Transmit the frame to the local port. */
2273                 ocelot_write_rix(ocelot, BIT(port), ANA_PGID_PGID, port);
2274                 /* Do not forward BPDU frames to the front ports. */
2275                 ocelot_write_gix(ocelot,
2276                                  ANA_PORT_CPU_FWD_BPDU_CFG_BPDU_REDIR_ENA(0xffff),
2277                                  ANA_PORT_CPU_FWD_BPDU_CFG,
2278                                  port);
2279                 /* Ensure bridging is disabled */
2280                 ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_SRC + port);
2281         }
2282
2283         /* Configure and enable the CPU port. */
2284         ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, cpu);
2285         ocelot_write_rix(ocelot, BIT(cpu), ANA_PGID_PGID, PGID_CPU);
2286         ocelot_write_gix(ocelot, ANA_PORT_PORT_CFG_RECV_ENA |
2287                          ANA_PORT_PORT_CFG_PORTID_VAL(cpu),
2288                          ANA_PORT_PORT_CFG, cpu);
2289
2290         /* Allow broadcast MAC frames. */
2291         for (i = ocelot->num_phys_ports + 1; i < PGID_CPU; i++) {
2292                 u32 val = ANA_PGID_PGID_PGID(GENMASK(ocelot->num_phys_ports - 1, 0));
2293
2294                 ocelot_write_rix(ocelot, val, ANA_PGID_PGID, i);
2295         }
2296         ocelot_write_rix(ocelot,
2297                          ANA_PGID_PGID_PGID(GENMASK(ocelot->num_phys_ports, 0)),
2298                          ANA_PGID_PGID, PGID_MC);
2299         ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_MCIPV4);
2300         ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_MCIPV6);
2301
2302         /* CPU port Injection/Extraction configuration */
2303         ocelot_write_rix(ocelot, QSYS_SWITCH_PORT_MODE_INGRESS_DROP_MODE |
2304                          QSYS_SWITCH_PORT_MODE_SCH_NEXT_CFG(1) |
2305                          QSYS_SWITCH_PORT_MODE_PORT_ENA,
2306                          QSYS_SWITCH_PORT_MODE, cpu);
2307         ocelot_write_rix(ocelot, SYS_PORT_MODE_INCL_XTR_HDR(1) |
2308                          SYS_PORT_MODE_INCL_INJ_HDR(1), SYS_PORT_MODE, cpu);
2309         /* Allow manual injection via DEVCPU_QS registers, and byte swap these
2310          * registers endianness.
2311          */
2312         ocelot_write_rix(ocelot, QS_INJ_GRP_CFG_BYTE_SWAP |
2313                          QS_INJ_GRP_CFG_MODE(1), QS_INJ_GRP_CFG, 0);
2314         ocelot_write_rix(ocelot, QS_XTR_GRP_CFG_BYTE_SWAP |
2315                          QS_XTR_GRP_CFG_MODE(1), QS_XTR_GRP_CFG, 0);
2316         ocelot_write(ocelot, ANA_CPUQ_CFG_CPUQ_MIRROR(2) |
2317                      ANA_CPUQ_CFG_CPUQ_LRN(2) |
2318                      ANA_CPUQ_CFG_CPUQ_MAC_COPY(2) |
2319                      ANA_CPUQ_CFG_CPUQ_SRC_COPY(2) |
2320                      ANA_CPUQ_CFG_CPUQ_LOCKED_PORTMOVE(2) |
2321                      ANA_CPUQ_CFG_CPUQ_ALLBRIDGE(6) |
2322                      ANA_CPUQ_CFG_CPUQ_IPMC_CTRL(6) |
2323                      ANA_CPUQ_CFG_CPUQ_IGMP(6) |
2324                      ANA_CPUQ_CFG_CPUQ_MLD(6), ANA_CPUQ_CFG);
2325         for (i = 0; i < 16; i++)
2326                 ocelot_write_rix(ocelot, ANA_CPUQ_8021_CFG_CPUQ_GARP_VAL(6) |
2327                                  ANA_CPUQ_8021_CFG_CPUQ_BPDU_VAL(6),
2328                                  ANA_CPUQ_8021_CFG, i);
2329
2330         INIT_DELAYED_WORK(&ocelot->stats_work, ocelot_check_stats_work);
2331         queue_delayed_work(ocelot->stats_queue, &ocelot->stats_work,
2332                            OCELOT_STATS_CHECK_DELAY);
2333
2334         if (ocelot->ptp) {
2335                 ret = ocelot_init_timestamp(ocelot);
2336                 if (ret) {
2337                         dev_err(ocelot->dev,
2338                                 "Timestamp initialization failed\n");
2339                         return ret;
2340                 }
2341         }
2342
2343         return 0;
2344 }
2345 EXPORT_SYMBOL(ocelot_init);
2346
2347 void ocelot_deinit(struct ocelot *ocelot)
2348 {
2349         struct list_head *pos, *tmp;
2350         struct ocelot_port *port;
2351         struct ocelot_skb *entry;
2352         int i;
2353
2354         cancel_delayed_work(&ocelot->stats_work);
2355         destroy_workqueue(ocelot->stats_queue);
2356         mutex_destroy(&ocelot->stats_lock);
2357         ocelot_ace_deinit();
2358
2359         for (i = 0; i < ocelot->num_phys_ports; i++) {
2360                 port = ocelot->ports[i];
2361
2362                 list_for_each_safe(pos, tmp, &port->skbs) {
2363                         entry = list_entry(pos, struct ocelot_skb, head);
2364
2365                         list_del(pos);
2366                         dev_kfree_skb_any(entry->skb);
2367                         kfree(entry);
2368                 }
2369         }
2370 }
2371 EXPORT_SYMBOL(ocelot_deinit);
2372
2373 MODULE_LICENSE("Dual MIT/GPL");