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