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                               struct netlink_ext_ack *extack)
 485 {
 486         struct prestera_switchdev *swdev = port->sw->swdev;
 487         struct prestera_bridge_port *br_port;
 488         struct prestera_bridge *bridge;
 489         int err;
 490
 491         bridge = prestera_bridge_by_dev(swdev, br_dev);
 492         if (!bridge) {
 493                 bridge = prestera_bridge_create(swdev, br_dev);
 494                 if (IS_ERR(bridge))
 495                         return PTR_ERR(bridge);
 496         }
 497
 498         br_port = prestera_bridge_port_add(bridge, port->dev);
 499         if (IS_ERR(br_port)) {
 500                 err = PTR_ERR(br_port);
 501                 goto err_brport_create;
 502         }
 503
 504         err = switchdev_bridge_port_offload(br_port->dev, port->dev, NULL,
 505                                             NULL, NULL, false, extack);
 506         if (err)
 507                 goto err_switchdev_offload;
 508
 509         if (bridge->vlan_enabled)
 510                 return 0;
 511
 512         err = prestera_bridge_1d_port_join(br_port);
 513         if (err)
 514                 goto err_port_join;
 515
 516         return 0;
 517
 518 err_port_join:
 519         switchdev_bridge_port_unoffload(br_port->dev, NULL, NULL, NULL);
 520 err_switchdev_offload:
 521         prestera_bridge_port_put(br_port);
 522 err_brport_create:
 523         prestera_bridge_put(bridge);
 524         return err;
 525 }
 526
 527 static void prestera_bridge_1q_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_port_pvid_set(port, PRESTERA_DEFAULT_VID);
 533 }
 534
 535 static void prestera_bridge_1d_port_leave(struct prestera_bridge_port *br_port)
 536 {
 537         struct prestera_port *port = netdev_priv(br_port->dev);
 538
 539         prestera_hw_fdb_flush_port(port, PRESTERA_FDB_FLUSH_MODE_ALL);
 540         prestera_hw_bridge_port_delete(port, br_port->bridge->bridge_id);
 541 }
 542
 543 static int prestera_port_vid_stp_set(struct prestera_port *port, u16 vid,
 544                                      u8 state)
 545 {
 546         u8 hw_state = state;
 547
 548         switch (state) {
 549         case BR_STATE_DISABLED:
 550                 hw_state = PRESTERA_STP_DISABLED;
 551                 break;
 552
 553         case BR_STATE_BLOCKING:
 554         case BR_STATE_LISTENING:
 555                 hw_state = PRESTERA_STP_BLOCK_LISTEN;
 556                 break;
 557
 558         case BR_STATE_LEARNING:
 559                 hw_state = PRESTERA_STP_LEARN;
 560                 break;
 561
 562         case BR_STATE_FORWARDING:
 563                 hw_state = PRESTERA_STP_FORWARD;
 564                 break;
 565
 566         default:
 567                 return -EINVAL;
 568         }
 569
 570         return prestera_hw_vlan_port_stp_set(port, vid, hw_state);
 571 }
 572
 573 void prestera_bridge_port_leave(struct net_device *br_dev,
 574                                 struct prestera_port *port)
 575 {
 576         struct prestera_switchdev *swdev = port->sw->swdev;
 577         struct prestera_bridge_port *br_port;
 578         struct prestera_bridge *bridge;
 579
 580         bridge = prestera_bridge_by_dev(swdev, br_dev);
 581         if (!bridge)
 582                 return;
 583
 584         br_port = __prestera_bridge_port_by_dev(bridge, port->dev);
 585         if (!br_port)
 586                 return;
 587
 588         bridge = br_port->bridge;
 589
 590         if (bridge->vlan_enabled)
 591                 prestera_bridge_1q_port_leave(br_port);
 592         else
 593                 prestera_bridge_1d_port_leave(br_port);
 594
 595         switchdev_bridge_port_unoffload(br_port->dev, NULL, NULL, NULL);
 596
 597         prestera_hw_port_learning_set(port, false);
 598         prestera_hw_port_flood_set(port, BR_FLOOD | BR_MCAST_FLOOD, 0);
 599         prestera_port_vid_stp_set(port, PRESTERA_VID_ALL, BR_STATE_FORWARDING);
 600         prestera_bridge_port_put(br_port);
 601 }
 602
 603 static int prestera_port_attr_br_flags_set(struct prestera_port *port,
 604                                            struct net_device *dev,
 605                                            struct switchdev_brport_flags flags)
 606 {
 607         struct prestera_bridge_port *br_port;
 608         int err;
 609
 610         br_port = prestera_bridge_port_by_dev(port->sw->swdev, dev);
 611         if (!br_port)
 612                 return 0;
 613
 614         err = prestera_hw_port_flood_set(port, flags.mask, flags.val);
 615         if (err)
 616                 return err;
 617
 618         if (flags.mask & BR_LEARNING) {
 619                 err = prestera_hw_port_learning_set(port,
 620                                                     flags.val & BR_LEARNING);
 621                 if (err)
 622                         return err;
 623         }
 624
 625         memcpy(&br_port->flags, &flags.val, sizeof(flags.val));
 626
 627         return 0;
 628 }
 629
 630 static int prestera_port_attr_br_ageing_set(struct prestera_port *port,
 631                                             unsigned long ageing_clock_t)
 632 {
 633         unsigned long ageing_jiffies = clock_t_to_jiffies(ageing_clock_t);
 634         u32 ageing_time_ms = jiffies_to_msecs(ageing_jiffies);
 635         struct prestera_switch *sw = port->sw;
 636
 637         if (ageing_time_ms < PRESTERA_MIN_AGEING_TIME_MS ||
 638             ageing_time_ms > PRESTERA_MAX_AGEING_TIME_MS)
 639                 return -ERANGE;
 640
 641         return prestera_hw_switch_ageing_set(sw, ageing_time_ms);
 642 }
 643
 644 static int prestera_port_attr_br_vlan_set(struct prestera_port *port,
 645                                           struct net_device *dev,
 646                                           bool vlan_enabled)
 647 {
 648         struct prestera_switch *sw = port->sw;
 649         struct prestera_bridge *bridge;
 650
 651         bridge = prestera_bridge_by_dev(sw->swdev, dev);
 652         if (WARN_ON(!bridge))
 653                 return -EINVAL;
 654
 655         if (bridge->vlan_enabled == vlan_enabled)
 656                 return 0;
 657
 658         netdev_err(bridge->dev, "VLAN filtering can't be changed for existing bridge\n");
 659
 660         return -EINVAL;
 661 }
 662
 663 static int prestera_port_bridge_vlan_stp_set(struct prestera_port *port,
 664                                              struct prestera_bridge_vlan *br_vlan,
 665                                              u8 state)
 666 {
 667         struct prestera_port_vlan *port_vlan;
 668
 669         list_for_each_entry(port_vlan, &br_vlan->port_vlan_list, br_vlan_head) {
 670                 if (port_vlan->port != port)
 671                         continue;
 672
 673                 return prestera_port_vid_stp_set(port, br_vlan->vid, state);
 674         }
 675
 676         return 0;
 677 }
 678
 679 static int prestera_port_attr_stp_state_set(struct prestera_port *port,
 680                                             struct net_device *dev,
 681                                             u8 state)
 682 {
 683         struct prestera_bridge_port *br_port;
 684         struct prestera_bridge_vlan *br_vlan;
 685         int err;
 686         u16 vid;
 687
 688         br_port = prestera_bridge_port_by_dev(port->sw->swdev, dev);
 689         if (!br_port)
 690                 return 0;
 691
 692         if (!br_port->bridge->vlan_enabled) {
 693                 vid = br_port->bridge->bridge_id;
 694                 err = prestera_port_vid_stp_set(port, vid, state);
 695                 if (err)
 696                         goto err_port_stp_set;
 697         } else {
 698                 list_for_each_entry(br_vlan, &br_port->vlan_list, head) {
 699                         err = prestera_port_bridge_vlan_stp_set(port, br_vlan,
 700                                                                 state);
 701                         if (err)
 702                                 goto err_port_vlan_stp_set;
 703                 }
 704         }
 705
 706         br_port->stp_state = state;
 707
 708         return 0;
 709
 710 err_port_vlan_stp_set:
 711         list_for_each_entry_continue_reverse(br_vlan, &br_port->vlan_list, head)
 712                 prestera_port_bridge_vlan_stp_set(port, br_vlan, br_port->stp_state);
 713         return err;
 714
 715 err_port_stp_set:
 716         prestera_port_vid_stp_set(port, vid, br_port->stp_state);
 717
 718         return err;
 719 }
 720
 721 static int prestera_port_obj_attr_set(struct net_device *dev, const void *ctx,
 722                                       const struct switchdev_attr *attr,
 723                                       struct netlink_ext_ack *extack)
 724 {
 725         struct prestera_port *port = netdev_priv(dev);
 726         int err = 0;
 727
 728         switch (attr->id) {
 729         case SWITCHDEV_ATTR_ID_PORT_STP_STATE:
 730                 err = prestera_port_attr_stp_state_set(port, attr->orig_dev,
 731                                                        attr->u.stp_state);
 732                 break;
 733         case SWITCHDEV_ATTR_ID_PORT_PRE_BRIDGE_FLAGS:
 734                 if (attr->u.brport_flags.mask &
 735                     ~(BR_LEARNING | BR_FLOOD | BR_MCAST_FLOOD))
 736                         err = -EINVAL;
 737                 break;
 738         case SWITCHDEV_ATTR_ID_PORT_BRIDGE_FLAGS:
 739                 err = prestera_port_attr_br_flags_set(port, attr->orig_dev,
 740                                                       attr->u.brport_flags);
 741                 break;
 742         case SWITCHDEV_ATTR_ID_BRIDGE_AGEING_TIME:
 743                 err = prestera_port_attr_br_ageing_set(port,
 744                                                        attr->u.ageing_time);
 745                 break;
 746         case SWITCHDEV_ATTR_ID_BRIDGE_VLAN_FILTERING:
 747                 err = prestera_port_attr_br_vlan_set(port, attr->orig_dev,
 748                                                      attr->u.vlan_filtering);
 749                 break;
 750         default:
 751                 err = -EOPNOTSUPP;
 752         }
 753
 754         return err;
 755 }
 756
 757 static void
 758 prestera_fdb_offload_notify(struct prestera_port *port,
 759                             struct switchdev_notifier_fdb_info *info)
 760 {
 761         struct switchdev_notifier_fdb_info send_info = {};
 762
 763         send_info.addr = info->addr;
 764         send_info.vid = info->vid;
 765         send_info.offloaded = true;
 766
 767         call_switchdev_notifiers(SWITCHDEV_FDB_OFFLOADED, port->dev,
 768                                  &send_info.info, NULL);
 769 }
 770
 771 static int prestera_port_fdb_set(struct prestera_port *port,
 772                                  struct switchdev_notifier_fdb_info *fdb_info,
 773                                  bool adding)
 774 {
 775         struct prestera_switch *sw = port->sw;
 776         struct prestera_bridge_port *br_port;
 777         struct prestera_bridge *bridge;
 778         int err;
 779         u16 vid;
 780
 781         br_port = prestera_bridge_port_by_dev(sw->swdev, port->dev);
 782         if (!br_port)
 783                 return -EINVAL;
 784
 785         bridge = br_port->bridge;
 786
 787         if (bridge->vlan_enabled)
 788                 vid = fdb_info->vid;
 789         else
 790                 vid = bridge->bridge_id;
 791
 792         if (adding)
 793                 err = prestera_fdb_add(port, fdb_info->addr, vid, false);
 794         else
 795                 err = prestera_fdb_del(port, fdb_info->addr, vid);
 796
 797         return err;
 798 }
 799
 800 static void prestera_fdb_event_work(struct work_struct *work)
 801 {
 802         struct switchdev_notifier_fdb_info *fdb_info;
 803         struct prestera_fdb_event_work *swdev_work;
 804         struct prestera_port *port;
 805         struct net_device *dev;
 806         int err;
 807
 808         swdev_work = container_of(work, struct prestera_fdb_event_work, work);
 809         dev = swdev_work->dev;
 810
 811         rtnl_lock();
 812
 813         port = prestera_port_dev_lower_find(dev);
 814         if (!port)
 815                 goto out_unlock;
 816
 817         switch (swdev_work->event) {
 818         case SWITCHDEV_FDB_ADD_TO_DEVICE:
 819                 fdb_info = &swdev_work->fdb_info;
 820                 if (!fdb_info->added_by_user || fdb_info->is_local)
 821                         break;
 822
 823                 err = prestera_port_fdb_set(port, fdb_info, true);
 824                 if (err)
 825                         break;
 826
 827                 prestera_fdb_offload_notify(port, fdb_info);
 828                 break;
 829
 830         case SWITCHDEV_FDB_DEL_TO_DEVICE:
 831                 fdb_info = &swdev_work->fdb_info;
 832                 prestera_port_fdb_set(port, fdb_info, false);
 833                 break;
 834         }
 835
 836 out_unlock:
 837         rtnl_unlock();
 838
 839         kfree(swdev_work->fdb_info.addr);
 840         kfree(swdev_work);
 841         dev_put(dev);
 842 }
 843
 844 static int prestera_switchdev_event(struct notifier_block *unused,
 845                                     unsigned long event, void *ptr)
 846 {
 847         struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
 848         struct switchdev_notifier_fdb_info *fdb_info;
 849         struct switchdev_notifier_info *info = ptr;
 850         struct prestera_fdb_event_work *swdev_work;
 851         struct net_device *upper;
 852         int err;
 853
 854         if (event == SWITCHDEV_PORT_ATTR_SET) {
 855                 err = switchdev_handle_port_attr_set(dev, ptr,
 856                                                      prestera_netdev_check,
 857                                                      prestera_port_obj_attr_set);
 858                 return notifier_from_errno(err);
 859         }
 860
 861         if (!prestera_netdev_check(dev))
 862                 return NOTIFY_DONE;
 863
 864         upper = netdev_master_upper_dev_get_rcu(dev);
 865         if (!upper)
 866                 return NOTIFY_DONE;
 867
 868         if (!netif_is_bridge_master(upper))
 869                 return NOTIFY_DONE;
 870
 871         swdev_work = kzalloc(sizeof(*swdev_work), GFP_ATOMIC);
 872         if (!swdev_work)
 873                 return NOTIFY_BAD;
 874
 875         swdev_work->event = event;
 876         swdev_work->dev = dev;
 877
 878         switch (event) {
 879         case SWITCHDEV_FDB_ADD_TO_DEVICE:
 880         case SWITCHDEV_FDB_DEL_TO_DEVICE:
 881                 fdb_info = container_of(info,
 882                                         struct switchdev_notifier_fdb_info,
 883                                         info);
 884
 885                 INIT_WORK(&swdev_work->work, prestera_fdb_event_work);
 886                 memcpy(&swdev_work->fdb_info, ptr,
 887                        sizeof(swdev_work->fdb_info));
 888
 889                 swdev_work->fdb_info.addr = kzalloc(ETH_ALEN, GFP_ATOMIC);
 890                 if (!swdev_work->fdb_info.addr)
 891                         goto out_bad;
 892
 893                 ether_addr_copy((u8 *)swdev_work->fdb_info.addr,
 894                                 fdb_info->addr);
 895                 dev_hold(dev);
 896                 break;
 897
 898         default:
 899                 kfree(swdev_work);
 900                 return NOTIFY_DONE;
 901         }
 902
 903         queue_work(swdev_wq, &swdev_work->work);
 904         return NOTIFY_DONE;
 905
 906 out_bad:
 907         kfree(swdev_work);
 908         return NOTIFY_BAD;
 909 }
 910
 911 static int
 912 prestera_port_vlan_bridge_join(struct prestera_port_vlan *port_vlan,
 913                                struct prestera_bridge_port *br_port)
 914 {
 915         struct prestera_port *port = port_vlan->port;
 916         struct prestera_bridge_vlan *br_vlan;
 917         u16 vid = port_vlan->vid;
 918         int err;
 919
 920         if (port_vlan->br_port)
 921                 return 0;
 922
 923         err = prestera_hw_port_flood_set(port, BR_FLOOD | BR_MCAST_FLOOD,
 924                                          br_port->flags);
 925         if (err)
 926                 return err;
 927
 928         err = prestera_hw_port_learning_set(port, br_port->flags & BR_LEARNING);
 929         if (err)
 930                 goto err_port_learning_set;
 931
 932         err = prestera_port_vid_stp_set(port, vid, br_port->stp_state);
 933         if (err)
 934                 goto err_port_vid_stp_set;
 935
 936         br_vlan = prestera_bridge_vlan_by_vid(br_port, vid);
 937         if (!br_vlan) {
 938                 br_vlan = prestera_bridge_vlan_create(br_port, vid);
 939                 if (!br_vlan) {
 940                         err = -ENOMEM;
 941                         goto err_bridge_vlan_get;
 942                 }
 943         }
 944
 945         list_add(&port_vlan->br_vlan_head, &br_vlan->port_vlan_list);
 946
 947         prestera_bridge_port_get(br_port);
 948         port_vlan->br_port = br_port;
 949
 950         return 0;
 951
 952 err_bridge_vlan_get:
 953         prestera_port_vid_stp_set(port, vid, BR_STATE_FORWARDING);
 954 err_port_vid_stp_set:
 955         prestera_hw_port_learning_set(port, false);
 956 err_port_learning_set:
 957         return err;
 958 }
 959
 960 static int
 961 prestera_bridge_port_vlan_add(struct prestera_port *port,
 962                               struct prestera_bridge_port *br_port,
 963                               u16 vid, bool is_untagged, bool is_pvid,
 964                               struct netlink_ext_ack *extack)
 965 {
 966         struct prestera_port_vlan *port_vlan;
 967         u16 old_pvid = port->pvid;
 968         u16 pvid;
 969         int err;
 970
 971         if (is_pvid)
 972                 pvid = vid;
 973         else
 974                 pvid = port->pvid == vid ? 0 : port->pvid;
 975
 976         port_vlan = prestera_port_vlan_by_vid(port, vid);
 977         if (port_vlan && port_vlan->br_port != br_port)
 978                 return -EEXIST;
 979
 980         if (!port_vlan) {
 981                 port_vlan = prestera_port_vlan_create(port, vid, is_untagged);
 982                 if (IS_ERR(port_vlan))
 983                         return PTR_ERR(port_vlan);
 984         } else {
 985                 err = prestera_hw_vlan_port_set(port, vid, true, is_untagged);
 986                 if (err)
 987                         goto err_port_vlan_set;
 988         }
 989
 990         err = prestera_port_pvid_set(port, pvid);
 991         if (err)
 992                 goto err_port_pvid_set;
 993
 994         err = prestera_port_vlan_bridge_join(port_vlan, br_port);
 995         if (err)
 996                 goto err_port_vlan_bridge_join;
 997
 998         return 0;
 999
1000 err_port_vlan_bridge_join:
1001         prestera_port_pvid_set(port, old_pvid);
1002 err_port_pvid_set:
1003         prestera_hw_vlan_port_set(port, vid, false, false);
1004 err_port_vlan_set:
1005         prestera_port_vlan_destroy(port_vlan);
1006
1007         return err;
1008 }
1009
1010 static void
1011 prestera_bridge_port_vlan_del(struct prestera_port *port,
1012                               struct prestera_bridge_port *br_port, u16 vid)
1013 {
1014         u16 pvid = port->pvid == vid ? 0 : port->pvid;
1015         struct prestera_port_vlan *port_vlan;
1016
1017         port_vlan = prestera_port_vlan_by_vid(port, vid);
1018         if (WARN_ON(!port_vlan))
1019                 return;
1020
1021         prestera_port_vlan_bridge_leave(port_vlan);
1022         prestera_port_pvid_set(port, pvid);
1023         prestera_port_vlan_destroy(port_vlan);
1024 }
1025
1026 static int prestera_port_vlans_add(struct prestera_port *port,
1027                                    const struct switchdev_obj_port_vlan *vlan,
1028                                    struct netlink_ext_ack *extack)
1029 {
1030         bool flag_untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
1031         bool flag_pvid = vlan->flags & BRIDGE_VLAN_INFO_PVID;
1032         struct net_device *orig_dev = vlan->obj.orig_dev;
1033         struct prestera_bridge_port *br_port;
1034         struct prestera_switch *sw = port->sw;
1035         struct prestera_bridge *bridge;
1036
1037         if (netif_is_bridge_master(orig_dev))
1038                 return 0;
1039
1040         br_port = prestera_bridge_port_by_dev(sw->swdev, port->dev);
1041         if (WARN_ON(!br_port))
1042                 return -EINVAL;
1043
1044         bridge = br_port->bridge;
1045         if (!bridge->vlan_enabled)
1046                 return 0;
1047
1048         return prestera_bridge_port_vlan_add(port, br_port,
1049                                              vlan->vid, flag_untagged,
1050                                              flag_pvid, extack);
1051 }
1052
1053 static int prestera_port_obj_add(struct net_device *dev, const void *ctx,
1054                                  const struct switchdev_obj *obj,
1055                                  struct netlink_ext_ack *extack)
1056 {
1057         struct prestera_port *port = netdev_priv(dev);
1058         const struct switchdev_obj_port_vlan *vlan;
1059
1060         switch (obj->id) {
1061         case SWITCHDEV_OBJ_ID_PORT_VLAN:
1062                 vlan = SWITCHDEV_OBJ_PORT_VLAN(obj);
1063                 return prestera_port_vlans_add(port, vlan, extack);
1064         default:
1065                 return -EOPNOTSUPP;
1066         }
1067 }
1068
1069 static int prestera_port_vlans_del(struct prestera_port *port,
1070                                    const struct switchdev_obj_port_vlan *vlan)
1071 {
1072         struct net_device *orig_dev = vlan->obj.orig_dev;
1073         struct prestera_bridge_port *br_port;
1074         struct prestera_switch *sw = port->sw;
1075
1076         if (netif_is_bridge_master(orig_dev))
1077                 return -EOPNOTSUPP;
1078
1079         br_port = prestera_bridge_port_by_dev(sw->swdev, port->dev);
1080         if (WARN_ON(!br_port))
1081                 return -EINVAL;
1082
1083         if (!br_port->bridge->vlan_enabled)
1084                 return 0;
1085
1086         prestera_bridge_port_vlan_del(port, br_port, vlan->vid);
1087
1088         return 0;
1089 }
1090
1091 static int prestera_port_obj_del(struct net_device *dev, const void *ctx,
1092                                  const struct switchdev_obj *obj)
1093 {
1094         struct prestera_port *port = netdev_priv(dev);
1095
1096         switch (obj->id) {
1097         case SWITCHDEV_OBJ_ID_PORT_VLAN:
1098                 return prestera_port_vlans_del(port, SWITCHDEV_OBJ_PORT_VLAN(obj));
1099         default:
1100                 return -EOPNOTSUPP;
1101         }
1102 }
1103
1104 static int prestera_switchdev_blk_event(struct notifier_block *unused,
1105                                         unsigned long event, void *ptr)
1106 {
1107         struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
1108         int err;
1109
1110         switch (event) {
1111         case SWITCHDEV_PORT_OBJ_ADD:
1112                 err = switchdev_handle_port_obj_add(dev, ptr,
1113                                                     prestera_netdev_check,
1114                                                     prestera_port_obj_add);
1115                 break;
1116         case SWITCHDEV_PORT_OBJ_DEL:
1117                 err = switchdev_handle_port_obj_del(dev, ptr,
1118                                                     prestera_netdev_check,
1119                                                     prestera_port_obj_del);
1120                 break;
1121         case SWITCHDEV_PORT_ATTR_SET:
1122                 err = switchdev_handle_port_attr_set(dev, ptr,
1123                                                      prestera_netdev_check,
1124                                                      prestera_port_obj_attr_set);
1125                 break;
1126         default:
1127                 err = -EOPNOTSUPP;
1128         }
1129
1130         return notifier_from_errno(err);
1131 }
1132
1133 static void prestera_fdb_event(struct prestera_switch *sw,
1134                                struct prestera_event *evt, void *arg)
1135 {
1136         struct switchdev_notifier_fdb_info info = {};
1137         struct net_device *dev = NULL;
1138         struct prestera_port *port;
1139         struct prestera_lag *lag;
1140
1141         switch (evt->fdb_evt.type) {
1142         case PRESTERA_FDB_ENTRY_TYPE_REG_PORT:
1143                 port = prestera_find_port(sw, evt->fdb_evt.dest.port_id);
1144                 if (port)
1145                         dev = port->dev;
1146                 break;
1147         case PRESTERA_FDB_ENTRY_TYPE_LAG:
1148                 lag = prestera_lag_by_id(sw, evt->fdb_evt.dest.lag_id);
1149                 if (lag)
1150                         dev = lag->dev;
1151                 break;
1152         default:
1153                 return;
1154         }
1155
1156         if (!dev)
1157                 return;
1158
1159         info.addr = evt->fdb_evt.data.mac;
1160         info.vid = evt->fdb_evt.vid;
1161         info.offloaded = true;
1162
1163         rtnl_lock();
1164
1165         switch (evt->id) {
1166         case PRESTERA_FDB_EVENT_LEARNED:
1167                 call_switchdev_notifiers(SWITCHDEV_FDB_ADD_TO_BRIDGE,
1168                                          dev, &info.info, NULL);
1169                 break;
1170         case PRESTERA_FDB_EVENT_AGED:
1171                 call_switchdev_notifiers(SWITCHDEV_FDB_DEL_TO_BRIDGE,
1172                                          dev, &info.info, NULL);
1173                 break;
1174         }
1175
1176         rtnl_unlock();
1177 }
1178
1179 static int prestera_fdb_init(struct prestera_switch *sw)
1180 {
1181         int err;
1182
1183         err = prestera_hw_event_handler_register(sw, PRESTERA_EVENT_TYPE_FDB,
1184                                                  prestera_fdb_event, NULL);
1185         if (err)
1186                 return err;
1187
1188         err = prestera_hw_switch_ageing_set(sw, PRESTERA_DEFAULT_AGEING_TIME_MS);
1189         if (err)
1190                 goto err_ageing_set;
1191
1192         return 0;
1193
1194 err_ageing_set:
1195         prestera_hw_event_handler_unregister(sw, PRESTERA_EVENT_TYPE_FDB,
1196                                              prestera_fdb_event);
1197         return err;
1198 }
1199
1200 static void prestera_fdb_fini(struct prestera_switch *sw)
1201 {
1202         prestera_hw_event_handler_unregister(sw, PRESTERA_EVENT_TYPE_FDB,
1203                                              prestera_fdb_event);
1204 }
1205
1206 static int prestera_switchdev_handler_init(struct prestera_switchdev *swdev)
1207 {
1208         int err;
1209
1210         swdev->swdev_nb.notifier_call = prestera_switchdev_event;
1211         err = register_switchdev_notifier(&swdev->swdev_nb);
1212         if (err)
1213                 goto err_register_swdev_notifier;
1214
1215         swdev->swdev_nb_blk.notifier_call = prestera_switchdev_blk_event;
1216         err = register_switchdev_blocking_notifier(&swdev->swdev_nb_blk);
1217         if (err)
1218                 goto err_register_blk_swdev_notifier;
1219
1220         return 0;
1221
1222 err_register_blk_swdev_notifier:
1223         unregister_switchdev_notifier(&swdev->swdev_nb);
1224 err_register_swdev_notifier:
1225         destroy_workqueue(swdev_wq);
1226         return err;
1227 }
1228
1229 static void prestera_switchdev_handler_fini(struct prestera_switchdev *swdev)
1230 {
1231         unregister_switchdev_blocking_notifier(&swdev->swdev_nb_blk);
1232         unregister_switchdev_notifier(&swdev->swdev_nb);
1233 }
1234
1235 int prestera_switchdev_init(struct prestera_switch *sw)
1236 {
1237         struct prestera_switchdev *swdev;
1238         int err;
1239
1240         swdev = kzalloc(sizeof(*swdev), GFP_KERNEL);
1241         if (!swdev)
1242                 return -ENOMEM;
1243
1244         sw->swdev = swdev;
1245         swdev->sw = sw;
1246
1247         INIT_LIST_HEAD(&swdev->bridge_list);
1248
1249         swdev_wq = alloc_ordered_workqueue("%s_ordered", 0, "prestera_br");
1250         if (!swdev_wq) {
1251                 err = -ENOMEM;
1252                 goto err_alloc_wq;
1253         }
1254
1255         err = prestera_switchdev_handler_init(swdev);
1256         if (err)
1257                 goto err_swdev_init;
1258
1259         err = prestera_fdb_init(sw);
1260         if (err)
1261                 goto err_fdb_init;
1262
1263         return 0;
1264
1265 err_fdb_init:
1266 err_swdev_init:
1267         destroy_workqueue(swdev_wq);
1268 err_alloc_wq:
1269         kfree(swdev);
1270
1271         return err;
1272 }
1273
1274 void prestera_switchdev_fini(struct prestera_switch *sw)
1275 {
1276         struct prestera_switchdev *swdev = sw->swdev;
1277
1278         prestera_fdb_fini(sw);
1279         prestera_switchdev_handler_fini(swdev);
1280         destroy_workqueue(swdev_wq);
1281         kfree(swdev);
1282 }