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 (p == ocelot->cpu || (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                         /* Avoid the NPI port from looping back to itself */
1402                         if (p != ocelot->cpu)
1403                                 mask |= BIT(ocelot->cpu);
1404
1405                         ocelot_write_rix(ocelot, mask,
1406                                          ANA_PGID_PGID, PGID_SRC + p);
1407                 } else {
1408                         /* Only the CPU port, this is compatible with link
1409                          * aggregation.
1410                          */
1411                         ocelot_write_rix(ocelot,
1412                                          BIT(ocelot->cpu),
1413                                          ANA_PGID_PGID, PGID_SRC + p);
1414                 }
1415         }
1416 }
1417
1418 static void ocelot_port_attr_stp_state_set(struct ocelot *ocelot, int port,
1419                                            struct switchdev_trans *trans,
1420                                            u8 state)
1421 {
1422         if (switchdev_trans_ph_prepare(trans))
1423                 return;
1424
1425         ocelot_bridge_stp_state_set(ocelot, port, state);
1426 }
1427
1428 static void ocelot_set_ageing_time(struct ocelot *ocelot, unsigned int msecs)
1429 {
1430         ocelot_write(ocelot, ANA_AUTOAGE_AGE_PERIOD(msecs / 2),
1431                      ANA_AUTOAGE);
1432 }
1433
1434 static void ocelot_port_attr_ageing_set(struct ocelot *ocelot, int port,
1435                                         unsigned long ageing_clock_t)
1436 {
1437         unsigned long ageing_jiffies = clock_t_to_jiffies(ageing_clock_t);
1438         u32 ageing_time = jiffies_to_msecs(ageing_jiffies) / 1000;
1439
1440         ocelot_set_ageing_time(ocelot, ageing_time);
1441 }
1442
1443 static void ocelot_port_attr_mc_set(struct ocelot *ocelot, int port, bool mc)
1444 {
1445         u32 cpu_fwd_mcast = ANA_PORT_CPU_FWD_CFG_CPU_IGMP_REDIR_ENA |
1446                             ANA_PORT_CPU_FWD_CFG_CPU_MLD_REDIR_ENA |
1447                             ANA_PORT_CPU_FWD_CFG_CPU_IPMC_CTRL_COPY_ENA;
1448         u32 val = 0;
1449
1450         if (mc)
1451                 val = cpu_fwd_mcast;
1452
1453         ocelot_rmw_gix(ocelot, val, cpu_fwd_mcast,
1454                        ANA_PORT_CPU_FWD_CFG, port);
1455 }
1456
1457 static int ocelot_port_attr_set(struct net_device *dev,
1458                                 const struct switchdev_attr *attr,
1459                                 struct switchdev_trans *trans)
1460 {
1461         struct ocelot_port_private *priv = netdev_priv(dev);
1462         struct ocelot *ocelot = priv->port.ocelot;
1463         int port = priv->chip_port;
1464         int err = 0;
1465
1466         switch (attr->id) {
1467         case SWITCHDEV_ATTR_ID_PORT_STP_STATE:
1468                 ocelot_port_attr_stp_state_set(ocelot, port, trans,
1469                                                attr->u.stp_state);
1470                 break;
1471         case SWITCHDEV_ATTR_ID_BRIDGE_AGEING_TIME:
1472                 ocelot_port_attr_ageing_set(ocelot, port, attr->u.ageing_time);
1473                 break;
1474         case SWITCHDEV_ATTR_ID_BRIDGE_VLAN_FILTERING:
1475                 priv->vlan_aware = attr->u.vlan_filtering;
1476                 ocelot_port_vlan_filtering(ocelot, port, priv->vlan_aware);
1477                 break;
1478         case SWITCHDEV_ATTR_ID_BRIDGE_MC_DISABLED:
1479                 ocelot_port_attr_mc_set(ocelot, port, !attr->u.mc_disabled);
1480                 break;
1481         default:
1482                 err = -EOPNOTSUPP;
1483                 break;
1484         }
1485
1486         return err;
1487 }
1488
1489 static int ocelot_port_obj_add_vlan(struct net_device *dev,
1490                                     const struct switchdev_obj_port_vlan *vlan,
1491                                     struct switchdev_trans *trans)
1492 {
1493         int ret;
1494         u16 vid;
1495
1496         for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
1497                 ret = ocelot_vlan_vid_add(dev, vid,
1498                                           vlan->flags & BRIDGE_VLAN_INFO_PVID,
1499                                           vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED);
1500                 if (ret)
1501                         return ret;
1502         }
1503
1504         return 0;
1505 }
1506
1507 static int ocelot_port_vlan_del_vlan(struct net_device *dev,
1508                                      const struct switchdev_obj_port_vlan *vlan)
1509 {
1510         int ret;
1511         u16 vid;
1512
1513         for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
1514                 ret = ocelot_vlan_vid_del(dev, vid);
1515
1516                 if (ret)
1517                         return ret;
1518         }
1519
1520         return 0;
1521 }
1522
1523 static struct ocelot_multicast *ocelot_multicast_get(struct ocelot *ocelot,
1524                                                      const unsigned char *addr,
1525                                                      u16 vid)
1526 {
1527         struct ocelot_multicast *mc;
1528
1529         list_for_each_entry(mc, &ocelot->multicast, list) {
1530                 if (ether_addr_equal(mc->addr, addr) && mc->vid == vid)
1531                         return mc;
1532         }
1533
1534         return NULL;
1535 }
1536
1537 static int ocelot_port_obj_add_mdb(struct net_device *dev,
1538                                    const struct switchdev_obj_port_mdb *mdb,
1539                                    struct switchdev_trans *trans)
1540 {
1541         struct ocelot_port_private *priv = netdev_priv(dev);
1542         struct ocelot_port *ocelot_port = &priv->port;
1543         struct ocelot *ocelot = ocelot_port->ocelot;
1544         unsigned char addr[ETH_ALEN];
1545         struct ocelot_multicast *mc;
1546         int port = priv->chip_port;
1547         u16 vid = mdb->vid;
1548         bool new = false;
1549
1550         if (!vid)
1551                 vid = ocelot_port->pvid;
1552
1553         mc = ocelot_multicast_get(ocelot, mdb->addr, vid);
1554         if (!mc) {
1555                 mc = devm_kzalloc(ocelot->dev, sizeof(*mc), GFP_KERNEL);
1556                 if (!mc)
1557                         return -ENOMEM;
1558
1559                 memcpy(mc->addr, mdb->addr, ETH_ALEN);
1560                 mc->vid = vid;
1561
1562                 list_add_tail(&mc->list, &ocelot->multicast);
1563                 new = true;
1564         }
1565
1566         memcpy(addr, mc->addr, ETH_ALEN);
1567         addr[0] = 0;
1568
1569         if (!new) {
1570                 addr[2] = mc->ports << 0;
1571                 addr[1] = mc->ports << 8;
1572                 ocelot_mact_forget(ocelot, addr, vid);
1573         }
1574
1575         mc->ports |= BIT(port);
1576         addr[2] = mc->ports << 0;
1577         addr[1] = mc->ports << 8;
1578
1579         return ocelot_mact_learn(ocelot, 0, addr, vid, ENTRYTYPE_MACv4);
1580 }
1581
1582 static int ocelot_port_obj_del_mdb(struct net_device *dev,
1583                                    const struct switchdev_obj_port_mdb *mdb)
1584 {
1585         struct ocelot_port_private *priv = netdev_priv(dev);
1586         struct ocelot_port *ocelot_port = &priv->port;
1587         struct ocelot *ocelot = ocelot_port->ocelot;
1588         unsigned char addr[ETH_ALEN];
1589         struct ocelot_multicast *mc;
1590         int port = priv->chip_port;
1591         u16 vid = mdb->vid;
1592
1593         if (!vid)
1594                 vid = ocelot_port->pvid;
1595
1596         mc = ocelot_multicast_get(ocelot, mdb->addr, vid);
1597         if (!mc)
1598                 return -ENOENT;
1599
1600         memcpy(addr, mc->addr, ETH_ALEN);
1601         addr[2] = mc->ports << 0;
1602         addr[1] = mc->ports << 8;
1603         addr[0] = 0;
1604         ocelot_mact_forget(ocelot, addr, vid);
1605
1606         mc->ports &= ~BIT(port);
1607         if (!mc->ports) {
1608                 list_del(&mc->list);
1609                 devm_kfree(ocelot->dev, mc);
1610                 return 0;
1611         }
1612
1613         addr[2] = mc->ports << 0;
1614         addr[1] = mc->ports << 8;
1615
1616         return ocelot_mact_learn(ocelot, 0, addr, vid, ENTRYTYPE_MACv4);
1617 }
1618
1619 static int ocelot_port_obj_add(struct net_device *dev,
1620                                const struct switchdev_obj *obj,
1621                                struct switchdev_trans *trans,
1622                                struct netlink_ext_ack *extack)
1623 {
1624         int ret = 0;
1625
1626         switch (obj->id) {
1627         case SWITCHDEV_OBJ_ID_PORT_VLAN:
1628                 ret = ocelot_port_obj_add_vlan(dev,
1629                                                SWITCHDEV_OBJ_PORT_VLAN(obj),
1630                                                trans);
1631                 break;
1632         case SWITCHDEV_OBJ_ID_PORT_MDB:
1633                 ret = ocelot_port_obj_add_mdb(dev, SWITCHDEV_OBJ_PORT_MDB(obj),
1634                                               trans);
1635                 break;
1636         default:
1637                 return -EOPNOTSUPP;
1638         }
1639
1640         return ret;
1641 }
1642
1643 static int ocelot_port_obj_del(struct net_device *dev,
1644                                const struct switchdev_obj *obj)
1645 {
1646         int ret = 0;
1647
1648         switch (obj->id) {
1649         case SWITCHDEV_OBJ_ID_PORT_VLAN:
1650                 ret = ocelot_port_vlan_del_vlan(dev,
1651                                                 SWITCHDEV_OBJ_PORT_VLAN(obj));
1652                 break;
1653         case SWITCHDEV_OBJ_ID_PORT_MDB:
1654                 ret = ocelot_port_obj_del_mdb(dev, SWITCHDEV_OBJ_PORT_MDB(obj));
1655                 break;
1656         default:
1657                 return -EOPNOTSUPP;
1658         }
1659
1660         return ret;
1661 }
1662
1663 static int ocelot_port_bridge_join(struct ocelot *ocelot, int port,
1664                                    struct net_device *bridge)
1665 {
1666         if (!ocelot->bridge_mask) {
1667                 ocelot->hw_bridge_dev = bridge;
1668         } else {
1669                 if (ocelot->hw_bridge_dev != bridge)
1670                         /* This is adding the port to a second bridge, this is
1671                          * unsupported */
1672                         return -ENODEV;
1673         }
1674
1675         ocelot->bridge_mask |= BIT(port);
1676
1677         return 0;
1678 }
1679
1680 static int ocelot_port_bridge_leave(struct ocelot *ocelot, int port,
1681                                     struct net_device *bridge)
1682 {
1683         ocelot->bridge_mask &= ~BIT(port);
1684
1685         if (!ocelot->bridge_mask)
1686                 ocelot->hw_bridge_dev = NULL;
1687
1688         ocelot_port_vlan_filtering(ocelot, port, 0);
1689         ocelot_port_set_pvid(ocelot, port, 0);
1690         return ocelot_port_set_native_vlan(ocelot, port, 0);
1691 }
1692
1693 static void ocelot_set_aggr_pgids(struct ocelot *ocelot)
1694 {
1695         int i, port, lag;
1696
1697         /* Reset destination and aggregation PGIDS */
1698         for (port = 0; port < ocelot->num_phys_ports; port++)
1699                 ocelot_write_rix(ocelot, BIT(port), ANA_PGID_PGID, port);
1700
1701         for (i = PGID_AGGR; i < PGID_SRC; i++)
1702                 ocelot_write_rix(ocelot, GENMASK(ocelot->num_phys_ports - 1, 0),
1703                                  ANA_PGID_PGID, i);
1704
1705         /* Now, set PGIDs for each LAG */
1706         for (lag = 0; lag < ocelot->num_phys_ports; lag++) {
1707                 unsigned long bond_mask;
1708                 int aggr_count = 0;
1709                 u8 aggr_idx[16];
1710
1711                 bond_mask = ocelot->lags[lag];
1712                 if (!bond_mask)
1713                         continue;
1714
1715                 for_each_set_bit(port, &bond_mask, ocelot->num_phys_ports) {
1716                         // Destination mask
1717                         ocelot_write_rix(ocelot, bond_mask,
1718                                          ANA_PGID_PGID, port);
1719                         aggr_idx[aggr_count] = port;
1720                         aggr_count++;
1721                 }
1722
1723                 for (i = PGID_AGGR; i < PGID_SRC; i++) {
1724                         u32 ac;
1725
1726                         ac = ocelot_read_rix(ocelot, ANA_PGID_PGID, i);
1727                         ac &= ~bond_mask;
1728                         ac |= BIT(aggr_idx[i % aggr_count]);
1729                         ocelot_write_rix(ocelot, ac, ANA_PGID_PGID, i);
1730                 }
1731         }
1732 }
1733
1734 static void ocelot_setup_lag(struct ocelot *ocelot, int lag)
1735 {
1736         unsigned long bond_mask = ocelot->lags[lag];
1737         unsigned int p;
1738
1739         for_each_set_bit(p, &bond_mask, ocelot->num_phys_ports) {
1740                 u32 port_cfg = ocelot_read_gix(ocelot, ANA_PORT_PORT_CFG, p);
1741
1742                 port_cfg &= ~ANA_PORT_PORT_CFG_PORTID_VAL_M;
1743
1744                 /* Use lag port as logical port for port i */
1745                 ocelot_write_gix(ocelot, port_cfg |
1746                                  ANA_PORT_PORT_CFG_PORTID_VAL(lag),
1747                                  ANA_PORT_PORT_CFG, p);
1748         }
1749 }
1750
1751 static int ocelot_port_lag_join(struct ocelot *ocelot, int port,
1752                                 struct net_device *bond)
1753 {
1754         struct net_device *ndev;
1755         u32 bond_mask = 0;
1756         int lag, lp;
1757
1758         rcu_read_lock();
1759         for_each_netdev_in_bond_rcu(bond, ndev) {
1760                 struct ocelot_port_private *priv = netdev_priv(ndev);
1761
1762                 bond_mask |= BIT(priv->chip_port);
1763         }
1764         rcu_read_unlock();
1765
1766         lp = __ffs(bond_mask);
1767
1768         /* If the new port is the lowest one, use it as the logical port from
1769          * now on
1770          */
1771         if (port == lp) {
1772                 lag = port;
1773                 ocelot->lags[port] = bond_mask;
1774                 bond_mask &= ~BIT(port);
1775                 if (bond_mask) {
1776                         lp = __ffs(bond_mask);
1777                         ocelot->lags[lp] = 0;
1778                 }
1779         } else {
1780                 lag = lp;
1781                 ocelot->lags[lp] |= BIT(port);
1782         }
1783
1784         ocelot_setup_lag(ocelot, lag);
1785         ocelot_set_aggr_pgids(ocelot);
1786
1787         return 0;
1788 }
1789
1790 static void ocelot_port_lag_leave(struct ocelot *ocelot, int port,
1791                                   struct net_device *bond)
1792 {
1793         u32 port_cfg;
1794         int i;
1795
1796         /* Remove port from any lag */
1797         for (i = 0; i < ocelot->num_phys_ports; i++)
1798                 ocelot->lags[i] &= ~BIT(port);
1799
1800         /* if it was the logical port of the lag, move the lag config to the
1801          * next port
1802          */
1803         if (ocelot->lags[port]) {
1804                 int n = __ffs(ocelot->lags[port]);
1805
1806                 ocelot->lags[n] = ocelot->lags[port];
1807                 ocelot->lags[port] = 0;
1808
1809                 ocelot_setup_lag(ocelot, n);
1810         }
1811
1812         port_cfg = ocelot_read_gix(ocelot, ANA_PORT_PORT_CFG, port);
1813         port_cfg &= ~ANA_PORT_PORT_CFG_PORTID_VAL_M;
1814         ocelot_write_gix(ocelot, port_cfg | ANA_PORT_PORT_CFG_PORTID_VAL(port),
1815                          ANA_PORT_PORT_CFG, port);
1816
1817         ocelot_set_aggr_pgids(ocelot);
1818 }
1819
1820 /* Checks if the net_device instance given to us originate from our driver. */
1821 static bool ocelot_netdevice_dev_check(const struct net_device *dev)
1822 {
1823         return dev->netdev_ops == &ocelot_port_netdev_ops;
1824 }
1825
1826 static int ocelot_netdevice_port_event(struct net_device *dev,
1827                                        unsigned long event,
1828                                        struct netdev_notifier_changeupper_info *info)
1829 {
1830         struct ocelot_port_private *priv = netdev_priv(dev);
1831         struct ocelot_port *ocelot_port = &priv->port;
1832         struct ocelot *ocelot = ocelot_port->ocelot;
1833         int port = priv->chip_port;
1834         int err = 0;
1835
1836         switch (event) {
1837         case NETDEV_CHANGEUPPER:
1838                 if (netif_is_bridge_master(info->upper_dev)) {
1839                         if (info->linking) {
1840                                 err = ocelot_port_bridge_join(ocelot, port,
1841                                                               info->upper_dev);
1842                         } else {
1843                                 err = ocelot_port_bridge_leave(ocelot, port,
1844                                                                info->upper_dev);
1845                                 priv->vlan_aware = false;
1846                         }
1847                 }
1848                 if (netif_is_lag_master(info->upper_dev)) {
1849                         if (info->linking)
1850                                 err = ocelot_port_lag_join(ocelot, port,
1851                                                            info->upper_dev);
1852                         else
1853                                 ocelot_port_lag_leave(ocelot, port,
1854                                                       info->upper_dev);
1855                 }
1856                 break;
1857         default:
1858                 break;
1859         }
1860
1861         return err;
1862 }
1863
1864 static int ocelot_netdevice_event(struct notifier_block *unused,
1865                                   unsigned long event, void *ptr)
1866 {
1867         struct netdev_notifier_changeupper_info *info = ptr;
1868         struct net_device *dev = netdev_notifier_info_to_dev(ptr);
1869         int ret = 0;
1870
1871         if (!ocelot_netdevice_dev_check(dev))
1872                 return 0;
1873
1874         if (event == NETDEV_PRECHANGEUPPER &&
1875             netif_is_lag_master(info->upper_dev)) {
1876                 struct netdev_lag_upper_info *lag_upper_info = info->upper_info;
1877                 struct netlink_ext_ack *extack;
1878
1879                 if (lag_upper_info &&
1880                     lag_upper_info->tx_type != NETDEV_LAG_TX_TYPE_HASH) {
1881                         extack = netdev_notifier_info_to_extack(&info->info);
1882                         NL_SET_ERR_MSG_MOD(extack, "LAG device using unsupported Tx type");
1883
1884                         ret = -EINVAL;
1885                         goto notify;
1886                 }
1887         }
1888
1889         if (netif_is_lag_master(dev)) {
1890                 struct net_device *slave;
1891                 struct list_head *iter;
1892
1893                 netdev_for_each_lower_dev(dev, slave, iter) {
1894                         ret = ocelot_netdevice_port_event(slave, event, info);
1895                         if (ret)
1896                                 goto notify;
1897                 }
1898         } else {
1899                 ret = ocelot_netdevice_port_event(dev, event, info);
1900         }
1901
1902 notify:
1903         return notifier_from_errno(ret);
1904 }
1905
1906 struct notifier_block ocelot_netdevice_nb __read_mostly = {
1907         .notifier_call = ocelot_netdevice_event,
1908 };
1909 EXPORT_SYMBOL(ocelot_netdevice_nb);
1910
1911 static int ocelot_switchdev_event(struct notifier_block *unused,
1912                                   unsigned long event, void *ptr)
1913 {
1914         struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
1915         int err;
1916
1917         switch (event) {
1918         case SWITCHDEV_PORT_ATTR_SET:
1919                 err = switchdev_handle_port_attr_set(dev, ptr,
1920                                                      ocelot_netdevice_dev_check,
1921                                                      ocelot_port_attr_set);
1922                 return notifier_from_errno(err);
1923         }
1924
1925         return NOTIFY_DONE;
1926 }
1927
1928 struct notifier_block ocelot_switchdev_nb __read_mostly = {
1929         .notifier_call = ocelot_switchdev_event,
1930 };
1931 EXPORT_SYMBOL(ocelot_switchdev_nb);
1932
1933 static int ocelot_switchdev_blocking_event(struct notifier_block *unused,
1934                                            unsigned long event, void *ptr)
1935 {
1936         struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
1937         int err;
1938
1939         switch (event) {
1940                 /* Blocking events. */
1941         case SWITCHDEV_PORT_OBJ_ADD:
1942                 err = switchdev_handle_port_obj_add(dev, ptr,
1943                                                     ocelot_netdevice_dev_check,
1944                                                     ocelot_port_obj_add);
1945                 return notifier_from_errno(err);
1946         case SWITCHDEV_PORT_OBJ_DEL:
1947                 err = switchdev_handle_port_obj_del(dev, ptr,
1948                                                     ocelot_netdevice_dev_check,
1949                                                     ocelot_port_obj_del);
1950                 return notifier_from_errno(err);
1951         case SWITCHDEV_PORT_ATTR_SET:
1952                 err = switchdev_handle_port_attr_set(dev, ptr,
1953                                                      ocelot_netdevice_dev_check,
1954                                                      ocelot_port_attr_set);
1955                 return notifier_from_errno(err);
1956         }
1957
1958         return NOTIFY_DONE;
1959 }
1960
1961 struct notifier_block ocelot_switchdev_blocking_nb __read_mostly = {
1962         .notifier_call = ocelot_switchdev_blocking_event,
1963 };
1964 EXPORT_SYMBOL(ocelot_switchdev_blocking_nb);
1965
1966 int ocelot_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts)
1967 {
1968         struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info);
1969         unsigned long flags;
1970         time64_t s;
1971         u32 val;
1972         s64 ns;
1973
1974         spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
1975
1976         val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
1977         val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
1978         val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE);
1979         ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
1980
1981         s = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_MSB, TOD_ACC_PIN) & 0xffff;
1982         s <<= 32;
1983         s += ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
1984         ns = ocelot_read_rix(ocelot, PTP_PIN_TOD_NSEC, TOD_ACC_PIN);
1985
1986         spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
1987
1988         /* Deal with negative values */
1989         if (ns >= 0x3ffffff0 && ns <= 0x3fffffff) {
1990                 s--;
1991                 ns &= 0xf;
1992                 ns += 999999984;
1993         }
1994
1995         set_normalized_timespec64(ts, s, ns);
1996         return 0;
1997 }
1998 EXPORT_SYMBOL(ocelot_ptp_gettime64);
1999
2000 static int ocelot_ptp_settime64(struct ptp_clock_info *ptp,
2001                                 const struct timespec64 *ts)
2002 {
2003         struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info);
2004         unsigned long flags;
2005         u32 val;
2006
2007         spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
2008
2009         val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
2010         val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
2011         val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_IDLE);
2012
2013         ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
2014
2015         ocelot_write_rix(ocelot, lower_32_bits(ts->tv_sec), PTP_PIN_TOD_SEC_LSB,
2016                          TOD_ACC_PIN);
2017         ocelot_write_rix(ocelot, upper_32_bits(ts->tv_sec), PTP_PIN_TOD_SEC_MSB,
2018                          TOD_ACC_PIN);
2019         ocelot_write_rix(ocelot, ts->tv_nsec, PTP_PIN_TOD_NSEC, TOD_ACC_PIN);
2020
2021         val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
2022         val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
2023         val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_LOAD);
2024
2025         ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
2026
2027         spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
2028         return 0;
2029 }
2030
2031 static int ocelot_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
2032 {
2033         if (delta > -(NSEC_PER_SEC / 2) && delta < (NSEC_PER_SEC / 2)) {
2034                 struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info);
2035                 unsigned long flags;
2036                 u32 val;
2037
2038                 spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
2039
2040                 val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
2041                 val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
2042                 val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_IDLE);
2043
2044                 ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
2045
2046                 ocelot_write_rix(ocelot, 0, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
2047                 ocelot_write_rix(ocelot, 0, PTP_PIN_TOD_SEC_MSB, TOD_ACC_PIN);
2048                 ocelot_write_rix(ocelot, delta, PTP_PIN_TOD_NSEC, TOD_ACC_PIN);
2049
2050                 val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
2051                 val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
2052                 val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_DELTA);
2053
2054                 ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
2055
2056                 spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
2057         } else {
2058                 /* Fall back using ocelot_ptp_settime64 which is not exact. */
2059                 struct timespec64 ts;
2060                 u64 now;
2061
2062                 ocelot_ptp_gettime64(ptp, &ts);
2063
2064                 now = ktime_to_ns(timespec64_to_ktime(ts));
2065                 ts = ns_to_timespec64(now + delta);
2066
2067                 ocelot_ptp_settime64(ptp, &ts);
2068         }
2069         return 0;
2070 }
2071
2072 static int ocelot_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
2073 {
2074         struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info);
2075         u32 unit = 0, direction = 0;
2076         unsigned long flags;
2077         u64 adj = 0;
2078
2079         spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
2080
2081         if (!scaled_ppm)
2082                 goto disable_adj;
2083
2084         if (scaled_ppm < 0) {
2085                 direction = PTP_CFG_CLK_ADJ_CFG_DIR;
2086                 scaled_ppm = -scaled_ppm;
2087         }
2088
2089         adj = PSEC_PER_SEC << 16;
2090         do_div(adj, scaled_ppm);
2091         do_div(adj, 1000);
2092
2093         /* If the adjustment value is too large, use ns instead */
2094         if (adj >= (1L << 30)) {
2095                 unit = PTP_CFG_CLK_ADJ_FREQ_NS;
2096                 do_div(adj, 1000);
2097         }
2098
2099         /* Still too big */
2100         if (adj >= (1L << 30))
2101                 goto disable_adj;
2102
2103         ocelot_write(ocelot, unit | adj, PTP_CLK_CFG_ADJ_FREQ);
2104         ocelot_write(ocelot, PTP_CFG_CLK_ADJ_CFG_ENA | direction,
2105                      PTP_CLK_CFG_ADJ_CFG);
2106
2107         spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
2108         return 0;
2109
2110 disable_adj:
2111         ocelot_write(ocelot, 0, PTP_CLK_CFG_ADJ_CFG);
2112
2113         spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
2114         return 0;
2115 }
2116
2117 static struct ptp_clock_info ocelot_ptp_clock_info = {
2118         .owner          = THIS_MODULE,
2119         .name           = "ocelot ptp",
2120         .max_adj        = 0x7fffffff,
2121         .n_alarm        = 0,
2122         .n_ext_ts       = 0,
2123         .n_per_out      = 0,
2124         .n_pins         = 0,
2125         .pps            = 0,
2126         .gettime64      = ocelot_ptp_gettime64,
2127         .settime64      = ocelot_ptp_settime64,
2128         .adjtime        = ocelot_ptp_adjtime,
2129         .adjfine        = ocelot_ptp_adjfine,
2130 };
2131
2132 static int ocelot_init_timestamp(struct ocelot *ocelot)
2133 {
2134         ocelot->ptp_info = ocelot_ptp_clock_info;
2135         ocelot->ptp_clock = ptp_clock_register(&ocelot->ptp_info, ocelot->dev);
2136         if (IS_ERR(ocelot->ptp_clock))
2137                 return PTR_ERR(ocelot->ptp_clock);
2138         /* Check if PHC support is missing at the configuration level */
2139         if (!ocelot->ptp_clock)
2140                 return 0;
2141
2142         ocelot_write(ocelot, SYS_PTP_CFG_PTP_STAMP_WID(30), SYS_PTP_CFG);
2143         ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_LOW);
2144         ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_HIGH);
2145
2146         ocelot_write(ocelot, PTP_CFG_MISC_PTP_EN, PTP_CFG_MISC);
2147
2148         /* There is no device reconfiguration, PTP Rx stamping is always
2149          * enabled.
2150          */
2151         ocelot->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
2152
2153         return 0;
2154 }
2155
2156 static void ocelot_init_port(struct ocelot *ocelot, int port)
2157 {
2158         struct ocelot_port *ocelot_port = ocelot->ports[port];
2159
2160         INIT_LIST_HEAD(&ocelot_port->skbs);
2161
2162         /* Basic L2 initialization */
2163
2164         /* Drop frames with multicast source address */
2165         ocelot_rmw_gix(ocelot, ANA_PORT_DROP_CFG_DROP_MC_SMAC_ENA,
2166                        ANA_PORT_DROP_CFG_DROP_MC_SMAC_ENA,
2167                        ANA_PORT_DROP_CFG, port);
2168
2169         /* Set default VLAN and tag type to 8021Q. */
2170         ocelot_rmw_gix(ocelot, REW_PORT_VLAN_CFG_PORT_TPID(ETH_P_8021Q),
2171                        REW_PORT_VLAN_CFG_PORT_TPID_M,
2172                        REW_PORT_VLAN_CFG, port);
2173
2174         /* Enable vcap lookups */
2175         ocelot_vcap_enable(ocelot, port);
2176 }
2177
2178 int ocelot_probe_port(struct ocelot *ocelot, u8 port,
2179                       void __iomem *regs,
2180                       struct phy_device *phy)
2181 {
2182         struct ocelot_port_private *priv;
2183         struct ocelot_port *ocelot_port;
2184         struct net_device *dev;
2185         int err;
2186
2187         dev = alloc_etherdev(sizeof(struct ocelot_port_private));
2188         if (!dev)
2189                 return -ENOMEM;
2190         SET_NETDEV_DEV(dev, ocelot->dev);
2191         priv = netdev_priv(dev);
2192         priv->dev = dev;
2193         priv->phy = phy;
2194         priv->chip_port = port;
2195         ocelot_port = &priv->port;
2196         ocelot_port->ocelot = ocelot;
2197         ocelot_port->regs = regs;
2198         ocelot->ports[port] = ocelot_port;
2199
2200         dev->netdev_ops = &ocelot_port_netdev_ops;
2201         dev->ethtool_ops = &ocelot_ethtool_ops;
2202
2203         dev->hw_features |= NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_RXFCS |
2204                 NETIF_F_HW_TC;
2205         dev->features |= NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_HW_TC;
2206
2207         memcpy(dev->dev_addr, ocelot->base_mac, ETH_ALEN);
2208         dev->dev_addr[ETH_ALEN - 1] += port;
2209         ocelot_mact_learn(ocelot, PGID_CPU, dev->dev_addr, ocelot_port->pvid,
2210                           ENTRYTYPE_LOCKED);
2211
2212         ocelot_init_port(ocelot, port);
2213
2214         err = register_netdev(dev);
2215         if (err) {
2216                 dev_err(ocelot->dev, "register_netdev failed\n");
2217                 free_netdev(dev);
2218         }
2219
2220         return err;
2221 }
2222 EXPORT_SYMBOL(ocelot_probe_port);
2223
2224 void ocelot_set_cpu_port(struct ocelot *ocelot, int cpu,
2225                          enum ocelot_tag_prefix injection,
2226                          enum ocelot_tag_prefix extraction)
2227 {
2228         /* Configure and enable the CPU port. */
2229         ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, cpu);
2230         ocelot_write_rix(ocelot, BIT(cpu), ANA_PGID_PGID, PGID_CPU);
2231         ocelot_write_gix(ocelot, ANA_PORT_PORT_CFG_RECV_ENA |
2232                          ANA_PORT_PORT_CFG_PORTID_VAL(cpu),
2233                          ANA_PORT_PORT_CFG, cpu);
2234
2235         /* If the CPU port is a physical port, set up the port in Node
2236          * Processor Interface (NPI) mode. This is the mode through which
2237          * frames can be injected from and extracted to an external CPU.
2238          * Only one port can be an NPI at the same time.
2239          */
2240         if (cpu < ocelot->num_phys_ports) {
2241                 ocelot_write(ocelot, QSYS_EXT_CPU_CFG_EXT_CPUQ_MSK_M |
2242                              QSYS_EXT_CPU_CFG_EXT_CPU_PORT(cpu),
2243                              QSYS_EXT_CPU_CFG);
2244         }
2245
2246         /* CPU port Injection/Extraction configuration */
2247         ocelot_write_rix(ocelot, QSYS_SWITCH_PORT_MODE_INGRESS_DROP_MODE |
2248                          QSYS_SWITCH_PORT_MODE_SCH_NEXT_CFG(1) |
2249                          QSYS_SWITCH_PORT_MODE_PORT_ENA,
2250                          QSYS_SWITCH_PORT_MODE, cpu);
2251         ocelot_write_rix(ocelot, SYS_PORT_MODE_INCL_XTR_HDR(extraction) |
2252                          SYS_PORT_MODE_INCL_INJ_HDR(injection),
2253                          SYS_PORT_MODE, cpu);
2254
2255         /* Configure the CPU port to be VLAN aware */
2256         ocelot_write_gix(ocelot, ANA_PORT_VLAN_CFG_VLAN_VID(0) |
2257                                  ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
2258                                  ANA_PORT_VLAN_CFG_VLAN_POP_CNT(1),
2259                          ANA_PORT_VLAN_CFG, cpu);
2260
2261         ocelot->cpu = cpu;
2262 }
2263 EXPORT_SYMBOL(ocelot_set_cpu_port);
2264
2265 int ocelot_init(struct ocelot *ocelot)
2266 {
2267         char queue_name[32];
2268         int i, ret;
2269         u32 port;
2270
2271         ocelot->lags = devm_kcalloc(ocelot->dev, ocelot->num_phys_ports,
2272                                     sizeof(u32), GFP_KERNEL);
2273         if (!ocelot->lags)
2274                 return -ENOMEM;
2275
2276         ocelot->stats = devm_kcalloc(ocelot->dev,
2277                                      ocelot->num_phys_ports * ocelot->num_stats,
2278                                      sizeof(u64), GFP_KERNEL);
2279         if (!ocelot->stats)
2280                 return -ENOMEM;
2281
2282         mutex_init(&ocelot->stats_lock);
2283         mutex_init(&ocelot->ptp_lock);
2284         spin_lock_init(&ocelot->ptp_clock_lock);
2285         snprintf(queue_name, sizeof(queue_name), "%s-stats",
2286                  dev_name(ocelot->dev));
2287         ocelot->stats_queue = create_singlethread_workqueue(queue_name);
2288         if (!ocelot->stats_queue)
2289                 return -ENOMEM;
2290
2291         INIT_LIST_HEAD(&ocelot->multicast);
2292         ocelot_mact_init(ocelot);
2293         ocelot_vlan_init(ocelot);
2294         ocelot_ace_init(ocelot);
2295
2296         for (port = 0; port < ocelot->num_phys_ports; port++) {
2297                 /* Clear all counters (5 groups) */
2298                 ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(port) |
2299                                      SYS_STAT_CFG_STAT_CLEAR_SHOT(0x7f),
2300                              SYS_STAT_CFG);
2301         }
2302
2303         /* Only use S-Tag */
2304         ocelot_write(ocelot, ETH_P_8021AD, SYS_VLAN_ETYPE_CFG);
2305
2306         /* Aggregation mode */
2307         ocelot_write(ocelot, ANA_AGGR_CFG_AC_SMAC_ENA |
2308                              ANA_AGGR_CFG_AC_DMAC_ENA |
2309                              ANA_AGGR_CFG_AC_IP4_SIPDIP_ENA |
2310                              ANA_AGGR_CFG_AC_IP4_TCPUDP_ENA, ANA_AGGR_CFG);
2311
2312         /* Set MAC age time to default value. The entry is aged after
2313          * 2*AGE_PERIOD
2314          */
2315         ocelot_write(ocelot,
2316                      ANA_AUTOAGE_AGE_PERIOD(BR_DEFAULT_AGEING_TIME / 2 / HZ),
2317                      ANA_AUTOAGE);
2318
2319         /* Disable learning for frames discarded by VLAN ingress filtering */
2320         regmap_field_write(ocelot->regfields[ANA_ADVLEARN_VLAN_CHK], 1);
2321
2322         /* Setup frame ageing - fixed value "2 sec" - in 6.5 us units */
2323         ocelot_write(ocelot, SYS_FRM_AGING_AGE_TX_ENA |
2324                      SYS_FRM_AGING_MAX_AGE(307692), SYS_FRM_AGING);
2325
2326         /* Setup flooding PGIDs */
2327         ocelot_write_rix(ocelot, ANA_FLOODING_FLD_MULTICAST(PGID_MC) |
2328                          ANA_FLOODING_FLD_BROADCAST(PGID_MC) |
2329                          ANA_FLOODING_FLD_UNICAST(PGID_UC),
2330                          ANA_FLOODING, 0);
2331         ocelot_write(ocelot, ANA_FLOODING_IPMC_FLD_MC6_DATA(PGID_MCIPV6) |
2332                      ANA_FLOODING_IPMC_FLD_MC6_CTRL(PGID_MC) |
2333                      ANA_FLOODING_IPMC_FLD_MC4_DATA(PGID_MCIPV4) |
2334                      ANA_FLOODING_IPMC_FLD_MC4_CTRL(PGID_MC),
2335                      ANA_FLOODING_IPMC);
2336
2337         for (port = 0; port < ocelot->num_phys_ports; port++) {
2338                 /* Transmit the frame to the local port. */
2339                 ocelot_write_rix(ocelot, BIT(port), ANA_PGID_PGID, port);
2340                 /* Do not forward BPDU frames to the front ports. */
2341                 ocelot_write_gix(ocelot,
2342                                  ANA_PORT_CPU_FWD_BPDU_CFG_BPDU_REDIR_ENA(0xffff),
2343                                  ANA_PORT_CPU_FWD_BPDU_CFG,
2344                                  port);
2345                 /* Ensure bridging is disabled */
2346                 ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_SRC + port);
2347         }
2348
2349         /* Allow broadcast MAC frames. */
2350         for (i = ocelot->num_phys_ports + 1; i < PGID_CPU; i++) {
2351                 u32 val = ANA_PGID_PGID_PGID(GENMASK(ocelot->num_phys_ports - 1, 0));
2352
2353                 ocelot_write_rix(ocelot, val, ANA_PGID_PGID, i);
2354         }
2355         ocelot_write_rix(ocelot,
2356                          ANA_PGID_PGID_PGID(GENMASK(ocelot->num_phys_ports, 0)),
2357                          ANA_PGID_PGID, PGID_MC);
2358         ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_MCIPV4);
2359         ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_MCIPV6);
2360
2361         /* Allow manual injection via DEVCPU_QS registers, and byte swap these
2362          * registers endianness.
2363          */
2364         ocelot_write_rix(ocelot, QS_INJ_GRP_CFG_BYTE_SWAP |
2365                          QS_INJ_GRP_CFG_MODE(1), QS_INJ_GRP_CFG, 0);
2366         ocelot_write_rix(ocelot, QS_XTR_GRP_CFG_BYTE_SWAP |
2367                          QS_XTR_GRP_CFG_MODE(1), QS_XTR_GRP_CFG, 0);
2368         ocelot_write(ocelot, ANA_CPUQ_CFG_CPUQ_MIRROR(2) |
2369                      ANA_CPUQ_CFG_CPUQ_LRN(2) |
2370                      ANA_CPUQ_CFG_CPUQ_MAC_COPY(2) |
2371                      ANA_CPUQ_CFG_CPUQ_SRC_COPY(2) |
2372                      ANA_CPUQ_CFG_CPUQ_LOCKED_PORTMOVE(2) |
2373                      ANA_CPUQ_CFG_CPUQ_ALLBRIDGE(6) |
2374                      ANA_CPUQ_CFG_CPUQ_IPMC_CTRL(6) |
2375                      ANA_CPUQ_CFG_CPUQ_IGMP(6) |
2376                      ANA_CPUQ_CFG_CPUQ_MLD(6), ANA_CPUQ_CFG);
2377         for (i = 0; i < 16; i++)
2378                 ocelot_write_rix(ocelot, ANA_CPUQ_8021_CFG_CPUQ_GARP_VAL(6) |
2379                                  ANA_CPUQ_8021_CFG_CPUQ_BPDU_VAL(6),
2380                                  ANA_CPUQ_8021_CFG, i);
2381
2382         INIT_DELAYED_WORK(&ocelot->stats_work, ocelot_check_stats_work);
2383         queue_delayed_work(ocelot->stats_queue, &ocelot->stats_work,
2384                            OCELOT_STATS_CHECK_DELAY);
2385
2386         if (ocelot->ptp) {
2387                 ret = ocelot_init_timestamp(ocelot);
2388                 if (ret) {
2389                         dev_err(ocelot->dev,
2390                                 "Timestamp initialization failed\n");
2391                         return ret;
2392                 }
2393         }
2394
2395         return 0;
2396 }
2397 EXPORT_SYMBOL(ocelot_init);
2398
2399 void ocelot_deinit(struct ocelot *ocelot)
2400 {
2401         struct list_head *pos, *tmp;
2402         struct ocelot_port *port;
2403         struct ocelot_skb *entry;
2404         int i;
2405
2406         cancel_delayed_work(&ocelot->stats_work);
2407         destroy_workqueue(ocelot->stats_queue);
2408         mutex_destroy(&ocelot->stats_lock);
2409         ocelot_ace_deinit();
2410
2411         for (i = 0; i < ocelot->num_phys_ports; i++) {
2412                 port = ocelot->ports[i];
2413
2414                 list_for_each_safe(pos, tmp, &port->skbs) {
2415                         entry = list_entry(pos, struct ocelot_skb, head);
2416
2417                         list_del(pos);
2418                         dev_kfree_skb_any(entry->skb);
2419                         kfree(entry);
2420                 }
2421         }
2422 }
2423 EXPORT_SYMBOL(ocelot_deinit);
2424
2425 MODULE_LICENSE("Dual MIT/GPL");