drivers/net/ethernet/mscc/ocelot_net.c

   1 // SPDX-License-Identifier: (GPL-2.0 OR MIT)
   2 /* Microsemi Ocelot Switch driver
   3  *
   4  * This contains glue logic between the switchdev driver operations and the
   5  * mscc_ocelot_switch_lib.
   6  *
   7  * Copyright (c) 2017, 2019 Microsemi Corporation
   8  * Copyright 2020-2021 NXP Semiconductors
   9  */
  10
  11 #include <linux/if_bridge.h>
  12 #include <linux/of_net.h>
  13 #include <linux/phy/phy.h>
  14 #include <net/pkt_cls.h>
  15 #include "ocelot.h"
  16 #include "ocelot_vcap.h"
  17
  18 #define OCELOT_MAC_QUIRKS       OCELOT_QUIRK_QSGMII_PORTS_MUST_BE_UP
  19
  20 static struct ocelot *devlink_port_to_ocelot(struct devlink_port *dlp)
  21 {
  22         return devlink_priv(dlp->devlink);
  23 }
  24
  25 static int devlink_port_to_port(struct devlink_port *dlp)
  26 {
  27         struct ocelot *ocelot = devlink_port_to_ocelot(dlp);
  28
  29         return dlp - ocelot->devlink_ports;
  30 }
  31
  32 static int ocelot_devlink_sb_pool_get(struct devlink *dl,
  33                                       unsigned int sb_index, u16 pool_index,
  34                                       struct devlink_sb_pool_info *pool_info)
  35 {
  36         struct ocelot *ocelot = devlink_priv(dl);
  37
  38         return ocelot_sb_pool_get(ocelot, sb_index, pool_index, pool_info);
  39 }
  40
  41 static int ocelot_devlink_sb_pool_set(struct devlink *dl, unsigned int sb_index,
  42                                       u16 pool_index, u32 size,
  43                                       enum devlink_sb_threshold_type threshold_type,
  44                                       struct netlink_ext_ack *extack)
  45 {
  46         struct ocelot *ocelot = devlink_priv(dl);
  47
  48         return ocelot_sb_pool_set(ocelot, sb_index, pool_index, size,
  49                                   threshold_type, extack);
  50 }
  51
  52 static int ocelot_devlink_sb_port_pool_get(struct devlink_port *dlp,
  53                                            unsigned int sb_index, u16 pool_index,
  54                                            u32 *p_threshold)
  55 {
  56         struct ocelot *ocelot = devlink_port_to_ocelot(dlp);
  57         int port = devlink_port_to_port(dlp);
  58
  59         return ocelot_sb_port_pool_get(ocelot, port, sb_index, pool_index,
  60                                        p_threshold);
  61 }
  62
  63 static int ocelot_devlink_sb_port_pool_set(struct devlink_port *dlp,
  64                                            unsigned int sb_index, u16 pool_index,
  65                                            u32 threshold,
  66                                            struct netlink_ext_ack *extack)
  67 {
  68         struct ocelot *ocelot = devlink_port_to_ocelot(dlp);
  69         int port = devlink_port_to_port(dlp);
  70
  71         return ocelot_sb_port_pool_set(ocelot, port, sb_index, pool_index,
  72                                        threshold, extack);
  73 }
  74
  75 static int
  76 ocelot_devlink_sb_tc_pool_bind_get(struct devlink_port *dlp,
  77                                    unsigned int sb_index, u16 tc_index,
  78                                    enum devlink_sb_pool_type pool_type,
  79                                    u16 *p_pool_index, u32 *p_threshold)
  80 {
  81         struct ocelot *ocelot = devlink_port_to_ocelot(dlp);
  82         int port = devlink_port_to_port(dlp);
  83
  84         return ocelot_sb_tc_pool_bind_get(ocelot, port, sb_index, tc_index,
  85                                           pool_type, p_pool_index,
  86                                           p_threshold);
  87 }
  88
  89 static int
  90 ocelot_devlink_sb_tc_pool_bind_set(struct devlink_port *dlp,
  91                                    unsigned int sb_index, u16 tc_index,
  92                                    enum devlink_sb_pool_type pool_type,
  93                                    u16 pool_index, u32 threshold,
  94                                    struct netlink_ext_ack *extack)
  95 {
  96         struct ocelot *ocelot = devlink_port_to_ocelot(dlp);
  97         int port = devlink_port_to_port(dlp);
  98
  99         return ocelot_sb_tc_pool_bind_set(ocelot, port, sb_index, tc_index,
 100                                           pool_type, pool_index, threshold,
 101                                           extack);
 102 }
 103
 104 static int ocelot_devlink_sb_occ_snapshot(struct devlink *dl,
 105                                           unsigned int sb_index)
 106 {
 107         struct ocelot *ocelot = devlink_priv(dl);
 108
 109         return ocelot_sb_occ_snapshot(ocelot, sb_index);
 110 }
 111
 112 static int ocelot_devlink_sb_occ_max_clear(struct devlink *dl,
 113                                            unsigned int sb_index)
 114 {
 115         struct ocelot *ocelot = devlink_priv(dl);
 116
 117         return ocelot_sb_occ_max_clear(ocelot, sb_index);
 118 }
 119
 120 static int ocelot_devlink_sb_occ_port_pool_get(struct devlink_port *dlp,
 121                                                unsigned int sb_index,
 122                                                u16 pool_index, u32 *p_cur,
 123                                                u32 *p_max)
 124 {
 125         struct ocelot *ocelot = devlink_port_to_ocelot(dlp);
 126         int port = devlink_port_to_port(dlp);
 127
 128         return ocelot_sb_occ_port_pool_get(ocelot, port, sb_index, pool_index,
 129                                            p_cur, p_max);
 130 }
 131
 132 static int
 133 ocelot_devlink_sb_occ_tc_port_bind_get(struct devlink_port *dlp,
 134                                        unsigned int sb_index, u16 tc_index,
 135                                        enum devlink_sb_pool_type pool_type,
 136                                        u32 *p_cur, u32 *p_max)
 137 {
 138         struct ocelot *ocelot = devlink_port_to_ocelot(dlp);
 139         int port = devlink_port_to_port(dlp);
 140
 141         return ocelot_sb_occ_tc_port_bind_get(ocelot, port, sb_index,
 142                                               tc_index, pool_type,
 143                                               p_cur, p_max);
 144 }
 145
 146 const struct devlink_ops ocelot_devlink_ops = {
 147         .sb_pool_get                    = ocelot_devlink_sb_pool_get,
 148         .sb_pool_set                    = ocelot_devlink_sb_pool_set,
 149         .sb_port_pool_get               = ocelot_devlink_sb_port_pool_get,
 150         .sb_port_pool_set               = ocelot_devlink_sb_port_pool_set,
 151         .sb_tc_pool_bind_get            = ocelot_devlink_sb_tc_pool_bind_get,
 152         .sb_tc_pool_bind_set            = ocelot_devlink_sb_tc_pool_bind_set,
 153         .sb_occ_snapshot                = ocelot_devlink_sb_occ_snapshot,
 154         .sb_occ_max_clear               = ocelot_devlink_sb_occ_max_clear,
 155         .sb_occ_port_pool_get           = ocelot_devlink_sb_occ_port_pool_get,
 156         .sb_occ_tc_port_bind_get        = ocelot_devlink_sb_occ_tc_port_bind_get,
 157 };
 158
 159 int ocelot_port_devlink_init(struct ocelot *ocelot, int port,
 160                              enum devlink_port_flavour flavour)
 161 {
 162         struct devlink_port *dlp = &ocelot->devlink_ports[port];
 163         int id_len = sizeof(ocelot->base_mac);
 164         struct devlink *dl = ocelot->devlink;
 165         struct devlink_port_attrs attrs = {};
 166
 167         memset(dlp, 0, sizeof(*dlp));
 168         memcpy(attrs.switch_id.id, &ocelot->base_mac, id_len);
 169         attrs.switch_id.id_len = id_len;
 170         attrs.phys.port_number = port;
 171         attrs.flavour = flavour;
 172
 173         devlink_port_attrs_set(dlp, &attrs);
 174
 175         return devlink_port_register(dl, dlp, port);
 176 }
 177
 178 void ocelot_port_devlink_teardown(struct ocelot *ocelot, int port)
 179 {
 180         struct devlink_port *dlp = &ocelot->devlink_ports[port];
 181
 182         devlink_port_unregister(dlp);
 183 }
 184
 185 static struct devlink_port *ocelot_get_devlink_port(struct net_device *dev)
 186 {
 187         struct ocelot_port_private *priv = netdev_priv(dev);
 188         struct ocelot *ocelot = priv->port.ocelot;
 189         int port = priv->chip_port;
 190
 191         return &ocelot->devlink_ports[port];
 192 }
 193
 194 int ocelot_setup_tc_cls_flower(struct ocelot_port_private *priv,
 195                                struct flow_cls_offload *f,
 196                                bool ingress)
 197 {
 198         struct ocelot *ocelot = priv->port.ocelot;
 199         int port = priv->chip_port;
 200
 201         if (!ingress)
 202                 return -EOPNOTSUPP;
 203
 204         switch (f->command) {
 205         case FLOW_CLS_REPLACE:
 206                 return ocelot_cls_flower_replace(ocelot, port, f, ingress);
 207         case FLOW_CLS_DESTROY:
 208                 return ocelot_cls_flower_destroy(ocelot, port, f, ingress);
 209         case FLOW_CLS_STATS:
 210                 return ocelot_cls_flower_stats(ocelot, port, f, ingress);
 211         default:
 212                 return -EOPNOTSUPP;
 213         }
 214 }
 215
 216 static int ocelot_setup_tc_cls_matchall(struct ocelot_port_private *priv,
 217                                         struct tc_cls_matchall_offload *f,
 218                                         bool ingress)
 219 {
 220         struct netlink_ext_ack *extack = f->common.extack;
 221         struct ocelot *ocelot = priv->port.ocelot;
 222         struct ocelot_policer pol = { 0 };
 223         struct flow_action_entry *action;
 224         int port = priv->chip_port;
 225         int err;
 226
 227         if (!ingress) {
 228                 NL_SET_ERR_MSG_MOD(extack, "Only ingress is supported");
 229                 return -EOPNOTSUPP;
 230         }
 231
 232         switch (f->command) {
 233         case TC_CLSMATCHALL_REPLACE:
 234                 if (!flow_offload_has_one_action(&f->rule->action)) {
 235                         NL_SET_ERR_MSG_MOD(extack,
 236                                            "Only one action is supported");
 237                         return -EOPNOTSUPP;
 238                 }
 239
 240                 if (priv->tc.block_shared) {
 241                         NL_SET_ERR_MSG_MOD(extack,
 242                                            "Rate limit is not supported on shared blocks");
 243                         return -EOPNOTSUPP;
 244                 }
 245
 246                 action = &f->rule->action.entries[0];
 247
 248                 if (action->id != FLOW_ACTION_POLICE) {
 249                         NL_SET_ERR_MSG_MOD(extack, "Unsupported action");
 250                         return -EOPNOTSUPP;
 251                 }
 252
 253                 if (priv->tc.police_id && priv->tc.police_id != f->cookie) {
 254                         NL_SET_ERR_MSG_MOD(extack,
 255                                            "Only one policer per port is supported");
 256                         return -EEXIST;
 257                 }
 258
 259                 if (action->police.rate_pkt_ps) {
 260                         NL_SET_ERR_MSG_MOD(extack,
 261                                            "QoS offload not support packets per second");
 262                         return -EOPNOTSUPP;
 263                 }
 264
 265                 pol.rate = (u32)div_u64(action->police.rate_bytes_ps, 1000) * 8;
 266                 pol.burst = action->police.burst;
 267
 268                 err = ocelot_port_policer_add(ocelot, port, &pol);
 269                 if (err) {
 270                         NL_SET_ERR_MSG_MOD(extack, "Could not add policer");
 271                         return err;
 272                 }
 273
 274                 priv->tc.police_id = f->cookie;
 275                 priv->tc.offload_cnt++;
 276                 return 0;
 277         case TC_CLSMATCHALL_DESTROY:
 278                 if (priv->tc.police_id != f->cookie)
 279                         return -ENOENT;
 280
 281                 err = ocelot_port_policer_del(ocelot, port);
 282                 if (err) {
 283                         NL_SET_ERR_MSG_MOD(extack,
 284                                            "Could not delete policer");
 285                         return err;
 286                 }
 287                 priv->tc.police_id = 0;
 288                 priv->tc.offload_cnt--;
 289                 return 0;
 290         case TC_CLSMATCHALL_STATS:
 291         default:
 292                 return -EOPNOTSUPP;
 293         }
 294 }
 295
 296 static int ocelot_setup_tc_block_cb(enum tc_setup_type type,
 297                                     void *type_data,
 298                                     void *cb_priv, bool ingress)
 299 {
 300         struct ocelot_port_private *priv = cb_priv;
 301
 302         if (!tc_cls_can_offload_and_chain0(priv->dev, type_data))
 303                 return -EOPNOTSUPP;
 304
 305         switch (type) {
 306         case TC_SETUP_CLSMATCHALL:
 307                 return ocelot_setup_tc_cls_matchall(priv, type_data, ingress);
 308         case TC_SETUP_CLSFLOWER:
 309                 return ocelot_setup_tc_cls_flower(priv, type_data, ingress);
 310         default:
 311                 return -EOPNOTSUPP;
 312         }
 313 }
 314
 315 static int ocelot_setup_tc_block_cb_ig(enum tc_setup_type type,
 316                                        void *type_data,
 317                                        void *cb_priv)
 318 {
 319         return ocelot_setup_tc_block_cb(type, type_data,
 320                                         cb_priv, true);
 321 }
 322
 323 static int ocelot_setup_tc_block_cb_eg(enum tc_setup_type type,
 324                                        void *type_data,
 325                                        void *cb_priv)
 326 {
 327         return ocelot_setup_tc_block_cb(type, type_data,
 328                                         cb_priv, false);
 329 }
 330
 331 static LIST_HEAD(ocelot_block_cb_list);
 332
 333 static int ocelot_setup_tc_block(struct ocelot_port_private *priv,
 334                                  struct flow_block_offload *f)
 335 {
 336         struct flow_block_cb *block_cb;
 337         flow_setup_cb_t *cb;
 338
 339         if (f->binder_type == FLOW_BLOCK_BINDER_TYPE_CLSACT_INGRESS) {
 340                 cb = ocelot_setup_tc_block_cb_ig;
 341                 priv->tc.block_shared = f->block_shared;
 342         } else if (f->binder_type == FLOW_BLOCK_BINDER_TYPE_CLSACT_EGRESS) {
 343                 cb = ocelot_setup_tc_block_cb_eg;
 344         } else {
 345                 return -EOPNOTSUPP;
 346         }
 347
 348         f->driver_block_list = &ocelot_block_cb_list;
 349
 350         switch (f->command) {
 351         case FLOW_BLOCK_BIND:
 352                 if (flow_block_cb_is_busy(cb, priv, &ocelot_block_cb_list))
 353                         return -EBUSY;
 354
 355                 block_cb = flow_block_cb_alloc(cb, priv, priv, NULL);
 356                 if (IS_ERR(block_cb))
 357                         return PTR_ERR(block_cb);
 358
 359                 flow_block_cb_add(block_cb, f);
 360                 list_add_tail(&block_cb->driver_list, f->driver_block_list);
 361                 return 0;
 362         case FLOW_BLOCK_UNBIND:
 363                 block_cb = flow_block_cb_lookup(f->block, cb, priv);
 364                 if (!block_cb)
 365                         return -ENOENT;
 366
 367                 flow_block_cb_remove(block_cb, f);
 368                 list_del(&block_cb->driver_list);
 369                 return 0;
 370         default:
 371                 return -EOPNOTSUPP;
 372         }
 373 }
 374
 375 static int ocelot_setup_tc(struct net_device *dev, enum tc_setup_type type,
 376                            void *type_data)
 377 {
 378         struct ocelot_port_private *priv = netdev_priv(dev);
 379
 380         switch (type) {
 381         case TC_SETUP_BLOCK:
 382                 return ocelot_setup_tc_block(priv, type_data);
 383         default:
 384                 return -EOPNOTSUPP;
 385         }
 386         return 0;
 387 }
 388
 389 static int ocelot_vlan_vid_add(struct net_device *dev, u16 vid, bool pvid,
 390                                bool untagged)
 391 {
 392         struct ocelot_port_private *priv = netdev_priv(dev);
 393         struct ocelot_port *ocelot_port = &priv->port;
 394         struct ocelot *ocelot = ocelot_port->ocelot;
 395         int port = priv->chip_port;
 396         int ret;
 397
 398         ret = ocelot_vlan_add(ocelot, port, vid, pvid, untagged);
 399         if (ret)
 400                 return ret;
 401
 402         /* Add the port MAC address to with the right VLAN information */
 403         ocelot_mact_learn(ocelot, PGID_CPU, dev->dev_addr, vid,
 404                           ENTRYTYPE_LOCKED);
 405
 406         return 0;
 407 }
 408
 409 static int ocelot_vlan_vid_del(struct net_device *dev, u16 vid)
 410 {
 411         struct ocelot_port_private *priv = netdev_priv(dev);
 412         struct ocelot *ocelot = priv->port.ocelot;
 413         int port = priv->chip_port;
 414         int ret;
 415
 416         /* 8021q removes VID 0 on module unload for all interfaces
 417          * with VLAN filtering feature. We need to keep it to receive
 418          * untagged traffic.
 419          */
 420         if (vid == 0)
 421                 return 0;
 422
 423         ret = ocelot_vlan_del(ocelot, port, vid);
 424         if (ret)
 425                 return ret;
 426
 427         /* Del the port MAC address to with the right VLAN information */
 428         ocelot_mact_forget(ocelot, dev->dev_addr, vid);
 429
 430         return 0;
 431 }
 432
 433 static int ocelot_port_open(struct net_device *dev)
 434 {
 435         struct ocelot_port_private *priv = netdev_priv(dev);
 436
 437         phylink_start(priv->phylink);
 438
 439         return 0;
 440 }
 441
 442 static int ocelot_port_stop(struct net_device *dev)
 443 {
 444         struct ocelot_port_private *priv = netdev_priv(dev);
 445
 446         phylink_stop(priv->phylink);
 447
 448         return 0;
 449 }
 450
 451 static netdev_tx_t ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
 452 {
 453         struct ocelot_port_private *priv = netdev_priv(dev);
 454         struct ocelot_port *ocelot_port = &priv->port;
 455         struct ocelot *ocelot = ocelot_port->ocelot;
 456         int port = priv->chip_port;
 457         u32 rew_op = 0;
 458
 459         if (!ocelot_can_inject(ocelot, 0))
 460                 return NETDEV_TX_BUSY;
 461
 462         /* Check if timestamping is needed */
 463         if (ocelot->ptp && (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP)) {
 464                 struct sk_buff *clone = NULL;
 465
 466                 if (ocelot_port_txtstamp_request(ocelot, port, skb, &clone)) {
 467                         kfree_skb(skb);
 468                         return NETDEV_TX_OK;
 469                 }
 470
 471                 if (clone)
 472                         OCELOT_SKB_CB(skb)->clone = clone;
 473
 474                 rew_op = ocelot_ptp_rew_op(skb);
 475         }
 476
 477         ocelot_port_inject_frame(ocelot, port, 0, rew_op, skb);
 478
 479         kfree_skb(skb);
 480
 481         return NETDEV_TX_OK;
 482 }
 483
 484 enum ocelot_action_type {
 485         OCELOT_MACT_LEARN,
 486         OCELOT_MACT_FORGET,
 487 };
 488
 489 struct ocelot_mact_work_ctx {
 490         struct work_struct work;
 491         struct ocelot *ocelot;
 492         enum ocelot_action_type type;
 493         union {
 494                 /* OCELOT_MACT_LEARN */
 495                 struct {
 496                         unsigned char addr[ETH_ALEN];
 497                         u16 vid;
 498                         enum macaccess_entry_type entry_type;
 499                         int pgid;
 500                 } learn;
 501                 /* OCELOT_MACT_FORGET */
 502                 struct {
 503                         unsigned char addr[ETH_ALEN];
 504                         u16 vid;
 505                 } forget;
 506         };
 507 };
 508
 509 #define ocelot_work_to_ctx(x) \
 510         container_of((x), struct ocelot_mact_work_ctx, work)
 511
 512 static void ocelot_mact_work(struct work_struct *work)
 513 {
 514         struct ocelot_mact_work_ctx *w = ocelot_work_to_ctx(work);
 515         struct ocelot *ocelot = w->ocelot;
 516
 517         switch (w->type) {
 518         case OCELOT_MACT_LEARN:
 519                 ocelot_mact_learn(ocelot, w->learn.pgid, w->learn.addr,
 520                                   w->learn.vid, w->learn.entry_type);
 521                 break;
 522         case OCELOT_MACT_FORGET:
 523                 ocelot_mact_forget(ocelot, w->forget.addr, w->forget.vid);
 524                 break;
 525         default:
 526                 break;
 527         }
 528
 529         kfree(w);
 530 }
 531
 532 static int ocelot_enqueue_mact_action(struct ocelot *ocelot,
 533                                       const struct ocelot_mact_work_ctx *ctx)
 534 {
 535         struct ocelot_mact_work_ctx *w = kmemdup(ctx, sizeof(*w), GFP_ATOMIC);
 536
 537         if (!w)
 538                 return -ENOMEM;
 539
 540         w->ocelot = ocelot;
 541         INIT_WORK(&w->work, ocelot_mact_work);
 542         queue_work(ocelot->owq, &w->work);
 543
 544         return 0;
 545 }
 546
 547 static int ocelot_mc_unsync(struct net_device *dev, const unsigned char *addr)
 548 {
 549         struct ocelot_port_private *priv = netdev_priv(dev);
 550         struct ocelot_port *ocelot_port = &priv->port;
 551         struct ocelot *ocelot = ocelot_port->ocelot;
 552         struct ocelot_mact_work_ctx w;
 553
 554         ether_addr_copy(w.forget.addr, addr);
 555         w.forget.vid = ocelot_port->pvid_vlan.vid;
 556         w.type = OCELOT_MACT_FORGET;
 557
 558         return ocelot_enqueue_mact_action(ocelot, &w);
 559 }
 560
 561 static int ocelot_mc_sync(struct net_device *dev, const unsigned char *addr)
 562 {
 563         struct ocelot_port_private *priv = netdev_priv(dev);
 564         struct ocelot_port *ocelot_port = &priv->port;
 565         struct ocelot *ocelot = ocelot_port->ocelot;
 566         struct ocelot_mact_work_ctx w;
 567
 568         ether_addr_copy(w.learn.addr, addr);
 569         w.learn.vid = ocelot_port->pvid_vlan.vid;
 570         w.learn.pgid = PGID_CPU;
 571         w.learn.entry_type = ENTRYTYPE_LOCKED;
 572         w.type = OCELOT_MACT_LEARN;
 573
 574         return ocelot_enqueue_mact_action(ocelot, &w);
 575 }
 576
 577 static void ocelot_set_rx_mode(struct net_device *dev)
 578 {
 579         struct ocelot_port_private *priv = netdev_priv(dev);
 580         struct ocelot *ocelot = priv->port.ocelot;
 581         u32 val;
 582         int i;
 583
 584         /* This doesn't handle promiscuous mode because the bridge core is
 585          * setting IFF_PROMISC on all slave interfaces and all frames would be
 586          * forwarded to the CPU port.
 587          */
 588         val = GENMASK(ocelot->num_phys_ports - 1, 0);
 589         for_each_nonreserved_multicast_dest_pgid(ocelot, i)
 590                 ocelot_write_rix(ocelot, val, ANA_PGID_PGID, i);
 591
 592         __dev_mc_sync(dev, ocelot_mc_sync, ocelot_mc_unsync);
 593 }
 594
 595 static int ocelot_port_set_mac_address(struct net_device *dev, void *p)
 596 {
 597         struct ocelot_port_private *priv = netdev_priv(dev);
 598         struct ocelot_port *ocelot_port = &priv->port;
 599         struct ocelot *ocelot = ocelot_port->ocelot;
 600         const struct sockaddr *addr = p;
 601
 602         /* Learn the new net device MAC address in the mac table. */
 603         ocelot_mact_learn(ocelot, PGID_CPU, addr->sa_data,
 604                           ocelot_port->pvid_vlan.vid, ENTRYTYPE_LOCKED);
 605         /* Then forget the previous one. */
 606         ocelot_mact_forget(ocelot, dev->dev_addr, ocelot_port->pvid_vlan.vid);
 607
 608         ether_addr_copy(dev->dev_addr, addr->sa_data);
 609         return 0;
 610 }
 611
 612 static void ocelot_get_stats64(struct net_device *dev,
 613                                struct rtnl_link_stats64 *stats)
 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         /* Configure the port to read the stats from */
 620         ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(port),
 621                      SYS_STAT_CFG);
 622
 623         /* Get Rx stats */
 624         stats->rx_bytes = ocelot_read(ocelot, SYS_COUNT_RX_OCTETS);
 625         stats->rx_packets = ocelot_read(ocelot, SYS_COUNT_RX_SHORTS) +
 626                             ocelot_read(ocelot, SYS_COUNT_RX_FRAGMENTS) +
 627                             ocelot_read(ocelot, SYS_COUNT_RX_JABBERS) +
 628                             ocelot_read(ocelot, SYS_COUNT_RX_LONGS) +
 629                             ocelot_read(ocelot, SYS_COUNT_RX_64) +
 630                             ocelot_read(ocelot, SYS_COUNT_RX_65_127) +
 631                             ocelot_read(ocelot, SYS_COUNT_RX_128_255) +
 632                             ocelot_read(ocelot, SYS_COUNT_RX_256_1023) +
 633                             ocelot_read(ocelot, SYS_COUNT_RX_1024_1526) +
 634                             ocelot_read(ocelot, SYS_COUNT_RX_1527_MAX);
 635         stats->multicast = ocelot_read(ocelot, SYS_COUNT_RX_MULTICAST);
 636         stats->rx_dropped = dev->stats.rx_dropped;
 637
 638         /* Get Tx stats */
 639         stats->tx_bytes = ocelot_read(ocelot, SYS_COUNT_TX_OCTETS);
 640         stats->tx_packets = ocelot_read(ocelot, SYS_COUNT_TX_64) +
 641                             ocelot_read(ocelot, SYS_COUNT_TX_65_127) +
 642                             ocelot_read(ocelot, SYS_COUNT_TX_128_511) +
 643                             ocelot_read(ocelot, SYS_COUNT_TX_512_1023) +
 644                             ocelot_read(ocelot, SYS_COUNT_TX_1024_1526) +
 645                             ocelot_read(ocelot, SYS_COUNT_TX_1527_MAX);
 646         stats->tx_dropped = ocelot_read(ocelot, SYS_COUNT_TX_DROPS) +
 647                             ocelot_read(ocelot, SYS_COUNT_TX_AGING);
 648         stats->collisions = ocelot_read(ocelot, SYS_COUNT_TX_COLLISION);
 649 }
 650
 651 static int ocelot_port_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
 652                                struct net_device *dev,
 653                                const unsigned char *addr,
 654                                u16 vid, u16 flags,
 655                                struct netlink_ext_ack *extack)
 656 {
 657         struct ocelot_port_private *priv = netdev_priv(dev);
 658         struct ocelot *ocelot = priv->port.ocelot;
 659         int port = priv->chip_port;
 660
 661         return ocelot_fdb_add(ocelot, port, addr, vid);
 662 }
 663
 664 static int ocelot_port_fdb_del(struct ndmsg *ndm, struct nlattr *tb[],
 665                                struct net_device *dev,
 666                                const unsigned char *addr, u16 vid)
 667 {
 668         struct ocelot_port_private *priv = netdev_priv(dev);
 669         struct ocelot *ocelot = priv->port.ocelot;
 670         int port = priv->chip_port;
 671
 672         return ocelot_fdb_del(ocelot, port, addr, vid);
 673 }
 674
 675 static int ocelot_port_fdb_dump(struct sk_buff *skb,
 676                                 struct netlink_callback *cb,
 677                                 struct net_device *dev,
 678                                 struct net_device *filter_dev, int *idx)
 679 {
 680         struct ocelot_port_private *priv = netdev_priv(dev);
 681         struct ocelot *ocelot = priv->port.ocelot;
 682         struct ocelot_dump_ctx dump = {
 683                 .dev = dev,
 684                 .skb = skb,
 685                 .cb = cb,
 686                 .idx = *idx,
 687         };
 688         int port = priv->chip_port;
 689         int ret;
 690
 691         ret = ocelot_fdb_dump(ocelot, port, ocelot_port_fdb_do_dump, &dump);
 692
 693         *idx = dump.idx;
 694
 695         return ret;
 696 }
 697
 698 static int ocelot_vlan_rx_add_vid(struct net_device *dev, __be16 proto,
 699                                   u16 vid)
 700 {
 701         return ocelot_vlan_vid_add(dev, vid, false, false);
 702 }
 703
 704 static int ocelot_vlan_rx_kill_vid(struct net_device *dev, __be16 proto,
 705                                    u16 vid)
 706 {
 707         return ocelot_vlan_vid_del(dev, vid);
 708 }
 709
 710 static void ocelot_vlan_mode(struct ocelot *ocelot, int port,
 711                              netdev_features_t features)
 712 {
 713         u32 val;
 714
 715         /* Filtering */
 716         val = ocelot_read(ocelot, ANA_VLANMASK);
 717         if (features & NETIF_F_HW_VLAN_CTAG_FILTER)
 718                 val |= BIT(port);
 719         else
 720                 val &= ~BIT(port);
 721         ocelot_write(ocelot, val, ANA_VLANMASK);
 722 }
 723
 724 static int ocelot_set_features(struct net_device *dev,
 725                                netdev_features_t features)
 726 {
 727         netdev_features_t changed = dev->features ^ features;
 728         struct ocelot_port_private *priv = netdev_priv(dev);
 729         struct ocelot *ocelot = priv->port.ocelot;
 730         int port = priv->chip_port;
 731
 732         if ((dev->features & NETIF_F_HW_TC) > (features & NETIF_F_HW_TC) &&
 733             priv->tc.offload_cnt) {
 734                 netdev_err(dev,
 735                            "Cannot disable HW TC offload while offloads active\n");
 736                 return -EBUSY;
 737         }
 738
 739         if (changed & NETIF_F_HW_VLAN_CTAG_FILTER)
 740                 ocelot_vlan_mode(ocelot, port, features);
 741
 742         return 0;
 743 }
 744
 745 static int ocelot_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 746 {
 747         struct ocelot_port_private *priv = netdev_priv(dev);
 748         struct ocelot *ocelot = priv->port.ocelot;
 749         int port = priv->chip_port;
 750
 751         /* If the attached PHY device isn't capable of timestamping operations,
 752          * use our own (when possible).
 753          */
 754         if (!phy_has_hwtstamp(dev->phydev) && ocelot->ptp) {
 755                 switch (cmd) {
 756                 case SIOCSHWTSTAMP:
 757                         return ocelot_hwstamp_set(ocelot, port, ifr);
 758                 case SIOCGHWTSTAMP:
 759                         return ocelot_hwstamp_get(ocelot, port, ifr);
 760                 }
 761         }
 762
 763         return phy_mii_ioctl(dev->phydev, ifr, cmd);
 764 }
 765
 766 static const struct net_device_ops ocelot_port_netdev_ops = {
 767         .ndo_open                       = ocelot_port_open,
 768         .ndo_stop                       = ocelot_port_stop,
 769         .ndo_start_xmit                 = ocelot_port_xmit,
 770         .ndo_set_rx_mode                = ocelot_set_rx_mode,
 771         .ndo_set_mac_address            = ocelot_port_set_mac_address,
 772         .ndo_get_stats64                = ocelot_get_stats64,
 773         .ndo_fdb_add                    = ocelot_port_fdb_add,
 774         .ndo_fdb_del                    = ocelot_port_fdb_del,
 775         .ndo_fdb_dump                   = ocelot_port_fdb_dump,
 776         .ndo_vlan_rx_add_vid            = ocelot_vlan_rx_add_vid,
 777         .ndo_vlan_rx_kill_vid           = ocelot_vlan_rx_kill_vid,
 778         .ndo_set_features               = ocelot_set_features,
 779         .ndo_setup_tc                   = ocelot_setup_tc,
 780         .ndo_eth_ioctl                  = ocelot_ioctl,
 781         .ndo_get_devlink_port           = ocelot_get_devlink_port,
 782 };
 783
 784 struct net_device *ocelot_port_to_netdev(struct ocelot *ocelot, int port)
 785 {
 786         struct ocelot_port *ocelot_port = ocelot->ports[port];
 787         struct ocelot_port_private *priv;
 788
 789         if (!ocelot_port)
 790                 return NULL;
 791
 792         priv = container_of(ocelot_port, struct ocelot_port_private, port);
 793
 794         return priv->dev;
 795 }
 796
 797 /* Checks if the net_device instance given to us originates from our driver */
 798 static bool ocelot_netdevice_dev_check(const struct net_device *dev)
 799 {
 800         return dev->netdev_ops == &ocelot_port_netdev_ops;
 801 }
 802
 803 int ocelot_netdev_to_port(struct net_device *dev)
 804 {
 805         struct ocelot_port_private *priv;
 806
 807         if (!dev || !ocelot_netdevice_dev_check(dev))
 808                 return -EINVAL;
 809
 810         priv = netdev_priv(dev);
 811
 812         return priv->chip_port;
 813 }
 814
 815 static void ocelot_port_get_strings(struct net_device *netdev, u32 sset,
 816                                     u8 *data)
 817 {
 818         struct ocelot_port_private *priv = netdev_priv(netdev);
 819         struct ocelot *ocelot = priv->port.ocelot;
 820         int port = priv->chip_port;
 821
 822         ocelot_get_strings(ocelot, port, sset, data);
 823 }
 824
 825 static void ocelot_port_get_ethtool_stats(struct net_device *dev,
 826                                           struct ethtool_stats *stats,
 827                                           u64 *data)
 828 {
 829         struct ocelot_port_private *priv = netdev_priv(dev);
 830         struct ocelot *ocelot = priv->port.ocelot;
 831         int port = priv->chip_port;
 832
 833         ocelot_get_ethtool_stats(ocelot, port, data);
 834 }
 835
 836 static int ocelot_port_get_sset_count(struct net_device *dev, int sset)
 837 {
 838         struct ocelot_port_private *priv = netdev_priv(dev);
 839         struct ocelot *ocelot = priv->port.ocelot;
 840         int port = priv->chip_port;
 841
 842         return ocelot_get_sset_count(ocelot, port, sset);
 843 }
 844
 845 static int ocelot_port_get_ts_info(struct net_device *dev,
 846                                    struct ethtool_ts_info *info)
 847 {
 848         struct ocelot_port_private *priv = netdev_priv(dev);
 849         struct ocelot *ocelot = priv->port.ocelot;
 850         int port = priv->chip_port;
 851
 852         if (!ocelot->ptp)
 853                 return ethtool_op_get_ts_info(dev, info);
 854
 855         return ocelot_get_ts_info(ocelot, port, info);
 856 }
 857
 858 static const struct ethtool_ops ocelot_ethtool_ops = {
 859         .get_strings            = ocelot_port_get_strings,
 860         .get_ethtool_stats      = ocelot_port_get_ethtool_stats,
 861         .get_sset_count         = ocelot_port_get_sset_count,
 862         .get_link_ksettings     = phy_ethtool_get_link_ksettings,
 863         .set_link_ksettings     = phy_ethtool_set_link_ksettings,
 864         .get_ts_info            = ocelot_port_get_ts_info,
 865 };
 866
 867 static void ocelot_port_attr_stp_state_set(struct ocelot *ocelot, int port,
 868                                            u8 state)
 869 {
 870         ocelot_bridge_stp_state_set(ocelot, port, state);
 871 }
 872
 873 static void ocelot_port_attr_ageing_set(struct ocelot *ocelot, int port,
 874                                         unsigned long ageing_clock_t)
 875 {
 876         unsigned long ageing_jiffies = clock_t_to_jiffies(ageing_clock_t);
 877         u32 ageing_time = jiffies_to_msecs(ageing_jiffies);
 878
 879         ocelot_set_ageing_time(ocelot, ageing_time);
 880 }
 881
 882 static void ocelot_port_attr_mc_set(struct ocelot *ocelot, int port, bool mc)
 883 {
 884         u32 cpu_fwd_mcast = ANA_PORT_CPU_FWD_CFG_CPU_IGMP_REDIR_ENA |
 885                             ANA_PORT_CPU_FWD_CFG_CPU_MLD_REDIR_ENA |
 886                             ANA_PORT_CPU_FWD_CFG_CPU_IPMC_CTRL_COPY_ENA;
 887         u32 val = 0;
 888
 889         if (mc)
 890                 val = cpu_fwd_mcast;
 891
 892         ocelot_rmw_gix(ocelot, val, cpu_fwd_mcast,
 893                        ANA_PORT_CPU_FWD_CFG, port);
 894 }
 895
 896 static int ocelot_port_attr_set(struct net_device *dev, const void *ctx,
 897                                 const struct switchdev_attr *attr,
 898                                 struct netlink_ext_ack *extack)
 899 {
 900         struct ocelot_port_private *priv = netdev_priv(dev);
 901         struct ocelot *ocelot = priv->port.ocelot;
 902         int port = priv->chip_port;
 903         int err = 0;
 904
 905         if (ctx && ctx != priv)
 906                 return 0;
 907
 908         switch (attr->id) {
 909         case SWITCHDEV_ATTR_ID_PORT_STP_STATE:
 910                 ocelot_port_attr_stp_state_set(ocelot, port, attr->u.stp_state);
 911                 break;
 912         case SWITCHDEV_ATTR_ID_BRIDGE_AGEING_TIME:
 913                 ocelot_port_attr_ageing_set(ocelot, port, attr->u.ageing_time);
 914                 break;
 915         case SWITCHDEV_ATTR_ID_BRIDGE_VLAN_FILTERING:
 916                 ocelot_port_vlan_filtering(ocelot, port, attr->u.vlan_filtering,
 917                                            extack);
 918                 break;
 919         case SWITCHDEV_ATTR_ID_BRIDGE_MC_DISABLED:
 920                 ocelot_port_attr_mc_set(ocelot, port, !attr->u.mc_disabled);
 921                 break;
 922         case SWITCHDEV_ATTR_ID_PORT_PRE_BRIDGE_FLAGS:
 923                 err = ocelot_port_pre_bridge_flags(ocelot, port,
 924                                                    attr->u.brport_flags);
 925                 break;
 926         case SWITCHDEV_ATTR_ID_PORT_BRIDGE_FLAGS:
 927                 ocelot_port_bridge_flags(ocelot, port, attr->u.brport_flags);
 928                 break;
 929         default:
 930                 err = -EOPNOTSUPP;
 931                 break;
 932         }
 933
 934         return err;
 935 }
 936
 937 static int ocelot_vlan_vid_prepare(struct net_device *dev, u16 vid, bool pvid,
 938                                    bool untagged, struct netlink_ext_ack *extack)
 939 {
 940         struct ocelot_port_private *priv = netdev_priv(dev);
 941         struct ocelot_port *ocelot_port = &priv->port;
 942         struct ocelot *ocelot = ocelot_port->ocelot;
 943         int port = priv->chip_port;
 944
 945         return ocelot_vlan_prepare(ocelot, port, vid, pvid, untagged, extack);
 946 }
 947
 948 static int ocelot_port_obj_add_vlan(struct net_device *dev,
 949                                     const struct switchdev_obj_port_vlan *vlan,
 950                                     struct netlink_ext_ack *extack)
 951 {
 952         bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
 953         bool pvid = vlan->flags & BRIDGE_VLAN_INFO_PVID;
 954         int ret;
 955
 956         ret = ocelot_vlan_vid_prepare(dev, vlan->vid, pvid, untagged, extack);
 957         if (ret)
 958                 return ret;
 959
 960         return ocelot_vlan_vid_add(dev, vlan->vid, pvid, untagged);
 961 }
 962
 963 static int ocelot_port_obj_add_mdb(struct net_device *dev,
 964                                    const struct switchdev_obj_port_mdb *mdb)
 965 {
 966         struct ocelot_port_private *priv = netdev_priv(dev);
 967         struct ocelot_port *ocelot_port = &priv->port;
 968         struct ocelot *ocelot = ocelot_port->ocelot;
 969         int port = priv->chip_port;
 970
 971         return ocelot_port_mdb_add(ocelot, port, mdb);
 972 }
 973
 974 static int ocelot_port_obj_del_mdb(struct net_device *dev,
 975                                    const struct switchdev_obj_port_mdb *mdb)
 976 {
 977         struct ocelot_port_private *priv = netdev_priv(dev);
 978         struct ocelot_port *ocelot_port = &priv->port;
 979         struct ocelot *ocelot = ocelot_port->ocelot;
 980         int port = priv->chip_port;
 981
 982         return ocelot_port_mdb_del(ocelot, port, mdb);
 983 }
 984
 985 static int ocelot_port_obj_mrp_add(struct net_device *dev,
 986                                    const struct switchdev_obj_mrp *mrp)
 987 {
 988         struct ocelot_port_private *priv = netdev_priv(dev);
 989         struct ocelot_port *ocelot_port = &priv->port;
 990         struct ocelot *ocelot = ocelot_port->ocelot;
 991         int port = priv->chip_port;
 992
 993         return ocelot_mrp_add(ocelot, port, mrp);
 994 }
 995
 996 static int ocelot_port_obj_mrp_del(struct net_device *dev,
 997                                    const struct switchdev_obj_mrp *mrp)
 998 {
 999         struct ocelot_port_private *priv = netdev_priv(dev);
1000         struct ocelot_port *ocelot_port = &priv->port;
1001         struct ocelot *ocelot = ocelot_port->ocelot;
1002         int port = priv->chip_port;
1003
1004         return ocelot_mrp_del(ocelot, port, mrp);
1005 }
1006
1007 static int
1008 ocelot_port_obj_mrp_add_ring_role(struct net_device *dev,
1009                                   const struct switchdev_obj_ring_role_mrp *mrp)
1010 {
1011         struct ocelot_port_private *priv = netdev_priv(dev);
1012         struct ocelot_port *ocelot_port = &priv->port;
1013         struct ocelot *ocelot = ocelot_port->ocelot;
1014         int port = priv->chip_port;
1015
1016         return ocelot_mrp_add_ring_role(ocelot, port, mrp);
1017 }
1018
1019 static int
1020 ocelot_port_obj_mrp_del_ring_role(struct net_device *dev,
1021                                   const struct switchdev_obj_ring_role_mrp *mrp)
1022 {
1023         struct ocelot_port_private *priv = netdev_priv(dev);
1024         struct ocelot_port *ocelot_port = &priv->port;
1025         struct ocelot *ocelot = ocelot_port->ocelot;
1026         int port = priv->chip_port;
1027
1028         return ocelot_mrp_del_ring_role(ocelot, port, mrp);
1029 }
1030
1031 static int ocelot_port_obj_add(struct net_device *dev, const void *ctx,
1032                                const struct switchdev_obj *obj,
1033                                struct netlink_ext_ack *extack)
1034 {
1035         struct ocelot_port_private *priv = netdev_priv(dev);
1036         int ret = 0;
1037
1038         if (ctx && ctx != priv)
1039                 return 0;
1040
1041         switch (obj->id) {
1042         case SWITCHDEV_OBJ_ID_PORT_VLAN:
1043                 ret = ocelot_port_obj_add_vlan(dev,
1044                                                SWITCHDEV_OBJ_PORT_VLAN(obj),
1045                                                extack);
1046                 break;
1047         case SWITCHDEV_OBJ_ID_PORT_MDB:
1048                 ret = ocelot_port_obj_add_mdb(dev, SWITCHDEV_OBJ_PORT_MDB(obj));
1049                 break;
1050         case SWITCHDEV_OBJ_ID_MRP:
1051                 ret = ocelot_port_obj_mrp_add(dev, SWITCHDEV_OBJ_MRP(obj));
1052                 break;
1053         case SWITCHDEV_OBJ_ID_RING_ROLE_MRP:
1054                 ret = ocelot_port_obj_mrp_add_ring_role(dev,
1055                                                         SWITCHDEV_OBJ_RING_ROLE_MRP(obj));
1056                 break;
1057         default:
1058                 return -EOPNOTSUPP;
1059         }
1060
1061         return ret;
1062 }
1063
1064 static int ocelot_port_obj_del(struct net_device *dev, const void *ctx,
1065                                const struct switchdev_obj *obj)
1066 {
1067         struct ocelot_port_private *priv = netdev_priv(dev);
1068         int ret = 0;
1069
1070         if (ctx && ctx != priv)
1071                 return 0;
1072
1073         switch (obj->id) {
1074         case SWITCHDEV_OBJ_ID_PORT_VLAN:
1075                 ret = ocelot_vlan_vid_del(dev,
1076                                           SWITCHDEV_OBJ_PORT_VLAN(obj)->vid);
1077                 break;
1078         case SWITCHDEV_OBJ_ID_PORT_MDB:
1079                 ret = ocelot_port_obj_del_mdb(dev, SWITCHDEV_OBJ_PORT_MDB(obj));
1080                 break;
1081         case SWITCHDEV_OBJ_ID_MRP:
1082                 ret = ocelot_port_obj_mrp_del(dev, SWITCHDEV_OBJ_MRP(obj));
1083                 break;
1084         case SWITCHDEV_OBJ_ID_RING_ROLE_MRP:
1085                 ret = ocelot_port_obj_mrp_del_ring_role(dev,
1086                                                         SWITCHDEV_OBJ_RING_ROLE_MRP(obj));
1087                 break;
1088         default:
1089                 return -EOPNOTSUPP;
1090         }
1091
1092         return ret;
1093 }
1094
1095 static void ocelot_inherit_brport_flags(struct ocelot *ocelot, int port,
1096                                         struct net_device *brport_dev)
1097 {
1098         struct switchdev_brport_flags flags = {0};
1099         int flag;
1100
1101         flags.mask = BR_LEARNING | BR_FLOOD | BR_MCAST_FLOOD | BR_BCAST_FLOOD;
1102
1103         for_each_set_bit(flag, &flags.mask, 32)
1104                 if (br_port_flag_is_set(brport_dev, BIT(flag)))
1105                         flags.val |= BIT(flag);
1106
1107         ocelot_port_bridge_flags(ocelot, port, flags);
1108 }
1109
1110 static void ocelot_clear_brport_flags(struct ocelot *ocelot, int port)
1111 {
1112         struct switchdev_brport_flags flags;
1113
1114         flags.mask = BR_LEARNING | BR_FLOOD | BR_MCAST_FLOOD | BR_BCAST_FLOOD;
1115         flags.val = flags.mask & ~BR_LEARNING;
1116
1117         ocelot_port_bridge_flags(ocelot, port, flags);
1118 }
1119
1120 static int ocelot_switchdev_sync(struct ocelot *ocelot, int port,
1121                                  struct net_device *brport_dev,
1122                                  struct net_device *bridge_dev,
1123                                  struct netlink_ext_ack *extack)
1124 {
1125         clock_t ageing_time;
1126         u8 stp_state;
1127
1128         ocelot_inherit_brport_flags(ocelot, port, brport_dev);
1129
1130         stp_state = br_port_get_stp_state(brport_dev);
1131         ocelot_bridge_stp_state_set(ocelot, port, stp_state);
1132
1133         ageing_time = br_get_ageing_time(bridge_dev);
1134         ocelot_port_attr_ageing_set(ocelot, port, ageing_time);
1135
1136         return ocelot_port_vlan_filtering(ocelot, port,
1137                                           br_vlan_enabled(bridge_dev),
1138                                           extack);
1139 }
1140
1141 static int ocelot_switchdev_unsync(struct ocelot *ocelot, int port)
1142 {
1143         int err;
1144
1145         err = ocelot_port_vlan_filtering(ocelot, port, false, NULL);
1146         if (err)
1147                 return err;
1148
1149         ocelot_clear_brport_flags(ocelot, port);
1150
1151         ocelot_bridge_stp_state_set(ocelot, port, BR_STATE_FORWARDING);
1152
1153         return 0;
1154 }
1155
1156 static int ocelot_netdevice_bridge_join(struct net_device *dev,
1157                                         struct net_device *brport_dev,
1158                                         struct net_device *bridge,
1159                                         struct netlink_ext_ack *extack)
1160 {
1161         struct ocelot_port_private *priv = netdev_priv(dev);
1162         struct ocelot_port *ocelot_port = &priv->port;
1163         struct ocelot *ocelot = ocelot_port->ocelot;
1164         int port = priv->chip_port;
1165         int err;
1166
1167         ocelot_port_bridge_join(ocelot, port, bridge);
1168
1169         err = switchdev_bridge_port_offload(brport_dev, dev, priv,
1170                                             &ocelot_netdevice_nb,
1171                                             &ocelot_switchdev_blocking_nb,
1172                                             false, extack);
1173         if (err)
1174                 goto err_switchdev_offload;
1175
1176         err = ocelot_switchdev_sync(ocelot, port, brport_dev, bridge, extack);
1177         if (err)
1178                 goto err_switchdev_sync;
1179
1180         return 0;
1181
1182 err_switchdev_sync:
1183         switchdev_bridge_port_unoffload(brport_dev, priv,
1184                                         &ocelot_netdevice_nb,
1185                                         &ocelot_switchdev_blocking_nb);
1186 err_switchdev_offload:
1187         ocelot_port_bridge_leave(ocelot, port, bridge);
1188         return err;
1189 }
1190
1191 static void ocelot_netdevice_pre_bridge_leave(struct net_device *dev,
1192                                               struct net_device *brport_dev)
1193 {
1194         struct ocelot_port_private *priv = netdev_priv(dev);
1195
1196         switchdev_bridge_port_unoffload(brport_dev, priv,
1197                                         &ocelot_netdevice_nb,
1198                                         &ocelot_switchdev_blocking_nb);
1199 }
1200
1201 static int ocelot_netdevice_bridge_leave(struct net_device *dev,
1202                                          struct net_device *brport_dev,
1203                                          struct net_device *bridge)
1204 {
1205         struct ocelot_port_private *priv = netdev_priv(dev);
1206         struct ocelot_port *ocelot_port = &priv->port;
1207         struct ocelot *ocelot = ocelot_port->ocelot;
1208         int port = priv->chip_port;
1209         int err;
1210
1211         err = ocelot_switchdev_unsync(ocelot, port);
1212         if (err)
1213                 return err;
1214
1215         ocelot_port_bridge_leave(ocelot, port, bridge);
1216
1217         return 0;
1218 }
1219
1220 static int ocelot_netdevice_lag_join(struct net_device *dev,
1221                                      struct net_device *bond,
1222                                      struct netdev_lag_upper_info *info,
1223                                      struct netlink_ext_ack *extack)
1224 {
1225         struct ocelot_port_private *priv = netdev_priv(dev);
1226         struct ocelot_port *ocelot_port = &priv->port;
1227         struct ocelot *ocelot = ocelot_port->ocelot;
1228         struct net_device *bridge_dev;
1229         int port = priv->chip_port;
1230         int err;
1231
1232         err = ocelot_port_lag_join(ocelot, port, bond, info);
1233         if (err == -EOPNOTSUPP) {
1234                 NL_SET_ERR_MSG_MOD(extack, "Offloading not supported");
1235                 return 0;
1236         }
1237
1238         bridge_dev = netdev_master_upper_dev_get(bond);
1239         if (!bridge_dev || !netif_is_bridge_master(bridge_dev))
1240                 return 0;
1241
1242         err = ocelot_netdevice_bridge_join(dev, bond, bridge_dev, extack);
1243         if (err)
1244                 goto err_bridge_join;
1245
1246         return 0;
1247
1248 err_bridge_join:
1249         ocelot_port_lag_leave(ocelot, port, bond);
1250         return err;
1251 }
1252
1253 static void ocelot_netdevice_pre_lag_leave(struct net_device *dev,
1254                                            struct net_device *bond)
1255 {
1256         struct net_device *bridge_dev;
1257
1258         bridge_dev = netdev_master_upper_dev_get(bond);
1259         if (!bridge_dev || !netif_is_bridge_master(bridge_dev))
1260                 return;
1261
1262         ocelot_netdevice_pre_bridge_leave(dev, bond);
1263 }
1264
1265 static int ocelot_netdevice_lag_leave(struct net_device *dev,
1266                                       struct net_device *bond)
1267 {
1268         struct ocelot_port_private *priv = netdev_priv(dev);
1269         struct ocelot_port *ocelot_port = &priv->port;
1270         struct ocelot *ocelot = ocelot_port->ocelot;
1271         struct net_device *bridge_dev;
1272         int port = priv->chip_port;
1273
1274         ocelot_port_lag_leave(ocelot, port, bond);
1275
1276         bridge_dev = netdev_master_upper_dev_get(bond);
1277         if (!bridge_dev || !netif_is_bridge_master(bridge_dev))
1278                 return 0;
1279
1280         return ocelot_netdevice_bridge_leave(dev, bond, bridge_dev);
1281 }
1282
1283 static int ocelot_netdevice_changeupper(struct net_device *dev,
1284                                         struct net_device *brport_dev,
1285                                         struct netdev_notifier_changeupper_info *info)
1286 {
1287         struct netlink_ext_ack *extack;
1288         int err = 0;
1289
1290         extack = netdev_notifier_info_to_extack(&info->info);
1291
1292         if (netif_is_bridge_master(info->upper_dev)) {
1293                 if (info->linking)
1294                         err = ocelot_netdevice_bridge_join(dev, brport_dev,
1295                                                            info->upper_dev,
1296                                                            extack);
1297                 else
1298                         err = ocelot_netdevice_bridge_leave(dev, brport_dev,
1299                                                             info->upper_dev);
1300         }
1301         if (netif_is_lag_master(info->upper_dev)) {
1302                 if (info->linking)
1303                         err = ocelot_netdevice_lag_join(dev, info->upper_dev,
1304                                                         info->upper_info, extack);
1305                 else
1306                         ocelot_netdevice_lag_leave(dev, info->upper_dev);
1307         }
1308
1309         return notifier_from_errno(err);
1310 }
1311
1312 /* Treat CHANGEUPPER events on an offloaded LAG as individual CHANGEUPPER
1313  * events for the lower physical ports of the LAG.
1314  * If the LAG upper isn't offloaded, ignore its CHANGEUPPER events.
1315  * In case the LAG joined a bridge, notify that we are offloading it and can do
1316  * forwarding in hardware towards it.
1317  */
1318 static int
1319 ocelot_netdevice_lag_changeupper(struct net_device *dev,
1320                                  struct netdev_notifier_changeupper_info *info)
1321 {
1322         struct net_device *lower;
1323         struct list_head *iter;
1324         int err = NOTIFY_DONE;
1325
1326         netdev_for_each_lower_dev(dev, lower, iter) {
1327                 struct ocelot_port_private *priv = netdev_priv(lower);
1328                 struct ocelot_port *ocelot_port = &priv->port;
1329
1330                 if (ocelot_port->bond != dev)
1331                         return NOTIFY_OK;
1332
1333                 err = ocelot_netdevice_changeupper(lower, dev, info);
1334                 if (err)
1335                         return notifier_from_errno(err);
1336         }
1337
1338         return NOTIFY_DONE;
1339 }
1340
1341 static int
1342 ocelot_netdevice_prechangeupper(struct net_device *dev,
1343                                 struct net_device *brport_dev,
1344                                 struct netdev_notifier_changeupper_info *info)
1345 {
1346         if (netif_is_bridge_master(info->upper_dev) && !info->linking)
1347                 ocelot_netdevice_pre_bridge_leave(dev, brport_dev);
1348
1349         if (netif_is_lag_master(info->upper_dev) && !info->linking)
1350                 ocelot_netdevice_pre_lag_leave(dev, info->upper_dev);
1351
1352         return NOTIFY_DONE;
1353 }
1354
1355 static int
1356 ocelot_netdevice_lag_prechangeupper(struct net_device *dev,
1357                                     struct netdev_notifier_changeupper_info *info)
1358 {
1359         struct net_device *lower;
1360         struct list_head *iter;
1361         int err = NOTIFY_DONE;
1362
1363         netdev_for_each_lower_dev(dev, lower, iter) {
1364                 struct ocelot_port_private *priv = netdev_priv(lower);
1365                 struct ocelot_port *ocelot_port = &priv->port;
1366
1367                 if (ocelot_port->bond != dev)
1368                         return NOTIFY_OK;
1369
1370                 err = ocelot_netdevice_prechangeupper(dev, lower, info);
1371                 if (err)
1372                         return err;
1373         }
1374
1375         return NOTIFY_DONE;
1376 }
1377
1378 static int
1379 ocelot_netdevice_changelowerstate(struct net_device *dev,
1380                                   struct netdev_lag_lower_state_info *info)
1381 {
1382         struct ocelot_port_private *priv = netdev_priv(dev);
1383         bool is_active = info->link_up && info->tx_enabled;
1384         struct ocelot_port *ocelot_port = &priv->port;
1385         struct ocelot *ocelot = ocelot_port->ocelot;
1386         int port = priv->chip_port;
1387
1388         if (!ocelot_port->bond)
1389                 return NOTIFY_DONE;
1390
1391         if (ocelot_port->lag_tx_active == is_active)
1392                 return NOTIFY_DONE;
1393
1394         ocelot_port_lag_change(ocelot, port, is_active);
1395
1396         return NOTIFY_OK;
1397 }
1398
1399 static int ocelot_netdevice_event(struct notifier_block *unused,
1400                                   unsigned long event, void *ptr)
1401 {
1402         struct net_device *dev = netdev_notifier_info_to_dev(ptr);
1403
1404         switch (event) {
1405         case NETDEV_PRECHANGEUPPER: {
1406                 struct netdev_notifier_changeupper_info *info = ptr;
1407
1408                 if (ocelot_netdevice_dev_check(dev))
1409                         return ocelot_netdevice_prechangeupper(dev, dev, info);
1410
1411                 if (netif_is_lag_master(dev))
1412                         return ocelot_netdevice_lag_prechangeupper(dev, info);
1413
1414                 break;
1415         }
1416         case NETDEV_CHANGEUPPER: {
1417                 struct netdev_notifier_changeupper_info *info = ptr;
1418
1419                 if (ocelot_netdevice_dev_check(dev))
1420                         return ocelot_netdevice_changeupper(dev, dev, info);
1421
1422                 if (netif_is_lag_master(dev))
1423                         return ocelot_netdevice_lag_changeupper(dev, info);
1424
1425                 break;
1426         }
1427         case NETDEV_CHANGELOWERSTATE: {
1428                 struct netdev_notifier_changelowerstate_info *info = ptr;
1429
1430                 if (!ocelot_netdevice_dev_check(dev))
1431                         break;
1432
1433                 return ocelot_netdevice_changelowerstate(dev,
1434                                                          info->lower_state_info);
1435         }
1436         default:
1437                 break;
1438         }
1439
1440         return NOTIFY_DONE;
1441 }
1442
1443 struct notifier_block ocelot_netdevice_nb __read_mostly = {
1444         .notifier_call = ocelot_netdevice_event,
1445 };
1446
1447 static int ocelot_switchdev_event(struct notifier_block *unused,
1448                                   unsigned long event, void *ptr)
1449 {
1450         struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
1451         int err;
1452
1453         switch (event) {
1454         case SWITCHDEV_PORT_ATTR_SET:
1455                 err = switchdev_handle_port_attr_set(dev, ptr,
1456                                                      ocelot_netdevice_dev_check,
1457                                                      ocelot_port_attr_set);
1458                 return notifier_from_errno(err);
1459         }
1460
1461         return NOTIFY_DONE;
1462 }
1463
1464 struct notifier_block ocelot_switchdev_nb __read_mostly = {
1465         .notifier_call = ocelot_switchdev_event,
1466 };
1467
1468 static int ocelot_switchdev_blocking_event(struct notifier_block *unused,
1469                                            unsigned long event, void *ptr)
1470 {
1471         struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
1472         int err;
1473
1474         switch (event) {
1475                 /* Blocking events. */
1476         case SWITCHDEV_PORT_OBJ_ADD:
1477                 err = switchdev_handle_port_obj_add(dev, ptr,
1478                                                     ocelot_netdevice_dev_check,
1479                                                     ocelot_port_obj_add);
1480                 return notifier_from_errno(err);
1481         case SWITCHDEV_PORT_OBJ_DEL:
1482                 err = switchdev_handle_port_obj_del(dev, ptr,
1483                                                     ocelot_netdevice_dev_check,
1484                                                     ocelot_port_obj_del);
1485                 return notifier_from_errno(err);
1486         case SWITCHDEV_PORT_ATTR_SET:
1487                 err = switchdev_handle_port_attr_set(dev, ptr,
1488                                                      ocelot_netdevice_dev_check,
1489                                                      ocelot_port_attr_set);
1490                 return notifier_from_errno(err);
1491         }
1492
1493         return NOTIFY_DONE;
1494 }
1495
1496 struct notifier_block ocelot_switchdev_blocking_nb __read_mostly = {
1497         .notifier_call = ocelot_switchdev_blocking_event,
1498 };
1499
1500 static void vsc7514_phylink_validate(struct phylink_config *config,
1501                                      unsigned long *supported,
1502                                      struct phylink_link_state *state)
1503 {
1504         struct net_device *ndev = to_net_dev(config->dev);
1505         struct ocelot_port_private *priv = netdev_priv(ndev);
1506         struct ocelot_port *ocelot_port = &priv->port;
1507         __ETHTOOL_DECLARE_LINK_MODE_MASK(mask) = {};
1508
1509         if (state->interface != PHY_INTERFACE_MODE_NA &&
1510             state->interface != ocelot_port->phy_mode) {
1511                 bitmap_zero(supported, __ETHTOOL_LINK_MODE_MASK_NBITS);
1512                 return;
1513         }
1514
1515         phylink_set_port_modes(mask);
1516
1517         phylink_set(mask, Pause);
1518         phylink_set(mask, Autoneg);
1519         phylink_set(mask, Asym_Pause);
1520         phylink_set(mask, 10baseT_Half);
1521         phylink_set(mask, 10baseT_Full);
1522         phylink_set(mask, 100baseT_Half);
1523         phylink_set(mask, 100baseT_Full);
1524         phylink_set(mask, 1000baseT_Half);
1525         phylink_set(mask, 1000baseT_Full);
1526         phylink_set(mask, 1000baseX_Full);
1527         phylink_set(mask, 2500baseT_Full);
1528         phylink_set(mask, 2500baseX_Full);
1529
1530         bitmap_and(supported, supported, mask, __ETHTOOL_LINK_MODE_MASK_NBITS);
1531         bitmap_and(state->advertising, state->advertising, mask,
1532                    __ETHTOOL_LINK_MODE_MASK_NBITS);
1533 }
1534
1535 static void vsc7514_phylink_mac_config(struct phylink_config *config,
1536                                        unsigned int link_an_mode,
1537                                        const struct phylink_link_state *state)
1538 {
1539         struct net_device *ndev = to_net_dev(config->dev);
1540         struct ocelot_port_private *priv = netdev_priv(ndev);
1541         struct ocelot_port *ocelot_port = &priv->port;
1542
1543         /* Disable HDX fast control */
1544         ocelot_port_writel(ocelot_port, DEV_PORT_MISC_HDX_FAST_DIS,
1545                            DEV_PORT_MISC);
1546
1547         /* SGMII only for now */
1548         ocelot_port_writel(ocelot_port, PCS1G_MODE_CFG_SGMII_MODE_ENA,
1549                            PCS1G_MODE_CFG);
1550         ocelot_port_writel(ocelot_port, PCS1G_SD_CFG_SD_SEL, PCS1G_SD_CFG);
1551
1552         /* Enable PCS */
1553         ocelot_port_writel(ocelot_port, PCS1G_CFG_PCS_ENA, PCS1G_CFG);
1554
1555         /* No aneg on SGMII */
1556         ocelot_port_writel(ocelot_port, 0, PCS1G_ANEG_CFG);
1557
1558         /* No loopback */
1559         ocelot_port_writel(ocelot_port, 0, PCS1G_LB_CFG);
1560 }
1561
1562 static void vsc7514_phylink_mac_link_down(struct phylink_config *config,
1563                                           unsigned int link_an_mode,
1564                                           phy_interface_t interface)
1565 {
1566         struct net_device *ndev = to_net_dev(config->dev);
1567         struct ocelot_port_private *priv = netdev_priv(ndev);
1568         struct ocelot *ocelot = priv->port.ocelot;
1569         int port = priv->chip_port;
1570
1571         ocelot_phylink_mac_link_down(ocelot, port, link_an_mode, interface,
1572                                      OCELOT_MAC_QUIRKS);
1573 }
1574
1575 static void vsc7514_phylink_mac_link_up(struct phylink_config *config,
1576                                         struct phy_device *phydev,
1577                                         unsigned int link_an_mode,
1578                                         phy_interface_t interface,
1579                                         int speed, int duplex,
1580                                         bool tx_pause, bool rx_pause)
1581 {
1582         struct net_device *ndev = to_net_dev(config->dev);
1583         struct ocelot_port_private *priv = netdev_priv(ndev);
1584         struct ocelot *ocelot = priv->port.ocelot;
1585         int port = priv->chip_port;
1586
1587         ocelot_phylink_mac_link_up(ocelot, port, phydev, link_an_mode,
1588                                    interface, speed, duplex,
1589                                    tx_pause, rx_pause, OCELOT_MAC_QUIRKS);
1590 }
1591
1592 static const struct phylink_mac_ops ocelot_phylink_ops = {
1593         .validate               = vsc7514_phylink_validate,
1594         .mac_config             = vsc7514_phylink_mac_config,
1595         .mac_link_down          = vsc7514_phylink_mac_link_down,
1596         .mac_link_up            = vsc7514_phylink_mac_link_up,
1597 };
1598
1599 static int ocelot_port_phylink_create(struct ocelot *ocelot, int port,
1600                                       struct device_node *portnp)
1601 {
1602         struct ocelot_port *ocelot_port = ocelot->ports[port];
1603         struct ocelot_port_private *priv;
1604         struct device *dev = ocelot->dev;
1605         phy_interface_t phy_mode;
1606         struct phylink *phylink;
1607         int err;
1608
1609         of_get_phy_mode(portnp, &phy_mode);
1610         /* DT bindings of internal PHY ports are broken and don't
1611          * specify a phy-mode
1612          */
1613         if (phy_mode == PHY_INTERFACE_MODE_NA)
1614                 phy_mode = PHY_INTERFACE_MODE_INTERNAL;
1615
1616         if (phy_mode != PHY_INTERFACE_MODE_SGMII &&
1617             phy_mode != PHY_INTERFACE_MODE_QSGMII &&
1618             phy_mode != PHY_INTERFACE_MODE_INTERNAL) {
1619                 dev_err(dev, "unsupported phy mode %s for port %d\n",
1620                         phy_modes(phy_mode), port);
1621                 return -EINVAL;
1622         }
1623
1624         /* Ensure clock signals and speed are set on all QSGMII links */
1625         if (phy_mode == PHY_INTERFACE_MODE_QSGMII)
1626                 ocelot_port_rmwl(ocelot_port, 0,
1627                                  DEV_CLOCK_CFG_MAC_TX_RST |
1628                                  DEV_CLOCK_CFG_MAC_TX_RST,
1629                                  DEV_CLOCK_CFG);
1630
1631         ocelot_port->phy_mode = phy_mode;
1632
1633         if (phy_mode != PHY_INTERFACE_MODE_INTERNAL) {
1634                 struct phy *serdes = of_phy_get(portnp, NULL);
1635
1636                 if (IS_ERR(serdes)) {
1637                         err = PTR_ERR(serdes);
1638                         dev_err_probe(dev, err,
1639                                       "missing SerDes phys for port %d\n",
1640                                       port);
1641                         return err;
1642                 }
1643
1644                 err = phy_set_mode_ext(serdes, PHY_MODE_ETHERNET, phy_mode);
1645                 of_phy_put(serdes);
1646                 if (err) {
1647                         dev_err(dev, "Could not SerDes mode on port %d: %pe\n",
1648                                 port, ERR_PTR(err));
1649                         return err;
1650                 }
1651         }
1652
1653         priv = container_of(ocelot_port, struct ocelot_port_private, port);
1654
1655         priv->phylink_config.dev = &priv->dev->dev;
1656         priv->phylink_config.type = PHYLINK_NETDEV;
1657
1658         phylink = phylink_create(&priv->phylink_config,
1659                                  of_fwnode_handle(portnp),
1660                                  phy_mode, &ocelot_phylink_ops);
1661         if (IS_ERR(phylink)) {
1662                 err = PTR_ERR(phylink);
1663                 dev_err(dev, "Could not create phylink (%pe)\n", phylink);
1664                 return err;
1665         }
1666
1667         priv->phylink = phylink;
1668
1669         err = phylink_of_phy_connect(phylink, portnp, 0);
1670         if (err) {
1671                 dev_err(dev, "Could not connect to PHY: %pe\n", ERR_PTR(err));
1672                 phylink_destroy(phylink);
1673                 priv->phylink = NULL;
1674                 return err;
1675         }
1676
1677         return 0;
1678 }
1679
1680 int ocelot_probe_port(struct ocelot *ocelot, int port, struct regmap *target,
1681                       struct device_node *portnp)
1682 {
1683         struct ocelot_port_private *priv;
1684         struct ocelot_port *ocelot_port;
1685         struct net_device *dev;
1686         int err;
1687
1688         dev = alloc_etherdev(sizeof(struct ocelot_port_private));
1689         if (!dev)
1690                 return -ENOMEM;
1691         SET_NETDEV_DEV(dev, ocelot->dev);
1692         priv = netdev_priv(dev);
1693         priv->dev = dev;
1694         priv->chip_port = port;
1695         ocelot_port = &priv->port;
1696         ocelot_port->ocelot = ocelot;
1697         ocelot_port->target = target;
1698         ocelot->ports[port] = ocelot_port;
1699
1700         dev->netdev_ops = &ocelot_port_netdev_ops;
1701         dev->ethtool_ops = &ocelot_ethtool_ops;
1702
1703         dev->hw_features |= NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_RXFCS |
1704                 NETIF_F_HW_TC;
1705         dev->features |= NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_HW_TC;
1706
1707         memcpy(dev->dev_addr, ocelot->base_mac, ETH_ALEN);
1708         dev->dev_addr[ETH_ALEN - 1] += port;
1709         ocelot_mact_learn(ocelot, PGID_CPU, dev->dev_addr,
1710                           ocelot_port->pvid_vlan.vid, ENTRYTYPE_LOCKED);
1711
1712         ocelot_init_port(ocelot, port);
1713
1714         err = ocelot_port_phylink_create(ocelot, port, portnp);
1715         if (err)
1716                 goto out;
1717
1718         err = register_netdev(dev);
1719         if (err) {
1720                 dev_err(ocelot->dev, "register_netdev failed\n");
1721                 goto out;
1722         }
1723
1724         return 0;
1725
1726 out:
1727         ocelot->ports[port] = NULL;
1728         free_netdev(dev);
1729
1730         return err;
1731 }
1732
1733 void ocelot_release_port(struct ocelot_port *ocelot_port)
1734 {
1735         struct ocelot_port_private *priv = container_of(ocelot_port,
1736                                                 struct ocelot_port_private,
1737                                                 port);
1738
1739         unregister_netdev(priv->dev);
1740
1741         if (priv->phylink) {
1742                 rtnl_lock();
1743                 phylink_disconnect_phy(priv->phylink);
1744                 rtnl_unlock();
1745
1746                 phylink_destroy(priv->phylink);
1747         }
1748
1749         free_netdev(priv->dev);
1750 }