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
  13 #include "prestera.h"
  14 #include "prestera_hw.h"
  15 #include "prestera_acl.h"
  16 #include "prestera_flow.h"
  17 #include "prestera_span.h"
  18 #include "prestera_rxtx.h"
  19 #include "prestera_devlink.h"
  20 #include "prestera_ethtool.h"
  21 #include "prestera_switchdev.h"
  22
  23 #define PRESTERA_MTU_DEFAULT    1536
  24
  25 #define PRESTERA_STATS_DELAY_MS 1000
  26
  27 #define PRESTERA_MAC_ADDR_NUM_MAX       255
  28
  29 static struct workqueue_struct *prestera_wq;
  30
  31 int prestera_port_pvid_set(struct prestera_port *port, u16 vid)
  32 {
  33         enum prestera_accept_frm_type frm_type;
  34         int err;
  35
  36         frm_type = PRESTERA_ACCEPT_FRAME_TYPE_TAGGED;
  37
  38         if (vid) {
  39                 err = prestera_hw_vlan_port_vid_set(port, vid);
  40                 if (err)
  41                         return err;
  42
  43                 frm_type = PRESTERA_ACCEPT_FRAME_TYPE_ALL;
  44         }
  45
  46         err = prestera_hw_port_accept_frm_type(port, frm_type);
  47         if (err && frm_type == PRESTERA_ACCEPT_FRAME_TYPE_ALL)
  48                 prestera_hw_vlan_port_vid_set(port, port->pvid);
  49
  50         port->pvid = vid;
  51         return 0;
  52 }
  53
  54 struct prestera_port *prestera_port_find_by_hwid(struct prestera_switch *sw,
  55                                                  u32 dev_id, u32 hw_id)
  56 {
  57         struct prestera_port *port = NULL, *tmp;
  58
  59         read_lock(&sw->port_list_lock);
  60         list_for_each_entry(tmp, &sw->port_list, list) {
  61                 if (tmp->dev_id == dev_id && tmp->hw_id == hw_id) {
  62                         port = tmp;
  63                         break;
  64                 }
  65         }
  66         read_unlock(&sw->port_list_lock);
  67
  68         return port;
  69 }
  70
  71 struct prestera_port *prestera_find_port(struct prestera_switch *sw, u32 id)
  72 {
  73         struct prestera_port *port = NULL, *tmp;
  74
  75         read_lock(&sw->port_list_lock);
  76         list_for_each_entry(tmp, &sw->port_list, list) {
  77                 if (tmp->id == id) {
  78                         port = tmp;
  79                         break;
  80                 }
  81         }
  82         read_unlock(&sw->port_list_lock);
  83
  84         return port;
  85 }
  86
  87 int prestera_port_cfg_mac_read(struct prestera_port *port,
  88                                struct prestera_port_mac_config *cfg)
  89 {
  90         *cfg = port->cfg_mac;
  91         return 0;
  92 }
  93
  94 int prestera_port_cfg_mac_write(struct prestera_port *port,
  95                                 struct prestera_port_mac_config *cfg)
  96 {
  97         int err;
  98
  99         err = prestera_hw_port_mac_mode_set(port, cfg->admin,
 100                                             cfg->mode, cfg->inband, cfg->speed,
 101                                             cfg->duplex, cfg->fec);
 102         if (err)
 103                 return err;
 104
 105         port->cfg_mac = *cfg;
 106         return 0;
 107 }
 108
 109 static int prestera_port_open(struct net_device *dev)
 110 {
 111         struct prestera_port *port = netdev_priv(dev);
 112         struct prestera_port_mac_config cfg_mac;
 113         int err = 0;
 114
 115         if (port->caps.transceiver == PRESTERA_PORT_TCVR_SFP) {
 116                 err = prestera_port_cfg_mac_read(port, &cfg_mac);
 117                 if (!err) {
 118                         cfg_mac.admin = true;
 119                         err = prestera_port_cfg_mac_write(port, &cfg_mac);
 120                 }
 121         } else {
 122                 port->cfg_phy.admin = true;
 123                 err = prestera_hw_port_phy_mode_set(port, true, port->autoneg,
 124                                                     port->cfg_phy.mode,
 125                                                     port->adver_link_modes,
 126                                                     port->cfg_phy.mdix);
 127         }
 128
 129         netif_start_queue(dev);
 130
 131         return err;
 132 }
 133
 134 static int prestera_port_close(struct net_device *dev)
 135 {
 136         struct prestera_port *port = netdev_priv(dev);
 137         struct prestera_port_mac_config cfg_mac;
 138         int err = 0;
 139
 140         netif_stop_queue(dev);
 141
 142         if (port->caps.transceiver == PRESTERA_PORT_TCVR_SFP) {
 143                 err = prestera_port_cfg_mac_read(port, &cfg_mac);
 144                 if (!err) {
 145                         cfg_mac.admin = false;
 146                         prestera_port_cfg_mac_write(port, &cfg_mac);
 147                 }
 148         } else {
 149                 port->cfg_phy.admin = false;
 150                 err = prestera_hw_port_phy_mode_set(port, false, port->autoneg,
 151                                                     port->cfg_phy.mode,
 152                                                     port->adver_link_modes,
 153                                                     port->cfg_phy.mdix);
 154         }
 155
 156         return err;
 157 }
 158
 159 static netdev_tx_t prestera_port_xmit(struct sk_buff *skb,
 160                                       struct net_device *dev)
 161 {
 162         return prestera_rxtx_xmit(netdev_priv(dev), skb);
 163 }
 164
 165 static int prestera_is_valid_mac_addr(struct prestera_port *port, u8 *addr)
 166 {
 167         if (!is_valid_ether_addr(addr))
 168                 return -EADDRNOTAVAIL;
 169
 170         /* firmware requires that port's MAC address contains first 5 bytes
 171          * of the base MAC address
 172          */
 173         if (memcmp(port->sw->base_mac, addr, ETH_ALEN - 1))
 174                 return -EINVAL;
 175
 176         return 0;
 177 }
 178
 179 static int prestera_port_set_mac_address(struct net_device *dev, void *p)
 180 {
 181         struct prestera_port *port = netdev_priv(dev);
 182         struct sockaddr *addr = p;
 183         int err;
 184
 185         err = prestera_is_valid_mac_addr(port, addr->sa_data);
 186         if (err)
 187                 return err;
 188
 189         err = prestera_hw_port_mac_set(port, addr->sa_data);
 190         if (err)
 191                 return err;
 192
 193         eth_hw_addr_set(dev, addr->sa_data);
 194
 195         return 0;
 196 }
 197
 198 static int prestera_port_change_mtu(struct net_device *dev, int mtu)
 199 {
 200         struct prestera_port *port = netdev_priv(dev);
 201         int err;
 202
 203         err = prestera_hw_port_mtu_set(port, mtu);
 204         if (err)
 205                 return err;
 206
 207         dev->mtu = mtu;
 208
 209         return 0;
 210 }
 211
 212 static void prestera_port_get_stats64(struct net_device *dev,
 213                                       struct rtnl_link_stats64 *stats)
 214 {
 215         struct prestera_port *port = netdev_priv(dev);
 216         struct prestera_port_stats *port_stats = &port->cached_hw_stats.stats;
 217
 218         stats->rx_packets = port_stats->broadcast_frames_received +
 219                                 port_stats->multicast_frames_received +
 220                                 port_stats->unicast_frames_received;
 221
 222         stats->tx_packets = port_stats->broadcast_frames_sent +
 223                                 port_stats->multicast_frames_sent +
 224                                 port_stats->unicast_frames_sent;
 225
 226         stats->rx_bytes = port_stats->good_octets_received;
 227
 228         stats->tx_bytes = port_stats->good_octets_sent;
 229
 230         stats->rx_errors = port_stats->rx_error_frame_received;
 231         stats->tx_errors = port_stats->mac_trans_error;
 232
 233         stats->rx_dropped = port_stats->buffer_overrun;
 234         stats->tx_dropped = 0;
 235
 236         stats->multicast = port_stats->multicast_frames_received;
 237         stats->collisions = port_stats->excessive_collision;
 238
 239         stats->rx_crc_errors = port_stats->bad_crc;
 240 }
 241
 242 static void prestera_port_get_hw_stats(struct prestera_port *port)
 243 {
 244         prestera_hw_port_stats_get(port, &port->cached_hw_stats.stats);
 245 }
 246
 247 static void prestera_port_stats_update(struct work_struct *work)
 248 {
 249         struct prestera_port *port =
 250                 container_of(work, struct prestera_port,
 251                              cached_hw_stats.caching_dw.work);
 252
 253         prestera_port_get_hw_stats(port);
 254
 255         queue_delayed_work(prestera_wq, &port->cached_hw_stats.caching_dw,
 256                            msecs_to_jiffies(PRESTERA_STATS_DELAY_MS));
 257 }
 258
 259 static int prestera_port_setup_tc(struct net_device *dev,
 260                                   enum tc_setup_type type,
 261                                   void *type_data)
 262 {
 263         struct prestera_port *port = netdev_priv(dev);
 264
 265         switch (type) {
 266         case TC_SETUP_BLOCK:
 267                 return prestera_flow_block_setup(port, type_data);
 268         default:
 269                 return -EOPNOTSUPP;
 270         }
 271 }
 272
 273 static const struct net_device_ops prestera_netdev_ops = {
 274         .ndo_open = prestera_port_open,
 275         .ndo_stop = prestera_port_close,
 276         .ndo_start_xmit = prestera_port_xmit,
 277         .ndo_setup_tc = prestera_port_setup_tc,
 278         .ndo_change_mtu = prestera_port_change_mtu,
 279         .ndo_get_stats64 = prestera_port_get_stats64,
 280         .ndo_set_mac_address = prestera_port_set_mac_address,
 281         .ndo_get_devlink_port = prestera_devlink_get_port,
 282 };
 283
 284 int prestera_port_autoneg_set(struct prestera_port *port, u64 link_modes)
 285 {
 286         int err;
 287
 288         if (port->autoneg && port->adver_link_modes == link_modes)
 289                 return 0;
 290
 291         err = prestera_hw_port_phy_mode_set(port, port->cfg_phy.admin,
 292                                             true, 0, link_modes,
 293                                             port->cfg_phy.mdix);
 294         if (err)
 295                 return err;
 296
 297         port->adver_fec = BIT(PRESTERA_PORT_FEC_OFF);
 298         port->adver_link_modes = link_modes;
 299         port->cfg_phy.mode = 0;
 300         port->autoneg = true;
 301
 302         return 0;
 303 }
 304
 305 static void prestera_port_list_add(struct prestera_port *port)
 306 {
 307         write_lock(&port->sw->port_list_lock);
 308         list_add(&port->list, &port->sw->port_list);
 309         write_unlock(&port->sw->port_list_lock);
 310 }
 311
 312 static void prestera_port_list_del(struct prestera_port *port)
 313 {
 314         write_lock(&port->sw->port_list_lock);
 315         list_del(&port->list);
 316         write_unlock(&port->sw->port_list_lock);
 317 }
 318
 319 static int prestera_port_create(struct prestera_switch *sw, u32 id)
 320 {
 321         struct prestera_port_mac_config cfg_mac;
 322         struct prestera_port *port;
 323         struct net_device *dev;
 324         int err;
 325
 326         dev = alloc_etherdev(sizeof(*port));
 327         if (!dev)
 328                 return -ENOMEM;
 329
 330         port = netdev_priv(dev);
 331
 332         INIT_LIST_HEAD(&port->vlans_list);
 333         port->pvid = PRESTERA_DEFAULT_VID;
 334         port->lag = NULL;
 335         port->dev = dev;
 336         port->id = id;
 337         port->sw = sw;
 338
 339         err = prestera_hw_port_info_get(port, &port->dev_id, &port->hw_id,
 340                                         &port->fp_id);
 341         if (err) {
 342                 dev_err(prestera_dev(sw), "Failed to get port(%u) info\n", id);
 343                 goto err_port_info_get;
 344         }
 345
 346         err = prestera_devlink_port_register(port);
 347         if (err)
 348                 goto err_dl_port_register;
 349
 350         dev->features |= NETIF_F_NETNS_LOCAL | NETIF_F_HW_TC;
 351         dev->netdev_ops = &prestera_netdev_ops;
 352         dev->ethtool_ops = &prestera_ethtool_ops;
 353
 354         netif_carrier_off(dev);
 355
 356         dev->mtu = min_t(unsigned int, sw->mtu_max, PRESTERA_MTU_DEFAULT);
 357         dev->min_mtu = sw->mtu_min;
 358         dev->max_mtu = sw->mtu_max;
 359
 360         err = prestera_hw_port_mtu_set(port, dev->mtu);
 361         if (err) {
 362                 dev_err(prestera_dev(sw), "Failed to set port(%u) mtu(%d)\n",
 363                         id, dev->mtu);
 364                 goto err_port_init;
 365         }
 366
 367         if (port->fp_id >= PRESTERA_MAC_ADDR_NUM_MAX) {
 368                 err = -EINVAL;
 369                 goto err_port_init;
 370         }
 371
 372         eth_hw_addr_gen(dev, sw->base_mac, port->fp_id);
 373         /* firmware requires that port's MAC address consist of the first
 374          * 5 bytes of the base MAC address
 375          */
 376         if (memcmp(dev->dev_addr, sw->base_mac, ETH_ALEN - 1)) {
 377                 dev_warn(prestera_dev(sw), "Port MAC address wraps for port(%u)\n", id);
 378                 dev_addr_mod(dev, 0, sw->base_mac, ETH_ALEN - 1);
 379         }
 380
 381         err = prestera_hw_port_mac_set(port, dev->dev_addr);
 382         if (err) {
 383                 dev_err(prestera_dev(sw), "Failed to set port(%u) mac addr\n", id);
 384                 goto err_port_init;
 385         }
 386
 387         err = prestera_hw_port_cap_get(port, &port->caps);
 388         if (err) {
 389                 dev_err(prestera_dev(sw), "Failed to get port(%u) caps\n", id);
 390                 goto err_port_init;
 391         }
 392
 393         port->adver_link_modes = port->caps.supp_link_modes;
 394         port->adver_fec = 0;
 395         port->autoneg = true;
 396
 397         /* initialize config mac */
 398         if (port->caps.transceiver != PRESTERA_PORT_TCVR_SFP) {
 399                 cfg_mac.admin = true;
 400                 cfg_mac.mode = PRESTERA_MAC_MODE_INTERNAL;
 401         } else {
 402                 cfg_mac.admin = false;
 403                 cfg_mac.mode = PRESTERA_MAC_MODE_MAX;
 404         }
 405         cfg_mac.inband = false;
 406         cfg_mac.speed = 0;
 407         cfg_mac.duplex = DUPLEX_UNKNOWN;
 408         cfg_mac.fec = PRESTERA_PORT_FEC_OFF;
 409
 410         err = prestera_port_cfg_mac_write(port, &cfg_mac);
 411         if (err) {
 412                 dev_err(prestera_dev(sw),
 413                         "Failed to set port(%u) mac mode\n", id);
 414                 goto err_port_init;
 415         }
 416
 417         /* initialize config phy (if this is inegral) */
 418         if (port->caps.transceiver != PRESTERA_PORT_TCVR_SFP) {
 419                 port->cfg_phy.mdix = ETH_TP_MDI_AUTO;
 420                 port->cfg_phy.admin = false;
 421                 err = prestera_hw_port_phy_mode_set(port,
 422                                                     port->cfg_phy.admin,
 423                                                     false, 0, 0,
 424                                                     port->cfg_phy.mdix);
 425                 if (err) {
 426                         dev_err(prestera_dev(sw),
 427                                 "Failed to set port(%u) phy mode\n", id);
 428                         goto err_port_init;
 429                 }
 430         }
 431
 432         err = prestera_rxtx_port_init(port);
 433         if (err)
 434                 goto err_port_init;
 435
 436         INIT_DELAYED_WORK(&port->cached_hw_stats.caching_dw,
 437                           &prestera_port_stats_update);
 438
 439         prestera_port_list_add(port);
 440
 441         err = register_netdev(dev);
 442         if (err)
 443                 goto err_register_netdev;
 444
 445         prestera_devlink_port_set(port);
 446
 447         return 0;
 448
 449 err_register_netdev:
 450         prestera_port_list_del(port);
 451 err_port_init:
 452         prestera_devlink_port_unregister(port);
 453 err_dl_port_register:
 454 err_port_info_get:
 455         free_netdev(dev);
 456         return err;
 457 }
 458
 459 static void prestera_port_destroy(struct prestera_port *port)
 460 {
 461         struct net_device *dev = port->dev;
 462
 463         cancel_delayed_work_sync(&port->cached_hw_stats.caching_dw);
 464         prestera_devlink_port_clear(port);
 465         unregister_netdev(dev);
 466         prestera_port_list_del(port);
 467         prestera_devlink_port_unregister(port);
 468         free_netdev(dev);
 469 }
 470
 471 static void prestera_destroy_ports(struct prestera_switch *sw)
 472 {
 473         struct prestera_port *port, *tmp;
 474
 475         list_for_each_entry_safe(port, tmp, &sw->port_list, list)
 476                 prestera_port_destroy(port);
 477 }
 478
 479 static int prestera_create_ports(struct prestera_switch *sw)
 480 {
 481         struct prestera_port *port, *tmp;
 482         u32 port_idx;
 483         int err;
 484
 485         for (port_idx = 0; port_idx < sw->port_count; port_idx++) {
 486                 err = prestera_port_create(sw, port_idx);
 487                 if (err)
 488                         goto err_port_create;
 489         }
 490
 491         return 0;
 492
 493 err_port_create:
 494         list_for_each_entry_safe(port, tmp, &sw->port_list, list)
 495                 prestera_port_destroy(port);
 496
 497         return err;
 498 }
 499
 500 static void prestera_port_handle_event(struct prestera_switch *sw,
 501                                        struct prestera_event *evt, void *arg)
 502 {
 503         struct delayed_work *caching_dw;
 504         struct prestera_port *port;
 505
 506         port = prestera_find_port(sw, evt->port_evt.port_id);
 507         if (!port || !port->dev)
 508                 return;
 509
 510         caching_dw = &port->cached_hw_stats.caching_dw;
 511
 512         prestera_ethtool_port_state_changed(port, &evt->port_evt);
 513
 514         if (evt->id == PRESTERA_PORT_EVENT_MAC_STATE_CHANGED) {
 515                 if (port->state_mac.oper) {
 516                         netif_carrier_on(port->dev);
 517                         if (!delayed_work_pending(caching_dw))
 518                                 queue_delayed_work(prestera_wq, caching_dw, 0);
 519                 } else if (netif_running(port->dev) &&
 520                            netif_carrier_ok(port->dev)) {
 521                         netif_carrier_off(port->dev);
 522                         if (delayed_work_pending(caching_dw))
 523                                 cancel_delayed_work(caching_dw);
 524                 }
 525         }
 526 }
 527
 528 static int prestera_event_handlers_register(struct prestera_switch *sw)
 529 {
 530         return prestera_hw_event_handler_register(sw, PRESTERA_EVENT_TYPE_PORT,
 531                                                   prestera_port_handle_event,
 532                                                   NULL);
 533 }
 534
 535 static void prestera_event_handlers_unregister(struct prestera_switch *sw)
 536 {
 537         prestera_hw_event_handler_unregister(sw, PRESTERA_EVENT_TYPE_PORT,
 538                                              prestera_port_handle_event);
 539 }
 540
 541 static int prestera_switch_set_base_mac_addr(struct prestera_switch *sw)
 542 {
 543         struct device_node *base_mac_np;
 544         struct device_node *np;
 545         int ret;
 546
 547         np = of_find_compatible_node(NULL, NULL, "marvell,prestera");
 548         base_mac_np = of_parse_phandle(np, "base-mac-provider", 0);
 549
 550         ret = of_get_mac_address(base_mac_np, sw->base_mac);
 551         if (ret) {
 552                 eth_random_addr(sw->base_mac);
 553                 dev_info(prestera_dev(sw), "using random base mac address\n");
 554         }
 555         of_node_put(base_mac_np);
 556
 557         return prestera_hw_switch_mac_set(sw, sw->base_mac);
 558 }
 559
 560 struct prestera_lag *prestera_lag_by_id(struct prestera_switch *sw, u16 id)
 561 {
 562         return id < sw->lag_max ? &sw->lags[id] : NULL;
 563 }
 564
 565 static struct prestera_lag *prestera_lag_by_dev(struct prestera_switch *sw,
 566                                                 struct net_device *dev)
 567 {
 568         struct prestera_lag *lag;
 569         u16 id;
 570
 571         for (id = 0; id < sw->lag_max; id++) {
 572                 lag = &sw->lags[id];
 573                 if (lag->dev == dev)
 574                         return lag;
 575         }
 576
 577         return NULL;
 578 }
 579
 580 static struct prestera_lag *prestera_lag_create(struct prestera_switch *sw,
 581                                                 struct net_device *lag_dev)
 582 {
 583         struct prestera_lag *lag = NULL;
 584         u16 id;
 585
 586         for (id = 0; id < sw->lag_max; id++) {
 587                 lag = &sw->lags[id];
 588                 if (!lag->dev)
 589                         break;
 590         }
 591         if (lag) {
 592                 INIT_LIST_HEAD(&lag->members);
 593                 lag->dev = lag_dev;
 594         }
 595
 596         return lag;
 597 }
 598
 599 static void prestera_lag_destroy(struct prestera_switch *sw,
 600                                  struct prestera_lag *lag)
 601 {
 602         WARN_ON(!list_empty(&lag->members));
 603         lag->member_count = 0;
 604         lag->dev = NULL;
 605 }
 606
 607 static int prestera_lag_port_add(struct prestera_port *port,
 608                                  struct net_device *lag_dev)
 609 {
 610         struct prestera_switch *sw = port->sw;
 611         struct prestera_lag *lag;
 612         int err;
 613
 614         lag = prestera_lag_by_dev(sw, lag_dev);
 615         if (!lag) {
 616                 lag = prestera_lag_create(sw, lag_dev);
 617                 if (!lag)
 618                         return -ENOSPC;
 619         }
 620
 621         if (lag->member_count >= sw->lag_member_max)
 622                 return -ENOSPC;
 623
 624         err = prestera_hw_lag_member_add(port, lag->lag_id);
 625         if (err) {
 626                 if (!lag->member_count)
 627                         prestera_lag_destroy(sw, lag);
 628                 return err;
 629         }
 630
 631         list_add(&port->lag_member, &lag->members);
 632         lag->member_count++;
 633         port->lag = lag;
 634
 635         return 0;
 636 }
 637
 638 static int prestera_lag_port_del(struct prestera_port *port)
 639 {
 640         struct prestera_switch *sw = port->sw;
 641         struct prestera_lag *lag = port->lag;
 642         int err;
 643
 644         if (!lag || !lag->member_count)
 645                 return -EINVAL;
 646
 647         err = prestera_hw_lag_member_del(port, lag->lag_id);
 648         if (err)
 649                 return err;
 650
 651         list_del(&port->lag_member);
 652         lag->member_count--;
 653         port->lag = NULL;
 654
 655         if (netif_is_bridge_port(lag->dev)) {
 656                 struct net_device *br_dev;
 657
 658                 br_dev = netdev_master_upper_dev_get(lag->dev);
 659
 660                 prestera_bridge_port_leave(br_dev, port);
 661         }
 662
 663         if (!lag->member_count)
 664                 prestera_lag_destroy(sw, lag);
 665
 666         return 0;
 667 }
 668
 669 bool prestera_port_is_lag_member(const struct prestera_port *port)
 670 {
 671         return !!port->lag;
 672 }
 673
 674 u16 prestera_port_lag_id(const struct prestera_port *port)
 675 {
 676         return port->lag->lag_id;
 677 }
 678
 679 static int prestera_lag_init(struct prestera_switch *sw)
 680 {
 681         u16 id;
 682
 683         sw->lags = kcalloc(sw->lag_max, sizeof(*sw->lags), GFP_KERNEL);
 684         if (!sw->lags)
 685                 return -ENOMEM;
 686
 687         for (id = 0; id < sw->lag_max; id++)
 688                 sw->lags[id].lag_id = id;
 689
 690         return 0;
 691 }
 692
 693 static void prestera_lag_fini(struct prestera_switch *sw)
 694 {
 695         u8 idx;
 696
 697         for (idx = 0; idx < sw->lag_max; idx++)
 698                 WARN_ON(sw->lags[idx].member_count);
 699
 700         kfree(sw->lags);
 701 }
 702
 703 bool prestera_netdev_check(const struct net_device *dev)
 704 {
 705         return dev->netdev_ops == &prestera_netdev_ops;
 706 }
 707
 708 static int prestera_lower_dev_walk(struct net_device *dev,
 709                                    struct netdev_nested_priv *priv)
 710 {
 711         struct prestera_port **pport = (struct prestera_port **)priv->data;
 712
 713         if (prestera_netdev_check(dev)) {
 714                 *pport = netdev_priv(dev);
 715                 return 1;
 716         }
 717
 718         return 0;
 719 }
 720
 721 struct prestera_port *prestera_port_dev_lower_find(struct net_device *dev)
 722 {
 723         struct prestera_port *port = NULL;
 724         struct netdev_nested_priv priv = {
 725                 .data = (void *)&port,
 726         };
 727
 728         if (prestera_netdev_check(dev))
 729                 return netdev_priv(dev);
 730
 731         netdev_walk_all_lower_dev(dev, prestera_lower_dev_walk, &priv);
 732
 733         return port;
 734 }
 735
 736 static int prestera_netdev_port_lower_event(struct net_device *dev,
 737                                             unsigned long event, void *ptr)
 738 {
 739         struct netdev_notifier_changelowerstate_info *info = ptr;
 740         struct netdev_lag_lower_state_info *lower_state_info;
 741         struct prestera_port *port = netdev_priv(dev);
 742         bool enabled;
 743
 744         if (!netif_is_lag_port(dev))
 745                 return 0;
 746         if (!prestera_port_is_lag_member(port))
 747                 return 0;
 748
 749         lower_state_info = info->lower_state_info;
 750         enabled = lower_state_info->link_up && lower_state_info->tx_enabled;
 751
 752         return prestera_hw_lag_member_enable(port, port->lag->lag_id, enabled);
 753 }
 754
 755 static bool prestera_lag_master_check(struct net_device *lag_dev,
 756                                       struct netdev_lag_upper_info *info,
 757                                       struct netlink_ext_ack *ext_ack)
 758 {
 759         if (info->tx_type != NETDEV_LAG_TX_TYPE_HASH) {
 760                 NL_SET_ERR_MSG_MOD(ext_ack, "Unsupported LAG Tx type");
 761                 return false;
 762         }
 763
 764         return true;
 765 }
 766
 767 static int prestera_netdev_port_event(struct net_device *lower,
 768                                       struct net_device *dev,
 769                                       unsigned long event, void *ptr)
 770 {
 771         struct netdev_notifier_info *info = ptr;
 772         struct netdev_notifier_changeupper_info *cu_info;
 773         struct prestera_port *port = netdev_priv(dev);
 774         struct netlink_ext_ack *extack;
 775         struct net_device *upper;
 776
 777         extack = netdev_notifier_info_to_extack(info);
 778         cu_info = container_of(info,
 779                                struct netdev_notifier_changeupper_info,
 780                                info);
 781
 782         switch (event) {
 783         case NETDEV_PRECHANGEUPPER:
 784                 upper = cu_info->upper_dev;
 785                 if (!netif_is_bridge_master(upper) &&
 786                     !netif_is_lag_master(upper)) {
 787                         NL_SET_ERR_MSG_MOD(extack, "Unknown upper device type");
 788                         return -EINVAL;
 789                 }
 790
 791                 if (!cu_info->linking)
 792                         break;
 793
 794                 if (netdev_has_any_upper_dev(upper)) {
 795                         NL_SET_ERR_MSG_MOD(extack, "Upper device is already enslaved");
 796                         return -EINVAL;
 797                 }
 798
 799                 if (netif_is_lag_master(upper) &&
 800                     !prestera_lag_master_check(upper, cu_info->upper_info, extack))
 801                         return -EOPNOTSUPP;
 802                 if (netif_is_lag_master(upper) && vlan_uses_dev(dev)) {
 803                         NL_SET_ERR_MSG_MOD(extack,
 804                                            "Master device is a LAG master and port has a VLAN");
 805                         return -EINVAL;
 806                 }
 807                 if (netif_is_lag_port(dev) && is_vlan_dev(upper) &&
 808                     !netif_is_lag_master(vlan_dev_real_dev(upper))) {
 809                         NL_SET_ERR_MSG_MOD(extack,
 810                                            "Can not put a VLAN on a LAG port");
 811                         return -EINVAL;
 812                 }
 813                 break;
 814
 815         case NETDEV_CHANGEUPPER:
 816                 upper = cu_info->upper_dev;
 817                 if (netif_is_bridge_master(upper)) {
 818                         if (cu_info->linking)
 819                                 return prestera_bridge_port_join(upper, port,
 820                                                                  extack);
 821                         else
 822                                 prestera_bridge_port_leave(upper, port);
 823                 } else if (netif_is_lag_master(upper)) {
 824                         if (cu_info->linking)
 825                                 return prestera_lag_port_add(port, upper);
 826                         else
 827                                 prestera_lag_port_del(port);
 828                 }
 829                 break;
 830
 831         case NETDEV_CHANGELOWERSTATE:
 832                 return prestera_netdev_port_lower_event(dev, event, ptr);
 833         }
 834
 835         return 0;
 836 }
 837
 838 static int prestera_netdevice_lag_event(struct net_device *lag_dev,
 839                                         unsigned long event, void *ptr)
 840 {
 841         struct net_device *dev;
 842         struct list_head *iter;
 843         int err;
 844
 845         netdev_for_each_lower_dev(lag_dev, dev, iter) {
 846                 if (prestera_netdev_check(dev)) {
 847                         err = prestera_netdev_port_event(lag_dev, dev, event,
 848                                                          ptr);
 849                         if (err)
 850                                 return err;
 851                 }
 852         }
 853
 854         return 0;
 855 }
 856
 857 static int prestera_netdev_event_handler(struct notifier_block *nb,
 858                                          unsigned long event, void *ptr)
 859 {
 860         struct net_device *dev = netdev_notifier_info_to_dev(ptr);
 861         int err = 0;
 862
 863         if (prestera_netdev_check(dev))
 864                 err = prestera_netdev_port_event(dev, dev, event, ptr);
 865         else if (netif_is_lag_master(dev))
 866                 err = prestera_netdevice_lag_event(dev, event, ptr);
 867
 868         return notifier_from_errno(err);
 869 }
 870
 871 static int prestera_netdev_event_handler_register(struct prestera_switch *sw)
 872 {
 873         sw->netdev_nb.notifier_call = prestera_netdev_event_handler;
 874
 875         return register_netdevice_notifier(&sw->netdev_nb);
 876 }
 877
 878 static void prestera_netdev_event_handler_unregister(struct prestera_switch *sw)
 879 {
 880         unregister_netdevice_notifier(&sw->netdev_nb);
 881 }
 882
 883 static int prestera_switch_init(struct prestera_switch *sw)
 884 {
 885         int err;
 886
 887         err = prestera_hw_switch_init(sw);
 888         if (err) {
 889                 dev_err(prestera_dev(sw), "Failed to init Switch device\n");
 890                 return err;
 891         }
 892
 893         rwlock_init(&sw->port_list_lock);
 894         INIT_LIST_HEAD(&sw->port_list);
 895
 896         err = prestera_switch_set_base_mac_addr(sw);
 897         if (err)
 898                 return err;
 899
 900         err = prestera_netdev_event_handler_register(sw);
 901         if (err)
 902                 return err;
 903
 904         err = prestera_switchdev_init(sw);
 905         if (err)
 906                 goto err_swdev_register;
 907
 908         err = prestera_rxtx_switch_init(sw);
 909         if (err)
 910                 goto err_rxtx_register;
 911
 912         err = prestera_event_handlers_register(sw);
 913         if (err)
 914                 goto err_handlers_register;
 915
 916         err = prestera_acl_init(sw);
 917         if (err)
 918                 goto err_acl_init;
 919
 920         err = prestera_span_init(sw);
 921         if (err)
 922                 goto err_span_init;
 923
 924         err = prestera_devlink_traps_register(sw);
 925         if (err)
 926                 goto err_dl_register;
 927
 928         err = prestera_lag_init(sw);
 929         if (err)
 930                 goto err_lag_init;
 931
 932         err = prestera_create_ports(sw);
 933         if (err)
 934                 goto err_ports_create;
 935
 936         prestera_devlink_register(sw);
 937         return 0;
 938
 939 err_ports_create:
 940         prestera_lag_fini(sw);
 941 err_lag_init:
 942         prestera_devlink_traps_unregister(sw);
 943 err_dl_register:
 944         prestera_span_fini(sw);
 945 err_span_init:
 946         prestera_acl_fini(sw);
 947 err_acl_init:
 948         prestera_event_handlers_unregister(sw);
 949 err_handlers_register:
 950         prestera_rxtx_switch_fini(sw);
 951 err_rxtx_register:
 952         prestera_switchdev_fini(sw);
 953 err_swdev_register:
 954         prestera_netdev_event_handler_unregister(sw);
 955         prestera_hw_switch_fini(sw);
 956
 957         return err;
 958 }
 959
 960 static void prestera_switch_fini(struct prestera_switch *sw)
 961 {
 962         prestera_devlink_unregister(sw);
 963         prestera_destroy_ports(sw);
 964         prestera_lag_fini(sw);
 965         prestera_devlink_traps_unregister(sw);
 966         prestera_span_fini(sw);
 967         prestera_acl_fini(sw);
 968         prestera_event_handlers_unregister(sw);
 969         prestera_rxtx_switch_fini(sw);
 970         prestera_switchdev_fini(sw);
 971         prestera_netdev_event_handler_unregister(sw);
 972         prestera_hw_switch_fini(sw);
 973 }
 974
 975 int prestera_device_register(struct prestera_device *dev)
 976 {
 977         struct prestera_switch *sw;
 978         int err;
 979
 980         sw = prestera_devlink_alloc(dev);
 981         if (!sw)
 982                 return -ENOMEM;
 983
 984         dev->priv = sw;
 985         sw->dev = dev;
 986
 987         err = prestera_switch_init(sw);
 988         if (err) {
 989                 prestera_devlink_free(sw);
 990                 return err;
 991         }
 992
 993         return 0;
 994 }
 995 EXPORT_SYMBOL(prestera_device_register);
 996
 997 void prestera_device_unregister(struct prestera_device *dev)
 998 {
 999         struct prestera_switch *sw = dev->priv;
1000
1001         prestera_switch_fini(sw);
1002         prestera_devlink_free(sw);
1003 }
1004 EXPORT_SYMBOL(prestera_device_unregister);
1005
1006 static int __init prestera_module_init(void)
1007 {
1008         prestera_wq = alloc_workqueue("prestera", 0, 0);
1009         if (!prestera_wq)
1010                 return -ENOMEM;
1011
1012         return 0;
1013 }
1014
1015 static void __exit prestera_module_exit(void)
1016 {
1017         destroy_workqueue(prestera_wq);
1018 }
1019
1020 module_init(prestera_module_init);
1021 module_exit(prestera_module_exit);
1022
1023 MODULE_LICENSE("Dual BSD/GPL");
1024 MODULE_DESCRIPTION("Marvell Prestera switch driver");