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/dsa/ocelot.h>
   8 #include <linux/if_bridge.h>
   9 #include <soc/mscc/ocelot_vcap.h>
  10 #include "ocelot.h"
  11 #include "ocelot_vcap.h"
  12
  13 #define TABLE_UPDATE_SLEEP_US 10
  14 #define TABLE_UPDATE_TIMEOUT_US 100000
  15
  16 struct ocelot_mact_entry {
  17         u8 mac[ETH_ALEN];
  18         u16 vid;
  19         enum macaccess_entry_type type;
  20 };
  21
  22 static inline u32 ocelot_mact_read_macaccess(struct ocelot *ocelot)
  23 {
  24         return ocelot_read(ocelot, ANA_TABLES_MACACCESS);
  25 }
  26
  27 static inline int ocelot_mact_wait_for_completion(struct ocelot *ocelot)
  28 {
  29         u32 val;
  30
  31         return readx_poll_timeout(ocelot_mact_read_macaccess,
  32                 ocelot, val,
  33                 (val & ANA_TABLES_MACACCESS_MAC_TABLE_CMD_M) ==
  34                 MACACCESS_CMD_IDLE,
  35                 TABLE_UPDATE_SLEEP_US, TABLE_UPDATE_TIMEOUT_US);
  36 }
  37
  38 static void ocelot_mact_select(struct ocelot *ocelot,
  39                                const unsigned char mac[ETH_ALEN],
  40                                unsigned int vid)
  41 {
  42         u32 macl = 0, mach = 0;
  43
  44         /* Set the MAC address to handle and the vlan associated in a format
  45          * understood by the hardware.
  46          */
  47         mach |= vid    << 16;
  48         mach |= mac[0] << 8;
  49         mach |= mac[1] << 0;
  50         macl |= mac[2] << 24;
  51         macl |= mac[3] << 16;
  52         macl |= mac[4] << 8;
  53         macl |= mac[5] << 0;
  54
  55         ocelot_write(ocelot, macl, ANA_TABLES_MACLDATA);
  56         ocelot_write(ocelot, mach, ANA_TABLES_MACHDATA);
  57
  58 }
  59
  60 int ocelot_mact_learn(struct ocelot *ocelot, int port,
  61                       const unsigned char mac[ETH_ALEN],
  62                       unsigned int vid, enum macaccess_entry_type type)
  63 {
  64         u32 cmd = ANA_TABLES_MACACCESS_VALID |
  65                 ANA_TABLES_MACACCESS_DEST_IDX(port) |
  66                 ANA_TABLES_MACACCESS_ENTRYTYPE(type) |
  67                 ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_LEARN);
  68         unsigned int mc_ports;
  69
  70         /* Set MAC_CPU_COPY if the CPU port is used by a multicast entry */
  71         if (type == ENTRYTYPE_MACv4)
  72                 mc_ports = (mac[1] << 8) | mac[2];
  73         else if (type == ENTRYTYPE_MACv6)
  74                 mc_ports = (mac[0] << 8) | mac[1];
  75         else
  76                 mc_ports = 0;
  77
  78         if (mc_ports & BIT(ocelot->num_phys_ports))
  79                 cmd |= ANA_TABLES_MACACCESS_MAC_CPU_COPY;
  80
  81         ocelot_mact_select(ocelot, mac, vid);
  82
  83         /* Issue a write command */
  84         ocelot_write(ocelot, cmd, ANA_TABLES_MACACCESS);
  85
  86         return ocelot_mact_wait_for_completion(ocelot);
  87 }
  88 EXPORT_SYMBOL(ocelot_mact_learn);
  89
  90 int ocelot_mact_forget(struct ocelot *ocelot,
  91                        const unsigned char mac[ETH_ALEN], unsigned int vid)
  92 {
  93         ocelot_mact_select(ocelot, mac, vid);
  94
  95         /* Issue a forget command */
  96         ocelot_write(ocelot,
  97                      ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_FORGET),
  98                      ANA_TABLES_MACACCESS);
  99
 100         return ocelot_mact_wait_for_completion(ocelot);
 101 }
 102 EXPORT_SYMBOL(ocelot_mact_forget);
 103
 104 static void ocelot_mact_init(struct ocelot *ocelot)
 105 {
 106         /* Configure the learning mode entries attributes:
 107          * - Do not copy the frame to the CPU extraction queues.
 108          * - Use the vlan and mac_cpoy for dmac lookup.
 109          */
 110         ocelot_rmw(ocelot, 0,
 111                    ANA_AGENCTRL_LEARN_CPU_COPY | ANA_AGENCTRL_IGNORE_DMAC_FLAGS
 112                    | ANA_AGENCTRL_LEARN_FWD_KILL
 113                    | ANA_AGENCTRL_LEARN_IGNORE_VLAN,
 114                    ANA_AGENCTRL);
 115
 116         /* Clear the MAC table */
 117         ocelot_write(ocelot, MACACCESS_CMD_INIT, ANA_TABLES_MACACCESS);
 118 }
 119
 120 static void ocelot_vcap_enable(struct ocelot *ocelot, int port)
 121 {
 122         ocelot_write_gix(ocelot, ANA_PORT_VCAP_S2_CFG_S2_ENA |
 123                          ANA_PORT_VCAP_S2_CFG_S2_IP6_CFG(0xa),
 124                          ANA_PORT_VCAP_S2_CFG, port);
 125
 126         ocelot_write_gix(ocelot, ANA_PORT_VCAP_CFG_S1_ENA,
 127                          ANA_PORT_VCAP_CFG, port);
 128
 129         ocelot_rmw_gix(ocelot, REW_PORT_CFG_ES0_EN,
 130                        REW_PORT_CFG_ES0_EN,
 131                        REW_PORT_CFG, port);
 132 }
 133
 134 static inline u32 ocelot_vlant_read_vlanaccess(struct ocelot *ocelot)
 135 {
 136         return ocelot_read(ocelot, ANA_TABLES_VLANACCESS);
 137 }
 138
 139 static inline int ocelot_vlant_wait_for_completion(struct ocelot *ocelot)
 140 {
 141         u32 val;
 142
 143         return readx_poll_timeout(ocelot_vlant_read_vlanaccess,
 144                 ocelot,
 145                 val,
 146                 (val & ANA_TABLES_VLANACCESS_VLAN_TBL_CMD_M) ==
 147                 ANA_TABLES_VLANACCESS_CMD_IDLE,
 148                 TABLE_UPDATE_SLEEP_US, TABLE_UPDATE_TIMEOUT_US);
 149 }
 150
 151 static int ocelot_vlant_set_mask(struct ocelot *ocelot, u16 vid, u32 mask)
 152 {
 153         /* Select the VID to configure */
 154         ocelot_write(ocelot, ANA_TABLES_VLANTIDX_V_INDEX(vid),
 155                      ANA_TABLES_VLANTIDX);
 156         /* Set the vlan port members mask and issue a write command */
 157         ocelot_write(ocelot, ANA_TABLES_VLANACCESS_VLAN_PORT_MASK(mask) |
 158                              ANA_TABLES_VLANACCESS_CMD_WRITE,
 159                      ANA_TABLES_VLANACCESS);
 160
 161         return ocelot_vlant_wait_for_completion(ocelot);
 162 }
 163
 164 static void ocelot_port_set_native_vlan(struct ocelot *ocelot, int port,
 165                                         struct ocelot_vlan native_vlan)
 166 {
 167         struct ocelot_port *ocelot_port = ocelot->ports[port];
 168         u32 val = 0;
 169
 170         ocelot_port->native_vlan = native_vlan;
 171
 172         ocelot_rmw_gix(ocelot, REW_PORT_VLAN_CFG_PORT_VID(native_vlan.vid),
 173                        REW_PORT_VLAN_CFG_PORT_VID_M,
 174                        REW_PORT_VLAN_CFG, port);
 175
 176         if (ocelot_port->vlan_aware) {
 177                 if (native_vlan.valid)
 178                         /* Tag all frames except when VID == DEFAULT_VLAN */
 179                         val = REW_TAG_CFG_TAG_CFG(1);
 180                 else
 181                         /* Tag all frames */
 182                         val = REW_TAG_CFG_TAG_CFG(3);
 183         } else {
 184                 /* Port tagging disabled. */
 185                 val = REW_TAG_CFG_TAG_CFG(0);
 186         }
 187         ocelot_rmw_gix(ocelot, val,
 188                        REW_TAG_CFG_TAG_CFG_M,
 189                        REW_TAG_CFG, port);
 190 }
 191
 192 /* Default vlan to clasify for untagged frames (may be zero) */
 193 static void ocelot_port_set_pvid(struct ocelot *ocelot, int port,
 194                                  struct ocelot_vlan pvid_vlan)
 195 {
 196         struct ocelot_port *ocelot_port = ocelot->ports[port];
 197         u32 val = 0;
 198
 199         ocelot_port->pvid_vlan = pvid_vlan;
 200
 201         if (!ocelot_port->vlan_aware)
 202                 pvid_vlan.vid = 0;
 203
 204         ocelot_rmw_gix(ocelot,
 205                        ANA_PORT_VLAN_CFG_VLAN_VID(pvid_vlan.vid),
 206                        ANA_PORT_VLAN_CFG_VLAN_VID_M,
 207                        ANA_PORT_VLAN_CFG, port);
 208
 209         /* If there's no pvid, we should drop not only untagged traffic (which
 210          * happens automatically), but also 802.1p traffic which gets
 211          * classified to VLAN 0, but that is always in our RX filter, so it
 212          * would get accepted were it not for this setting.
 213          */
 214         if (!pvid_vlan.valid && ocelot_port->vlan_aware)
 215                 val = ANA_PORT_DROP_CFG_DROP_PRIO_S_TAGGED_ENA |
 216                       ANA_PORT_DROP_CFG_DROP_PRIO_C_TAGGED_ENA;
 217
 218         ocelot_rmw_gix(ocelot, val,
 219                        ANA_PORT_DROP_CFG_DROP_PRIO_S_TAGGED_ENA |
 220                        ANA_PORT_DROP_CFG_DROP_PRIO_C_TAGGED_ENA,
 221                        ANA_PORT_DROP_CFG, port);
 222 }
 223
 224 int ocelot_port_vlan_filtering(struct ocelot *ocelot, int port,
 225                                bool vlan_aware)
 226 {
 227         struct ocelot_vcap_block *block = &ocelot->block[VCAP_IS1];
 228         struct ocelot_port *ocelot_port = ocelot->ports[port];
 229         struct ocelot_vcap_filter *filter;
 230         u32 val;
 231
 232         list_for_each_entry(filter, &block->rules, list) {
 233                 if (filter->ingress_port_mask & BIT(port) &&
 234                     filter->action.vid_replace_ena) {
 235                         dev_err(ocelot->dev,
 236                                 "Cannot change VLAN state with vlan modify rules active\n");
 237                         return -EBUSY;
 238                 }
 239         }
 240
 241         ocelot_port->vlan_aware = vlan_aware;
 242
 243         if (vlan_aware)
 244                 val = ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
 245                       ANA_PORT_VLAN_CFG_VLAN_POP_CNT(1);
 246         else
 247                 val = 0;
 248         ocelot_rmw_gix(ocelot, val,
 249                        ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
 250                        ANA_PORT_VLAN_CFG_VLAN_POP_CNT_M,
 251                        ANA_PORT_VLAN_CFG, port);
 252
 253         ocelot_port_set_pvid(ocelot, port, ocelot_port->pvid_vlan);
 254         ocelot_port_set_native_vlan(ocelot, port, ocelot_port->native_vlan);
 255
 256         return 0;
 257 }
 258 EXPORT_SYMBOL(ocelot_port_vlan_filtering);
 259
 260 int ocelot_vlan_prepare(struct ocelot *ocelot, int port, u16 vid, bool pvid,
 261                         bool untagged)
 262 {
 263         struct ocelot_port *ocelot_port = ocelot->ports[port];
 264
 265         /* Deny changing the native VLAN, but always permit deleting it */
 266         if (untagged && ocelot_port->native_vlan.vid != vid &&
 267             ocelot_port->native_vlan.valid) {
 268                 dev_err(ocelot->dev,
 269                         "Port already has a native VLAN: %d\n",
 270                         ocelot_port->native_vlan.vid);
 271                 return -EBUSY;
 272         }
 273
 274         return 0;
 275 }
 276 EXPORT_SYMBOL(ocelot_vlan_prepare);
 277
 278 int ocelot_vlan_add(struct ocelot *ocelot, int port, u16 vid, bool pvid,
 279                     bool untagged)
 280 {
 281         int ret;
 282
 283         /* Make the port a member of the VLAN */
 284         ocelot->vlan_mask[vid] |= BIT(port);
 285         ret = ocelot_vlant_set_mask(ocelot, vid, ocelot->vlan_mask[vid]);
 286         if (ret)
 287                 return ret;
 288
 289         /* Default ingress vlan classification */
 290         if (pvid) {
 291                 struct ocelot_vlan pvid_vlan;
 292
 293                 pvid_vlan.vid = vid;
 294                 pvid_vlan.valid = true;
 295                 ocelot_port_set_pvid(ocelot, port, pvid_vlan);
 296         }
 297
 298         /* Untagged egress vlan clasification */
 299         if (untagged) {
 300                 struct ocelot_vlan native_vlan;
 301
 302                 native_vlan.vid = vid;
 303                 native_vlan.valid = true;
 304                 ocelot_port_set_native_vlan(ocelot, port, native_vlan);
 305         }
 306
 307         return 0;
 308 }
 309 EXPORT_SYMBOL(ocelot_vlan_add);
 310
 311 int ocelot_vlan_del(struct ocelot *ocelot, int port, u16 vid)
 312 {
 313         struct ocelot_port *ocelot_port = ocelot->ports[port];
 314         int ret;
 315
 316         /* Stop the port from being a member of the vlan */
 317         ocelot->vlan_mask[vid] &= ~BIT(port);
 318         ret = ocelot_vlant_set_mask(ocelot, vid, ocelot->vlan_mask[vid]);
 319         if (ret)
 320                 return ret;
 321
 322         /* Ingress */
 323         if (ocelot_port->pvid_vlan.vid == vid) {
 324                 struct ocelot_vlan pvid_vlan = {0};
 325
 326                 ocelot_port_set_pvid(ocelot, port, pvid_vlan);
 327         }
 328
 329         /* Egress */
 330         if (ocelot_port->native_vlan.vid == vid) {
 331                 struct ocelot_vlan native_vlan = {0};
 332
 333                 ocelot_port_set_native_vlan(ocelot, port, native_vlan);
 334         }
 335
 336         return 0;
 337 }
 338 EXPORT_SYMBOL(ocelot_vlan_del);
 339
 340 static void ocelot_vlan_init(struct ocelot *ocelot)
 341 {
 342         u16 port, vid;
 343
 344         /* Clear VLAN table, by default all ports are members of all VLANs */
 345         ocelot_write(ocelot, ANA_TABLES_VLANACCESS_CMD_INIT,
 346                      ANA_TABLES_VLANACCESS);
 347         ocelot_vlant_wait_for_completion(ocelot);
 348
 349         /* Configure the port VLAN memberships */
 350         for (vid = 1; vid < VLAN_N_VID; vid++) {
 351                 ocelot->vlan_mask[vid] = 0;
 352                 ocelot_vlant_set_mask(ocelot, vid, ocelot->vlan_mask[vid]);
 353         }
 354
 355         /* Because VLAN filtering is enabled, we need VID 0 to get untagged
 356          * traffic.  It is added automatically if 8021q module is loaded, but
 357          * we can't rely on it since module may be not loaded.
 358          */
 359         ocelot->vlan_mask[0] = GENMASK(ocelot->num_phys_ports - 1, 0);
 360         ocelot_vlant_set_mask(ocelot, 0, ocelot->vlan_mask[0]);
 361
 362         /* Set vlan ingress filter mask to all ports but the CPU port by
 363          * default.
 364          */
 365         ocelot_write(ocelot, GENMASK(ocelot->num_phys_ports - 1, 0),
 366                      ANA_VLANMASK);
 367
 368         for (port = 0; port < ocelot->num_phys_ports; port++) {
 369                 ocelot_write_gix(ocelot, 0, REW_PORT_VLAN_CFG, port);
 370                 ocelot_write_gix(ocelot, 0, REW_TAG_CFG, port);
 371         }
 372 }
 373
 374 static u32 ocelot_read_eq_avail(struct ocelot *ocelot, int port)
 375 {
 376         return ocelot_read_rix(ocelot, QSYS_SW_STATUS, port);
 377 }
 378
 379 int ocelot_port_flush(struct ocelot *ocelot, int port)
 380 {
 381         int err, val;
 382
 383         /* Disable dequeuing from the egress queues */
 384         ocelot_rmw_rix(ocelot, QSYS_PORT_MODE_DEQUEUE_DIS,
 385                        QSYS_PORT_MODE_DEQUEUE_DIS,
 386                        QSYS_PORT_MODE, port);
 387
 388         /* Disable flow control */
 389         ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_ENA, 0);
 390
 391         /* Disable priority flow control */
 392         ocelot_fields_write(ocelot, port,
 393                             QSYS_SWITCH_PORT_MODE_TX_PFC_ENA, 0);
 394
 395         /* Wait at least the time it takes to receive a frame of maximum length
 396          * at the port.
 397          * Worst-case delays for 10 kilobyte jumbo frames are:
 398          * 8 ms on a 10M port
 399          * 800 μs on a 100M port
 400          * 80 μs on a 1G port
 401          * 32 μs on a 2.5G port
 402          */
 403         usleep_range(8000, 10000);
 404
 405         /* Disable half duplex backpressure. */
 406         ocelot_rmw_rix(ocelot, 0, SYS_FRONT_PORT_MODE_HDX_MODE,
 407                        SYS_FRONT_PORT_MODE, port);
 408
 409         /* Flush the queues associated with the port. */
 410         ocelot_rmw_gix(ocelot, REW_PORT_CFG_FLUSH_ENA, REW_PORT_CFG_FLUSH_ENA,
 411                        REW_PORT_CFG, port);
 412
 413         /* Enable dequeuing from the egress queues. */
 414         ocelot_rmw_rix(ocelot, 0, QSYS_PORT_MODE_DEQUEUE_DIS, QSYS_PORT_MODE,
 415                        port);
 416
 417         /* Wait until flushing is complete. */
 418         err = read_poll_timeout(ocelot_read_eq_avail, val, !val,
 419                                 100, 2000000, false, ocelot, port);
 420
 421         /* Clear flushing again. */
 422         ocelot_rmw_gix(ocelot, 0, REW_PORT_CFG_FLUSH_ENA, REW_PORT_CFG, port);
 423
 424         return err;
 425 }
 426 EXPORT_SYMBOL(ocelot_port_flush);
 427
 428 void ocelot_adjust_link(struct ocelot *ocelot, int port,
 429                         struct phy_device *phydev)
 430 {
 431         struct ocelot_port *ocelot_port = ocelot->ports[port];
 432         int speed, mode = 0;
 433
 434         switch (phydev->speed) {
 435         case SPEED_10:
 436                 speed = OCELOT_SPEED_10;
 437                 break;
 438         case SPEED_100:
 439                 speed = OCELOT_SPEED_100;
 440                 break;
 441         case SPEED_1000:
 442                 speed = OCELOT_SPEED_1000;
 443                 mode = DEV_MAC_MODE_CFG_GIGA_MODE_ENA;
 444                 break;
 445         case SPEED_2500:
 446                 speed = OCELOT_SPEED_2500;
 447                 mode = DEV_MAC_MODE_CFG_GIGA_MODE_ENA;
 448                 break;
 449         default:
 450                 dev_err(ocelot->dev, "Unsupported PHY speed on port %d: %d\n",
 451                         port, phydev->speed);
 452                 return;
 453         }
 454
 455         phy_print_status(phydev);
 456
 457         if (!phydev->link)
 458                 return;
 459
 460         /* Only full duplex supported for now */
 461         ocelot_port_writel(ocelot_port, DEV_MAC_MODE_CFG_FDX_ENA |
 462                            mode, DEV_MAC_MODE_CFG);
 463
 464         /* Disable HDX fast control */
 465         ocelot_port_writel(ocelot_port, DEV_PORT_MISC_HDX_FAST_DIS,
 466                            DEV_PORT_MISC);
 467
 468         /* SGMII only for now */
 469         ocelot_port_writel(ocelot_port, PCS1G_MODE_CFG_SGMII_MODE_ENA,
 470                            PCS1G_MODE_CFG);
 471         ocelot_port_writel(ocelot_port, PCS1G_SD_CFG_SD_SEL, PCS1G_SD_CFG);
 472
 473         /* Enable PCS */
 474         ocelot_port_writel(ocelot_port, PCS1G_CFG_PCS_ENA, PCS1G_CFG);
 475
 476         /* No aneg on SGMII */
 477         ocelot_port_writel(ocelot_port, 0, PCS1G_ANEG_CFG);
 478
 479         /* No loopback */
 480         ocelot_port_writel(ocelot_port, 0, PCS1G_LB_CFG);
 481
 482         /* Enable MAC module */
 483         ocelot_port_writel(ocelot_port, DEV_MAC_ENA_CFG_RX_ENA |
 484                            DEV_MAC_ENA_CFG_TX_ENA, DEV_MAC_ENA_CFG);
 485
 486         /* Take MAC, Port, Phy (intern) and PCS (SGMII/Serdes) clock out of
 487          * reset */
 488         ocelot_port_writel(ocelot_port, DEV_CLOCK_CFG_LINK_SPEED(speed),
 489                            DEV_CLOCK_CFG);
 490
 491         /* No PFC */
 492         ocelot_write_gix(ocelot, ANA_PFC_PFC_CFG_FC_LINK_SPEED(speed),
 493                          ANA_PFC_PFC_CFG, port);
 494
 495         /* Core: Enable port for frame transfer */
 496         ocelot_fields_write(ocelot, port,
 497                             QSYS_SWITCH_PORT_MODE_PORT_ENA, 1);
 498
 499         /* Flow control */
 500         ocelot_write_rix(ocelot, SYS_MAC_FC_CFG_PAUSE_VAL_CFG(0xffff) |
 501                          SYS_MAC_FC_CFG_RX_FC_ENA | SYS_MAC_FC_CFG_TX_FC_ENA |
 502                          SYS_MAC_FC_CFG_ZERO_PAUSE_ENA |
 503                          SYS_MAC_FC_CFG_FC_LATENCY_CFG(0x7) |
 504                          SYS_MAC_FC_CFG_FC_LINK_SPEED(speed),
 505                          SYS_MAC_FC_CFG, port);
 506         ocelot_write_rix(ocelot, 0, ANA_POL_FLOWC, port);
 507 }
 508 EXPORT_SYMBOL(ocelot_adjust_link);
 509
 510 void ocelot_port_enable(struct ocelot *ocelot, int port,
 511                         struct phy_device *phy)
 512 {
 513         /* Enable receiving frames on the port, and activate auto-learning of
 514          * MAC addresses.
 515          */
 516         ocelot_write_gix(ocelot, ANA_PORT_PORT_CFG_LEARNAUTO |
 517                          ANA_PORT_PORT_CFG_RECV_ENA |
 518                          ANA_PORT_PORT_CFG_PORTID_VAL(port),
 519                          ANA_PORT_PORT_CFG, port);
 520 }
 521 EXPORT_SYMBOL(ocelot_port_enable);
 522
 523 void ocelot_port_disable(struct ocelot *ocelot, int port)
 524 {
 525         struct ocelot_port *ocelot_port = ocelot->ports[port];
 526
 527         ocelot_port_writel(ocelot_port, 0, DEV_MAC_ENA_CFG);
 528         ocelot_fields_write(ocelot, port, QSYS_SWITCH_PORT_MODE_PORT_ENA, 0);
 529 }
 530 EXPORT_SYMBOL(ocelot_port_disable);
 531
 532 void ocelot_port_add_txtstamp_skb(struct ocelot *ocelot, int port,
 533                                   struct sk_buff *clone)
 534 {
 535         struct ocelot_port *ocelot_port = ocelot->ports[port];
 536
 537         spin_lock(&ocelot_port->ts_id_lock);
 538
 539         skb_shinfo(clone)->tx_flags |= SKBTX_IN_PROGRESS;
 540         /* Store timestamp ID in cb[0] of sk_buff */
 541         clone->cb[0] = ocelot_port->ts_id;
 542         ocelot_port->ts_id = (ocelot_port->ts_id + 1) % 4;
 543         skb_queue_tail(&ocelot_port->tx_skbs, clone);
 544
 545         spin_unlock(&ocelot_port->ts_id_lock);
 546 }
 547 EXPORT_SYMBOL(ocelot_port_add_txtstamp_skb);
 548
 549 static void ocelot_get_hwtimestamp(struct ocelot *ocelot,
 550                                    struct timespec64 *ts)
 551 {
 552         unsigned long flags;
 553         u32 val;
 554
 555         spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
 556
 557         /* Read current PTP time to get seconds */
 558         val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
 559
 560         val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
 561         val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE);
 562         ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
 563         ts->tv_sec = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
 564
 565         /* Read packet HW timestamp from FIFO */
 566         val = ocelot_read(ocelot, SYS_PTP_TXSTAMP);
 567         ts->tv_nsec = SYS_PTP_TXSTAMP_PTP_TXSTAMP(val);
 568
 569         /* Sec has incremented since the ts was registered */
 570         if ((ts->tv_sec & 0x1) != !!(val & SYS_PTP_TXSTAMP_PTP_TXSTAMP_SEC))
 571                 ts->tv_sec--;
 572
 573         spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
 574 }
 575
 576 void ocelot_get_txtstamp(struct ocelot *ocelot)
 577 {
 578         int budget = OCELOT_PTP_QUEUE_SZ;
 579
 580         while (budget--) {
 581                 struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
 582                 struct skb_shared_hwtstamps shhwtstamps;
 583                 struct ocelot_port *port;
 584                 struct timespec64 ts;
 585                 unsigned long flags;
 586                 u32 val, id, txport;
 587
 588                 val = ocelot_read(ocelot, SYS_PTP_STATUS);
 589
 590                 /* Check if a timestamp can be retrieved */
 591                 if (!(val & SYS_PTP_STATUS_PTP_MESS_VLD))
 592                         break;
 593
 594                 WARN_ON(val & SYS_PTP_STATUS_PTP_OVFL);
 595
 596                 /* Retrieve the ts ID and Tx port */
 597                 id = SYS_PTP_STATUS_PTP_MESS_ID_X(val);
 598                 txport = SYS_PTP_STATUS_PTP_MESS_TXPORT_X(val);
 599
 600                 /* Retrieve its associated skb */
 601                 port = ocelot->ports[txport];
 602
 603                 spin_lock_irqsave(&port->tx_skbs.lock, flags);
 604
 605                 skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
 606                         if (skb->cb[0] != id)
 607                                 continue;
 608                         __skb_unlink(skb, &port->tx_skbs);
 609                         skb_match = skb;
 610                         break;
 611                 }
 612
 613                 spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
 614
 615                 /* Get the h/w timestamp */
 616                 ocelot_get_hwtimestamp(ocelot, &ts);
 617
 618                 if (unlikely(!skb_match))
 619                         continue;
 620
 621                 /* Set the timestamp into the skb */
 622                 memset(&shhwtstamps, 0, sizeof(shhwtstamps));
 623                 shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
 624                 skb_complete_tx_timestamp(skb_match, &shhwtstamps);
 625
 626                 /* Next ts */
 627                 ocelot_write(ocelot, SYS_PTP_NXT_PTP_NXT, SYS_PTP_NXT);
 628         }
 629 }
 630 EXPORT_SYMBOL(ocelot_get_txtstamp);
 631
 632 static int ocelot_rx_frame_word(struct ocelot *ocelot, u8 grp, bool ifh,
 633                                 u32 *rval)
 634 {
 635         u32 bytes_valid, val;
 636
 637         val = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
 638         if (val == XTR_NOT_READY) {
 639                 if (ifh)
 640                         return -EIO;
 641
 642                 do {
 643                         val = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
 644                 } while (val == XTR_NOT_READY);
 645         }
 646
 647         switch (val) {
 648         case XTR_ABORT:
 649                 return -EIO;
 650         case XTR_EOF_0:
 651         case XTR_EOF_1:
 652         case XTR_EOF_2:
 653         case XTR_EOF_3:
 654         case XTR_PRUNED:
 655                 bytes_valid = XTR_VALID_BYTES(val);
 656                 val = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
 657                 if (val == XTR_ESCAPE)
 658                         *rval = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
 659                 else
 660                         *rval = val;
 661
 662                 return bytes_valid;
 663         case XTR_ESCAPE:
 664                 *rval = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
 665
 666                 return 4;
 667         default:
 668                 *rval = val;
 669
 670                 return 4;
 671         }
 672 }
 673
 674 static int ocelot_xtr_poll_xfh(struct ocelot *ocelot, int grp, u32 *xfh)
 675 {
 676         int i, err = 0;
 677
 678         for (i = 0; i < OCELOT_TAG_LEN / 4; i++) {
 679                 err = ocelot_rx_frame_word(ocelot, grp, true, &xfh[i]);
 680                 if (err != 4)
 681                         return (err < 0) ? err : -EIO;
 682         }
 683
 684         return 0;
 685 }
 686
 687 int ocelot_xtr_poll_frame(struct ocelot *ocelot, int grp, struct sk_buff **nskb)
 688 {
 689         struct skb_shared_hwtstamps *shhwtstamps;
 690         u64 tod_in_ns, full_ts_in_ns, cpuq;
 691         u64 timestamp, src_port, len;
 692         u32 xfh[OCELOT_TAG_LEN / 4];
 693         struct net_device *dev;
 694         struct timespec64 ts;
 695         struct sk_buff *skb;
 696         int sz, buf_len;
 697         u32 val, *buf;
 698         int err;
 699
 700         err = ocelot_xtr_poll_xfh(ocelot, grp, xfh);
 701         if (err)
 702                 return err;
 703
 704         ocelot_xfh_get_src_port(xfh, &src_port);
 705         ocelot_xfh_get_len(xfh, &len);
 706         ocelot_xfh_get_rew_val(xfh, &timestamp);
 707         ocelot_xfh_get_cpuq(xfh, &cpuq);
 708
 709         if (WARN_ON(src_port >= ocelot->num_phys_ports))
 710                 return -EINVAL;
 711
 712         dev = ocelot->ops->port_to_netdev(ocelot, src_port);
 713         if (!dev)
 714                 return -EINVAL;
 715
 716         skb = netdev_alloc_skb(dev, len);
 717         if (unlikely(!skb)) {
 718                 netdev_err(dev, "Unable to allocate sk_buff\n");
 719                 return -ENOMEM;
 720         }
 721
 722         buf_len = len - ETH_FCS_LEN;
 723         buf = (u32 *)skb_put(skb, buf_len);
 724
 725         len = 0;
 726         do {
 727                 sz = ocelot_rx_frame_word(ocelot, grp, false, &val);
 728                 if (sz < 0) {
 729                         err = sz;
 730                         goto out_free_skb;
 731                 }
 732                 *buf++ = val;
 733                 len += sz;
 734         } while (len < buf_len);
 735
 736         /* Read the FCS */
 737         sz = ocelot_rx_frame_word(ocelot, grp, false, &val);
 738         if (sz < 0) {
 739                 err = sz;
 740                 goto out_free_skb;
 741         }
 742
 743         /* Update the statistics if part of the FCS was read before */
 744         len -= ETH_FCS_LEN - sz;
 745
 746         if (unlikely(dev->features & NETIF_F_RXFCS)) {
 747                 buf = (u32 *)skb_put(skb, ETH_FCS_LEN);
 748                 *buf = val;
 749         }
 750
 751         if (ocelot->ptp) {
 752                 ocelot_ptp_gettime64(&ocelot->ptp_info, &ts);
 753
 754                 tod_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
 755                 if ((tod_in_ns & 0xffffffff) < timestamp)
 756                         full_ts_in_ns = (((tod_in_ns >> 32) - 1) << 32) |
 757                                         timestamp;
 758                 else
 759                         full_ts_in_ns = (tod_in_ns & GENMASK_ULL(63, 32)) |
 760                                         timestamp;
 761
 762                 shhwtstamps = skb_hwtstamps(skb);
 763                 memset(shhwtstamps, 0, sizeof(struct skb_shared_hwtstamps));
 764                 shhwtstamps->hwtstamp = full_ts_in_ns;
 765         }
 766
 767         /* Everything we see on an interface that is in the HW bridge
 768          * has already been forwarded.
 769          */
 770         if (ocelot->bridge_mask & BIT(src_port))
 771                 skb->offload_fwd_mark = 1;
 772
 773         skb->protocol = eth_type_trans(skb, dev);
 774
 775 #if IS_ENABLED(CONFIG_BRIDGE_MRP)
 776         if (skb->protocol == cpu_to_be16(ETH_P_MRP) &&
 777             cpuq & BIT(OCELOT_MRP_CPUQ))
 778                 skb->offload_fwd_mark = 0;
 779 #endif
 780
 781         *nskb = skb;
 782
 783         return 0;
 784
 785 out_free_skb:
 786         kfree_skb(skb);
 787         return err;
 788 }
 789 EXPORT_SYMBOL(ocelot_xtr_poll_frame);
 790
 791 bool ocelot_can_inject(struct ocelot *ocelot, int grp)
 792 {
 793         u32 val = ocelot_read(ocelot, QS_INJ_STATUS);
 794
 795         if (!(val & QS_INJ_STATUS_FIFO_RDY(BIT(grp))))
 796                 return false;
 797         if (val & QS_INJ_STATUS_WMARK_REACHED(BIT(grp)))
 798                 return false;
 799
 800         return true;
 801 }
 802 EXPORT_SYMBOL(ocelot_can_inject);
 803
 804 void ocelot_port_inject_frame(struct ocelot *ocelot, int port, int grp,
 805                               u32 rew_op, struct sk_buff *skb)
 806 {
 807         u32 ifh[OCELOT_TAG_LEN / 4] = {0};
 808         unsigned int i, count, last;
 809
 810         ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
 811                          QS_INJ_CTRL_SOF, QS_INJ_CTRL, grp);
 812
 813         ocelot_ifh_set_bypass(ifh, 1);
 814         ocelot_ifh_set_dest(ifh, BIT_ULL(port));
 815         ocelot_ifh_set_tag_type(ifh, IFH_TAG_TYPE_C);
 816         ocelot_ifh_set_vid(ifh, skb_vlan_tag_get(skb));
 817         ocelot_ifh_set_rew_op(ifh, rew_op);
 818
 819         for (i = 0; i < OCELOT_TAG_LEN / 4; i++)
 820                 ocelot_write_rix(ocelot, ifh[i], QS_INJ_WR, grp);
 821
 822         count = DIV_ROUND_UP(skb->len, 4);
 823         last = skb->len % 4;
 824         for (i = 0; i < count; i++)
 825                 ocelot_write_rix(ocelot, ((u32 *)skb->data)[i], QS_INJ_WR, grp);
 826
 827         /* Add padding */
 828         while (i < (OCELOT_BUFFER_CELL_SZ / 4)) {
 829                 ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
 830                 i++;
 831         }
 832
 833         /* Indicate EOF and valid bytes in last word */
 834         ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
 835                          QS_INJ_CTRL_VLD_BYTES(skb->len < OCELOT_BUFFER_CELL_SZ ? 0 : last) |
 836                          QS_INJ_CTRL_EOF,
 837                          QS_INJ_CTRL, grp);
 838
 839         /* Add dummy CRC */
 840         ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
 841         skb_tx_timestamp(skb);
 842
 843         skb->dev->stats.tx_packets++;
 844         skb->dev->stats.tx_bytes += skb->len;
 845 }
 846 EXPORT_SYMBOL(ocelot_port_inject_frame);
 847
 848 void ocelot_drain_cpu_queue(struct ocelot *ocelot, int grp)
 849 {
 850         while (ocelot_read(ocelot, QS_XTR_DATA_PRESENT) & BIT(grp))
 851                 ocelot_read_rix(ocelot, QS_XTR_RD, grp);
 852 }
 853 EXPORT_SYMBOL(ocelot_drain_cpu_queue);
 854
 855 int ocelot_fdb_add(struct ocelot *ocelot, int port,
 856                    const unsigned char *addr, u16 vid)
 857 {
 858         int pgid = port;
 859
 860         if (port == ocelot->npi)
 861                 pgid = PGID_CPU;
 862
 863         return ocelot_mact_learn(ocelot, pgid, addr, vid, ENTRYTYPE_LOCKED);
 864 }
 865 EXPORT_SYMBOL(ocelot_fdb_add);
 866
 867 int ocelot_fdb_del(struct ocelot *ocelot, int port,
 868                    const unsigned char *addr, u16 vid)
 869 {
 870         return ocelot_mact_forget(ocelot, addr, vid);
 871 }
 872 EXPORT_SYMBOL(ocelot_fdb_del);
 873
 874 int ocelot_port_fdb_do_dump(const unsigned char *addr, u16 vid,
 875                             bool is_static, void *data)
 876 {
 877         struct ocelot_dump_ctx *dump = data;
 878         u32 portid = NETLINK_CB(dump->cb->skb).portid;
 879         u32 seq = dump->cb->nlh->nlmsg_seq;
 880         struct nlmsghdr *nlh;
 881         struct ndmsg *ndm;
 882
 883         if (dump->idx < dump->cb->args[2])
 884                 goto skip;
 885
 886         nlh = nlmsg_put(dump->skb, portid, seq, RTM_NEWNEIGH,
 887                         sizeof(*ndm), NLM_F_MULTI);
 888         if (!nlh)
 889                 return -EMSGSIZE;
 890
 891         ndm = nlmsg_data(nlh);
 892         ndm->ndm_family  = AF_BRIDGE;
 893         ndm->ndm_pad1    = 0;
 894         ndm->ndm_pad2    = 0;
 895         ndm->ndm_flags   = NTF_SELF;
 896         ndm->ndm_type    = 0;
 897         ndm->ndm_ifindex = dump->dev->ifindex;
 898         ndm->ndm_state   = is_static ? NUD_NOARP : NUD_REACHABLE;
 899
 900         if (nla_put(dump->skb, NDA_LLADDR, ETH_ALEN, addr))
 901                 goto nla_put_failure;
 902
 903         if (vid && nla_put_u16(dump->skb, NDA_VLAN, vid))
 904                 goto nla_put_failure;
 905
 906         nlmsg_end(dump->skb, nlh);
 907
 908 skip:
 909         dump->idx++;
 910         return 0;
 911
 912 nla_put_failure:
 913         nlmsg_cancel(dump->skb, nlh);
 914         return -EMSGSIZE;
 915 }
 916 EXPORT_SYMBOL(ocelot_port_fdb_do_dump);
 917
 918 static int ocelot_mact_read(struct ocelot *ocelot, int port, int row, int col,
 919                             struct ocelot_mact_entry *entry)
 920 {
 921         u32 val, dst, macl, mach;
 922         char mac[ETH_ALEN];
 923
 924         /* Set row and column to read from */
 925         ocelot_field_write(ocelot, ANA_TABLES_MACTINDX_M_INDEX, row);
 926         ocelot_field_write(ocelot, ANA_TABLES_MACTINDX_BUCKET, col);
 927
 928         /* Issue a read command */
 929         ocelot_write(ocelot,
 930                      ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_READ),
 931                      ANA_TABLES_MACACCESS);
 932
 933         if (ocelot_mact_wait_for_completion(ocelot))
 934                 return -ETIMEDOUT;
 935
 936         /* Read the entry flags */
 937         val = ocelot_read(ocelot, ANA_TABLES_MACACCESS);
 938         if (!(val & ANA_TABLES_MACACCESS_VALID))
 939                 return -EINVAL;
 940
 941         /* If the entry read has another port configured as its destination,
 942          * do not report it.
 943          */
 944         dst = (val & ANA_TABLES_MACACCESS_DEST_IDX_M) >> 3;
 945         if (dst != port)
 946                 return -EINVAL;
 947
 948         /* Get the entry's MAC address and VLAN id */
 949         macl = ocelot_read(ocelot, ANA_TABLES_MACLDATA);
 950         mach = ocelot_read(ocelot, ANA_TABLES_MACHDATA);
 951
 952         mac[0] = (mach >> 8)  & 0xff;
 953         mac[1] = (mach >> 0)  & 0xff;
 954         mac[2] = (macl >> 24) & 0xff;
 955         mac[3] = (macl >> 16) & 0xff;
 956         mac[4] = (macl >> 8)  & 0xff;
 957         mac[5] = (macl >> 0)  & 0xff;
 958
 959         entry->vid = (mach >> 16) & 0xfff;
 960         ether_addr_copy(entry->mac, mac);
 961
 962         return 0;
 963 }
 964
 965 int ocelot_fdb_dump(struct ocelot *ocelot, int port,
 966                     dsa_fdb_dump_cb_t *cb, void *data)
 967 {
 968         int i, j;
 969
 970         /* Loop through all the mac tables entries. */
 971         for (i = 0; i < ocelot->num_mact_rows; i++) {
 972                 for (j = 0; j < 4; j++) {
 973                         struct ocelot_mact_entry entry;
 974                         bool is_static;
 975                         int ret;
 976
 977                         ret = ocelot_mact_read(ocelot, port, i, j, &entry);
 978                         /* If the entry is invalid (wrong port, invalid...),
 979                          * skip it.
 980                          */
 981                         if (ret == -EINVAL)
 982                                 continue;
 983                         else if (ret)
 984                                 return ret;
 985
 986                         is_static = (entry.type == ENTRYTYPE_LOCKED);
 987
 988                         ret = cb(entry.mac, entry.vid, is_static, data);
 989                         if (ret)
 990                                 return ret;
 991                 }
 992         }
 993
 994         return 0;
 995 }
 996 EXPORT_SYMBOL(ocelot_fdb_dump);
 997
 998 int ocelot_hwstamp_get(struct ocelot *ocelot, int port, struct ifreq *ifr)
 999 {
1000         return copy_to_user(ifr->ifr_data, &ocelot->hwtstamp_config,
1001                             sizeof(ocelot->hwtstamp_config)) ? -EFAULT : 0;
1002 }
1003 EXPORT_SYMBOL(ocelot_hwstamp_get);
1004
1005 int ocelot_hwstamp_set(struct ocelot *ocelot, int port, struct ifreq *ifr)
1006 {
1007         struct ocelot_port *ocelot_port = ocelot->ports[port];
1008         struct hwtstamp_config cfg;
1009
1010         if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
1011                 return -EFAULT;
1012
1013         /* reserved for future extensions */
1014         if (cfg.flags)
1015                 return -EINVAL;
1016
1017         /* Tx type sanity check */
1018         switch (cfg.tx_type) {
1019         case HWTSTAMP_TX_ON:
1020                 ocelot_port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
1021                 break;
1022         case HWTSTAMP_TX_ONESTEP_SYNC:
1023                 /* IFH_REW_OP_ONE_STEP_PTP updates the correctional field, we
1024                  * need to update the origin time.
1025                  */
1026                 ocelot_port->ptp_cmd = IFH_REW_OP_ORIGIN_PTP;
1027                 break;
1028         case HWTSTAMP_TX_OFF:
1029                 ocelot_port->ptp_cmd = 0;
1030                 break;
1031         default:
1032                 return -ERANGE;
1033         }
1034
1035         mutex_lock(&ocelot->ptp_lock);
1036
1037         switch (cfg.rx_filter) {
1038         case HWTSTAMP_FILTER_NONE:
1039                 break;
1040         case HWTSTAMP_FILTER_ALL:
1041         case HWTSTAMP_FILTER_SOME:
1042         case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
1043         case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
1044         case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
1045         case HWTSTAMP_FILTER_NTP_ALL:
1046         case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
1047         case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
1048         case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
1049         case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
1050         case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
1051         case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
1052         case HWTSTAMP_FILTER_PTP_V2_EVENT:
1053         case HWTSTAMP_FILTER_PTP_V2_SYNC:
1054         case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
1055                 cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
1056                 break;
1057         default:
1058                 mutex_unlock(&ocelot->ptp_lock);
1059                 return -ERANGE;
1060         }
1061
1062         /* Commit back the result & save it */
1063         memcpy(&ocelot->hwtstamp_config, &cfg, sizeof(cfg));
1064         mutex_unlock(&ocelot->ptp_lock);
1065
1066         return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
1067 }
1068 EXPORT_SYMBOL(ocelot_hwstamp_set);
1069
1070 void ocelot_get_strings(struct ocelot *ocelot, int port, u32 sset, u8 *data)
1071 {
1072         int i;
1073
1074         if (sset != ETH_SS_STATS)
1075                 return;
1076
1077         for (i = 0; i < ocelot->num_stats; i++)
1078                 memcpy(data + i * ETH_GSTRING_LEN, ocelot->stats_layout[i].name,
1079                        ETH_GSTRING_LEN);
1080 }
1081 EXPORT_SYMBOL(ocelot_get_strings);
1082
1083 static void ocelot_update_stats(struct ocelot *ocelot)
1084 {
1085         int i, j;
1086
1087         mutex_lock(&ocelot->stats_lock);
1088
1089         for (i = 0; i < ocelot->num_phys_ports; i++) {
1090                 /* Configure the port to read the stats from */
1091                 ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(i), SYS_STAT_CFG);
1092
1093                 for (j = 0; j < ocelot->num_stats; j++) {
1094                         u32 val;
1095                         unsigned int idx = i * ocelot->num_stats + j;
1096
1097                         val = ocelot_read_rix(ocelot, SYS_COUNT_RX_OCTETS,
1098                                               ocelot->stats_layout[j].offset);
1099
1100                         if (val < (ocelot->stats[idx] & U32_MAX))
1101                                 ocelot->stats[idx] += (u64)1 << 32;
1102
1103                         ocelot->stats[idx] = (ocelot->stats[idx] &
1104                                               ~(u64)U32_MAX) + val;
1105                 }
1106         }
1107
1108         mutex_unlock(&ocelot->stats_lock);
1109 }
1110
1111 static void ocelot_check_stats_work(struct work_struct *work)
1112 {
1113         struct delayed_work *del_work = to_delayed_work(work);
1114         struct ocelot *ocelot = container_of(del_work, struct ocelot,
1115                                              stats_work);
1116
1117         ocelot_update_stats(ocelot);
1118
1119         queue_delayed_work(ocelot->stats_queue, &ocelot->stats_work,
1120                            OCELOT_STATS_CHECK_DELAY);
1121 }
1122
1123 void ocelot_get_ethtool_stats(struct ocelot *ocelot, int port, u64 *data)
1124 {
1125         int i;
1126
1127         /* check and update now */
1128         ocelot_update_stats(ocelot);
1129
1130         /* Copy all counters */
1131         for (i = 0; i < ocelot->num_stats; i++)
1132                 *data++ = ocelot->stats[port * ocelot->num_stats + i];
1133 }
1134 EXPORT_SYMBOL(ocelot_get_ethtool_stats);
1135
1136 int ocelot_get_sset_count(struct ocelot *ocelot, int port, int sset)
1137 {
1138         if (sset != ETH_SS_STATS)
1139                 return -EOPNOTSUPP;
1140
1141         return ocelot->num_stats;
1142 }
1143 EXPORT_SYMBOL(ocelot_get_sset_count);
1144
1145 int ocelot_get_ts_info(struct ocelot *ocelot, int port,
1146                        struct ethtool_ts_info *info)
1147 {
1148         info->phc_index = ocelot->ptp_clock ?
1149                           ptp_clock_index(ocelot->ptp_clock) : -1;
1150         if (info->phc_index == -1) {
1151                 info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE |
1152                                          SOF_TIMESTAMPING_RX_SOFTWARE |
1153                                          SOF_TIMESTAMPING_SOFTWARE;
1154                 return 0;
1155         }
1156         info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE |
1157                                  SOF_TIMESTAMPING_RX_SOFTWARE |
1158                                  SOF_TIMESTAMPING_SOFTWARE |
1159                                  SOF_TIMESTAMPING_TX_HARDWARE |
1160                                  SOF_TIMESTAMPING_RX_HARDWARE |
1161                                  SOF_TIMESTAMPING_RAW_HARDWARE;
1162         info->tx_types = BIT(HWTSTAMP_TX_OFF) | BIT(HWTSTAMP_TX_ON) |
1163                          BIT(HWTSTAMP_TX_ONESTEP_SYNC);
1164         info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) | BIT(HWTSTAMP_FILTER_ALL);
1165
1166         return 0;
1167 }
1168 EXPORT_SYMBOL(ocelot_get_ts_info);
1169
1170 static u32 ocelot_get_bond_mask(struct ocelot *ocelot, struct net_device *bond,
1171                                 bool only_active_ports)
1172 {
1173         u32 mask = 0;
1174         int port;
1175
1176         for (port = 0; port < ocelot->num_phys_ports; port++) {
1177                 struct ocelot_port *ocelot_port = ocelot->ports[port];
1178
1179                 if (!ocelot_port)
1180                         continue;
1181
1182                 if (ocelot_port->bond == bond) {
1183                         if (only_active_ports && !ocelot_port->lag_tx_active)
1184                                 continue;
1185
1186                         mask |= BIT(port);
1187                 }
1188         }
1189
1190         return mask;
1191 }
1192
1193 static u32 ocelot_get_dsa_8021q_cpu_mask(struct ocelot *ocelot)
1194 {
1195         u32 mask = 0;
1196         int port;
1197
1198         for (port = 0; port < ocelot->num_phys_ports; port++) {
1199                 struct ocelot_port *ocelot_port = ocelot->ports[port];
1200
1201                 if (!ocelot_port)
1202                         continue;
1203
1204                 if (ocelot_port->is_dsa_8021q_cpu)
1205                         mask |= BIT(port);
1206         }
1207
1208         return mask;
1209 }
1210
1211 void ocelot_apply_bridge_fwd_mask(struct ocelot *ocelot)
1212 {
1213         unsigned long cpu_fwd_mask;
1214         int port;
1215
1216         /* If a DSA tag_8021q CPU exists, it needs to be included in the
1217          * regular forwarding path of the front ports regardless of whether
1218          * those are bridged or standalone.
1219          * If DSA tag_8021q is not used, this returns 0, which is fine because
1220          * the hardware-based CPU port module can be a destination for packets
1221          * even if it isn't part of PGID_SRC.
1222          */
1223         cpu_fwd_mask = ocelot_get_dsa_8021q_cpu_mask(ocelot);
1224
1225         /* Apply FWD mask. The loop is needed to add/remove the current port as
1226          * a source for the other ports.
1227          */
1228         for (port = 0; port < ocelot->num_phys_ports; port++) {
1229                 struct ocelot_port *ocelot_port = ocelot->ports[port];
1230                 unsigned long mask;
1231
1232                 if (!ocelot_port) {
1233                         /* Unused ports can't send anywhere */
1234                         mask = 0;
1235                 } else if (ocelot_port->is_dsa_8021q_cpu) {
1236                         /* The DSA tag_8021q CPU ports need to be able to
1237                          * forward packets to all other ports except for
1238                          * themselves
1239                          */
1240                         mask = GENMASK(ocelot->num_phys_ports - 1, 0);
1241                         mask &= ~cpu_fwd_mask;
1242                 } else if (ocelot->bridge_fwd_mask & BIT(port)) {
1243                         struct net_device *bond = ocelot_port->bond;
1244
1245                         mask = ocelot->bridge_fwd_mask & ~BIT(port);
1246                         if (bond) {
1247                                 mask &= ~ocelot_get_bond_mask(ocelot, bond,
1248                                                               false);
1249                         }
1250                 } else {
1251                         /* Standalone ports forward only to DSA tag_8021q CPU
1252                          * ports (if those exist), or to the hardware CPU port
1253                          * module otherwise.
1254                          */
1255                         mask = cpu_fwd_mask;
1256                 }
1257
1258                 ocelot_write_rix(ocelot, mask, ANA_PGID_PGID, PGID_SRC + port);
1259         }
1260 }
1261 EXPORT_SYMBOL(ocelot_apply_bridge_fwd_mask);
1262
1263 void ocelot_bridge_stp_state_set(struct ocelot *ocelot, int port, u8 state)
1264 {
1265         struct ocelot_port *ocelot_port = ocelot->ports[port];
1266         u32 port_cfg;
1267
1268         if (!(BIT(port) & ocelot->bridge_mask))
1269                 return;
1270
1271         port_cfg = ocelot_read_gix(ocelot, ANA_PORT_PORT_CFG, port);
1272
1273         switch (state) {
1274         case BR_STATE_FORWARDING:
1275                 ocelot->bridge_fwd_mask |= BIT(port);
1276                 fallthrough;
1277         case BR_STATE_LEARNING:
1278                 if (ocelot_port->learn_ena)
1279                         port_cfg |= ANA_PORT_PORT_CFG_LEARN_ENA;
1280                 break;
1281
1282         default:
1283                 port_cfg &= ~ANA_PORT_PORT_CFG_LEARN_ENA;
1284                 ocelot->bridge_fwd_mask &= ~BIT(port);
1285                 break;
1286         }
1287
1288         ocelot_write_gix(ocelot, port_cfg, ANA_PORT_PORT_CFG, port);
1289
1290         ocelot_apply_bridge_fwd_mask(ocelot);
1291 }
1292 EXPORT_SYMBOL(ocelot_bridge_stp_state_set);
1293
1294 void ocelot_set_ageing_time(struct ocelot *ocelot, unsigned int msecs)
1295 {
1296         unsigned int age_period = ANA_AUTOAGE_AGE_PERIOD(msecs / 2000);
1297
1298         /* Setting AGE_PERIOD to zero effectively disables automatic aging,
1299          * which is clearly not what our intention is. So avoid that.
1300          */
1301         if (!age_period)
1302                 age_period = 1;
1303
1304         ocelot_rmw(ocelot, age_period, ANA_AUTOAGE_AGE_PERIOD_M, ANA_AUTOAGE);
1305 }
1306 EXPORT_SYMBOL(ocelot_set_ageing_time);
1307
1308 static struct ocelot_multicast *ocelot_multicast_get(struct ocelot *ocelot,
1309                                                      const unsigned char *addr,
1310                                                      u16 vid)
1311 {
1312         struct ocelot_multicast *mc;
1313
1314         list_for_each_entry(mc, &ocelot->multicast, list) {
1315                 if (ether_addr_equal(mc->addr, addr) && mc->vid == vid)
1316                         return mc;
1317         }
1318
1319         return NULL;
1320 }
1321
1322 static enum macaccess_entry_type ocelot_classify_mdb(const unsigned char *addr)
1323 {
1324         if (addr[0] == 0x01 && addr[1] == 0x00 && addr[2] == 0x5e)
1325                 return ENTRYTYPE_MACv4;
1326         if (addr[0] == 0x33 && addr[1] == 0x33)
1327                 return ENTRYTYPE_MACv6;
1328         return ENTRYTYPE_LOCKED;
1329 }
1330
1331 static struct ocelot_pgid *ocelot_pgid_alloc(struct ocelot *ocelot, int index,
1332                                              unsigned long ports)
1333 {
1334         struct ocelot_pgid *pgid;
1335
1336         pgid = kzalloc(sizeof(*pgid), GFP_KERNEL);
1337         if (!pgid)
1338                 return ERR_PTR(-ENOMEM);
1339
1340         pgid->ports = ports;
1341         pgid->index = index;
1342         refcount_set(&pgid->refcount, 1);
1343         list_add_tail(&pgid->list, &ocelot->pgids);
1344
1345         return pgid;
1346 }
1347
1348 static void ocelot_pgid_free(struct ocelot *ocelot, struct ocelot_pgid *pgid)
1349 {
1350         if (!refcount_dec_and_test(&pgid->refcount))
1351                 return;
1352
1353         list_del(&pgid->list);
1354         kfree(pgid);
1355 }
1356
1357 static struct ocelot_pgid *ocelot_mdb_get_pgid(struct ocelot *ocelot,
1358                                                const struct ocelot_multicast *mc)
1359 {
1360         struct ocelot_pgid *pgid;
1361         int index;
1362
1363         /* According to VSC7514 datasheet 3.9.1.5 IPv4 Multicast Entries and
1364          * 3.9.1.6 IPv6 Multicast Entries, "Instead of a lookup in the
1365          * destination mask table (PGID), the destination set is programmed as
1366          * part of the entry MAC address.", and the DEST_IDX is set to 0.
1367          */
1368         if (mc->entry_type == ENTRYTYPE_MACv4 ||
1369             mc->entry_type == ENTRYTYPE_MACv6)
1370                 return ocelot_pgid_alloc(ocelot, 0, mc->ports);
1371
1372         list_for_each_entry(pgid, &ocelot->pgids, list) {
1373                 /* When searching for a nonreserved multicast PGID, ignore the
1374                  * dummy PGID of zero that we have for MACv4/MACv6 entries
1375                  */
1376                 if (pgid->index && pgid->ports == mc->ports) {
1377                         refcount_inc(&pgid->refcount);
1378                         return pgid;
1379                 }
1380         }
1381
1382         /* Search for a free index in the nonreserved multicast PGID area */
1383         for_each_nonreserved_multicast_dest_pgid(ocelot, index) {
1384                 bool used = false;
1385
1386                 list_for_each_entry(pgid, &ocelot->pgids, list) {
1387                         if (pgid->index == index) {
1388                                 used = true;
1389                                 break;
1390                         }
1391                 }
1392
1393                 if (!used)
1394                         return ocelot_pgid_alloc(ocelot, index, mc->ports);
1395         }
1396
1397         return ERR_PTR(-ENOSPC);
1398 }
1399
1400 static void ocelot_encode_ports_to_mdb(unsigned char *addr,
1401                                        struct ocelot_multicast *mc)
1402 {
1403         ether_addr_copy(addr, mc->addr);
1404
1405         if (mc->entry_type == ENTRYTYPE_MACv4) {
1406                 addr[0] = 0;
1407                 addr[1] = mc->ports >> 8;
1408                 addr[2] = mc->ports & 0xff;
1409         } else if (mc->entry_type == ENTRYTYPE_MACv6) {
1410                 addr[0] = mc->ports >> 8;
1411                 addr[1] = mc->ports & 0xff;
1412         }
1413 }
1414
1415 int ocelot_port_mdb_add(struct ocelot *ocelot, int port,
1416                         const struct switchdev_obj_port_mdb *mdb)
1417 {
1418         unsigned char addr[ETH_ALEN];
1419         struct ocelot_multicast *mc;
1420         struct ocelot_pgid *pgid;
1421         u16 vid = mdb->vid;
1422
1423         if (port == ocelot->npi)
1424                 port = ocelot->num_phys_ports;
1425
1426         mc = ocelot_multicast_get(ocelot, mdb->addr, vid);
1427         if (!mc) {
1428                 /* New entry */
1429                 mc = devm_kzalloc(ocelot->dev, sizeof(*mc), GFP_KERNEL);
1430                 if (!mc)
1431                         return -ENOMEM;
1432
1433                 mc->entry_type = ocelot_classify_mdb(mdb->addr);
1434                 ether_addr_copy(mc->addr, mdb->addr);
1435                 mc->vid = vid;
1436
1437                 list_add_tail(&mc->list, &ocelot->multicast);
1438         } else {
1439                 /* Existing entry. Clean up the current port mask from
1440                  * hardware now, because we'll be modifying it.
1441                  */
1442                 ocelot_pgid_free(ocelot, mc->pgid);
1443                 ocelot_encode_ports_to_mdb(addr, mc);
1444                 ocelot_mact_forget(ocelot, addr, vid);
1445         }
1446
1447         mc->ports |= BIT(port);
1448
1449         pgid = ocelot_mdb_get_pgid(ocelot, mc);
1450         if (IS_ERR(pgid)) {
1451                 dev_err(ocelot->dev,
1452                         "Cannot allocate PGID for mdb %pM vid %d\n",
1453                         mc->addr, mc->vid);
1454                 devm_kfree(ocelot->dev, mc);
1455                 return PTR_ERR(pgid);
1456         }
1457         mc->pgid = pgid;
1458
1459         ocelot_encode_ports_to_mdb(addr, mc);
1460
1461         if (mc->entry_type != ENTRYTYPE_MACv4 &&
1462             mc->entry_type != ENTRYTYPE_MACv6)
1463                 ocelot_write_rix(ocelot, pgid->ports, ANA_PGID_PGID,
1464                                  pgid->index);
1465
1466         return ocelot_mact_learn(ocelot, pgid->index, addr, vid,
1467                                  mc->entry_type);
1468 }
1469 EXPORT_SYMBOL(ocelot_port_mdb_add);
1470
1471 int ocelot_port_mdb_del(struct ocelot *ocelot, int port,
1472                         const struct switchdev_obj_port_mdb *mdb)
1473 {
1474         unsigned char addr[ETH_ALEN];
1475         struct ocelot_multicast *mc;
1476         struct ocelot_pgid *pgid;
1477         u16 vid = mdb->vid;
1478
1479         if (port == ocelot->npi)
1480                 port = ocelot->num_phys_ports;
1481
1482         mc = ocelot_multicast_get(ocelot, mdb->addr, vid);
1483         if (!mc)
1484                 return -ENOENT;
1485
1486         ocelot_encode_ports_to_mdb(addr, mc);
1487         ocelot_mact_forget(ocelot, addr, vid);
1488
1489         ocelot_pgid_free(ocelot, mc->pgid);
1490         mc->ports &= ~BIT(port);
1491         if (!mc->ports) {
1492                 list_del(&mc->list);
1493                 devm_kfree(ocelot->dev, mc);
1494                 return 0;
1495         }
1496
1497         /* We have a PGID with fewer ports now */
1498         pgid = ocelot_mdb_get_pgid(ocelot, mc);
1499         if (IS_ERR(pgid))
1500                 return PTR_ERR(pgid);
1501         mc->pgid = pgid;
1502
1503         ocelot_encode_ports_to_mdb(addr, mc);
1504
1505         if (mc->entry_type != ENTRYTYPE_MACv4 &&
1506             mc->entry_type != ENTRYTYPE_MACv6)
1507                 ocelot_write_rix(ocelot, pgid->ports, ANA_PGID_PGID,
1508                                  pgid->index);
1509
1510         return ocelot_mact_learn(ocelot, pgid->index, addr, vid,
1511                                  mc->entry_type);
1512 }
1513 EXPORT_SYMBOL(ocelot_port_mdb_del);
1514
1515 int ocelot_port_bridge_join(struct ocelot *ocelot, int port,
1516                             struct net_device *bridge)
1517 {
1518         if (!ocelot->bridge_mask) {
1519                 ocelot->hw_bridge_dev = bridge;
1520         } else {
1521                 if (ocelot->hw_bridge_dev != bridge)
1522                         /* This is adding the port to a second bridge, this is
1523                          * unsupported */
1524                         return -ENODEV;
1525         }
1526
1527         ocelot->bridge_mask |= BIT(port);
1528
1529         return 0;
1530 }
1531 EXPORT_SYMBOL(ocelot_port_bridge_join);
1532
1533 int ocelot_port_bridge_leave(struct ocelot *ocelot, int port,
1534                              struct net_device *bridge)
1535 {
1536         struct ocelot_vlan pvid = {0}, native_vlan = {0};
1537         int ret;
1538
1539         ocelot->bridge_mask &= ~BIT(port);
1540
1541         if (!ocelot->bridge_mask)
1542                 ocelot->hw_bridge_dev = NULL;
1543
1544         ret = ocelot_port_vlan_filtering(ocelot, port, false);
1545         if (ret)
1546                 return ret;
1547
1548         ocelot_port_set_pvid(ocelot, port, pvid);
1549         ocelot_port_set_native_vlan(ocelot, port, native_vlan);
1550
1551         return 0;
1552 }
1553 EXPORT_SYMBOL(ocelot_port_bridge_leave);
1554
1555 static void ocelot_set_aggr_pgids(struct ocelot *ocelot)
1556 {
1557         unsigned long visited = GENMASK(ocelot->num_phys_ports - 1, 0);
1558         int i, port, lag;
1559
1560         /* Reset destination and aggregation PGIDS */
1561         for_each_unicast_dest_pgid(ocelot, port)
1562                 ocelot_write_rix(ocelot, BIT(port), ANA_PGID_PGID, port);
1563
1564         for_each_aggr_pgid(ocelot, i)
1565                 ocelot_write_rix(ocelot, GENMASK(ocelot->num_phys_ports - 1, 0),
1566                                  ANA_PGID_PGID, i);
1567
1568         /* The visited ports bitmask holds the list of ports offloading any
1569          * bonding interface. Initially we mark all these ports as unvisited,
1570          * then every time we visit a port in this bitmask, we know that it is
1571          * the lowest numbered port, i.e. the one whose logical ID == physical
1572          * port ID == LAG ID. So we mark as visited all further ports in the
1573          * bitmask that are offloading the same bonding interface. This way,
1574          * we set up the aggregation PGIDs only once per bonding interface.
1575          */
1576         for (port = 0; port < ocelot->num_phys_ports; port++) {
1577                 struct ocelot_port *ocelot_port = ocelot->ports[port];
1578
1579                 if (!ocelot_port || !ocelot_port->bond)
1580                         continue;
1581
1582                 visited &= ~BIT(port);
1583         }
1584
1585         /* Now, set PGIDs for each active LAG */
1586         for (lag = 0; lag < ocelot->num_phys_ports; lag++) {
1587                 struct net_device *bond = ocelot->ports[lag]->bond;
1588                 int num_active_ports = 0;
1589                 unsigned long bond_mask;
1590                 u8 aggr_idx[16];
1591
1592                 if (!bond || (visited & BIT(lag)))
1593                         continue;
1594
1595                 bond_mask = ocelot_get_bond_mask(ocelot, bond, true);
1596
1597                 for_each_set_bit(port, &bond_mask, ocelot->num_phys_ports) {
1598                         // Destination mask
1599                         ocelot_write_rix(ocelot, bond_mask,
1600                                          ANA_PGID_PGID, port);
1601                         aggr_idx[num_active_ports++] = port;
1602                 }
1603
1604                 for_each_aggr_pgid(ocelot, i) {
1605                         u32 ac;
1606
1607                         ac = ocelot_read_rix(ocelot, ANA_PGID_PGID, i);
1608                         ac &= ~bond_mask;
1609                         /* Don't do division by zero if there was no active
1610                          * port. Just make all aggregation codes zero.
1611                          */
1612                         if (num_active_ports)
1613                                 ac |= BIT(aggr_idx[i % num_active_ports]);
1614                         ocelot_write_rix(ocelot, ac, ANA_PGID_PGID, i);
1615                 }
1616
1617                 /* Mark all ports in the same LAG as visited to avoid applying
1618                  * the same config again.
1619                  */
1620                 for (port = lag; port < ocelot->num_phys_ports; port++) {
1621                         struct ocelot_port *ocelot_port = ocelot->ports[port];
1622
1623                         if (!ocelot_port)
1624                                 continue;
1625
1626                         if (ocelot_port->bond == bond)
1627                                 visited |= BIT(port);
1628                 }
1629         }
1630 }
1631
1632 /* When offloading a bonding interface, the switch ports configured under the
1633  * same bond must have the same logical port ID, equal to the physical port ID
1634  * of the lowest numbered physical port in that bond. Otherwise, in standalone/
1635  * bridged mode, each port has a logical port ID equal to its physical port ID.
1636  */
1637 static void ocelot_setup_logical_port_ids(struct ocelot *ocelot)
1638 {
1639         int port;
1640
1641         for (port = 0; port < ocelot->num_phys_ports; port++) {
1642                 struct ocelot_port *ocelot_port = ocelot->ports[port];
1643                 struct net_device *bond;
1644
1645                 if (!ocelot_port)
1646                         continue;
1647
1648                 bond = ocelot_port->bond;
1649                 if (bond) {
1650                         int lag = __ffs(ocelot_get_bond_mask(ocelot, bond,
1651                                                              false));
1652
1653                         ocelot_rmw_gix(ocelot,
1654                                        ANA_PORT_PORT_CFG_PORTID_VAL(lag),
1655                                        ANA_PORT_PORT_CFG_PORTID_VAL_M,
1656                                        ANA_PORT_PORT_CFG, port);
1657                 } else {
1658                         ocelot_rmw_gix(ocelot,
1659                                        ANA_PORT_PORT_CFG_PORTID_VAL(port),
1660                                        ANA_PORT_PORT_CFG_PORTID_VAL_M,
1661                                        ANA_PORT_PORT_CFG, port);
1662                 }
1663         }
1664 }
1665
1666 int ocelot_port_lag_join(struct ocelot *ocelot, int port,
1667                          struct net_device *bond,
1668                          struct netdev_lag_upper_info *info)
1669 {
1670         if (info->tx_type != NETDEV_LAG_TX_TYPE_HASH)
1671                 return -EOPNOTSUPP;
1672
1673         ocelot->ports[port]->bond = bond;
1674
1675         ocelot_setup_logical_port_ids(ocelot);
1676         ocelot_apply_bridge_fwd_mask(ocelot);
1677         ocelot_set_aggr_pgids(ocelot);
1678
1679         return 0;
1680 }
1681 EXPORT_SYMBOL(ocelot_port_lag_join);
1682
1683 void ocelot_port_lag_leave(struct ocelot *ocelot, int port,
1684                            struct net_device *bond)
1685 {
1686         ocelot->ports[port]->bond = NULL;
1687
1688         ocelot_setup_logical_port_ids(ocelot);
1689         ocelot_apply_bridge_fwd_mask(ocelot);
1690         ocelot_set_aggr_pgids(ocelot);
1691 }
1692 EXPORT_SYMBOL(ocelot_port_lag_leave);
1693
1694 void ocelot_port_lag_change(struct ocelot *ocelot, int port, bool lag_tx_active)
1695 {
1696         struct ocelot_port *ocelot_port = ocelot->ports[port];
1697
1698         ocelot_port->lag_tx_active = lag_tx_active;
1699
1700         /* Rebalance the LAGs */
1701         ocelot_set_aggr_pgids(ocelot);
1702 }
1703 EXPORT_SYMBOL(ocelot_port_lag_change);
1704
1705 /* Configure the maximum SDU (L2 payload) on RX to the value specified in @sdu.
1706  * The length of VLAN tags is accounted for automatically via DEV_MAC_TAGS_CFG.
1707  * In the special case that it's the NPI port that we're configuring, the
1708  * length of the tag and optional prefix needs to be accounted for privately,
1709  * in order to be able to sustain communication at the requested @sdu.
1710  */
1711 void ocelot_port_set_maxlen(struct ocelot *ocelot, int port, size_t sdu)
1712 {
1713         struct ocelot_port *ocelot_port = ocelot->ports[port];
1714         int maxlen = sdu + ETH_HLEN + ETH_FCS_LEN;
1715         int pause_start, pause_stop;
1716         int atop, atop_tot;
1717
1718         if (port == ocelot->npi) {
1719                 maxlen += OCELOT_TAG_LEN;
1720
1721                 if (ocelot->npi_inj_prefix == OCELOT_TAG_PREFIX_SHORT)
1722                         maxlen += OCELOT_SHORT_PREFIX_LEN;
1723                 else if (ocelot->npi_inj_prefix == OCELOT_TAG_PREFIX_LONG)
1724                         maxlen += OCELOT_LONG_PREFIX_LEN;
1725         }
1726
1727         ocelot_port_writel(ocelot_port, maxlen, DEV_MAC_MAXLEN_CFG);
1728
1729         /* Set Pause watermark hysteresis */
1730         pause_start = 6 * maxlen / OCELOT_BUFFER_CELL_SZ;
1731         pause_stop = 4 * maxlen / OCELOT_BUFFER_CELL_SZ;
1732         ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_START,
1733                             pause_start);
1734         ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_STOP,
1735                             pause_stop);
1736
1737         /* Tail dropping watermarks */
1738         atop_tot = (ocelot->packet_buffer_size - 9 * maxlen) /
1739                    OCELOT_BUFFER_CELL_SZ;
1740         atop = (9 * maxlen) / OCELOT_BUFFER_CELL_SZ;
1741         ocelot_write_rix(ocelot, ocelot->ops->wm_enc(atop), SYS_ATOP, port);
1742         ocelot_write(ocelot, ocelot->ops->wm_enc(atop_tot), SYS_ATOP_TOT_CFG);
1743 }
1744 EXPORT_SYMBOL(ocelot_port_set_maxlen);
1745
1746 int ocelot_get_max_mtu(struct ocelot *ocelot, int port)
1747 {
1748         int max_mtu = 65535 - ETH_HLEN - ETH_FCS_LEN;
1749
1750         if (port == ocelot->npi) {
1751                 max_mtu -= OCELOT_TAG_LEN;
1752
1753                 if (ocelot->npi_inj_prefix == OCELOT_TAG_PREFIX_SHORT)
1754                         max_mtu -= OCELOT_SHORT_PREFIX_LEN;
1755                 else if (ocelot->npi_inj_prefix == OCELOT_TAG_PREFIX_LONG)
1756                         max_mtu -= OCELOT_LONG_PREFIX_LEN;
1757         }
1758
1759         return max_mtu;
1760 }
1761 EXPORT_SYMBOL(ocelot_get_max_mtu);
1762
1763 static void ocelot_port_set_learning(struct ocelot *ocelot, int port,
1764                                      bool enabled)
1765 {
1766         struct ocelot_port *ocelot_port = ocelot->ports[port];
1767         u32 val = 0;
1768
1769         if (enabled)
1770                 val = ANA_PORT_PORT_CFG_LEARN_ENA;
1771
1772         ocelot_rmw_gix(ocelot, val, ANA_PORT_PORT_CFG_LEARN_ENA,
1773                        ANA_PORT_PORT_CFG, port);
1774
1775         ocelot_port->learn_ena = enabled;
1776 }
1777
1778 static void ocelot_port_set_ucast_flood(struct ocelot *ocelot, int port,
1779                                         bool enabled)
1780 {
1781         u32 val = 0;
1782
1783         if (enabled)
1784                 val = BIT(port);
1785
1786         ocelot_rmw_rix(ocelot, val, BIT(port), ANA_PGID_PGID, PGID_UC);
1787 }
1788
1789 static void ocelot_port_set_mcast_flood(struct ocelot *ocelot, int port,
1790                                         bool enabled)
1791 {
1792         u32 val = 0;
1793
1794         if (enabled)
1795                 val = BIT(port);
1796
1797         ocelot_rmw_rix(ocelot, val, BIT(port), ANA_PGID_PGID, PGID_MC);
1798 }
1799
1800 static void ocelot_port_set_bcast_flood(struct ocelot *ocelot, int port,
1801                                         bool enabled)
1802 {
1803         u32 val = 0;
1804
1805         if (enabled)
1806                 val = BIT(port);
1807
1808         ocelot_rmw_rix(ocelot, val, BIT(port), ANA_PGID_PGID, PGID_BC);
1809 }
1810
1811 int ocelot_port_pre_bridge_flags(struct ocelot *ocelot, int port,
1812                                  struct switchdev_brport_flags flags)
1813 {
1814         if (flags.mask & ~(BR_LEARNING | BR_FLOOD | BR_MCAST_FLOOD |
1815                            BR_BCAST_FLOOD))
1816                 return -EINVAL;
1817
1818         return 0;
1819 }
1820 EXPORT_SYMBOL(ocelot_port_pre_bridge_flags);
1821
1822 void ocelot_port_bridge_flags(struct ocelot *ocelot, int port,
1823                               struct switchdev_brport_flags flags)
1824 {
1825         if (flags.mask & BR_LEARNING)
1826                 ocelot_port_set_learning(ocelot, port,
1827                                          !!(flags.val & BR_LEARNING));
1828
1829         if (flags.mask & BR_FLOOD)
1830                 ocelot_port_set_ucast_flood(ocelot, port,
1831                                             !!(flags.val & BR_FLOOD));
1832
1833         if (flags.mask & BR_MCAST_FLOOD)
1834                 ocelot_port_set_mcast_flood(ocelot, port,
1835                                             !!(flags.val & BR_MCAST_FLOOD));
1836
1837         if (flags.mask & BR_BCAST_FLOOD)
1838                 ocelot_port_set_bcast_flood(ocelot, port,
1839                                             !!(flags.val & BR_BCAST_FLOOD));
1840 }
1841 EXPORT_SYMBOL(ocelot_port_bridge_flags);
1842
1843 void ocelot_init_port(struct ocelot *ocelot, int port)
1844 {
1845         struct ocelot_port *ocelot_port = ocelot->ports[port];
1846
1847         skb_queue_head_init(&ocelot_port->tx_skbs);
1848         spin_lock_init(&ocelot_port->ts_id_lock);
1849
1850         /* Basic L2 initialization */
1851
1852         /* Set MAC IFG Gaps
1853          * FDX: TX_IFG = 5, RX_IFG1 = RX_IFG2 = 0
1854          * !FDX: TX_IFG = 5, RX_IFG1 = RX_IFG2 = 5
1855          */
1856         ocelot_port_writel(ocelot_port, DEV_MAC_IFG_CFG_TX_IFG(5),
1857                            DEV_MAC_IFG_CFG);
1858
1859         /* Load seed (0) and set MAC HDX late collision  */
1860         ocelot_port_writel(ocelot_port, DEV_MAC_HDX_CFG_LATE_COL_POS(67) |
1861                            DEV_MAC_HDX_CFG_SEED_LOAD,
1862                            DEV_MAC_HDX_CFG);
1863         mdelay(1);
1864         ocelot_port_writel(ocelot_port, DEV_MAC_HDX_CFG_LATE_COL_POS(67),
1865                            DEV_MAC_HDX_CFG);
1866
1867         /* Set Max Length and maximum tags allowed */
1868         ocelot_port_set_maxlen(ocelot, port, ETH_DATA_LEN);
1869         ocelot_port_writel(ocelot_port, DEV_MAC_TAGS_CFG_TAG_ID(ETH_P_8021AD) |
1870                            DEV_MAC_TAGS_CFG_VLAN_AWR_ENA |
1871                            DEV_MAC_TAGS_CFG_VLAN_DBL_AWR_ENA |
1872                            DEV_MAC_TAGS_CFG_VLAN_LEN_AWR_ENA,
1873                            DEV_MAC_TAGS_CFG);
1874
1875         /* Set SMAC of Pause frame (00:00:00:00:00:00) */
1876         ocelot_port_writel(ocelot_port, 0, DEV_MAC_FC_MAC_HIGH_CFG);
1877         ocelot_port_writel(ocelot_port, 0, DEV_MAC_FC_MAC_LOW_CFG);
1878
1879         /* Enable transmission of pause frames */
1880         ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_ENA, 1);
1881
1882         /* Drop frames with multicast source address */
1883         ocelot_rmw_gix(ocelot, ANA_PORT_DROP_CFG_DROP_MC_SMAC_ENA,
1884                        ANA_PORT_DROP_CFG_DROP_MC_SMAC_ENA,
1885                        ANA_PORT_DROP_CFG, port);
1886
1887         /* Set default VLAN and tag type to 8021Q. */
1888         ocelot_rmw_gix(ocelot, REW_PORT_VLAN_CFG_PORT_TPID(ETH_P_8021Q),
1889                        REW_PORT_VLAN_CFG_PORT_TPID_M,
1890                        REW_PORT_VLAN_CFG, port);
1891
1892         /* Disable source address learning for standalone mode */
1893         ocelot_port_set_learning(ocelot, port, false);
1894
1895         /* Enable vcap lookups */
1896         ocelot_vcap_enable(ocelot, port);
1897 }
1898 EXPORT_SYMBOL(ocelot_init_port);
1899
1900 /* Configure and enable the CPU port module, which is a set of queues
1901  * accessible through register MMIO, frame DMA or Ethernet (in case
1902  * NPI mode is used).
1903  */
1904 static void ocelot_cpu_port_init(struct ocelot *ocelot)
1905 {
1906         int cpu = ocelot->num_phys_ports;
1907
1908         /* The unicast destination PGID for the CPU port module is unused */
1909         ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, cpu);
1910         /* Instead set up a multicast destination PGID for traffic copied to
1911          * the CPU. Whitelisted MAC addresses like the port netdevice MAC
1912          * addresses will be copied to the CPU via this PGID.
1913          */
1914         ocelot_write_rix(ocelot, BIT(cpu), ANA_PGID_PGID, PGID_CPU);
1915         ocelot_write_gix(ocelot, ANA_PORT_PORT_CFG_RECV_ENA |
1916                          ANA_PORT_PORT_CFG_PORTID_VAL(cpu),
1917                          ANA_PORT_PORT_CFG, cpu);
1918
1919         /* Enable CPU port module */
1920         ocelot_fields_write(ocelot, cpu, QSYS_SWITCH_PORT_MODE_PORT_ENA, 1);
1921         /* CPU port Injection/Extraction configuration */
1922         ocelot_fields_write(ocelot, cpu, SYS_PORT_MODE_INCL_XTR_HDR,
1923                             OCELOT_TAG_PREFIX_NONE);
1924         ocelot_fields_write(ocelot, cpu, SYS_PORT_MODE_INCL_INJ_HDR,
1925                             OCELOT_TAG_PREFIX_NONE);
1926
1927         /* Configure the CPU port to be VLAN aware */
1928         ocelot_write_gix(ocelot, ANA_PORT_VLAN_CFG_VLAN_VID(0) |
1929                                  ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
1930                                  ANA_PORT_VLAN_CFG_VLAN_POP_CNT(1),
1931                          ANA_PORT_VLAN_CFG, cpu);
1932 }
1933
1934 static void ocelot_detect_features(struct ocelot *ocelot)
1935 {
1936         int mmgt, eq_ctrl;
1937
1938         /* For Ocelot, Felix, Seville, Serval etc, SYS:MMGT:MMGT:FREECNT holds
1939          * the number of 240-byte free memory words (aka 4-cell chunks) and not
1940          * 192 bytes as the documentation incorrectly says.
1941          */
1942         mmgt = ocelot_read(ocelot, SYS_MMGT);
1943         ocelot->packet_buffer_size = 240 * SYS_MMGT_FREECNT(mmgt);
1944
1945         eq_ctrl = ocelot_read(ocelot, QSYS_EQ_CTRL);
1946         ocelot->num_frame_refs = QSYS_MMGT_EQ_CTRL_FP_FREE_CNT(eq_ctrl);
1947 }
1948
1949 int ocelot_init(struct ocelot *ocelot)
1950 {
1951         char queue_name[32];
1952         int i, ret;
1953         u32 port;
1954
1955         if (ocelot->ops->reset) {
1956                 ret = ocelot->ops->reset(ocelot);
1957                 if (ret) {
1958                         dev_err(ocelot->dev, "Switch reset failed\n");
1959                         return ret;
1960                 }
1961         }
1962
1963         ocelot->stats = devm_kcalloc(ocelot->dev,
1964                                      ocelot->num_phys_ports * ocelot->num_stats,
1965                                      sizeof(u64), GFP_KERNEL);
1966         if (!ocelot->stats)
1967                 return -ENOMEM;
1968
1969         mutex_init(&ocelot->stats_lock);
1970         mutex_init(&ocelot->ptp_lock);
1971         spin_lock_init(&ocelot->ptp_clock_lock);
1972         snprintf(queue_name, sizeof(queue_name), "%s-stats",
1973                  dev_name(ocelot->dev));
1974         ocelot->stats_queue = create_singlethread_workqueue(queue_name);
1975         if (!ocelot->stats_queue)
1976                 return -ENOMEM;
1977
1978         ocelot->owq = alloc_ordered_workqueue("ocelot-owq", 0);
1979         if (!ocelot->owq) {
1980                 destroy_workqueue(ocelot->stats_queue);
1981                 return -ENOMEM;
1982         }
1983
1984         INIT_LIST_HEAD(&ocelot->multicast);
1985         INIT_LIST_HEAD(&ocelot->pgids);
1986         ocelot_detect_features(ocelot);
1987         ocelot_mact_init(ocelot);
1988         ocelot_vlan_init(ocelot);
1989         ocelot_vcap_init(ocelot);
1990         ocelot_cpu_port_init(ocelot);
1991
1992         for (port = 0; port < ocelot->num_phys_ports; port++) {
1993                 /* Clear all counters (5 groups) */
1994                 ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(port) |
1995                                      SYS_STAT_CFG_STAT_CLEAR_SHOT(0x7f),
1996                              SYS_STAT_CFG);
1997         }
1998
1999         /* Only use S-Tag */
2000         ocelot_write(ocelot, ETH_P_8021AD, SYS_VLAN_ETYPE_CFG);
2001
2002         /* Aggregation mode */
2003         ocelot_write(ocelot, ANA_AGGR_CFG_AC_SMAC_ENA |
2004                              ANA_AGGR_CFG_AC_DMAC_ENA |
2005                              ANA_AGGR_CFG_AC_IP4_SIPDIP_ENA |
2006                              ANA_AGGR_CFG_AC_IP4_TCPUDP_ENA |
2007                              ANA_AGGR_CFG_AC_IP6_FLOW_LBL_ENA |
2008                              ANA_AGGR_CFG_AC_IP6_TCPUDP_ENA,
2009                              ANA_AGGR_CFG);
2010
2011         /* Set MAC age time to default value. The entry is aged after
2012          * 2*AGE_PERIOD
2013          */
2014         ocelot_write(ocelot,
2015                      ANA_AUTOAGE_AGE_PERIOD(BR_DEFAULT_AGEING_TIME / 2 / HZ),
2016                      ANA_AUTOAGE);
2017
2018         /* Disable learning for frames discarded by VLAN ingress filtering */
2019         regmap_field_write(ocelot->regfields[ANA_ADVLEARN_VLAN_CHK], 1);
2020
2021         /* Setup frame ageing - fixed value "2 sec" - in 6.5 us units */
2022         ocelot_write(ocelot, SYS_FRM_AGING_AGE_TX_ENA |
2023                      SYS_FRM_AGING_MAX_AGE(307692), SYS_FRM_AGING);
2024
2025         /* Setup flooding PGIDs */
2026         for (i = 0; i < ocelot->num_flooding_pgids; i++)
2027                 ocelot_write_rix(ocelot, ANA_FLOODING_FLD_MULTICAST(PGID_MC) |
2028                                  ANA_FLOODING_FLD_BROADCAST(PGID_BC) |
2029                                  ANA_FLOODING_FLD_UNICAST(PGID_UC),
2030                                  ANA_FLOODING, i);
2031         ocelot_write(ocelot, ANA_FLOODING_IPMC_FLD_MC6_DATA(PGID_MCIPV6) |
2032                      ANA_FLOODING_IPMC_FLD_MC6_CTRL(PGID_MC) |
2033                      ANA_FLOODING_IPMC_FLD_MC4_DATA(PGID_MCIPV4) |
2034                      ANA_FLOODING_IPMC_FLD_MC4_CTRL(PGID_MC),
2035                      ANA_FLOODING_IPMC);
2036
2037         for (port = 0; port < ocelot->num_phys_ports; port++) {
2038                 /* Transmit the frame to the local port. */
2039                 ocelot_write_rix(ocelot, BIT(port), ANA_PGID_PGID, port);
2040                 /* Do not forward BPDU frames to the front ports. */
2041                 ocelot_write_gix(ocelot,
2042                                  ANA_PORT_CPU_FWD_BPDU_CFG_BPDU_REDIR_ENA(0xffff),
2043                                  ANA_PORT_CPU_FWD_BPDU_CFG,
2044                                  port);
2045                 /* Ensure bridging is disabled */
2046                 ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_SRC + port);
2047         }
2048
2049         for_each_nonreserved_multicast_dest_pgid(ocelot, i) {
2050                 u32 val = ANA_PGID_PGID_PGID(GENMASK(ocelot->num_phys_ports - 1, 0));
2051
2052                 ocelot_write_rix(ocelot, val, ANA_PGID_PGID, i);
2053         }
2054
2055         ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_BLACKHOLE);
2056
2057         /* Allow broadcast and unknown L2 multicast to the CPU. */
2058         ocelot_rmw_rix(ocelot, ANA_PGID_PGID_PGID(BIT(ocelot->num_phys_ports)),
2059                        ANA_PGID_PGID_PGID(BIT(ocelot->num_phys_ports)),
2060                        ANA_PGID_PGID, PGID_MC);
2061         ocelot_rmw_rix(ocelot, ANA_PGID_PGID_PGID(BIT(ocelot->num_phys_ports)),
2062                        ANA_PGID_PGID_PGID(BIT(ocelot->num_phys_ports)),
2063                        ANA_PGID_PGID, PGID_BC);
2064         ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_MCIPV4);
2065         ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_MCIPV6);
2066
2067         /* Allow manual injection via DEVCPU_QS registers, and byte swap these
2068          * registers endianness.
2069          */
2070         ocelot_write_rix(ocelot, QS_INJ_GRP_CFG_BYTE_SWAP |
2071                          QS_INJ_GRP_CFG_MODE(1), QS_INJ_GRP_CFG, 0);
2072         ocelot_write_rix(ocelot, QS_XTR_GRP_CFG_BYTE_SWAP |
2073                          QS_XTR_GRP_CFG_MODE(1), QS_XTR_GRP_CFG, 0);
2074         ocelot_write(ocelot, ANA_CPUQ_CFG_CPUQ_MIRROR(2) |
2075                      ANA_CPUQ_CFG_CPUQ_LRN(2) |
2076                      ANA_CPUQ_CFG_CPUQ_MAC_COPY(2) |
2077                      ANA_CPUQ_CFG_CPUQ_SRC_COPY(2) |
2078                      ANA_CPUQ_CFG_CPUQ_LOCKED_PORTMOVE(2) |
2079                      ANA_CPUQ_CFG_CPUQ_ALLBRIDGE(6) |
2080                      ANA_CPUQ_CFG_CPUQ_IPMC_CTRL(6) |
2081                      ANA_CPUQ_CFG_CPUQ_IGMP(6) |
2082                      ANA_CPUQ_CFG_CPUQ_MLD(6), ANA_CPUQ_CFG);
2083         for (i = 0; i < 16; i++)
2084                 ocelot_write_rix(ocelot, ANA_CPUQ_8021_CFG_CPUQ_GARP_VAL(6) |
2085                                  ANA_CPUQ_8021_CFG_CPUQ_BPDU_VAL(6),
2086                                  ANA_CPUQ_8021_CFG, i);
2087
2088         INIT_DELAYED_WORK(&ocelot->stats_work, ocelot_check_stats_work);
2089         queue_delayed_work(ocelot->stats_queue, &ocelot->stats_work,
2090                            OCELOT_STATS_CHECK_DELAY);
2091
2092         return 0;
2093 }
2094 EXPORT_SYMBOL(ocelot_init);
2095
2096 void ocelot_deinit(struct ocelot *ocelot)
2097 {
2098         cancel_delayed_work(&ocelot->stats_work);
2099         destroy_workqueue(ocelot->stats_queue);
2100         destroy_workqueue(ocelot->owq);
2101         mutex_destroy(&ocelot->stats_lock);
2102 }
2103 EXPORT_SYMBOL(ocelot_deinit);
2104
2105 void ocelot_deinit_port(struct ocelot *ocelot, int port)
2106 {
2107         struct ocelot_port *ocelot_port = ocelot->ports[port];
2108
2109         skb_queue_purge(&ocelot_port->tx_skbs);
2110 }
2111 EXPORT_SYMBOL(ocelot_deinit_port);
2112
2113 MODULE_LICENSE("Dual MIT/GPL");