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 static int ocelot_mc_unsync(struct net_device *dev, const unsigned char *addr)
 418 {
 419         struct ocelot_port_private *priv = netdev_priv(dev);
 420         struct ocelot_port *ocelot_port = &priv->port;
 421         struct ocelot *ocelot = ocelot_port->ocelot;
 422
 423         return ocelot_mact_forget(ocelot, addr, ocelot_port->pvid_vlan.vid);
 424 }
 425
 426 static int ocelot_mc_sync(struct net_device *dev, const unsigned char *addr)
 427 {
 428         struct ocelot_port_private *priv = netdev_priv(dev);
 429         struct ocelot_port *ocelot_port = &priv->port;
 430         struct ocelot *ocelot = ocelot_port->ocelot;
 431
 432         return ocelot_mact_learn(ocelot, PGID_CPU, addr,
 433                                  ocelot_port->pvid_vlan.vid, ENTRYTYPE_LOCKED);
 434 }
 435
 436 static void ocelot_set_rx_mode(struct net_device *dev)
 437 {
 438         struct ocelot_port_private *priv = netdev_priv(dev);
 439         struct ocelot *ocelot = priv->port.ocelot;
 440         u32 val;
 441         int i;
 442
 443         /* This doesn't handle promiscuous mode because the bridge core is
 444          * setting IFF_PROMISC on all slave interfaces and all frames would be
 445          * forwarded to the CPU port.
 446          */
 447         val = GENMASK(ocelot->num_phys_ports - 1, 0);
 448         for_each_nonreserved_multicast_dest_pgid(ocelot, i)
 449                 ocelot_write_rix(ocelot, val, ANA_PGID_PGID, i);
 450
 451         __dev_mc_sync(dev, ocelot_mc_sync, ocelot_mc_unsync);
 452 }
 453
 454 static int ocelot_port_get_phys_port_name(struct net_device *dev,
 455                                           char *buf, size_t len)
 456 {
 457         struct ocelot_port_private *priv = netdev_priv(dev);
 458         int port = priv->chip_port;
 459         int ret;
 460
 461         ret = snprintf(buf, len, "p%d", port);
 462         if (ret >= len)
 463                 return -EINVAL;
 464
 465         return 0;
 466 }
 467
 468 static int ocelot_port_set_mac_address(struct net_device *dev, void *p)
 469 {
 470         struct ocelot_port_private *priv = netdev_priv(dev);
 471         struct ocelot_port *ocelot_port = &priv->port;
 472         struct ocelot *ocelot = ocelot_port->ocelot;
 473         const struct sockaddr *addr = p;
 474
 475         /* Learn the new net device MAC address in the mac table. */
 476         ocelot_mact_learn(ocelot, PGID_CPU, addr->sa_data,
 477                           ocelot_port->pvid_vlan.vid, ENTRYTYPE_LOCKED);
 478         /* Then forget the previous one. */
 479         ocelot_mact_forget(ocelot, dev->dev_addr, ocelot_port->pvid_vlan.vid);
 480
 481         ether_addr_copy(dev->dev_addr, addr->sa_data);
 482         return 0;
 483 }
 484
 485 static void ocelot_get_stats64(struct net_device *dev,
 486                                struct rtnl_link_stats64 *stats)
 487 {
 488         struct ocelot_port_private *priv = netdev_priv(dev);
 489         struct ocelot *ocelot = priv->port.ocelot;
 490         int port = priv->chip_port;
 491
 492         /* Configure the port to read the stats from */
 493         ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(port),
 494                      SYS_STAT_CFG);
 495
 496         /* Get Rx stats */
 497         stats->rx_bytes = ocelot_read(ocelot, SYS_COUNT_RX_OCTETS);
 498         stats->rx_packets = ocelot_read(ocelot, SYS_COUNT_RX_SHORTS) +
 499                             ocelot_read(ocelot, SYS_COUNT_RX_FRAGMENTS) +
 500                             ocelot_read(ocelot, SYS_COUNT_RX_JABBERS) +
 501                             ocelot_read(ocelot, SYS_COUNT_RX_LONGS) +
 502                             ocelot_read(ocelot, SYS_COUNT_RX_64) +
 503                             ocelot_read(ocelot, SYS_COUNT_RX_65_127) +
 504                             ocelot_read(ocelot, SYS_COUNT_RX_128_255) +
 505                             ocelot_read(ocelot, SYS_COUNT_RX_256_1023) +
 506                             ocelot_read(ocelot, SYS_COUNT_RX_1024_1526) +
 507                             ocelot_read(ocelot, SYS_COUNT_RX_1527_MAX);
 508         stats->multicast = ocelot_read(ocelot, SYS_COUNT_RX_MULTICAST);
 509         stats->rx_dropped = dev->stats.rx_dropped;
 510
 511         /* Get Tx stats */
 512         stats->tx_bytes = ocelot_read(ocelot, SYS_COUNT_TX_OCTETS);
 513         stats->tx_packets = ocelot_read(ocelot, SYS_COUNT_TX_64) +
 514                             ocelot_read(ocelot, SYS_COUNT_TX_65_127) +
 515                             ocelot_read(ocelot, SYS_COUNT_TX_128_511) +
 516                             ocelot_read(ocelot, SYS_COUNT_TX_512_1023) +
 517                             ocelot_read(ocelot, SYS_COUNT_TX_1024_1526) +
 518                             ocelot_read(ocelot, SYS_COUNT_TX_1527_MAX);
 519         stats->tx_dropped = ocelot_read(ocelot, SYS_COUNT_TX_DROPS) +
 520                             ocelot_read(ocelot, SYS_COUNT_TX_AGING);
 521         stats->collisions = ocelot_read(ocelot, SYS_COUNT_TX_COLLISION);
 522 }
 523
 524 static int ocelot_port_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
 525                                struct net_device *dev,
 526                                const unsigned char *addr,
 527                                u16 vid, u16 flags,
 528                                struct netlink_ext_ack *extack)
 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_add(ocelot, port, addr, vid);
 535 }
 536
 537 static int ocelot_port_fdb_del(struct ndmsg *ndm, struct nlattr *tb[],
 538                                struct net_device *dev,
 539                                const unsigned char *addr, u16 vid)
 540 {
 541         struct ocelot_port_private *priv = netdev_priv(dev);
 542         struct ocelot *ocelot = priv->port.ocelot;
 543         int port = priv->chip_port;
 544
 545         return ocelot_fdb_del(ocelot, port, addr, vid);
 546 }
 547
 548 static int ocelot_port_fdb_dump(struct sk_buff *skb,
 549                                 struct netlink_callback *cb,
 550                                 struct net_device *dev,
 551                                 struct net_device *filter_dev, int *idx)
 552 {
 553         struct ocelot_port_private *priv = netdev_priv(dev);
 554         struct ocelot *ocelot = priv->port.ocelot;
 555         struct ocelot_dump_ctx dump = {
 556                 .dev = dev,
 557                 .skb = skb,
 558                 .cb = cb,
 559                 .idx = *idx,
 560         };
 561         int port = priv->chip_port;
 562         int ret;
 563
 564         ret = ocelot_fdb_dump(ocelot, port, ocelot_port_fdb_do_dump, &dump);
 565
 566         *idx = dump.idx;
 567
 568         return ret;
 569 }
 570
 571 static int ocelot_vlan_rx_add_vid(struct net_device *dev, __be16 proto,
 572                                   u16 vid)
 573 {
 574         return ocelot_vlan_vid_add(dev, vid, false, false);
 575 }
 576
 577 static int ocelot_vlan_rx_kill_vid(struct net_device *dev, __be16 proto,
 578                                    u16 vid)
 579 {
 580         return ocelot_vlan_vid_del(dev, vid);
 581 }
 582
 583 static void ocelot_vlan_mode(struct ocelot *ocelot, int port,
 584                              netdev_features_t features)
 585 {
 586         u32 val;
 587
 588         /* Filtering */
 589         val = ocelot_read(ocelot, ANA_VLANMASK);
 590         if (features & NETIF_F_HW_VLAN_CTAG_FILTER)
 591                 val |= BIT(port);
 592         else
 593                 val &= ~BIT(port);
 594         ocelot_write(ocelot, val, ANA_VLANMASK);
 595 }
 596
 597 static int ocelot_set_features(struct net_device *dev,
 598                                netdev_features_t features)
 599 {
 600         netdev_features_t changed = dev->features ^ features;
 601         struct ocelot_port_private *priv = netdev_priv(dev);
 602         struct ocelot *ocelot = priv->port.ocelot;
 603         int port = priv->chip_port;
 604
 605         if ((dev->features & NETIF_F_HW_TC) > (features & NETIF_F_HW_TC) &&
 606             priv->tc.offload_cnt) {
 607                 netdev_err(dev,
 608                            "Cannot disable HW TC offload while offloads active\n");
 609                 return -EBUSY;
 610         }
 611
 612         if (changed & NETIF_F_HW_VLAN_CTAG_FILTER)
 613                 ocelot_vlan_mode(ocelot, port, features);
 614
 615         return 0;
 616 }
 617
 618 static int ocelot_get_port_parent_id(struct net_device *dev,
 619                                      struct netdev_phys_item_id *ppid)
 620 {
 621         struct ocelot_port_private *priv = netdev_priv(dev);
 622         struct ocelot *ocelot = priv->port.ocelot;
 623
 624         ppid->id_len = sizeof(ocelot->base_mac);
 625         memcpy(&ppid->id, &ocelot->base_mac, ppid->id_len);
 626
 627         return 0;
 628 }
 629
 630 static int ocelot_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 631 {
 632         struct ocelot_port_private *priv = netdev_priv(dev);
 633         struct ocelot *ocelot = priv->port.ocelot;
 634         int port = priv->chip_port;
 635
 636         /* If the attached PHY device isn't capable of timestamping operations,
 637          * use our own (when possible).
 638          */
 639         if (!phy_has_hwtstamp(dev->phydev) && ocelot->ptp) {
 640                 switch (cmd) {
 641                 case SIOCSHWTSTAMP:
 642                         return ocelot_hwstamp_set(ocelot, port, ifr);
 643                 case SIOCGHWTSTAMP:
 644                         return ocelot_hwstamp_get(ocelot, port, ifr);
 645                 }
 646         }
 647
 648         return phy_mii_ioctl(dev->phydev, ifr, cmd);
 649 }
 650
 651 static const struct net_device_ops ocelot_port_netdev_ops = {
 652         .ndo_open                       = ocelot_port_open,
 653         .ndo_stop                       = ocelot_port_stop,
 654         .ndo_start_xmit                 = ocelot_port_xmit,
 655         .ndo_set_rx_mode                = ocelot_set_rx_mode,
 656         .ndo_get_phys_port_name         = ocelot_port_get_phys_port_name,
 657         .ndo_set_mac_address            = ocelot_port_set_mac_address,
 658         .ndo_get_stats64                = ocelot_get_stats64,
 659         .ndo_fdb_add                    = ocelot_port_fdb_add,
 660         .ndo_fdb_del                    = ocelot_port_fdb_del,
 661         .ndo_fdb_dump                   = ocelot_port_fdb_dump,
 662         .ndo_vlan_rx_add_vid            = ocelot_vlan_rx_add_vid,
 663         .ndo_vlan_rx_kill_vid           = ocelot_vlan_rx_kill_vid,
 664         .ndo_set_features               = ocelot_set_features,
 665         .ndo_get_port_parent_id         = ocelot_get_port_parent_id,
 666         .ndo_setup_tc                   = ocelot_setup_tc,
 667         .ndo_do_ioctl                   = ocelot_ioctl,
 668 };
 669
 670 struct net_device *ocelot_port_to_netdev(struct ocelot *ocelot, int port)
 671 {
 672         struct ocelot_port *ocelot_port = ocelot->ports[port];
 673         struct ocelot_port_private *priv;
 674
 675         if (!ocelot_port)
 676                 return NULL;
 677
 678         priv = container_of(ocelot_port, struct ocelot_port_private, port);
 679
 680         return priv->dev;
 681 }
 682
 683 /* Checks if the net_device instance given to us originates from our driver */
 684 static bool ocelot_netdevice_dev_check(const struct net_device *dev)
 685 {
 686         return dev->netdev_ops == &ocelot_port_netdev_ops;
 687 }
 688
 689 int ocelot_netdev_to_port(struct net_device *dev)
 690 {
 691         struct ocelot_port_private *priv;
 692
 693         if (!dev || !ocelot_netdevice_dev_check(dev))
 694                 return -EINVAL;
 695
 696         priv = netdev_priv(dev);
 697
 698         return priv->chip_port;
 699 }
 700
 701 static void ocelot_port_get_strings(struct net_device *netdev, u32 sset,
 702                                     u8 *data)
 703 {
 704         struct ocelot_port_private *priv = netdev_priv(netdev);
 705         struct ocelot *ocelot = priv->port.ocelot;
 706         int port = priv->chip_port;
 707
 708         ocelot_get_strings(ocelot, port, sset, data);
 709 }
 710
 711 static void ocelot_port_get_ethtool_stats(struct net_device *dev,
 712                                           struct ethtool_stats *stats,
 713                                           u64 *data)
 714 {
 715         struct ocelot_port_private *priv = netdev_priv(dev);
 716         struct ocelot *ocelot = priv->port.ocelot;
 717         int port = priv->chip_port;
 718
 719         ocelot_get_ethtool_stats(ocelot, port, data);
 720 }
 721
 722 static int ocelot_port_get_sset_count(struct net_device *dev, int sset)
 723 {
 724         struct ocelot_port_private *priv = netdev_priv(dev);
 725         struct ocelot *ocelot = priv->port.ocelot;
 726         int port = priv->chip_port;
 727
 728         return ocelot_get_sset_count(ocelot, port, sset);
 729 }
 730
 731 static int ocelot_port_get_ts_info(struct net_device *dev,
 732                                    struct ethtool_ts_info *info)
 733 {
 734         struct ocelot_port_private *priv = netdev_priv(dev);
 735         struct ocelot *ocelot = priv->port.ocelot;
 736         int port = priv->chip_port;
 737
 738         if (!ocelot->ptp)
 739                 return ethtool_op_get_ts_info(dev, info);
 740
 741         return ocelot_get_ts_info(ocelot, port, info);
 742 }
 743
 744 static const struct ethtool_ops ocelot_ethtool_ops = {
 745         .get_strings            = ocelot_port_get_strings,
 746         .get_ethtool_stats      = ocelot_port_get_ethtool_stats,
 747         .get_sset_count         = ocelot_port_get_sset_count,
 748         .get_link_ksettings     = phy_ethtool_get_link_ksettings,
 749         .set_link_ksettings     = phy_ethtool_set_link_ksettings,
 750         .get_ts_info            = ocelot_port_get_ts_info,
 751 };
 752
 753 static void ocelot_port_attr_stp_state_set(struct ocelot *ocelot, int port,
 754                                            struct switchdev_trans *trans,
 755                                            u8 state)
 756 {
 757         if (switchdev_trans_ph_prepare(trans))
 758                 return;
 759
 760         ocelot_bridge_stp_state_set(ocelot, port, state);
 761 }
 762
 763 static void ocelot_port_attr_ageing_set(struct ocelot *ocelot, int port,
 764                                         unsigned long ageing_clock_t)
 765 {
 766         unsigned long ageing_jiffies = clock_t_to_jiffies(ageing_clock_t);
 767         u32 ageing_time = jiffies_to_msecs(ageing_jiffies);
 768
 769         ocelot_set_ageing_time(ocelot, ageing_time);
 770 }
 771
 772 static void ocelot_port_attr_mc_set(struct ocelot *ocelot, int port, bool mc)
 773 {
 774         u32 cpu_fwd_mcast = ANA_PORT_CPU_FWD_CFG_CPU_IGMP_REDIR_ENA |
 775                             ANA_PORT_CPU_FWD_CFG_CPU_MLD_REDIR_ENA |
 776                             ANA_PORT_CPU_FWD_CFG_CPU_IPMC_CTRL_COPY_ENA;
 777         u32 val = 0;
 778
 779         if (mc)
 780                 val = cpu_fwd_mcast;
 781
 782         ocelot_rmw_gix(ocelot, val, cpu_fwd_mcast,
 783                        ANA_PORT_CPU_FWD_CFG, port);
 784 }
 785
 786 static int ocelot_port_attr_set(struct net_device *dev,
 787                                 const struct switchdev_attr *attr,
 788                                 struct switchdev_trans *trans)
 789 {
 790         struct ocelot_port_private *priv = netdev_priv(dev);
 791         struct ocelot *ocelot = priv->port.ocelot;
 792         int port = priv->chip_port;
 793         int err = 0;
 794
 795         switch (attr->id) {
 796         case SWITCHDEV_ATTR_ID_PORT_STP_STATE:
 797                 ocelot_port_attr_stp_state_set(ocelot, port, trans,
 798                                                attr->u.stp_state);
 799                 break;
 800         case SWITCHDEV_ATTR_ID_BRIDGE_AGEING_TIME:
 801                 ocelot_port_attr_ageing_set(ocelot, port, attr->u.ageing_time);
 802                 break;
 803         case SWITCHDEV_ATTR_ID_BRIDGE_VLAN_FILTERING:
 804                 ocelot_port_vlan_filtering(ocelot, port,
 805                                            attr->u.vlan_filtering, trans);
 806                 break;
 807         case SWITCHDEV_ATTR_ID_BRIDGE_MC_DISABLED:
 808                 ocelot_port_attr_mc_set(ocelot, port, !attr->u.mc_disabled);
 809                 break;
 810         default:
 811                 err = -EOPNOTSUPP;
 812                 break;
 813         }
 814
 815         return err;
 816 }
 817
 818 static int ocelot_port_obj_add_vlan(struct net_device *dev,
 819                                     const struct switchdev_obj_port_vlan *vlan,
 820                                     struct switchdev_trans *trans)
 821 {
 822         int ret;
 823         u16 vid;
 824
 825         for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
 826                 bool pvid = vlan->flags & BRIDGE_VLAN_INFO_PVID;
 827                 bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
 828
 829                 if (switchdev_trans_ph_prepare(trans))
 830                         ret = ocelot_vlan_vid_prepare(dev, vid, pvid,
 831                                                       untagged);
 832                 else
 833                         ret = ocelot_vlan_vid_add(dev, vid, pvid, untagged);
 834                 if (ret)
 835                         return ret;
 836         }
 837
 838         return 0;
 839 }
 840
 841 static int ocelot_port_vlan_del_vlan(struct net_device *dev,
 842                                      const struct switchdev_obj_port_vlan *vlan)
 843 {
 844         int ret;
 845         u16 vid;
 846
 847         for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
 848                 ret = ocelot_vlan_vid_del(dev, vid);
 849
 850                 if (ret)
 851                         return ret;
 852         }
 853
 854         return 0;
 855 }
 856
 857 static int ocelot_port_obj_add_mdb(struct net_device *dev,
 858                                    const struct switchdev_obj_port_mdb *mdb,
 859                                    struct switchdev_trans *trans)
 860 {
 861         struct ocelot_port_private *priv = netdev_priv(dev);
 862         struct ocelot_port *ocelot_port = &priv->port;
 863         struct ocelot *ocelot = ocelot_port->ocelot;
 864         int port = priv->chip_port;
 865
 866         if (switchdev_trans_ph_prepare(trans))
 867                 return 0;
 868
 869         return ocelot_port_mdb_add(ocelot, port, mdb);
 870 }
 871
 872 static int ocelot_port_obj_del_mdb(struct net_device *dev,
 873                                    const struct switchdev_obj_port_mdb *mdb)
 874 {
 875         struct ocelot_port_private *priv = netdev_priv(dev);
 876         struct ocelot_port *ocelot_port = &priv->port;
 877         struct ocelot *ocelot = ocelot_port->ocelot;
 878         int port = priv->chip_port;
 879
 880         return ocelot_port_mdb_del(ocelot, port, mdb);
 881 }
 882
 883 static int ocelot_port_obj_add(struct net_device *dev,
 884                                const struct switchdev_obj *obj,
 885                                struct switchdev_trans *trans,
 886                                struct netlink_ext_ack *extack)
 887 {
 888         int ret = 0;
 889
 890         switch (obj->id) {
 891         case SWITCHDEV_OBJ_ID_PORT_VLAN:
 892                 ret = ocelot_port_obj_add_vlan(dev,
 893                                                SWITCHDEV_OBJ_PORT_VLAN(obj),
 894                                                trans);
 895                 break;
 896         case SWITCHDEV_OBJ_ID_PORT_MDB:
 897                 ret = ocelot_port_obj_add_mdb(dev, SWITCHDEV_OBJ_PORT_MDB(obj),
 898                                               trans);
 899                 break;
 900         default:
 901                 return -EOPNOTSUPP;
 902         }
 903
 904         return ret;
 905 }
 906
 907 static int ocelot_port_obj_del(struct net_device *dev,
 908                                const struct switchdev_obj *obj)
 909 {
 910         int ret = 0;
 911
 912         switch (obj->id) {
 913         case SWITCHDEV_OBJ_ID_PORT_VLAN:
 914                 ret = ocelot_port_vlan_del_vlan(dev,
 915                                                 SWITCHDEV_OBJ_PORT_VLAN(obj));
 916                 break;
 917         case SWITCHDEV_OBJ_ID_PORT_MDB:
 918                 ret = ocelot_port_obj_del_mdb(dev, SWITCHDEV_OBJ_PORT_MDB(obj));
 919                 break;
 920         default:
 921                 return -EOPNOTSUPP;
 922         }
 923
 924         return ret;
 925 }
 926
 927 static int ocelot_netdevice_port_event(struct net_device *dev,
 928                                        unsigned long event,
 929                                        struct netdev_notifier_changeupper_info *info)
 930 {
 931         struct ocelot_port_private *priv = netdev_priv(dev);
 932         struct ocelot_port *ocelot_port = &priv->port;
 933         struct ocelot *ocelot = ocelot_port->ocelot;
 934         int port = priv->chip_port;
 935         int err = 0;
 936
 937         switch (event) {
 938         case NETDEV_CHANGEUPPER:
 939                 if (netif_is_bridge_master(info->upper_dev)) {
 940                         if (info->linking) {
 941                                 err = ocelot_port_bridge_join(ocelot, port,
 942                                                               info->upper_dev);
 943                         } else {
 944                                 err = ocelot_port_bridge_leave(ocelot, port,
 945                                                                info->upper_dev);
 946                         }
 947                 }
 948                 if (netif_is_lag_master(info->upper_dev)) {
 949                         if (info->linking)
 950                                 err = ocelot_port_lag_join(ocelot, port,
 951                                                            info->upper_dev);
 952                         else
 953                                 ocelot_port_lag_leave(ocelot, port,
 954                                                       info->upper_dev);
 955                 }
 956                 break;
 957         default:
 958                 break;
 959         }
 960
 961         return err;
 962 }
 963
 964 static int ocelot_netdevice_event(struct notifier_block *unused,
 965                                   unsigned long event, void *ptr)
 966 {
 967         struct netdev_notifier_changeupper_info *info = ptr;
 968         struct net_device *dev = netdev_notifier_info_to_dev(ptr);
 969         int ret = 0;
 970
 971         if (!ocelot_netdevice_dev_check(dev))
 972                 return 0;
 973
 974         if (event == NETDEV_PRECHANGEUPPER &&
 975             netif_is_lag_master(info->upper_dev)) {
 976                 struct netdev_lag_upper_info *lag_upper_info = info->upper_info;
 977                 struct netlink_ext_ack *extack;
 978
 979                 if (lag_upper_info &&
 980                     lag_upper_info->tx_type != NETDEV_LAG_TX_TYPE_HASH) {
 981                         extack = netdev_notifier_info_to_extack(&info->info);
 982                         NL_SET_ERR_MSG_MOD(extack, "LAG device using unsupported Tx type");
 983
 984                         ret = -EINVAL;
 985                         goto notify;
 986                 }
 987         }
 988
 989         if (netif_is_lag_master(dev)) {
 990                 struct net_device *slave;
 991                 struct list_head *iter;
 992
 993                 netdev_for_each_lower_dev(dev, slave, iter) {
 994                         ret = ocelot_netdevice_port_event(slave, event, info);
 995                         if (ret)
 996                                 goto notify;
 997                 }
 998         } else {
 999                 ret = ocelot_netdevice_port_event(dev, event, info);
1000         }
1001
1002 notify:
1003         return notifier_from_errno(ret);
1004 }
1005
1006 struct notifier_block ocelot_netdevice_nb __read_mostly = {
1007         .notifier_call = ocelot_netdevice_event,
1008 };
1009
1010 static int ocelot_switchdev_event(struct notifier_block *unused,
1011                                   unsigned long event, void *ptr)
1012 {
1013         struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
1014         int err;
1015
1016         switch (event) {
1017         case SWITCHDEV_PORT_ATTR_SET:
1018                 err = switchdev_handle_port_attr_set(dev, ptr,
1019                                                      ocelot_netdevice_dev_check,
1020                                                      ocelot_port_attr_set);
1021                 return notifier_from_errno(err);
1022         }
1023
1024         return NOTIFY_DONE;
1025 }
1026
1027 struct notifier_block ocelot_switchdev_nb __read_mostly = {
1028         .notifier_call = ocelot_switchdev_event,
1029 };
1030
1031 static int ocelot_switchdev_blocking_event(struct notifier_block *unused,
1032                                            unsigned long event, void *ptr)
1033 {
1034         struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
1035         int err;
1036
1037         switch (event) {
1038                 /* Blocking events. */
1039         case SWITCHDEV_PORT_OBJ_ADD:
1040                 err = switchdev_handle_port_obj_add(dev, ptr,
1041                                                     ocelot_netdevice_dev_check,
1042                                                     ocelot_port_obj_add);
1043                 return notifier_from_errno(err);
1044         case SWITCHDEV_PORT_OBJ_DEL:
1045                 err = switchdev_handle_port_obj_del(dev, ptr,
1046                                                     ocelot_netdevice_dev_check,
1047                                                     ocelot_port_obj_del);
1048                 return notifier_from_errno(err);
1049         case SWITCHDEV_PORT_ATTR_SET:
1050                 err = switchdev_handle_port_attr_set(dev, ptr,
1051                                                      ocelot_netdevice_dev_check,
1052                                                      ocelot_port_attr_set);
1053                 return notifier_from_errno(err);
1054         }
1055
1056         return NOTIFY_DONE;
1057 }
1058
1059 struct notifier_block ocelot_switchdev_blocking_nb __read_mostly = {
1060         .notifier_call = ocelot_switchdev_blocking_event,
1061 };
1062
1063 int ocelot_probe_port(struct ocelot *ocelot, int port, struct regmap *target,
1064                       struct phy_device *phy)
1065 {
1066         struct ocelot_port_private *priv;
1067         struct ocelot_port *ocelot_port;
1068         struct net_device *dev;
1069         int err;
1070
1071         dev = alloc_etherdev(sizeof(struct ocelot_port_private));
1072         if (!dev)
1073                 return -ENOMEM;
1074         SET_NETDEV_DEV(dev, ocelot->dev);
1075         priv = netdev_priv(dev);
1076         priv->dev = dev;
1077         priv->phy = phy;
1078         priv->chip_port = port;
1079         ocelot_port = &priv->port;
1080         ocelot_port->ocelot = ocelot;
1081         ocelot_port->target = target;
1082         ocelot->ports[port] = ocelot_port;
1083
1084         dev->netdev_ops = &ocelot_port_netdev_ops;
1085         dev->ethtool_ops = &ocelot_ethtool_ops;
1086
1087         dev->hw_features |= NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_RXFCS |
1088                 NETIF_F_HW_TC;
1089         dev->features |= NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_HW_TC;
1090
1091         memcpy(dev->dev_addr, ocelot->base_mac, ETH_ALEN);
1092         dev->dev_addr[ETH_ALEN - 1] += port;
1093         ocelot_mact_learn(ocelot, PGID_CPU, dev->dev_addr,
1094                           ocelot_port->pvid_vlan.vid, ENTRYTYPE_LOCKED);
1095
1096         ocelot_init_port(ocelot, port);
1097
1098         err = register_netdev(dev);
1099         if (err) {
1100                 dev_err(ocelot->dev, "register_netdev failed\n");
1101                 free_netdev(dev);
1102         }
1103
1104         return err;
1105 }