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