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,
 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                                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         u32 portid = NETLINK_CB(dump->cb->skb).portid;
 762         u32 seq = dump->cb->nlh->nlmsg_seq;
 763         struct nlmsghdr *nlh;
 764         struct ndmsg *ndm;
 765
 766         if (dump->idx < dump->cb->args[2])
 767                 goto skip;
 768
 769         nlh = nlmsg_put(dump->skb, portid, seq, RTM_NEWNEIGH,
 770                         sizeof(*ndm), NLM_F_MULTI);
 771         if (!nlh)
 772                 return -EMSGSIZE;
 773
 774         ndm = nlmsg_data(nlh);
 775         ndm->ndm_family  = AF_BRIDGE;
 776         ndm->ndm_pad1    = 0;
 777         ndm->ndm_pad2    = 0;
 778         ndm->ndm_flags   = NTF_SELF;
 779         ndm->ndm_type    = 0;
 780         ndm->ndm_ifindex = dump->dev->ifindex;
 781         ndm->ndm_state   = is_static ? NUD_NOARP : NUD_REACHABLE;
 782
 783         if (nla_put(dump->skb, NDA_LLADDR, ETH_ALEN, addr))
 784                 goto nla_put_failure;
 785
 786         if (vid && nla_put_u16(dump->skb, NDA_VLAN, vid))
 787                 goto nla_put_failure;
 788
 789         nlmsg_end(dump->skb, nlh);
 790
 791 skip:
 792         dump->idx++;
 793         return 0;
 794
 795 nla_put_failure:
 796         nlmsg_cancel(dump->skb, nlh);
 797         return -EMSGSIZE;
 798 }
 799
 800 static int ocelot_port_fdb_dump(struct sk_buff *skb,
 801                                 struct netlink_callback *cb,
 802                                 struct net_device *dev,
 803                                 struct net_device *filter_dev, int *idx)
 804 {
 805         struct ocelot_port_private *priv = netdev_priv(dev);
 806         struct ocelot *ocelot = priv->port.ocelot;
 807         struct ocelot_dump_ctx dump = {
 808                 .dev = dev,
 809                 .skb = skb,
 810                 .cb = cb,
 811                 .idx = *idx,
 812         };
 813         int port = priv->port.index;
 814         int ret;
 815
 816         ret = ocelot_fdb_dump(ocelot, port, ocelot_port_fdb_do_dump, &dump);
 817
 818         *idx = dump.idx;
 819
 820         return ret;
 821 }
 822
 823 static int ocelot_vlan_rx_add_vid(struct net_device *dev, __be16 proto,
 824                                   u16 vid)
 825 {
 826         return ocelot_vlan_vid_add(dev, vid, false, false);
 827 }
 828
 829 static int ocelot_vlan_rx_kill_vid(struct net_device *dev, __be16 proto,
 830                                    u16 vid)
 831 {
 832         return ocelot_vlan_vid_del(dev, vid);
 833 }
 834
 835 static void ocelot_vlan_mode(struct ocelot *ocelot, int port,
 836                              netdev_features_t features)
 837 {
 838         u32 val;
 839
 840         /* Filtering */
 841         val = ocelot_read(ocelot, ANA_VLANMASK);
 842         if (features & NETIF_F_HW_VLAN_CTAG_FILTER)
 843                 val |= BIT(port);
 844         else
 845                 val &= ~BIT(port);
 846         ocelot_write(ocelot, val, ANA_VLANMASK);
 847 }
 848
 849 static int ocelot_set_features(struct net_device *dev,
 850                                netdev_features_t features)
 851 {
 852         netdev_features_t changed = dev->features ^ features;
 853         struct ocelot_port_private *priv = netdev_priv(dev);
 854         struct ocelot *ocelot = priv->port.ocelot;
 855         int port = priv->port.index;
 856
 857         if ((dev->features & NETIF_F_HW_TC) > (features & NETIF_F_HW_TC) &&
 858             priv->tc.offload_cnt) {
 859                 netdev_err(dev,
 860                            "Cannot disable HW TC offload while offloads active\n");
 861                 return -EBUSY;
 862         }
 863
 864         if (changed & NETIF_F_HW_VLAN_CTAG_FILTER)
 865                 ocelot_vlan_mode(ocelot, port, features);
 866
 867         return 0;
 868 }
 869
 870 static int ocelot_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 871 {
 872         struct ocelot_port_private *priv = netdev_priv(dev);
 873         struct ocelot *ocelot = priv->port.ocelot;
 874         int port = priv->port.index;
 875
 876         /* If the attached PHY device isn't capable of timestamping operations,
 877          * use our own (when possible).
 878          */
 879         if (!phy_has_hwtstamp(dev->phydev) && ocelot->ptp) {
 880                 switch (cmd) {
 881                 case SIOCSHWTSTAMP:
 882                         return ocelot_hwstamp_set(ocelot, port, ifr);
 883                 case SIOCGHWTSTAMP:
 884                         return ocelot_hwstamp_get(ocelot, port, ifr);
 885                 }
 886         }
 887
 888         return phy_mii_ioctl(dev->phydev, ifr, cmd);
 889 }
 890
 891 static int ocelot_change_mtu(struct net_device *dev, int new_mtu)
 892 {
 893         struct ocelot_port_private *priv = netdev_priv(dev);
 894         struct ocelot_port *ocelot_port = &priv->port;
 895         struct ocelot *ocelot = ocelot_port->ocelot;
 896
 897         ocelot_port_set_maxlen(ocelot, priv->port.index, new_mtu);
 898         WRITE_ONCE(dev->mtu, new_mtu);
 899
 900         return 0;
 901 }
 902
 903 static const struct net_device_ops ocelot_port_netdev_ops = {
 904         .ndo_open                       = ocelot_port_open,
 905         .ndo_stop                       = ocelot_port_stop,
 906         .ndo_start_xmit                 = ocelot_port_xmit,
 907         .ndo_change_mtu                 = ocelot_change_mtu,
 908         .ndo_set_rx_mode                = ocelot_set_rx_mode,
 909         .ndo_set_mac_address            = ocelot_port_set_mac_address,
 910         .ndo_get_stats64                = ocelot_get_stats64,
 911         .ndo_fdb_add                    = ocelot_port_fdb_add,
 912         .ndo_fdb_del                    = ocelot_port_fdb_del,
 913         .ndo_fdb_dump                   = ocelot_port_fdb_dump,
 914         .ndo_vlan_rx_add_vid            = ocelot_vlan_rx_add_vid,
 915         .ndo_vlan_rx_kill_vid           = ocelot_vlan_rx_kill_vid,
 916         .ndo_set_features               = ocelot_set_features,
 917         .ndo_setup_tc                   = ocelot_setup_tc,
 918         .ndo_eth_ioctl                  = ocelot_ioctl,
 919 };
 920
 921 struct net_device *ocelot_port_to_netdev(struct ocelot *ocelot, int port)
 922 {
 923         struct ocelot_port *ocelot_port = ocelot->ports[port];
 924         struct ocelot_port_private *priv;
 925
 926         if (!ocelot_port)
 927                 return NULL;
 928
 929         priv = container_of(ocelot_port, struct ocelot_port_private, port);
 930
 931         return priv->dev;
 932 }
 933
 934 /* Checks if the net_device instance given to us originates from our driver */
 935 static bool ocelot_netdevice_dev_check(const struct net_device *dev)
 936 {
 937         return dev->netdev_ops == &ocelot_port_netdev_ops;
 938 }
 939
 940 int ocelot_netdev_to_port(struct net_device *dev)
 941 {
 942         struct ocelot_port_private *priv;
 943
 944         if (!dev || !ocelot_netdevice_dev_check(dev))
 945                 return -EINVAL;
 946
 947         priv = netdev_priv(dev);
 948
 949         return priv->port.index;
 950 }
 951
 952 static void ocelot_port_get_strings(struct net_device *netdev, u32 sset,
 953                                     u8 *data)
 954 {
 955         struct ocelot_port_private *priv = netdev_priv(netdev);
 956         struct ocelot *ocelot = priv->port.ocelot;
 957         int port = priv->port.index;
 958
 959         ocelot_get_strings(ocelot, port, sset, data);
 960 }
 961
 962 static void ocelot_port_get_ethtool_stats(struct net_device *dev,
 963                                           struct ethtool_stats *stats,
 964                                           u64 *data)
 965 {
 966         struct ocelot_port_private *priv = netdev_priv(dev);
 967         struct ocelot *ocelot = priv->port.ocelot;
 968         int port = priv->port.index;
 969
 970         ocelot_get_ethtool_stats(ocelot, port, data);
 971 }
 972
 973 static int ocelot_port_get_sset_count(struct net_device *dev, int sset)
 974 {
 975         struct ocelot_port_private *priv = netdev_priv(dev);
 976         struct ocelot *ocelot = priv->port.ocelot;
 977         int port = priv->port.index;
 978
 979         return ocelot_get_sset_count(ocelot, port, sset);
 980 }
 981
 982 static int ocelot_port_get_ts_info(struct net_device *dev,
 983                                    struct ethtool_ts_info *info)
 984 {
 985         struct ocelot_port_private *priv = netdev_priv(dev);
 986         struct ocelot *ocelot = priv->port.ocelot;
 987         int port = priv->port.index;
 988
 989         if (!ocelot->ptp)
 990                 return ethtool_op_get_ts_info(dev, info);
 991
 992         return ocelot_get_ts_info(ocelot, port, info);
 993 }
 994
 995 static const struct ethtool_ops ocelot_ethtool_ops = {
 996         .get_strings            = ocelot_port_get_strings,
 997         .get_ethtool_stats      = ocelot_port_get_ethtool_stats,
 998         .get_sset_count         = ocelot_port_get_sset_count,
 999         .get_link_ksettings     = phy_ethtool_get_link_ksettings,
1000         .set_link_ksettings     = phy_ethtool_set_link_ksettings,
1001         .get_ts_info            = ocelot_port_get_ts_info,
1002 };
1003
1004 static void ocelot_port_attr_stp_state_set(struct ocelot *ocelot, int port,
1005                                            u8 state)
1006 {
1007         ocelot_bridge_stp_state_set(ocelot, port, state);
1008 }
1009
1010 static void ocelot_port_attr_ageing_set(struct ocelot *ocelot, int port,
1011                                         unsigned long ageing_clock_t)
1012 {
1013         unsigned long ageing_jiffies = clock_t_to_jiffies(ageing_clock_t);
1014         u32 ageing_time = jiffies_to_msecs(ageing_jiffies);
1015
1016         ocelot_set_ageing_time(ocelot, ageing_time);
1017 }
1018
1019 static void ocelot_port_attr_mc_set(struct ocelot *ocelot, int port, bool mc)
1020 {
1021         u32 cpu_fwd_mcast = ANA_PORT_CPU_FWD_CFG_CPU_IGMP_REDIR_ENA |
1022                             ANA_PORT_CPU_FWD_CFG_CPU_MLD_REDIR_ENA |
1023                             ANA_PORT_CPU_FWD_CFG_CPU_IPMC_CTRL_COPY_ENA;
1024         u32 val = 0;
1025
1026         if (mc)
1027                 val = cpu_fwd_mcast;
1028
1029         ocelot_rmw_gix(ocelot, val, cpu_fwd_mcast,
1030                        ANA_PORT_CPU_FWD_CFG, port);
1031 }
1032
1033 static int ocelot_port_attr_set(struct net_device *dev, const void *ctx,
1034                                 const struct switchdev_attr *attr,
1035                                 struct netlink_ext_ack *extack)
1036 {
1037         struct ocelot_port_private *priv = netdev_priv(dev);
1038         struct ocelot *ocelot = priv->port.ocelot;
1039         int port = priv->port.index;
1040         int err = 0;
1041
1042         if (ctx && ctx != priv)
1043                 return 0;
1044
1045         switch (attr->id) {
1046         case SWITCHDEV_ATTR_ID_PORT_STP_STATE:
1047                 ocelot_port_attr_stp_state_set(ocelot, port, attr->u.stp_state);
1048                 break;
1049         case SWITCHDEV_ATTR_ID_BRIDGE_AGEING_TIME:
1050                 ocelot_port_attr_ageing_set(ocelot, port, attr->u.ageing_time);
1051                 break;
1052         case SWITCHDEV_ATTR_ID_BRIDGE_VLAN_FILTERING:
1053                 ocelot_port_vlan_filtering(ocelot, port, attr->u.vlan_filtering,
1054                                            extack);
1055                 break;
1056         case SWITCHDEV_ATTR_ID_BRIDGE_MC_DISABLED:
1057                 ocelot_port_attr_mc_set(ocelot, port, !attr->u.mc_disabled);
1058                 break;
1059         case SWITCHDEV_ATTR_ID_PORT_PRE_BRIDGE_FLAGS:
1060                 err = ocelot_port_pre_bridge_flags(ocelot, port,
1061                                                    attr->u.brport_flags);
1062                 break;
1063         case SWITCHDEV_ATTR_ID_PORT_BRIDGE_FLAGS:
1064                 ocelot_port_bridge_flags(ocelot, port, attr->u.brport_flags);
1065                 break;
1066         default:
1067                 err = -EOPNOTSUPP;
1068                 break;
1069         }
1070
1071         return err;
1072 }
1073
1074 static int ocelot_vlan_vid_prepare(struct net_device *dev, u16 vid, bool pvid,
1075                                    bool untagged, struct netlink_ext_ack *extack)
1076 {
1077         struct ocelot_port_private *priv = netdev_priv(dev);
1078         struct ocelot_port *ocelot_port = &priv->port;
1079         struct ocelot *ocelot = ocelot_port->ocelot;
1080         int port = priv->port.index;
1081
1082         return ocelot_vlan_prepare(ocelot, port, vid, pvid, untagged, extack);
1083 }
1084
1085 static int ocelot_port_obj_add_vlan(struct net_device *dev,
1086                                     const struct switchdev_obj_port_vlan *vlan,
1087                                     struct netlink_ext_ack *extack)
1088 {
1089         bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
1090         bool pvid = vlan->flags & BRIDGE_VLAN_INFO_PVID;
1091         int ret;
1092
1093         ret = ocelot_vlan_vid_prepare(dev, vlan->vid, pvid, untagged, extack);
1094         if (ret)
1095                 return ret;
1096
1097         return ocelot_vlan_vid_add(dev, vlan->vid, pvid, untagged);
1098 }
1099
1100 static int ocelot_port_obj_add_mdb(struct net_device *dev,
1101                                    const struct switchdev_obj_port_mdb *mdb)
1102 {
1103         struct ocelot_port_private *priv = netdev_priv(dev);
1104         struct ocelot_port *ocelot_port = &priv->port;
1105         struct ocelot *ocelot = ocelot_port->ocelot;
1106         int port = priv->port.index;
1107
1108         return ocelot_port_mdb_add(ocelot, port, mdb, ocelot_port->bridge);
1109 }
1110
1111 static int ocelot_port_obj_del_mdb(struct net_device *dev,
1112                                    const struct switchdev_obj_port_mdb *mdb)
1113 {
1114         struct ocelot_port_private *priv = netdev_priv(dev);
1115         struct ocelot_port *ocelot_port = &priv->port;
1116         struct ocelot *ocelot = ocelot_port->ocelot;
1117         int port = priv->port.index;
1118
1119         return ocelot_port_mdb_del(ocelot, port, mdb, ocelot_port->bridge);
1120 }
1121
1122 static int ocelot_port_obj_mrp_add(struct net_device *dev,
1123                                    const struct switchdev_obj_mrp *mrp)
1124 {
1125         struct ocelot_port_private *priv = netdev_priv(dev);
1126         struct ocelot_port *ocelot_port = &priv->port;
1127         struct ocelot *ocelot = ocelot_port->ocelot;
1128         int port = priv->port.index;
1129
1130         return ocelot_mrp_add(ocelot, port, mrp);
1131 }
1132
1133 static int ocelot_port_obj_mrp_del(struct net_device *dev,
1134                                    const struct switchdev_obj_mrp *mrp)
1135 {
1136         struct ocelot_port_private *priv = netdev_priv(dev);
1137         struct ocelot_port *ocelot_port = &priv->port;
1138         struct ocelot *ocelot = ocelot_port->ocelot;
1139         int port = priv->port.index;
1140
1141         return ocelot_mrp_del(ocelot, port, mrp);
1142 }
1143
1144 static int
1145 ocelot_port_obj_mrp_add_ring_role(struct net_device *dev,
1146                                   const struct switchdev_obj_ring_role_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_add_ring_role(ocelot, port, mrp);
1154 }
1155
1156 static int
1157 ocelot_port_obj_mrp_del_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_del_ring_role(ocelot, port, mrp);
1166 }
1167
1168 static int ocelot_port_obj_add(struct net_device *dev, const void *ctx,
1169                                const struct switchdev_obj *obj,
1170                                struct netlink_ext_ack *extack)
1171 {
1172         struct ocelot_port_private *priv = netdev_priv(dev);
1173         int ret = 0;
1174
1175         if (ctx && ctx != priv)
1176                 return 0;
1177
1178         switch (obj->id) {
1179         case SWITCHDEV_OBJ_ID_PORT_VLAN:
1180                 ret = ocelot_port_obj_add_vlan(dev,
1181                                                SWITCHDEV_OBJ_PORT_VLAN(obj),
1182                                                extack);
1183                 break;
1184         case SWITCHDEV_OBJ_ID_PORT_MDB:
1185                 ret = ocelot_port_obj_add_mdb(dev, SWITCHDEV_OBJ_PORT_MDB(obj));
1186                 break;
1187         case SWITCHDEV_OBJ_ID_MRP:
1188                 ret = ocelot_port_obj_mrp_add(dev, SWITCHDEV_OBJ_MRP(obj));
1189                 break;
1190         case SWITCHDEV_OBJ_ID_RING_ROLE_MRP:
1191                 ret = ocelot_port_obj_mrp_add_ring_role(dev,
1192                                                         SWITCHDEV_OBJ_RING_ROLE_MRP(obj));
1193                 break;
1194         default:
1195                 return -EOPNOTSUPP;
1196         }
1197
1198         return ret;
1199 }
1200
1201 static int ocelot_port_obj_del(struct net_device *dev, const void *ctx,
1202                                const struct switchdev_obj *obj)
1203 {
1204         struct ocelot_port_private *priv = netdev_priv(dev);
1205         int ret = 0;
1206
1207         if (ctx && ctx != priv)
1208                 return 0;
1209
1210         switch (obj->id) {
1211         case SWITCHDEV_OBJ_ID_PORT_VLAN:
1212                 ret = ocelot_vlan_vid_del(dev,
1213                                           SWITCHDEV_OBJ_PORT_VLAN(obj)->vid);
1214                 break;
1215         case SWITCHDEV_OBJ_ID_PORT_MDB:
1216                 ret = ocelot_port_obj_del_mdb(dev, SWITCHDEV_OBJ_PORT_MDB(obj));
1217                 break;
1218         case SWITCHDEV_OBJ_ID_MRP:
1219                 ret = ocelot_port_obj_mrp_del(dev, SWITCHDEV_OBJ_MRP(obj));
1220                 break;
1221         case SWITCHDEV_OBJ_ID_RING_ROLE_MRP:
1222                 ret = ocelot_port_obj_mrp_del_ring_role(dev,
1223                                                         SWITCHDEV_OBJ_RING_ROLE_MRP(obj));
1224                 break;
1225         default:
1226                 return -EOPNOTSUPP;
1227         }
1228
1229         return ret;
1230 }
1231
1232 static void ocelot_inherit_brport_flags(struct ocelot *ocelot, int port,
1233                                         struct net_device *brport_dev)
1234 {
1235         struct switchdev_brport_flags flags = {0};
1236         int flag;
1237
1238         flags.mask = BR_LEARNING | BR_FLOOD | BR_MCAST_FLOOD | BR_BCAST_FLOOD;
1239
1240         for_each_set_bit(flag, &flags.mask, 32)
1241                 if (br_port_flag_is_set(brport_dev, BIT(flag)))
1242                         flags.val |= BIT(flag);
1243
1244         ocelot_port_bridge_flags(ocelot, port, flags);
1245 }
1246
1247 static void ocelot_clear_brport_flags(struct ocelot *ocelot, int port)
1248 {
1249         struct switchdev_brport_flags flags;
1250
1251         flags.mask = BR_LEARNING | BR_FLOOD | BR_MCAST_FLOOD | BR_BCAST_FLOOD;
1252         flags.val = flags.mask & ~BR_LEARNING;
1253
1254         ocelot_port_bridge_flags(ocelot, port, flags);
1255 }
1256
1257 static int ocelot_switchdev_sync(struct ocelot *ocelot, int port,
1258                                  struct net_device *brport_dev,
1259                                  struct net_device *bridge_dev,
1260                                  struct netlink_ext_ack *extack)
1261 {
1262         clock_t ageing_time;
1263         u8 stp_state;
1264
1265         ocelot_inherit_brport_flags(ocelot, port, brport_dev);
1266
1267         stp_state = br_port_get_stp_state(brport_dev);
1268         ocelot_bridge_stp_state_set(ocelot, port, stp_state);
1269
1270         ageing_time = br_get_ageing_time(bridge_dev);
1271         ocelot_port_attr_ageing_set(ocelot, port, ageing_time);
1272
1273         return ocelot_port_vlan_filtering(ocelot, port,
1274                                           br_vlan_enabled(bridge_dev),
1275                                           extack);
1276 }
1277
1278 static int ocelot_switchdev_unsync(struct ocelot *ocelot, int port)
1279 {
1280         int err;
1281
1282         err = ocelot_port_vlan_filtering(ocelot, port, false, NULL);
1283         if (err)
1284                 return err;
1285
1286         ocelot_clear_brport_flags(ocelot, port);
1287
1288         ocelot_bridge_stp_state_set(ocelot, port, BR_STATE_FORWARDING);
1289
1290         return 0;
1291 }
1292
1293 static int ocelot_bridge_num_get(struct ocelot *ocelot,
1294                                  const struct net_device *bridge_dev)
1295 {
1296         int bridge_num = ocelot_bridge_num_find(ocelot, bridge_dev);
1297
1298         if (bridge_num < 0) {
1299                 /* First port that offloads this bridge */
1300                 bridge_num = find_first_zero_bit(&ocelot->bridges,
1301                                                  ocelot->num_phys_ports);
1302
1303                 set_bit(bridge_num, &ocelot->bridges);
1304         }
1305
1306         return bridge_num;
1307 }
1308
1309 static void ocelot_bridge_num_put(struct ocelot *ocelot,
1310                                   const struct net_device *bridge_dev,
1311                                   int bridge_num)
1312 {
1313         /* Check if the bridge is still in use, otherwise it is time
1314          * to clean it up so we can reuse this bridge_num later.
1315          */
1316         if (!ocelot_bridge_num_find(ocelot, bridge_dev))
1317                 clear_bit(bridge_num, &ocelot->bridges);
1318 }
1319
1320 static int ocelot_netdevice_bridge_join(struct net_device *dev,
1321                                         struct net_device *brport_dev,
1322                                         struct net_device *bridge,
1323                                         struct netlink_ext_ack *extack)
1324 {
1325         struct ocelot_port_private *priv = netdev_priv(dev);
1326         struct ocelot_port *ocelot_port = &priv->port;
1327         struct ocelot *ocelot = ocelot_port->ocelot;
1328         int port = priv->port.index;
1329         int bridge_num, err;
1330
1331         bridge_num = ocelot_bridge_num_get(ocelot, bridge);
1332
1333         err = ocelot_port_bridge_join(ocelot, port, bridge, bridge_num,
1334                                       extack);
1335         if (err)
1336                 goto err_join;
1337
1338         err = switchdev_bridge_port_offload(brport_dev, dev, priv,
1339                                             &ocelot_switchdev_nb,
1340                                             &ocelot_switchdev_blocking_nb,
1341                                             false, extack);
1342         if (err)
1343                 goto err_switchdev_offload;
1344
1345         err = ocelot_switchdev_sync(ocelot, port, brport_dev, bridge, extack);
1346         if (err)
1347                 goto err_switchdev_sync;
1348
1349         return 0;
1350
1351 err_switchdev_sync:
1352         switchdev_bridge_port_unoffload(brport_dev, priv,
1353                                         &ocelot_switchdev_nb,
1354                                         &ocelot_switchdev_blocking_nb);
1355 err_switchdev_offload:
1356         ocelot_port_bridge_leave(ocelot, port, bridge);
1357 err_join:
1358         ocelot_bridge_num_put(ocelot, bridge, bridge_num);
1359         return err;
1360 }
1361
1362 static void ocelot_netdevice_pre_bridge_leave(struct net_device *dev,
1363                                               struct net_device *brport_dev)
1364 {
1365         struct ocelot_port_private *priv = netdev_priv(dev);
1366
1367         switchdev_bridge_port_unoffload(brport_dev, priv,
1368                                         &ocelot_switchdev_nb,
1369                                         &ocelot_switchdev_blocking_nb);
1370 }
1371
1372 static int ocelot_netdevice_bridge_leave(struct net_device *dev,
1373                                          struct net_device *brport_dev,
1374                                          struct net_device *bridge)
1375 {
1376         struct ocelot_port_private *priv = netdev_priv(dev);
1377         struct ocelot_port *ocelot_port = &priv->port;
1378         struct ocelot *ocelot = ocelot_port->ocelot;
1379         int bridge_num = ocelot_port->bridge_num;
1380         int port = priv->port.index;
1381         int err;
1382
1383         err = ocelot_switchdev_unsync(ocelot, port);
1384         if (err)
1385                 return err;
1386
1387         ocelot_port_bridge_leave(ocelot, port, bridge);
1388         ocelot_bridge_num_put(ocelot, bridge, bridge_num);
1389
1390         return 0;
1391 }
1392
1393 static int ocelot_netdevice_lag_join(struct net_device *dev,
1394                                      struct net_device *bond,
1395                                      struct netdev_lag_upper_info *info,
1396                                      struct netlink_ext_ack *extack)
1397 {
1398         struct ocelot_port_private *priv = netdev_priv(dev);
1399         struct ocelot_port *ocelot_port = &priv->port;
1400         struct ocelot *ocelot = ocelot_port->ocelot;
1401         struct net_device *bridge_dev;
1402         int port = priv->port.index;
1403         int err;
1404
1405         err = ocelot_port_lag_join(ocelot, port, bond, info, extack);
1406         if (err == -EOPNOTSUPP)
1407                 /* Offloading not supported, fall back to software LAG */
1408                 return 0;
1409
1410         bridge_dev = netdev_master_upper_dev_get(bond);
1411         if (!bridge_dev || !netif_is_bridge_master(bridge_dev))
1412                 return 0;
1413
1414         err = ocelot_netdevice_bridge_join(dev, bond, bridge_dev, extack);
1415         if (err)
1416                 goto err_bridge_join;
1417
1418         return 0;
1419
1420 err_bridge_join:
1421         ocelot_port_lag_leave(ocelot, port, bond);
1422         return err;
1423 }
1424
1425 static void ocelot_netdevice_pre_lag_leave(struct net_device *dev,
1426                                            struct net_device *bond)
1427 {
1428         struct net_device *bridge_dev;
1429
1430         bridge_dev = netdev_master_upper_dev_get(bond);
1431         if (!bridge_dev || !netif_is_bridge_master(bridge_dev))
1432                 return;
1433
1434         ocelot_netdevice_pre_bridge_leave(dev, bond);
1435 }
1436
1437 static int ocelot_netdevice_lag_leave(struct net_device *dev,
1438                                       struct net_device *bond)
1439 {
1440         struct ocelot_port_private *priv = netdev_priv(dev);
1441         struct ocelot_port *ocelot_port = &priv->port;
1442         struct ocelot *ocelot = ocelot_port->ocelot;
1443         struct net_device *bridge_dev;
1444         int port = priv->port.index;
1445
1446         ocelot_port_lag_leave(ocelot, port, bond);
1447
1448         bridge_dev = netdev_master_upper_dev_get(bond);
1449         if (!bridge_dev || !netif_is_bridge_master(bridge_dev))
1450                 return 0;
1451
1452         return ocelot_netdevice_bridge_leave(dev, bond, bridge_dev);
1453 }
1454
1455 static int ocelot_netdevice_changeupper(struct net_device *dev,
1456                                         struct net_device *brport_dev,
1457                                         struct netdev_notifier_changeupper_info *info)
1458 {
1459         struct netlink_ext_ack *extack;
1460         int err = 0;
1461
1462         extack = netdev_notifier_info_to_extack(&info->info);
1463
1464         if (netif_is_bridge_master(info->upper_dev)) {
1465                 if (info->linking)
1466                         err = ocelot_netdevice_bridge_join(dev, brport_dev,
1467                                                            info->upper_dev,
1468                                                            extack);
1469                 else
1470                         err = ocelot_netdevice_bridge_leave(dev, brport_dev,
1471                                                             info->upper_dev);
1472         }
1473         if (netif_is_lag_master(info->upper_dev)) {
1474                 if (info->linking)
1475                         err = ocelot_netdevice_lag_join(dev, info->upper_dev,
1476                                                         info->upper_info, extack);
1477                 else
1478                         ocelot_netdevice_lag_leave(dev, info->upper_dev);
1479         }
1480
1481         return notifier_from_errno(err);
1482 }
1483
1484 /* Treat CHANGEUPPER events on an offloaded LAG as individual CHANGEUPPER
1485  * events for the lower physical ports of the LAG.
1486  * If the LAG upper isn't offloaded, ignore its CHANGEUPPER events.
1487  * In case the LAG joined a bridge, notify that we are offloading it and can do
1488  * forwarding in hardware towards it.
1489  */
1490 static int
1491 ocelot_netdevice_lag_changeupper(struct net_device *dev,
1492                                  struct netdev_notifier_changeupper_info *info)
1493 {
1494         struct net_device *lower;
1495         struct list_head *iter;
1496         int err = NOTIFY_DONE;
1497
1498         netdev_for_each_lower_dev(dev, lower, iter) {
1499                 struct ocelot_port_private *priv = netdev_priv(lower);
1500                 struct ocelot_port *ocelot_port = &priv->port;
1501
1502                 if (ocelot_port->bond != dev)
1503                         return NOTIFY_OK;
1504
1505                 err = ocelot_netdevice_changeupper(lower, dev, info);
1506                 if (err)
1507                         return notifier_from_errno(err);
1508         }
1509
1510         return NOTIFY_DONE;
1511 }
1512
1513 static int
1514 ocelot_netdevice_prechangeupper(struct net_device *dev,
1515                                 struct net_device *brport_dev,
1516                                 struct netdev_notifier_changeupper_info *info)
1517 {
1518         if (netif_is_bridge_master(info->upper_dev) && !info->linking)
1519                 ocelot_netdevice_pre_bridge_leave(dev, brport_dev);
1520
1521         if (netif_is_lag_master(info->upper_dev) && !info->linking)
1522                 ocelot_netdevice_pre_lag_leave(dev, info->upper_dev);
1523
1524         return NOTIFY_DONE;
1525 }
1526
1527 static int
1528 ocelot_netdevice_lag_prechangeupper(struct net_device *dev,
1529                                     struct netdev_notifier_changeupper_info *info)
1530 {
1531         struct net_device *lower;
1532         struct list_head *iter;
1533         int err = NOTIFY_DONE;
1534
1535         netdev_for_each_lower_dev(dev, lower, iter) {
1536                 struct ocelot_port_private *priv = netdev_priv(lower);
1537                 struct ocelot_port *ocelot_port = &priv->port;
1538
1539                 if (ocelot_port->bond != dev)
1540                         return NOTIFY_OK;
1541
1542                 err = ocelot_netdevice_prechangeupper(dev, lower, info);
1543                 if (err)
1544                         return err;
1545         }
1546
1547         return NOTIFY_DONE;
1548 }
1549
1550 static int
1551 ocelot_netdevice_changelowerstate(struct net_device *dev,
1552                                   struct netdev_lag_lower_state_info *info)
1553 {
1554         struct ocelot_port_private *priv = netdev_priv(dev);
1555         bool is_active = info->link_up && info->tx_enabled;
1556         struct ocelot_port *ocelot_port = &priv->port;
1557         struct ocelot *ocelot = ocelot_port->ocelot;
1558         int port = priv->port.index;
1559
1560         if (!ocelot_port->bond)
1561                 return NOTIFY_DONE;
1562
1563         if (ocelot_port->lag_tx_active == is_active)
1564                 return NOTIFY_DONE;
1565
1566         ocelot_port_lag_change(ocelot, port, is_active);
1567
1568         return NOTIFY_OK;
1569 }
1570
1571 static int ocelot_netdevice_event(struct notifier_block *unused,
1572                                   unsigned long event, void *ptr)
1573 {
1574         struct net_device *dev = netdev_notifier_info_to_dev(ptr);
1575
1576         switch (event) {
1577         case NETDEV_PRECHANGEUPPER: {
1578                 struct netdev_notifier_changeupper_info *info = ptr;
1579
1580                 if (ocelot_netdevice_dev_check(dev))
1581                         return ocelot_netdevice_prechangeupper(dev, dev, info);
1582
1583                 if (netif_is_lag_master(dev))
1584                         return ocelot_netdevice_lag_prechangeupper(dev, info);
1585
1586                 break;
1587         }
1588         case NETDEV_CHANGEUPPER: {
1589                 struct netdev_notifier_changeupper_info *info = ptr;
1590
1591                 if (ocelot_netdevice_dev_check(dev))
1592                         return ocelot_netdevice_changeupper(dev, dev, info);
1593
1594                 if (netif_is_lag_master(dev))
1595                         return ocelot_netdevice_lag_changeupper(dev, info);
1596
1597                 break;
1598         }
1599         case NETDEV_CHANGELOWERSTATE: {
1600                 struct netdev_notifier_changelowerstate_info *info = ptr;
1601
1602                 if (!ocelot_netdevice_dev_check(dev))
1603                         break;
1604
1605                 return ocelot_netdevice_changelowerstate(dev,
1606                                                          info->lower_state_info);
1607         }
1608         default:
1609                 break;
1610         }
1611
1612         return NOTIFY_DONE;
1613 }
1614
1615 struct notifier_block ocelot_netdevice_nb __read_mostly = {
1616         .notifier_call = ocelot_netdevice_event,
1617 };
1618
1619 static int ocelot_switchdev_event(struct notifier_block *unused,
1620                                   unsigned long event, void *ptr)
1621 {
1622         struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
1623         int err;
1624
1625         switch (event) {
1626         case SWITCHDEV_PORT_ATTR_SET:
1627                 err = switchdev_handle_port_attr_set(dev, ptr,
1628                                                      ocelot_netdevice_dev_check,
1629                                                      ocelot_port_attr_set);
1630                 return notifier_from_errno(err);
1631         }
1632
1633         return NOTIFY_DONE;
1634 }
1635
1636 struct notifier_block ocelot_switchdev_nb __read_mostly = {
1637         .notifier_call = ocelot_switchdev_event,
1638 };
1639
1640 static int ocelot_switchdev_blocking_event(struct notifier_block *unused,
1641                                            unsigned long event, void *ptr)
1642 {
1643         struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
1644         int err;
1645
1646         switch (event) {
1647                 /* Blocking events. */
1648         case SWITCHDEV_PORT_OBJ_ADD:
1649                 err = switchdev_handle_port_obj_add(dev, ptr,
1650                                                     ocelot_netdevice_dev_check,
1651                                                     ocelot_port_obj_add);
1652                 return notifier_from_errno(err);
1653         case SWITCHDEV_PORT_OBJ_DEL:
1654                 err = switchdev_handle_port_obj_del(dev, ptr,
1655                                                     ocelot_netdevice_dev_check,
1656                                                     ocelot_port_obj_del);
1657                 return notifier_from_errno(err);
1658         case SWITCHDEV_PORT_ATTR_SET:
1659                 err = switchdev_handle_port_attr_set(dev, ptr,
1660                                                      ocelot_netdevice_dev_check,
1661                                                      ocelot_port_attr_set);
1662                 return notifier_from_errno(err);
1663         }
1664
1665         return NOTIFY_DONE;
1666 }
1667
1668 struct notifier_block ocelot_switchdev_blocking_nb __read_mostly = {
1669         .notifier_call = ocelot_switchdev_blocking_event,
1670 };
1671
1672 static void vsc7514_phylink_mac_config(struct phylink_config *config,
1673                                        unsigned int link_an_mode,
1674                                        const struct phylink_link_state *state)
1675 {
1676         struct net_device *ndev = to_net_dev(config->dev);
1677         struct ocelot_port_private *priv = netdev_priv(ndev);
1678         struct ocelot *ocelot = priv->port.ocelot;
1679         int port = priv->port.index;
1680
1681         ocelot_phylink_mac_config(ocelot, port, link_an_mode, state);
1682 }
1683
1684 static void vsc7514_phylink_mac_link_down(struct phylink_config *config,
1685                                           unsigned int link_an_mode,
1686                                           phy_interface_t interface)
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_link_down(ocelot, port, link_an_mode, interface,
1694                                      OCELOT_MAC_QUIRKS);
1695 }
1696
1697 static void vsc7514_phylink_mac_link_up(struct phylink_config *config,
1698                                         struct phy_device *phydev,
1699                                         unsigned int link_an_mode,
1700                                         phy_interface_t interface,
1701                                         int speed, int duplex,
1702                                         bool tx_pause, bool rx_pause)
1703 {
1704         struct net_device *ndev = to_net_dev(config->dev);
1705         struct ocelot_port_private *priv = netdev_priv(ndev);
1706         struct ocelot *ocelot = priv->port.ocelot;
1707         int port = priv->port.index;
1708
1709         ocelot_phylink_mac_link_up(ocelot, port, phydev, link_an_mode,
1710                                    interface, speed, duplex,
1711                                    tx_pause, rx_pause, OCELOT_MAC_QUIRKS);
1712 }
1713
1714 static const struct phylink_mac_ops ocelot_phylink_ops = {
1715         .mac_config             = vsc7514_phylink_mac_config,
1716         .mac_link_down          = vsc7514_phylink_mac_link_down,
1717         .mac_link_up            = vsc7514_phylink_mac_link_up,
1718 };
1719
1720 static int ocelot_port_phylink_create(struct ocelot *ocelot, int port,
1721                                       struct device_node *portnp)
1722 {
1723         struct ocelot_port *ocelot_port = ocelot->ports[port];
1724         struct ocelot_port_private *priv;
1725         struct device *dev = ocelot->dev;
1726         phy_interface_t phy_mode;
1727         struct phylink *phylink;
1728         int err;
1729
1730         of_get_phy_mode(portnp, &phy_mode);
1731         /* DT bindings of internal PHY ports are broken and don't
1732          * specify a phy-mode
1733          */
1734         if (phy_mode == PHY_INTERFACE_MODE_NA)
1735                 phy_mode = PHY_INTERFACE_MODE_INTERNAL;
1736
1737         if (phy_mode != PHY_INTERFACE_MODE_SGMII &&
1738             phy_mode != PHY_INTERFACE_MODE_QSGMII &&
1739             phy_mode != PHY_INTERFACE_MODE_INTERNAL) {
1740                 dev_err(dev, "unsupported phy mode %s for port %d\n",
1741                         phy_modes(phy_mode), port);
1742                 return -EINVAL;
1743         }
1744
1745         ocelot_port->phy_mode = phy_mode;
1746
1747         err = ocelot_port_configure_serdes(ocelot, port, portnp);
1748         if (err)
1749                 return err;
1750
1751         priv = container_of(ocelot_port, struct ocelot_port_private, port);
1752
1753         priv->phylink_config.dev = &priv->dev->dev;
1754         priv->phylink_config.type = PHYLINK_NETDEV;
1755         priv->phylink_config.mac_capabilities = MAC_ASYM_PAUSE | MAC_SYM_PAUSE |
1756                 MAC_10 | MAC_100 | MAC_1000FD | MAC_2500FD;
1757
1758         __set_bit(ocelot_port->phy_mode,
1759                   priv->phylink_config.supported_interfaces);
1760
1761         phylink = phylink_create(&priv->phylink_config,
1762                                  of_fwnode_handle(portnp),
1763                                  phy_mode, &ocelot_phylink_ops);
1764         if (IS_ERR(phylink)) {
1765                 err = PTR_ERR(phylink);
1766                 dev_err(dev, "Could not create phylink (%pe)\n", phylink);
1767                 return err;
1768         }
1769
1770         priv->phylink = phylink;
1771
1772         err = phylink_of_phy_connect(phylink, portnp, 0);
1773         if (err) {
1774                 dev_err(dev, "Could not connect to PHY: %pe\n", ERR_PTR(err));
1775                 phylink_destroy(phylink);
1776                 priv->phylink = NULL;
1777                 return err;
1778         }
1779
1780         return 0;
1781 }
1782
1783 int ocelot_probe_port(struct ocelot *ocelot, int port, struct regmap *target,
1784                       struct device_node *portnp)
1785 {
1786         struct ocelot_port_private *priv;
1787         struct ocelot_port *ocelot_port;
1788         struct net_device *dev;
1789         int err;
1790
1791         dev = alloc_etherdev(sizeof(struct ocelot_port_private));
1792         if (!dev)
1793                 return -ENOMEM;
1794         SET_NETDEV_DEV(dev, ocelot->dev);
1795         priv = netdev_priv(dev);
1796         priv->dev = dev;
1797         ocelot_port = &priv->port;
1798         ocelot_port->ocelot = ocelot;
1799         ocelot_port->index = port;
1800         ocelot_port->target = target;
1801         ocelot->ports[port] = ocelot_port;
1802
1803         dev->netdev_ops = &ocelot_port_netdev_ops;
1804         dev->ethtool_ops = &ocelot_ethtool_ops;
1805         dev->max_mtu = OCELOT_JUMBO_MTU;
1806
1807         dev->hw_features |= NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_RXFCS |
1808                 NETIF_F_HW_TC;
1809         dev->features |= NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_HW_TC;
1810
1811         err = of_get_ethdev_address(portnp, dev);
1812         if (err)
1813                 eth_hw_addr_gen(dev, ocelot->base_mac, port);
1814
1815         ocelot_mact_learn(ocelot, PGID_CPU, dev->dev_addr,
1816                           OCELOT_STANDALONE_PVID, ENTRYTYPE_LOCKED);
1817
1818         ocelot_init_port(ocelot, port);
1819
1820         err = ocelot_port_phylink_create(ocelot, port, portnp);
1821         if (err)
1822                 goto out;
1823
1824         if (ocelot->fdma)
1825                 ocelot_fdma_netdev_init(ocelot, dev);
1826
1827         SET_NETDEV_DEVLINK_PORT(dev, &ocelot->devlink_ports[port]);
1828         err = register_netdev(dev);
1829         if (err) {
1830                 dev_err(ocelot->dev, "register_netdev failed\n");
1831                 goto out_fdma_deinit;
1832         }
1833
1834         return 0;
1835
1836 out_fdma_deinit:
1837         if (ocelot->fdma)
1838                 ocelot_fdma_netdev_deinit(ocelot, dev);
1839 out:
1840         ocelot->ports[port] = NULL;
1841         free_netdev(dev);
1842
1843         return err;
1844 }
1845
1846 void ocelot_release_port(struct ocelot_port *ocelot_port)
1847 {
1848         struct ocelot_port_private *priv = container_of(ocelot_port,
1849                                                 struct ocelot_port_private,
1850                                                 port);
1851         struct ocelot *ocelot = ocelot_port->ocelot;
1852         struct ocelot_fdma *fdma = ocelot->fdma;
1853
1854         unregister_netdev(priv->dev);
1855
1856         if (fdma)
1857                 ocelot_fdma_netdev_deinit(ocelot, priv->dev);
1858
1859         if (priv->phylink) {
1860                 rtnl_lock();
1861                 phylink_disconnect_phy(priv->phylink);
1862                 rtnl_unlock();
1863
1864                 phylink_destroy(priv->phylink);
1865         }
1866
1867         free_netdev(priv->dev);
1868 }