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