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