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