drivers/net/ethernet/marvell/prestera/prestera_main.c

   1 // SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
   2 /* Copyright (c) 2019-2020 Marvell International Ltd. All rights reserved */
   3
   4 #include <linux/etherdevice.h>
   5 #include <linux/jiffies.h>
   6 #include <linux/list.h>
   7 #include <linux/module.h>
   8 #include <linux/netdev_features.h>
   9 #include <linux/of.h>
  10 #include <linux/of_net.h>
  11 #include <linux/if_vlan.h>
  12 #include <linux/phylink.h>
  13
  14 #include "prestera.h"
  15 #include "prestera_hw.h"
  16 #include "prestera_acl.h"
  17 #include "prestera_flow.h"
  18 #include "prestera_span.h"
  19 #include "prestera_rxtx.h"
  20 #include "prestera_devlink.h"
  21 #include "prestera_ethtool.h"
  22 #include "prestera_counter.h"
  23 #include "prestera_switchdev.h"
  24
  25 #define PRESTERA_MTU_DEFAULT    1536
  26
  27 #define PRESTERA_STATS_DELAY_MS 1000
  28
  29 #define PRESTERA_MAC_ADDR_NUM_MAX       255
  30
  31 static struct workqueue_struct *prestera_wq;
  32 static struct workqueue_struct *prestera_owq;
  33
  34 void prestera_queue_work(struct work_struct *work)
  35 {
  36         queue_work(prestera_owq, work);
  37 }
  38
  39 void prestera_queue_delayed_work(struct delayed_work *work, unsigned long delay)
  40 {
  41         queue_delayed_work(prestera_wq, work, delay);
  42 }
  43
  44 void prestera_queue_drain(void)
  45 {
  46         drain_workqueue(prestera_wq);
  47         drain_workqueue(prestera_owq);
  48 }
  49
  50 int prestera_port_learning_set(struct prestera_port *port, bool learn)
  51 {
  52         return prestera_hw_port_learning_set(port, learn);
  53 }
  54
  55 int prestera_port_uc_flood_set(struct prestera_port *port, bool flood)
  56 {
  57         return prestera_hw_port_uc_flood_set(port, flood);
  58 }
  59
  60 int prestera_port_mc_flood_set(struct prestera_port *port, bool flood)
  61 {
  62         return prestera_hw_port_mc_flood_set(port, flood);
  63 }
  64
  65 int prestera_port_br_locked_set(struct prestera_port *port, bool br_locked)
  66 {
  67         return prestera_hw_port_br_locked_set(port, br_locked);
  68 }
  69
  70 int prestera_port_pvid_set(struct prestera_port *port, u16 vid)
  71 {
  72         enum prestera_accept_frm_type frm_type;
  73         int err;
  74
  75         frm_type = PRESTERA_ACCEPT_FRAME_TYPE_TAGGED;
  76
  77         if (vid) {
  78                 err = prestera_hw_vlan_port_vid_set(port, vid);
  79                 if (err)
  80                         return err;
  81
  82                 frm_type = PRESTERA_ACCEPT_FRAME_TYPE_ALL;
  83         }
  84
  85         err = prestera_hw_port_accept_frm_type(port, frm_type);
  86         if (err && frm_type == PRESTERA_ACCEPT_FRAME_TYPE_ALL)
  87                 prestera_hw_vlan_port_vid_set(port, port->pvid);
  88
  89         port->pvid = vid;
  90         return 0;
  91 }
  92
  93 struct prestera_port *prestera_port_find_by_hwid(struct prestera_switch *sw,
  94                                                  u32 dev_id, u32 hw_id)
  95 {
  96         struct prestera_port *port = NULL, *tmp;
  97
  98         read_lock(&sw->port_list_lock);
  99         list_for_each_entry(tmp, &sw->port_list, list) {
 100                 if (tmp->dev_id == dev_id && tmp->hw_id == hw_id) {
 101                         port = tmp;
 102                         break;
 103                 }
 104         }
 105         read_unlock(&sw->port_list_lock);
 106
 107         return port;
 108 }
 109
 110 struct prestera_port *prestera_find_port(struct prestera_switch *sw, u32 id)
 111 {
 112         struct prestera_port *port = NULL, *tmp;
 113
 114         read_lock(&sw->port_list_lock);
 115         list_for_each_entry(tmp, &sw->port_list, list) {
 116                 if (tmp->id == id) {
 117                         port = tmp;
 118                         break;
 119                 }
 120         }
 121         read_unlock(&sw->port_list_lock);
 122
 123         return port;
 124 }
 125
 126 struct prestera_switch *prestera_switch_get(struct net_device *dev)
 127 {
 128         struct prestera_port *port;
 129
 130         port = prestera_port_dev_lower_find(dev);
 131         return port ? port->sw : NULL;
 132 }
 133
 134 int prestera_port_cfg_mac_read(struct prestera_port *port,
 135                                struct prestera_port_mac_config *cfg)
 136 {
 137         *cfg = port->cfg_mac;
 138         return 0;
 139 }
 140
 141 int prestera_port_cfg_mac_write(struct prestera_port *port,
 142                                 struct prestera_port_mac_config *cfg)
 143 {
 144         int err;
 145
 146         err = prestera_hw_port_mac_mode_set(port, cfg->admin,
 147                                             cfg->mode, cfg->inband, cfg->speed,
 148                                             cfg->duplex, cfg->fec);
 149         if (err)
 150                 return err;
 151
 152         port->cfg_mac = *cfg;
 153         return 0;
 154 }
 155
 156 static int prestera_port_open(struct net_device *dev)
 157 {
 158         struct prestera_port *port = netdev_priv(dev);
 159         struct prestera_port_mac_config cfg_mac;
 160         int err = 0;
 161
 162         if (port->phy_link) {
 163                 phylink_start(port->phy_link);
 164         } else {
 165                 if (port->caps.transceiver == PRESTERA_PORT_TCVR_SFP) {
 166                         err = prestera_port_cfg_mac_read(port, &cfg_mac);
 167                         if (!err) {
 168                                 cfg_mac.admin = true;
 169                                 err = prestera_port_cfg_mac_write(port,
 170                                                                   &cfg_mac);
 171                         }
 172                 } else {
 173                         port->cfg_phy.admin = true;
 174                         err = prestera_hw_port_phy_mode_set(port, true,
 175                                                             port->autoneg,
 176                                                             port->cfg_phy.mode,
 177                                                             port->adver_link_modes,
 178                                                             port->cfg_phy.mdix);
 179                 }
 180         }
 181
 182         netif_start_queue(dev);
 183
 184         return err;
 185 }
 186
 187 static int prestera_port_close(struct net_device *dev)
 188 {
 189         struct prestera_port *port = netdev_priv(dev);
 190         struct prestera_port_mac_config cfg_mac;
 191         int err = 0;
 192
 193         netif_stop_queue(dev);
 194
 195         if (port->phy_link) {
 196                 phylink_stop(port->phy_link);
 197                 phylink_disconnect_phy(port->phy_link);
 198                 err = prestera_port_cfg_mac_read(port, &cfg_mac);
 199                 if (!err) {
 200                         cfg_mac.admin = false;
 201                         prestera_port_cfg_mac_write(port, &cfg_mac);
 202                 }
 203         } else {
 204                 if (port->caps.transceiver == PRESTERA_PORT_TCVR_SFP) {
 205                         err = prestera_port_cfg_mac_read(port, &cfg_mac);
 206                         if (!err) {
 207                                 cfg_mac.admin = false;
 208                                 prestera_port_cfg_mac_write(port, &cfg_mac);
 209                         }
 210                 } else {
 211                         port->cfg_phy.admin = false;
 212                         err = prestera_hw_port_phy_mode_set(port, false, port->autoneg,
 213                                                             port->cfg_phy.mode,
 214                                                             port->adver_link_modes,
 215                                                             port->cfg_phy.mdix);
 216                 }
 217         }
 218
 219         return err;
 220 }
 221
 222 static void
 223 prestera_port_mac_state_cache_read(struct prestera_port *port,
 224                                    struct prestera_port_mac_state *state)
 225 {
 226         spin_lock(&port->state_mac_lock);
 227         *state = port->state_mac;
 228         spin_unlock(&port->state_mac_lock);
 229 }
 230
 231 static void
 232 prestera_port_mac_state_cache_write(struct prestera_port *port,
 233                                     struct prestera_port_mac_state *state)
 234 {
 235         spin_lock(&port->state_mac_lock);
 236         port->state_mac = *state;
 237         spin_unlock(&port->state_mac_lock);
 238 }
 239
 240 static struct prestera_port *prestera_pcs_to_port(struct phylink_pcs *pcs)
 241 {
 242         return container_of(pcs, struct prestera_port, phylink_pcs);
 243 }
 244
 245 static void prestera_mac_config(struct phylink_config *config,
 246                                 unsigned int an_mode,
 247                                 const struct phylink_link_state *state)
 248 {
 249 }
 250
 251 static void prestera_mac_link_down(struct phylink_config *config,
 252                                    unsigned int mode, phy_interface_t interface)
 253 {
 254         struct net_device *ndev = to_net_dev(config->dev);
 255         struct prestera_port *port = netdev_priv(ndev);
 256         struct prestera_port_mac_state state_mac;
 257
 258         /* Invalidate. Parameters will update on next link event. */
 259         memset(&state_mac, 0, sizeof(state_mac));
 260         state_mac.valid = false;
 261         prestera_port_mac_state_cache_write(port, &state_mac);
 262 }
 263
 264 static void prestera_mac_link_up(struct phylink_config *config,
 265                                  struct phy_device *phy,
 266                                  unsigned int mode, phy_interface_t interface,
 267                                  int speed, int duplex,
 268                                  bool tx_pause, bool rx_pause)
 269 {
 270 }
 271
 272 static struct phylink_pcs *
 273 prestera_mac_select_pcs(struct phylink_config *config,
 274                         phy_interface_t interface)
 275 {
 276         struct net_device *dev = to_net_dev(config->dev);
 277         struct prestera_port *port = netdev_priv(dev);
 278
 279         return &port->phylink_pcs;
 280 }
 281
 282 static void prestera_pcs_get_state(struct phylink_pcs *pcs,
 283                                    struct phylink_link_state *state)
 284 {
 285         struct prestera_port *port = container_of(pcs, struct prestera_port,
 286                                                   phylink_pcs);
 287         struct prestera_port_mac_state smac;
 288
 289         prestera_port_mac_state_cache_read(port, &smac);
 290
 291         if (smac.valid) {
 292                 state->link = smac.oper ? 1 : 0;
 293                 /* AN is completed, when port is up */
 294                 state->an_complete = (smac.oper && port->autoneg) ? 1 : 0;
 295                 state->speed = smac.speed;
 296                 state->duplex = smac.duplex;
 297         } else {
 298                 state->link = 0;
 299                 state->an_complete = 0;
 300         }
 301 }
 302
 303 static int prestera_pcs_config(struct phylink_pcs *pcs, unsigned int neg_mode,
 304                                phy_interface_t interface,
 305                                const unsigned long *advertising,
 306                                bool permit_pause_to_mac)
 307 {
 308         struct prestera_port *port = prestera_pcs_to_port(pcs);
 309         struct prestera_port_mac_config cfg_mac;
 310         int err;
 311
 312         err = prestera_port_cfg_mac_read(port, &cfg_mac);
 313         if (err)
 314                 return err;
 315
 316         cfg_mac.admin = true;
 317         cfg_mac.fec = PRESTERA_PORT_FEC_OFF;
 318         cfg_mac.inband = neg_mode == PHYLINK_PCS_NEG_INBAND_ENABLED;
 319
 320         switch (interface) {
 321         case PHY_INTERFACE_MODE_10GBASER:
 322                 cfg_mac.speed = SPEED_10000;
 323                 cfg_mac.mode = PRESTERA_MAC_MODE_SR_LR;
 324                 break;
 325         case PHY_INTERFACE_MODE_2500BASEX:
 326                 cfg_mac.speed = SPEED_2500;
 327                 cfg_mac.duplex = DUPLEX_FULL;
 328                 cfg_mac.mode = PRESTERA_MAC_MODE_SGMII;
 329                 break;
 330         case PHY_INTERFACE_MODE_SGMII:
 331                 cfg_mac.mode = PRESTERA_MAC_MODE_SGMII;
 332                 break;
 333         case PHY_INTERFACE_MODE_1000BASEX:
 334         default:
 335                 cfg_mac.speed = SPEED_1000;
 336                 cfg_mac.duplex = DUPLEX_FULL;
 337                 cfg_mac.mode = PRESTERA_MAC_MODE_1000BASE_X;
 338                 break;
 339         }
 340
 341         err = prestera_port_cfg_mac_write(port, &cfg_mac);
 342         if (err)
 343                 return err;
 344
 345         return 0;
 346 }
 347
 348 static void prestera_pcs_an_restart(struct phylink_pcs *pcs)
 349 {
 350         /* TODO: add 1000basex AN restart support
 351          * (Currently FW has no support for 1000baseX AN restart, but it will in the future,
 352          * so as for now the function would stay empty.)
 353          */
 354 }
 355
 356 static const struct phylink_mac_ops prestera_mac_ops = {
 357         .mac_select_pcs = prestera_mac_select_pcs,
 358         .mac_config = prestera_mac_config,
 359         .mac_link_down = prestera_mac_link_down,
 360         .mac_link_up = prestera_mac_link_up,
 361 };
 362
 363 static const struct phylink_pcs_ops prestera_pcs_ops = {
 364         .pcs_get_state = prestera_pcs_get_state,
 365         .pcs_config = prestera_pcs_config,
 366         .pcs_an_restart = prestera_pcs_an_restart,
 367 };
 368
 369 static int prestera_port_sfp_bind(struct prestera_port *port)
 370 {
 371         struct prestera_switch *sw = port->sw;
 372         struct device_node *ports, *node;
 373         struct fwnode_handle *fwnode;
 374         struct phylink *phy_link;
 375         int err;
 376
 377         if (!sw->np)
 378                 return 0;
 379
 380         of_node_get(sw->np);
 381         ports = of_find_node_by_name(sw->np, "ports");
 382
 383         for_each_child_of_node(ports, node) {
 384                 int num;
 385
 386                 err = of_property_read_u32(node, "prestera,port-num", &num);
 387                 if (err) {
 388                         dev_err(sw->dev->dev,
 389                                 "device node %pOF has no valid reg property: %d\n",
 390                                 node, err);
 391                         goto out;
 392                 }
 393
 394                 if (port->fp_id != num)
 395                         continue;
 396
 397                 port->phylink_pcs.ops = &prestera_pcs_ops;
 398                 port->phylink_pcs.neg_mode = true;
 399
 400                 port->phy_config.dev = &port->dev->dev;
 401                 port->phy_config.type = PHYLINK_NETDEV;
 402
 403                 fwnode = of_fwnode_handle(node);
 404
 405                 __set_bit(PHY_INTERFACE_MODE_10GBASER,
 406                           port->phy_config.supported_interfaces);
 407                 __set_bit(PHY_INTERFACE_MODE_2500BASEX,
 408                           port->phy_config.supported_interfaces);
 409                 __set_bit(PHY_INTERFACE_MODE_SGMII,
 410                           port->phy_config.supported_interfaces);
 411                 __set_bit(PHY_INTERFACE_MODE_1000BASEX,
 412                           port->phy_config.supported_interfaces);
 413
 414                 port->phy_config.mac_capabilities =
 415                         MAC_1000 | MAC_2500FD | MAC_10000FD;
 416
 417                 phy_link = phylink_create(&port->phy_config, fwnode,
 418                                           PHY_INTERFACE_MODE_INTERNAL,
 419                                           &prestera_mac_ops);
 420                 if (IS_ERR(phy_link)) {
 421                         netdev_err(port->dev, "failed to create phylink\n");
 422                         err = PTR_ERR(phy_link);
 423                         goto out;
 424                 }
 425
 426                 port->phy_link = phy_link;
 427                 break;
 428         }
 429
 430 out:
 431         of_node_put(node);
 432         of_node_put(ports);
 433         return err;
 434 }
 435
 436 static int prestera_port_sfp_unbind(struct prestera_port *port)
 437 {
 438         if (port->phy_link)
 439                 phylink_destroy(port->phy_link);
 440
 441         return 0;
 442 }
 443
 444 static netdev_tx_t prestera_port_xmit(struct sk_buff *skb,
 445                                       struct net_device *dev)
 446 {
 447         return prestera_rxtx_xmit(netdev_priv(dev), skb);
 448 }
 449
 450 int prestera_is_valid_mac_addr(struct prestera_port *port, const u8 *addr)
 451 {
 452         if (!is_valid_ether_addr(addr))
 453                 return -EADDRNOTAVAIL;
 454
 455         /* firmware requires that port's MAC address contains first 5 bytes
 456          * of the base MAC address
 457          */
 458         if (memcmp(port->sw->base_mac, addr, ETH_ALEN - 1))
 459                 return -EINVAL;
 460
 461         return 0;
 462 }
 463
 464 static int prestera_port_set_mac_address(struct net_device *dev, void *p)
 465 {
 466         struct prestera_port *port = netdev_priv(dev);
 467         struct sockaddr *addr = p;
 468         int err;
 469
 470         err = prestera_is_valid_mac_addr(port, addr->sa_data);
 471         if (err)
 472                 return err;
 473
 474         err = prestera_hw_port_mac_set(port, addr->sa_data);
 475         if (err)
 476                 return err;
 477
 478         eth_hw_addr_set(dev, addr->sa_data);
 479
 480         return 0;
 481 }
 482
 483 static int prestera_port_change_mtu(struct net_device *dev, int mtu)
 484 {
 485         struct prestera_port *port = netdev_priv(dev);
 486         int err;
 487
 488         err = prestera_hw_port_mtu_set(port, mtu);
 489         if (err)
 490                 return err;
 491
 492         dev->mtu = mtu;
 493
 494         return 0;
 495 }
 496
 497 static void prestera_port_get_stats64(struct net_device *dev,
 498                                       struct rtnl_link_stats64 *stats)
 499 {
 500         struct prestera_port *port = netdev_priv(dev);
 501         struct prestera_port_stats *port_stats = &port->cached_hw_stats.stats;
 502
 503         stats->rx_packets = port_stats->broadcast_frames_received +
 504                                 port_stats->multicast_frames_received +
 505                                 port_stats->unicast_frames_received;
 506
 507         stats->tx_packets = port_stats->broadcast_frames_sent +
 508                                 port_stats->multicast_frames_sent +
 509                                 port_stats->unicast_frames_sent;
 510
 511         stats->rx_bytes = port_stats->good_octets_received;
 512
 513         stats->tx_bytes = port_stats->good_octets_sent;
 514
 515         stats->rx_errors = port_stats->rx_error_frame_received;
 516         stats->tx_errors = port_stats->mac_trans_error;
 517
 518         stats->rx_dropped = port_stats->buffer_overrun;
 519         stats->tx_dropped = 0;
 520
 521         stats->multicast = port_stats->multicast_frames_received;
 522         stats->collisions = port_stats->excessive_collision;
 523
 524         stats->rx_crc_errors = port_stats->bad_crc;
 525 }
 526
 527 static void prestera_port_get_hw_stats(struct prestera_port *port)
 528 {
 529         prestera_hw_port_stats_get(port, &port->cached_hw_stats.stats);
 530 }
 531
 532 static void prestera_port_stats_update(struct work_struct *work)
 533 {
 534         struct prestera_port *port =
 535                 container_of(work, struct prestera_port,
 536                              cached_hw_stats.caching_dw.work);
 537
 538         prestera_port_get_hw_stats(port);
 539
 540         queue_delayed_work(prestera_wq, &port->cached_hw_stats.caching_dw,
 541                            msecs_to_jiffies(PRESTERA_STATS_DELAY_MS));
 542 }
 543
 544 static int prestera_port_setup_tc(struct net_device *dev,
 545                                   enum tc_setup_type type,
 546                                   void *type_data)
 547 {
 548         struct prestera_port *port = netdev_priv(dev);
 549
 550         switch (type) {
 551         case TC_SETUP_BLOCK:
 552                 return prestera_flow_block_setup(port, type_data);
 553         default:
 554                 return -EOPNOTSUPP;
 555         }
 556 }
 557
 558 static const struct net_device_ops prestera_netdev_ops = {
 559         .ndo_open = prestera_port_open,
 560         .ndo_stop = prestera_port_close,
 561         .ndo_start_xmit = prestera_port_xmit,
 562         .ndo_setup_tc = prestera_port_setup_tc,
 563         .ndo_change_mtu = prestera_port_change_mtu,
 564         .ndo_get_stats64 = prestera_port_get_stats64,
 565         .ndo_set_mac_address = prestera_port_set_mac_address,
 566 };
 567
 568 int prestera_port_autoneg_set(struct prestera_port *port, u64 link_modes)
 569 {
 570         int err;
 571
 572         if (port->autoneg && port->adver_link_modes == link_modes)
 573                 return 0;
 574
 575         err = prestera_hw_port_phy_mode_set(port, port->cfg_phy.admin,
 576                                             true, 0, link_modes,
 577                                             port->cfg_phy.mdix);
 578         if (err)
 579                 return err;
 580
 581         port->adver_fec = BIT(PRESTERA_PORT_FEC_OFF);
 582         port->adver_link_modes = link_modes;
 583         port->cfg_phy.mode = 0;
 584         port->autoneg = true;
 585
 586         return 0;
 587 }
 588
 589 static void prestera_port_list_add(struct prestera_port *port)
 590 {
 591         write_lock(&port->sw->port_list_lock);
 592         list_add(&port->list, &port->sw->port_list);
 593         write_unlock(&port->sw->port_list_lock);
 594 }
 595
 596 static void prestera_port_list_del(struct prestera_port *port)
 597 {
 598         write_lock(&port->sw->port_list_lock);
 599         list_del(&port->list);
 600         write_unlock(&port->sw->port_list_lock);
 601 }
 602
 603 static int prestera_port_create(struct prestera_switch *sw, u32 id)
 604 {
 605         struct prestera_port_mac_config cfg_mac;
 606         struct prestera_port *port;
 607         struct net_device *dev;
 608         int err;
 609
 610         dev = alloc_etherdev(sizeof(*port));
 611         if (!dev)
 612                 return -ENOMEM;
 613
 614         port = netdev_priv(dev);
 615
 616         INIT_LIST_HEAD(&port->vlans_list);
 617         port->pvid = PRESTERA_DEFAULT_VID;
 618         port->lag = NULL;
 619         port->dev = dev;
 620         port->id = id;
 621         port->sw = sw;
 622
 623         spin_lock_init(&port->state_mac_lock);
 624
 625         err = prestera_hw_port_info_get(port, &port->dev_id, &port->hw_id,
 626                                         &port->fp_id);
 627         if (err) {
 628                 dev_err(prestera_dev(sw), "Failed to get port(%u) info\n", id);
 629                 goto err_port_info_get;
 630         }
 631
 632         err = prestera_devlink_port_register(port);
 633         if (err)
 634                 goto err_dl_port_register;
 635
 636         dev->features |= NETIF_F_NETNS_LOCAL | NETIF_F_HW_TC;
 637         dev->netdev_ops = &prestera_netdev_ops;
 638         dev->ethtool_ops = &prestera_ethtool_ops;
 639         SET_NETDEV_DEV(dev, sw->dev->dev);
 640         SET_NETDEV_DEVLINK_PORT(dev, &port->dl_port);
 641
 642         if (port->caps.transceiver != PRESTERA_PORT_TCVR_SFP)
 643                 netif_carrier_off(dev);
 644
 645         dev->mtu = min_t(unsigned int, sw->mtu_max, PRESTERA_MTU_DEFAULT);
 646         dev->min_mtu = sw->mtu_min;
 647         dev->max_mtu = sw->mtu_max;
 648
 649         err = prestera_hw_port_mtu_set(port, dev->mtu);
 650         if (err) {
 651                 dev_err(prestera_dev(sw), "Failed to set port(%u) mtu(%d)\n",
 652                         id, dev->mtu);
 653                 goto err_port_init;
 654         }
 655
 656         if (port->fp_id >= PRESTERA_MAC_ADDR_NUM_MAX) {
 657                 err = -EINVAL;
 658                 goto err_port_init;
 659         }
 660
 661         eth_hw_addr_gen(dev, sw->base_mac, port->fp_id);
 662         /* firmware requires that port's MAC address consist of the first
 663          * 5 bytes of the base MAC address
 664          */
 665         if (memcmp(dev->dev_addr, sw->base_mac, ETH_ALEN - 1)) {
 666                 dev_warn(prestera_dev(sw), "Port MAC address wraps for port(%u)\n", id);
 667                 dev_addr_mod(dev, 0, sw->base_mac, ETH_ALEN - 1);
 668         }
 669
 670         err = prestera_hw_port_mac_set(port, dev->dev_addr);
 671         if (err) {
 672                 dev_err(prestera_dev(sw), "Failed to set port(%u) mac addr\n", id);
 673                 goto err_port_init;
 674         }
 675
 676         err = prestera_hw_port_cap_get(port, &port->caps);
 677         if (err) {
 678                 dev_err(prestera_dev(sw), "Failed to get port(%u) caps\n", id);
 679                 goto err_port_init;
 680         }
 681
 682         port->adver_link_modes = port->caps.supp_link_modes;
 683         port->adver_fec = 0;
 684         port->autoneg = true;
 685
 686         /* initialize config mac */
 687         if (port->caps.transceiver != PRESTERA_PORT_TCVR_SFP) {
 688                 cfg_mac.admin = true;
 689                 cfg_mac.mode = PRESTERA_MAC_MODE_INTERNAL;
 690         } else {
 691                 cfg_mac.admin = false;
 692                 cfg_mac.mode = PRESTERA_MAC_MODE_MAX;
 693         }
 694         cfg_mac.inband = 0;
 695         cfg_mac.speed = 0;
 696         cfg_mac.duplex = DUPLEX_UNKNOWN;
 697         cfg_mac.fec = PRESTERA_PORT_FEC_OFF;
 698
 699         err = prestera_port_cfg_mac_write(port, &cfg_mac);
 700         if (err) {
 701                 dev_err(prestera_dev(sw),
 702                         "Failed to set port(%u) mac mode\n", id);
 703                 goto err_port_init;
 704         }
 705
 706         /* initialize config phy (if this is inegral) */
 707         if (port->caps.transceiver != PRESTERA_PORT_TCVR_SFP) {
 708                 port->cfg_phy.mdix = ETH_TP_MDI_AUTO;
 709                 port->cfg_phy.admin = false;
 710                 err = prestera_hw_port_phy_mode_set(port,
 711                                                     port->cfg_phy.admin,
 712                                                     false, 0, 0,
 713                                                     port->cfg_phy.mdix);
 714                 if (err) {
 715                         dev_err(prestera_dev(sw),
 716                                 "Failed to set port(%u) phy mode\n", id);
 717                         goto err_port_init;
 718                 }
 719         }
 720
 721         err = prestera_rxtx_port_init(port);
 722         if (err)
 723                 goto err_port_init;
 724
 725         INIT_DELAYED_WORK(&port->cached_hw_stats.caching_dw,
 726                           &prestera_port_stats_update);
 727
 728         prestera_port_list_add(port);
 729
 730         err = register_netdev(dev);
 731         if (err)
 732                 goto err_register_netdev;
 733
 734         err = prestera_port_sfp_bind(port);
 735         if (err)
 736                 goto err_sfp_bind;
 737
 738         return 0;
 739
 740 err_sfp_bind:
 741         unregister_netdev(dev);
 742 err_register_netdev:
 743         prestera_port_list_del(port);
 744 err_port_init:
 745         prestera_devlink_port_unregister(port);
 746 err_dl_port_register:
 747 err_port_info_get:
 748         free_netdev(dev);
 749         return err;
 750 }
 751
 752 static void prestera_port_destroy(struct prestera_port *port)
 753 {
 754         struct net_device *dev = port->dev;
 755
 756         cancel_delayed_work_sync(&port->cached_hw_stats.caching_dw);
 757         unregister_netdev(dev);
 758         prestera_port_list_del(port);
 759         prestera_devlink_port_unregister(port);
 760         free_netdev(dev);
 761 }
 762
 763 static void prestera_destroy_ports(struct prestera_switch *sw)
 764 {
 765         struct prestera_port *port, *tmp;
 766
 767         list_for_each_entry_safe(port, tmp, &sw->port_list, list)
 768                 prestera_port_destroy(port);
 769 }
 770
 771 static int prestera_create_ports(struct prestera_switch *sw)
 772 {
 773         struct prestera_port *port, *tmp;
 774         u32 port_idx;
 775         int err;
 776
 777         for (port_idx = 0; port_idx < sw->port_count; port_idx++) {
 778                 err = prestera_port_create(sw, port_idx);
 779                 if (err)
 780                         goto err_port_create;
 781         }
 782
 783         return 0;
 784
 785 err_port_create:
 786         list_for_each_entry_safe(port, tmp, &sw->port_list, list) {
 787                 prestera_port_sfp_unbind(port);
 788                 prestera_port_destroy(port);
 789         }
 790
 791         return err;
 792 }
 793
 794 static void prestera_port_handle_event(struct prestera_switch *sw,
 795                                        struct prestera_event *evt, void *arg)
 796 {
 797         struct prestera_port_mac_state smac;
 798         struct prestera_port_event *pevt;
 799         struct delayed_work *caching_dw;
 800         struct prestera_port *port;
 801
 802         if (evt->id == PRESTERA_PORT_EVENT_MAC_STATE_CHANGED) {
 803                 pevt = &evt->port_evt;
 804                 port = prestera_find_port(sw, pevt->port_id);
 805                 if (!port || !port->dev)
 806                         return;
 807
 808                 caching_dw = &port->cached_hw_stats.caching_dw;
 809
 810                 memset(&smac, 0, sizeof(smac));
 811                 smac.valid = true;
 812                 smac.oper = pevt->data.mac.oper;
 813                 if (smac.oper) {
 814                         smac.mode = pevt->data.mac.mode;
 815                         smac.speed = pevt->data.mac.speed;
 816                         smac.duplex = pevt->data.mac.duplex;
 817                         smac.fc = pevt->data.mac.fc;
 818                         smac.fec = pevt->data.mac.fec;
 819                 }
 820                 prestera_port_mac_state_cache_write(port, &smac);
 821
 822                 if (port->state_mac.oper) {
 823                         if (port->phy_link)
 824                                 phylink_mac_change(port->phy_link, true);
 825                         else
 826                                 netif_carrier_on(port->dev);
 827
 828                         if (!delayed_work_pending(caching_dw))
 829                                 queue_delayed_work(prestera_wq, caching_dw, 0);
 830                 } else {
 831                         if (port->phy_link)
 832                                 phylink_mac_change(port->phy_link, false);
 833                         else if (netif_running(port->dev) && netif_carrier_ok(port->dev))
 834                                 netif_carrier_off(port->dev);
 835
 836                         if (delayed_work_pending(caching_dw))
 837                                 cancel_delayed_work(caching_dw);
 838                 }
 839         }
 840 }
 841
 842 static int prestera_event_handlers_register(struct prestera_switch *sw)
 843 {
 844         return prestera_hw_event_handler_register(sw, PRESTERA_EVENT_TYPE_PORT,
 845                                                   prestera_port_handle_event,
 846                                                   NULL);
 847 }
 848
 849 static void prestera_event_handlers_unregister(struct prestera_switch *sw)
 850 {
 851         prestera_hw_event_handler_unregister(sw, PRESTERA_EVENT_TYPE_PORT,
 852                                              prestera_port_handle_event);
 853 }
 854
 855 static int prestera_switch_set_base_mac_addr(struct prestera_switch *sw)
 856 {
 857         int ret;
 858
 859         if (sw->np)
 860                 ret = of_get_mac_address(sw->np, sw->base_mac);
 861         if (!is_valid_ether_addr(sw->base_mac) || ret) {
 862                 eth_random_addr(sw->base_mac);
 863                 dev_info(prestera_dev(sw), "using random base mac address\n");
 864         }
 865
 866         return prestera_hw_switch_mac_set(sw, sw->base_mac);
 867 }
 868
 869 struct prestera_lag *prestera_lag_by_id(struct prestera_switch *sw, u16 id)
 870 {
 871         return id < sw->lag_max ? &sw->lags[id] : NULL;
 872 }
 873
 874 static struct prestera_lag *prestera_lag_by_dev(struct prestera_switch *sw,
 875                                                 struct net_device *dev)
 876 {
 877         struct prestera_lag *lag;
 878         u16 id;
 879
 880         for (id = 0; id < sw->lag_max; id++) {
 881                 lag = &sw->lags[id];
 882                 if (lag->dev == dev)
 883                         return lag;
 884         }
 885
 886         return NULL;
 887 }
 888
 889 int prestera_lag_id(struct prestera_switch *sw,
 890                     struct net_device *lag_dev, u16 *lag_id)
 891 {
 892         struct prestera_lag *lag;
 893         int free_id = -1;
 894         int id;
 895
 896         for (id = 0; id < sw->lag_max; id++) {
 897                 lag = prestera_lag_by_id(sw, id);
 898                 if (lag->member_count) {
 899                         if (lag->dev == lag_dev) {
 900                                 *lag_id = id;
 901                                 return 0;
 902                         }
 903                 } else if (free_id < 0) {
 904                         free_id = id;
 905                 }
 906         }
 907         if (free_id < 0)
 908                 return -ENOSPC;
 909         *lag_id = free_id;
 910         return 0;
 911 }
 912
 913 static struct prestera_lag *prestera_lag_create(struct prestera_switch *sw,
 914                                                 struct net_device *lag_dev)
 915 {
 916         struct prestera_lag *lag = NULL;
 917         u16 id;
 918
 919         for (id = 0; id < sw->lag_max; id++) {
 920                 lag = &sw->lags[id];
 921                 if (!lag->dev)
 922                         break;
 923         }
 924         if (lag) {
 925                 INIT_LIST_HEAD(&lag->members);
 926                 lag->dev = lag_dev;
 927         }
 928
 929         return lag;
 930 }
 931
 932 static void prestera_lag_destroy(struct prestera_switch *sw,
 933                                  struct prestera_lag *lag)
 934 {
 935         WARN_ON(!list_empty(&lag->members));
 936         lag->member_count = 0;
 937         lag->dev = NULL;
 938 }
 939
 940 static int prestera_lag_port_add(struct prestera_port *port,
 941                                  struct net_device *lag_dev)
 942 {
 943         struct prestera_switch *sw = port->sw;
 944         struct prestera_lag *lag;
 945         int err;
 946
 947         lag = prestera_lag_by_dev(sw, lag_dev);
 948         if (!lag) {
 949                 lag = prestera_lag_create(sw, lag_dev);
 950                 if (!lag)
 951                         return -ENOSPC;
 952         }
 953
 954         if (lag->member_count >= sw->lag_member_max)
 955                 return -ENOSPC;
 956
 957         err = prestera_hw_lag_member_add(port, lag->lag_id);
 958         if (err) {
 959                 if (!lag->member_count)
 960                         prestera_lag_destroy(sw, lag);
 961                 return err;
 962         }
 963
 964         list_add(&port->lag_member, &lag->members);
 965         lag->member_count++;
 966         port->lag = lag;
 967
 968         return 0;
 969 }
 970
 971 static int prestera_lag_port_del(struct prestera_port *port)
 972 {
 973         struct prestera_switch *sw = port->sw;
 974         struct prestera_lag *lag = port->lag;
 975         int err;
 976
 977         if (!lag || !lag->member_count)
 978                 return -EINVAL;
 979
 980         err = prestera_hw_lag_member_del(port, lag->lag_id);
 981         if (err)
 982                 return err;
 983
 984         list_del(&port->lag_member);
 985         lag->member_count--;
 986         port->lag = NULL;
 987
 988         if (netif_is_bridge_port(lag->dev)) {
 989                 struct net_device *br_dev;
 990
 991                 br_dev = netdev_master_upper_dev_get(lag->dev);
 992
 993                 prestera_bridge_port_leave(br_dev, port);
 994         }
 995
 996         if (!lag->member_count)
 997                 prestera_lag_destroy(sw, lag);
 998
 999         return 0;
1000 }
1001
1002 bool prestera_port_is_lag_member(const struct prestera_port *port)
1003 {
1004         return !!port->lag;
1005 }
1006
1007 u16 prestera_port_lag_id(const struct prestera_port *port)
1008 {
1009         return port->lag->lag_id;
1010 }
1011
1012 static int prestera_lag_init(struct prestera_switch *sw)
1013 {
1014         u16 id;
1015
1016         sw->lags = kcalloc(sw->lag_max, sizeof(*sw->lags), GFP_KERNEL);
1017         if (!sw->lags)
1018                 return -ENOMEM;
1019
1020         for (id = 0; id < sw->lag_max; id++)
1021                 sw->lags[id].lag_id = id;
1022
1023         return 0;
1024 }
1025
1026 static void prestera_lag_fini(struct prestera_switch *sw)
1027 {
1028         u8 idx;
1029
1030         for (idx = 0; idx < sw->lag_max; idx++)
1031                 WARN_ON(sw->lags[idx].member_count);
1032
1033         kfree(sw->lags);
1034 }
1035
1036 bool prestera_netdev_check(const struct net_device *dev)
1037 {
1038         return dev->netdev_ops == &prestera_netdev_ops;
1039 }
1040
1041 static int prestera_lower_dev_walk(struct net_device *dev,
1042                                    struct netdev_nested_priv *priv)
1043 {
1044         struct prestera_port **pport = (struct prestera_port **)priv->data;
1045
1046         if (prestera_netdev_check(dev)) {
1047                 *pport = netdev_priv(dev);
1048                 return 1;
1049         }
1050
1051         return 0;
1052 }
1053
1054 struct prestera_port *prestera_port_dev_lower_find(struct net_device *dev)
1055 {
1056         struct prestera_port *port = NULL;
1057         struct netdev_nested_priv priv = {
1058                 .data = (void *)&port,
1059         };
1060
1061         if (prestera_netdev_check(dev))
1062                 return netdev_priv(dev);
1063
1064         netdev_walk_all_lower_dev(dev, prestera_lower_dev_walk, &priv);
1065
1066         return port;
1067 }
1068
1069 static int prestera_netdev_port_lower_event(struct net_device *dev,
1070                                             unsigned long event, void *ptr)
1071 {
1072         struct netdev_notifier_changelowerstate_info *info = ptr;
1073         struct netdev_lag_lower_state_info *lower_state_info;
1074         struct prestera_port *port = netdev_priv(dev);
1075         bool enabled;
1076
1077         if (!netif_is_lag_port(dev))
1078                 return 0;
1079         if (!prestera_port_is_lag_member(port))
1080                 return 0;
1081
1082         lower_state_info = info->lower_state_info;
1083         enabled = lower_state_info->link_up && lower_state_info->tx_enabled;
1084
1085         return prestera_hw_lag_member_enable(port, port->lag->lag_id, enabled);
1086 }
1087
1088 static bool prestera_lag_master_check(struct net_device *lag_dev,
1089                                       struct netdev_lag_upper_info *info,
1090                                       struct netlink_ext_ack *ext_ack)
1091 {
1092         if (info->tx_type != NETDEV_LAG_TX_TYPE_HASH) {
1093                 NL_SET_ERR_MSG_MOD(ext_ack, "Unsupported LAG Tx type");
1094                 return false;
1095         }
1096
1097         return true;
1098 }
1099
1100 static int prestera_netdev_port_event(struct net_device *lower,
1101                                       struct net_device *dev,
1102                                       unsigned long event, void *ptr)
1103 {
1104         struct netdev_notifier_info *info = ptr;
1105         struct netdev_notifier_changeupper_info *cu_info;
1106         struct prestera_port *port = netdev_priv(dev);
1107         struct netlink_ext_ack *extack;
1108         struct net_device *upper;
1109
1110         extack = netdev_notifier_info_to_extack(info);
1111         cu_info = container_of(info,
1112                                struct netdev_notifier_changeupper_info,
1113                                info);
1114
1115         switch (event) {
1116         case NETDEV_PRECHANGEUPPER:
1117                 upper = cu_info->upper_dev;
1118                 if (!netif_is_bridge_master(upper) &&
1119                     !netif_is_lag_master(upper)) {
1120                         NL_SET_ERR_MSG_MOD(extack, "Unknown upper device type");
1121                         return -EINVAL;
1122                 }
1123
1124                 if (!cu_info->linking)
1125                         break;
1126
1127                 if (netdev_has_any_upper_dev(upper)) {
1128                         NL_SET_ERR_MSG_MOD(extack, "Upper device is already enslaved");
1129                         return -EINVAL;
1130                 }
1131
1132                 if (netif_is_lag_master(upper) &&
1133                     !prestera_lag_master_check(upper, cu_info->upper_info, extack))
1134                         return -EOPNOTSUPP;
1135                 if (netif_is_lag_master(upper) && vlan_uses_dev(dev)) {
1136                         NL_SET_ERR_MSG_MOD(extack,
1137                                            "Master device is a LAG master and port has a VLAN");
1138                         return -EINVAL;
1139                 }
1140                 if (netif_is_lag_port(dev) && is_vlan_dev(upper) &&
1141                     !netif_is_lag_master(vlan_dev_real_dev(upper))) {
1142                         NL_SET_ERR_MSG_MOD(extack,
1143                                            "Can not put a VLAN on a LAG port");
1144                         return -EINVAL;
1145                 }
1146                 break;
1147
1148         case NETDEV_CHANGEUPPER:
1149                 upper = cu_info->upper_dev;
1150                 if (netif_is_bridge_master(upper)) {
1151                         if (cu_info->linking)
1152                                 return prestera_bridge_port_join(upper, port,
1153                                                                  extack);
1154                         else
1155                                 prestera_bridge_port_leave(upper, port);
1156                 } else if (netif_is_lag_master(upper)) {
1157                         if (cu_info->linking)
1158                                 return prestera_lag_port_add(port, upper);
1159                         else
1160                                 prestera_lag_port_del(port);
1161                 }
1162                 break;
1163
1164         case NETDEV_CHANGELOWERSTATE:
1165                 return prestera_netdev_port_lower_event(dev, event, ptr);
1166         }
1167
1168         return 0;
1169 }
1170
1171 static int prestera_netdevice_lag_event(struct net_device *lag_dev,
1172                                         unsigned long event, void *ptr)
1173 {
1174         struct net_device *dev;
1175         struct list_head *iter;
1176         int err;
1177
1178         netdev_for_each_lower_dev(lag_dev, dev, iter) {
1179                 if (prestera_netdev_check(dev)) {
1180                         err = prestera_netdev_port_event(lag_dev, dev, event,
1181                                                          ptr);
1182                         if (err)
1183                                 return err;
1184                 }
1185         }
1186
1187         return 0;
1188 }
1189
1190 static int prestera_netdev_event_handler(struct notifier_block *nb,
1191                                          unsigned long event, void *ptr)
1192 {
1193         struct net_device *dev = netdev_notifier_info_to_dev(ptr);
1194         int err = 0;
1195
1196         if (prestera_netdev_check(dev))
1197                 err = prestera_netdev_port_event(dev, dev, event, ptr);
1198         else if (netif_is_lag_master(dev))
1199                 err = prestera_netdevice_lag_event(dev, event, ptr);
1200
1201         return notifier_from_errno(err);
1202 }
1203
1204 struct prestera_mdb_entry *
1205 prestera_mdb_entry_create(struct prestera_switch *sw,
1206                           const unsigned char *addr, u16 vid)
1207 {
1208         struct prestera_flood_domain *flood_domain;
1209         struct prestera_mdb_entry *mdb_entry;
1210
1211         mdb_entry = kzalloc(sizeof(*mdb_entry), GFP_KERNEL);
1212         if (!mdb_entry)
1213                 goto err_mdb_alloc;
1214
1215         flood_domain = prestera_flood_domain_create(sw);
1216         if (!flood_domain)
1217                 goto err_flood_domain_create;
1218
1219         mdb_entry->sw = sw;
1220         mdb_entry->vid = vid;
1221         mdb_entry->flood_domain = flood_domain;
1222         ether_addr_copy(mdb_entry->addr, addr);
1223
1224         if (prestera_hw_mdb_create(mdb_entry))
1225                 goto err_mdb_hw_create;
1226
1227         return mdb_entry;
1228
1229 err_mdb_hw_create:
1230         prestera_flood_domain_destroy(flood_domain);
1231 err_flood_domain_create:
1232         kfree(mdb_entry);
1233 err_mdb_alloc:
1234         return NULL;
1235 }
1236
1237 void prestera_mdb_entry_destroy(struct prestera_mdb_entry *mdb_entry)
1238 {
1239         prestera_hw_mdb_destroy(mdb_entry);
1240         prestera_flood_domain_destroy(mdb_entry->flood_domain);
1241         kfree(mdb_entry);
1242 }
1243
1244 struct prestera_flood_domain *
1245 prestera_flood_domain_create(struct prestera_switch *sw)
1246 {
1247         struct prestera_flood_domain *domain;
1248
1249         domain = kzalloc(sizeof(*domain), GFP_KERNEL);
1250         if (!domain)
1251                 return NULL;
1252
1253         domain->sw = sw;
1254
1255         if (prestera_hw_flood_domain_create(domain)) {
1256                 kfree(domain);
1257                 return NULL;
1258         }
1259
1260         INIT_LIST_HEAD(&domain->flood_domain_port_list);
1261
1262         return domain;
1263 }
1264
1265 void prestera_flood_domain_destroy(struct prestera_flood_domain *flood_domain)
1266 {
1267         WARN_ON(!list_empty(&flood_domain->flood_domain_port_list));
1268         WARN_ON_ONCE(prestera_hw_flood_domain_destroy(flood_domain));
1269         kfree(flood_domain);
1270 }
1271
1272 int
1273 prestera_flood_domain_port_create(struct prestera_flood_domain *flood_domain,
1274                                   struct net_device *dev,
1275                                   u16 vid)
1276 {
1277         struct prestera_flood_domain_port *flood_domain_port;
1278         bool is_first_port_in_list = false;
1279         int err;
1280
1281         flood_domain_port = kzalloc(sizeof(*flood_domain_port), GFP_KERNEL);
1282         if (!flood_domain_port) {
1283                 err = -ENOMEM;
1284                 goto err_port_alloc;
1285         }
1286
1287         flood_domain_port->vid = vid;
1288
1289         if (list_empty(&flood_domain->flood_domain_port_list))
1290                 is_first_port_in_list = true;
1291
1292         list_add(&flood_domain_port->flood_domain_port_node,
1293                  &flood_domain->flood_domain_port_list);
1294
1295         flood_domain_port->flood_domain = flood_domain;
1296         flood_domain_port->dev = dev;
1297
1298         if (!is_first_port_in_list) {
1299                 err = prestera_hw_flood_domain_ports_reset(flood_domain);
1300                 if (err)
1301                         goto err_prestera_mdb_port_create_hw;
1302         }
1303
1304         err = prestera_hw_flood_domain_ports_set(flood_domain);
1305         if (err)
1306                 goto err_prestera_mdb_port_create_hw;
1307
1308         return 0;
1309
1310 err_prestera_mdb_port_create_hw:
1311         list_del(&flood_domain_port->flood_domain_port_node);
1312         kfree(flood_domain_port);
1313 err_port_alloc:
1314         return err;
1315 }
1316
1317 void
1318 prestera_flood_domain_port_destroy(struct prestera_flood_domain_port *port)
1319 {
1320         struct prestera_flood_domain *flood_domain = port->flood_domain;
1321
1322         list_del(&port->flood_domain_port_node);
1323
1324         WARN_ON_ONCE(prestera_hw_flood_domain_ports_reset(flood_domain));
1325
1326         if (!list_empty(&flood_domain->flood_domain_port_list))
1327                 WARN_ON_ONCE(prestera_hw_flood_domain_ports_set(flood_domain));
1328
1329         kfree(port);
1330 }
1331
1332 struct prestera_flood_domain_port *
1333 prestera_flood_domain_port_find(struct prestera_flood_domain *flood_domain,
1334                                 struct net_device *dev, u16 vid)
1335 {
1336         struct prestera_flood_domain_port *flood_domain_port;
1337
1338         list_for_each_entry(flood_domain_port,
1339                             &flood_domain->flood_domain_port_list,
1340                             flood_domain_port_node)
1341                 if (flood_domain_port->dev == dev &&
1342                     vid == flood_domain_port->vid)
1343                         return flood_domain_port;
1344
1345         return NULL;
1346 }
1347
1348 static int prestera_netdev_event_handler_register(struct prestera_switch *sw)
1349 {
1350         sw->netdev_nb.notifier_call = prestera_netdev_event_handler;
1351
1352         return register_netdevice_notifier(&sw->netdev_nb);
1353 }
1354
1355 static void prestera_netdev_event_handler_unregister(struct prestera_switch *sw)
1356 {
1357         unregister_netdevice_notifier(&sw->netdev_nb);
1358 }
1359
1360 static int prestera_switch_init(struct prestera_switch *sw)
1361 {
1362         int err;
1363
1364         sw->np = sw->dev->dev->of_node;
1365
1366         err = prestera_hw_switch_init(sw);
1367         if (err) {
1368                 dev_err(prestera_dev(sw), "Failed to init Switch device\n");
1369                 return err;
1370         }
1371
1372         rwlock_init(&sw->port_list_lock);
1373         INIT_LIST_HEAD(&sw->port_list);
1374
1375         err = prestera_switch_set_base_mac_addr(sw);
1376         if (err)
1377                 return err;
1378
1379         err = prestera_netdev_event_handler_register(sw);
1380         if (err)
1381                 return err;
1382
1383         err = prestera_router_init(sw);
1384         if (err)
1385                 goto err_router_init;
1386
1387         err = prestera_switchdev_init(sw);
1388         if (err)
1389                 goto err_swdev_register;
1390
1391         err = prestera_rxtx_switch_init(sw);
1392         if (err)
1393                 goto err_rxtx_register;
1394
1395         err = prestera_event_handlers_register(sw);
1396         if (err)
1397                 goto err_handlers_register;
1398
1399         err = prestera_counter_init(sw);
1400         if (err)
1401                 goto err_counter_init;
1402
1403         err = prestera_acl_init(sw);
1404         if (err)
1405                 goto err_acl_init;
1406
1407         err = prestera_span_init(sw);
1408         if (err)
1409                 goto err_span_init;
1410
1411         err = prestera_devlink_traps_register(sw);
1412         if (err)
1413                 goto err_dl_register;
1414
1415         err = prestera_lag_init(sw);
1416         if (err)
1417                 goto err_lag_init;
1418
1419         err = prestera_create_ports(sw);
1420         if (err)
1421                 goto err_ports_create;
1422
1423         prestera_devlink_register(sw);
1424         return 0;
1425
1426 err_ports_create:
1427         prestera_lag_fini(sw);
1428 err_lag_init:
1429         prestera_devlink_traps_unregister(sw);
1430 err_dl_register:
1431         prestera_span_fini(sw);
1432 err_span_init:
1433         prestera_acl_fini(sw);
1434 err_acl_init:
1435         prestera_counter_fini(sw);
1436 err_counter_init:
1437         prestera_event_handlers_unregister(sw);
1438 err_handlers_register:
1439         prestera_rxtx_switch_fini(sw);
1440 err_rxtx_register:
1441         prestera_switchdev_fini(sw);
1442 err_swdev_register:
1443         prestera_router_fini(sw);
1444 err_router_init:
1445         prestera_netdev_event_handler_unregister(sw);
1446         prestera_hw_switch_fini(sw);
1447
1448         return err;
1449 }
1450
1451 static void prestera_switch_fini(struct prestera_switch *sw)
1452 {
1453         prestera_devlink_unregister(sw);
1454         prestera_destroy_ports(sw);
1455         prestera_lag_fini(sw);
1456         prestera_devlink_traps_unregister(sw);
1457         prestera_span_fini(sw);
1458         prestera_acl_fini(sw);
1459         prestera_counter_fini(sw);
1460         prestera_event_handlers_unregister(sw);
1461         prestera_rxtx_switch_fini(sw);
1462         prestera_switchdev_fini(sw);
1463         prestera_router_fini(sw);
1464         prestera_netdev_event_handler_unregister(sw);
1465         prestera_hw_switch_fini(sw);
1466         of_node_put(sw->np);
1467 }
1468
1469 int prestera_device_register(struct prestera_device *dev)
1470 {
1471         struct prestera_switch *sw;
1472         int err;
1473
1474         sw = prestera_devlink_alloc(dev);
1475         if (!sw)
1476                 return -ENOMEM;
1477
1478         dev->priv = sw;
1479         sw->dev = dev;
1480
1481         err = prestera_switch_init(sw);
1482         if (err) {
1483                 prestera_devlink_free(sw);
1484                 return err;
1485         }
1486
1487         return 0;
1488 }
1489 EXPORT_SYMBOL(prestera_device_register);
1490
1491 void prestera_device_unregister(struct prestera_device *dev)
1492 {
1493         struct prestera_switch *sw = dev->priv;
1494
1495         prestera_switch_fini(sw);
1496         prestera_devlink_free(sw);
1497 }
1498 EXPORT_SYMBOL(prestera_device_unregister);
1499
1500 static int __init prestera_module_init(void)
1501 {
1502         prestera_wq = alloc_workqueue("prestera", 0, 0);
1503         if (!prestera_wq)
1504                 return -ENOMEM;
1505
1506         prestera_owq = alloc_ordered_workqueue("prestera_ordered", 0);
1507         if (!prestera_owq) {
1508                 destroy_workqueue(prestera_wq);
1509                 return -ENOMEM;
1510         }
1511
1512         return 0;
1513 }
1514
1515 static void __exit prestera_module_exit(void)
1516 {
1517         destroy_workqueue(prestera_wq);
1518         destroy_workqueue(prestera_owq);
1519 }
1520
1521 module_init(prestera_module_init);
1522 module_exit(prestera_module_exit);
1523
1524 MODULE_LICENSE("Dual BSD/GPL");
1525 MODULE_DESCRIPTION("Marvell Prestera switch driver");