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/if_bridge.h>
   8 #include "ocelot.h"
   9 #include "ocelot_vcap.h"
  10
  11 #define TABLE_UPDATE_SLEEP_US 10
  12 #define TABLE_UPDATE_TIMEOUT_US 100000
  13
  14 struct ocelot_mact_entry {
  15         u8 mac[ETH_ALEN];
  16         u16 vid;
  17         enum macaccess_entry_type type;
  18 };
  19
  20 static inline u32 ocelot_mact_read_macaccess(struct ocelot *ocelot)
  21 {
  22         return ocelot_read(ocelot, ANA_TABLES_MACACCESS);
  23 }
  24
  25 static inline int ocelot_mact_wait_for_completion(struct ocelot *ocelot)
  26 {
  27         u32 val;
  28
  29         return readx_poll_timeout(ocelot_mact_read_macaccess,
  30                 ocelot, val,
  31                 (val & ANA_TABLES_MACACCESS_MAC_TABLE_CMD_M) ==
  32                 MACACCESS_CMD_IDLE,
  33                 TABLE_UPDATE_SLEEP_US, TABLE_UPDATE_TIMEOUT_US);
  34 }
  35
  36 static void ocelot_mact_select(struct ocelot *ocelot,
  37                                const unsigned char mac[ETH_ALEN],
  38                                unsigned int vid)
  39 {
  40         u32 macl = 0, mach = 0;
  41
  42         /* Set the MAC address to handle and the vlan associated in a format
  43          * understood by the hardware.
  44          */
  45         mach |= vid    << 16;
  46         mach |= mac[0] << 8;
  47         mach |= mac[1] << 0;
  48         macl |= mac[2] << 24;
  49         macl |= mac[3] << 16;
  50         macl |= mac[4] << 8;
  51         macl |= mac[5] << 0;
  52
  53         ocelot_write(ocelot, macl, ANA_TABLES_MACLDATA);
  54         ocelot_write(ocelot, mach, ANA_TABLES_MACHDATA);
  55
  56 }
  57
  58 int ocelot_mact_learn(struct ocelot *ocelot, int port,
  59                       const unsigned char mac[ETH_ALEN],
  60                       unsigned int vid, enum macaccess_entry_type type)
  61 {
  62         ocelot_mact_select(ocelot, mac, vid);
  63
  64         /* Issue a write command */
  65         ocelot_write(ocelot, ANA_TABLES_MACACCESS_VALID |
  66                              ANA_TABLES_MACACCESS_DEST_IDX(port) |
  67                              ANA_TABLES_MACACCESS_ENTRYTYPE(type) |
  68                              ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_LEARN),
  69                              ANA_TABLES_MACACCESS);
  70
  71         return ocelot_mact_wait_for_completion(ocelot);
  72 }
  73 EXPORT_SYMBOL(ocelot_mact_learn);
  74
  75 int ocelot_mact_forget(struct ocelot *ocelot,
  76                        const unsigned char mac[ETH_ALEN], unsigned int vid)
  77 {
  78         ocelot_mact_select(ocelot, mac, vid);
  79
  80         /* Issue a forget command */
  81         ocelot_write(ocelot,
  82                      ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_FORGET),
  83                      ANA_TABLES_MACACCESS);
  84
  85         return ocelot_mact_wait_for_completion(ocelot);
  86 }
  87 EXPORT_SYMBOL(ocelot_mact_forget);
  88
  89 static void ocelot_mact_init(struct ocelot *ocelot)
  90 {
  91         /* Configure the learning mode entries attributes:
  92          * - Do not copy the frame to the CPU extraction queues.
  93          * - Use the vlan and mac_cpoy for dmac lookup.
  94          */
  95         ocelot_rmw(ocelot, 0,
  96                    ANA_AGENCTRL_LEARN_CPU_COPY | ANA_AGENCTRL_IGNORE_DMAC_FLAGS
  97                    | ANA_AGENCTRL_LEARN_FWD_KILL
  98                    | ANA_AGENCTRL_LEARN_IGNORE_VLAN,
  99                    ANA_AGENCTRL);
 100
 101         /* Clear the MAC table */
 102         ocelot_write(ocelot, MACACCESS_CMD_INIT, ANA_TABLES_MACACCESS);
 103 }
 104
 105 static void ocelot_vcap_enable(struct ocelot *ocelot, int port)
 106 {
 107         ocelot_write_gix(ocelot, ANA_PORT_VCAP_S2_CFG_S2_ENA |
 108                          ANA_PORT_VCAP_S2_CFG_S2_IP6_CFG(0xa),
 109                          ANA_PORT_VCAP_S2_CFG, port);
 110 }
 111
 112 static inline u32 ocelot_vlant_read_vlanaccess(struct ocelot *ocelot)
 113 {
 114         return ocelot_read(ocelot, ANA_TABLES_VLANACCESS);
 115 }
 116
 117 static inline int ocelot_vlant_wait_for_completion(struct ocelot *ocelot)
 118 {
 119         u32 val;
 120
 121         return readx_poll_timeout(ocelot_vlant_read_vlanaccess,
 122                 ocelot,
 123                 val,
 124                 (val & ANA_TABLES_VLANACCESS_VLAN_TBL_CMD_M) ==
 125                 ANA_TABLES_VLANACCESS_CMD_IDLE,
 126                 TABLE_UPDATE_SLEEP_US, TABLE_UPDATE_TIMEOUT_US);
 127 }
 128
 129 static int ocelot_vlant_set_mask(struct ocelot *ocelot, u16 vid, u32 mask)
 130 {
 131         /* Select the VID to configure */
 132         ocelot_write(ocelot, ANA_TABLES_VLANTIDX_V_INDEX(vid),
 133                      ANA_TABLES_VLANTIDX);
 134         /* Set the vlan port members mask and issue a write command */
 135         ocelot_write(ocelot, ANA_TABLES_VLANACCESS_VLAN_PORT_MASK(mask) |
 136                              ANA_TABLES_VLANACCESS_CMD_WRITE,
 137                      ANA_TABLES_VLANACCESS);
 138
 139         return ocelot_vlant_wait_for_completion(ocelot);
 140 }
 141
 142 static int ocelot_port_set_native_vlan(struct ocelot *ocelot, int port,
 143                                        u16 vid)
 144 {
 145         struct ocelot_port *ocelot_port = ocelot->ports[port];
 146         u32 val = 0;
 147
 148         if (ocelot_port->vid != vid) {
 149                 /* Always permit deleting the native VLAN (vid = 0) */
 150                 if (ocelot_port->vid && vid) {
 151                         dev_err(ocelot->dev,
 152                                 "Port already has a native VLAN: %d\n",
 153                                 ocelot_port->vid);
 154                         return -EBUSY;
 155                 }
 156                 ocelot_port->vid = vid;
 157         }
 158
 159         ocelot_rmw_gix(ocelot, REW_PORT_VLAN_CFG_PORT_VID(vid),
 160                        REW_PORT_VLAN_CFG_PORT_VID_M,
 161                        REW_PORT_VLAN_CFG, port);
 162
 163         if (ocelot_port->vlan_aware && !ocelot_port->vid)
 164                 /* If port is vlan-aware and tagged, drop untagged and priority
 165                  * tagged frames.
 166                  */
 167                 val = ANA_PORT_DROP_CFG_DROP_UNTAGGED_ENA |
 168                       ANA_PORT_DROP_CFG_DROP_PRIO_S_TAGGED_ENA |
 169                       ANA_PORT_DROP_CFG_DROP_PRIO_C_TAGGED_ENA;
 170         ocelot_rmw_gix(ocelot, val,
 171                        ANA_PORT_DROP_CFG_DROP_UNTAGGED_ENA |
 172                        ANA_PORT_DROP_CFG_DROP_PRIO_S_TAGGED_ENA |
 173                        ANA_PORT_DROP_CFG_DROP_PRIO_C_TAGGED_ENA,
 174                        ANA_PORT_DROP_CFG, port);
 175
 176         if (ocelot_port->vlan_aware) {
 177                 if (ocelot_port->vid)
 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         return 0;
 192 }
 193
 194 void ocelot_port_vlan_filtering(struct ocelot *ocelot, int port,
 195                                 bool vlan_aware)
 196 {
 197         struct ocelot_port *ocelot_port = ocelot->ports[port];
 198         u32 val;
 199
 200         ocelot_port->vlan_aware = vlan_aware;
 201
 202         if (vlan_aware)
 203                 val = ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
 204                       ANA_PORT_VLAN_CFG_VLAN_POP_CNT(1);
 205         else
 206                 val = 0;
 207         ocelot_rmw_gix(ocelot, val,
 208                        ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
 209                        ANA_PORT_VLAN_CFG_VLAN_POP_CNT_M,
 210                        ANA_PORT_VLAN_CFG, port);
 211
 212         ocelot_port_set_native_vlan(ocelot, port, ocelot_port->vid);
 213 }
 214 EXPORT_SYMBOL(ocelot_port_vlan_filtering);
 215
 216 /* Default vlan to clasify for untagged frames (may be zero) */
 217 static void ocelot_port_set_pvid(struct ocelot *ocelot, int port, u16 pvid)
 218 {
 219         struct ocelot_port *ocelot_port = ocelot->ports[port];
 220
 221         ocelot_rmw_gix(ocelot,
 222                        ANA_PORT_VLAN_CFG_VLAN_VID(pvid),
 223                        ANA_PORT_VLAN_CFG_VLAN_VID_M,
 224                        ANA_PORT_VLAN_CFG, port);
 225
 226         ocelot_port->pvid = pvid;
 227 }
 228
 229 int ocelot_vlan_add(struct ocelot *ocelot, int port, u16 vid, bool pvid,
 230                     bool untagged)
 231 {
 232         int ret;
 233
 234         /* Make the port a member of the VLAN */
 235         ocelot->vlan_mask[vid] |= BIT(port);
 236         ret = ocelot_vlant_set_mask(ocelot, vid, ocelot->vlan_mask[vid]);
 237         if (ret)
 238                 return ret;
 239
 240         /* Default ingress vlan classification */
 241         if (pvid)
 242                 ocelot_port_set_pvid(ocelot, port, vid);
 243
 244         /* Untagged egress vlan clasification */
 245         if (untagged) {
 246                 ret = ocelot_port_set_native_vlan(ocelot, port, vid);
 247                 if (ret)
 248                         return ret;
 249         }
 250
 251         return 0;
 252 }
 253 EXPORT_SYMBOL(ocelot_vlan_add);
 254
 255 int ocelot_vlan_del(struct ocelot *ocelot, int port, u16 vid)
 256 {
 257         struct ocelot_port *ocelot_port = ocelot->ports[port];
 258         int ret;
 259
 260         /* Stop the port from being a member of the vlan */
 261         ocelot->vlan_mask[vid] &= ~BIT(port);
 262         ret = ocelot_vlant_set_mask(ocelot, vid, ocelot->vlan_mask[vid]);
 263         if (ret)
 264                 return ret;
 265
 266         /* Ingress */
 267         if (ocelot_port->pvid == vid)
 268                 ocelot_port_set_pvid(ocelot, port, 0);
 269
 270         /* Egress */
 271         if (ocelot_port->vid == vid)
 272                 ocelot_port_set_native_vlan(ocelot, port, 0);
 273
 274         return 0;
 275 }
 276 EXPORT_SYMBOL(ocelot_vlan_del);
 277
 278 static void ocelot_vlan_init(struct ocelot *ocelot)
 279 {
 280         u16 port, vid;
 281
 282         /* Clear VLAN table, by default all ports are members of all VLANs */
 283         ocelot_write(ocelot, ANA_TABLES_VLANACCESS_CMD_INIT,
 284                      ANA_TABLES_VLANACCESS);
 285         ocelot_vlant_wait_for_completion(ocelot);
 286
 287         /* Configure the port VLAN memberships */
 288         for (vid = 1; vid < VLAN_N_VID; vid++) {
 289                 ocelot->vlan_mask[vid] = 0;
 290                 ocelot_vlant_set_mask(ocelot, vid, ocelot->vlan_mask[vid]);
 291         }
 292
 293         /* Because VLAN filtering is enabled, we need VID 0 to get untagged
 294          * traffic.  It is added automatically if 8021q module is loaded, but
 295          * we can't rely on it since module may be not loaded.
 296          */
 297         ocelot->vlan_mask[0] = GENMASK(ocelot->num_phys_ports - 1, 0);
 298         ocelot_vlant_set_mask(ocelot, 0, ocelot->vlan_mask[0]);
 299
 300         /* Set vlan ingress filter mask to all ports but the CPU port by
 301          * default.
 302          */
 303         ocelot_write(ocelot, GENMASK(ocelot->num_phys_ports - 1, 0),
 304                      ANA_VLANMASK);
 305
 306         for (port = 0; port < ocelot->num_phys_ports; port++) {
 307                 ocelot_write_gix(ocelot, 0, REW_PORT_VLAN_CFG, port);
 308                 ocelot_write_gix(ocelot, 0, REW_TAG_CFG, port);
 309         }
 310 }
 311
 312 void ocelot_adjust_link(struct ocelot *ocelot, int port,
 313                         struct phy_device *phydev)
 314 {
 315         struct ocelot_port *ocelot_port = ocelot->ports[port];
 316         int speed, mode = 0;
 317
 318         switch (phydev->speed) {
 319         case SPEED_10:
 320                 speed = OCELOT_SPEED_10;
 321                 break;
 322         case SPEED_100:
 323                 speed = OCELOT_SPEED_100;
 324                 break;
 325         case SPEED_1000:
 326                 speed = OCELOT_SPEED_1000;
 327                 mode = DEV_MAC_MODE_CFG_GIGA_MODE_ENA;
 328                 break;
 329         case SPEED_2500:
 330                 speed = OCELOT_SPEED_2500;
 331                 mode = DEV_MAC_MODE_CFG_GIGA_MODE_ENA;
 332                 break;
 333         default:
 334                 dev_err(ocelot->dev, "Unsupported PHY speed on port %d: %d\n",
 335                         port, phydev->speed);
 336                 return;
 337         }
 338
 339         phy_print_status(phydev);
 340
 341         if (!phydev->link)
 342                 return;
 343
 344         /* Only full duplex supported for now */
 345         ocelot_port_writel(ocelot_port, DEV_MAC_MODE_CFG_FDX_ENA |
 346                            mode, DEV_MAC_MODE_CFG);
 347
 348         /* Disable HDX fast control */
 349         ocelot_port_writel(ocelot_port, DEV_PORT_MISC_HDX_FAST_DIS,
 350                            DEV_PORT_MISC);
 351
 352         /* SGMII only for now */
 353         ocelot_port_writel(ocelot_port, PCS1G_MODE_CFG_SGMII_MODE_ENA,
 354                            PCS1G_MODE_CFG);
 355         ocelot_port_writel(ocelot_port, PCS1G_SD_CFG_SD_SEL, PCS1G_SD_CFG);
 356
 357         /* Enable PCS */
 358         ocelot_port_writel(ocelot_port, PCS1G_CFG_PCS_ENA, PCS1G_CFG);
 359
 360         /* No aneg on SGMII */
 361         ocelot_port_writel(ocelot_port, 0, PCS1G_ANEG_CFG);
 362
 363         /* No loopback */
 364         ocelot_port_writel(ocelot_port, 0, PCS1G_LB_CFG);
 365
 366         /* Enable MAC module */
 367         ocelot_port_writel(ocelot_port, DEV_MAC_ENA_CFG_RX_ENA |
 368                            DEV_MAC_ENA_CFG_TX_ENA, DEV_MAC_ENA_CFG);
 369
 370         /* Take MAC, Port, Phy (intern) and PCS (SGMII/Serdes) clock out of
 371          * reset */
 372         ocelot_port_writel(ocelot_port, DEV_CLOCK_CFG_LINK_SPEED(speed),
 373                            DEV_CLOCK_CFG);
 374
 375         /* No PFC */
 376         ocelot_write_gix(ocelot, ANA_PFC_PFC_CFG_FC_LINK_SPEED(speed),
 377                          ANA_PFC_PFC_CFG, port);
 378
 379         /* Core: Enable port for frame transfer */
 380         ocelot_fields_write(ocelot, port,
 381                             QSYS_SWITCH_PORT_MODE_PORT_ENA, 1);
 382
 383         /* Flow control */
 384         ocelot_write_rix(ocelot, SYS_MAC_FC_CFG_PAUSE_VAL_CFG(0xffff) |
 385                          SYS_MAC_FC_CFG_RX_FC_ENA | SYS_MAC_FC_CFG_TX_FC_ENA |
 386                          SYS_MAC_FC_CFG_ZERO_PAUSE_ENA |
 387                          SYS_MAC_FC_CFG_FC_LATENCY_CFG(0x7) |
 388                          SYS_MAC_FC_CFG_FC_LINK_SPEED(speed),
 389                          SYS_MAC_FC_CFG, port);
 390         ocelot_write_rix(ocelot, 0, ANA_POL_FLOWC, port);
 391 }
 392 EXPORT_SYMBOL(ocelot_adjust_link);
 393
 394 void ocelot_port_enable(struct ocelot *ocelot, int port,
 395                         struct phy_device *phy)
 396 {
 397         /* Enable receiving frames on the port, and activate auto-learning of
 398          * MAC addresses.
 399          */
 400         ocelot_write_gix(ocelot, ANA_PORT_PORT_CFG_LEARNAUTO |
 401                          ANA_PORT_PORT_CFG_RECV_ENA |
 402                          ANA_PORT_PORT_CFG_PORTID_VAL(port),
 403                          ANA_PORT_PORT_CFG, port);
 404 }
 405 EXPORT_SYMBOL(ocelot_port_enable);
 406
 407 void ocelot_port_disable(struct ocelot *ocelot, int port)
 408 {
 409         struct ocelot_port *ocelot_port = ocelot->ports[port];
 410
 411         ocelot_port_writel(ocelot_port, 0, DEV_MAC_ENA_CFG);
 412         ocelot_fields_write(ocelot, port, QSYS_SWITCH_PORT_MODE_PORT_ENA, 0);
 413 }
 414 EXPORT_SYMBOL(ocelot_port_disable);
 415
 416 int ocelot_port_add_txtstamp_skb(struct ocelot_port *ocelot_port,
 417                                  struct sk_buff *skb)
 418 {
 419         struct skb_shared_info *shinfo = skb_shinfo(skb);
 420         struct ocelot *ocelot = ocelot_port->ocelot;
 421
 422         if (ocelot->ptp && shinfo->tx_flags & SKBTX_HW_TSTAMP &&
 423             ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
 424                 shinfo->tx_flags |= SKBTX_IN_PROGRESS;
 425                 /* Store timestamp ID in cb[0] of sk_buff */
 426                 skb->cb[0] = ocelot_port->ts_id % 4;
 427                 skb_queue_tail(&ocelot_port->tx_skbs, skb);
 428                 return 0;
 429         }
 430         return -ENODATA;
 431 }
 432 EXPORT_SYMBOL(ocelot_port_add_txtstamp_skb);
 433
 434 static void ocelot_get_hwtimestamp(struct ocelot *ocelot,
 435                                    struct timespec64 *ts)
 436 {
 437         unsigned long flags;
 438         u32 val;
 439
 440         spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
 441
 442         /* Read current PTP time to get seconds */
 443         val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
 444
 445         val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
 446         val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE);
 447         ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
 448         ts->tv_sec = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
 449
 450         /* Read packet HW timestamp from FIFO */
 451         val = ocelot_read(ocelot, SYS_PTP_TXSTAMP);
 452         ts->tv_nsec = SYS_PTP_TXSTAMP_PTP_TXSTAMP(val);
 453
 454         /* Sec has incremented since the ts was registered */
 455         if ((ts->tv_sec & 0x1) != !!(val & SYS_PTP_TXSTAMP_PTP_TXSTAMP_SEC))
 456                 ts->tv_sec--;
 457
 458         spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
 459 }
 460
 461 void ocelot_get_txtstamp(struct ocelot *ocelot)
 462 {
 463         int budget = OCELOT_PTP_QUEUE_SZ;
 464
 465         while (budget--) {
 466                 struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
 467                 struct skb_shared_hwtstamps shhwtstamps;
 468                 struct ocelot_port *port;
 469                 struct timespec64 ts;
 470                 unsigned long flags;
 471                 u32 val, id, txport;
 472
 473                 val = ocelot_read(ocelot, SYS_PTP_STATUS);
 474
 475                 /* Check if a timestamp can be retrieved */
 476                 if (!(val & SYS_PTP_STATUS_PTP_MESS_VLD))
 477                         break;
 478
 479                 WARN_ON(val & SYS_PTP_STATUS_PTP_OVFL);
 480
 481                 /* Retrieve the ts ID and Tx port */
 482                 id = SYS_PTP_STATUS_PTP_MESS_ID_X(val);
 483                 txport = SYS_PTP_STATUS_PTP_MESS_TXPORT_X(val);
 484
 485                 /* Retrieve its associated skb */
 486                 port = ocelot->ports[txport];
 487
 488                 spin_lock_irqsave(&port->tx_skbs.lock, flags);
 489
 490                 skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
 491                         if (skb->cb[0] != id)
 492                                 continue;
 493                         __skb_unlink(skb, &port->tx_skbs);
 494                         skb_match = skb;
 495                         break;
 496                 }
 497
 498                 spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
 499
 500                 /* Get the h/w timestamp */
 501                 ocelot_get_hwtimestamp(ocelot, &ts);
 502
 503                 if (unlikely(!skb_match))
 504                         continue;
 505
 506                 /* Set the timestamp into the skb */
 507                 memset(&shhwtstamps, 0, sizeof(shhwtstamps));
 508                 shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
 509                 skb_tstamp_tx(skb_match, &shhwtstamps);
 510
 511                 dev_kfree_skb_any(skb_match);
 512
 513                 /* Next ts */
 514                 ocelot_write(ocelot, SYS_PTP_NXT_PTP_NXT, SYS_PTP_NXT);
 515         }
 516 }
 517 EXPORT_SYMBOL(ocelot_get_txtstamp);
 518
 519 int ocelot_fdb_add(struct ocelot *ocelot, int port,
 520                    const unsigned char *addr, u16 vid)
 521 {
 522         struct ocelot_port *ocelot_port = ocelot->ports[port];
 523         int pgid = port;
 524
 525         if (port == ocelot->npi)
 526                 pgid = PGID_CPU;
 527
 528         if (!vid) {
 529                 if (!ocelot_port->vlan_aware)
 530                         /* If the bridge is not VLAN aware and no VID was
 531                          * provided, set it to pvid to ensure the MAC entry
 532                          * matches incoming untagged packets
 533                          */
 534                         vid = ocelot_port->pvid;
 535                 else
 536                         /* If the bridge is VLAN aware a VID must be provided as
 537                          * otherwise the learnt entry wouldn't match any frame.
 538                          */
 539                         return -EINVAL;
 540         }
 541
 542         return ocelot_mact_learn(ocelot, pgid, addr, vid, ENTRYTYPE_LOCKED);
 543 }
 544 EXPORT_SYMBOL(ocelot_fdb_add);
 545
 546 int ocelot_fdb_del(struct ocelot *ocelot, int port,
 547                    const unsigned char *addr, u16 vid)
 548 {
 549         return ocelot_mact_forget(ocelot, addr, vid);
 550 }
 551 EXPORT_SYMBOL(ocelot_fdb_del);
 552
 553 int ocelot_port_fdb_do_dump(const unsigned char *addr, u16 vid,
 554                             bool is_static, void *data)
 555 {
 556         struct ocelot_dump_ctx *dump = data;
 557         u32 portid = NETLINK_CB(dump->cb->skb).portid;
 558         u32 seq = dump->cb->nlh->nlmsg_seq;
 559         struct nlmsghdr *nlh;
 560         struct ndmsg *ndm;
 561
 562         if (dump->idx < dump->cb->args[2])
 563                 goto skip;
 564
 565         nlh = nlmsg_put(dump->skb, portid, seq, RTM_NEWNEIGH,
 566                         sizeof(*ndm), NLM_F_MULTI);
 567         if (!nlh)
 568                 return -EMSGSIZE;
 569
 570         ndm = nlmsg_data(nlh);
 571         ndm->ndm_family  = AF_BRIDGE;
 572         ndm->ndm_pad1    = 0;
 573         ndm->ndm_pad2    = 0;
 574         ndm->ndm_flags   = NTF_SELF;
 575         ndm->ndm_type    = 0;
 576         ndm->ndm_ifindex = dump->dev->ifindex;
 577         ndm->ndm_state   = is_static ? NUD_NOARP : NUD_REACHABLE;
 578
 579         if (nla_put(dump->skb, NDA_LLADDR, ETH_ALEN, addr))
 580                 goto nla_put_failure;
 581
 582         if (vid && nla_put_u16(dump->skb, NDA_VLAN, vid))
 583                 goto nla_put_failure;
 584
 585         nlmsg_end(dump->skb, nlh);
 586
 587 skip:
 588         dump->idx++;
 589         return 0;
 590
 591 nla_put_failure:
 592         nlmsg_cancel(dump->skb, nlh);
 593         return -EMSGSIZE;
 594 }
 595 EXPORT_SYMBOL(ocelot_port_fdb_do_dump);
 596
 597 static int ocelot_mact_read(struct ocelot *ocelot, int port, int row, int col,
 598                             struct ocelot_mact_entry *entry)
 599 {
 600         u32 val, dst, macl, mach;
 601         char mac[ETH_ALEN];
 602
 603         /* Set row and column to read from */
 604         ocelot_field_write(ocelot, ANA_TABLES_MACTINDX_M_INDEX, row);
 605         ocelot_field_write(ocelot, ANA_TABLES_MACTINDX_BUCKET, col);
 606
 607         /* Issue a read command */
 608         ocelot_write(ocelot,
 609                      ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_READ),
 610                      ANA_TABLES_MACACCESS);
 611
 612         if (ocelot_mact_wait_for_completion(ocelot))
 613                 return -ETIMEDOUT;
 614
 615         /* Read the entry flags */
 616         val = ocelot_read(ocelot, ANA_TABLES_MACACCESS);
 617         if (!(val & ANA_TABLES_MACACCESS_VALID))
 618                 return -EINVAL;
 619
 620         /* If the entry read has another port configured as its destination,
 621          * do not report it.
 622          */
 623         dst = (val & ANA_TABLES_MACACCESS_DEST_IDX_M) >> 3;
 624         if (dst != port)
 625                 return -EINVAL;
 626
 627         /* Get the entry's MAC address and VLAN id */
 628         macl = ocelot_read(ocelot, ANA_TABLES_MACLDATA);
 629         mach = ocelot_read(ocelot, ANA_TABLES_MACHDATA);
 630
 631         mac[0] = (mach >> 8)  & 0xff;
 632         mac[1] = (mach >> 0)  & 0xff;
 633         mac[2] = (macl >> 24) & 0xff;
 634         mac[3] = (macl >> 16) & 0xff;
 635         mac[4] = (macl >> 8)  & 0xff;
 636         mac[5] = (macl >> 0)  & 0xff;
 637
 638         entry->vid = (mach >> 16) & 0xfff;
 639         ether_addr_copy(entry->mac, mac);
 640
 641         return 0;
 642 }
 643
 644 int ocelot_fdb_dump(struct ocelot *ocelot, int port,
 645                     dsa_fdb_dump_cb_t *cb, void *data)
 646 {
 647         int i, j;
 648
 649         /* Loop through all the mac tables entries. */
 650         for (i = 0; i < ocelot->num_mact_rows; i++) {
 651                 for (j = 0; j < 4; j++) {
 652                         struct ocelot_mact_entry entry;
 653                         bool is_static;
 654                         int ret;
 655
 656                         ret = ocelot_mact_read(ocelot, port, i, j, &entry);
 657                         /* If the entry is invalid (wrong port, invalid...),
 658                          * skip it.
 659                          */
 660                         if (ret == -EINVAL)
 661                                 continue;
 662                         else if (ret)
 663                                 return ret;
 664
 665                         is_static = (entry.type == ENTRYTYPE_LOCKED);
 666
 667                         ret = cb(entry.mac, entry.vid, is_static, data);
 668                         if (ret)
 669                                 return ret;
 670                 }
 671         }
 672
 673         return 0;
 674 }
 675 EXPORT_SYMBOL(ocelot_fdb_dump);
 676
 677 int ocelot_hwstamp_get(struct ocelot *ocelot, int port, struct ifreq *ifr)
 678 {
 679         return copy_to_user(ifr->ifr_data, &ocelot->hwtstamp_config,
 680                             sizeof(ocelot->hwtstamp_config)) ? -EFAULT : 0;
 681 }
 682 EXPORT_SYMBOL(ocelot_hwstamp_get);
 683
 684 int ocelot_hwstamp_set(struct ocelot *ocelot, int port, struct ifreq *ifr)
 685 {
 686         struct ocelot_port *ocelot_port = ocelot->ports[port];
 687         struct hwtstamp_config cfg;
 688
 689         if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
 690                 return -EFAULT;
 691
 692         /* reserved for future extensions */
 693         if (cfg.flags)
 694                 return -EINVAL;
 695
 696         /* Tx type sanity check */
 697         switch (cfg.tx_type) {
 698         case HWTSTAMP_TX_ON:
 699                 ocelot_port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
 700                 break;
 701         case HWTSTAMP_TX_ONESTEP_SYNC:
 702                 /* IFH_REW_OP_ONE_STEP_PTP updates the correctional field, we
 703                  * need to update the origin time.
 704                  */
 705                 ocelot_port->ptp_cmd = IFH_REW_OP_ORIGIN_PTP;
 706                 break;
 707         case HWTSTAMP_TX_OFF:
 708                 ocelot_port->ptp_cmd = 0;
 709                 break;
 710         default:
 711                 return -ERANGE;
 712         }
 713
 714         mutex_lock(&ocelot->ptp_lock);
 715
 716         switch (cfg.rx_filter) {
 717         case HWTSTAMP_FILTER_NONE:
 718                 break;
 719         case HWTSTAMP_FILTER_ALL:
 720         case HWTSTAMP_FILTER_SOME:
 721         case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
 722         case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
 723         case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
 724         case HWTSTAMP_FILTER_NTP_ALL:
 725         case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
 726         case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
 727         case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
 728         case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
 729         case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
 730         case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
 731         case HWTSTAMP_FILTER_PTP_V2_EVENT:
 732         case HWTSTAMP_FILTER_PTP_V2_SYNC:
 733         case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
 734                 cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
 735                 break;
 736         default:
 737                 mutex_unlock(&ocelot->ptp_lock);
 738                 return -ERANGE;
 739         }
 740
 741         /* Commit back the result & save it */
 742         memcpy(&ocelot->hwtstamp_config, &cfg, sizeof(cfg));
 743         mutex_unlock(&ocelot->ptp_lock);
 744
 745         return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
 746 }
 747 EXPORT_SYMBOL(ocelot_hwstamp_set);
 748
 749 void ocelot_get_strings(struct ocelot *ocelot, int port, u32 sset, u8 *data)
 750 {
 751         int i;
 752
 753         if (sset != ETH_SS_STATS)
 754                 return;
 755
 756         for (i = 0; i < ocelot->num_stats; i++)
 757                 memcpy(data + i * ETH_GSTRING_LEN, ocelot->stats_layout[i].name,
 758                        ETH_GSTRING_LEN);
 759 }
 760 EXPORT_SYMBOL(ocelot_get_strings);
 761
 762 static void ocelot_update_stats(struct ocelot *ocelot)
 763 {
 764         int i, j;
 765
 766         mutex_lock(&ocelot->stats_lock);
 767
 768         for (i = 0; i < ocelot->num_phys_ports; i++) {
 769                 /* Configure the port to read the stats from */
 770                 ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(i), SYS_STAT_CFG);
 771
 772                 for (j = 0; j < ocelot->num_stats; j++) {
 773                         u32 val;
 774                         unsigned int idx = i * ocelot->num_stats + j;
 775
 776                         val = ocelot_read_rix(ocelot, SYS_COUNT_RX_OCTETS,
 777                                               ocelot->stats_layout[j].offset);
 778
 779                         if (val < (ocelot->stats[idx] & U32_MAX))
 780                                 ocelot->stats[idx] += (u64)1 << 32;
 781
 782                         ocelot->stats[idx] = (ocelot->stats[idx] &
 783                                               ~(u64)U32_MAX) + val;
 784                 }
 785         }
 786
 787         mutex_unlock(&ocelot->stats_lock);
 788 }
 789
 790 static void ocelot_check_stats_work(struct work_struct *work)
 791 {
 792         struct delayed_work *del_work = to_delayed_work(work);
 793         struct ocelot *ocelot = container_of(del_work, struct ocelot,
 794                                              stats_work);
 795
 796         ocelot_update_stats(ocelot);
 797
 798         queue_delayed_work(ocelot->stats_queue, &ocelot->stats_work,
 799                            OCELOT_STATS_CHECK_DELAY);
 800 }
 801
 802 void ocelot_get_ethtool_stats(struct ocelot *ocelot, int port, u64 *data)
 803 {
 804         int i;
 805
 806         /* check and update now */
 807         ocelot_update_stats(ocelot);
 808
 809         /* Copy all counters */
 810         for (i = 0; i < ocelot->num_stats; i++)
 811                 *data++ = ocelot->stats[port * ocelot->num_stats + i];
 812 }
 813 EXPORT_SYMBOL(ocelot_get_ethtool_stats);
 814
 815 int ocelot_get_sset_count(struct ocelot *ocelot, int port, int sset)
 816 {
 817         if (sset != ETH_SS_STATS)
 818                 return -EOPNOTSUPP;
 819
 820         return ocelot->num_stats;
 821 }
 822 EXPORT_SYMBOL(ocelot_get_sset_count);
 823
 824 int ocelot_get_ts_info(struct ocelot *ocelot, int port,
 825                        struct ethtool_ts_info *info)
 826 {
 827         info->phc_index = ocelot->ptp_clock ?
 828                           ptp_clock_index(ocelot->ptp_clock) : -1;
 829         if (info->phc_index == -1) {
 830                 info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE |
 831                                          SOF_TIMESTAMPING_RX_SOFTWARE |
 832                                          SOF_TIMESTAMPING_SOFTWARE;
 833                 return 0;
 834         }
 835         info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE |
 836                                  SOF_TIMESTAMPING_RX_SOFTWARE |
 837                                  SOF_TIMESTAMPING_SOFTWARE |
 838                                  SOF_TIMESTAMPING_TX_HARDWARE |
 839                                  SOF_TIMESTAMPING_RX_HARDWARE |
 840                                  SOF_TIMESTAMPING_RAW_HARDWARE;
 841         info->tx_types = BIT(HWTSTAMP_TX_OFF) | BIT(HWTSTAMP_TX_ON) |
 842                          BIT(HWTSTAMP_TX_ONESTEP_SYNC);
 843         info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) | BIT(HWTSTAMP_FILTER_ALL);
 844
 845         return 0;
 846 }
 847 EXPORT_SYMBOL(ocelot_get_ts_info);
 848
 849 void ocelot_bridge_stp_state_set(struct ocelot *ocelot, int port, u8 state)
 850 {
 851         u32 port_cfg;
 852         int p, i;
 853
 854         if (!(BIT(port) & ocelot->bridge_mask))
 855                 return;
 856
 857         port_cfg = ocelot_read_gix(ocelot, ANA_PORT_PORT_CFG, port);
 858
 859         switch (state) {
 860         case BR_STATE_FORWARDING:
 861                 ocelot->bridge_fwd_mask |= BIT(port);
 862                 /* Fallthrough */
 863         case BR_STATE_LEARNING:
 864                 port_cfg |= ANA_PORT_PORT_CFG_LEARN_ENA;
 865                 break;
 866
 867         default:
 868                 port_cfg &= ~ANA_PORT_PORT_CFG_LEARN_ENA;
 869                 ocelot->bridge_fwd_mask &= ~BIT(port);
 870                 break;
 871         }
 872
 873         ocelot_write_gix(ocelot, port_cfg, ANA_PORT_PORT_CFG, port);
 874
 875         /* Apply FWD mask. The loop is needed to add/remove the current port as
 876          * a source for the other ports.
 877          */
 878         for (p = 0; p < ocelot->num_phys_ports; p++) {
 879                 if (ocelot->bridge_fwd_mask & BIT(p)) {
 880                         unsigned long mask = ocelot->bridge_fwd_mask & ~BIT(p);
 881
 882                         for (i = 0; i < ocelot->num_phys_ports; i++) {
 883                                 unsigned long bond_mask = ocelot->lags[i];
 884
 885                                 if (!bond_mask)
 886                                         continue;
 887
 888                                 if (bond_mask & BIT(p)) {
 889                                         mask &= ~bond_mask;
 890                                         break;
 891                                 }
 892                         }
 893
 894                         ocelot_write_rix(ocelot, mask,
 895                                          ANA_PGID_PGID, PGID_SRC + p);
 896                 } else {
 897                         ocelot_write_rix(ocelot, 0,
 898                                          ANA_PGID_PGID, PGID_SRC + p);
 899                 }
 900         }
 901 }
 902 EXPORT_SYMBOL(ocelot_bridge_stp_state_set);
 903
 904 void ocelot_set_ageing_time(struct ocelot *ocelot, unsigned int msecs)
 905 {
 906         unsigned int age_period = ANA_AUTOAGE_AGE_PERIOD(msecs / 2000);
 907
 908         /* Setting AGE_PERIOD to zero effectively disables automatic aging,
 909          * which is clearly not what our intention is. So avoid that.
 910          */
 911         if (!age_period)
 912                 age_period = 1;
 913
 914         ocelot_rmw(ocelot, age_period, ANA_AUTOAGE_AGE_PERIOD_M, ANA_AUTOAGE);
 915 }
 916 EXPORT_SYMBOL(ocelot_set_ageing_time);
 917
 918 static struct ocelot_multicast *ocelot_multicast_get(struct ocelot *ocelot,
 919                                                      const unsigned char *addr,
 920                                                      u16 vid)
 921 {
 922         struct ocelot_multicast *mc;
 923
 924         list_for_each_entry(mc, &ocelot->multicast, list) {
 925                 if (ether_addr_equal(mc->addr, addr) && mc->vid == vid)
 926                         return mc;
 927         }
 928
 929         return NULL;
 930 }
 931
 932 static enum macaccess_entry_type ocelot_classify_mdb(const unsigned char *addr)
 933 {
 934         if (addr[0] == 0x01 && addr[1] == 0x00 && addr[2] == 0x5e)
 935                 return ENTRYTYPE_MACv4;
 936         if (addr[0] == 0x33 && addr[1] == 0x33)
 937                 return ENTRYTYPE_MACv6;
 938         return ENTRYTYPE_NORMAL;
 939 }
 940
 941 static int ocelot_mdb_get_pgid(struct ocelot *ocelot,
 942                                enum macaccess_entry_type entry_type)
 943 {
 944         int pgid;
 945
 946         /* According to VSC7514 datasheet 3.9.1.5 IPv4 Multicast Entries and
 947          * 3.9.1.6 IPv6 Multicast Entries, "Instead of a lookup in the
 948          * destination mask table (PGID), the destination set is programmed as
 949          * part of the entry MAC address.", and the DEST_IDX is set to 0.
 950          */
 951         if (entry_type == ENTRYTYPE_MACv4 ||
 952             entry_type == ENTRYTYPE_MACv6)
 953                 return 0;
 954
 955         for_each_nonreserved_multicast_dest_pgid(ocelot, pgid) {
 956                 struct ocelot_multicast *mc;
 957                 bool used = false;
 958
 959                 list_for_each_entry(mc, &ocelot->multicast, list) {
 960                         if (mc->pgid == pgid) {
 961                                 used = true;
 962                                 break;
 963                         }
 964                 }
 965
 966                 if (!used)
 967                         return pgid;
 968         }
 969
 970         return -1;
 971 }
 972
 973 static void ocelot_encode_ports_to_mdb(unsigned char *addr,
 974                                        struct ocelot_multicast *mc,
 975                                        enum macaccess_entry_type entry_type)
 976 {
 977         memcpy(addr, mc->addr, ETH_ALEN);
 978
 979         if (entry_type == ENTRYTYPE_MACv4) {
 980                 addr[0] = 0;
 981                 addr[1] = mc->ports >> 8;
 982                 addr[2] = mc->ports & 0xff;
 983         } else if (entry_type == ENTRYTYPE_MACv6) {
 984                 addr[0] = mc->ports >> 8;
 985                 addr[1] = mc->ports & 0xff;
 986         }
 987 }
 988
 989 int ocelot_port_mdb_add(struct ocelot *ocelot, int port,
 990                         const struct switchdev_obj_port_mdb *mdb)
 991 {
 992         struct ocelot_port *ocelot_port = ocelot->ports[port];
 993         enum macaccess_entry_type entry_type;
 994         unsigned char addr[ETH_ALEN];
 995         struct ocelot_multicast *mc;
 996         u16 vid = mdb->vid;
 997         bool new = false;
 998
 999         if (port == ocelot->npi)
1000                 port = ocelot->num_phys_ports;
1001
1002         if (!vid)
1003                 vid = ocelot_port->pvid;
1004
1005         entry_type = ocelot_classify_mdb(mdb->addr);
1006
1007         mc = ocelot_multicast_get(ocelot, mdb->addr, vid);
1008         if (!mc) {
1009                 int pgid = ocelot_mdb_get_pgid(ocelot, entry_type);
1010
1011                 if (pgid < 0) {
1012                         dev_err(ocelot->dev,
1013                                 "No more PGIDs available for mdb %pM vid %d\n",
1014                                 mdb->addr, vid);
1015                         return -ENOSPC;
1016                 }
1017
1018                 mc = devm_kzalloc(ocelot->dev, sizeof(*mc), GFP_KERNEL);
1019                 if (!mc)
1020                         return -ENOMEM;
1021
1022                 memcpy(mc->addr, mdb->addr, ETH_ALEN);
1023                 mc->vid = vid;
1024                 mc->pgid = pgid;
1025
1026                 list_add_tail(&mc->list, &ocelot->multicast);
1027                 new = true;
1028         }
1029
1030         if (!new) {
1031                 ocelot_encode_ports_to_mdb(addr, mc, entry_type);
1032                 ocelot_mact_forget(ocelot, addr, vid);
1033         }
1034
1035         mc->ports |= BIT(port);
1036         ocelot_encode_ports_to_mdb(addr, mc, entry_type);
1037
1038         return ocelot_mact_learn(ocelot, mc->pgid, addr, vid, entry_type);
1039 }
1040 EXPORT_SYMBOL(ocelot_port_mdb_add);
1041
1042 int ocelot_port_mdb_del(struct ocelot *ocelot, int port,
1043                         const struct switchdev_obj_port_mdb *mdb)
1044 {
1045         struct ocelot_port *ocelot_port = ocelot->ports[port];
1046         enum macaccess_entry_type entry_type;
1047         unsigned char addr[ETH_ALEN];
1048         struct ocelot_multicast *mc;
1049         u16 vid = mdb->vid;
1050
1051         if (port == ocelot->npi)
1052                 port = ocelot->num_phys_ports;
1053
1054         if (!vid)
1055                 vid = ocelot_port->pvid;
1056
1057         mc = ocelot_multicast_get(ocelot, mdb->addr, vid);
1058         if (!mc)
1059                 return -ENOENT;
1060
1061         entry_type = ocelot_classify_mdb(mdb->addr);
1062
1063         ocelot_encode_ports_to_mdb(addr, mc, entry_type);
1064         ocelot_mact_forget(ocelot, addr, vid);
1065
1066         mc->ports &= ~BIT(port);
1067         if (!mc->ports) {
1068                 list_del(&mc->list);
1069                 devm_kfree(ocelot->dev, mc);
1070                 return 0;
1071         }
1072
1073         ocelot_encode_ports_to_mdb(addr, mc, entry_type);
1074
1075         return ocelot_mact_learn(ocelot, mc->pgid, addr, vid, entry_type);
1076 }
1077 EXPORT_SYMBOL(ocelot_port_mdb_del);
1078
1079 int ocelot_port_bridge_join(struct ocelot *ocelot, int port,
1080                             struct net_device *bridge)
1081 {
1082         if (!ocelot->bridge_mask) {
1083                 ocelot->hw_bridge_dev = bridge;
1084         } else {
1085                 if (ocelot->hw_bridge_dev != bridge)
1086                         /* This is adding the port to a second bridge, this is
1087                          * unsupported */
1088                         return -ENODEV;
1089         }
1090
1091         ocelot->bridge_mask |= BIT(port);
1092
1093         return 0;
1094 }
1095 EXPORT_SYMBOL(ocelot_port_bridge_join);
1096
1097 int ocelot_port_bridge_leave(struct ocelot *ocelot, int port,
1098                              struct net_device *bridge)
1099 {
1100         ocelot->bridge_mask &= ~BIT(port);
1101
1102         if (!ocelot->bridge_mask)
1103                 ocelot->hw_bridge_dev = NULL;
1104
1105         ocelot_port_vlan_filtering(ocelot, port, 0);
1106         ocelot_port_set_pvid(ocelot, port, 0);
1107         return ocelot_port_set_native_vlan(ocelot, port, 0);
1108 }
1109 EXPORT_SYMBOL(ocelot_port_bridge_leave);
1110
1111 static void ocelot_set_aggr_pgids(struct ocelot *ocelot)
1112 {
1113         int i, port, lag;
1114
1115         /* Reset destination and aggregation PGIDS */
1116         for_each_unicast_dest_pgid(ocelot, port)
1117                 ocelot_write_rix(ocelot, BIT(port), ANA_PGID_PGID, port);
1118
1119         for_each_aggr_pgid(ocelot, i)
1120                 ocelot_write_rix(ocelot, GENMASK(ocelot->num_phys_ports - 1, 0),
1121                                  ANA_PGID_PGID, i);
1122
1123         /* Now, set PGIDs for each LAG */
1124         for (lag = 0; lag < ocelot->num_phys_ports; lag++) {
1125                 unsigned long bond_mask;
1126                 int aggr_count = 0;
1127                 u8 aggr_idx[16];
1128
1129                 bond_mask = ocelot->lags[lag];
1130                 if (!bond_mask)
1131                         continue;
1132
1133                 for_each_set_bit(port, &bond_mask, ocelot->num_phys_ports) {
1134                         // Destination mask
1135                         ocelot_write_rix(ocelot, bond_mask,
1136                                          ANA_PGID_PGID, port);
1137                         aggr_idx[aggr_count] = port;
1138                         aggr_count++;
1139                 }
1140
1141                 for_each_aggr_pgid(ocelot, i) {
1142                         u32 ac;
1143
1144                         ac = ocelot_read_rix(ocelot, ANA_PGID_PGID, i);
1145                         ac &= ~bond_mask;
1146                         ac |= BIT(aggr_idx[i % aggr_count]);
1147                         ocelot_write_rix(ocelot, ac, ANA_PGID_PGID, i);
1148                 }
1149         }
1150 }
1151
1152 static void ocelot_setup_lag(struct ocelot *ocelot, int lag)
1153 {
1154         unsigned long bond_mask = ocelot->lags[lag];
1155         unsigned int p;
1156
1157         for_each_set_bit(p, &bond_mask, ocelot->num_phys_ports) {
1158                 u32 port_cfg = ocelot_read_gix(ocelot, ANA_PORT_PORT_CFG, p);
1159
1160                 port_cfg &= ~ANA_PORT_PORT_CFG_PORTID_VAL_M;
1161
1162                 /* Use lag port as logical port for port i */
1163                 ocelot_write_gix(ocelot, port_cfg |
1164                                  ANA_PORT_PORT_CFG_PORTID_VAL(lag),
1165                                  ANA_PORT_PORT_CFG, p);
1166         }
1167 }
1168
1169 int ocelot_port_lag_join(struct ocelot *ocelot, int port,
1170                          struct net_device *bond)
1171 {
1172         struct net_device *ndev;
1173         u32 bond_mask = 0;
1174         int lag, lp;
1175
1176         rcu_read_lock();
1177         for_each_netdev_in_bond_rcu(bond, ndev) {
1178                 struct ocelot_port_private *priv = netdev_priv(ndev);
1179
1180                 bond_mask |= BIT(priv->chip_port);
1181         }
1182         rcu_read_unlock();
1183
1184         lp = __ffs(bond_mask);
1185
1186         /* If the new port is the lowest one, use it as the logical port from
1187          * now on
1188          */
1189         if (port == lp) {
1190                 lag = port;
1191                 ocelot->lags[port] = bond_mask;
1192                 bond_mask &= ~BIT(port);
1193                 if (bond_mask) {
1194                         lp = __ffs(bond_mask);
1195                         ocelot->lags[lp] = 0;
1196                 }
1197         } else {
1198                 lag = lp;
1199                 ocelot->lags[lp] |= BIT(port);
1200         }
1201
1202         ocelot_setup_lag(ocelot, lag);
1203         ocelot_set_aggr_pgids(ocelot);
1204
1205         return 0;
1206 }
1207 EXPORT_SYMBOL(ocelot_port_lag_join);
1208
1209 void ocelot_port_lag_leave(struct ocelot *ocelot, int port,
1210                            struct net_device *bond)
1211 {
1212         u32 port_cfg;
1213         int i;
1214
1215         /* Remove port from any lag */
1216         for (i = 0; i < ocelot->num_phys_ports; i++)
1217                 ocelot->lags[i] &= ~BIT(port);
1218
1219         /* if it was the logical port of the lag, move the lag config to the
1220          * next port
1221          */
1222         if (ocelot->lags[port]) {
1223                 int n = __ffs(ocelot->lags[port]);
1224
1225                 ocelot->lags[n] = ocelot->lags[port];
1226                 ocelot->lags[port] = 0;
1227
1228                 ocelot_setup_lag(ocelot, n);
1229         }
1230
1231         port_cfg = ocelot_read_gix(ocelot, ANA_PORT_PORT_CFG, port);
1232         port_cfg &= ~ANA_PORT_PORT_CFG_PORTID_VAL_M;
1233         ocelot_write_gix(ocelot, port_cfg | ANA_PORT_PORT_CFG_PORTID_VAL(port),
1234                          ANA_PORT_PORT_CFG, port);
1235
1236         ocelot_set_aggr_pgids(ocelot);
1237 }
1238 EXPORT_SYMBOL(ocelot_port_lag_leave);
1239
1240 /* Configure the maximum SDU (L2 payload) on RX to the value specified in @sdu.
1241  * The length of VLAN tags is accounted for automatically via DEV_MAC_TAGS_CFG.
1242  * In the special case that it's the NPI port that we're configuring, the
1243  * length of the tag and optional prefix needs to be accounted for privately,
1244  * in order to be able to sustain communication at the requested @sdu.
1245  */
1246 void ocelot_port_set_maxlen(struct ocelot *ocelot, int port, size_t sdu)
1247 {
1248         struct ocelot_port *ocelot_port = ocelot->ports[port];
1249         int maxlen = sdu + ETH_HLEN + ETH_FCS_LEN;
1250         int pause_start, pause_stop;
1251         int atop_wm;
1252
1253         if (port == ocelot->npi) {
1254                 maxlen += OCELOT_TAG_LEN;
1255
1256                 if (ocelot->inj_prefix == OCELOT_TAG_PREFIX_SHORT)
1257                         maxlen += OCELOT_SHORT_PREFIX_LEN;
1258                 else if (ocelot->inj_prefix == OCELOT_TAG_PREFIX_LONG)
1259                         maxlen += OCELOT_LONG_PREFIX_LEN;
1260         }
1261
1262         ocelot_port_writel(ocelot_port, maxlen, DEV_MAC_MAXLEN_CFG);
1263
1264         /* Set Pause watermark hysteresis */
1265         pause_start = 6 * maxlen / OCELOT_BUFFER_CELL_SZ;
1266         pause_stop = 4 * maxlen / OCELOT_BUFFER_CELL_SZ;
1267         ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_START,
1268                             pause_start);
1269         ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_STOP,
1270                             pause_stop);
1271
1272         /* Tail dropping watermark */
1273         atop_wm = (ocelot->shared_queue_sz - 9 * maxlen) /
1274                    OCELOT_BUFFER_CELL_SZ;
1275         ocelot_write_rix(ocelot, ocelot->ops->wm_enc(9 * maxlen),
1276                          SYS_ATOP, port);
1277         ocelot_write(ocelot, ocelot->ops->wm_enc(atop_wm), SYS_ATOP_TOT_CFG);
1278 }
1279 EXPORT_SYMBOL(ocelot_port_set_maxlen);
1280
1281 int ocelot_get_max_mtu(struct ocelot *ocelot, int port)
1282 {
1283         int max_mtu = 65535 - ETH_HLEN - ETH_FCS_LEN;
1284
1285         if (port == ocelot->npi) {
1286                 max_mtu -= OCELOT_TAG_LEN;
1287
1288                 if (ocelot->inj_prefix == OCELOT_TAG_PREFIX_SHORT)
1289                         max_mtu -= OCELOT_SHORT_PREFIX_LEN;
1290                 else if (ocelot->inj_prefix == OCELOT_TAG_PREFIX_LONG)
1291                         max_mtu -= OCELOT_LONG_PREFIX_LEN;
1292         }
1293
1294         return max_mtu;
1295 }
1296 EXPORT_SYMBOL(ocelot_get_max_mtu);
1297
1298 void ocelot_init_port(struct ocelot *ocelot, int port)
1299 {
1300         struct ocelot_port *ocelot_port = ocelot->ports[port];
1301
1302         skb_queue_head_init(&ocelot_port->tx_skbs);
1303
1304         /* Basic L2 initialization */
1305
1306         /* Set MAC IFG Gaps
1307          * FDX: TX_IFG = 5, RX_IFG1 = RX_IFG2 = 0
1308          * !FDX: TX_IFG = 5, RX_IFG1 = RX_IFG2 = 5
1309          */
1310         ocelot_port_writel(ocelot_port, DEV_MAC_IFG_CFG_TX_IFG(5),
1311                            DEV_MAC_IFG_CFG);
1312
1313         /* Load seed (0) and set MAC HDX late collision  */
1314         ocelot_port_writel(ocelot_port, DEV_MAC_HDX_CFG_LATE_COL_POS(67) |
1315                            DEV_MAC_HDX_CFG_SEED_LOAD,
1316                            DEV_MAC_HDX_CFG);
1317         mdelay(1);
1318         ocelot_port_writel(ocelot_port, DEV_MAC_HDX_CFG_LATE_COL_POS(67),
1319                            DEV_MAC_HDX_CFG);
1320
1321         /* Set Max Length and maximum tags allowed */
1322         ocelot_port_set_maxlen(ocelot, port, ETH_DATA_LEN);
1323         ocelot_port_writel(ocelot_port, DEV_MAC_TAGS_CFG_TAG_ID(ETH_P_8021AD) |
1324                            DEV_MAC_TAGS_CFG_VLAN_AWR_ENA |
1325                            DEV_MAC_TAGS_CFG_VLAN_DBL_AWR_ENA |
1326                            DEV_MAC_TAGS_CFG_VLAN_LEN_AWR_ENA,
1327                            DEV_MAC_TAGS_CFG);
1328
1329         /* Set SMAC of Pause frame (00:00:00:00:00:00) */
1330         ocelot_port_writel(ocelot_port, 0, DEV_MAC_FC_MAC_HIGH_CFG);
1331         ocelot_port_writel(ocelot_port, 0, DEV_MAC_FC_MAC_LOW_CFG);
1332
1333         /* Enable transmission of pause frames */
1334         ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_ENA, 1);
1335
1336         /* Drop frames with multicast source address */
1337         ocelot_rmw_gix(ocelot, ANA_PORT_DROP_CFG_DROP_MC_SMAC_ENA,
1338                        ANA_PORT_DROP_CFG_DROP_MC_SMAC_ENA,
1339                        ANA_PORT_DROP_CFG, port);
1340
1341         /* Set default VLAN and tag type to 8021Q. */
1342         ocelot_rmw_gix(ocelot, REW_PORT_VLAN_CFG_PORT_TPID(ETH_P_8021Q),
1343                        REW_PORT_VLAN_CFG_PORT_TPID_M,
1344                        REW_PORT_VLAN_CFG, port);
1345
1346         /* Enable vcap lookups */
1347         ocelot_vcap_enable(ocelot, port);
1348 }
1349 EXPORT_SYMBOL(ocelot_init_port);
1350
1351 /* Configure and enable the CPU port module, which is a set of queues.
1352  * If @npi contains a valid port index, the CPU port module is connected
1353  * to the Node Processor Interface (NPI). This is the mode through which
1354  * frames can be injected from and extracted to an external CPU,
1355  * over Ethernet.
1356  */
1357 void ocelot_configure_cpu(struct ocelot *ocelot, int npi,
1358                           enum ocelot_tag_prefix injection,
1359                           enum ocelot_tag_prefix extraction)
1360 {
1361         int cpu = ocelot->num_phys_ports;
1362
1363         ocelot->npi = npi;
1364         ocelot->inj_prefix = injection;
1365         ocelot->xtr_prefix = extraction;
1366
1367         /* The unicast destination PGID for the CPU port module is unused */
1368         ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, cpu);
1369         /* Instead set up a multicast destination PGID for traffic copied to
1370          * the CPU. Whitelisted MAC addresses like the port netdevice MAC
1371          * addresses will be copied to the CPU via this PGID.
1372          */
1373         ocelot_write_rix(ocelot, BIT(cpu), ANA_PGID_PGID, PGID_CPU);
1374         ocelot_write_gix(ocelot, ANA_PORT_PORT_CFG_RECV_ENA |
1375                          ANA_PORT_PORT_CFG_PORTID_VAL(cpu),
1376                          ANA_PORT_PORT_CFG, cpu);
1377
1378         if (npi >= 0 && npi < ocelot->num_phys_ports) {
1379                 ocelot_write(ocelot, QSYS_EXT_CPU_CFG_EXT_CPUQ_MSK_M |
1380                              QSYS_EXT_CPU_CFG_EXT_CPU_PORT(npi),
1381                              QSYS_EXT_CPU_CFG);
1382
1383                 /* Enable NPI port */
1384                 ocelot_fields_write(ocelot, npi,
1385                                     QSYS_SWITCH_PORT_MODE_PORT_ENA, 1);
1386                 /* NPI port Injection/Extraction configuration */
1387                 ocelot_fields_write(ocelot, npi, SYS_PORT_MODE_INCL_XTR_HDR,
1388                                     extraction);
1389                 ocelot_fields_write(ocelot, npi, SYS_PORT_MODE_INCL_INJ_HDR,
1390                                     injection);
1391
1392                 /* Disable transmission of pause frames */
1393                 ocelot_fields_write(ocelot, npi, SYS_PAUSE_CFG_PAUSE_ENA, 0);
1394         }
1395
1396         /* Enable CPU port module */
1397         ocelot_fields_write(ocelot, cpu, QSYS_SWITCH_PORT_MODE_PORT_ENA, 1);
1398         /* CPU port Injection/Extraction configuration */
1399         ocelot_fields_write(ocelot, cpu, SYS_PORT_MODE_INCL_XTR_HDR,
1400                             extraction);
1401         ocelot_fields_write(ocelot, cpu, SYS_PORT_MODE_INCL_INJ_HDR,
1402                             injection);
1403
1404         /* Configure the CPU port to be VLAN aware */
1405         ocelot_write_gix(ocelot, ANA_PORT_VLAN_CFG_VLAN_VID(0) |
1406                                  ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
1407                                  ANA_PORT_VLAN_CFG_VLAN_POP_CNT(1),
1408                          ANA_PORT_VLAN_CFG, cpu);
1409 }
1410 EXPORT_SYMBOL(ocelot_configure_cpu);
1411
1412 int ocelot_init(struct ocelot *ocelot)
1413 {
1414         char queue_name[32];
1415         int i, ret;
1416         u32 port;
1417
1418         if (ocelot->ops->reset) {
1419                 ret = ocelot->ops->reset(ocelot);
1420                 if (ret) {
1421                         dev_err(ocelot->dev, "Switch reset failed\n");
1422                         return ret;
1423                 }
1424         }
1425
1426         ocelot->lags = devm_kcalloc(ocelot->dev, ocelot->num_phys_ports,
1427                                     sizeof(u32), GFP_KERNEL);
1428         if (!ocelot->lags)
1429                 return -ENOMEM;
1430
1431         ocelot->stats = devm_kcalloc(ocelot->dev,
1432                                      ocelot->num_phys_ports * ocelot->num_stats,
1433                                      sizeof(u64), GFP_KERNEL);
1434         if (!ocelot->stats)
1435                 return -ENOMEM;
1436
1437         mutex_init(&ocelot->stats_lock);
1438         mutex_init(&ocelot->ptp_lock);
1439         spin_lock_init(&ocelot->ptp_clock_lock);
1440         snprintf(queue_name, sizeof(queue_name), "%s-stats",
1441                  dev_name(ocelot->dev));
1442         ocelot->stats_queue = create_singlethread_workqueue(queue_name);
1443         if (!ocelot->stats_queue)
1444                 return -ENOMEM;
1445
1446         INIT_LIST_HEAD(&ocelot->multicast);
1447         ocelot_mact_init(ocelot);
1448         ocelot_vlan_init(ocelot);
1449         ocelot_vcap_init(ocelot);
1450
1451         for (port = 0; port < ocelot->num_phys_ports; port++) {
1452                 /* Clear all counters (5 groups) */
1453                 ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(port) |
1454                                      SYS_STAT_CFG_STAT_CLEAR_SHOT(0x7f),
1455                              SYS_STAT_CFG);
1456         }
1457
1458         /* Only use S-Tag */
1459         ocelot_write(ocelot, ETH_P_8021AD, SYS_VLAN_ETYPE_CFG);
1460
1461         /* Aggregation mode */
1462         ocelot_write(ocelot, ANA_AGGR_CFG_AC_SMAC_ENA |
1463                              ANA_AGGR_CFG_AC_DMAC_ENA |
1464                              ANA_AGGR_CFG_AC_IP4_SIPDIP_ENA |
1465                              ANA_AGGR_CFG_AC_IP4_TCPUDP_ENA, ANA_AGGR_CFG);
1466
1467         /* Set MAC age time to default value. The entry is aged after
1468          * 2*AGE_PERIOD
1469          */
1470         ocelot_write(ocelot,
1471                      ANA_AUTOAGE_AGE_PERIOD(BR_DEFAULT_AGEING_TIME / 2 / HZ),
1472                      ANA_AUTOAGE);
1473
1474         /* Disable learning for frames discarded by VLAN ingress filtering */
1475         regmap_field_write(ocelot->regfields[ANA_ADVLEARN_VLAN_CHK], 1);
1476
1477         /* Setup frame ageing - fixed value "2 sec" - in 6.5 us units */
1478         ocelot_write(ocelot, SYS_FRM_AGING_AGE_TX_ENA |
1479                      SYS_FRM_AGING_MAX_AGE(307692), SYS_FRM_AGING);
1480
1481         /* Setup flooding PGIDs */
1482         ocelot_write_rix(ocelot, ANA_FLOODING_FLD_MULTICAST(PGID_MC) |
1483                          ANA_FLOODING_FLD_BROADCAST(PGID_MC) |
1484                          ANA_FLOODING_FLD_UNICAST(PGID_UC),
1485                          ANA_FLOODING, 0);
1486         ocelot_write(ocelot, ANA_FLOODING_IPMC_FLD_MC6_DATA(PGID_MCIPV6) |
1487                      ANA_FLOODING_IPMC_FLD_MC6_CTRL(PGID_MC) |
1488                      ANA_FLOODING_IPMC_FLD_MC4_DATA(PGID_MCIPV4) |
1489                      ANA_FLOODING_IPMC_FLD_MC4_CTRL(PGID_MC),
1490                      ANA_FLOODING_IPMC);
1491
1492         for (port = 0; port < ocelot->num_phys_ports; port++) {
1493                 /* Transmit the frame to the local port. */
1494                 ocelot_write_rix(ocelot, BIT(port), ANA_PGID_PGID, port);
1495                 /* Do not forward BPDU frames to the front ports. */
1496                 ocelot_write_gix(ocelot,
1497                                  ANA_PORT_CPU_FWD_BPDU_CFG_BPDU_REDIR_ENA(0xffff),
1498                                  ANA_PORT_CPU_FWD_BPDU_CFG,
1499                                  port);
1500                 /* Ensure bridging is disabled */
1501                 ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_SRC + port);
1502         }
1503
1504         /* Allow broadcast MAC frames. */
1505         for_each_nonreserved_multicast_dest_pgid(ocelot, i) {
1506                 u32 val = ANA_PGID_PGID_PGID(GENMASK(ocelot->num_phys_ports - 1, 0));
1507
1508                 ocelot_write_rix(ocelot, val, ANA_PGID_PGID, i);
1509         }
1510         ocelot_write_rix(ocelot,
1511                          ANA_PGID_PGID_PGID(GENMASK(ocelot->num_phys_ports, 0)),
1512                          ANA_PGID_PGID, PGID_MC);
1513         ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_MCIPV4);
1514         ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_MCIPV6);
1515
1516         /* Allow manual injection via DEVCPU_QS registers, and byte swap these
1517          * registers endianness.
1518          */
1519         ocelot_write_rix(ocelot, QS_INJ_GRP_CFG_BYTE_SWAP |
1520                          QS_INJ_GRP_CFG_MODE(1), QS_INJ_GRP_CFG, 0);
1521         ocelot_write_rix(ocelot, QS_XTR_GRP_CFG_BYTE_SWAP |
1522                          QS_XTR_GRP_CFG_MODE(1), QS_XTR_GRP_CFG, 0);
1523         ocelot_write(ocelot, ANA_CPUQ_CFG_CPUQ_MIRROR(2) |
1524                      ANA_CPUQ_CFG_CPUQ_LRN(2) |
1525                      ANA_CPUQ_CFG_CPUQ_MAC_COPY(2) |
1526                      ANA_CPUQ_CFG_CPUQ_SRC_COPY(2) |
1527                      ANA_CPUQ_CFG_CPUQ_LOCKED_PORTMOVE(2) |
1528                      ANA_CPUQ_CFG_CPUQ_ALLBRIDGE(6) |
1529                      ANA_CPUQ_CFG_CPUQ_IPMC_CTRL(6) |
1530                      ANA_CPUQ_CFG_CPUQ_IGMP(6) |
1531                      ANA_CPUQ_CFG_CPUQ_MLD(6), ANA_CPUQ_CFG);
1532         for (i = 0; i < 16; i++)
1533                 ocelot_write_rix(ocelot, ANA_CPUQ_8021_CFG_CPUQ_GARP_VAL(6) |
1534                                  ANA_CPUQ_8021_CFG_CPUQ_BPDU_VAL(6),
1535                                  ANA_CPUQ_8021_CFG, i);
1536
1537         INIT_DELAYED_WORK(&ocelot->stats_work, ocelot_check_stats_work);
1538         queue_delayed_work(ocelot->stats_queue, &ocelot->stats_work,
1539                            OCELOT_STATS_CHECK_DELAY);
1540
1541         return 0;
1542 }
1543 EXPORT_SYMBOL(ocelot_init);
1544
1545 void ocelot_deinit(struct ocelot *ocelot)
1546 {
1547         struct ocelot_port *port;
1548         int i;
1549
1550         cancel_delayed_work(&ocelot->stats_work);
1551         destroy_workqueue(ocelot->stats_queue);
1552         mutex_destroy(&ocelot->stats_lock);
1553
1554         for (i = 0; i < ocelot->num_phys_ports; i++) {
1555                 port = ocelot->ports[i];
1556                 skb_queue_purge(&port->tx_skbs);
1557         }
1558 }
1559 EXPORT_SYMBOL(ocelot_deinit);
1560
1561 MODULE_LICENSE("Dual MIT/GPL");