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 #include <linux/phylink.h>
13 
14 #include "prestera.h"
15 #include "prestera_hw.h"
16 #include "prestera_acl.h"
17 #include "prestera_flow.h"
18 #include "prestera_span.h"
19 #include "prestera_rxtx.h"
20 #include "prestera_devlink.h"
21 #include "prestera_ethtool.h"
22 #include "prestera_counter.h"
23 #include "prestera_switchdev.h"
24 
25 #define PRESTERA_MTU_DEFAULT	1536
26 
27 #define PRESTERA_STATS_DELAY_MS	1000
28 
29 #define PRESTERA_MAC_ADDR_NUM_MAX	255
30 
31 static struct workqueue_struct *prestera_wq;
32 static struct workqueue_struct *prestera_owq;
33 
34 void prestera_queue_work(struct work_struct *work)
35 {
36 	queue_work(prestera_owq, work);
37 }
38 
39 int prestera_port_learning_set(struct prestera_port *port, bool learn)
40 {
41 	return prestera_hw_port_learning_set(port, learn);
42 }
43 
44 int prestera_port_uc_flood_set(struct prestera_port *port, bool flood)
45 {
46 	return prestera_hw_port_uc_flood_set(port, flood);
47 }
48 
49 int prestera_port_mc_flood_set(struct prestera_port *port, bool flood)
50 {
51 	return prestera_hw_port_mc_flood_set(port, flood);
52 }
53 
54 int prestera_port_pvid_set(struct prestera_port *port, u16 vid)
55 {
56 	enum prestera_accept_frm_type frm_type;
57 	int err;
58 
59 	frm_type = PRESTERA_ACCEPT_FRAME_TYPE_TAGGED;
60 
61 	if (vid) {
62 		err = prestera_hw_vlan_port_vid_set(port, vid);
63 		if (err)
64 			return err;
65 
66 		frm_type = PRESTERA_ACCEPT_FRAME_TYPE_ALL;
67 	}
68 
69 	err = prestera_hw_port_accept_frm_type(port, frm_type);
70 	if (err && frm_type == PRESTERA_ACCEPT_FRAME_TYPE_ALL)
71 		prestera_hw_vlan_port_vid_set(port, port->pvid);
72 
73 	port->pvid = vid;
74 	return 0;
75 }
76 
77 struct prestera_port *prestera_port_find_by_hwid(struct prestera_switch *sw,
78 						 u32 dev_id, u32 hw_id)
79 {
80 	struct prestera_port *port = NULL, *tmp;
81 
82 	read_lock(&sw->port_list_lock);
83 	list_for_each_entry(tmp, &sw->port_list, list) {
84 		if (tmp->dev_id == dev_id && tmp->hw_id == hw_id) {
85 			port = tmp;
86 			break;
87 		}
88 	}
89 	read_unlock(&sw->port_list_lock);
90 
91 	return port;
92 }
93 
94 struct prestera_port *prestera_find_port(struct prestera_switch *sw, u32 id)
95 {
96 	struct prestera_port *port = NULL, *tmp;
97 
98 	read_lock(&sw->port_list_lock);
99 	list_for_each_entry(tmp, &sw->port_list, list) {
100 		if (tmp->id == id) {
101 			port = tmp;
102 			break;
103 		}
104 	}
105 	read_unlock(&sw->port_list_lock);
106 
107 	return port;
108 }
109 
110 struct prestera_switch *prestera_switch_get(struct net_device *dev)
111 {
112 	struct prestera_port *port;
113 
114 	port = prestera_port_dev_lower_find(dev);
115 	return port ? port->sw : NULL;
116 }
117 
118 int prestera_port_cfg_mac_read(struct prestera_port *port,
119 			       struct prestera_port_mac_config *cfg)
120 {
121 	*cfg = port->cfg_mac;
122 	return 0;
123 }
124 
125 int prestera_port_cfg_mac_write(struct prestera_port *port,
126 				struct prestera_port_mac_config *cfg)
127 {
128 	int err;
129 
130 	err = prestera_hw_port_mac_mode_set(port, cfg->admin,
131 					    cfg->mode, cfg->inband, cfg->speed,
132 					    cfg->duplex, cfg->fec);
133 	if (err)
134 		return err;
135 
136 	port->cfg_mac = *cfg;
137 	return 0;
138 }
139 
140 static int prestera_port_open(struct net_device *dev)
141 {
142 	struct prestera_port *port = netdev_priv(dev);
143 	struct prestera_port_mac_config cfg_mac;
144 	int err = 0;
145 
146 	if (port->phy_link) {
147 		phylink_start(port->phy_link);
148 	} else {
149 		if (port->caps.transceiver == PRESTERA_PORT_TCVR_SFP) {
150 			err = prestera_port_cfg_mac_read(port, &cfg_mac);
151 			if (!err) {
152 				cfg_mac.admin = true;
153 				err = prestera_port_cfg_mac_write(port,
154 								  &cfg_mac);
155 			}
156 		} else {
157 			port->cfg_phy.admin = true;
158 			err = prestera_hw_port_phy_mode_set(port, true,
159 							    port->autoneg,
160 							    port->cfg_phy.mode,
161 							    port->adver_link_modes,
162 							    port->cfg_phy.mdix);
163 		}
164 	}
165 
166 	netif_start_queue(dev);
167 
168 	return err;
169 }
170 
171 static int prestera_port_close(struct net_device *dev)
172 {
173 	struct prestera_port *port = netdev_priv(dev);
174 	struct prestera_port_mac_config cfg_mac;
175 	int err = 0;
176 
177 	netif_stop_queue(dev);
178 
179 	if (port->phy_link) {
180 		phylink_stop(port->phy_link);
181 		phylink_disconnect_phy(port->phy_link);
182 		err = prestera_port_cfg_mac_read(port, &cfg_mac);
183 		if (!err) {
184 			cfg_mac.admin = false;
185 			prestera_port_cfg_mac_write(port, &cfg_mac);
186 		}
187 	} else {
188 		if (port->caps.transceiver == PRESTERA_PORT_TCVR_SFP) {
189 			err = prestera_port_cfg_mac_read(port, &cfg_mac);
190 			if (!err) {
191 				cfg_mac.admin = false;
192 				prestera_port_cfg_mac_write(port, &cfg_mac);
193 			}
194 		} else {
195 			port->cfg_phy.admin = false;
196 			err = prestera_hw_port_phy_mode_set(port, false, port->autoneg,
197 							    port->cfg_phy.mode,
198 							    port->adver_link_modes,
199 							    port->cfg_phy.mdix);
200 		}
201 	}
202 
203 	return err;
204 }
205 
206 static void
207 prestera_port_mac_state_cache_read(struct prestera_port *port,
208 				   struct prestera_port_mac_state *state)
209 {
210 	spin_lock(&port->state_mac_lock);
211 	*state = port->state_mac;
212 	spin_unlock(&port->state_mac_lock);
213 }
214 
215 static void
216 prestera_port_mac_state_cache_write(struct prestera_port *port,
217 				    struct prestera_port_mac_state *state)
218 {
219 	spin_lock(&port->state_mac_lock);
220 	port->state_mac = *state;
221 	spin_unlock(&port->state_mac_lock);
222 }
223 
224 static struct prestera_port *prestera_pcs_to_port(struct phylink_pcs *pcs)
225 {
226 	return container_of(pcs, struct prestera_port, phylink_pcs);
227 }
228 
229 static void prestera_mac_config(struct phylink_config *config,
230 				unsigned int an_mode,
231 				const struct phylink_link_state *state)
232 {
233 }
234 
235 static void prestera_mac_link_down(struct phylink_config *config,
236 				   unsigned int mode, phy_interface_t interface)
237 {
238 	struct net_device *ndev = to_net_dev(config->dev);
239 	struct prestera_port *port = netdev_priv(ndev);
240 	struct prestera_port_mac_state state_mac;
241 
242 	/* Invalidate. Parameters will update on next link event. */
243 	memset(&state_mac, 0, sizeof(state_mac));
244 	state_mac.valid = false;
245 	prestera_port_mac_state_cache_write(port, &state_mac);
246 }
247 
248 static void prestera_mac_link_up(struct phylink_config *config,
249 				 struct phy_device *phy,
250 				 unsigned int mode, phy_interface_t interface,
251 				 int speed, int duplex,
252 				 bool tx_pause, bool rx_pause)
253 {
254 }
255 
256 static struct phylink_pcs *
257 prestera_mac_select_pcs(struct phylink_config *config,
258 			phy_interface_t interface)
259 {
260 	struct net_device *dev = to_net_dev(config->dev);
261 	struct prestera_port *port = netdev_priv(dev);
262 
263 	return &port->phylink_pcs;
264 }
265 
266 static void prestera_pcs_get_state(struct phylink_pcs *pcs,
267 				   struct phylink_link_state *state)
268 {
269 	struct prestera_port *port = container_of(pcs, struct prestera_port,
270 						  phylink_pcs);
271 	struct prestera_port_mac_state smac;
272 
273 	prestera_port_mac_state_cache_read(port, &smac);
274 
275 	if (smac.valid) {
276 		state->link = smac.oper ? 1 : 0;
277 		/* AN is completed, when port is up */
278 		state->an_complete = (smac.oper && port->autoneg) ? 1 : 0;
279 		state->speed = smac.speed;
280 		state->duplex = smac.duplex;
281 	} else {
282 		state->link = 0;
283 		state->an_complete = 0;
284 	}
285 }
286 
287 static int prestera_pcs_config(struct phylink_pcs *pcs,
288 			       unsigned int mode,
289 			       phy_interface_t interface,
290 			       const unsigned long *advertising,
291 			       bool permit_pause_to_mac)
292 {
293 	struct prestera_port *port = prestera_pcs_to_port(pcs);
294 	struct prestera_port_mac_config cfg_mac;
295 	int err;
296 
297 	err = prestera_port_cfg_mac_read(port, &cfg_mac);
298 	if (err)
299 		return err;
300 
301 	cfg_mac.admin = true;
302 	cfg_mac.fec = PRESTERA_PORT_FEC_OFF;
303 
304 	switch (interface) {
305 	case PHY_INTERFACE_MODE_10GBASER:
306 		cfg_mac.speed = SPEED_10000;
307 		cfg_mac.inband = 0;
308 		cfg_mac.mode = PRESTERA_MAC_MODE_SR_LR;
309 		break;
310 	case PHY_INTERFACE_MODE_2500BASEX:
311 		cfg_mac.speed = SPEED_2500;
312 		cfg_mac.duplex = DUPLEX_FULL;
313 		cfg_mac.inband = test_bit(ETHTOOL_LINK_MODE_Autoneg_BIT,
314 					  advertising);
315 		cfg_mac.mode = PRESTERA_MAC_MODE_SGMII;
316 		break;
317 	case PHY_INTERFACE_MODE_SGMII:
318 		cfg_mac.inband = 1;
319 		cfg_mac.mode = PRESTERA_MAC_MODE_SGMII;
320 		break;
321 	case PHY_INTERFACE_MODE_1000BASEX:
322 	default:
323 		cfg_mac.speed = SPEED_1000;
324 		cfg_mac.duplex = DUPLEX_FULL;
325 		cfg_mac.inband = test_bit(ETHTOOL_LINK_MODE_Autoneg_BIT,
326 					  advertising);
327 		cfg_mac.mode = PRESTERA_MAC_MODE_1000BASE_X;
328 		break;
329 	}
330 
331 	err = prestera_port_cfg_mac_write(port, &cfg_mac);
332 	if (err)
333 		return err;
334 
335 	return 0;
336 }
337 
338 static void prestera_pcs_an_restart(struct phylink_pcs *pcs)
339 {
340 	/* TODO: add 1000basex AN restart support
341 	 * (Currently FW has no support for 1000baseX AN restart, but it will in the future,
342 	 * so as for now the function would stay empty.)
343 	 */
344 }
345 
346 static const struct phylink_mac_ops prestera_mac_ops = {
347 	.validate = phylink_generic_validate,
348 	.mac_select_pcs = prestera_mac_select_pcs,
349 	.mac_config = prestera_mac_config,
350 	.mac_link_down = prestera_mac_link_down,
351 	.mac_link_up = prestera_mac_link_up,
352 };
353 
354 static const struct phylink_pcs_ops prestera_pcs_ops = {
355 	.pcs_get_state = prestera_pcs_get_state,
356 	.pcs_config = prestera_pcs_config,
357 	.pcs_an_restart = prestera_pcs_an_restart,
358 };
359 
360 static int prestera_port_sfp_bind(struct prestera_port *port)
361 {
362 	struct prestera_switch *sw = port->sw;
363 	struct device_node *ports, *node;
364 	struct fwnode_handle *fwnode;
365 	struct phylink *phy_link;
366 	int err;
367 
368 	if (!sw->np)
369 		return 0;
370 
371 	ports = of_find_node_by_name(sw->np, "ports");
372 
373 	for_each_child_of_node(ports, node) {
374 		int num;
375 
376 		err = of_property_read_u32(node, "prestera,port-num", &num);
377 		if (err) {
378 			dev_err(sw->dev->dev,
379 				"device node %pOF has no valid reg property: %d\n",
380 				node, err);
381 			goto out;
382 		}
383 
384 		if (port->fp_id != num)
385 			continue;
386 
387 		port->phylink_pcs.ops = &prestera_pcs_ops;
388 
389 		port->phy_config.dev = &port->dev->dev;
390 		port->phy_config.type = PHYLINK_NETDEV;
391 
392 		fwnode = of_fwnode_handle(node);
393 
394 		__set_bit(PHY_INTERFACE_MODE_10GBASER,
395 			  port->phy_config.supported_interfaces);
396 		__set_bit(PHY_INTERFACE_MODE_2500BASEX,
397 			  port->phy_config.supported_interfaces);
398 		__set_bit(PHY_INTERFACE_MODE_SGMII,
399 			  port->phy_config.supported_interfaces);
400 		__set_bit(PHY_INTERFACE_MODE_1000BASEX,
401 			  port->phy_config.supported_interfaces);
402 
403 		port->phy_config.mac_capabilities =
404 			MAC_1000 | MAC_2500FD | MAC_10000FD;
405 
406 		phy_link = phylink_create(&port->phy_config, fwnode,
407 					  PHY_INTERFACE_MODE_INTERNAL,
408 					  &prestera_mac_ops);
409 		if (IS_ERR(phy_link)) {
410 			netdev_err(port->dev, "failed to create phylink\n");
411 			err = PTR_ERR(phy_link);
412 			goto out;
413 		}
414 
415 		port->phy_link = phy_link;
416 		break;
417 	}
418 
419 out:
420 	of_node_put(ports);
421 	return err;
422 }
423 
424 static int prestera_port_sfp_unbind(struct prestera_port *port)
425 {
426 	if (port->phy_link)
427 		phylink_destroy(port->phy_link);
428 
429 	return 0;
430 }
431 
432 static netdev_tx_t prestera_port_xmit(struct sk_buff *skb,
433 				      struct net_device *dev)
434 {
435 	return prestera_rxtx_xmit(netdev_priv(dev), skb);
436 }
437 
438 int prestera_is_valid_mac_addr(struct prestera_port *port, const u8 *addr)
439 {
440 	if (!is_valid_ether_addr(addr))
441 		return -EADDRNOTAVAIL;
442 
443 	/* firmware requires that port's MAC address contains first 5 bytes
444 	 * of the base MAC address
445 	 */
446 	if (memcmp(port->sw->base_mac, addr, ETH_ALEN - 1))
447 		return -EINVAL;
448 
449 	return 0;
450 }
451 
452 static int prestera_port_set_mac_address(struct net_device *dev, void *p)
453 {
454 	struct prestera_port *port = netdev_priv(dev);
455 	struct sockaddr *addr = p;
456 	int err;
457 
458 	err = prestera_is_valid_mac_addr(port, addr->sa_data);
459 	if (err)
460 		return err;
461 
462 	err = prestera_hw_port_mac_set(port, addr->sa_data);
463 	if (err)
464 		return err;
465 
466 	eth_hw_addr_set(dev, addr->sa_data);
467 
468 	return 0;
469 }
470 
471 static int prestera_port_change_mtu(struct net_device *dev, int mtu)
472 {
473 	struct prestera_port *port = netdev_priv(dev);
474 	int err;
475 
476 	err = prestera_hw_port_mtu_set(port, mtu);
477 	if (err)
478 		return err;
479 
480 	dev->mtu = mtu;
481 
482 	return 0;
483 }
484 
485 static void prestera_port_get_stats64(struct net_device *dev,
486 				      struct rtnl_link_stats64 *stats)
487 {
488 	struct prestera_port *port = netdev_priv(dev);
489 	struct prestera_port_stats *port_stats = &port->cached_hw_stats.stats;
490 
491 	stats->rx_packets = port_stats->broadcast_frames_received +
492 				port_stats->multicast_frames_received +
493 				port_stats->unicast_frames_received;
494 
495 	stats->tx_packets = port_stats->broadcast_frames_sent +
496 				port_stats->multicast_frames_sent +
497 				port_stats->unicast_frames_sent;
498 
499 	stats->rx_bytes = port_stats->good_octets_received;
500 
501 	stats->tx_bytes = port_stats->good_octets_sent;
502 
503 	stats->rx_errors = port_stats->rx_error_frame_received;
504 	stats->tx_errors = port_stats->mac_trans_error;
505 
506 	stats->rx_dropped = port_stats->buffer_overrun;
507 	stats->tx_dropped = 0;
508 
509 	stats->multicast = port_stats->multicast_frames_received;
510 	stats->collisions = port_stats->excessive_collision;
511 
512 	stats->rx_crc_errors = port_stats->bad_crc;
513 }
514 
515 static void prestera_port_get_hw_stats(struct prestera_port *port)
516 {
517 	prestera_hw_port_stats_get(port, &port->cached_hw_stats.stats);
518 }
519 
520 static void prestera_port_stats_update(struct work_struct *work)
521 {
522 	struct prestera_port *port =
523 		container_of(work, struct prestera_port,
524 			     cached_hw_stats.caching_dw.work);
525 
526 	prestera_port_get_hw_stats(port);
527 
528 	queue_delayed_work(prestera_wq, &port->cached_hw_stats.caching_dw,
529 			   msecs_to_jiffies(PRESTERA_STATS_DELAY_MS));
530 }
531 
532 static int prestera_port_setup_tc(struct net_device *dev,
533 				  enum tc_setup_type type,
534 				  void *type_data)
535 {
536 	struct prestera_port *port = netdev_priv(dev);
537 
538 	switch (type) {
539 	case TC_SETUP_BLOCK:
540 		return prestera_flow_block_setup(port, type_data);
541 	default:
542 		return -EOPNOTSUPP;
543 	}
544 }
545 
546 static const struct net_device_ops prestera_netdev_ops = {
547 	.ndo_open = prestera_port_open,
548 	.ndo_stop = prestera_port_close,
549 	.ndo_start_xmit = prestera_port_xmit,
550 	.ndo_setup_tc = prestera_port_setup_tc,
551 	.ndo_change_mtu = prestera_port_change_mtu,
552 	.ndo_get_stats64 = prestera_port_get_stats64,
553 	.ndo_set_mac_address = prestera_port_set_mac_address,
554 	.ndo_get_devlink_port = prestera_devlink_get_port,
555 };
556 
557 int prestera_port_autoneg_set(struct prestera_port *port, u64 link_modes)
558 {
559 	int err;
560 
561 	if (port->autoneg && port->adver_link_modes == link_modes)
562 		return 0;
563 
564 	err = prestera_hw_port_phy_mode_set(port, port->cfg_phy.admin,
565 					    true, 0, link_modes,
566 					    port->cfg_phy.mdix);
567 	if (err)
568 		return err;
569 
570 	port->adver_fec = BIT(PRESTERA_PORT_FEC_OFF);
571 	port->adver_link_modes = link_modes;
572 	port->cfg_phy.mode = 0;
573 	port->autoneg = true;
574 
575 	return 0;
576 }
577 
578 static void prestera_port_list_add(struct prestera_port *port)
579 {
580 	write_lock(&port->sw->port_list_lock);
581 	list_add(&port->list, &port->sw->port_list);
582 	write_unlock(&port->sw->port_list_lock);
583 }
584 
585 static void prestera_port_list_del(struct prestera_port *port)
586 {
587 	write_lock(&port->sw->port_list_lock);
588 	list_del(&port->list);
589 	write_unlock(&port->sw->port_list_lock);
590 }
591 
592 static int prestera_port_create(struct prestera_switch *sw, u32 id)
593 {
594 	struct prestera_port_mac_config cfg_mac;
595 	struct prestera_port *port;
596 	struct net_device *dev;
597 	int err;
598 
599 	dev = alloc_etherdev(sizeof(*port));
600 	if (!dev)
601 		return -ENOMEM;
602 
603 	port = netdev_priv(dev);
604 
605 	INIT_LIST_HEAD(&port->vlans_list);
606 	port->pvid = PRESTERA_DEFAULT_VID;
607 	port->lag = NULL;
608 	port->dev = dev;
609 	port->id = id;
610 	port->sw = sw;
611 
612 	spin_lock_init(&port->state_mac_lock);
613 
614 	err = prestera_hw_port_info_get(port, &port->dev_id, &port->hw_id,
615 					&port->fp_id);
616 	if (err) {
617 		dev_err(prestera_dev(sw), "Failed to get port(%u) info\n", id);
618 		goto err_port_info_get;
619 	}
620 
621 	err = prestera_devlink_port_register(port);
622 	if (err)
623 		goto err_dl_port_register;
624 
625 	dev->features |= NETIF_F_NETNS_LOCAL | NETIF_F_HW_TC;
626 	dev->netdev_ops = &prestera_netdev_ops;
627 	dev->ethtool_ops = &prestera_ethtool_ops;
628 	SET_NETDEV_DEV(dev, sw->dev->dev);
629 
630 	if (port->caps.transceiver != PRESTERA_PORT_TCVR_SFP)
631 		netif_carrier_off(dev);
632 
633 	dev->mtu = min_t(unsigned int, sw->mtu_max, PRESTERA_MTU_DEFAULT);
634 	dev->min_mtu = sw->mtu_min;
635 	dev->max_mtu = sw->mtu_max;
636 
637 	err = prestera_hw_port_mtu_set(port, dev->mtu);
638 	if (err) {
639 		dev_err(prestera_dev(sw), "Failed to set port(%u) mtu(%d)\n",
640 			id, dev->mtu);
641 		goto err_port_init;
642 	}
643 
644 	if (port->fp_id >= PRESTERA_MAC_ADDR_NUM_MAX) {
645 		err = -EINVAL;
646 		goto err_port_init;
647 	}
648 
649 	eth_hw_addr_gen(dev, sw->base_mac, port->fp_id);
650 	/* firmware requires that port's MAC address consist of the first
651 	 * 5 bytes of the base MAC address
652 	 */
653 	if (memcmp(dev->dev_addr, sw->base_mac, ETH_ALEN - 1)) {
654 		dev_warn(prestera_dev(sw), "Port MAC address wraps for port(%u)\n", id);
655 		dev_addr_mod(dev, 0, sw->base_mac, ETH_ALEN - 1);
656 	}
657 
658 	err = prestera_hw_port_mac_set(port, dev->dev_addr);
659 	if (err) {
660 		dev_err(prestera_dev(sw), "Failed to set port(%u) mac addr\n", id);
661 		goto err_port_init;
662 	}
663 
664 	err = prestera_hw_port_cap_get(port, &port->caps);
665 	if (err) {
666 		dev_err(prestera_dev(sw), "Failed to get port(%u) caps\n", id);
667 		goto err_port_init;
668 	}
669 
670 	port->adver_link_modes = port->caps.supp_link_modes;
671 	port->adver_fec = 0;
672 	port->autoneg = true;
673 
674 	/* initialize config mac */
675 	if (port->caps.transceiver != PRESTERA_PORT_TCVR_SFP) {
676 		cfg_mac.admin = true;
677 		cfg_mac.mode = PRESTERA_MAC_MODE_INTERNAL;
678 	} else {
679 		cfg_mac.admin = false;
680 		cfg_mac.mode = PRESTERA_MAC_MODE_MAX;
681 	}
682 	cfg_mac.inband = 0;
683 	cfg_mac.speed = 0;
684 	cfg_mac.duplex = DUPLEX_UNKNOWN;
685 	cfg_mac.fec = PRESTERA_PORT_FEC_OFF;
686 
687 	err = prestera_port_cfg_mac_write(port, &cfg_mac);
688 	if (err) {
689 		dev_err(prestera_dev(sw),
690 			"Failed to set port(%u) mac mode\n", id);
691 		goto err_port_init;
692 	}
693 
694 	/* initialize config phy (if this is inegral) */
695 	if (port->caps.transceiver != PRESTERA_PORT_TCVR_SFP) {
696 		port->cfg_phy.mdix = ETH_TP_MDI_AUTO;
697 		port->cfg_phy.admin = false;
698 		err = prestera_hw_port_phy_mode_set(port,
699 						    port->cfg_phy.admin,
700 						    false, 0, 0,
701 						    port->cfg_phy.mdix);
702 		if (err) {
703 			dev_err(prestera_dev(sw),
704 				"Failed to set port(%u) phy mode\n", id);
705 			goto err_port_init;
706 		}
707 	}
708 
709 	err = prestera_rxtx_port_init(port);
710 	if (err)
711 		goto err_port_init;
712 
713 	INIT_DELAYED_WORK(&port->cached_hw_stats.caching_dw,
714 			  &prestera_port_stats_update);
715 
716 	prestera_port_list_add(port);
717 
718 	err = register_netdev(dev);
719 	if (err)
720 		goto err_register_netdev;
721 
722 	prestera_devlink_port_set(port);
723 
724 	err = prestera_port_sfp_bind(port);
725 	if (err)
726 		goto err_sfp_bind;
727 
728 	return 0;
729 
730 err_sfp_bind:
731 err_register_netdev:
732 	prestera_port_list_del(port);
733 err_port_init:
734 	prestera_devlink_port_unregister(port);
735 err_dl_port_register:
736 err_port_info_get:
737 	free_netdev(dev);
738 	return err;
739 }
740 
741 static void prestera_port_destroy(struct prestera_port *port)
742 {
743 	struct net_device *dev = port->dev;
744 
745 	cancel_delayed_work_sync(&port->cached_hw_stats.caching_dw);
746 	prestera_devlink_port_clear(port);
747 	unregister_netdev(dev);
748 	prestera_port_list_del(port);
749 	prestera_devlink_port_unregister(port);
750 	free_netdev(dev);
751 }
752 
753 static void prestera_destroy_ports(struct prestera_switch *sw)
754 {
755 	struct prestera_port *port, *tmp;
756 
757 	list_for_each_entry_safe(port, tmp, &sw->port_list, list)
758 		prestera_port_destroy(port);
759 }
760 
761 static int prestera_create_ports(struct prestera_switch *sw)
762 {
763 	struct prestera_port *port, *tmp;
764 	u32 port_idx;
765 	int err;
766 
767 	for (port_idx = 0; port_idx < sw->port_count; port_idx++) {
768 		err = prestera_port_create(sw, port_idx);
769 		if (err)
770 			goto err_port_create;
771 	}
772 
773 	return 0;
774 
775 err_port_create:
776 	list_for_each_entry_safe(port, tmp, &sw->port_list, list) {
777 		prestera_port_sfp_unbind(port);
778 		prestera_port_destroy(port);
779 	}
780 
781 	return err;
782 }
783 
784 static void prestera_port_handle_event(struct prestera_switch *sw,
785 				       struct prestera_event *evt, void *arg)
786 {
787 	struct prestera_port_mac_state smac;
788 	struct prestera_port_event *pevt;
789 	struct delayed_work *caching_dw;
790 	struct prestera_port *port;
791 
792 	if (evt->id == PRESTERA_PORT_EVENT_MAC_STATE_CHANGED) {
793 		pevt = &evt->port_evt;
794 		port = prestera_find_port(sw, pevt->port_id);
795 		if (!port || !port->dev)
796 			return;
797 
798 		caching_dw = &port->cached_hw_stats.caching_dw;
799 
800 		memset(&smac, 0, sizeof(smac));
801 		smac.valid = true;
802 		smac.oper = pevt->data.mac.oper;
803 		if (smac.oper) {
804 			smac.mode = pevt->data.mac.mode;
805 			smac.speed = pevt->data.mac.speed;
806 			smac.duplex = pevt->data.mac.duplex;
807 			smac.fc = pevt->data.mac.fc;
808 			smac.fec = pevt->data.mac.fec;
809 		}
810 		prestera_port_mac_state_cache_write(port, &smac);
811 
812 		if (port->state_mac.oper) {
813 			if (port->phy_link)
814 				phylink_mac_change(port->phy_link, true);
815 			else
816 				netif_carrier_on(port->dev);
817 
818 			if (!delayed_work_pending(caching_dw))
819 				queue_delayed_work(prestera_wq, caching_dw, 0);
820 		} else {
821 			if (port->phy_link)
822 				phylink_mac_change(port->phy_link, false);
823 			else if (netif_running(port->dev) && netif_carrier_ok(port->dev))
824 				netif_carrier_off(port->dev);
825 
826 			if (delayed_work_pending(caching_dw))
827 				cancel_delayed_work(caching_dw);
828 		}
829 	}
830 }
831 
832 static int prestera_event_handlers_register(struct prestera_switch *sw)
833 {
834 	return prestera_hw_event_handler_register(sw, PRESTERA_EVENT_TYPE_PORT,
835 						  prestera_port_handle_event,
836 						  NULL);
837 }
838 
839 static void prestera_event_handlers_unregister(struct prestera_switch *sw)
840 {
841 	prestera_hw_event_handler_unregister(sw, PRESTERA_EVENT_TYPE_PORT,
842 					     prestera_port_handle_event);
843 }
844 
845 static int prestera_switch_set_base_mac_addr(struct prestera_switch *sw)
846 {
847 	struct device_node *base_mac_np;
848 	int ret = 0;
849 
850 	if (sw->np) {
851 		base_mac_np = of_parse_phandle(sw->np, "base-mac-provider", 0);
852 		if (base_mac_np) {
853 			ret = of_get_mac_address(base_mac_np, sw->base_mac);
854 			of_node_put(base_mac_np);
855 		}
856 	}
857 
858 	if (!is_valid_ether_addr(sw->base_mac) || ret) {
859 		eth_random_addr(sw->base_mac);
860 		dev_info(prestera_dev(sw), "using random base mac address\n");
861 	}
862 
863 	return prestera_hw_switch_mac_set(sw, sw->base_mac);
864 }
865 
866 struct prestera_lag *prestera_lag_by_id(struct prestera_switch *sw, u16 id)
867 {
868 	return id < sw->lag_max ? &sw->lags[id] : NULL;
869 }
870 
871 static struct prestera_lag *prestera_lag_by_dev(struct prestera_switch *sw,
872 						struct net_device *dev)
873 {
874 	struct prestera_lag *lag;
875 	u16 id;
876 
877 	for (id = 0; id < sw->lag_max; id++) {
878 		lag = &sw->lags[id];
879 		if (lag->dev == dev)
880 			return lag;
881 	}
882 
883 	return NULL;
884 }
885 
886 int prestera_lag_id(struct prestera_switch *sw,
887 		    struct net_device *lag_dev, u16 *lag_id)
888 {
889 	struct prestera_lag *lag;
890 	int free_id = -1;
891 	int id;
892 
893 	for (id = 0; id < sw->lag_max; id++) {
894 		lag = prestera_lag_by_id(sw, id);
895 		if (lag->member_count) {
896 			if (lag->dev == lag_dev) {
897 				*lag_id = id;
898 				return 0;
899 			}
900 		} else if (free_id < 0) {
901 			free_id = id;
902 		}
903 	}
904 	if (free_id < 0)
905 		return -ENOSPC;
906 	*lag_id = free_id;
907 	return 0;
908 }
909 
910 static struct prestera_lag *prestera_lag_create(struct prestera_switch *sw,
911 						struct net_device *lag_dev)
912 {
913 	struct prestera_lag *lag = NULL;
914 	u16 id;
915 
916 	for (id = 0; id < sw->lag_max; id++) {
917 		lag = &sw->lags[id];
918 		if (!lag->dev)
919 			break;
920 	}
921 	if (lag) {
922 		INIT_LIST_HEAD(&lag->members);
923 		lag->dev = lag_dev;
924 	}
925 
926 	return lag;
927 }
928 
929 static void prestera_lag_destroy(struct prestera_switch *sw,
930 				 struct prestera_lag *lag)
931 {
932 	WARN_ON(!list_empty(&lag->members));
933 	lag->member_count = 0;
934 	lag->dev = NULL;
935 }
936 
937 static int prestera_lag_port_add(struct prestera_port *port,
938 				 struct net_device *lag_dev)
939 {
940 	struct prestera_switch *sw = port->sw;
941 	struct prestera_lag *lag;
942 	int err;
943 
944 	lag = prestera_lag_by_dev(sw, lag_dev);
945 	if (!lag) {
946 		lag = prestera_lag_create(sw, lag_dev);
947 		if (!lag)
948 			return -ENOSPC;
949 	}
950 
951 	if (lag->member_count >= sw->lag_member_max)
952 		return -ENOSPC;
953 
954 	err = prestera_hw_lag_member_add(port, lag->lag_id);
955 	if (err) {
956 		if (!lag->member_count)
957 			prestera_lag_destroy(sw, lag);
958 		return err;
959 	}
960 
961 	list_add(&port->lag_member, &lag->members);
962 	lag->member_count++;
963 	port->lag = lag;
964 
965 	return 0;
966 }
967 
968 static int prestera_lag_port_del(struct prestera_port *port)
969 {
970 	struct prestera_switch *sw = port->sw;
971 	struct prestera_lag *lag = port->lag;
972 	int err;
973 
974 	if (!lag || !lag->member_count)
975 		return -EINVAL;
976 
977 	err = prestera_hw_lag_member_del(port, lag->lag_id);
978 	if (err)
979 		return err;
980 
981 	list_del(&port->lag_member);
982 	lag->member_count--;
983 	port->lag = NULL;
984 
985 	if (netif_is_bridge_port(lag->dev)) {
986 		struct net_device *br_dev;
987 
988 		br_dev = netdev_master_upper_dev_get(lag->dev);
989 
990 		prestera_bridge_port_leave(br_dev, port);
991 	}
992 
993 	if (!lag->member_count)
994 		prestera_lag_destroy(sw, lag);
995 
996 	return 0;
997 }
998 
999 bool prestera_port_is_lag_member(const struct prestera_port *port)
1000 {
1001 	return !!port->lag;
1002 }
1003 
1004 u16 prestera_port_lag_id(const struct prestera_port *port)
1005 {
1006 	return port->lag->lag_id;
1007 }
1008 
1009 static int prestera_lag_init(struct prestera_switch *sw)
1010 {
1011 	u16 id;
1012 
1013 	sw->lags = kcalloc(sw->lag_max, sizeof(*sw->lags), GFP_KERNEL);
1014 	if (!sw->lags)
1015 		return -ENOMEM;
1016 
1017 	for (id = 0; id < sw->lag_max; id++)
1018 		sw->lags[id].lag_id = id;
1019 
1020 	return 0;
1021 }
1022 
1023 static void prestera_lag_fini(struct prestera_switch *sw)
1024 {
1025 	u8 idx;
1026 
1027 	for (idx = 0; idx < sw->lag_max; idx++)
1028 		WARN_ON(sw->lags[idx].member_count);
1029 
1030 	kfree(sw->lags);
1031 }
1032 
1033 bool prestera_netdev_check(const struct net_device *dev)
1034 {
1035 	return dev->netdev_ops == &prestera_netdev_ops;
1036 }
1037 
1038 static int prestera_lower_dev_walk(struct net_device *dev,
1039 				   struct netdev_nested_priv *priv)
1040 {
1041 	struct prestera_port **pport = (struct prestera_port **)priv->data;
1042 
1043 	if (prestera_netdev_check(dev)) {
1044 		*pport = netdev_priv(dev);
1045 		return 1;
1046 	}
1047 
1048 	return 0;
1049 }
1050 
1051 struct prestera_port *prestera_port_dev_lower_find(struct net_device *dev)
1052 {
1053 	struct prestera_port *port = NULL;
1054 	struct netdev_nested_priv priv = {
1055 		.data = (void *)&port,
1056 	};
1057 
1058 	if (prestera_netdev_check(dev))
1059 		return netdev_priv(dev);
1060 
1061 	netdev_walk_all_lower_dev(dev, prestera_lower_dev_walk, &priv);
1062 
1063 	return port;
1064 }
1065 
1066 static int prestera_netdev_port_lower_event(struct net_device *dev,
1067 					    unsigned long event, void *ptr)
1068 {
1069 	struct netdev_notifier_changelowerstate_info *info = ptr;
1070 	struct netdev_lag_lower_state_info *lower_state_info;
1071 	struct prestera_port *port = netdev_priv(dev);
1072 	bool enabled;
1073 
1074 	if (!netif_is_lag_port(dev))
1075 		return 0;
1076 	if (!prestera_port_is_lag_member(port))
1077 		return 0;
1078 
1079 	lower_state_info = info->lower_state_info;
1080 	enabled = lower_state_info->link_up && lower_state_info->tx_enabled;
1081 
1082 	return prestera_hw_lag_member_enable(port, port->lag->lag_id, enabled);
1083 }
1084 
1085 static bool prestera_lag_master_check(struct net_device *lag_dev,
1086 				      struct netdev_lag_upper_info *info,
1087 				      struct netlink_ext_ack *ext_ack)
1088 {
1089 	if (info->tx_type != NETDEV_LAG_TX_TYPE_HASH) {
1090 		NL_SET_ERR_MSG_MOD(ext_ack, "Unsupported LAG Tx type");
1091 		return false;
1092 	}
1093 
1094 	return true;
1095 }
1096 
1097 static int prestera_netdev_port_event(struct net_device *lower,
1098 				      struct net_device *dev,
1099 				      unsigned long event, void *ptr)
1100 {
1101 	struct netdev_notifier_info *info = ptr;
1102 	struct netdev_notifier_changeupper_info *cu_info;
1103 	struct prestera_port *port = netdev_priv(dev);
1104 	struct netlink_ext_ack *extack;
1105 	struct net_device *upper;
1106 
1107 	extack = netdev_notifier_info_to_extack(info);
1108 	cu_info = container_of(info,
1109 			       struct netdev_notifier_changeupper_info,
1110 			       info);
1111 
1112 	switch (event) {
1113 	case NETDEV_PRECHANGEUPPER:
1114 		upper = cu_info->upper_dev;
1115 		if (!netif_is_bridge_master(upper) &&
1116 		    !netif_is_lag_master(upper)) {
1117 			NL_SET_ERR_MSG_MOD(extack, "Unknown upper device type");
1118 			return -EINVAL;
1119 		}
1120 
1121 		if (!cu_info->linking)
1122 			break;
1123 
1124 		if (netdev_has_any_upper_dev(upper)) {
1125 			NL_SET_ERR_MSG_MOD(extack, "Upper device is already enslaved");
1126 			return -EINVAL;
1127 		}
1128 
1129 		if (netif_is_lag_master(upper) &&
1130 		    !prestera_lag_master_check(upper, cu_info->upper_info, extack))
1131 			return -EOPNOTSUPP;
1132 		if (netif_is_lag_master(upper) && vlan_uses_dev(dev)) {
1133 			NL_SET_ERR_MSG_MOD(extack,
1134 					   "Master device is a LAG master and port has a VLAN");
1135 			return -EINVAL;
1136 		}
1137 		if (netif_is_lag_port(dev) && is_vlan_dev(upper) &&
1138 		    !netif_is_lag_master(vlan_dev_real_dev(upper))) {
1139 			NL_SET_ERR_MSG_MOD(extack,
1140 					   "Can not put a VLAN on a LAG port");
1141 			return -EINVAL;
1142 		}
1143 		break;
1144 
1145 	case NETDEV_CHANGEUPPER:
1146 		upper = cu_info->upper_dev;
1147 		if (netif_is_bridge_master(upper)) {
1148 			if (cu_info->linking)
1149 				return prestera_bridge_port_join(upper, port,
1150 								 extack);
1151 			else
1152 				prestera_bridge_port_leave(upper, port);
1153 		} else if (netif_is_lag_master(upper)) {
1154 			if (cu_info->linking)
1155 				return prestera_lag_port_add(port, upper);
1156 			else
1157 				prestera_lag_port_del(port);
1158 		}
1159 		break;
1160 
1161 	case NETDEV_CHANGELOWERSTATE:
1162 		return prestera_netdev_port_lower_event(dev, event, ptr);
1163 	}
1164 
1165 	return 0;
1166 }
1167 
1168 static int prestera_netdevice_lag_event(struct net_device *lag_dev,
1169 					unsigned long event, void *ptr)
1170 {
1171 	struct net_device *dev;
1172 	struct list_head *iter;
1173 	int err;
1174 
1175 	netdev_for_each_lower_dev(lag_dev, dev, iter) {
1176 		if (prestera_netdev_check(dev)) {
1177 			err = prestera_netdev_port_event(lag_dev, dev, event,
1178 							 ptr);
1179 			if (err)
1180 				return err;
1181 		}
1182 	}
1183 
1184 	return 0;
1185 }
1186 
1187 static int prestera_netdev_event_handler(struct notifier_block *nb,
1188 					 unsigned long event, void *ptr)
1189 {
1190 	struct net_device *dev = netdev_notifier_info_to_dev(ptr);
1191 	int err = 0;
1192 
1193 	if (prestera_netdev_check(dev))
1194 		err = prestera_netdev_port_event(dev, dev, event, ptr);
1195 	else if (netif_is_lag_master(dev))
1196 		err = prestera_netdevice_lag_event(dev, event, ptr);
1197 
1198 	return notifier_from_errno(err);
1199 }
1200 
1201 struct prestera_mdb_entry *
1202 prestera_mdb_entry_create(struct prestera_switch *sw,
1203 			  const unsigned char *addr, u16 vid)
1204 {
1205 	struct prestera_flood_domain *flood_domain;
1206 	struct prestera_mdb_entry *mdb_entry;
1207 
1208 	mdb_entry = kzalloc(sizeof(*mdb_entry), GFP_KERNEL);
1209 	if (!mdb_entry)
1210 		goto err_mdb_alloc;
1211 
1212 	flood_domain = prestera_flood_domain_create(sw);
1213 	if (!flood_domain)
1214 		goto err_flood_domain_create;
1215 
1216 	mdb_entry->sw = sw;
1217 	mdb_entry->vid = vid;
1218 	mdb_entry->flood_domain = flood_domain;
1219 	ether_addr_copy(mdb_entry->addr, addr);
1220 
1221 	if (prestera_hw_mdb_create(mdb_entry))
1222 		goto err_mdb_hw_create;
1223 
1224 	return mdb_entry;
1225 
1226 err_mdb_hw_create:
1227 	prestera_flood_domain_destroy(flood_domain);
1228 err_flood_domain_create:
1229 	kfree(mdb_entry);
1230 err_mdb_alloc:
1231 	return NULL;
1232 }
1233 
1234 void prestera_mdb_entry_destroy(struct prestera_mdb_entry *mdb_entry)
1235 {
1236 	prestera_hw_mdb_destroy(mdb_entry);
1237 	prestera_flood_domain_destroy(mdb_entry->flood_domain);
1238 	kfree(mdb_entry);
1239 }
1240 
1241 struct prestera_flood_domain *
1242 prestera_flood_domain_create(struct prestera_switch *sw)
1243 {
1244 	struct prestera_flood_domain *domain;
1245 
1246 	domain = kzalloc(sizeof(*domain), GFP_KERNEL);
1247 	if (!domain)
1248 		return NULL;
1249 
1250 	domain->sw = sw;
1251 
1252 	if (prestera_hw_flood_domain_create(domain)) {
1253 		kfree(domain);
1254 		return NULL;
1255 	}
1256 
1257 	INIT_LIST_HEAD(&domain->flood_domain_port_list);
1258 
1259 	return domain;
1260 }
1261 
1262 void prestera_flood_domain_destroy(struct prestera_flood_domain *flood_domain)
1263 {
1264 	WARN_ON(!list_empty(&flood_domain->flood_domain_port_list));
1265 	WARN_ON_ONCE(prestera_hw_flood_domain_destroy(flood_domain));
1266 	kfree(flood_domain);
1267 }
1268 
1269 int
1270 prestera_flood_domain_port_create(struct prestera_flood_domain *flood_domain,
1271 				  struct net_device *dev,
1272 				  u16 vid)
1273 {
1274 	struct prestera_flood_domain_port *flood_domain_port;
1275 	bool is_first_port_in_list = false;
1276 	int err;
1277 
1278 	flood_domain_port = kzalloc(sizeof(*flood_domain_port), GFP_KERNEL);
1279 	if (!flood_domain_port) {
1280 		err = -ENOMEM;
1281 		goto err_port_alloc;
1282 	}
1283 
1284 	flood_domain_port->vid = vid;
1285 
1286 	if (list_empty(&flood_domain->flood_domain_port_list))
1287 		is_first_port_in_list = true;
1288 
1289 	list_add(&flood_domain_port->flood_domain_port_node,
1290 		 &flood_domain->flood_domain_port_list);
1291 
1292 	flood_domain_port->flood_domain = flood_domain;
1293 	flood_domain_port->dev = dev;
1294 
1295 	if (!is_first_port_in_list) {
1296 		err = prestera_hw_flood_domain_ports_reset(flood_domain);
1297 		if (err)
1298 			goto err_prestera_mdb_port_create_hw;
1299 	}
1300 
1301 	err = prestera_hw_flood_domain_ports_set(flood_domain);
1302 	if (err)
1303 		goto err_prestera_mdb_port_create_hw;
1304 
1305 	return 0;
1306 
1307 err_prestera_mdb_port_create_hw:
1308 	list_del(&flood_domain_port->flood_domain_port_node);
1309 	kfree(flood_domain_port);
1310 err_port_alloc:
1311 	return err;
1312 }
1313 
1314 void
1315 prestera_flood_domain_port_destroy(struct prestera_flood_domain_port *port)
1316 {
1317 	struct prestera_flood_domain *flood_domain = port->flood_domain;
1318 
1319 	list_del(&port->flood_domain_port_node);
1320 
1321 	WARN_ON_ONCE(prestera_hw_flood_domain_ports_reset(flood_domain));
1322 
1323 	if (!list_empty(&flood_domain->flood_domain_port_list))
1324 		WARN_ON_ONCE(prestera_hw_flood_domain_ports_set(flood_domain));
1325 
1326 	kfree(port);
1327 }
1328 
1329 struct prestera_flood_domain_port *
1330 prestera_flood_domain_port_find(struct prestera_flood_domain *flood_domain,
1331 				struct net_device *dev, u16 vid)
1332 {
1333 	struct prestera_flood_domain_port *flood_domain_port;
1334 
1335 	list_for_each_entry(flood_domain_port,
1336 			    &flood_domain->flood_domain_port_list,
1337 			    flood_domain_port_node)
1338 		if (flood_domain_port->dev == dev &&
1339 		    vid == flood_domain_port->vid)
1340 			return flood_domain_port;
1341 
1342 	return NULL;
1343 }
1344 
1345 static int prestera_netdev_event_handler_register(struct prestera_switch *sw)
1346 {
1347 	sw->netdev_nb.notifier_call = prestera_netdev_event_handler;
1348 
1349 	return register_netdevice_notifier(&sw->netdev_nb);
1350 }
1351 
1352 static void prestera_netdev_event_handler_unregister(struct prestera_switch *sw)
1353 {
1354 	unregister_netdevice_notifier(&sw->netdev_nb);
1355 }
1356 
1357 static int prestera_switch_init(struct prestera_switch *sw)
1358 {
1359 	int err;
1360 
1361 	sw->np = of_find_compatible_node(NULL, NULL, "marvell,prestera");
1362 
1363 	err = prestera_hw_switch_init(sw);
1364 	if (err) {
1365 		dev_err(prestera_dev(sw), "Failed to init Switch device\n");
1366 		return err;
1367 	}
1368 
1369 	rwlock_init(&sw->port_list_lock);
1370 	INIT_LIST_HEAD(&sw->port_list);
1371 
1372 	err = prestera_switch_set_base_mac_addr(sw);
1373 	if (err)
1374 		return err;
1375 
1376 	err = prestera_netdev_event_handler_register(sw);
1377 	if (err)
1378 		return err;
1379 
1380 	err = prestera_router_init(sw);
1381 	if (err)
1382 		goto err_router_init;
1383 
1384 	err = prestera_switchdev_init(sw);
1385 	if (err)
1386 		goto err_swdev_register;
1387 
1388 	err = prestera_rxtx_switch_init(sw);
1389 	if (err)
1390 		goto err_rxtx_register;
1391 
1392 	err = prestera_event_handlers_register(sw);
1393 	if (err)
1394 		goto err_handlers_register;
1395 
1396 	err = prestera_counter_init(sw);
1397 	if (err)
1398 		goto err_counter_init;
1399 
1400 	err = prestera_acl_init(sw);
1401 	if (err)
1402 		goto err_acl_init;
1403 
1404 	err = prestera_span_init(sw);
1405 	if (err)
1406 		goto err_span_init;
1407 
1408 	err = prestera_devlink_traps_register(sw);
1409 	if (err)
1410 		goto err_dl_register;
1411 
1412 	err = prestera_lag_init(sw);
1413 	if (err)
1414 		goto err_lag_init;
1415 
1416 	err = prestera_create_ports(sw);
1417 	if (err)
1418 		goto err_ports_create;
1419 
1420 	prestera_devlink_register(sw);
1421 	return 0;
1422 
1423 err_ports_create:
1424 	prestera_lag_fini(sw);
1425 err_lag_init:
1426 	prestera_devlink_traps_unregister(sw);
1427 err_dl_register:
1428 	prestera_span_fini(sw);
1429 err_span_init:
1430 	prestera_acl_fini(sw);
1431 err_acl_init:
1432 	prestera_counter_fini(sw);
1433 err_counter_init:
1434 	prestera_event_handlers_unregister(sw);
1435 err_handlers_register:
1436 	prestera_rxtx_switch_fini(sw);
1437 err_rxtx_register:
1438 	prestera_switchdev_fini(sw);
1439 err_swdev_register:
1440 	prestera_router_fini(sw);
1441 err_router_init:
1442 	prestera_netdev_event_handler_unregister(sw);
1443 	prestera_hw_switch_fini(sw);
1444 
1445 	return err;
1446 }
1447 
1448 static void prestera_switch_fini(struct prestera_switch *sw)
1449 {
1450 	prestera_devlink_unregister(sw);
1451 	prestera_destroy_ports(sw);
1452 	prestera_lag_fini(sw);
1453 	prestera_devlink_traps_unregister(sw);
1454 	prestera_span_fini(sw);
1455 	prestera_acl_fini(sw);
1456 	prestera_counter_fini(sw);
1457 	prestera_event_handlers_unregister(sw);
1458 	prestera_rxtx_switch_fini(sw);
1459 	prestera_switchdev_fini(sw);
1460 	prestera_router_fini(sw);
1461 	prestera_netdev_event_handler_unregister(sw);
1462 	prestera_hw_switch_fini(sw);
1463 	of_node_put(sw->np);
1464 }
1465 
1466 int prestera_device_register(struct prestera_device *dev)
1467 {
1468 	struct prestera_switch *sw;
1469 	int err;
1470 
1471 	sw = prestera_devlink_alloc(dev);
1472 	if (!sw)
1473 		return -ENOMEM;
1474 
1475 	dev->priv = sw;
1476 	sw->dev = dev;
1477 
1478 	err = prestera_switch_init(sw);
1479 	if (err) {
1480 		prestera_devlink_free(sw);
1481 		return err;
1482 	}
1483 
1484 	return 0;
1485 }
1486 EXPORT_SYMBOL(prestera_device_register);
1487 
1488 void prestera_device_unregister(struct prestera_device *dev)
1489 {
1490 	struct prestera_switch *sw = dev->priv;
1491 
1492 	prestera_switch_fini(sw);
1493 	prestera_devlink_free(sw);
1494 }
1495 EXPORT_SYMBOL(prestera_device_unregister);
1496 
1497 static int __init prestera_module_init(void)
1498 {
1499 	prestera_wq = alloc_workqueue("prestera", 0, 0);
1500 	if (!prestera_wq)
1501 		return -ENOMEM;
1502 
1503 	prestera_owq = alloc_ordered_workqueue("prestera_ordered", 0);
1504 	if (!prestera_owq) {
1505 		destroy_workqueue(prestera_wq);
1506 		return -ENOMEM;
1507 	}
1508 
1509 	return 0;
1510 }
1511 
1512 static void __exit prestera_module_exit(void)
1513 {
1514 	destroy_workqueue(prestera_wq);
1515 	destroy_workqueue(prestera_owq);
1516 }
1517 
1518 module_init(prestera_module_init);
1519 module_exit(prestera_module_exit);
1520 
1521 MODULE_LICENSE("Dual BSD/GPL");
1522 MODULE_DESCRIPTION("Marvell Prestera switch driver");
1523