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