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