drivers/net/ethernet/marvell/prestera/prestera_switchdev.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/if_bridge.h>
   5 #include <linux/if_vlan.h>
   6 #include <linux/kernel.h>
   7 #include <linux/module.h>
   8 #include <linux/notifier.h>
   9 #include <net/netevent.h>
  10 #include <net/switchdev.h>
  11
  12 #include "prestera.h"
  13 #include "prestera_hw.h"
  14 #include "prestera_switchdev.h"
  15
  16 #define PRESTERA_VID_ALL (0xffff)
  17
  18 #define PRESTERA_DEFAULT_AGEING_TIME_MS 300000
  19 #define PRESTERA_MAX_AGEING_TIME_MS 1000000000
  20 #define PRESTERA_MIN_AGEING_TIME_MS 32000
  21
  22 struct prestera_fdb_event_work {
  23         struct work_struct work;
  24         struct switchdev_notifier_fdb_info fdb_info;
  25         struct net_device *dev;
  26         unsigned long event;
  27 };
  28
  29 struct prestera_switchdev {
  30         struct prestera_switch *sw;
  31         struct list_head bridge_list;
  32         bool bridge_8021q_exists;
  33         struct notifier_block swdev_nb_blk;
  34         struct notifier_block swdev_nb;
  35 };
  36
  37 struct prestera_bridge {
  38         struct list_head head;
  39         struct net_device *dev;
  40         struct prestera_switchdev *swdev;
  41         struct list_head port_list;
  42         bool vlan_enabled;
  43         u16 bridge_id;
  44 };
  45
  46 struct prestera_bridge_port {
  47         struct list_head head;
  48         struct net_device *dev;
  49         struct prestera_bridge *bridge;
  50         struct list_head vlan_list;
  51         refcount_t ref_count;
  52         unsigned long flags;
  53         u8 stp_state;
  54 };
  55
  56 struct prestera_bridge_vlan {
  57         struct list_head head;
  58         struct list_head port_vlan_list;
  59         u16 vid;
  60 };
  61
  62 struct prestera_port_vlan {
  63         struct list_head br_vlan_head;
  64         struct list_head port_head;
  65         struct prestera_port *port;
  66         struct prestera_bridge_port *br_port;
  67         u16 vid;
  68 };
  69
  70 static struct workqueue_struct *swdev_wq;
  71
  72 static void prestera_bridge_port_put(struct prestera_bridge_port *br_port);
  73
  74 static int prestera_port_vid_stp_set(struct prestera_port *port, u16 vid,
  75                                      u8 state);
  76
  77 static struct prestera_bridge_vlan *
  78 prestera_bridge_vlan_create(struct prestera_bridge_port *br_port, u16 vid)
  79 {
  80         struct prestera_bridge_vlan *br_vlan;
  81
  82         br_vlan = kzalloc(sizeof(*br_vlan), GFP_KERNEL);
  83         if (!br_vlan)
  84                 return NULL;
  85
  86         INIT_LIST_HEAD(&br_vlan->port_vlan_list);
  87         br_vlan->vid = vid;
  88         list_add(&br_vlan->head, &br_port->vlan_list);
  89
  90         return br_vlan;
  91 }
  92
  93 static void prestera_bridge_vlan_destroy(struct prestera_bridge_vlan *br_vlan)
  94 {
  95         list_del(&br_vlan->head);
  96         WARN_ON(!list_empty(&br_vlan->port_vlan_list));
  97         kfree(br_vlan);
  98 }
  99
 100 static struct prestera_bridge_vlan *
 101 prestera_bridge_vlan_by_vid(struct prestera_bridge_port *br_port, u16 vid)
 102 {
 103         struct prestera_bridge_vlan *br_vlan;
 104
 105         list_for_each_entry(br_vlan, &br_port->vlan_list, head) {
 106                 if (br_vlan->vid == vid)
 107                         return br_vlan;
 108         }
 109
 110         return NULL;
 111 }
 112
 113 static int prestera_bridge_vlan_port_count(struct prestera_bridge *bridge,
 114                                            u16 vid)
 115 {
 116         struct prestera_bridge_port *br_port;
 117         struct prestera_bridge_vlan *br_vlan;
 118         int count = 0;
 119
 120         list_for_each_entry(br_port, &bridge->port_list, head) {
 121                 list_for_each_entry(br_vlan, &br_port->vlan_list, head) {
 122                         if (br_vlan->vid == vid) {
 123                                 count += 1;
 124                                 break;
 125                         }
 126                 }
 127         }
 128
 129         return count;
 130 }
 131
 132 static void prestera_bridge_vlan_put(struct prestera_bridge_vlan *br_vlan)
 133 {
 134         if (list_empty(&br_vlan->port_vlan_list))
 135                 prestera_bridge_vlan_destroy(br_vlan);
 136 }
 137
 138 static struct prestera_port_vlan *
 139 prestera_port_vlan_by_vid(struct prestera_port *port, u16 vid)
 140 {
 141         struct prestera_port_vlan *port_vlan;
 142
 143         list_for_each_entry(port_vlan, &port->vlans_list, port_head) {
 144                 if (port_vlan->vid == vid)
 145                         return port_vlan;
 146         }
 147
 148         return NULL;
 149 }
 150
 151 static struct prestera_port_vlan *
 152 prestera_port_vlan_create(struct prestera_port *port, u16 vid, bool untagged)
 153 {
 154         struct prestera_port_vlan *port_vlan;
 155         int err;
 156
 157         port_vlan = prestera_port_vlan_by_vid(port, vid);
 158         if (port_vlan)
 159                 return ERR_PTR(-EEXIST);
 160
 161         err = prestera_hw_vlan_port_set(port, vid, true, untagged);
 162         if (err)
 163                 return ERR_PTR(err);
 164
 165         port_vlan = kzalloc(sizeof(*port_vlan), GFP_KERNEL);
 166         if (!port_vlan) {
 167                 err = -ENOMEM;
 168                 goto err_port_vlan_alloc;
 169         }
 170
 171         port_vlan->port = port;
 172         port_vlan->vid = vid;
 173
 174         list_add(&port_vlan->port_head, &port->vlans_list);
 175
 176         return port_vlan;
 177
 178 err_port_vlan_alloc:
 179         prestera_hw_vlan_port_set(port, vid, false, false);
 180         return ERR_PTR(err);
 181 }
 182
 183 static int prestera_fdb_add(struct prestera_port *port,
 184                             const unsigned char *mac, u16 vid, bool dynamic)
 185 {
 186         if (prestera_port_is_lag_member(port))
 187                 return prestera_hw_lag_fdb_add(port->sw, prestera_port_lag_id(port),
 188                                               mac, vid, dynamic);
 189
 190         return prestera_hw_fdb_add(port, mac, vid, dynamic);
 191 }
 192
 193 static int prestera_fdb_del(struct prestera_port *port,
 194                             const unsigned char *mac, u16 vid)
 195 {
 196         if (prestera_port_is_lag_member(port))
 197                 return prestera_hw_lag_fdb_del(port->sw, prestera_port_lag_id(port),
 198                                               mac, vid);
 199         else
 200                 return prestera_hw_fdb_del(port, mac, vid);
 201 }
 202
 203 static int prestera_fdb_flush_port_vlan(struct prestera_port *port, u16 vid,
 204                                         u32 mode)
 205 {
 206         if (prestera_port_is_lag_member(port))
 207                 return prestera_hw_fdb_flush_lag_vlan(port->sw, prestera_port_lag_id(port),
 208                                                       vid, mode);
 209         else
 210                 return prestera_hw_fdb_flush_port_vlan(port, vid, mode);
 211 }
 212
 213 static int prestera_fdb_flush_port(struct prestera_port *port, u32 mode)
 214 {
 215         if (prestera_port_is_lag_member(port))
 216                 return prestera_hw_fdb_flush_lag(port->sw, prestera_port_lag_id(port),
 217                                                  mode);
 218         else
 219                 return prestera_hw_fdb_flush_port(port, mode);
 220 }
 221
 222 static void
 223 prestera_port_vlan_bridge_leave(struct prestera_port_vlan *port_vlan)
 224 {
 225         u32 fdb_flush_mode = PRESTERA_FDB_FLUSH_MODE_DYNAMIC;
 226         struct prestera_port *port = port_vlan->port;
 227         struct prestera_bridge_vlan *br_vlan;
 228         struct prestera_bridge_port *br_port;
 229         bool last_port, last_vlan;
 230         u16 vid = port_vlan->vid;
 231         int port_count;
 232
 233         br_port = port_vlan->br_port;
 234         port_count = prestera_bridge_vlan_port_count(br_port->bridge, vid);
 235         br_vlan = prestera_bridge_vlan_by_vid(br_port, vid);
 236
 237         last_vlan = list_is_singular(&br_port->vlan_list);
 238         last_port = port_count == 1;
 239
 240         if (last_vlan)
 241                 prestera_fdb_flush_port(port, fdb_flush_mode);
 242         else if (last_port)
 243                 prestera_hw_fdb_flush_vlan(port->sw, vid, fdb_flush_mode);
 244         else
 245                 prestera_fdb_flush_port_vlan(port, vid, fdb_flush_mode);
 246
 247         list_del(&port_vlan->br_vlan_head);
 248         prestera_bridge_vlan_put(br_vlan);
 249         prestera_bridge_port_put(br_port);
 250         port_vlan->br_port = NULL;
 251 }
 252
 253 static void prestera_port_vlan_destroy(struct prestera_port_vlan *port_vlan)
 254 {
 255         struct prestera_port *port = port_vlan->port;
 256         u16 vid = port_vlan->vid;
 257
 258         if (port_vlan->br_port)
 259                 prestera_port_vlan_bridge_leave(port_vlan);
 260
 261         prestera_hw_vlan_port_set(port, vid, false, false);
 262         list_del(&port_vlan->port_head);
 263         kfree(port_vlan);
 264 }
 265
 266 static struct prestera_bridge *
 267 prestera_bridge_create(struct prestera_switchdev *swdev, struct net_device *dev)
 268 {
 269         bool vlan_enabled = br_vlan_enabled(dev);
 270         struct prestera_bridge *bridge;
 271         u16 bridge_id;
 272         int err;
 273
 274         if (vlan_enabled && swdev->bridge_8021q_exists) {
 275                 netdev_err(dev, "Only one VLAN-aware bridge is supported\n");
 276                 return ERR_PTR(-EINVAL);
 277         }
 278
 279         bridge = kzalloc(sizeof(*bridge), GFP_KERNEL);
 280         if (!bridge)
 281                 return ERR_PTR(-ENOMEM);
 282
 283         if (vlan_enabled) {
 284                 swdev->bridge_8021q_exists = true;
 285         } else {
 286                 err = prestera_hw_bridge_create(swdev->sw, &bridge_id);
 287                 if (err) {
 288                         kfree(bridge);
 289                         return ERR_PTR(err);
 290                 }
 291
 292                 bridge->bridge_id = bridge_id;
 293         }
 294
 295         bridge->vlan_enabled = vlan_enabled;
 296         bridge->swdev = swdev;
 297         bridge->dev = dev;
 298
 299         INIT_LIST_HEAD(&bridge->port_list);
 300
 301         list_add(&bridge->head, &swdev->bridge_list);
 302
 303         return bridge;
 304 }
 305
 306 static void prestera_bridge_destroy(struct prestera_bridge *bridge)
 307 {
 308         struct prestera_switchdev *swdev = bridge->swdev;
 309
 310         list_del(&bridge->head);
 311
 312         if (bridge->vlan_enabled)
 313                 swdev->bridge_8021q_exists = false;
 314         else
 315                 prestera_hw_bridge_delete(swdev->sw, bridge->bridge_id);
 316
 317         WARN_ON(!list_empty(&bridge->port_list));
 318         kfree(bridge);
 319 }
 320
 321 static void prestera_bridge_put(struct prestera_bridge *bridge)
 322 {
 323         if (list_empty(&bridge->port_list))
 324                 prestera_bridge_destroy(bridge);
 325 }
 326
 327 static
 328 struct prestera_bridge *prestera_bridge_by_dev(struct prestera_switchdev *swdev,
 329                                                const struct net_device *dev)
 330 {
 331         struct prestera_bridge *bridge;
 332
 333         list_for_each_entry(bridge, &swdev->bridge_list, head)
 334                 if (bridge->dev == dev)
 335                         return bridge;
 336
 337         return NULL;
 338 }
 339
 340 static struct prestera_bridge_port *
 341 __prestera_bridge_port_by_dev(struct prestera_bridge *bridge,
 342                               struct net_device *dev)
 343 {
 344         struct prestera_bridge_port *br_port;
 345
 346         list_for_each_entry(br_port, &bridge->port_list, head) {
 347                 if (br_port->dev == dev)
 348                         return br_port;
 349         }
 350
 351         return NULL;
 352 }
 353
 354 static int prestera_match_upper_bridge_dev(struct net_device *dev,
 355                                            struct netdev_nested_priv *priv)
 356 {
 357         if (netif_is_bridge_master(dev))
 358                 priv->data = dev;
 359
 360         return 0;
 361 }
 362
 363 static struct net_device *prestera_get_upper_bridge_dev(struct net_device *dev)
 364 {
 365         struct netdev_nested_priv priv = { };
 366
 367         netdev_walk_all_upper_dev_rcu(dev, prestera_match_upper_bridge_dev,
 368                                       &priv);
 369         return priv.data;
 370 }
 371
 372 static struct prestera_bridge_port *
 373 prestera_bridge_port_by_dev(struct prestera_switchdev *swdev,
 374                             struct net_device *dev)
 375 {
 376         struct net_device *br_dev = prestera_get_upper_bridge_dev(dev);
 377         struct prestera_bridge *bridge;
 378
 379         if (!br_dev)
 380                 return NULL;
 381
 382         bridge = prestera_bridge_by_dev(swdev, br_dev);
 383         if (!bridge)
 384                 return NULL;
 385
 386         return __prestera_bridge_port_by_dev(bridge, dev);
 387 }
 388
 389 static struct prestera_bridge_port *
 390 prestera_bridge_port_create(struct prestera_bridge *bridge,
 391                             struct net_device *dev)
 392 {
 393         struct prestera_bridge_port *br_port;
 394
 395         br_port = kzalloc(sizeof(*br_port), GFP_KERNEL);
 396         if (!br_port)
 397                 return NULL;
 398
 399         br_port->flags = BR_LEARNING | BR_FLOOD | BR_LEARNING_SYNC |
 400                                 BR_MCAST_FLOOD;
 401         br_port->stp_state = BR_STATE_DISABLED;
 402         refcount_set(&br_port->ref_count, 1);
 403         br_port->bridge = bridge;
 404         br_port->dev = dev;
 405
 406         INIT_LIST_HEAD(&br_port->vlan_list);
 407         list_add(&br_port->head, &bridge->port_list);
 408
 409         return br_port;
 410 }
 411
 412 static void
 413 prestera_bridge_port_destroy(struct prestera_bridge_port *br_port)
 414 {
 415         list_del(&br_port->head);
 416         WARN_ON(!list_empty(&br_port->vlan_list));
 417         kfree(br_port);
 418 }
 419
 420 static void prestera_bridge_port_get(struct prestera_bridge_port *br_port)
 421 {
 422         refcount_inc(&br_port->ref_count);
 423 }
 424
 425 static void prestera_bridge_port_put(struct prestera_bridge_port *br_port)
 426 {
 427         struct prestera_bridge *bridge = br_port->bridge;
 428
 429         if (refcount_dec_and_test(&br_port->ref_count)) {
 430                 prestera_bridge_port_destroy(br_port);
 431                 prestera_bridge_put(bridge);
 432         }
 433 }
 434
 435 static struct prestera_bridge_port *
 436 prestera_bridge_port_add(struct prestera_bridge *bridge, struct net_device *dev)
 437 {
 438         struct prestera_bridge_port *br_port;
 439
 440         br_port = __prestera_bridge_port_by_dev(bridge, dev);
 441         if (br_port) {
 442                 prestera_bridge_port_get(br_port);
 443                 return br_port;
 444         }
 445
 446         br_port = prestera_bridge_port_create(bridge, dev);
 447         if (!br_port)
 448                 return ERR_PTR(-ENOMEM);
 449
 450         return br_port;
 451 }
 452
 453 static int
 454 prestera_bridge_1d_port_join(struct prestera_bridge_port *br_port)
 455 {
 456         struct prestera_port *port = netdev_priv(br_port->dev);
 457         struct prestera_bridge *bridge = br_port->bridge;
 458         int err;
 459
 460         err = prestera_hw_bridge_port_add(port, bridge->bridge_id);
 461         if (err)
 462                 return err;
 463
 464         err = prestera_hw_port_flood_set(port, BR_FLOOD | BR_MCAST_FLOOD,
 465                                          br_port->flags);
 466         if (err)
 467                 goto err_port_flood_set;
 468
 469         err = prestera_hw_port_learning_set(port, br_port->flags & BR_LEARNING);
 470         if (err)
 471                 goto err_port_learning_set;
 472
 473         return 0;
 474
 475 err_port_learning_set:
 476 err_port_flood_set:
 477         prestera_hw_bridge_port_delete(port, bridge->bridge_id);
 478
 479         return err;
 480 }
 481
 482 int prestera_bridge_port_join(struct net_device *br_dev,
 483                               struct prestera_port *port)
 484 {
 485         struct prestera_switchdev *swdev = port->sw->swdev;
 486         struct prestera_bridge_port *br_port;
 487         struct prestera_bridge *bridge;
 488         int err;
 489
 490         bridge = prestera_bridge_by_dev(swdev, br_dev);
 491         if (!bridge) {
 492                 bridge = prestera_bridge_create(swdev, br_dev);
 493                 if (IS_ERR(bridge))
 494                         return PTR_ERR(bridge);
 495         }
 496
 497         br_port = prestera_bridge_port_add(bridge, port->dev);
 498         if (IS_ERR(br_port)) {
 499                 err = PTR_ERR(br_port);
 500                 goto err_brport_create;
 501         }
 502
 503         if (bridge->vlan_enabled)
 504                 return 0;
 505
 506         err = prestera_bridge_1d_port_join(br_port);
 507         if (err)
 508                 goto err_port_join;
 509
 510         return 0;
 511
 512 err_port_join:
 513         prestera_bridge_port_put(br_port);
 514 err_brport_create:
 515         prestera_bridge_put(bridge);
 516         return err;
 517 }
 518
 519 static void prestera_bridge_1q_port_leave(struct prestera_bridge_port *br_port)
 520 {
 521         struct prestera_port *port = netdev_priv(br_port->dev);
 522
 523         prestera_hw_fdb_flush_port(port, PRESTERA_FDB_FLUSH_MODE_ALL);
 524         prestera_port_pvid_set(port, PRESTERA_DEFAULT_VID);
 525 }
 526
 527 static void prestera_bridge_1d_port_leave(struct prestera_bridge_port *br_port)
 528 {
 529         struct prestera_port *port = netdev_priv(br_port->dev);
 530
 531         prestera_hw_fdb_flush_port(port, PRESTERA_FDB_FLUSH_MODE_ALL);
 532         prestera_hw_bridge_port_delete(port, br_port->bridge->bridge_id);
 533 }
 534
 535 static int prestera_port_vid_stp_set(struct prestera_port *port, u16 vid,
 536                                      u8 state)
 537 {
 538         u8 hw_state = state;
 539
 540         switch (state) {
 541         case BR_STATE_DISABLED:
 542                 hw_state = PRESTERA_STP_DISABLED;
 543                 break;
 544
 545         case BR_STATE_BLOCKING:
 546         case BR_STATE_LISTENING:
 547                 hw_state = PRESTERA_STP_BLOCK_LISTEN;
 548                 break;
 549
 550         case BR_STATE_LEARNING:
 551                 hw_state = PRESTERA_STP_LEARN;
 552                 break;
 553
 554         case BR_STATE_FORWARDING:
 555                 hw_state = PRESTERA_STP_FORWARD;
 556                 break;
 557
 558         default:
 559                 return -EINVAL;
 560         }
 561
 562         return prestera_hw_vlan_port_stp_set(port, vid, hw_state);
 563 }
 564
 565 void prestera_bridge_port_leave(struct net_device *br_dev,
 566                                 struct prestera_port *port)
 567 {
 568         struct prestera_switchdev *swdev = port->sw->swdev;
 569         struct prestera_bridge_port *br_port;
 570         struct prestera_bridge *bridge;
 571
 572         bridge = prestera_bridge_by_dev(swdev, br_dev);
 573         if (!bridge)
 574                 return;
 575
 576         br_port = __prestera_bridge_port_by_dev(bridge, port->dev);
 577         if (!br_port)
 578                 return;
 579
 580         bridge = br_port->bridge;
 581
 582         if (bridge->vlan_enabled)
 583                 prestera_bridge_1q_port_leave(br_port);
 584         else
 585                 prestera_bridge_1d_port_leave(br_port);
 586
 587         prestera_hw_port_learning_set(port, false);
 588         prestera_hw_port_flood_set(port, BR_FLOOD | BR_MCAST_FLOOD, 0);
 589         prestera_port_vid_stp_set(port, PRESTERA_VID_ALL, BR_STATE_FORWARDING);
 590         prestera_bridge_port_put(br_port);
 591 }
 592
 593 static int prestera_port_attr_br_flags_set(struct prestera_port *port,
 594                                            struct net_device *dev,
 595                                            struct switchdev_brport_flags flags)
 596 {
 597         struct prestera_bridge_port *br_port;
 598         int err;
 599
 600         br_port = prestera_bridge_port_by_dev(port->sw->swdev, dev);
 601         if (!br_port)
 602                 return 0;
 603
 604         err = prestera_hw_port_flood_set(port, flags.mask, flags.val);
 605         if (err)
 606                 return err;
 607
 608         if (flags.mask & BR_LEARNING) {
 609                 err = prestera_hw_port_learning_set(port,
 610                                                     flags.val & BR_LEARNING);
 611                 if (err)
 612                         return err;
 613         }
 614
 615         memcpy(&br_port->flags, &flags.val, sizeof(flags.val));
 616
 617         return 0;
 618 }
 619
 620 static int prestera_port_attr_br_ageing_set(struct prestera_port *port,
 621                                             unsigned long ageing_clock_t)
 622 {
 623         unsigned long ageing_jiffies = clock_t_to_jiffies(ageing_clock_t);
 624         u32 ageing_time_ms = jiffies_to_msecs(ageing_jiffies);
 625         struct prestera_switch *sw = port->sw;
 626
 627         if (ageing_time_ms < PRESTERA_MIN_AGEING_TIME_MS ||
 628             ageing_time_ms > PRESTERA_MAX_AGEING_TIME_MS)
 629                 return -ERANGE;
 630
 631         return prestera_hw_switch_ageing_set(sw, ageing_time_ms);
 632 }
 633
 634 static int prestera_port_attr_br_vlan_set(struct prestera_port *port,
 635                                           struct net_device *dev,
 636                                           bool vlan_enabled)
 637 {
 638         struct prestera_switch *sw = port->sw;
 639         struct prestera_bridge *bridge;
 640
 641         bridge = prestera_bridge_by_dev(sw->swdev, dev);
 642         if (WARN_ON(!bridge))
 643                 return -EINVAL;
 644
 645         if (bridge->vlan_enabled == vlan_enabled)
 646                 return 0;
 647
 648         netdev_err(bridge->dev, "VLAN filtering can't be changed for existing bridge\n");
 649
 650         return -EINVAL;
 651 }
 652
 653 static int prestera_port_bridge_vlan_stp_set(struct prestera_port *port,
 654                                              struct prestera_bridge_vlan *br_vlan,
 655                                              u8 state)
 656 {
 657         struct prestera_port_vlan *port_vlan;
 658
 659         list_for_each_entry(port_vlan, &br_vlan->port_vlan_list, br_vlan_head) {
 660                 if (port_vlan->port != port)
 661                         continue;
 662
 663                 return prestera_port_vid_stp_set(port, br_vlan->vid, state);
 664         }
 665
 666         return 0;
 667 }
 668
 669 static int prestera_port_attr_stp_state_set(struct prestera_port *port,
 670                                             struct net_device *dev,
 671                                             u8 state)
 672 {
 673         struct prestera_bridge_port *br_port;
 674         struct prestera_bridge_vlan *br_vlan;
 675         int err;
 676         u16 vid;
 677
 678         br_port = prestera_bridge_port_by_dev(port->sw->swdev, dev);
 679         if (!br_port)
 680                 return 0;
 681
 682         if (!br_port->bridge->vlan_enabled) {
 683                 vid = br_port->bridge->bridge_id;
 684                 err = prestera_port_vid_stp_set(port, vid, state);
 685                 if (err)
 686                         goto err_port_stp_set;
 687         } else {
 688                 list_for_each_entry(br_vlan, &br_port->vlan_list, head) {
 689                         err = prestera_port_bridge_vlan_stp_set(port, br_vlan,
 690                                                                 state);
 691                         if (err)
 692                                 goto err_port_vlan_stp_set;
 693                 }
 694         }
 695
 696         br_port->stp_state = state;
 697
 698         return 0;
 699
 700 err_port_vlan_stp_set:
 701         list_for_each_entry_continue_reverse(br_vlan, &br_port->vlan_list, head)
 702                 prestera_port_bridge_vlan_stp_set(port, br_vlan, br_port->stp_state);
 703         return err;
 704
 705 err_port_stp_set:
 706         prestera_port_vid_stp_set(port, vid, br_port->stp_state);
 707
 708         return err;
 709 }
 710
 711 static int prestera_port_obj_attr_set(struct net_device *dev, const void *ctx,
 712                                       const struct switchdev_attr *attr,
 713                                       struct netlink_ext_ack *extack)
 714 {
 715         struct prestera_port *port = netdev_priv(dev);
 716         int err = 0;
 717
 718         switch (attr->id) {
 719         case SWITCHDEV_ATTR_ID_PORT_STP_STATE:
 720                 err = prestera_port_attr_stp_state_set(port, attr->orig_dev,
 721                                                        attr->u.stp_state);
 722                 break;
 723         case SWITCHDEV_ATTR_ID_PORT_PRE_BRIDGE_FLAGS:
 724                 if (attr->u.brport_flags.mask &
 725                     ~(BR_LEARNING | BR_FLOOD | BR_MCAST_FLOOD))
 726                         err = -EINVAL;
 727                 break;
 728         case SWITCHDEV_ATTR_ID_PORT_BRIDGE_FLAGS:
 729                 err = prestera_port_attr_br_flags_set(port, attr->orig_dev,
 730                                                       attr->u.brport_flags);
 731                 break;
 732         case SWITCHDEV_ATTR_ID_BRIDGE_AGEING_TIME:
 733                 err = prestera_port_attr_br_ageing_set(port,
 734                                                        attr->u.ageing_time);
 735                 break;
 736         case SWITCHDEV_ATTR_ID_BRIDGE_VLAN_FILTERING:
 737                 err = prestera_port_attr_br_vlan_set(port, attr->orig_dev,
 738                                                      attr->u.vlan_filtering);
 739                 break;
 740         default:
 741                 err = -EOPNOTSUPP;
 742         }
 743
 744         return err;
 745 }
 746
 747 static void
 748 prestera_fdb_offload_notify(struct prestera_port *port,
 749                             struct switchdev_notifier_fdb_info *info)
 750 {
 751         struct switchdev_notifier_fdb_info send_info = {};
 752
 753         send_info.addr = info->addr;
 754         send_info.vid = info->vid;
 755         send_info.offloaded = true;
 756
 757         call_switchdev_notifiers(SWITCHDEV_FDB_OFFLOADED, port->dev,
 758                                  &send_info.info, NULL);
 759 }
 760
 761 static int prestera_port_fdb_set(struct prestera_port *port,
 762                                  struct switchdev_notifier_fdb_info *fdb_info,
 763                                  bool adding)
 764 {
 765         struct prestera_switch *sw = port->sw;
 766         struct prestera_bridge_port *br_port;
 767         struct prestera_bridge *bridge;
 768         int err;
 769         u16 vid;
 770
 771         br_port = prestera_bridge_port_by_dev(sw->swdev, port->dev);
 772         if (!br_port)
 773                 return -EINVAL;
 774
 775         bridge = br_port->bridge;
 776
 777         if (bridge->vlan_enabled)
 778                 vid = fdb_info->vid;
 779         else
 780                 vid = bridge->bridge_id;
 781
 782         if (adding)
 783                 err = prestera_fdb_add(port, fdb_info->addr, vid, false);
 784         else
 785                 err = prestera_fdb_del(port, fdb_info->addr, vid);
 786
 787         return err;
 788 }
 789
 790 static void prestera_fdb_event_work(struct work_struct *work)
 791 {
 792         struct switchdev_notifier_fdb_info *fdb_info;
 793         struct prestera_fdb_event_work *swdev_work;
 794         struct prestera_port *port;
 795         struct net_device *dev;
 796         int err;
 797
 798         swdev_work = container_of(work, struct prestera_fdb_event_work, work);
 799         dev = swdev_work->dev;
 800
 801         rtnl_lock();
 802
 803         port = prestera_port_dev_lower_find(dev);
 804         if (!port)
 805                 goto out_unlock;
 806
 807         switch (swdev_work->event) {
 808         case SWITCHDEV_FDB_ADD_TO_DEVICE:
 809                 fdb_info = &swdev_work->fdb_info;
 810                 if (!fdb_info->added_by_user || fdb_info->is_local)
 811                         break;
 812
 813                 err = prestera_port_fdb_set(port, fdb_info, true);
 814                 if (err)
 815                         break;
 816
 817                 prestera_fdb_offload_notify(port, fdb_info);
 818                 break;
 819
 820         case SWITCHDEV_FDB_DEL_TO_DEVICE:
 821                 fdb_info = &swdev_work->fdb_info;
 822                 prestera_port_fdb_set(port, fdb_info, false);
 823                 break;
 824         }
 825
 826 out_unlock:
 827         rtnl_unlock();
 828
 829         kfree(swdev_work->fdb_info.addr);
 830         kfree(swdev_work);
 831         dev_put(dev);
 832 }
 833
 834 static int prestera_switchdev_event(struct notifier_block *unused,
 835                                     unsigned long event, void *ptr)
 836 {
 837         struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
 838         struct switchdev_notifier_fdb_info *fdb_info;
 839         struct switchdev_notifier_info *info = ptr;
 840         struct prestera_fdb_event_work *swdev_work;
 841         struct net_device *upper;
 842         int err;
 843
 844         if (event == SWITCHDEV_PORT_ATTR_SET) {
 845                 err = switchdev_handle_port_attr_set(dev, ptr,
 846                                                      prestera_netdev_check,
 847                                                      prestera_port_obj_attr_set);
 848                 return notifier_from_errno(err);
 849         }
 850
 851         if (!prestera_netdev_check(dev))
 852                 return NOTIFY_DONE;
 853
 854         upper = netdev_master_upper_dev_get_rcu(dev);
 855         if (!upper)
 856                 return NOTIFY_DONE;
 857
 858         if (!netif_is_bridge_master(upper))
 859                 return NOTIFY_DONE;
 860
 861         swdev_work = kzalloc(sizeof(*swdev_work), GFP_ATOMIC);
 862         if (!swdev_work)
 863                 return NOTIFY_BAD;
 864
 865         swdev_work->event = event;
 866         swdev_work->dev = dev;
 867
 868         switch (event) {
 869         case SWITCHDEV_FDB_ADD_TO_DEVICE:
 870         case SWITCHDEV_FDB_DEL_TO_DEVICE:
 871                 fdb_info = container_of(info,
 872                                         struct switchdev_notifier_fdb_info,
 873                                         info);
 874
 875                 INIT_WORK(&swdev_work->work, prestera_fdb_event_work);
 876                 memcpy(&swdev_work->fdb_info, ptr,
 877                        sizeof(swdev_work->fdb_info));
 878
 879                 swdev_work->fdb_info.addr = kzalloc(ETH_ALEN, GFP_ATOMIC);
 880                 if (!swdev_work->fdb_info.addr)
 881                         goto out_bad;
 882
 883                 ether_addr_copy((u8 *)swdev_work->fdb_info.addr,
 884                                 fdb_info->addr);
 885                 dev_hold(dev);
 886                 break;
 887
 888         default:
 889                 kfree(swdev_work);
 890                 return NOTIFY_DONE;
 891         }
 892
 893         queue_work(swdev_wq, &swdev_work->work);
 894         return NOTIFY_DONE;
 895
 896 out_bad:
 897         kfree(swdev_work);
 898         return NOTIFY_BAD;
 899 }
 900
 901 static int
 902 prestera_port_vlan_bridge_join(struct prestera_port_vlan *port_vlan,
 903                                struct prestera_bridge_port *br_port)
 904 {
 905         struct prestera_port *port = port_vlan->port;
 906         struct prestera_bridge_vlan *br_vlan;
 907         u16 vid = port_vlan->vid;
 908         int err;
 909
 910         if (port_vlan->br_port)
 911                 return 0;
 912
 913         err = prestera_hw_port_flood_set(port, BR_FLOOD | BR_MCAST_FLOOD,
 914                                          br_port->flags);
 915         if (err)
 916                 return err;
 917
 918         err = prestera_hw_port_learning_set(port, br_port->flags & BR_LEARNING);
 919         if (err)
 920                 goto err_port_learning_set;
 921
 922         err = prestera_port_vid_stp_set(port, vid, br_port->stp_state);
 923         if (err)
 924                 goto err_port_vid_stp_set;
 925
 926         br_vlan = prestera_bridge_vlan_by_vid(br_port, vid);
 927         if (!br_vlan) {
 928                 br_vlan = prestera_bridge_vlan_create(br_port, vid);
 929                 if (!br_vlan) {
 930                         err = -ENOMEM;
 931                         goto err_bridge_vlan_get;
 932                 }
 933         }
 934
 935         list_add(&port_vlan->br_vlan_head, &br_vlan->port_vlan_list);
 936
 937         prestera_bridge_port_get(br_port);
 938         port_vlan->br_port = br_port;
 939
 940         return 0;
 941
 942 err_bridge_vlan_get:
 943         prestera_port_vid_stp_set(port, vid, BR_STATE_FORWARDING);
 944 err_port_vid_stp_set:
 945         prestera_hw_port_learning_set(port, false);
 946 err_port_learning_set:
 947         return err;
 948 }
 949
 950 static int
 951 prestera_bridge_port_vlan_add(struct prestera_port *port,
 952                               struct prestera_bridge_port *br_port,
 953                               u16 vid, bool is_untagged, bool is_pvid,
 954                               struct netlink_ext_ack *extack)
 955 {
 956         struct prestera_port_vlan *port_vlan;
 957         u16 old_pvid = port->pvid;
 958         u16 pvid;
 959         int err;
 960
 961         if (is_pvid)
 962                 pvid = vid;
 963         else
 964                 pvid = port->pvid == vid ? 0 : port->pvid;
 965
 966         port_vlan = prestera_port_vlan_by_vid(port, vid);
 967         if (port_vlan && port_vlan->br_port != br_port)
 968                 return -EEXIST;
 969
 970         if (!port_vlan) {
 971                 port_vlan = prestera_port_vlan_create(port, vid, is_untagged);
 972                 if (IS_ERR(port_vlan))
 973                         return PTR_ERR(port_vlan);
 974         } else {
 975                 err = prestera_hw_vlan_port_set(port, vid, true, is_untagged);
 976                 if (err)
 977                         goto err_port_vlan_set;
 978         }
 979
 980         err = prestera_port_pvid_set(port, pvid);
 981         if (err)
 982                 goto err_port_pvid_set;
 983
 984         err = prestera_port_vlan_bridge_join(port_vlan, br_port);
 985         if (err)
 986                 goto err_port_vlan_bridge_join;
 987
 988         return 0;
 989
 990 err_port_vlan_bridge_join:
 991         prestera_port_pvid_set(port, old_pvid);
 992 err_port_pvid_set:
 993         prestera_hw_vlan_port_set(port, vid, false, false);
 994 err_port_vlan_set:
 995         prestera_port_vlan_destroy(port_vlan);
 996
 997         return err;
 998 }
 999
1000 static void
1001 prestera_bridge_port_vlan_del(struct prestera_port *port,
1002                               struct prestera_bridge_port *br_port, u16 vid)
1003 {
1004         u16 pvid = port->pvid == vid ? 0 : port->pvid;
1005         struct prestera_port_vlan *port_vlan;
1006
1007         port_vlan = prestera_port_vlan_by_vid(port, vid);
1008         if (WARN_ON(!port_vlan))
1009                 return;
1010
1011         prestera_port_vlan_bridge_leave(port_vlan);
1012         prestera_port_pvid_set(port, pvid);
1013         prestera_port_vlan_destroy(port_vlan);
1014 }
1015
1016 static int prestera_port_vlans_add(struct prestera_port *port,
1017                                    const struct switchdev_obj_port_vlan *vlan,
1018                                    struct netlink_ext_ack *extack)
1019 {
1020         bool flag_untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
1021         bool flag_pvid = vlan->flags & BRIDGE_VLAN_INFO_PVID;
1022         struct net_device *orig_dev = vlan->obj.orig_dev;
1023         struct prestera_bridge_port *br_port;
1024         struct prestera_switch *sw = port->sw;
1025         struct prestera_bridge *bridge;
1026
1027         if (netif_is_bridge_master(orig_dev))
1028                 return 0;
1029
1030         br_port = prestera_bridge_port_by_dev(sw->swdev, port->dev);
1031         if (WARN_ON(!br_port))
1032                 return -EINVAL;
1033
1034         bridge = br_port->bridge;
1035         if (!bridge->vlan_enabled)
1036                 return 0;
1037
1038         return prestera_bridge_port_vlan_add(port, br_port,
1039                                              vlan->vid, flag_untagged,
1040                                              flag_pvid, extack);
1041 }
1042
1043 static int prestera_port_obj_add(struct net_device *dev, const void *ctx,
1044                                  const struct switchdev_obj *obj,
1045                                  struct netlink_ext_ack *extack)
1046 {
1047         struct prestera_port *port = netdev_priv(dev);
1048         const struct switchdev_obj_port_vlan *vlan;
1049
1050         switch (obj->id) {
1051         case SWITCHDEV_OBJ_ID_PORT_VLAN:
1052                 vlan = SWITCHDEV_OBJ_PORT_VLAN(obj);
1053                 return prestera_port_vlans_add(port, vlan, extack);
1054         default:
1055                 return -EOPNOTSUPP;
1056         }
1057 }
1058
1059 static int prestera_port_vlans_del(struct prestera_port *port,
1060                                    const struct switchdev_obj_port_vlan *vlan)
1061 {
1062         struct net_device *orig_dev = vlan->obj.orig_dev;
1063         struct prestera_bridge_port *br_port;
1064         struct prestera_switch *sw = port->sw;
1065
1066         if (netif_is_bridge_master(orig_dev))
1067                 return -EOPNOTSUPP;
1068
1069         br_port = prestera_bridge_port_by_dev(sw->swdev, port->dev);
1070         if (WARN_ON(!br_port))
1071                 return -EINVAL;
1072
1073         if (!br_port->bridge->vlan_enabled)
1074                 return 0;
1075
1076         prestera_bridge_port_vlan_del(port, br_port, vlan->vid);
1077
1078         return 0;
1079 }
1080
1081 static int prestera_port_obj_del(struct net_device *dev, const void *ctx,
1082                                  const struct switchdev_obj *obj)
1083 {
1084         struct prestera_port *port = netdev_priv(dev);
1085
1086         switch (obj->id) {
1087         case SWITCHDEV_OBJ_ID_PORT_VLAN:
1088                 return prestera_port_vlans_del(port, SWITCHDEV_OBJ_PORT_VLAN(obj));
1089         default:
1090                 return -EOPNOTSUPP;
1091         }
1092 }
1093
1094 static int prestera_switchdev_blk_event(struct notifier_block *unused,
1095                                         unsigned long event, void *ptr)
1096 {
1097         struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
1098         int err;
1099
1100         switch (event) {
1101         case SWITCHDEV_PORT_OBJ_ADD:
1102                 err = switchdev_handle_port_obj_add(dev, ptr,
1103                                                     prestera_netdev_check,
1104                                                     prestera_port_obj_add);
1105                 break;
1106         case SWITCHDEV_PORT_OBJ_DEL:
1107                 err = switchdev_handle_port_obj_del(dev, ptr,
1108                                                     prestera_netdev_check,
1109                                                     prestera_port_obj_del);
1110                 break;
1111         case SWITCHDEV_PORT_ATTR_SET:
1112                 err = switchdev_handle_port_attr_set(dev, ptr,
1113                                                      prestera_netdev_check,
1114                                                      prestera_port_obj_attr_set);
1115                 break;
1116         default:
1117                 err = -EOPNOTSUPP;
1118         }
1119
1120         return notifier_from_errno(err);
1121 }
1122
1123 static void prestera_fdb_event(struct prestera_switch *sw,
1124                                struct prestera_event *evt, void *arg)
1125 {
1126         struct switchdev_notifier_fdb_info info = {};
1127         struct net_device *dev = NULL;
1128         struct prestera_port *port;
1129         struct prestera_lag *lag;
1130
1131         switch (evt->fdb_evt.type) {
1132         case PRESTERA_FDB_ENTRY_TYPE_REG_PORT:
1133                 port = prestera_find_port(sw, evt->fdb_evt.dest.port_id);
1134                 if (port)
1135                         dev = port->dev;
1136                 break;
1137         case PRESTERA_FDB_ENTRY_TYPE_LAG:
1138                 lag = prestera_lag_by_id(sw, evt->fdb_evt.dest.lag_id);
1139                 if (lag)
1140                         dev = lag->dev;
1141                 break;
1142         default:
1143                 return;
1144         }
1145
1146         if (!dev)
1147                 return;
1148
1149         info.addr = evt->fdb_evt.data.mac;
1150         info.vid = evt->fdb_evt.vid;
1151         info.offloaded = true;
1152
1153         rtnl_lock();
1154
1155         switch (evt->id) {
1156         case PRESTERA_FDB_EVENT_LEARNED:
1157                 call_switchdev_notifiers(SWITCHDEV_FDB_ADD_TO_BRIDGE,
1158                                          dev, &info.info, NULL);
1159                 break;
1160         case PRESTERA_FDB_EVENT_AGED:
1161                 call_switchdev_notifiers(SWITCHDEV_FDB_DEL_TO_BRIDGE,
1162                                          dev, &info.info, NULL);
1163                 break;
1164         }
1165
1166         rtnl_unlock();
1167 }
1168
1169 static int prestera_fdb_init(struct prestera_switch *sw)
1170 {
1171         int err;
1172
1173         err = prestera_hw_event_handler_register(sw, PRESTERA_EVENT_TYPE_FDB,
1174                                                  prestera_fdb_event, NULL);
1175         if (err)
1176                 return err;
1177
1178         err = prestera_hw_switch_ageing_set(sw, PRESTERA_DEFAULT_AGEING_TIME_MS);
1179         if (err)
1180                 goto err_ageing_set;
1181
1182         return 0;
1183
1184 err_ageing_set:
1185         prestera_hw_event_handler_unregister(sw, PRESTERA_EVENT_TYPE_FDB,
1186                                              prestera_fdb_event);
1187         return err;
1188 }
1189
1190 static void prestera_fdb_fini(struct prestera_switch *sw)
1191 {
1192         prestera_hw_event_handler_unregister(sw, PRESTERA_EVENT_TYPE_FDB,
1193                                              prestera_fdb_event);
1194 }
1195
1196 static int prestera_switchdev_handler_init(struct prestera_switchdev *swdev)
1197 {
1198         int err;
1199
1200         swdev->swdev_nb.notifier_call = prestera_switchdev_event;
1201         err = register_switchdev_notifier(&swdev->swdev_nb);
1202         if (err)
1203                 goto err_register_swdev_notifier;
1204
1205         swdev->swdev_nb_blk.notifier_call = prestera_switchdev_blk_event;
1206         err = register_switchdev_blocking_notifier(&swdev->swdev_nb_blk);
1207         if (err)
1208                 goto err_register_blk_swdev_notifier;
1209
1210         return 0;
1211
1212 err_register_blk_swdev_notifier:
1213         unregister_switchdev_notifier(&swdev->swdev_nb);
1214 err_register_swdev_notifier:
1215         destroy_workqueue(swdev_wq);
1216         return err;
1217 }
1218
1219 static void prestera_switchdev_handler_fini(struct prestera_switchdev *swdev)
1220 {
1221         unregister_switchdev_blocking_notifier(&swdev->swdev_nb_blk);
1222         unregister_switchdev_notifier(&swdev->swdev_nb);
1223 }
1224
1225 int prestera_switchdev_init(struct prestera_switch *sw)
1226 {
1227         struct prestera_switchdev *swdev;
1228         int err;
1229
1230         swdev = kzalloc(sizeof(*swdev), GFP_KERNEL);
1231         if (!swdev)
1232                 return -ENOMEM;
1233
1234         sw->swdev = swdev;
1235         swdev->sw = sw;
1236
1237         INIT_LIST_HEAD(&swdev->bridge_list);
1238
1239         swdev_wq = alloc_ordered_workqueue("%s_ordered", 0, "prestera_br");
1240         if (!swdev_wq) {
1241                 err = -ENOMEM;
1242                 goto err_alloc_wq;
1243         }
1244
1245         err = prestera_switchdev_handler_init(swdev);
1246         if (err)
1247                 goto err_swdev_init;
1248
1249         err = prestera_fdb_init(sw);
1250         if (err)
1251                 goto err_fdb_init;
1252
1253         return 0;
1254
1255 err_fdb_init:
1256 err_swdev_init:
1257         destroy_workqueue(swdev_wq);
1258 err_alloc_wq:
1259         kfree(swdev);
1260
1261         return err;
1262 }
1263
1264 void prestera_switchdev_fini(struct prestera_switch *sw)
1265 {
1266         struct prestera_switchdev *swdev = sw->swdev;
1267
1268         prestera_fdb_fini(sw);
1269         prestera_switchdev_handler_fini(swdev);
1270         destroy_workqueue(swdev_wq);
1271         kfree(swdev);
1272 }