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