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_add(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         int ret;
 217
 218         ret = ocelot_vlan_add(ocelot, port, vid, pvid, untagged);
 219         if (ret)
 220                 return ret;
 221
 222         /* Add the port MAC address to with the right VLAN information */
 223         ocelot_mact_learn(ocelot, PGID_CPU, dev->dev_addr, vid,
 224                           ENTRYTYPE_LOCKED);
 225
 226         return 0;
 227 }
 228
 229 static int ocelot_vlan_vid_del(struct net_device *dev, u16 vid)
 230 {
 231         struct ocelot_port_private *priv = netdev_priv(dev);
 232         struct ocelot *ocelot = priv->port.ocelot;
 233         int port = priv->chip_port;
 234         int ret;
 235
 236         /* 8021q removes VID 0 on module unload for all interfaces
 237          * with VLAN filtering feature. We need to keep it to receive
 238          * untagged traffic.
 239          */
 240         if (vid == 0)
 241                 return 0;
 242
 243         ret = ocelot_vlan_del(ocelot, port, vid);
 244         if (ret)
 245                 return ret;
 246
 247         /* Del the port MAC address to with the right VLAN information */
 248         ocelot_mact_forget(ocelot, dev->dev_addr, vid);
 249
 250         return 0;
 251 }
 252
 253 static int ocelot_port_open(struct net_device *dev)
 254 {
 255         struct ocelot_port_private *priv = netdev_priv(dev);
 256         struct ocelot_port *ocelot_port = &priv->port;
 257         struct ocelot *ocelot = ocelot_port->ocelot;
 258         int port = priv->chip_port;
 259         int err;
 260
 261         if (priv->serdes) {
 262                 err = phy_set_mode_ext(priv->serdes, PHY_MODE_ETHERNET,
 263                                        ocelot_port->phy_mode);
 264                 if (err) {
 265                         netdev_err(dev, "Could not set mode of SerDes\n");
 266                         return err;
 267                 }
 268         }
 269
 270         err = phy_connect_direct(dev, priv->phy, &ocelot_port_adjust_link,
 271                                  ocelot_port->phy_mode);
 272         if (err) {
 273                 netdev_err(dev, "Could not attach to PHY\n");
 274                 return err;
 275         }
 276
 277         dev->phydev = priv->phy;
 278
 279         phy_attached_info(priv->phy);
 280         phy_start(priv->phy);
 281
 282         ocelot_port_enable(ocelot, port, priv->phy);
 283
 284         return 0;
 285 }
 286
 287 static int ocelot_port_stop(struct net_device *dev)
 288 {
 289         struct ocelot_port_private *priv = netdev_priv(dev);
 290         struct ocelot *ocelot = priv->port.ocelot;
 291         int port = priv->chip_port;
 292
 293         phy_disconnect(priv->phy);
 294
 295         dev->phydev = NULL;
 296
 297         ocelot_port_disable(ocelot, port);
 298
 299         return 0;
 300 }
 301
 302 /* Generate the IFH for frame injection
 303  *
 304  * The IFH is a 128bit-value
 305  * bit 127: bypass the analyzer processing
 306  * bit 56-67: destination mask
 307  * bit 28-29: pop_cnt: 3 disables all rewriting of the frame
 308  * bit 20-27: cpu extraction queue mask
 309  * bit 16: tag type 0: C-tag, 1: S-tag
 310  * bit 0-11: VID
 311  */
 312 static int ocelot_gen_ifh(u32 *ifh, struct frame_info *info)
 313 {
 314         ifh[0] = IFH_INJ_BYPASS | ((0x1ff & info->rew_op) << 21);
 315         ifh[1] = (0xf00 & info->port) >> 8;
 316         ifh[2] = (0xff & info->port) << 24;
 317         ifh[3] = (info->tag_type << 16) | info->vid;
 318
 319         return 0;
 320 }
 321
 322 static int ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
 323 {
 324         struct ocelot_port_private *priv = netdev_priv(dev);
 325         struct skb_shared_info *shinfo = skb_shinfo(skb);
 326         struct ocelot_port *ocelot_port = &priv->port;
 327         struct ocelot *ocelot = ocelot_port->ocelot;
 328         u32 val, ifh[OCELOT_TAG_LEN / 4];
 329         struct frame_info info = {};
 330         u8 grp = 0; /* Send everything on CPU group 0 */
 331         unsigned int i, count, last;
 332         int port = priv->chip_port;
 333
 334         val = ocelot_read(ocelot, QS_INJ_STATUS);
 335         if (!(val & QS_INJ_STATUS_FIFO_RDY(BIT(grp))) ||
 336             (val & QS_INJ_STATUS_WMARK_REACHED(BIT(grp))))
 337                 return NETDEV_TX_BUSY;
 338
 339         ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
 340                          QS_INJ_CTRL_SOF, QS_INJ_CTRL, grp);
 341
 342         info.port = BIT(port);
 343         info.tag_type = IFH_TAG_TYPE_C;
 344         info.vid = skb_vlan_tag_get(skb);
 345
 346         /* Check if timestamping is needed */
 347         if (ocelot->ptp && (shinfo->tx_flags & SKBTX_HW_TSTAMP)) {
 348                 info.rew_op = ocelot_port->ptp_cmd;
 349
 350                 if (ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
 351                         struct sk_buff *clone;
 352
 353                         clone = skb_clone_sk(skb);
 354                         if (!clone) {
 355                                 kfree_skb(skb);
 356                                 return NETDEV_TX_OK;
 357                         }
 358
 359                         ocelot_port_add_txtstamp_skb(ocelot, port, clone);
 360
 361                         info.rew_op |= clone->cb[0] << 3;
 362                 }
 363         }
 364
 365         if (ocelot->ptp && shinfo->tx_flags & SKBTX_HW_TSTAMP) {
 366                 info.rew_op = ocelot_port->ptp_cmd;
 367                 if (ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP)
 368                         info.rew_op |= skb->cb[0] << 3;
 369         }
 370
 371         ocelot_gen_ifh(ifh, &info);
 372
 373         for (i = 0; i < OCELOT_TAG_LEN / 4; i++)
 374                 ocelot_write_rix(ocelot, (__force u32)cpu_to_be32(ifh[i]),
 375                                  QS_INJ_WR, grp);
 376
 377         count = (skb->len + 3) / 4;
 378         last = skb->len % 4;
 379         for (i = 0; i < count; i++)
 380                 ocelot_write_rix(ocelot, ((u32 *)skb->data)[i], QS_INJ_WR, grp);
 381
 382         /* Add padding */
 383         while (i < (OCELOT_BUFFER_CELL_SZ / 4)) {
 384                 ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
 385                 i++;
 386         }
 387
 388         /* Indicate EOF and valid bytes in last word */
 389         ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
 390                          QS_INJ_CTRL_VLD_BYTES(skb->len < OCELOT_BUFFER_CELL_SZ ? 0 : last) |
 391                          QS_INJ_CTRL_EOF,
 392                          QS_INJ_CTRL, grp);
 393
 394         /* Add dummy CRC */
 395         ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
 396         skb_tx_timestamp(skb);
 397
 398         dev->stats.tx_packets++;
 399         dev->stats.tx_bytes += skb->len;
 400
 401         kfree_skb(skb);
 402
 403         return NETDEV_TX_OK;
 404 }
 405
 406 static int ocelot_mc_unsync(struct net_device *dev, const unsigned char *addr)
 407 {
 408         struct ocelot_port_private *priv = netdev_priv(dev);
 409         struct ocelot_port *ocelot_port = &priv->port;
 410         struct ocelot *ocelot = ocelot_port->ocelot;
 411
 412         return ocelot_mact_forget(ocelot, addr, ocelot_port->pvid);
 413 }
 414
 415 static int ocelot_mc_sync(struct net_device *dev, const unsigned char *addr)
 416 {
 417         struct ocelot_port_private *priv = netdev_priv(dev);
 418         struct ocelot_port *ocelot_port = &priv->port;
 419         struct ocelot *ocelot = ocelot_port->ocelot;
 420
 421         return ocelot_mact_learn(ocelot, PGID_CPU, addr, ocelot_port->pvid,
 422                                  ENTRYTYPE_LOCKED);
 423 }
 424
 425 static void ocelot_set_rx_mode(struct net_device *dev)
 426 {
 427         struct ocelot_port_private *priv = netdev_priv(dev);
 428         struct ocelot *ocelot = priv->port.ocelot;
 429         u32 val;
 430         int i;
 431
 432         /* This doesn't handle promiscuous mode because the bridge core is
 433          * setting IFF_PROMISC on all slave interfaces and all frames would be
 434          * forwarded to the CPU port.
 435          */
 436         val = GENMASK(ocelot->num_phys_ports - 1, 0);
 437         for_each_nonreserved_multicast_dest_pgid(ocelot, i)
 438                 ocelot_write_rix(ocelot, val, ANA_PGID_PGID, i);
 439
 440         __dev_mc_sync(dev, ocelot_mc_sync, ocelot_mc_unsync);
 441 }
 442
 443 static int ocelot_port_get_phys_port_name(struct net_device *dev,
 444                                           char *buf, size_t len)
 445 {
 446         struct ocelot_port_private *priv = netdev_priv(dev);
 447         int port = priv->chip_port;
 448         int ret;
 449
 450         ret = snprintf(buf, len, "p%d", port);
 451         if (ret >= len)
 452                 return -EINVAL;
 453
 454         return 0;
 455 }
 456
 457 static int ocelot_port_set_mac_address(struct net_device *dev, void *p)
 458 {
 459         struct ocelot_port_private *priv = netdev_priv(dev);
 460         struct ocelot_port *ocelot_port = &priv->port;
 461         struct ocelot *ocelot = ocelot_port->ocelot;
 462         const struct sockaddr *addr = p;
 463
 464         /* Learn the new net device MAC address in the mac table. */
 465         ocelot_mact_learn(ocelot, PGID_CPU, addr->sa_data, ocelot_port->pvid,
 466                           ENTRYTYPE_LOCKED);
 467         /* Then forget the previous one. */
 468         ocelot_mact_forget(ocelot, dev->dev_addr, ocelot_port->pvid);
 469
 470         ether_addr_copy(dev->dev_addr, addr->sa_data);
 471         return 0;
 472 }
 473
 474 static void ocelot_get_stats64(struct net_device *dev,
 475                                struct rtnl_link_stats64 *stats)
 476 {
 477         struct ocelot_port_private *priv = netdev_priv(dev);
 478         struct ocelot *ocelot = priv->port.ocelot;
 479         int port = priv->chip_port;
 480
 481         /* Configure the port to read the stats from */
 482         ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(port),
 483                      SYS_STAT_CFG);
 484
 485         /* Get Rx stats */
 486         stats->rx_bytes = ocelot_read(ocelot, SYS_COUNT_RX_OCTETS);
 487         stats->rx_packets = ocelot_read(ocelot, SYS_COUNT_RX_SHORTS) +
 488                             ocelot_read(ocelot, SYS_COUNT_RX_FRAGMENTS) +
 489                             ocelot_read(ocelot, SYS_COUNT_RX_JABBERS) +
 490                             ocelot_read(ocelot, SYS_COUNT_RX_LONGS) +
 491                             ocelot_read(ocelot, SYS_COUNT_RX_64) +
 492                             ocelot_read(ocelot, SYS_COUNT_RX_65_127) +
 493                             ocelot_read(ocelot, SYS_COUNT_RX_128_255) +
 494                             ocelot_read(ocelot, SYS_COUNT_RX_256_1023) +
 495                             ocelot_read(ocelot, SYS_COUNT_RX_1024_1526) +
 496                             ocelot_read(ocelot, SYS_COUNT_RX_1527_MAX);
 497         stats->multicast = ocelot_read(ocelot, SYS_COUNT_RX_MULTICAST);
 498         stats->rx_dropped = dev->stats.rx_dropped;
 499
 500         /* Get Tx stats */
 501         stats->tx_bytes = ocelot_read(ocelot, SYS_COUNT_TX_OCTETS);
 502         stats->tx_packets = ocelot_read(ocelot, SYS_COUNT_TX_64) +
 503                             ocelot_read(ocelot, SYS_COUNT_TX_65_127) +
 504                             ocelot_read(ocelot, SYS_COUNT_TX_128_511) +
 505                             ocelot_read(ocelot, SYS_COUNT_TX_512_1023) +
 506                             ocelot_read(ocelot, SYS_COUNT_TX_1024_1526) +
 507                             ocelot_read(ocelot, SYS_COUNT_TX_1527_MAX);
 508         stats->tx_dropped = ocelot_read(ocelot, SYS_COUNT_TX_DROPS) +
 509                             ocelot_read(ocelot, SYS_COUNT_TX_AGING);
 510         stats->collisions = ocelot_read(ocelot, SYS_COUNT_TX_COLLISION);
 511 }
 512
 513 static int ocelot_port_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
 514                                struct net_device *dev,
 515                                const unsigned char *addr,
 516                                u16 vid, u16 flags,
 517                                struct netlink_ext_ack *extack)
 518 {
 519         struct ocelot_port_private *priv = netdev_priv(dev);
 520         struct ocelot *ocelot = priv->port.ocelot;
 521         int port = priv->chip_port;
 522
 523         return ocelot_fdb_add(ocelot, port, addr, vid);
 524 }
 525
 526 static int ocelot_port_fdb_del(struct ndmsg *ndm, struct nlattr *tb[],
 527                                struct net_device *dev,
 528                                const unsigned char *addr, u16 vid)
 529 {
 530         struct ocelot_port_private *priv = netdev_priv(dev);
 531         struct ocelot *ocelot = priv->port.ocelot;
 532         int port = priv->chip_port;
 533
 534         return ocelot_fdb_del(ocelot, port, addr, vid);
 535 }
 536
 537 static int ocelot_port_fdb_dump(struct sk_buff *skb,
 538                                 struct netlink_callback *cb,
 539                                 struct net_device *dev,
 540                                 struct net_device *filter_dev, int *idx)
 541 {
 542         struct ocelot_port_private *priv = netdev_priv(dev);
 543         struct ocelot *ocelot = priv->port.ocelot;
 544         struct ocelot_dump_ctx dump = {
 545                 .dev = dev,
 546                 .skb = skb,
 547                 .cb = cb,
 548                 .idx = *idx,
 549         };
 550         int port = priv->chip_port;
 551         int ret;
 552
 553         ret = ocelot_fdb_dump(ocelot, port, ocelot_port_fdb_do_dump, &dump);
 554
 555         *idx = dump.idx;
 556
 557         return ret;
 558 }
 559
 560 static int ocelot_vlan_rx_add_vid(struct net_device *dev, __be16 proto,
 561                                   u16 vid)
 562 {
 563         return ocelot_vlan_vid_add(dev, vid, false, false);
 564 }
 565
 566 static int ocelot_vlan_rx_kill_vid(struct net_device *dev, __be16 proto,
 567                                    u16 vid)
 568 {
 569         return ocelot_vlan_vid_del(dev, vid);
 570 }
 571
 572 static void ocelot_vlan_mode(struct ocelot *ocelot, int port,
 573                              netdev_features_t features)
 574 {
 575         u32 val;
 576
 577         /* Filtering */
 578         val = ocelot_read(ocelot, ANA_VLANMASK);
 579         if (features & NETIF_F_HW_VLAN_CTAG_FILTER)
 580                 val |= BIT(port);
 581         else
 582                 val &= ~BIT(port);
 583         ocelot_write(ocelot, val, ANA_VLANMASK);
 584 }
 585
 586 static int ocelot_set_features(struct net_device *dev,
 587                                netdev_features_t features)
 588 {
 589         netdev_features_t changed = dev->features ^ features;
 590         struct ocelot_port_private *priv = netdev_priv(dev);
 591         struct ocelot *ocelot = priv->port.ocelot;
 592         int port = priv->chip_port;
 593
 594         if ((dev->features & NETIF_F_HW_TC) > (features & NETIF_F_HW_TC) &&
 595             priv->tc.offload_cnt) {
 596                 netdev_err(dev,
 597                            "Cannot disable HW TC offload while offloads active\n");
 598                 return -EBUSY;
 599         }
 600
 601         if (changed & NETIF_F_HW_VLAN_CTAG_FILTER)
 602                 ocelot_vlan_mode(ocelot, port, features);
 603
 604         return 0;
 605 }
 606
 607 static int ocelot_get_port_parent_id(struct net_device *dev,
 608                                      struct netdev_phys_item_id *ppid)
 609 {
 610         struct ocelot_port_private *priv = netdev_priv(dev);
 611         struct ocelot *ocelot = priv->port.ocelot;
 612
 613         ppid->id_len = sizeof(ocelot->base_mac);
 614         memcpy(&ppid->id, &ocelot->base_mac, ppid->id_len);
 615
 616         return 0;
 617 }
 618
 619 static int ocelot_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 620 {
 621         struct ocelot_port_private *priv = netdev_priv(dev);
 622         struct ocelot *ocelot = priv->port.ocelot;
 623         int port = priv->chip_port;
 624
 625         /* If the attached PHY device isn't capable of timestamping operations,
 626          * use our own (when possible).
 627          */
 628         if (!phy_has_hwtstamp(dev->phydev) && ocelot->ptp) {
 629                 switch (cmd) {
 630                 case SIOCSHWTSTAMP:
 631                         return ocelot_hwstamp_set(ocelot, port, ifr);
 632                 case SIOCGHWTSTAMP:
 633                         return ocelot_hwstamp_get(ocelot, port, ifr);
 634                 }
 635         }
 636
 637         return phy_mii_ioctl(dev->phydev, ifr, cmd);
 638 }
 639
 640 static const struct net_device_ops ocelot_port_netdev_ops = {
 641         .ndo_open                       = ocelot_port_open,
 642         .ndo_stop                       = ocelot_port_stop,
 643         .ndo_start_xmit                 = ocelot_port_xmit,
 644         .ndo_set_rx_mode                = ocelot_set_rx_mode,
 645         .ndo_get_phys_port_name         = ocelot_port_get_phys_port_name,
 646         .ndo_set_mac_address            = ocelot_port_set_mac_address,
 647         .ndo_get_stats64                = ocelot_get_stats64,
 648         .ndo_fdb_add                    = ocelot_port_fdb_add,
 649         .ndo_fdb_del                    = ocelot_port_fdb_del,
 650         .ndo_fdb_dump                   = ocelot_port_fdb_dump,
 651         .ndo_vlan_rx_add_vid            = ocelot_vlan_rx_add_vid,
 652         .ndo_vlan_rx_kill_vid           = ocelot_vlan_rx_kill_vid,
 653         .ndo_set_features               = ocelot_set_features,
 654         .ndo_get_port_parent_id         = ocelot_get_port_parent_id,
 655         .ndo_setup_tc                   = ocelot_setup_tc,
 656         .ndo_do_ioctl                   = ocelot_ioctl,
 657 };
 658
 659 struct net_device *ocelot_port_to_netdev(struct ocelot *ocelot, int port)
 660 {
 661         struct ocelot_port *ocelot_port = ocelot->ports[port];
 662         struct ocelot_port_private *priv;
 663
 664         if (!ocelot_port)
 665                 return NULL;
 666
 667         priv = container_of(ocelot_port, struct ocelot_port_private, port);
 668
 669         return priv->dev;
 670 }
 671
 672 /* Checks if the net_device instance given to us originates from our driver */
 673 static bool ocelot_netdevice_dev_check(const struct net_device *dev)
 674 {
 675         return dev->netdev_ops == &ocelot_port_netdev_ops;
 676 }
 677
 678 int ocelot_netdev_to_port(struct net_device *dev)
 679 {
 680         struct ocelot_port_private *priv;
 681
 682         if (!dev || !ocelot_netdevice_dev_check(dev))
 683                 return -EINVAL;
 684
 685         priv = netdev_priv(dev);
 686
 687         return priv->chip_port;
 688 }
 689
 690 static void ocelot_port_get_strings(struct net_device *netdev, u32 sset,
 691                                     u8 *data)
 692 {
 693         struct ocelot_port_private *priv = netdev_priv(netdev);
 694         struct ocelot *ocelot = priv->port.ocelot;
 695         int port = priv->chip_port;
 696
 697         ocelot_get_strings(ocelot, port, sset, data);
 698 }
 699
 700 static void ocelot_port_get_ethtool_stats(struct net_device *dev,
 701                                           struct ethtool_stats *stats,
 702                                           u64 *data)
 703 {
 704         struct ocelot_port_private *priv = netdev_priv(dev);
 705         struct ocelot *ocelot = priv->port.ocelot;
 706         int port = priv->chip_port;
 707
 708         ocelot_get_ethtool_stats(ocelot, port, data);
 709 }
 710
 711 static int ocelot_port_get_sset_count(struct net_device *dev, int sset)
 712 {
 713         struct ocelot_port_private *priv = netdev_priv(dev);
 714         struct ocelot *ocelot = priv->port.ocelot;
 715         int port = priv->chip_port;
 716
 717         return ocelot_get_sset_count(ocelot, port, sset);
 718 }
 719
 720 static int ocelot_port_get_ts_info(struct net_device *dev,
 721                                    struct ethtool_ts_info *info)
 722 {
 723         struct ocelot_port_private *priv = netdev_priv(dev);
 724         struct ocelot *ocelot = priv->port.ocelot;
 725         int port = priv->chip_port;
 726
 727         if (!ocelot->ptp)
 728                 return ethtool_op_get_ts_info(dev, info);
 729
 730         return ocelot_get_ts_info(ocelot, port, info);
 731 }
 732
 733 static const struct ethtool_ops ocelot_ethtool_ops = {
 734         .get_strings            = ocelot_port_get_strings,
 735         .get_ethtool_stats      = ocelot_port_get_ethtool_stats,
 736         .get_sset_count         = ocelot_port_get_sset_count,
 737         .get_link_ksettings     = phy_ethtool_get_link_ksettings,
 738         .set_link_ksettings     = phy_ethtool_set_link_ksettings,
 739         .get_ts_info            = ocelot_port_get_ts_info,
 740 };
 741
 742 static void ocelot_port_attr_stp_state_set(struct ocelot *ocelot, int port,
 743                                            struct switchdev_trans *trans,
 744                                            u8 state)
 745 {
 746         if (switchdev_trans_ph_prepare(trans))
 747                 return;
 748
 749         ocelot_bridge_stp_state_set(ocelot, port, state);
 750 }
 751
 752 static void ocelot_port_attr_ageing_set(struct ocelot *ocelot, int port,
 753                                         unsigned long ageing_clock_t)
 754 {
 755         unsigned long ageing_jiffies = clock_t_to_jiffies(ageing_clock_t);
 756         u32 ageing_time = jiffies_to_msecs(ageing_jiffies);
 757
 758         ocelot_set_ageing_time(ocelot, ageing_time);
 759 }
 760
 761 static void ocelot_port_attr_mc_set(struct ocelot *ocelot, int port, bool mc)
 762 {
 763         u32 cpu_fwd_mcast = ANA_PORT_CPU_FWD_CFG_CPU_IGMP_REDIR_ENA |
 764                             ANA_PORT_CPU_FWD_CFG_CPU_MLD_REDIR_ENA |
 765                             ANA_PORT_CPU_FWD_CFG_CPU_IPMC_CTRL_COPY_ENA;
 766         u32 val = 0;
 767
 768         if (mc)
 769                 val = cpu_fwd_mcast;
 770
 771         ocelot_rmw_gix(ocelot, val, cpu_fwd_mcast,
 772                        ANA_PORT_CPU_FWD_CFG, port);
 773 }
 774
 775 static int ocelot_port_attr_set(struct net_device *dev,
 776                                 const struct switchdev_attr *attr,
 777                                 struct switchdev_trans *trans)
 778 {
 779         struct ocelot_port_private *priv = netdev_priv(dev);
 780         struct ocelot *ocelot = priv->port.ocelot;
 781         int port = priv->chip_port;
 782         int err = 0;
 783
 784         switch (attr->id) {
 785         case SWITCHDEV_ATTR_ID_PORT_STP_STATE:
 786                 ocelot_port_attr_stp_state_set(ocelot, port, trans,
 787                                                attr->u.stp_state);
 788                 break;
 789         case SWITCHDEV_ATTR_ID_BRIDGE_AGEING_TIME:
 790                 ocelot_port_attr_ageing_set(ocelot, port, attr->u.ageing_time);
 791                 break;
 792         case SWITCHDEV_ATTR_ID_BRIDGE_VLAN_FILTERING:
 793                 ocelot_port_vlan_filtering(ocelot, port,
 794                                            attr->u.vlan_filtering, trans);
 795                 break;
 796         case SWITCHDEV_ATTR_ID_BRIDGE_MC_DISABLED:
 797                 ocelot_port_attr_mc_set(ocelot, port, !attr->u.mc_disabled);
 798                 break;
 799         default:
 800                 err = -EOPNOTSUPP;
 801                 break;
 802         }
 803
 804         return err;
 805 }
 806
 807 static int ocelot_port_obj_add_vlan(struct net_device *dev,
 808                                     const struct switchdev_obj_port_vlan *vlan,
 809                                     struct switchdev_trans *trans)
 810 {
 811         int ret;
 812         u16 vid;
 813
 814         for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
 815                 ret = ocelot_vlan_vid_add(dev, vid,
 816                                           vlan->flags & BRIDGE_VLAN_INFO_PVID,
 817                                           vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED);
 818                 if (ret)
 819                         return ret;
 820         }
 821
 822         return 0;
 823 }
 824
 825 static int ocelot_port_vlan_del_vlan(struct net_device *dev,
 826                                      const struct switchdev_obj_port_vlan *vlan)
 827 {
 828         int ret;
 829         u16 vid;
 830
 831         for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
 832                 ret = ocelot_vlan_vid_del(dev, vid);
 833
 834                 if (ret)
 835                         return ret;
 836         }
 837
 838         return 0;
 839 }
 840
 841 static int ocelot_port_obj_add_mdb(struct net_device *dev,
 842                                    const struct switchdev_obj_port_mdb *mdb,
 843                                    struct switchdev_trans *trans)
 844 {
 845         struct ocelot_port_private *priv = netdev_priv(dev);
 846         struct ocelot_port *ocelot_port = &priv->port;
 847         struct ocelot *ocelot = ocelot_port->ocelot;
 848         int port = priv->chip_port;
 849
 850         if (switchdev_trans_ph_prepare(trans))
 851                 return 0;
 852
 853         return ocelot_port_mdb_add(ocelot, port, mdb);
 854 }
 855
 856 static int ocelot_port_obj_del_mdb(struct net_device *dev,
 857                                    const struct switchdev_obj_port_mdb *mdb)
 858 {
 859         struct ocelot_port_private *priv = netdev_priv(dev);
 860         struct ocelot_port *ocelot_port = &priv->port;
 861         struct ocelot *ocelot = ocelot_port->ocelot;
 862         int port = priv->chip_port;
 863
 864         return ocelot_port_mdb_del(ocelot, port, mdb);
 865 }
 866
 867 static int ocelot_port_obj_add(struct net_device *dev,
 868                                const struct switchdev_obj *obj,
 869                                struct switchdev_trans *trans,
 870                                struct netlink_ext_ack *extack)
 871 {
 872         int ret = 0;
 873
 874         switch (obj->id) {
 875         case SWITCHDEV_OBJ_ID_PORT_VLAN:
 876                 ret = ocelot_port_obj_add_vlan(dev,
 877                                                SWITCHDEV_OBJ_PORT_VLAN(obj),
 878                                                trans);
 879                 break;
 880         case SWITCHDEV_OBJ_ID_PORT_MDB:
 881                 ret = ocelot_port_obj_add_mdb(dev, SWITCHDEV_OBJ_PORT_MDB(obj),
 882                                               trans);
 883                 break;
 884         default:
 885                 return -EOPNOTSUPP;
 886         }
 887
 888         return ret;
 889 }
 890
 891 static int ocelot_port_obj_del(struct net_device *dev,
 892                                const struct switchdev_obj *obj)
 893 {
 894         int ret = 0;
 895
 896         switch (obj->id) {
 897         case SWITCHDEV_OBJ_ID_PORT_VLAN:
 898                 ret = ocelot_port_vlan_del_vlan(dev,
 899                                                 SWITCHDEV_OBJ_PORT_VLAN(obj));
 900                 break;
 901         case SWITCHDEV_OBJ_ID_PORT_MDB:
 902                 ret = ocelot_port_obj_del_mdb(dev, SWITCHDEV_OBJ_PORT_MDB(obj));
 903                 break;
 904         default:
 905                 return -EOPNOTSUPP;
 906         }
 907
 908         return ret;
 909 }
 910
 911 static int ocelot_netdevice_port_event(struct net_device *dev,
 912                                        unsigned long event,
 913                                        struct netdev_notifier_changeupper_info *info)
 914 {
 915         struct ocelot_port_private *priv = netdev_priv(dev);
 916         struct ocelot_port *ocelot_port = &priv->port;
 917         struct ocelot *ocelot = ocelot_port->ocelot;
 918         int port = priv->chip_port;
 919         int err = 0;
 920
 921         switch (event) {
 922         case NETDEV_CHANGEUPPER:
 923                 if (netif_is_bridge_master(info->upper_dev)) {
 924                         if (info->linking) {
 925                                 err = ocelot_port_bridge_join(ocelot, port,
 926                                                               info->upper_dev);
 927                         } else {
 928                                 err = ocelot_port_bridge_leave(ocelot, port,
 929                                                                info->upper_dev);
 930                         }
 931                 }
 932                 if (netif_is_lag_master(info->upper_dev)) {
 933                         if (info->linking)
 934                                 err = ocelot_port_lag_join(ocelot, port,
 935                                                            info->upper_dev);
 936                         else
 937                                 ocelot_port_lag_leave(ocelot, port,
 938                                                       info->upper_dev);
 939                 }
 940                 break;
 941         default:
 942                 break;
 943         }
 944
 945         return err;
 946 }
 947
 948 static int ocelot_netdevice_event(struct notifier_block *unused,
 949                                   unsigned long event, void *ptr)
 950 {
 951         struct netdev_notifier_changeupper_info *info = ptr;
 952         struct net_device *dev = netdev_notifier_info_to_dev(ptr);
 953         int ret = 0;
 954
 955         if (!ocelot_netdevice_dev_check(dev))
 956                 return 0;
 957
 958         if (event == NETDEV_PRECHANGEUPPER &&
 959             netif_is_lag_master(info->upper_dev)) {
 960                 struct netdev_lag_upper_info *lag_upper_info = info->upper_info;
 961                 struct netlink_ext_ack *extack;
 962
 963                 if (lag_upper_info &&
 964                     lag_upper_info->tx_type != NETDEV_LAG_TX_TYPE_HASH) {
 965                         extack = netdev_notifier_info_to_extack(&info->info);
 966                         NL_SET_ERR_MSG_MOD(extack, "LAG device using unsupported Tx type");
 967
 968                         ret = -EINVAL;
 969                         goto notify;
 970                 }
 971         }
 972
 973         if (netif_is_lag_master(dev)) {
 974                 struct net_device *slave;
 975                 struct list_head *iter;
 976
 977                 netdev_for_each_lower_dev(dev, slave, iter) {
 978                         ret = ocelot_netdevice_port_event(slave, event, info);
 979                         if (ret)
 980                                 goto notify;
 981                 }
 982         } else {
 983                 ret = ocelot_netdevice_port_event(dev, event, info);
 984         }
 985
 986 notify:
 987         return notifier_from_errno(ret);
 988 }
 989
 990 struct notifier_block ocelot_netdevice_nb __read_mostly = {
 991         .notifier_call = ocelot_netdevice_event,
 992 };
 993
 994 static int ocelot_switchdev_event(struct notifier_block *unused,
 995                                   unsigned long event, void *ptr)
 996 {
 997         struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
 998         int err;
 999
1000         switch (event) {
1001         case SWITCHDEV_PORT_ATTR_SET:
1002                 err = switchdev_handle_port_attr_set(dev, ptr,
1003                                                      ocelot_netdevice_dev_check,
1004                                                      ocelot_port_attr_set);
1005                 return notifier_from_errno(err);
1006         }
1007
1008         return NOTIFY_DONE;
1009 }
1010
1011 struct notifier_block ocelot_switchdev_nb __read_mostly = {
1012         .notifier_call = ocelot_switchdev_event,
1013 };
1014
1015 static int ocelot_switchdev_blocking_event(struct notifier_block *unused,
1016                                            unsigned long event, void *ptr)
1017 {
1018         struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
1019         int err;
1020
1021         switch (event) {
1022                 /* Blocking events. */
1023         case SWITCHDEV_PORT_OBJ_ADD:
1024                 err = switchdev_handle_port_obj_add(dev, ptr,
1025                                                     ocelot_netdevice_dev_check,
1026                                                     ocelot_port_obj_add);
1027                 return notifier_from_errno(err);
1028         case SWITCHDEV_PORT_OBJ_DEL:
1029                 err = switchdev_handle_port_obj_del(dev, ptr,
1030                                                     ocelot_netdevice_dev_check,
1031                                                     ocelot_port_obj_del);
1032                 return notifier_from_errno(err);
1033         case SWITCHDEV_PORT_ATTR_SET:
1034                 err = switchdev_handle_port_attr_set(dev, ptr,
1035                                                      ocelot_netdevice_dev_check,
1036                                                      ocelot_port_attr_set);
1037                 return notifier_from_errno(err);
1038         }
1039
1040         return NOTIFY_DONE;
1041 }
1042
1043 struct notifier_block ocelot_switchdev_blocking_nb __read_mostly = {
1044         .notifier_call = ocelot_switchdev_blocking_event,
1045 };
1046
1047 int ocelot_probe_port(struct ocelot *ocelot, int port, struct regmap *target,
1048                       struct phy_device *phy)
1049 {
1050         struct ocelot_port_private *priv;
1051         struct ocelot_port *ocelot_port;
1052         struct net_device *dev;
1053         int err;
1054
1055         dev = alloc_etherdev(sizeof(struct ocelot_port_private));
1056         if (!dev)
1057                 return -ENOMEM;
1058         SET_NETDEV_DEV(dev, ocelot->dev);
1059         priv = netdev_priv(dev);
1060         priv->dev = dev;
1061         priv->phy = phy;
1062         priv->chip_port = port;
1063         ocelot_port = &priv->port;
1064         ocelot_port->ocelot = ocelot;
1065         ocelot_port->target = target;
1066         ocelot->ports[port] = ocelot_port;
1067
1068         dev->netdev_ops = &ocelot_port_netdev_ops;
1069         dev->ethtool_ops = &ocelot_ethtool_ops;
1070
1071         dev->hw_features |= NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_RXFCS |
1072                 NETIF_F_HW_TC;
1073         dev->features |= NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_HW_TC;
1074
1075         memcpy(dev->dev_addr, ocelot->base_mac, ETH_ALEN);
1076         dev->dev_addr[ETH_ALEN - 1] += port;
1077         ocelot_mact_learn(ocelot, PGID_CPU, dev->dev_addr, ocelot_port->pvid,
1078                           ENTRYTYPE_LOCKED);
1079
1080         ocelot_init_port(ocelot, port);
1081
1082         err = register_netdev(dev);
1083         if (err) {
1084                 dev_err(ocelot->dev, "register_netdev failed\n");
1085                 free_netdev(dev);
1086         }
1087
1088         return err;
1089 }