drivers/net/ethernet/mscc/ocelot_net.c

   1 // SPDX-License-Identifier: (GPL-2.0 OR MIT)
   2 /* Microsemi Ocelot Switch driver
   3  *
   4  * Copyright (c) 2017, 2019 Microsemi Corporation
   5  */
   6
   7 #include <linux/if_bridge.h>
   8 #include "ocelot.h"
   9 #include "ocelot_vcap.h"
  10
  11 int ocelot_setup_tc_cls_flower(struct ocelot_port_private *priv,
  12                                struct flow_cls_offload *f,
  13                                bool ingress)
  14 {
  15         struct ocelot *ocelot = priv->port.ocelot;
  16         int port = priv->chip_port;
  17
  18         if (!ingress)
  19                 return -EOPNOTSUPP;
  20
  21         switch (f->command) {
  22         case FLOW_CLS_REPLACE:
  23                 return ocelot_cls_flower_replace(ocelot, port, f, ingress);
  24         case FLOW_CLS_DESTROY:
  25                 return ocelot_cls_flower_destroy(ocelot, port, f, ingress);
  26         case FLOW_CLS_STATS:
  27                 return ocelot_cls_flower_stats(ocelot, port, f, ingress);
  28         default:
  29                 return -EOPNOTSUPP;
  30         }
  31 }
  32
  33 static int ocelot_setup_tc_cls_matchall(struct ocelot_port_private *priv,
  34                                         struct tc_cls_matchall_offload *f,
  35                                         bool ingress)
  36 {
  37         struct netlink_ext_ack *extack = f->common.extack;
  38         struct ocelot *ocelot = priv->port.ocelot;
  39         struct ocelot_policer pol = { 0 };
  40         struct flow_action_entry *action;
  41         int port = priv->chip_port;
  42         int err;
  43
  44         if (!ingress) {
  45                 NL_SET_ERR_MSG_MOD(extack, "Only ingress is supported");
  46                 return -EOPNOTSUPP;
  47         }
  48
  49         switch (f->command) {
  50         case TC_CLSMATCHALL_REPLACE:
  51                 if (!flow_offload_has_one_action(&f->rule->action)) {
  52                         NL_SET_ERR_MSG_MOD(extack,
  53                                            "Only one action is supported");
  54                         return -EOPNOTSUPP;
  55                 }
  56
  57                 if (priv->tc.block_shared) {
  58                         NL_SET_ERR_MSG_MOD(extack,
  59                                            "Rate limit is not supported on shared blocks");
  60                         return -EOPNOTSUPP;
  61                 }
  62
  63                 action = &f->rule->action.entries[0];
  64
  65                 if (action->id != FLOW_ACTION_POLICE) {
  66                         NL_SET_ERR_MSG_MOD(extack, "Unsupported action");
  67                         return -EOPNOTSUPP;
  68                 }
  69
  70                 if (priv->tc.police_id && priv->tc.police_id != f->cookie) {
  71                         NL_SET_ERR_MSG_MOD(extack,
  72                                            "Only one policer per port is supported");
  73                         return -EEXIST;
  74                 }
  75
  76                 pol.rate = (u32)div_u64(action->police.rate_bytes_ps, 1000) * 8;
  77                 pol.burst = action->police.burst;
  78
  79                 err = ocelot_port_policer_add(ocelot, port, &pol);
  80                 if (err) {
  81                         NL_SET_ERR_MSG_MOD(extack, "Could not add policer");
  82                         return err;
  83                 }
  84
  85                 priv->tc.police_id = f->cookie;
  86                 priv->tc.offload_cnt++;
  87                 return 0;
  88         case TC_CLSMATCHALL_DESTROY:
  89                 if (priv->tc.police_id != f->cookie)
  90                         return -ENOENT;
  91
  92                 err = ocelot_port_policer_del(ocelot, port);
  93                 if (err) {
  94                         NL_SET_ERR_MSG_MOD(extack,
  95                                            "Could not delete policer");
  96                         return err;
  97                 }
  98                 priv->tc.police_id = 0;
  99                 priv->tc.offload_cnt--;
 100                 return 0;
 101         case TC_CLSMATCHALL_STATS:
 102         default:
 103                 return -EOPNOTSUPP;
 104         }
 105 }
 106
 107 static int ocelot_setup_tc_block_cb(enum tc_setup_type type,
 108                                     void *type_data,
 109                                     void *cb_priv, bool ingress)
 110 {
 111         struct ocelot_port_private *priv = cb_priv;
 112
 113         if (!tc_cls_can_offload_and_chain0(priv->dev, type_data))
 114                 return -EOPNOTSUPP;
 115
 116         switch (type) {
 117         case TC_SETUP_CLSMATCHALL:
 118                 return ocelot_setup_tc_cls_matchall(priv, type_data, ingress);
 119         case TC_SETUP_CLSFLOWER:
 120                 return ocelot_setup_tc_cls_flower(priv, type_data, ingress);
 121         default:
 122                 return -EOPNOTSUPP;
 123         }
 124 }
 125
 126 static int ocelot_setup_tc_block_cb_ig(enum tc_setup_type type,
 127                                        void *type_data,
 128                                        void *cb_priv)
 129 {
 130         return ocelot_setup_tc_block_cb(type, type_data,
 131                                         cb_priv, true);
 132 }
 133
 134 static int ocelot_setup_tc_block_cb_eg(enum tc_setup_type type,
 135                                        void *type_data,
 136                                        void *cb_priv)
 137 {
 138         return ocelot_setup_tc_block_cb(type, type_data,
 139                                         cb_priv, false);
 140 }
 141
 142 static LIST_HEAD(ocelot_block_cb_list);
 143
 144 static int ocelot_setup_tc_block(struct ocelot_port_private *priv,
 145                                  struct flow_block_offload *f)
 146 {
 147         struct flow_block_cb *block_cb;
 148         flow_setup_cb_t *cb;
 149
 150         if (f->binder_type == FLOW_BLOCK_BINDER_TYPE_CLSACT_INGRESS) {
 151                 cb = ocelot_setup_tc_block_cb_ig;
 152                 priv->tc.block_shared = f->block_shared;
 153         } else if (f->binder_type == FLOW_BLOCK_BINDER_TYPE_CLSACT_EGRESS) {
 154                 cb = ocelot_setup_tc_block_cb_eg;
 155         } else {
 156                 return -EOPNOTSUPP;
 157         }
 158
 159         f->driver_block_list = &ocelot_block_cb_list;
 160
 161         switch (f->command) {
 162         case FLOW_BLOCK_BIND:
 163                 if (flow_block_cb_is_busy(cb, priv, &ocelot_block_cb_list))
 164                         return -EBUSY;
 165
 166                 block_cb = flow_block_cb_alloc(cb, priv, priv, NULL);
 167                 if (IS_ERR(block_cb))
 168                         return PTR_ERR(block_cb);
 169
 170                 flow_block_cb_add(block_cb, f);
 171                 list_add_tail(&block_cb->driver_list, f->driver_block_list);
 172                 return 0;
 173         case FLOW_BLOCK_UNBIND:
 174                 block_cb = flow_block_cb_lookup(f->block, cb, priv);
 175                 if (!block_cb)
 176                         return -ENOENT;
 177
 178                 flow_block_cb_remove(block_cb, f);
 179                 list_del(&block_cb->driver_list);
 180                 return 0;
 181         default:
 182                 return -EOPNOTSUPP;
 183         }
 184 }
 185
 186 static int ocelot_setup_tc(struct net_device *dev, enum tc_setup_type type,
 187                            void *type_data)
 188 {
 189         struct ocelot_port_private *priv = netdev_priv(dev);
 190
 191         switch (type) {
 192         case TC_SETUP_BLOCK:
 193                 return ocelot_setup_tc_block(priv, type_data);
 194         default:
 195                 return -EOPNOTSUPP;
 196         }
 197         return 0;
 198 }
 199
 200 static void ocelot_port_adjust_link(struct net_device *dev)
 201 {
 202         struct ocelot_port_private *priv = netdev_priv(dev);
 203         struct ocelot *ocelot = priv->port.ocelot;
 204         int port = priv->chip_port;
 205
 206         ocelot_adjust_link(ocelot, port, dev->phydev);
 207 }
 208
 209 static int ocelot_vlan_vid_prepare(struct net_device *dev, u16 vid, bool pvid,
 210                                    bool untagged)
 211 {
 212         struct ocelot_port_private *priv = netdev_priv(dev);
 213         struct ocelot_port *ocelot_port = &priv->port;
 214         struct ocelot *ocelot = ocelot_port->ocelot;
 215         int port = priv->chip_port;
 216
 217         return ocelot_vlan_prepare(ocelot, port, vid, pvid, untagged);
 218 }
 219
 220 static int ocelot_vlan_vid_add(struct net_device *dev, u16 vid, bool pvid,
 221                                bool untagged)
 222 {
 223         struct ocelot_port_private *priv = netdev_priv(dev);
 224         struct ocelot_port *ocelot_port = &priv->port;
 225         struct ocelot *ocelot = ocelot_port->ocelot;
 226         int port = priv->chip_port;
 227         int ret;
 228
 229         ret = ocelot_vlan_add(ocelot, port, vid, pvid, untagged);
 230         if (ret)
 231                 return ret;
 232
 233         /* Add the port MAC address to with the right VLAN information */
 234         ocelot_mact_learn(ocelot, PGID_CPU, dev->dev_addr, vid,
 235                           ENTRYTYPE_LOCKED);
 236
 237         return 0;
 238 }
 239
 240 static int ocelot_vlan_vid_del(struct net_device *dev, u16 vid)
 241 {
 242         struct ocelot_port_private *priv = netdev_priv(dev);
 243         struct ocelot *ocelot = priv->port.ocelot;
 244         int port = priv->chip_port;
 245         int ret;
 246
 247         /* 8021q removes VID 0 on module unload for all interfaces
 248          * with VLAN filtering feature. We need to keep it to receive
 249          * untagged traffic.
 250          */
 251         if (vid == 0)
 252                 return 0;
 253
 254         ret = ocelot_vlan_del(ocelot, port, vid);
 255         if (ret)
 256                 return ret;
 257
 258         /* Del the port MAC address to with the right VLAN information */
 259         ocelot_mact_forget(ocelot, dev->dev_addr, vid);
 260
 261         return 0;
 262 }
 263
 264 static int ocelot_port_open(struct net_device *dev)
 265 {
 266         struct ocelot_port_private *priv = netdev_priv(dev);
 267         struct ocelot_port *ocelot_port = &priv->port;
 268         struct ocelot *ocelot = ocelot_port->ocelot;
 269         int port = priv->chip_port;
 270         int err;
 271
 272         if (priv->serdes) {
 273                 err = phy_set_mode_ext(priv->serdes, PHY_MODE_ETHERNET,
 274                                        ocelot_port->phy_mode);
 275                 if (err) {
 276                         netdev_err(dev, "Could not set mode of SerDes\n");
 277                         return err;
 278                 }
 279         }
 280
 281         err = phy_connect_direct(dev, priv->phy, &ocelot_port_adjust_link,
 282                                  ocelot_port->phy_mode);
 283         if (err) {
 284                 netdev_err(dev, "Could not attach to PHY\n");
 285                 return err;
 286         }
 287
 288         dev->phydev = priv->phy;
 289
 290         phy_attached_info(priv->phy);
 291         phy_start(priv->phy);
 292
 293         ocelot_port_enable(ocelot, port, priv->phy);
 294
 295         return 0;
 296 }
 297
 298 static int ocelot_port_stop(struct net_device *dev)
 299 {
 300         struct ocelot_port_private *priv = netdev_priv(dev);
 301         struct ocelot *ocelot = priv->port.ocelot;
 302         int port = priv->chip_port;
 303
 304         phy_disconnect(priv->phy);
 305
 306         dev->phydev = NULL;
 307
 308         ocelot_port_disable(ocelot, port);
 309
 310         return 0;
 311 }
 312
 313 /* Generate the IFH for frame injection
 314  *
 315  * The IFH is a 128bit-value
 316  * bit 127: bypass the analyzer processing
 317  * bit 56-67: destination mask
 318  * bit 28-29: pop_cnt: 3 disables all rewriting of the frame
 319  * bit 20-27: cpu extraction queue mask
 320  * bit 16: tag type 0: C-tag, 1: S-tag
 321  * bit 0-11: VID
 322  */
 323 static int ocelot_gen_ifh(u32 *ifh, struct frame_info *info)
 324 {
 325         ifh[0] = IFH_INJ_BYPASS | ((0x1ff & info->rew_op) << 21);
 326         ifh[1] = (0xf00 & info->port) >> 8;
 327         ifh[2] = (0xff & info->port) << 24;
 328         ifh[3] = (info->tag_type << 16) | info->vid;
 329
 330         return 0;
 331 }
 332
 333 static int ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
 334 {
 335         struct ocelot_port_private *priv = netdev_priv(dev);
 336         struct skb_shared_info *shinfo = skb_shinfo(skb);
 337         struct ocelot_port *ocelot_port = &priv->port;
 338         struct ocelot *ocelot = ocelot_port->ocelot;
 339         u32 val, ifh[OCELOT_TAG_LEN / 4];
 340         struct frame_info info = {};
 341         u8 grp = 0; /* Send everything on CPU group 0 */
 342         unsigned int i, count, last;
 343         int port = priv->chip_port;
 344
 345         val = ocelot_read(ocelot, QS_INJ_STATUS);
 346         if (!(val & QS_INJ_STATUS_FIFO_RDY(BIT(grp))) ||
 347             (val & QS_INJ_STATUS_WMARK_REACHED(BIT(grp))))
 348                 return NETDEV_TX_BUSY;
 349
 350         ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
 351                          QS_INJ_CTRL_SOF, QS_INJ_CTRL, grp);
 352
 353         info.port = BIT(port);
 354         info.tag_type = IFH_TAG_TYPE_C;
 355         info.vid = skb_vlan_tag_get(skb);
 356
 357         /* Check if timestamping is needed */
 358         if (ocelot->ptp && (shinfo->tx_flags & SKBTX_HW_TSTAMP)) {
 359                 info.rew_op = ocelot_port->ptp_cmd;
 360
 361                 if (ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
 362                         struct sk_buff *clone;
 363
 364                         clone = skb_clone_sk(skb);
 365                         if (!clone) {
 366                                 kfree_skb(skb);
 367                                 return NETDEV_TX_OK;
 368                         }
 369
 370                         ocelot_port_add_txtstamp_skb(ocelot, port, clone);
 371
 372                         info.rew_op |= clone->cb[0] << 3;
 373                 }
 374         }
 375
 376         if (ocelot->ptp && shinfo->tx_flags & SKBTX_HW_TSTAMP) {
 377                 info.rew_op = ocelot_port->ptp_cmd;
 378                 if (ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP)
 379                         info.rew_op |= skb->cb[0] << 3;
 380         }
 381
 382         ocelot_gen_ifh(ifh, &info);
 383
 384         for (i = 0; i < OCELOT_TAG_LEN / 4; i++)
 385                 ocelot_write_rix(ocelot, (__force u32)cpu_to_be32(ifh[i]),
 386                                  QS_INJ_WR, grp);
 387
 388         count = (skb->len + 3) / 4;
 389         last = skb->len % 4;
 390         for (i = 0; i < count; i++)
 391                 ocelot_write_rix(ocelot, ((u32 *)skb->data)[i], QS_INJ_WR, grp);
 392
 393         /* Add padding */
 394         while (i < (OCELOT_BUFFER_CELL_SZ / 4)) {
 395                 ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
 396                 i++;
 397         }
 398
 399         /* Indicate EOF and valid bytes in last word */
 400         ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
 401                          QS_INJ_CTRL_VLD_BYTES(skb->len < OCELOT_BUFFER_CELL_SZ ? 0 : last) |
 402                          QS_INJ_CTRL_EOF,
 403                          QS_INJ_CTRL, grp);
 404
 405         /* Add dummy CRC */
 406         ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
 407         skb_tx_timestamp(skb);
 408
 409         dev->stats.tx_packets++;
 410         dev->stats.tx_bytes += skb->len;
 411
 412         kfree_skb(skb);
 413
 414         return NETDEV_TX_OK;
 415 }
 416
 417 enum ocelot_action_type {
 418         OCELOT_MACT_LEARN,
 419         OCELOT_MACT_FORGET,
 420 };
 421
 422 struct ocelot_mact_work_ctx {
 423         struct work_struct work;
 424         struct ocelot *ocelot;
 425         enum ocelot_action_type type;
 426         union {
 427                 /* OCELOT_MACT_LEARN */
 428                 struct {
 429                         unsigned char addr[ETH_ALEN];
 430                         u16 vid;
 431                         enum macaccess_entry_type entry_type;
 432                         int pgid;
 433                 } learn;
 434                 /* OCELOT_MACT_FORGET */
 435                 struct {
 436                         unsigned char addr[ETH_ALEN];
 437                         u16 vid;
 438                 } forget;
 439         };
 440 };
 441
 442 #define ocelot_work_to_ctx(x) \
 443         container_of((x), struct ocelot_mact_work_ctx, work)
 444
 445 static void ocelot_mact_work(struct work_struct *work)
 446 {
 447         struct ocelot_mact_work_ctx *w = ocelot_work_to_ctx(work);
 448         struct ocelot *ocelot = w->ocelot;
 449
 450         switch (w->type) {
 451         case OCELOT_MACT_LEARN:
 452                 ocelot_mact_learn(ocelot, w->learn.pgid, w->learn.addr,
 453                                   w->learn.vid, w->learn.entry_type);
 454                 break;
 455         case OCELOT_MACT_FORGET:
 456                 ocelot_mact_forget(ocelot, w->forget.addr, w->forget.vid);
 457                 break;
 458         default:
 459                 break;
 460         };
 461
 462         kfree(w);
 463 }
 464
 465 static int ocelot_enqueue_mact_action(struct ocelot *ocelot,
 466                                       const struct ocelot_mact_work_ctx *ctx)
 467 {
 468         struct ocelot_mact_work_ctx *w = kmemdup(ctx, sizeof(*w), GFP_ATOMIC);
 469
 470         if (!w)
 471                 return -ENOMEM;
 472
 473         w->ocelot = ocelot;
 474         INIT_WORK(&w->work, ocelot_mact_work);
 475         queue_work(ocelot->owq, &w->work);
 476
 477         return 0;
 478 }
 479
 480 static int ocelot_mc_unsync(struct net_device *dev, const unsigned char *addr)
 481 {
 482         struct ocelot_port_private *priv = netdev_priv(dev);
 483         struct ocelot_port *ocelot_port = &priv->port;
 484         struct ocelot *ocelot = ocelot_port->ocelot;
 485         struct ocelot_mact_work_ctx w;
 486
 487         ether_addr_copy(w.forget.addr, addr);
 488         w.forget.vid = ocelot_port->pvid_vlan.vid;
 489         w.type = OCELOT_MACT_FORGET;
 490
 491         return ocelot_enqueue_mact_action(ocelot, &w);
 492 }
 493
 494 static int ocelot_mc_sync(struct net_device *dev, const unsigned char *addr)
 495 {
 496         struct ocelot_port_private *priv = netdev_priv(dev);
 497         struct ocelot_port *ocelot_port = &priv->port;
 498         struct ocelot *ocelot = ocelot_port->ocelot;
 499         struct ocelot_mact_work_ctx w;
 500
 501         ether_addr_copy(w.learn.addr, addr);
 502         w.learn.vid = ocelot_port->pvid_vlan.vid;
 503         w.learn.pgid = PGID_CPU;
 504         w.learn.entry_type = ENTRYTYPE_LOCKED;
 505         w.type = OCELOT_MACT_LEARN;
 506
 507         return ocelot_enqueue_mact_action(ocelot, &w);
 508 }
 509
 510 static void ocelot_set_rx_mode(struct net_device *dev)
 511 {
 512         struct ocelot_port_private *priv = netdev_priv(dev);
 513         struct ocelot *ocelot = priv->port.ocelot;
 514         u32 val;
 515         int i;
 516
 517         /* This doesn't handle promiscuous mode because the bridge core is
 518          * setting IFF_PROMISC on all slave interfaces and all frames would be
 519          * forwarded to the CPU port.
 520          */
 521         val = GENMASK(ocelot->num_phys_ports - 1, 0);
 522         for_each_nonreserved_multicast_dest_pgid(ocelot, i)
 523                 ocelot_write_rix(ocelot, val, ANA_PGID_PGID, i);
 524
 525         __dev_mc_sync(dev, ocelot_mc_sync, ocelot_mc_unsync);
 526 }
 527
 528 static int ocelot_port_get_phys_port_name(struct net_device *dev,
 529                                           char *buf, size_t len)
 530 {
 531         struct ocelot_port_private *priv = netdev_priv(dev);
 532         int port = priv->chip_port;
 533         int ret;
 534
 535         ret = snprintf(buf, len, "p%d", port);
 536         if (ret >= len)
 537                 return -EINVAL;
 538
 539         return 0;
 540 }
 541
 542 static int ocelot_port_set_mac_address(struct net_device *dev, void *p)
 543 {
 544         struct ocelot_port_private *priv = netdev_priv(dev);
 545         struct ocelot_port *ocelot_port = &priv->port;
 546         struct ocelot *ocelot = ocelot_port->ocelot;
 547         const struct sockaddr *addr = p;
 548
 549         /* Learn the new net device MAC address in the mac table. */
 550         ocelot_mact_learn(ocelot, PGID_CPU, addr->sa_data,
 551                           ocelot_port->pvid_vlan.vid, ENTRYTYPE_LOCKED);
 552         /* Then forget the previous one. */
 553         ocelot_mact_forget(ocelot, dev->dev_addr, ocelot_port->pvid_vlan.vid);
 554
 555         ether_addr_copy(dev->dev_addr, addr->sa_data);
 556         return 0;
 557 }
 558
 559 static void ocelot_get_stats64(struct net_device *dev,
 560                                struct rtnl_link_stats64 *stats)
 561 {
 562         struct ocelot_port_private *priv = netdev_priv(dev);
 563         struct ocelot *ocelot = priv->port.ocelot;
 564         int port = priv->chip_port;
 565
 566         /* Configure the port to read the stats from */
 567         ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(port),
 568                      SYS_STAT_CFG);
 569
 570         /* Get Rx stats */
 571         stats->rx_bytes = ocelot_read(ocelot, SYS_COUNT_RX_OCTETS);
 572         stats->rx_packets = ocelot_read(ocelot, SYS_COUNT_RX_SHORTS) +
 573                             ocelot_read(ocelot, SYS_COUNT_RX_FRAGMENTS) +
 574                             ocelot_read(ocelot, SYS_COUNT_RX_JABBERS) +
 575                             ocelot_read(ocelot, SYS_COUNT_RX_LONGS) +
 576                             ocelot_read(ocelot, SYS_COUNT_RX_64) +
 577                             ocelot_read(ocelot, SYS_COUNT_RX_65_127) +
 578                             ocelot_read(ocelot, SYS_COUNT_RX_128_255) +
 579                             ocelot_read(ocelot, SYS_COUNT_RX_256_1023) +
 580                             ocelot_read(ocelot, SYS_COUNT_RX_1024_1526) +
 581                             ocelot_read(ocelot, SYS_COUNT_RX_1527_MAX);
 582         stats->multicast = ocelot_read(ocelot, SYS_COUNT_RX_MULTICAST);
 583         stats->rx_dropped = dev->stats.rx_dropped;
 584
 585         /* Get Tx stats */
 586         stats->tx_bytes = ocelot_read(ocelot, SYS_COUNT_TX_OCTETS);
 587         stats->tx_packets = ocelot_read(ocelot, SYS_COUNT_TX_64) +
 588                             ocelot_read(ocelot, SYS_COUNT_TX_65_127) +
 589                             ocelot_read(ocelot, SYS_COUNT_TX_128_511) +
 590                             ocelot_read(ocelot, SYS_COUNT_TX_512_1023) +
 591                             ocelot_read(ocelot, SYS_COUNT_TX_1024_1526) +
 592                             ocelot_read(ocelot, SYS_COUNT_TX_1527_MAX);
 593         stats->tx_dropped = ocelot_read(ocelot, SYS_COUNT_TX_DROPS) +
 594                             ocelot_read(ocelot, SYS_COUNT_TX_AGING);
 595         stats->collisions = ocelot_read(ocelot, SYS_COUNT_TX_COLLISION);
 596 }
 597
 598 static int ocelot_port_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
 599                                struct net_device *dev,
 600                                const unsigned char *addr,
 601                                u16 vid, u16 flags,
 602                                struct netlink_ext_ack *extack)
 603 {
 604         struct ocelot_port_private *priv = netdev_priv(dev);
 605         struct ocelot *ocelot = priv->port.ocelot;
 606         int port = priv->chip_port;
 607
 608         return ocelot_fdb_add(ocelot, port, addr, vid);
 609 }
 610
 611 static int ocelot_port_fdb_del(struct ndmsg *ndm, struct nlattr *tb[],
 612                                struct net_device *dev,
 613                                const unsigned char *addr, u16 vid)
 614 {
 615         struct ocelot_port_private *priv = netdev_priv(dev);
 616         struct ocelot *ocelot = priv->port.ocelot;
 617         int port = priv->chip_port;
 618
 619         return ocelot_fdb_del(ocelot, port, addr, vid);
 620 }
 621
 622 static int ocelot_port_fdb_dump(struct sk_buff *skb,
 623                                 struct netlink_callback *cb,
 624                                 struct net_device *dev,
 625                                 struct net_device *filter_dev, int *idx)
 626 {
 627         struct ocelot_port_private *priv = netdev_priv(dev);
 628         struct ocelot *ocelot = priv->port.ocelot;
 629         struct ocelot_dump_ctx dump = {
 630                 .dev = dev,
 631                 .skb = skb,
 632                 .cb = cb,
 633                 .idx = *idx,
 634         };
 635         int port = priv->chip_port;
 636         int ret;
 637
 638         ret = ocelot_fdb_dump(ocelot, port, ocelot_port_fdb_do_dump, &dump);
 639
 640         *idx = dump.idx;
 641
 642         return ret;
 643 }
 644
 645 static int ocelot_vlan_rx_add_vid(struct net_device *dev, __be16 proto,
 646                                   u16 vid)
 647 {
 648         return ocelot_vlan_vid_add(dev, vid, false, false);
 649 }
 650
 651 static int ocelot_vlan_rx_kill_vid(struct net_device *dev, __be16 proto,
 652                                    u16 vid)
 653 {
 654         return ocelot_vlan_vid_del(dev, vid);
 655 }
 656
 657 static void ocelot_vlan_mode(struct ocelot *ocelot, int port,
 658                              netdev_features_t features)
 659 {
 660         u32 val;
 661
 662         /* Filtering */
 663         val = ocelot_read(ocelot, ANA_VLANMASK);
 664         if (features & NETIF_F_HW_VLAN_CTAG_FILTER)
 665                 val |= BIT(port);
 666         else
 667                 val &= ~BIT(port);
 668         ocelot_write(ocelot, val, ANA_VLANMASK);
 669 }
 670
 671 static int ocelot_set_features(struct net_device *dev,
 672                                netdev_features_t features)
 673 {
 674         netdev_features_t changed = dev->features ^ features;
 675         struct ocelot_port_private *priv = netdev_priv(dev);
 676         struct ocelot *ocelot = priv->port.ocelot;
 677         int port = priv->chip_port;
 678
 679         if ((dev->features & NETIF_F_HW_TC) > (features & NETIF_F_HW_TC) &&
 680             priv->tc.offload_cnt) {
 681                 netdev_err(dev,
 682                            "Cannot disable HW TC offload while offloads active\n");
 683                 return -EBUSY;
 684         }
 685
 686         if (changed & NETIF_F_HW_VLAN_CTAG_FILTER)
 687                 ocelot_vlan_mode(ocelot, port, features);
 688
 689         return 0;
 690 }
 691
 692 static int ocelot_get_port_parent_id(struct net_device *dev,
 693                                      struct netdev_phys_item_id *ppid)
 694 {
 695         struct ocelot_port_private *priv = netdev_priv(dev);
 696         struct ocelot *ocelot = priv->port.ocelot;
 697
 698         ppid->id_len = sizeof(ocelot->base_mac);
 699         memcpy(&ppid->id, &ocelot->base_mac, ppid->id_len);
 700
 701         return 0;
 702 }
 703
 704 static int ocelot_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 705 {
 706         struct ocelot_port_private *priv = netdev_priv(dev);
 707         struct ocelot *ocelot = priv->port.ocelot;
 708         int port = priv->chip_port;
 709
 710         /* If the attached PHY device isn't capable of timestamping operations,
 711          * use our own (when possible).
 712          */
 713         if (!phy_has_hwtstamp(dev->phydev) && ocelot->ptp) {
 714                 switch (cmd) {
 715                 case SIOCSHWTSTAMP:
 716                         return ocelot_hwstamp_set(ocelot, port, ifr);
 717                 case SIOCGHWTSTAMP:
 718                         return ocelot_hwstamp_get(ocelot, port, ifr);
 719                 }
 720         }
 721
 722         return phy_mii_ioctl(dev->phydev, ifr, cmd);
 723 }
 724
 725 static const struct net_device_ops ocelot_port_netdev_ops = {
 726         .ndo_open                       = ocelot_port_open,
 727         .ndo_stop                       = ocelot_port_stop,
 728         .ndo_start_xmit                 = ocelot_port_xmit,
 729         .ndo_set_rx_mode                = ocelot_set_rx_mode,
 730         .ndo_get_phys_port_name         = ocelot_port_get_phys_port_name,
 731         .ndo_set_mac_address            = ocelot_port_set_mac_address,
 732         .ndo_get_stats64                = ocelot_get_stats64,
 733         .ndo_fdb_add                    = ocelot_port_fdb_add,
 734         .ndo_fdb_del                    = ocelot_port_fdb_del,
 735         .ndo_fdb_dump                   = ocelot_port_fdb_dump,
 736         .ndo_vlan_rx_add_vid            = ocelot_vlan_rx_add_vid,
 737         .ndo_vlan_rx_kill_vid           = ocelot_vlan_rx_kill_vid,
 738         .ndo_set_features               = ocelot_set_features,
 739         .ndo_get_port_parent_id         = ocelot_get_port_parent_id,
 740         .ndo_setup_tc                   = ocelot_setup_tc,
 741         .ndo_do_ioctl                   = ocelot_ioctl,
 742 };
 743
 744 struct net_device *ocelot_port_to_netdev(struct ocelot *ocelot, int port)
 745 {
 746         struct ocelot_port *ocelot_port = ocelot->ports[port];
 747         struct ocelot_port_private *priv;
 748
 749         if (!ocelot_port)
 750                 return NULL;
 751
 752         priv = container_of(ocelot_port, struct ocelot_port_private, port);
 753
 754         return priv->dev;
 755 }
 756
 757 /* Checks if the net_device instance given to us originates from our driver */
 758 static bool ocelot_netdevice_dev_check(const struct net_device *dev)
 759 {
 760         return dev->netdev_ops == &ocelot_port_netdev_ops;
 761 }
 762
 763 int ocelot_netdev_to_port(struct net_device *dev)
 764 {
 765         struct ocelot_port_private *priv;
 766
 767         if (!dev || !ocelot_netdevice_dev_check(dev))
 768                 return -EINVAL;
 769
 770         priv = netdev_priv(dev);
 771
 772         return priv->chip_port;
 773 }
 774
 775 static void ocelot_port_get_strings(struct net_device *netdev, u32 sset,
 776                                     u8 *data)
 777 {
 778         struct ocelot_port_private *priv = netdev_priv(netdev);
 779         struct ocelot *ocelot = priv->port.ocelot;
 780         int port = priv->chip_port;
 781
 782         ocelot_get_strings(ocelot, port, sset, data);
 783 }
 784
 785 static void ocelot_port_get_ethtool_stats(struct net_device *dev,
 786                                           struct ethtool_stats *stats,
 787                                           u64 *data)
 788 {
 789         struct ocelot_port_private *priv = netdev_priv(dev);
 790         struct ocelot *ocelot = priv->port.ocelot;
 791         int port = priv->chip_port;
 792
 793         ocelot_get_ethtool_stats(ocelot, port, data);
 794 }
 795
 796 static int ocelot_port_get_sset_count(struct net_device *dev, int sset)
 797 {
 798         struct ocelot_port_private *priv = netdev_priv(dev);
 799         struct ocelot *ocelot = priv->port.ocelot;
 800         int port = priv->chip_port;
 801
 802         return ocelot_get_sset_count(ocelot, port, sset);
 803 }
 804
 805 static int ocelot_port_get_ts_info(struct net_device *dev,
 806                                    struct ethtool_ts_info *info)
 807 {
 808         struct ocelot_port_private *priv = netdev_priv(dev);
 809         struct ocelot *ocelot = priv->port.ocelot;
 810         int port = priv->chip_port;
 811
 812         if (!ocelot->ptp)
 813                 return ethtool_op_get_ts_info(dev, info);
 814
 815         return ocelot_get_ts_info(ocelot, port, info);
 816 }
 817
 818 static const struct ethtool_ops ocelot_ethtool_ops = {
 819         .get_strings            = ocelot_port_get_strings,
 820         .get_ethtool_stats      = ocelot_port_get_ethtool_stats,
 821         .get_sset_count         = ocelot_port_get_sset_count,
 822         .get_link_ksettings     = phy_ethtool_get_link_ksettings,
 823         .set_link_ksettings     = phy_ethtool_set_link_ksettings,
 824         .get_ts_info            = ocelot_port_get_ts_info,
 825 };
 826
 827 static void ocelot_port_attr_stp_state_set(struct ocelot *ocelot, int port,
 828                                            struct switchdev_trans *trans,
 829                                            u8 state)
 830 {
 831         if (switchdev_trans_ph_prepare(trans))
 832                 return;
 833
 834         ocelot_bridge_stp_state_set(ocelot, port, state);
 835 }
 836
 837 static void ocelot_port_attr_ageing_set(struct ocelot *ocelot, int port,
 838                                         unsigned long ageing_clock_t)
 839 {
 840         unsigned long ageing_jiffies = clock_t_to_jiffies(ageing_clock_t);
 841         u32 ageing_time = jiffies_to_msecs(ageing_jiffies);
 842
 843         ocelot_set_ageing_time(ocelot, ageing_time);
 844 }
 845
 846 static void ocelot_port_attr_mc_set(struct ocelot *ocelot, int port, bool mc)
 847 {
 848         u32 cpu_fwd_mcast = ANA_PORT_CPU_FWD_CFG_CPU_IGMP_REDIR_ENA |
 849                             ANA_PORT_CPU_FWD_CFG_CPU_MLD_REDIR_ENA |
 850                             ANA_PORT_CPU_FWD_CFG_CPU_IPMC_CTRL_COPY_ENA;
 851         u32 val = 0;
 852
 853         if (mc)
 854                 val = cpu_fwd_mcast;
 855
 856         ocelot_rmw_gix(ocelot, val, cpu_fwd_mcast,
 857                        ANA_PORT_CPU_FWD_CFG, port);
 858 }
 859
 860 static int ocelot_port_attr_set(struct net_device *dev,
 861                                 const struct switchdev_attr *attr,
 862                                 struct switchdev_trans *trans)
 863 {
 864         struct ocelot_port_private *priv = netdev_priv(dev);
 865         struct ocelot *ocelot = priv->port.ocelot;
 866         int port = priv->chip_port;
 867         int err = 0;
 868
 869         switch (attr->id) {
 870         case SWITCHDEV_ATTR_ID_PORT_STP_STATE:
 871                 ocelot_port_attr_stp_state_set(ocelot, port, trans,
 872                                                attr->u.stp_state);
 873                 break;
 874         case SWITCHDEV_ATTR_ID_BRIDGE_AGEING_TIME:
 875                 ocelot_port_attr_ageing_set(ocelot, port, attr->u.ageing_time);
 876                 break;
 877         case SWITCHDEV_ATTR_ID_BRIDGE_VLAN_FILTERING:
 878                 ocelot_port_vlan_filtering(ocelot, port,
 879                                            attr->u.vlan_filtering, trans);
 880                 break;
 881         case SWITCHDEV_ATTR_ID_BRIDGE_MC_DISABLED:
 882                 ocelot_port_attr_mc_set(ocelot, port, !attr->u.mc_disabled);
 883                 break;
 884         default:
 885                 err = -EOPNOTSUPP;
 886                 break;
 887         }
 888
 889         return err;
 890 }
 891
 892 static int ocelot_port_obj_add_vlan(struct net_device *dev,
 893                                     const struct switchdev_obj_port_vlan *vlan,
 894                                     struct switchdev_trans *trans)
 895 {
 896         int ret;
 897         u16 vid;
 898
 899         for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
 900                 bool pvid = vlan->flags & BRIDGE_VLAN_INFO_PVID;
 901                 bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
 902
 903                 if (switchdev_trans_ph_prepare(trans))
 904                         ret = ocelot_vlan_vid_prepare(dev, vid, pvid,
 905                                                       untagged);
 906                 else
 907                         ret = ocelot_vlan_vid_add(dev, vid, pvid, untagged);
 908                 if (ret)
 909                         return ret;
 910         }
 911
 912         return 0;
 913 }
 914
 915 static int ocelot_port_vlan_del_vlan(struct net_device *dev,
 916                                      const struct switchdev_obj_port_vlan *vlan)
 917 {
 918         int ret;
 919         u16 vid;
 920
 921         for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
 922                 ret = ocelot_vlan_vid_del(dev, vid);
 923
 924                 if (ret)
 925                         return ret;
 926         }
 927
 928         return 0;
 929 }
 930
 931 static int ocelot_port_obj_add_mdb(struct net_device *dev,
 932                                    const struct switchdev_obj_port_mdb *mdb,
 933                                    struct switchdev_trans *trans)
 934 {
 935         struct ocelot_port_private *priv = netdev_priv(dev);
 936         struct ocelot_port *ocelot_port = &priv->port;
 937         struct ocelot *ocelot = ocelot_port->ocelot;
 938         int port = priv->chip_port;
 939
 940         if (switchdev_trans_ph_prepare(trans))
 941                 return 0;
 942
 943         return ocelot_port_mdb_add(ocelot, port, mdb);
 944 }
 945
 946 static int ocelot_port_obj_del_mdb(struct net_device *dev,
 947                                    const struct switchdev_obj_port_mdb *mdb)
 948 {
 949         struct ocelot_port_private *priv = netdev_priv(dev);
 950         struct ocelot_port *ocelot_port = &priv->port;
 951         struct ocelot *ocelot = ocelot_port->ocelot;
 952         int port = priv->chip_port;
 953
 954         return ocelot_port_mdb_del(ocelot, port, mdb);
 955 }
 956
 957 static int ocelot_port_obj_add(struct net_device *dev,
 958                                const struct switchdev_obj *obj,
 959                                struct switchdev_trans *trans,
 960                                struct netlink_ext_ack *extack)
 961 {
 962         int ret = 0;
 963
 964         switch (obj->id) {
 965         case SWITCHDEV_OBJ_ID_PORT_VLAN:
 966                 ret = ocelot_port_obj_add_vlan(dev,
 967                                                SWITCHDEV_OBJ_PORT_VLAN(obj),
 968                                                trans);
 969                 break;
 970         case SWITCHDEV_OBJ_ID_PORT_MDB:
 971                 ret = ocelot_port_obj_add_mdb(dev, SWITCHDEV_OBJ_PORT_MDB(obj),
 972                                               trans);
 973                 break;
 974         default:
 975                 return -EOPNOTSUPP;
 976         }
 977
 978         return ret;
 979 }
 980
 981 static int ocelot_port_obj_del(struct net_device *dev,
 982                                const struct switchdev_obj *obj)
 983 {
 984         int ret = 0;
 985
 986         switch (obj->id) {
 987         case SWITCHDEV_OBJ_ID_PORT_VLAN:
 988                 ret = ocelot_port_vlan_del_vlan(dev,
 989                                                 SWITCHDEV_OBJ_PORT_VLAN(obj));
 990                 break;
 991         case SWITCHDEV_OBJ_ID_PORT_MDB:
 992                 ret = ocelot_port_obj_del_mdb(dev, SWITCHDEV_OBJ_PORT_MDB(obj));
 993                 break;
 994         default:
 995                 return -EOPNOTSUPP;
 996         }
 997
 998         return ret;
 999 }
1000
1001 static int ocelot_netdevice_port_event(struct net_device *dev,
1002                                        unsigned long event,
1003                                        struct netdev_notifier_changeupper_info *info)
1004 {
1005         struct ocelot_port_private *priv = netdev_priv(dev);
1006         struct ocelot_port *ocelot_port = &priv->port;
1007         struct ocelot *ocelot = ocelot_port->ocelot;
1008         int port = priv->chip_port;
1009         int err = 0;
1010
1011         switch (event) {
1012         case NETDEV_CHANGEUPPER:
1013                 if (netif_is_bridge_master(info->upper_dev)) {
1014                         if (info->linking) {
1015                                 err = ocelot_port_bridge_join(ocelot, port,
1016                                                               info->upper_dev);
1017                         } else {
1018                                 err = ocelot_port_bridge_leave(ocelot, port,
1019                                                                info->upper_dev);
1020                         }
1021                 }
1022                 if (netif_is_lag_master(info->upper_dev)) {
1023                         if (info->linking)
1024                                 err = ocelot_port_lag_join(ocelot, port,
1025                                                            info->upper_dev);
1026                         else
1027                                 ocelot_port_lag_leave(ocelot, port,
1028                                                       info->upper_dev);
1029                 }
1030                 break;
1031         default:
1032                 break;
1033         }
1034
1035         return err;
1036 }
1037
1038 static int ocelot_netdevice_event(struct notifier_block *unused,
1039                                   unsigned long event, void *ptr)
1040 {
1041         struct netdev_notifier_changeupper_info *info = ptr;
1042         struct net_device *dev = netdev_notifier_info_to_dev(ptr);
1043         int ret = 0;
1044
1045         if (event == NETDEV_PRECHANGEUPPER &&
1046             ocelot_netdevice_dev_check(dev) &&
1047             netif_is_lag_master(info->upper_dev)) {
1048                 struct netdev_lag_upper_info *lag_upper_info = info->upper_info;
1049                 struct netlink_ext_ack *extack;
1050
1051                 if (lag_upper_info &&
1052                     lag_upper_info->tx_type != NETDEV_LAG_TX_TYPE_HASH) {
1053                         extack = netdev_notifier_info_to_extack(&info->info);
1054                         NL_SET_ERR_MSG_MOD(extack, "LAG device using unsupported Tx type");
1055
1056                         ret = -EINVAL;
1057                         goto notify;
1058                 }
1059         }
1060
1061         if (netif_is_lag_master(dev)) {
1062                 struct net_device *slave;
1063                 struct list_head *iter;
1064
1065                 netdev_for_each_lower_dev(dev, slave, iter) {
1066                         ret = ocelot_netdevice_port_event(slave, event, info);
1067                         if (ret)
1068                                 goto notify;
1069                 }
1070         } else {
1071                 ret = ocelot_netdevice_port_event(dev, event, info);
1072         }
1073
1074 notify:
1075         return notifier_from_errno(ret);
1076 }
1077
1078 struct notifier_block ocelot_netdevice_nb __read_mostly = {
1079         .notifier_call = ocelot_netdevice_event,
1080 };
1081
1082 static int ocelot_switchdev_event(struct notifier_block *unused,
1083                                   unsigned long event, void *ptr)
1084 {
1085         struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
1086         int err;
1087
1088         switch (event) {
1089         case SWITCHDEV_PORT_ATTR_SET:
1090                 err = switchdev_handle_port_attr_set(dev, ptr,
1091                                                      ocelot_netdevice_dev_check,
1092                                                      ocelot_port_attr_set);
1093                 return notifier_from_errno(err);
1094         }
1095
1096         return NOTIFY_DONE;
1097 }
1098
1099 struct notifier_block ocelot_switchdev_nb __read_mostly = {
1100         .notifier_call = ocelot_switchdev_event,
1101 };
1102
1103 static int ocelot_switchdev_blocking_event(struct notifier_block *unused,
1104                                            unsigned long event, void *ptr)
1105 {
1106         struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
1107         int err;
1108
1109         switch (event) {
1110                 /* Blocking events. */
1111         case SWITCHDEV_PORT_OBJ_ADD:
1112                 err = switchdev_handle_port_obj_add(dev, ptr,
1113                                                     ocelot_netdevice_dev_check,
1114                                                     ocelot_port_obj_add);
1115                 return notifier_from_errno(err);
1116         case SWITCHDEV_PORT_OBJ_DEL:
1117                 err = switchdev_handle_port_obj_del(dev, ptr,
1118                                                     ocelot_netdevice_dev_check,
1119                                                     ocelot_port_obj_del);
1120                 return notifier_from_errno(err);
1121         case SWITCHDEV_PORT_ATTR_SET:
1122                 err = switchdev_handle_port_attr_set(dev, ptr,
1123                                                      ocelot_netdevice_dev_check,
1124                                                      ocelot_port_attr_set);
1125                 return notifier_from_errno(err);
1126         }
1127
1128         return NOTIFY_DONE;
1129 }
1130
1131 struct notifier_block ocelot_switchdev_blocking_nb __read_mostly = {
1132         .notifier_call = ocelot_switchdev_blocking_event,
1133 };
1134
1135 int ocelot_probe_port(struct ocelot *ocelot, int port, struct regmap *target,
1136                       struct phy_device *phy)
1137 {
1138         struct ocelot_port_private *priv;
1139         struct ocelot_port *ocelot_port;
1140         struct net_device *dev;
1141         int err;
1142
1143         dev = alloc_etherdev(sizeof(struct ocelot_port_private));
1144         if (!dev)
1145                 return -ENOMEM;
1146         SET_NETDEV_DEV(dev, ocelot->dev);
1147         priv = netdev_priv(dev);
1148         priv->dev = dev;
1149         priv->phy = phy;
1150         priv->chip_port = port;
1151         ocelot_port = &priv->port;
1152         ocelot_port->ocelot = ocelot;
1153         ocelot_port->target = target;
1154         ocelot->ports[port] = ocelot_port;
1155
1156         dev->netdev_ops = &ocelot_port_netdev_ops;
1157         dev->ethtool_ops = &ocelot_ethtool_ops;
1158
1159         dev->hw_features |= NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_RXFCS |
1160                 NETIF_F_HW_TC;
1161         dev->features |= NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_HW_TC;
1162
1163         memcpy(dev->dev_addr, ocelot->base_mac, ETH_ALEN);
1164         dev->dev_addr[ETH_ALEN - 1] += port;
1165         ocelot_mact_learn(ocelot, PGID_CPU, dev->dev_addr,
1166                           ocelot_port->pvid_vlan.vid, ENTRYTYPE_LOCKED);
1167
1168         ocelot_init_port(ocelot, port);
1169
1170         err = register_netdev(dev);
1171         if (err) {
1172                 dev_err(ocelot->dev, "register_netdev failed\n");
1173                 free_netdev(dev);
1174         }
1175
1176         return err;
1177 }