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