xref: /openbmc/linux/drivers/net/ethernet/mscc/ocelot.c (revision 400928bf928be153cddd76d9ac4e39978cb43fd3)
1a556c76aSAlexandre Belloni // SPDX-License-Identifier: (GPL-2.0 OR MIT)
2a556c76aSAlexandre Belloni /*
3a556c76aSAlexandre Belloni  * Microsemi Ocelot Switch driver
4a556c76aSAlexandre Belloni  *
5a556c76aSAlexandre Belloni  * Copyright (c) 2017 Microsemi Corporation
6a556c76aSAlexandre Belloni  */
7a556c76aSAlexandre Belloni #include <linux/etherdevice.h>
8a556c76aSAlexandre Belloni #include <linux/ethtool.h>
9a556c76aSAlexandre Belloni #include <linux/if_bridge.h>
10a556c76aSAlexandre Belloni #include <linux/if_ether.h>
11a556c76aSAlexandre Belloni #include <linux/if_vlan.h>
12a556c76aSAlexandre Belloni #include <linux/interrupt.h>
13a556c76aSAlexandre Belloni #include <linux/kernel.h>
14a556c76aSAlexandre Belloni #include <linux/module.h>
15a556c76aSAlexandre Belloni #include <linux/netdevice.h>
16a556c76aSAlexandre Belloni #include <linux/phy.h>
174e3b0468SAntoine Tenart #include <linux/ptp_clock_kernel.h>
18a556c76aSAlexandre Belloni #include <linux/skbuff.h>
19639c1b26SSteen Hegelund #include <linux/iopoll.h>
20a556c76aSAlexandre Belloni #include <net/arp.h>
21a556c76aSAlexandre Belloni #include <net/netevent.h>
22a556c76aSAlexandre Belloni #include <net/rtnetlink.h>
23a556c76aSAlexandre Belloni #include <net/switchdev.h>
24a556c76aSAlexandre Belloni 
25a556c76aSAlexandre Belloni #include "ocelot.h"
26b5962294SHoratiu Vultur #include "ocelot_ace.h"
27a556c76aSAlexandre Belloni 
28639c1b26SSteen Hegelund #define TABLE_UPDATE_SLEEP_US 10
29639c1b26SSteen Hegelund #define TABLE_UPDATE_TIMEOUT_US 100000
30639c1b26SSteen Hegelund 
31a556c76aSAlexandre Belloni /* MAC table entry types.
32a556c76aSAlexandre Belloni  * ENTRYTYPE_NORMAL is subject to aging.
33a556c76aSAlexandre Belloni  * ENTRYTYPE_LOCKED is not subject to aging.
34a556c76aSAlexandre Belloni  * ENTRYTYPE_MACv4 is not subject to aging. For IPv4 multicast.
35a556c76aSAlexandre Belloni  * ENTRYTYPE_MACv6 is not subject to aging. For IPv6 multicast.
36a556c76aSAlexandre Belloni  */
37a556c76aSAlexandre Belloni enum macaccess_entry_type {
38a556c76aSAlexandre Belloni 	ENTRYTYPE_NORMAL = 0,
39a556c76aSAlexandre Belloni 	ENTRYTYPE_LOCKED,
40a556c76aSAlexandre Belloni 	ENTRYTYPE_MACv4,
41a556c76aSAlexandre Belloni 	ENTRYTYPE_MACv6,
42a556c76aSAlexandre Belloni };
43a556c76aSAlexandre Belloni 
44a556c76aSAlexandre Belloni struct ocelot_mact_entry {
45a556c76aSAlexandre Belloni 	u8 mac[ETH_ALEN];
46a556c76aSAlexandre Belloni 	u16 vid;
47a556c76aSAlexandre Belloni 	enum macaccess_entry_type type;
48a556c76aSAlexandre Belloni };
49a556c76aSAlexandre Belloni 
50639c1b26SSteen Hegelund static inline u32 ocelot_mact_read_macaccess(struct ocelot *ocelot)
51639c1b26SSteen Hegelund {
52639c1b26SSteen Hegelund 	return ocelot_read(ocelot, ANA_TABLES_MACACCESS);
53639c1b26SSteen Hegelund }
54639c1b26SSteen Hegelund 
55a556c76aSAlexandre Belloni static inline int ocelot_mact_wait_for_completion(struct ocelot *ocelot)
56a556c76aSAlexandre Belloni {
57639c1b26SSteen Hegelund 	u32 val;
58a556c76aSAlexandre Belloni 
59639c1b26SSteen Hegelund 	return readx_poll_timeout(ocelot_mact_read_macaccess,
60639c1b26SSteen Hegelund 		ocelot, val,
61639c1b26SSteen Hegelund 		(val & ANA_TABLES_MACACCESS_MAC_TABLE_CMD_M) ==
62639c1b26SSteen Hegelund 		MACACCESS_CMD_IDLE,
63639c1b26SSteen Hegelund 		TABLE_UPDATE_SLEEP_US, TABLE_UPDATE_TIMEOUT_US);
64a556c76aSAlexandre Belloni }
65a556c76aSAlexandre Belloni 
66a556c76aSAlexandre Belloni static void ocelot_mact_select(struct ocelot *ocelot,
67a556c76aSAlexandre Belloni 			       const unsigned char mac[ETH_ALEN],
68a556c76aSAlexandre Belloni 			       unsigned int vid)
69a556c76aSAlexandre Belloni {
70a556c76aSAlexandre Belloni 	u32 macl = 0, mach = 0;
71a556c76aSAlexandre Belloni 
72a556c76aSAlexandre Belloni 	/* Set the MAC address to handle and the vlan associated in a format
73a556c76aSAlexandre Belloni 	 * understood by the hardware.
74a556c76aSAlexandre Belloni 	 */
75a556c76aSAlexandre Belloni 	mach |= vid    << 16;
76a556c76aSAlexandre Belloni 	mach |= mac[0] << 8;
77a556c76aSAlexandre Belloni 	mach |= mac[1] << 0;
78a556c76aSAlexandre Belloni 	macl |= mac[2] << 24;
79a556c76aSAlexandre Belloni 	macl |= mac[3] << 16;
80a556c76aSAlexandre Belloni 	macl |= mac[4] << 8;
81a556c76aSAlexandre Belloni 	macl |= mac[5] << 0;
82a556c76aSAlexandre Belloni 
83a556c76aSAlexandre Belloni 	ocelot_write(ocelot, macl, ANA_TABLES_MACLDATA);
84a556c76aSAlexandre Belloni 	ocelot_write(ocelot, mach, ANA_TABLES_MACHDATA);
85a556c76aSAlexandre Belloni 
86a556c76aSAlexandre Belloni }
87a556c76aSAlexandre Belloni 
88a556c76aSAlexandre Belloni static int ocelot_mact_learn(struct ocelot *ocelot, int port,
89a556c76aSAlexandre Belloni 			     const unsigned char mac[ETH_ALEN],
90a556c76aSAlexandre Belloni 			     unsigned int vid,
91a556c76aSAlexandre Belloni 			     enum macaccess_entry_type type)
92a556c76aSAlexandre Belloni {
93a556c76aSAlexandre Belloni 	ocelot_mact_select(ocelot, mac, vid);
94a556c76aSAlexandre Belloni 
95a556c76aSAlexandre Belloni 	/* Issue a write command */
96a556c76aSAlexandre Belloni 	ocelot_write(ocelot, ANA_TABLES_MACACCESS_VALID |
97a556c76aSAlexandre Belloni 			     ANA_TABLES_MACACCESS_DEST_IDX(port) |
98a556c76aSAlexandre Belloni 			     ANA_TABLES_MACACCESS_ENTRYTYPE(type) |
99a556c76aSAlexandre Belloni 			     ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_LEARN),
100a556c76aSAlexandre Belloni 			     ANA_TABLES_MACACCESS);
101a556c76aSAlexandre Belloni 
102a556c76aSAlexandre Belloni 	return ocelot_mact_wait_for_completion(ocelot);
103a556c76aSAlexandre Belloni }
104a556c76aSAlexandre Belloni 
105a556c76aSAlexandre Belloni static int ocelot_mact_forget(struct ocelot *ocelot,
106a556c76aSAlexandre Belloni 			      const unsigned char mac[ETH_ALEN],
107a556c76aSAlexandre Belloni 			      unsigned int vid)
108a556c76aSAlexandre Belloni {
109a556c76aSAlexandre Belloni 	ocelot_mact_select(ocelot, mac, vid);
110a556c76aSAlexandre Belloni 
111a556c76aSAlexandre Belloni 	/* Issue a forget command */
112a556c76aSAlexandre Belloni 	ocelot_write(ocelot,
113a556c76aSAlexandre Belloni 		     ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_FORGET),
114a556c76aSAlexandre Belloni 		     ANA_TABLES_MACACCESS);
115a556c76aSAlexandre Belloni 
116a556c76aSAlexandre Belloni 	return ocelot_mact_wait_for_completion(ocelot);
117a556c76aSAlexandre Belloni }
118a556c76aSAlexandre Belloni 
119a556c76aSAlexandre Belloni static void ocelot_mact_init(struct ocelot *ocelot)
120a556c76aSAlexandre Belloni {
121a556c76aSAlexandre Belloni 	/* Configure the learning mode entries attributes:
122a556c76aSAlexandre Belloni 	 * - Do not copy the frame to the CPU extraction queues.
123a556c76aSAlexandre Belloni 	 * - Use the vlan and mac_cpoy for dmac lookup.
124a556c76aSAlexandre Belloni 	 */
125a556c76aSAlexandre Belloni 	ocelot_rmw(ocelot, 0,
126a556c76aSAlexandre Belloni 		   ANA_AGENCTRL_LEARN_CPU_COPY | ANA_AGENCTRL_IGNORE_DMAC_FLAGS
127a556c76aSAlexandre Belloni 		   | ANA_AGENCTRL_LEARN_FWD_KILL
128a556c76aSAlexandre Belloni 		   | ANA_AGENCTRL_LEARN_IGNORE_VLAN,
129a556c76aSAlexandre Belloni 		   ANA_AGENCTRL);
130a556c76aSAlexandre Belloni 
131a556c76aSAlexandre Belloni 	/* Clear the MAC table */
132a556c76aSAlexandre Belloni 	ocelot_write(ocelot, MACACCESS_CMD_INIT, ANA_TABLES_MACACCESS);
133a556c76aSAlexandre Belloni }
134a556c76aSAlexandre Belloni 
135f270dbfaSVladimir Oltean static void ocelot_vcap_enable(struct ocelot *ocelot, int port)
136b5962294SHoratiu Vultur {
137b5962294SHoratiu Vultur 	ocelot_write_gix(ocelot, ANA_PORT_VCAP_S2_CFG_S2_ENA |
138b5962294SHoratiu Vultur 			 ANA_PORT_VCAP_S2_CFG_S2_IP6_CFG(0xa),
139f270dbfaSVladimir Oltean 			 ANA_PORT_VCAP_S2_CFG, port);
140b5962294SHoratiu Vultur }
141b5962294SHoratiu Vultur 
142639c1b26SSteen Hegelund static inline u32 ocelot_vlant_read_vlanaccess(struct ocelot *ocelot)
143639c1b26SSteen Hegelund {
144639c1b26SSteen Hegelund 	return ocelot_read(ocelot, ANA_TABLES_VLANACCESS);
145639c1b26SSteen Hegelund }
146639c1b26SSteen Hegelund 
147a556c76aSAlexandre Belloni static inline int ocelot_vlant_wait_for_completion(struct ocelot *ocelot)
148a556c76aSAlexandre Belloni {
149639c1b26SSteen Hegelund 	u32 val;
150a556c76aSAlexandre Belloni 
151639c1b26SSteen Hegelund 	return readx_poll_timeout(ocelot_vlant_read_vlanaccess,
152639c1b26SSteen Hegelund 		ocelot,
153639c1b26SSteen Hegelund 		val,
154639c1b26SSteen Hegelund 		(val & ANA_TABLES_VLANACCESS_VLAN_TBL_CMD_M) ==
155639c1b26SSteen Hegelund 		ANA_TABLES_VLANACCESS_CMD_IDLE,
156639c1b26SSteen Hegelund 		TABLE_UPDATE_SLEEP_US, TABLE_UPDATE_TIMEOUT_US);
157a556c76aSAlexandre Belloni }
158a556c76aSAlexandre Belloni 
1597142529fSAntoine Tenart static int ocelot_vlant_set_mask(struct ocelot *ocelot, u16 vid, u32 mask)
1607142529fSAntoine Tenart {
1617142529fSAntoine Tenart 	/* Select the VID to configure */
1627142529fSAntoine Tenart 	ocelot_write(ocelot, ANA_TABLES_VLANTIDX_V_INDEX(vid),
1637142529fSAntoine Tenart 		     ANA_TABLES_VLANTIDX);
1647142529fSAntoine Tenart 	/* Set the vlan port members mask and issue a write command */
1657142529fSAntoine Tenart 	ocelot_write(ocelot, ANA_TABLES_VLANACCESS_VLAN_PORT_MASK(mask) |
1667142529fSAntoine Tenart 			     ANA_TABLES_VLANACCESS_CMD_WRITE,
1677142529fSAntoine Tenart 		     ANA_TABLES_VLANACCESS);
1687142529fSAntoine Tenart 
1697142529fSAntoine Tenart 	return ocelot_vlant_wait_for_completion(ocelot);
1707142529fSAntoine Tenart }
1717142529fSAntoine Tenart 
172f270dbfaSVladimir Oltean static void ocelot_vlan_mode(struct ocelot *ocelot, int port,
1737142529fSAntoine Tenart 			     netdev_features_t features)
1747142529fSAntoine Tenart {
1757142529fSAntoine Tenart 	u32 val;
1767142529fSAntoine Tenart 
1777142529fSAntoine Tenart 	/* Filtering */
1787142529fSAntoine Tenart 	val = ocelot_read(ocelot, ANA_VLANMASK);
1797142529fSAntoine Tenart 	if (features & NETIF_F_HW_VLAN_CTAG_FILTER)
180f270dbfaSVladimir Oltean 		val |= BIT(port);
1817142529fSAntoine Tenart 	else
182f270dbfaSVladimir Oltean 		val &= ~BIT(port);
1837142529fSAntoine Tenart 	ocelot_write(ocelot, val, ANA_VLANMASK);
1847142529fSAntoine Tenart }
1857142529fSAntoine Tenart 
1865e256365SVladimir Oltean void ocelot_port_vlan_filtering(struct ocelot *ocelot, int port,
18797bb69e1SVladimir Oltean 				bool vlan_aware)
1887142529fSAntoine Tenart {
18997bb69e1SVladimir Oltean 	struct ocelot_port *ocelot_port = ocelot->ports[port];
1907142529fSAntoine Tenart 	u32 val;
1917142529fSAntoine Tenart 
19297bb69e1SVladimir Oltean 	if (vlan_aware)
19397bb69e1SVladimir Oltean 		val = ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
1947142529fSAntoine Tenart 		      ANA_PORT_VLAN_CFG_VLAN_POP_CNT(1);
19597bb69e1SVladimir Oltean 	else
19697bb69e1SVladimir Oltean 		val = 0;
1977142529fSAntoine Tenart 	ocelot_rmw_gix(ocelot, val,
1987142529fSAntoine Tenart 		       ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
1997142529fSAntoine Tenart 		       ANA_PORT_VLAN_CFG_VLAN_POP_CNT_M,
20097bb69e1SVladimir Oltean 		       ANA_PORT_VLAN_CFG, port);
2017142529fSAntoine Tenart 
20297bb69e1SVladimir Oltean 	if (vlan_aware && !ocelot_port->vid)
2037142529fSAntoine Tenart 		/* If port is vlan-aware and tagged, drop untagged and priority
2047142529fSAntoine Tenart 		 * tagged frames.
2057142529fSAntoine Tenart 		 */
20697bb69e1SVladimir Oltean 		val = ANA_PORT_DROP_CFG_DROP_UNTAGGED_ENA |
2077142529fSAntoine Tenart 		      ANA_PORT_DROP_CFG_DROP_PRIO_S_TAGGED_ENA |
2087142529fSAntoine Tenart 		      ANA_PORT_DROP_CFG_DROP_PRIO_C_TAGGED_ENA;
20997bb69e1SVladimir Oltean 	else
21097bb69e1SVladimir Oltean 		val = 0;
21197bb69e1SVladimir Oltean 	ocelot_rmw_gix(ocelot, val,
21297bb69e1SVladimir Oltean 		       ANA_PORT_DROP_CFG_DROP_UNTAGGED_ENA |
21397bb69e1SVladimir Oltean 		       ANA_PORT_DROP_CFG_DROP_PRIO_S_TAGGED_ENA |
21497bb69e1SVladimir Oltean 		       ANA_PORT_DROP_CFG_DROP_PRIO_C_TAGGED_ENA,
21597bb69e1SVladimir Oltean 		       ANA_PORT_DROP_CFG, port);
2167142529fSAntoine Tenart 
21797bb69e1SVladimir Oltean 	if (vlan_aware) {
21897bb69e1SVladimir Oltean 		if (ocelot_port->vid)
2197142529fSAntoine Tenart 			/* Tag all frames except when VID == DEFAULT_VLAN */
2207142529fSAntoine Tenart 			val |= REW_TAG_CFG_TAG_CFG(1);
2217142529fSAntoine Tenart 		else
2227142529fSAntoine Tenart 			/* Tag all frames */
2237142529fSAntoine Tenart 			val |= REW_TAG_CFG_TAG_CFG(3);
22497bb69e1SVladimir Oltean 	} else {
22597bb69e1SVladimir Oltean 		/* Port tagging disabled. */
22697bb69e1SVladimir Oltean 		val = REW_TAG_CFG_TAG_CFG(0);
2277142529fSAntoine Tenart 	}
2287142529fSAntoine Tenart 	ocelot_rmw_gix(ocelot, val,
2297142529fSAntoine Tenart 		       REW_TAG_CFG_TAG_CFG_M,
23097bb69e1SVladimir Oltean 		       REW_TAG_CFG, port);
23197bb69e1SVladimir Oltean }
2325e256365SVladimir Oltean EXPORT_SYMBOL(ocelot_port_vlan_filtering);
23397bb69e1SVladimir Oltean 
23497bb69e1SVladimir Oltean static int ocelot_port_set_native_vlan(struct ocelot *ocelot, int port,
23597bb69e1SVladimir Oltean 				       u16 vid)
23697bb69e1SVladimir Oltean {
23797bb69e1SVladimir Oltean 	struct ocelot_port *ocelot_port = ocelot->ports[port];
23897bb69e1SVladimir Oltean 
23997bb69e1SVladimir Oltean 	if (ocelot_port->vid != vid) {
24097bb69e1SVladimir Oltean 		/* Always permit deleting the native VLAN (vid = 0) */
24197bb69e1SVladimir Oltean 		if (ocelot_port->vid && vid) {
24297bb69e1SVladimir Oltean 			dev_err(ocelot->dev,
24397bb69e1SVladimir Oltean 				"Port already has a native VLAN: %d\n",
24497bb69e1SVladimir Oltean 				ocelot_port->vid);
24597bb69e1SVladimir Oltean 			return -EBUSY;
24697bb69e1SVladimir Oltean 		}
24797bb69e1SVladimir Oltean 		ocelot_port->vid = vid;
24897bb69e1SVladimir Oltean 	}
24997bb69e1SVladimir Oltean 
25097bb69e1SVladimir Oltean 	ocelot_rmw_gix(ocelot, REW_PORT_VLAN_CFG_PORT_VID(vid),
2517142529fSAntoine Tenart 		       REW_PORT_VLAN_CFG_PORT_VID_M,
25297bb69e1SVladimir Oltean 		       REW_PORT_VLAN_CFG, port);
25397bb69e1SVladimir Oltean 
25497bb69e1SVladimir Oltean 	return 0;
25597bb69e1SVladimir Oltean }
25697bb69e1SVladimir Oltean 
25797bb69e1SVladimir Oltean /* Default vlan to clasify for untagged frames (may be zero) */
25897bb69e1SVladimir Oltean static void ocelot_port_set_pvid(struct ocelot *ocelot, int port, u16 pvid)
25997bb69e1SVladimir Oltean {
26097bb69e1SVladimir Oltean 	struct ocelot_port *ocelot_port = ocelot->ports[port];
26197bb69e1SVladimir Oltean 
26297bb69e1SVladimir Oltean 	ocelot_rmw_gix(ocelot,
26397bb69e1SVladimir Oltean 		       ANA_PORT_VLAN_CFG_VLAN_VID(pvid),
26497bb69e1SVladimir Oltean 		       ANA_PORT_VLAN_CFG_VLAN_VID_M,
26597bb69e1SVladimir Oltean 		       ANA_PORT_VLAN_CFG, port);
26697bb69e1SVladimir Oltean 
26797bb69e1SVladimir Oltean 	ocelot_port->pvid = pvid;
2687142529fSAntoine Tenart }
2697142529fSAntoine Tenart 
2705e256365SVladimir Oltean int ocelot_vlan_add(struct ocelot *ocelot, int port, u16 vid, bool pvid,
2717142529fSAntoine Tenart 		    bool untagged)
2727142529fSAntoine Tenart {
2737142529fSAntoine Tenart 	int ret;
2747142529fSAntoine Tenart 
2757142529fSAntoine Tenart 	/* Make the port a member of the VLAN */
27697bb69e1SVladimir Oltean 	ocelot->vlan_mask[vid] |= BIT(port);
2777142529fSAntoine Tenart 	ret = ocelot_vlant_set_mask(ocelot, vid, ocelot->vlan_mask[vid]);
2787142529fSAntoine Tenart 	if (ret)
2797142529fSAntoine Tenart 		return ret;
2807142529fSAntoine Tenart 
2817142529fSAntoine Tenart 	/* Default ingress vlan classification */
2827142529fSAntoine Tenart 	if (pvid)
28397bb69e1SVladimir Oltean 		ocelot_port_set_pvid(ocelot, port, vid);
2847142529fSAntoine Tenart 
2857142529fSAntoine Tenart 	/* Untagged egress vlan clasification */
28697bb69e1SVladimir Oltean 	if (untagged) {
28797bb69e1SVladimir Oltean 		ret = ocelot_port_set_native_vlan(ocelot, port, vid);
28897bb69e1SVladimir Oltean 		if (ret)
28997bb69e1SVladimir Oltean 			return ret;
290b9cd75e6SVladimir Oltean 	}
2917142529fSAntoine Tenart 
2927142529fSAntoine Tenart 	return 0;
2937142529fSAntoine Tenart }
2945e256365SVladimir Oltean EXPORT_SYMBOL(ocelot_vlan_add);
2957142529fSAntoine Tenart 
2969855934cSVladimir Oltean static int ocelot_vlan_vid_add(struct net_device *dev, u16 vid, bool pvid,
2979855934cSVladimir Oltean 			       bool untagged)
2987142529fSAntoine Tenart {
299004d44f6SVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(dev);
300004d44f6SVladimir Oltean 	struct ocelot_port *ocelot_port = &priv->port;
30197bb69e1SVladimir Oltean 	struct ocelot *ocelot = ocelot_port->ocelot;
302004d44f6SVladimir Oltean 	int port = priv->chip_port;
3037142529fSAntoine Tenart 	int ret;
3047142529fSAntoine Tenart 
3059855934cSVladimir Oltean 	ret = ocelot_vlan_add(ocelot, port, vid, pvid, untagged);
3069855934cSVladimir Oltean 	if (ret)
3079855934cSVladimir Oltean 		return ret;
3087142529fSAntoine Tenart 
3099855934cSVladimir Oltean 	/* Add the port MAC address to with the right VLAN information */
3109855934cSVladimir Oltean 	ocelot_mact_learn(ocelot, PGID_CPU, dev->dev_addr, vid,
3119855934cSVladimir Oltean 			  ENTRYTYPE_LOCKED);
3129855934cSVladimir Oltean 
3139855934cSVladimir Oltean 	return 0;
3149855934cSVladimir Oltean }
3159855934cSVladimir Oltean 
3165e256365SVladimir Oltean int ocelot_vlan_del(struct ocelot *ocelot, int port, u16 vid)
3179855934cSVladimir Oltean {
3189855934cSVladimir Oltean 	struct ocelot_port *ocelot_port = ocelot->ports[port];
3199855934cSVladimir Oltean 	int ret;
3207142529fSAntoine Tenart 
3217142529fSAntoine Tenart 	/* Stop the port from being a member of the vlan */
32297bb69e1SVladimir Oltean 	ocelot->vlan_mask[vid] &= ~BIT(port);
3237142529fSAntoine Tenart 	ret = ocelot_vlant_set_mask(ocelot, vid, ocelot->vlan_mask[vid]);
3247142529fSAntoine Tenart 	if (ret)
3257142529fSAntoine Tenart 		return ret;
3267142529fSAntoine Tenart 
3277142529fSAntoine Tenart 	/* Ingress */
32897bb69e1SVladimir Oltean 	if (ocelot_port->pvid == vid)
32997bb69e1SVladimir Oltean 		ocelot_port_set_pvid(ocelot, port, 0);
3307142529fSAntoine Tenart 
3317142529fSAntoine Tenart 	/* Egress */
33297bb69e1SVladimir Oltean 	if (ocelot_port->vid == vid)
33397bb69e1SVladimir Oltean 		ocelot_port_set_native_vlan(ocelot, port, 0);
3347142529fSAntoine Tenart 
3357142529fSAntoine Tenart 	return 0;
3367142529fSAntoine Tenart }
3375e256365SVladimir Oltean EXPORT_SYMBOL(ocelot_vlan_del);
3387142529fSAntoine Tenart 
3399855934cSVladimir Oltean static int ocelot_vlan_vid_del(struct net_device *dev, u16 vid)
3409855934cSVladimir Oltean {
341004d44f6SVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(dev);
342004d44f6SVladimir Oltean 	struct ocelot *ocelot = priv->port.ocelot;
343004d44f6SVladimir Oltean 	int port = priv->chip_port;
3449855934cSVladimir Oltean 	int ret;
3459855934cSVladimir Oltean 
3469855934cSVladimir Oltean 	/* 8021q removes VID 0 on module unload for all interfaces
3479855934cSVladimir Oltean 	 * with VLAN filtering feature. We need to keep it to receive
3489855934cSVladimir Oltean 	 * untagged traffic.
3499855934cSVladimir Oltean 	 */
3509855934cSVladimir Oltean 	if (vid == 0)
3519855934cSVladimir Oltean 		return 0;
3529855934cSVladimir Oltean 
3539855934cSVladimir Oltean 	ret = ocelot_vlan_del(ocelot, port, vid);
3549855934cSVladimir Oltean 	if (ret)
3559855934cSVladimir Oltean 		return ret;
3569855934cSVladimir Oltean 
3579855934cSVladimir Oltean 	/* Del the port MAC address to with the right VLAN information */
3589855934cSVladimir Oltean 	ocelot_mact_forget(ocelot, dev->dev_addr, vid);
3599855934cSVladimir Oltean 
3609855934cSVladimir Oltean 	return 0;
3619855934cSVladimir Oltean }
3629855934cSVladimir Oltean 
363a556c76aSAlexandre Belloni static void ocelot_vlan_init(struct ocelot *ocelot)
364a556c76aSAlexandre Belloni {
3657142529fSAntoine Tenart 	u16 port, vid;
3667142529fSAntoine Tenart 
367a556c76aSAlexandre Belloni 	/* Clear VLAN table, by default all ports are members of all VLANs */
368a556c76aSAlexandre Belloni 	ocelot_write(ocelot, ANA_TABLES_VLANACCESS_CMD_INIT,
369a556c76aSAlexandre Belloni 		     ANA_TABLES_VLANACCESS);
370a556c76aSAlexandre Belloni 	ocelot_vlant_wait_for_completion(ocelot);
3717142529fSAntoine Tenart 
3727142529fSAntoine Tenart 	/* Configure the port VLAN memberships */
3737142529fSAntoine Tenart 	for (vid = 1; vid < VLAN_N_VID; vid++) {
3747142529fSAntoine Tenart 		ocelot->vlan_mask[vid] = 0;
3757142529fSAntoine Tenart 		ocelot_vlant_set_mask(ocelot, vid, ocelot->vlan_mask[vid]);
3767142529fSAntoine Tenart 	}
3777142529fSAntoine Tenart 
3787142529fSAntoine Tenart 	/* Because VLAN filtering is enabled, we need VID 0 to get untagged
3797142529fSAntoine Tenart 	 * traffic.  It is added automatically if 8021q module is loaded, but
3807142529fSAntoine Tenart 	 * we can't rely on it since module may be not loaded.
3817142529fSAntoine Tenart 	 */
3827142529fSAntoine Tenart 	ocelot->vlan_mask[0] = GENMASK(ocelot->num_phys_ports - 1, 0);
3837142529fSAntoine Tenart 	ocelot_vlant_set_mask(ocelot, 0, ocelot->vlan_mask[0]);
3847142529fSAntoine Tenart 
3857142529fSAntoine Tenart 	/* Set vlan ingress filter mask to all ports but the CPU port by
3867142529fSAntoine Tenart 	 * default.
3877142529fSAntoine Tenart 	 */
388714d0ffaSVladimir Oltean 	ocelot_write(ocelot, GENMASK(ocelot->num_phys_ports - 1, 0),
389714d0ffaSVladimir Oltean 		     ANA_VLANMASK);
3907142529fSAntoine Tenart 
3917142529fSAntoine Tenart 	for (port = 0; port < ocelot->num_phys_ports; port++) {
3927142529fSAntoine Tenart 		ocelot_write_gix(ocelot, 0, REW_PORT_VLAN_CFG, port);
3937142529fSAntoine Tenart 		ocelot_write_gix(ocelot, 0, REW_TAG_CFG, port);
3947142529fSAntoine Tenart 	}
395a556c76aSAlexandre Belloni }
396a556c76aSAlexandre Belloni 
397a556c76aSAlexandre Belloni /* Watermark encode
398a556c76aSAlexandre Belloni  * Bit 8:   Unit; 0:1, 1:16
399a556c76aSAlexandre Belloni  * Bit 7-0: Value to be multiplied with unit
400a556c76aSAlexandre Belloni  */
401a556c76aSAlexandre Belloni static u16 ocelot_wm_enc(u16 value)
402a556c76aSAlexandre Belloni {
403a556c76aSAlexandre Belloni 	if (value >= BIT(8))
404a556c76aSAlexandre Belloni 		return BIT(8) | (value / 16);
405a556c76aSAlexandre Belloni 
406a556c76aSAlexandre Belloni 	return value;
407a556c76aSAlexandre Belloni }
408a556c76aSAlexandre Belloni 
4095e256365SVladimir Oltean void ocelot_adjust_link(struct ocelot *ocelot, int port,
41026f4dbabSVladimir Oltean 			struct phy_device *phydev)
411a556c76aSAlexandre Belloni {
41226f4dbabSVladimir Oltean 	struct ocelot_port *ocelot_port = ocelot->ports[port];
4135bc9d2e6SVladimir Oltean 	int speed, mode = 0;
414a556c76aSAlexandre Belloni 
41526f4dbabSVladimir Oltean 	switch (phydev->speed) {
416a556c76aSAlexandre Belloni 	case SPEED_10:
417a556c76aSAlexandre Belloni 		speed = OCELOT_SPEED_10;
418a556c76aSAlexandre Belloni 		break;
419a556c76aSAlexandre Belloni 	case SPEED_100:
420a556c76aSAlexandre Belloni 		speed = OCELOT_SPEED_100;
421a556c76aSAlexandre Belloni 		break;
422a556c76aSAlexandre Belloni 	case SPEED_1000:
423a556c76aSAlexandre Belloni 		speed = OCELOT_SPEED_1000;
424a556c76aSAlexandre Belloni 		mode = DEV_MAC_MODE_CFG_GIGA_MODE_ENA;
425a556c76aSAlexandre Belloni 		break;
426a556c76aSAlexandre Belloni 	case SPEED_2500:
427a556c76aSAlexandre Belloni 		speed = OCELOT_SPEED_2500;
428a556c76aSAlexandre Belloni 		mode = DEV_MAC_MODE_CFG_GIGA_MODE_ENA;
429a556c76aSAlexandre Belloni 		break;
430a556c76aSAlexandre Belloni 	default:
43126f4dbabSVladimir Oltean 		dev_err(ocelot->dev, "Unsupported PHY speed on port %d: %d\n",
43226f4dbabSVladimir Oltean 			port, phydev->speed);
433a556c76aSAlexandre Belloni 		return;
434a556c76aSAlexandre Belloni 	}
435a556c76aSAlexandre Belloni 
43626f4dbabSVladimir Oltean 	phy_print_status(phydev);
437a556c76aSAlexandre Belloni 
43826f4dbabSVladimir Oltean 	if (!phydev->link)
439a556c76aSAlexandre Belloni 		return;
440a556c76aSAlexandre Belloni 
441a556c76aSAlexandre Belloni 	/* Only full duplex supported for now */
442004d44f6SVladimir Oltean 	ocelot_port_writel(ocelot_port, DEV_MAC_MODE_CFG_FDX_ENA |
443a556c76aSAlexandre Belloni 			   mode, DEV_MAC_MODE_CFG);
444a556c76aSAlexandre Belloni 
445dc3de2a2SClaudiu Manoil 	if (ocelot->ops->pcs_init)
446dc3de2a2SClaudiu Manoil 		ocelot->ops->pcs_init(ocelot, port);
447a556c76aSAlexandre Belloni 
448a556c76aSAlexandre Belloni 	/* Enable MAC module */
449004d44f6SVladimir Oltean 	ocelot_port_writel(ocelot_port, DEV_MAC_ENA_CFG_RX_ENA |
450a556c76aSAlexandre Belloni 			   DEV_MAC_ENA_CFG_TX_ENA, DEV_MAC_ENA_CFG);
451a556c76aSAlexandre Belloni 
452a556c76aSAlexandre Belloni 	/* Take MAC, Port, Phy (intern) and PCS (SGMII/Serdes) clock out of
453a556c76aSAlexandre Belloni 	 * reset */
454004d44f6SVladimir Oltean 	ocelot_port_writel(ocelot_port, DEV_CLOCK_CFG_LINK_SPEED(speed),
455a556c76aSAlexandre Belloni 			   DEV_CLOCK_CFG);
456a556c76aSAlexandre Belloni 
457a556c76aSAlexandre Belloni 	/* No PFC */
458a556c76aSAlexandre Belloni 	ocelot_write_gix(ocelot, ANA_PFC_PFC_CFG_FC_LINK_SPEED(speed),
459004d44f6SVladimir Oltean 			 ANA_PFC_PFC_CFG, port);
460a556c76aSAlexandre Belloni 
461a556c76aSAlexandre Belloni 	/* Core: Enable port for frame transfer */
462a556c76aSAlexandre Belloni 	ocelot_write_rix(ocelot, QSYS_SWITCH_PORT_MODE_INGRESS_DROP_MODE |
463a556c76aSAlexandre Belloni 			 QSYS_SWITCH_PORT_MODE_SCH_NEXT_CFG(1) |
464a556c76aSAlexandre Belloni 			 QSYS_SWITCH_PORT_MODE_PORT_ENA,
465004d44f6SVladimir Oltean 			 QSYS_SWITCH_PORT_MODE, port);
466a556c76aSAlexandre Belloni 
467a556c76aSAlexandre Belloni 	/* Flow control */
468a556c76aSAlexandre Belloni 	ocelot_write_rix(ocelot, SYS_MAC_FC_CFG_PAUSE_VAL_CFG(0xffff) |
469a556c76aSAlexandre Belloni 			 SYS_MAC_FC_CFG_RX_FC_ENA | SYS_MAC_FC_CFG_TX_FC_ENA |
470a556c76aSAlexandre Belloni 			 SYS_MAC_FC_CFG_ZERO_PAUSE_ENA |
471a556c76aSAlexandre Belloni 			 SYS_MAC_FC_CFG_FC_LATENCY_CFG(0x7) |
472a556c76aSAlexandre Belloni 			 SYS_MAC_FC_CFG_FC_LINK_SPEED(speed),
473004d44f6SVladimir Oltean 			 SYS_MAC_FC_CFG, port);
474004d44f6SVladimir Oltean 	ocelot_write_rix(ocelot, 0, ANA_POL_FLOWC, port);
475a556c76aSAlexandre Belloni }
4765e256365SVladimir Oltean EXPORT_SYMBOL(ocelot_adjust_link);
477a556c76aSAlexandre Belloni 
47826f4dbabSVladimir Oltean static void ocelot_port_adjust_link(struct net_device *dev)
47926f4dbabSVladimir Oltean {
48026f4dbabSVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(dev);
48126f4dbabSVladimir Oltean 	struct ocelot *ocelot = priv->port.ocelot;
48226f4dbabSVladimir Oltean 	int port = priv->chip_port;
48326f4dbabSVladimir Oltean 
48426f4dbabSVladimir Oltean 	ocelot_adjust_link(ocelot, port, dev->phydev);
48526f4dbabSVladimir Oltean }
48626f4dbabSVladimir Oltean 
4875e256365SVladimir Oltean void ocelot_port_enable(struct ocelot *ocelot, int port,
488889b8950SVladimir Oltean 			struct phy_device *phy)
489a556c76aSAlexandre Belloni {
490a556c76aSAlexandre Belloni 	/* Enable receiving frames on the port, and activate auto-learning of
491a556c76aSAlexandre Belloni 	 * MAC addresses.
492a556c76aSAlexandre Belloni 	 */
493a556c76aSAlexandre Belloni 	ocelot_write_gix(ocelot, ANA_PORT_PORT_CFG_LEARNAUTO |
494a556c76aSAlexandre Belloni 			 ANA_PORT_PORT_CFG_RECV_ENA |
495004d44f6SVladimir Oltean 			 ANA_PORT_PORT_CFG_PORTID_VAL(port),
496004d44f6SVladimir Oltean 			 ANA_PORT_PORT_CFG, port);
497889b8950SVladimir Oltean }
4985e256365SVladimir Oltean EXPORT_SYMBOL(ocelot_port_enable);
499889b8950SVladimir Oltean 
500889b8950SVladimir Oltean static int ocelot_port_open(struct net_device *dev)
501889b8950SVladimir Oltean {
502889b8950SVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(dev);
503889b8950SVladimir Oltean 	struct ocelot *ocelot = priv->port.ocelot;
504889b8950SVladimir Oltean 	int port = priv->chip_port;
505889b8950SVladimir Oltean 	int err;
506a556c76aSAlexandre Belloni 
507004d44f6SVladimir Oltean 	if (priv->serdes) {
508004d44f6SVladimir Oltean 		err = phy_set_mode_ext(priv->serdes, PHY_MODE_ETHERNET,
509004d44f6SVladimir Oltean 				       priv->phy_mode);
51071e32a20SQuentin Schulz 		if (err) {
51171e32a20SQuentin Schulz 			netdev_err(dev, "Could not set mode of SerDes\n");
51271e32a20SQuentin Schulz 			return err;
51371e32a20SQuentin Schulz 		}
51471e32a20SQuentin Schulz 	}
51571e32a20SQuentin Schulz 
516004d44f6SVladimir Oltean 	err = phy_connect_direct(dev, priv->phy, &ocelot_port_adjust_link,
517004d44f6SVladimir Oltean 				 priv->phy_mode);
518a556c76aSAlexandre Belloni 	if (err) {
519a556c76aSAlexandre Belloni 		netdev_err(dev, "Could not attach to PHY\n");
520a556c76aSAlexandre Belloni 		return err;
521a556c76aSAlexandre Belloni 	}
522a556c76aSAlexandre Belloni 
523004d44f6SVladimir Oltean 	dev->phydev = priv->phy;
524a556c76aSAlexandre Belloni 
525004d44f6SVladimir Oltean 	phy_attached_info(priv->phy);
526004d44f6SVladimir Oltean 	phy_start(priv->phy);
527889b8950SVladimir Oltean 
528889b8950SVladimir Oltean 	ocelot_port_enable(ocelot, port, priv->phy);
529889b8950SVladimir Oltean 
530a556c76aSAlexandre Belloni 	return 0;
531a556c76aSAlexandre Belloni }
532a556c76aSAlexandre Belloni 
5335e256365SVladimir Oltean void ocelot_port_disable(struct ocelot *ocelot, int port)
534889b8950SVladimir Oltean {
535889b8950SVladimir Oltean 	struct ocelot_port *ocelot_port = ocelot->ports[port];
536889b8950SVladimir Oltean 
537889b8950SVladimir Oltean 	ocelot_port_writel(ocelot_port, 0, DEV_MAC_ENA_CFG);
538889b8950SVladimir Oltean 	ocelot_rmw_rix(ocelot, 0, QSYS_SWITCH_PORT_MODE_PORT_ENA,
539889b8950SVladimir Oltean 		       QSYS_SWITCH_PORT_MODE, port);
540889b8950SVladimir Oltean }
5415e256365SVladimir Oltean EXPORT_SYMBOL(ocelot_port_disable);
542889b8950SVladimir Oltean 
543a556c76aSAlexandre Belloni static int ocelot_port_stop(struct net_device *dev)
544a556c76aSAlexandre Belloni {
545004d44f6SVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(dev);
546889b8950SVladimir Oltean 	struct ocelot *ocelot = priv->port.ocelot;
547889b8950SVladimir Oltean 	int port = priv->chip_port;
548a556c76aSAlexandre Belloni 
549004d44f6SVladimir Oltean 	phy_disconnect(priv->phy);
550a556c76aSAlexandre Belloni 
551a556c76aSAlexandre Belloni 	dev->phydev = NULL;
552a556c76aSAlexandre Belloni 
553889b8950SVladimir Oltean 	ocelot_port_disable(ocelot, port);
554889b8950SVladimir Oltean 
555a556c76aSAlexandre Belloni 	return 0;
556a556c76aSAlexandre Belloni }
557a556c76aSAlexandre Belloni 
558a556c76aSAlexandre Belloni /* Generate the IFH for frame injection
559a556c76aSAlexandre Belloni  *
560a556c76aSAlexandre Belloni  * The IFH is a 128bit-value
561a556c76aSAlexandre Belloni  * bit 127: bypass the analyzer processing
562a556c76aSAlexandre Belloni  * bit 56-67: destination mask
563a556c76aSAlexandre Belloni  * bit 28-29: pop_cnt: 3 disables all rewriting of the frame
564a556c76aSAlexandre Belloni  * bit 20-27: cpu extraction queue mask
565a556c76aSAlexandre Belloni  * bit 16: tag type 0: C-tag, 1: S-tag
566a556c76aSAlexandre Belloni  * bit 0-11: VID
567a556c76aSAlexandre Belloni  */
568a556c76aSAlexandre Belloni static int ocelot_gen_ifh(u32 *ifh, struct frame_info *info)
569a556c76aSAlexandre Belloni {
5704e3b0468SAntoine Tenart 	ifh[0] = IFH_INJ_BYPASS | ((0x1ff & info->rew_op) << 21);
57108d02364SAntoine Tenart 	ifh[1] = (0xf00 & info->port) >> 8;
572a556c76aSAlexandre Belloni 	ifh[2] = (0xff & info->port) << 24;
57308d02364SAntoine Tenart 	ifh[3] = (info->tag_type << 16) | info->vid;
574a556c76aSAlexandre Belloni 
575a556c76aSAlexandre Belloni 	return 0;
576a556c76aSAlexandre Belloni }
577a556c76aSAlexandre Belloni 
578*400928bfSYangbo Lu int ocelot_port_add_txtstamp_skb(struct ocelot_port *ocelot_port,
579*400928bfSYangbo Lu 				 struct sk_buff *skb)
580*400928bfSYangbo Lu {
581*400928bfSYangbo Lu 	struct skb_shared_info *shinfo = skb_shinfo(skb);
582*400928bfSYangbo Lu 	struct ocelot *ocelot = ocelot_port->ocelot;
583*400928bfSYangbo Lu 
584*400928bfSYangbo Lu 	if (ocelot->ptp && shinfo->tx_flags & SKBTX_HW_TSTAMP &&
585*400928bfSYangbo Lu 	    ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
586*400928bfSYangbo Lu 		struct ocelot_skb *oskb =
587*400928bfSYangbo Lu 			kzalloc(sizeof(struct ocelot_skb), GFP_ATOMIC);
588*400928bfSYangbo Lu 
589*400928bfSYangbo Lu 		if (unlikely(!oskb))
590*400928bfSYangbo Lu 			return -ENOMEM;
591*400928bfSYangbo Lu 
592*400928bfSYangbo Lu 		shinfo->tx_flags |= SKBTX_IN_PROGRESS;
593*400928bfSYangbo Lu 
594*400928bfSYangbo Lu 		oskb->skb = skb;
595*400928bfSYangbo Lu 		oskb->id = ocelot_port->ts_id % 4;
596*400928bfSYangbo Lu 
597*400928bfSYangbo Lu 		list_add_tail(&oskb->head, &ocelot_port->skbs);
598*400928bfSYangbo Lu 		return 0;
599*400928bfSYangbo Lu 	}
600*400928bfSYangbo Lu 	return -ENODATA;
601*400928bfSYangbo Lu }
602*400928bfSYangbo Lu EXPORT_SYMBOL(ocelot_port_add_txtstamp_skb);
603*400928bfSYangbo Lu 
604a556c76aSAlexandre Belloni static int ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
605a556c76aSAlexandre Belloni {
606004d44f6SVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(dev);
6074e3b0468SAntoine Tenart 	struct skb_shared_info *shinfo = skb_shinfo(skb);
608004d44f6SVladimir Oltean 	struct ocelot_port *ocelot_port = &priv->port;
609004d44f6SVladimir Oltean 	struct ocelot *ocelot = ocelot_port->ocelot;
610f24711fdSVladimir Oltean 	u32 val, ifh[OCELOT_TAG_LEN / 4];
611a556c76aSAlexandre Belloni 	struct frame_info info = {};
612a556c76aSAlexandre Belloni 	u8 grp = 0; /* Send everything on CPU group 0 */
613a556c76aSAlexandre Belloni 	unsigned int i, count, last;
614004d44f6SVladimir Oltean 	int port = priv->chip_port;
615a556c76aSAlexandre Belloni 
616a556c76aSAlexandre Belloni 	val = ocelot_read(ocelot, QS_INJ_STATUS);
617a556c76aSAlexandre Belloni 	if (!(val & QS_INJ_STATUS_FIFO_RDY(BIT(grp))) ||
618a556c76aSAlexandre Belloni 	    (val & QS_INJ_STATUS_WMARK_REACHED(BIT(grp))))
619a556c76aSAlexandre Belloni 		return NETDEV_TX_BUSY;
620a556c76aSAlexandre Belloni 
621a556c76aSAlexandre Belloni 	ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
622a556c76aSAlexandre Belloni 			 QS_INJ_CTRL_SOF, QS_INJ_CTRL, grp);
623a556c76aSAlexandre Belloni 
624004d44f6SVladimir Oltean 	info.port = BIT(port);
62508d02364SAntoine Tenart 	info.tag_type = IFH_TAG_TYPE_C;
62608d02364SAntoine Tenart 	info.vid = skb_vlan_tag_get(skb);
6274e3b0468SAntoine Tenart 
6284e3b0468SAntoine Tenart 	/* Check if timestamping is needed */
6294e3b0468SAntoine Tenart 	if (ocelot->ptp && shinfo->tx_flags & SKBTX_HW_TSTAMP) {
630004d44f6SVladimir Oltean 		info.rew_op = ocelot_port->ptp_cmd;
631004d44f6SVladimir Oltean 		if (ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP)
632004d44f6SVladimir Oltean 			info.rew_op |= (ocelot_port->ts_id  % 4) << 3;
6334e3b0468SAntoine Tenart 	}
6344e3b0468SAntoine Tenart 
635a556c76aSAlexandre Belloni 	ocelot_gen_ifh(ifh, &info);
636a556c76aSAlexandre Belloni 
637f24711fdSVladimir Oltean 	for (i = 0; i < OCELOT_TAG_LEN / 4; i++)
638c2cd650bSAntoine Tenart 		ocelot_write_rix(ocelot, (__force u32)cpu_to_be32(ifh[i]),
639c2cd650bSAntoine Tenart 				 QS_INJ_WR, grp);
640a556c76aSAlexandre Belloni 
641a556c76aSAlexandre Belloni 	count = (skb->len + 3) / 4;
642a556c76aSAlexandre Belloni 	last = skb->len % 4;
643a556c76aSAlexandre Belloni 	for (i = 0; i < count; i++) {
644a556c76aSAlexandre Belloni 		ocelot_write_rix(ocelot, ((u32 *)skb->data)[i], QS_INJ_WR, grp);
645a556c76aSAlexandre Belloni 	}
646a556c76aSAlexandre Belloni 
647a556c76aSAlexandre Belloni 	/* Add padding */
648a556c76aSAlexandre Belloni 	while (i < (OCELOT_BUFFER_CELL_SZ / 4)) {
649a556c76aSAlexandre Belloni 		ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
650a556c76aSAlexandre Belloni 		i++;
651a556c76aSAlexandre Belloni 	}
652a556c76aSAlexandre Belloni 
653a556c76aSAlexandre Belloni 	/* Indicate EOF and valid bytes in last word */
654a556c76aSAlexandre Belloni 	ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
655a556c76aSAlexandre Belloni 			 QS_INJ_CTRL_VLD_BYTES(skb->len < OCELOT_BUFFER_CELL_SZ ? 0 : last) |
656a556c76aSAlexandre Belloni 			 QS_INJ_CTRL_EOF,
657a556c76aSAlexandre Belloni 			 QS_INJ_CTRL, grp);
658a556c76aSAlexandre Belloni 
659a556c76aSAlexandre Belloni 	/* Add dummy CRC */
660a556c76aSAlexandre Belloni 	ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
661a556c76aSAlexandre Belloni 	skb_tx_timestamp(skb);
662a556c76aSAlexandre Belloni 
663a556c76aSAlexandre Belloni 	dev->stats.tx_packets++;
664a556c76aSAlexandre Belloni 	dev->stats.tx_bytes += skb->len;
6654e3b0468SAntoine Tenart 
666*400928bfSYangbo Lu 	if (!ocelot_port_add_txtstamp_skb(ocelot_port, skb)) {
667004d44f6SVladimir Oltean 		ocelot_port->ts_id++;
668a556c76aSAlexandre Belloni 		return NETDEV_TX_OK;
669a556c76aSAlexandre Belloni 	}
670a556c76aSAlexandre Belloni 
6714e3b0468SAntoine Tenart 	dev_kfree_skb_any(skb);
6724e3b0468SAntoine Tenart 	return NETDEV_TX_OK;
6734e3b0468SAntoine Tenart }
6744e3b0468SAntoine Tenart 
675e23a7b3eSYangbo Lu static void ocelot_get_hwtimestamp(struct ocelot *ocelot,
676e23a7b3eSYangbo Lu 				   struct timespec64 *ts)
6774e3b0468SAntoine Tenart {
6784e3b0468SAntoine Tenart 	unsigned long flags;
6794e3b0468SAntoine Tenart 	u32 val;
6804e3b0468SAntoine Tenart 
6814e3b0468SAntoine Tenart 	spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
6824e3b0468SAntoine Tenart 
6834e3b0468SAntoine Tenart 	/* Read current PTP time to get seconds */
6844e3b0468SAntoine Tenart 	val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
6854e3b0468SAntoine Tenart 
6864e3b0468SAntoine Tenart 	val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
6874e3b0468SAntoine Tenart 	val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE);
6884e3b0468SAntoine Tenart 	ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
6894e3b0468SAntoine Tenart 	ts->tv_sec = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
6904e3b0468SAntoine Tenart 
6914e3b0468SAntoine Tenart 	/* Read packet HW timestamp from FIFO */
6924e3b0468SAntoine Tenart 	val = ocelot_read(ocelot, SYS_PTP_TXSTAMP);
6934e3b0468SAntoine Tenart 	ts->tv_nsec = SYS_PTP_TXSTAMP_PTP_TXSTAMP(val);
6944e3b0468SAntoine Tenart 
6954e3b0468SAntoine Tenart 	/* Sec has incremented since the ts was registered */
6964e3b0468SAntoine Tenart 	if ((ts->tv_sec & 0x1) != !!(val & SYS_PTP_TXSTAMP_PTP_TXSTAMP_SEC))
6974e3b0468SAntoine Tenart 		ts->tv_sec--;
6984e3b0468SAntoine Tenart 
6994e3b0468SAntoine Tenart 	spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
7004e3b0468SAntoine Tenart }
701e23a7b3eSYangbo Lu 
702e23a7b3eSYangbo Lu void ocelot_get_txtstamp(struct ocelot *ocelot)
703e23a7b3eSYangbo Lu {
704e23a7b3eSYangbo Lu 	int budget = OCELOT_PTP_QUEUE_SZ;
705e23a7b3eSYangbo Lu 
706e23a7b3eSYangbo Lu 	while (budget--) {
707e23a7b3eSYangbo Lu 		struct skb_shared_hwtstamps shhwtstamps;
708e23a7b3eSYangbo Lu 		struct list_head *pos, *tmp;
709e23a7b3eSYangbo Lu 		struct sk_buff *skb = NULL;
710e23a7b3eSYangbo Lu 		struct ocelot_skb *entry;
711e23a7b3eSYangbo Lu 		struct ocelot_port *port;
712e23a7b3eSYangbo Lu 		struct timespec64 ts;
713e23a7b3eSYangbo Lu 		u32 val, id, txport;
714e23a7b3eSYangbo Lu 
715e23a7b3eSYangbo Lu 		val = ocelot_read(ocelot, SYS_PTP_STATUS);
716e23a7b3eSYangbo Lu 
717e23a7b3eSYangbo Lu 		/* Check if a timestamp can be retrieved */
718e23a7b3eSYangbo Lu 		if (!(val & SYS_PTP_STATUS_PTP_MESS_VLD))
719e23a7b3eSYangbo Lu 			break;
720e23a7b3eSYangbo Lu 
721e23a7b3eSYangbo Lu 		WARN_ON(val & SYS_PTP_STATUS_PTP_OVFL);
722e23a7b3eSYangbo Lu 
723e23a7b3eSYangbo Lu 		/* Retrieve the ts ID and Tx port */
724e23a7b3eSYangbo Lu 		id = SYS_PTP_STATUS_PTP_MESS_ID_X(val);
725e23a7b3eSYangbo Lu 		txport = SYS_PTP_STATUS_PTP_MESS_TXPORT_X(val);
726e23a7b3eSYangbo Lu 
727e23a7b3eSYangbo Lu 		/* Retrieve its associated skb */
728e23a7b3eSYangbo Lu 		port = ocelot->ports[txport];
729e23a7b3eSYangbo Lu 
730e23a7b3eSYangbo Lu 		list_for_each_safe(pos, tmp, &port->skbs) {
731e23a7b3eSYangbo Lu 			entry = list_entry(pos, struct ocelot_skb, head);
732e23a7b3eSYangbo Lu 			if (entry->id != id)
733e23a7b3eSYangbo Lu 				continue;
734e23a7b3eSYangbo Lu 
735e23a7b3eSYangbo Lu 			skb = entry->skb;
736e23a7b3eSYangbo Lu 
737e23a7b3eSYangbo Lu 			list_del(pos);
738e23a7b3eSYangbo Lu 			kfree(entry);
739e23a7b3eSYangbo Lu 		}
740e23a7b3eSYangbo Lu 
741e23a7b3eSYangbo Lu 		/* Next ts */
742e23a7b3eSYangbo Lu 		ocelot_write(ocelot, SYS_PTP_NXT_PTP_NXT, SYS_PTP_NXT);
743e23a7b3eSYangbo Lu 
744e23a7b3eSYangbo Lu 		if (unlikely(!skb))
745e23a7b3eSYangbo Lu 			continue;
746e23a7b3eSYangbo Lu 
747e23a7b3eSYangbo Lu 		/* Get the h/w timestamp */
748e23a7b3eSYangbo Lu 		ocelot_get_hwtimestamp(ocelot, &ts);
749e23a7b3eSYangbo Lu 
750e23a7b3eSYangbo Lu 		/* Set the timestamp into the skb */
751e23a7b3eSYangbo Lu 		memset(&shhwtstamps, 0, sizeof(shhwtstamps));
752e23a7b3eSYangbo Lu 		shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
753e23a7b3eSYangbo Lu 		skb_tstamp_tx(skb, &shhwtstamps);
754e23a7b3eSYangbo Lu 
755e23a7b3eSYangbo Lu 		dev_kfree_skb_any(skb);
756e23a7b3eSYangbo Lu 	}
757e23a7b3eSYangbo Lu }
758e23a7b3eSYangbo Lu EXPORT_SYMBOL(ocelot_get_txtstamp);
7594e3b0468SAntoine Tenart 
76040a1578dSClaudiu Manoil static int ocelot_mc_unsync(struct net_device *dev, const unsigned char *addr)
761a556c76aSAlexandre Belloni {
762004d44f6SVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(dev);
763004d44f6SVladimir Oltean 	struct ocelot_port *ocelot_port = &priv->port;
764004d44f6SVladimir Oltean 	struct ocelot *ocelot = ocelot_port->ocelot;
765a556c76aSAlexandre Belloni 
766004d44f6SVladimir Oltean 	return ocelot_mact_forget(ocelot, addr, ocelot_port->pvid);
767a556c76aSAlexandre Belloni }
768a556c76aSAlexandre Belloni 
76940a1578dSClaudiu Manoil static int ocelot_mc_sync(struct net_device *dev, const unsigned char *addr)
770a556c76aSAlexandre Belloni {
771004d44f6SVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(dev);
772004d44f6SVladimir Oltean 	struct ocelot_port *ocelot_port = &priv->port;
773004d44f6SVladimir Oltean 	struct ocelot *ocelot = ocelot_port->ocelot;
774a556c76aSAlexandre Belloni 
775004d44f6SVladimir Oltean 	return ocelot_mact_learn(ocelot, PGID_CPU, addr, ocelot_port->pvid,
776a556c76aSAlexandre Belloni 				 ENTRYTYPE_LOCKED);
777a556c76aSAlexandre Belloni }
778a556c76aSAlexandre Belloni 
779a556c76aSAlexandre Belloni static void ocelot_set_rx_mode(struct net_device *dev)
780a556c76aSAlexandre Belloni {
781004d44f6SVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(dev);
782004d44f6SVladimir Oltean 	struct ocelot *ocelot = priv->port.ocelot;
783a556c76aSAlexandre Belloni 	u32 val;
784004d44f6SVladimir Oltean 	int i;
785a556c76aSAlexandre Belloni 
786a556c76aSAlexandre Belloni 	/* This doesn't handle promiscuous mode because the bridge core is
787a556c76aSAlexandre Belloni 	 * setting IFF_PROMISC on all slave interfaces and all frames would be
788a556c76aSAlexandre Belloni 	 * forwarded to the CPU port.
789a556c76aSAlexandre Belloni 	 */
790a556c76aSAlexandre Belloni 	val = GENMASK(ocelot->num_phys_ports - 1, 0);
791a556c76aSAlexandre Belloni 	for (i = ocelot->num_phys_ports + 1; i < PGID_CPU; i++)
792a556c76aSAlexandre Belloni 		ocelot_write_rix(ocelot, val, ANA_PGID_PGID, i);
793a556c76aSAlexandre Belloni 
79440a1578dSClaudiu Manoil 	__dev_mc_sync(dev, ocelot_mc_sync, ocelot_mc_unsync);
795a556c76aSAlexandre Belloni }
796a556c76aSAlexandre Belloni 
797a556c76aSAlexandre Belloni static int ocelot_port_get_phys_port_name(struct net_device *dev,
798a556c76aSAlexandre Belloni 					  char *buf, size_t len)
799a556c76aSAlexandre Belloni {
800004d44f6SVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(dev);
801004d44f6SVladimir Oltean 	int port = priv->chip_port;
802a556c76aSAlexandre Belloni 	int ret;
803a556c76aSAlexandre Belloni 
804004d44f6SVladimir Oltean 	ret = snprintf(buf, len, "p%d", port);
805a556c76aSAlexandre Belloni 	if (ret >= len)
806a556c76aSAlexandre Belloni 		return -EINVAL;
807a556c76aSAlexandre Belloni 
808a556c76aSAlexandre Belloni 	return 0;
809a556c76aSAlexandre Belloni }
810a556c76aSAlexandre Belloni 
811a556c76aSAlexandre Belloni static int ocelot_port_set_mac_address(struct net_device *dev, void *p)
812a556c76aSAlexandre Belloni {
813004d44f6SVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(dev);
814004d44f6SVladimir Oltean 	struct ocelot_port *ocelot_port = &priv->port;
815004d44f6SVladimir Oltean 	struct ocelot *ocelot = ocelot_port->ocelot;
816a556c76aSAlexandre Belloni 	const struct sockaddr *addr = p;
817a556c76aSAlexandre Belloni 
818a556c76aSAlexandre Belloni 	/* Learn the new net device MAC address in the mac table. */
819004d44f6SVladimir Oltean 	ocelot_mact_learn(ocelot, PGID_CPU, addr->sa_data, ocelot_port->pvid,
820a556c76aSAlexandre Belloni 			  ENTRYTYPE_LOCKED);
821a556c76aSAlexandre Belloni 	/* Then forget the previous one. */
822004d44f6SVladimir Oltean 	ocelot_mact_forget(ocelot, dev->dev_addr, ocelot_port->pvid);
823a556c76aSAlexandre Belloni 
824a556c76aSAlexandre Belloni 	ether_addr_copy(dev->dev_addr, addr->sa_data);
825a556c76aSAlexandre Belloni 	return 0;
826a556c76aSAlexandre Belloni }
827a556c76aSAlexandre Belloni 
828a556c76aSAlexandre Belloni static void ocelot_get_stats64(struct net_device *dev,
829a556c76aSAlexandre Belloni 			       struct rtnl_link_stats64 *stats)
830a556c76aSAlexandre Belloni {
831004d44f6SVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(dev);
832004d44f6SVladimir Oltean 	struct ocelot *ocelot = priv->port.ocelot;
833004d44f6SVladimir Oltean 	int port = priv->chip_port;
834a556c76aSAlexandre Belloni 
835a556c76aSAlexandre Belloni 	/* Configure the port to read the stats from */
836004d44f6SVladimir Oltean 	ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(port),
837a556c76aSAlexandre Belloni 		     SYS_STAT_CFG);
838a556c76aSAlexandre Belloni 
839a556c76aSAlexandre Belloni 	/* Get Rx stats */
840a556c76aSAlexandre Belloni 	stats->rx_bytes = ocelot_read(ocelot, SYS_COUNT_RX_OCTETS);
841a556c76aSAlexandre Belloni 	stats->rx_packets = ocelot_read(ocelot, SYS_COUNT_RX_SHORTS) +
842a556c76aSAlexandre Belloni 			    ocelot_read(ocelot, SYS_COUNT_RX_FRAGMENTS) +
843a556c76aSAlexandre Belloni 			    ocelot_read(ocelot, SYS_COUNT_RX_JABBERS) +
844a556c76aSAlexandre Belloni 			    ocelot_read(ocelot, SYS_COUNT_RX_LONGS) +
845a556c76aSAlexandre Belloni 			    ocelot_read(ocelot, SYS_COUNT_RX_64) +
846a556c76aSAlexandre Belloni 			    ocelot_read(ocelot, SYS_COUNT_RX_65_127) +
847a556c76aSAlexandre Belloni 			    ocelot_read(ocelot, SYS_COUNT_RX_128_255) +
848a556c76aSAlexandre Belloni 			    ocelot_read(ocelot, SYS_COUNT_RX_256_1023) +
849a556c76aSAlexandre Belloni 			    ocelot_read(ocelot, SYS_COUNT_RX_1024_1526) +
850a556c76aSAlexandre Belloni 			    ocelot_read(ocelot, SYS_COUNT_RX_1527_MAX);
851a556c76aSAlexandre Belloni 	stats->multicast = ocelot_read(ocelot, SYS_COUNT_RX_MULTICAST);
852a556c76aSAlexandre Belloni 	stats->rx_dropped = dev->stats.rx_dropped;
853a556c76aSAlexandre Belloni 
854a556c76aSAlexandre Belloni 	/* Get Tx stats */
855a556c76aSAlexandre Belloni 	stats->tx_bytes = ocelot_read(ocelot, SYS_COUNT_TX_OCTETS);
856a556c76aSAlexandre Belloni 	stats->tx_packets = ocelot_read(ocelot, SYS_COUNT_TX_64) +
857a556c76aSAlexandre Belloni 			    ocelot_read(ocelot, SYS_COUNT_TX_65_127) +
858a556c76aSAlexandre Belloni 			    ocelot_read(ocelot, SYS_COUNT_TX_128_511) +
859a556c76aSAlexandre Belloni 			    ocelot_read(ocelot, SYS_COUNT_TX_512_1023) +
860a556c76aSAlexandre Belloni 			    ocelot_read(ocelot, SYS_COUNT_TX_1024_1526) +
861a556c76aSAlexandre Belloni 			    ocelot_read(ocelot, SYS_COUNT_TX_1527_MAX);
862a556c76aSAlexandre Belloni 	stats->tx_dropped = ocelot_read(ocelot, SYS_COUNT_TX_DROPS) +
863a556c76aSAlexandre Belloni 			    ocelot_read(ocelot, SYS_COUNT_TX_AGING);
864a556c76aSAlexandre Belloni 	stats->collisions = ocelot_read(ocelot, SYS_COUNT_TX_COLLISION);
865a556c76aSAlexandre Belloni }
866a556c76aSAlexandre Belloni 
8675e256365SVladimir Oltean int ocelot_fdb_add(struct ocelot *ocelot, int port,
8685e256365SVladimir Oltean 		   const unsigned char *addr, u16 vid, bool vlan_aware)
869a556c76aSAlexandre Belloni {
870531ee1a6SVladimir Oltean 	struct ocelot_port *ocelot_port = ocelot->ports[port];
871a556c76aSAlexandre Belloni 
8727142529fSAntoine Tenart 	if (!vid) {
873004d44f6SVladimir Oltean 		if (!vlan_aware)
8747142529fSAntoine Tenart 			/* If the bridge is not VLAN aware and no VID was
8757142529fSAntoine Tenart 			 * provided, set it to pvid to ensure the MAC entry
8767142529fSAntoine Tenart 			 * matches incoming untagged packets
8777142529fSAntoine Tenart 			 */
878531ee1a6SVladimir Oltean 			vid = ocelot_port->pvid;
8797142529fSAntoine Tenart 		else
8807142529fSAntoine Tenart 			/* If the bridge is VLAN aware a VID must be provided as
8817142529fSAntoine Tenart 			 * otherwise the learnt entry wouldn't match any frame.
8827142529fSAntoine Tenart 			 */
8837142529fSAntoine Tenart 			return -EINVAL;
8847142529fSAntoine Tenart 	}
8857142529fSAntoine Tenart 
886531ee1a6SVladimir Oltean 	return ocelot_mact_learn(ocelot, port, addr, vid, ENTRYTYPE_LOCKED);
887a556c76aSAlexandre Belloni }
8885e256365SVladimir Oltean EXPORT_SYMBOL(ocelot_fdb_add);
889a556c76aSAlexandre Belloni 
890531ee1a6SVladimir Oltean static int ocelot_port_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
891531ee1a6SVladimir Oltean 			       struct net_device *dev,
892531ee1a6SVladimir Oltean 			       const unsigned char *addr,
893531ee1a6SVladimir Oltean 			       u16 vid, u16 flags,
894531ee1a6SVladimir Oltean 			       struct netlink_ext_ack *extack)
895531ee1a6SVladimir Oltean {
896004d44f6SVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(dev);
897004d44f6SVladimir Oltean 	struct ocelot *ocelot = priv->port.ocelot;
898004d44f6SVladimir Oltean 	int port = priv->chip_port;
899531ee1a6SVladimir Oltean 
900004d44f6SVladimir Oltean 	return ocelot_fdb_add(ocelot, port, addr, vid, priv->vlan_aware);
901531ee1a6SVladimir Oltean }
902531ee1a6SVladimir Oltean 
9035e256365SVladimir Oltean int ocelot_fdb_del(struct ocelot *ocelot, int port,
904531ee1a6SVladimir Oltean 		   const unsigned char *addr, u16 vid)
905531ee1a6SVladimir Oltean {
906531ee1a6SVladimir Oltean 	return ocelot_mact_forget(ocelot, addr, vid);
907531ee1a6SVladimir Oltean }
9085e256365SVladimir Oltean EXPORT_SYMBOL(ocelot_fdb_del);
909531ee1a6SVladimir Oltean 
910531ee1a6SVladimir Oltean static int ocelot_port_fdb_del(struct ndmsg *ndm, struct nlattr *tb[],
911a556c76aSAlexandre Belloni 			       struct net_device *dev,
912a556c76aSAlexandre Belloni 			       const unsigned char *addr, u16 vid)
913a556c76aSAlexandre Belloni {
914004d44f6SVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(dev);
915004d44f6SVladimir Oltean 	struct ocelot *ocelot = priv->port.ocelot;
916004d44f6SVladimir Oltean 	int port = priv->chip_port;
917a556c76aSAlexandre Belloni 
918004d44f6SVladimir Oltean 	return ocelot_fdb_del(ocelot, port, addr, vid);
919a556c76aSAlexandre Belloni }
920a556c76aSAlexandre Belloni 
921a556c76aSAlexandre Belloni struct ocelot_dump_ctx {
922a556c76aSAlexandre Belloni 	struct net_device *dev;
923a556c76aSAlexandre Belloni 	struct sk_buff *skb;
924a556c76aSAlexandre Belloni 	struct netlink_callback *cb;
925a556c76aSAlexandre Belloni 	int idx;
926a556c76aSAlexandre Belloni };
927a556c76aSAlexandre Belloni 
928531ee1a6SVladimir Oltean static int ocelot_port_fdb_do_dump(const unsigned char *addr, u16 vid,
929531ee1a6SVladimir Oltean 				   bool is_static, void *data)
930a556c76aSAlexandre Belloni {
931531ee1a6SVladimir Oltean 	struct ocelot_dump_ctx *dump = data;
932a556c76aSAlexandre Belloni 	u32 portid = NETLINK_CB(dump->cb->skb).portid;
933a556c76aSAlexandre Belloni 	u32 seq = dump->cb->nlh->nlmsg_seq;
934a556c76aSAlexandre Belloni 	struct nlmsghdr *nlh;
935a556c76aSAlexandre Belloni 	struct ndmsg *ndm;
936a556c76aSAlexandre Belloni 
937a556c76aSAlexandre Belloni 	if (dump->idx < dump->cb->args[2])
938a556c76aSAlexandre Belloni 		goto skip;
939a556c76aSAlexandre Belloni 
940a556c76aSAlexandre Belloni 	nlh = nlmsg_put(dump->skb, portid, seq, RTM_NEWNEIGH,
941a556c76aSAlexandre Belloni 			sizeof(*ndm), NLM_F_MULTI);
942a556c76aSAlexandre Belloni 	if (!nlh)
943a556c76aSAlexandre Belloni 		return -EMSGSIZE;
944a556c76aSAlexandre Belloni 
945a556c76aSAlexandre Belloni 	ndm = nlmsg_data(nlh);
946a556c76aSAlexandre Belloni 	ndm->ndm_family  = AF_BRIDGE;
947a556c76aSAlexandre Belloni 	ndm->ndm_pad1    = 0;
948a556c76aSAlexandre Belloni 	ndm->ndm_pad2    = 0;
949a556c76aSAlexandre Belloni 	ndm->ndm_flags   = NTF_SELF;
950a556c76aSAlexandre Belloni 	ndm->ndm_type    = 0;
951a556c76aSAlexandre Belloni 	ndm->ndm_ifindex = dump->dev->ifindex;
952531ee1a6SVladimir Oltean 	ndm->ndm_state   = is_static ? NUD_NOARP : NUD_REACHABLE;
953a556c76aSAlexandre Belloni 
954531ee1a6SVladimir Oltean 	if (nla_put(dump->skb, NDA_LLADDR, ETH_ALEN, addr))
955a556c76aSAlexandre Belloni 		goto nla_put_failure;
956a556c76aSAlexandre Belloni 
957531ee1a6SVladimir Oltean 	if (vid && nla_put_u16(dump->skb, NDA_VLAN, vid))
958a556c76aSAlexandre Belloni 		goto nla_put_failure;
959a556c76aSAlexandre Belloni 
960a556c76aSAlexandre Belloni 	nlmsg_end(dump->skb, nlh);
961a556c76aSAlexandre Belloni 
962a556c76aSAlexandre Belloni skip:
963a556c76aSAlexandre Belloni 	dump->idx++;
964a556c76aSAlexandre Belloni 	return 0;
965a556c76aSAlexandre Belloni 
966a556c76aSAlexandre Belloni nla_put_failure:
967a556c76aSAlexandre Belloni 	nlmsg_cancel(dump->skb, nlh);
968a556c76aSAlexandre Belloni 	return -EMSGSIZE;
969a556c76aSAlexandre Belloni }
970a556c76aSAlexandre Belloni 
971531ee1a6SVladimir Oltean static int ocelot_mact_read(struct ocelot *ocelot, int port, int row, int col,
972a556c76aSAlexandre Belloni 			    struct ocelot_mact_entry *entry)
973a556c76aSAlexandre Belloni {
974a556c76aSAlexandre Belloni 	u32 val, dst, macl, mach;
975531ee1a6SVladimir Oltean 	char mac[ETH_ALEN];
976a556c76aSAlexandre Belloni 
977a556c76aSAlexandre Belloni 	/* Set row and column to read from */
978a556c76aSAlexandre Belloni 	ocelot_field_write(ocelot, ANA_TABLES_MACTINDX_M_INDEX, row);
979a556c76aSAlexandre Belloni 	ocelot_field_write(ocelot, ANA_TABLES_MACTINDX_BUCKET, col);
980a556c76aSAlexandre Belloni 
981a556c76aSAlexandre Belloni 	/* Issue a read command */
982a556c76aSAlexandre Belloni 	ocelot_write(ocelot,
983a556c76aSAlexandre Belloni 		     ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_READ),
984a556c76aSAlexandre Belloni 		     ANA_TABLES_MACACCESS);
985a556c76aSAlexandre Belloni 
986a556c76aSAlexandre Belloni 	if (ocelot_mact_wait_for_completion(ocelot))
987a556c76aSAlexandre Belloni 		return -ETIMEDOUT;
988a556c76aSAlexandre Belloni 
989a556c76aSAlexandre Belloni 	/* Read the entry flags */
990a556c76aSAlexandre Belloni 	val = ocelot_read(ocelot, ANA_TABLES_MACACCESS);
991a556c76aSAlexandre Belloni 	if (!(val & ANA_TABLES_MACACCESS_VALID))
992a556c76aSAlexandre Belloni 		return -EINVAL;
993a556c76aSAlexandre Belloni 
994a556c76aSAlexandre Belloni 	/* If the entry read has another port configured as its destination,
995a556c76aSAlexandre Belloni 	 * do not report it.
996a556c76aSAlexandre Belloni 	 */
997a556c76aSAlexandre Belloni 	dst = (val & ANA_TABLES_MACACCESS_DEST_IDX_M) >> 3;
998531ee1a6SVladimir Oltean 	if (dst != port)
999a556c76aSAlexandre Belloni 		return -EINVAL;
1000a556c76aSAlexandre Belloni 
1001a556c76aSAlexandre Belloni 	/* Get the entry's MAC address and VLAN id */
1002a556c76aSAlexandre Belloni 	macl = ocelot_read(ocelot, ANA_TABLES_MACLDATA);
1003a556c76aSAlexandre Belloni 	mach = ocelot_read(ocelot, ANA_TABLES_MACHDATA);
1004a556c76aSAlexandre Belloni 
1005a556c76aSAlexandre Belloni 	mac[0] = (mach >> 8)  & 0xff;
1006a556c76aSAlexandre Belloni 	mac[1] = (mach >> 0)  & 0xff;
1007a556c76aSAlexandre Belloni 	mac[2] = (macl >> 24) & 0xff;
1008a556c76aSAlexandre Belloni 	mac[3] = (macl >> 16) & 0xff;
1009a556c76aSAlexandre Belloni 	mac[4] = (macl >> 8)  & 0xff;
1010a556c76aSAlexandre Belloni 	mac[5] = (macl >> 0)  & 0xff;
1011a556c76aSAlexandre Belloni 
1012a556c76aSAlexandre Belloni 	entry->vid = (mach >> 16) & 0xfff;
1013a556c76aSAlexandre Belloni 	ether_addr_copy(entry->mac, mac);
1014a556c76aSAlexandre Belloni 
1015a556c76aSAlexandre Belloni 	return 0;
1016a556c76aSAlexandre Belloni }
1017a556c76aSAlexandre Belloni 
10185e256365SVladimir Oltean int ocelot_fdb_dump(struct ocelot *ocelot, int port,
1019531ee1a6SVladimir Oltean 		    dsa_fdb_dump_cb_t *cb, void *data)
1020a556c76aSAlexandre Belloni {
1021531ee1a6SVladimir Oltean 	int i, j;
1022a556c76aSAlexandre Belloni 
1023a556c76aSAlexandre Belloni 	/* Loop through all the mac tables entries. There are 1024 rows of 4
1024a556c76aSAlexandre Belloni 	 * entries.
1025a556c76aSAlexandre Belloni 	 */
1026a556c76aSAlexandre Belloni 	for (i = 0; i < 1024; i++) {
1027a556c76aSAlexandre Belloni 		for (j = 0; j < 4; j++) {
1028531ee1a6SVladimir Oltean 			struct ocelot_mact_entry entry;
1029531ee1a6SVladimir Oltean 			bool is_static;
1030531ee1a6SVladimir Oltean 			int ret;
1031531ee1a6SVladimir Oltean 
1032531ee1a6SVladimir Oltean 			ret = ocelot_mact_read(ocelot, port, i, j, &entry);
1033a556c76aSAlexandre Belloni 			/* If the entry is invalid (wrong port, invalid...),
1034a556c76aSAlexandre Belloni 			 * skip it.
1035a556c76aSAlexandre Belloni 			 */
1036a556c76aSAlexandre Belloni 			if (ret == -EINVAL)
1037a556c76aSAlexandre Belloni 				continue;
1038a556c76aSAlexandre Belloni 			else if (ret)
1039531ee1a6SVladimir Oltean 				return ret;
1040a556c76aSAlexandre Belloni 
1041531ee1a6SVladimir Oltean 			is_static = (entry.type == ENTRYTYPE_LOCKED);
1042531ee1a6SVladimir Oltean 
1043531ee1a6SVladimir Oltean 			ret = cb(entry.mac, entry.vid, is_static, data);
1044a556c76aSAlexandre Belloni 			if (ret)
1045531ee1a6SVladimir Oltean 				return ret;
1046a556c76aSAlexandre Belloni 		}
1047a556c76aSAlexandre Belloni 	}
1048a556c76aSAlexandre Belloni 
1049531ee1a6SVladimir Oltean 	return 0;
1050531ee1a6SVladimir Oltean }
10515e256365SVladimir Oltean EXPORT_SYMBOL(ocelot_fdb_dump);
1052531ee1a6SVladimir Oltean 
1053531ee1a6SVladimir Oltean static int ocelot_port_fdb_dump(struct sk_buff *skb,
1054531ee1a6SVladimir Oltean 				struct netlink_callback *cb,
1055531ee1a6SVladimir Oltean 				struct net_device *dev,
1056531ee1a6SVladimir Oltean 				struct net_device *filter_dev, int *idx)
1057531ee1a6SVladimir Oltean {
1058004d44f6SVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(dev);
1059004d44f6SVladimir Oltean 	struct ocelot *ocelot = priv->port.ocelot;
1060531ee1a6SVladimir Oltean 	struct ocelot_dump_ctx dump = {
1061531ee1a6SVladimir Oltean 		.dev = dev,
1062531ee1a6SVladimir Oltean 		.skb = skb,
1063531ee1a6SVladimir Oltean 		.cb = cb,
1064531ee1a6SVladimir Oltean 		.idx = *idx,
1065531ee1a6SVladimir Oltean 	};
1066004d44f6SVladimir Oltean 	int port = priv->chip_port;
1067531ee1a6SVladimir Oltean 	int ret;
1068531ee1a6SVladimir Oltean 
1069004d44f6SVladimir Oltean 	ret = ocelot_fdb_dump(ocelot, port, ocelot_port_fdb_do_dump, &dump);
1070531ee1a6SVladimir Oltean 
1071a556c76aSAlexandre Belloni 	*idx = dump.idx;
1072531ee1a6SVladimir Oltean 
1073a556c76aSAlexandre Belloni 	return ret;
1074a556c76aSAlexandre Belloni }
1075a556c76aSAlexandre Belloni 
10767142529fSAntoine Tenart static int ocelot_vlan_rx_add_vid(struct net_device *dev, __be16 proto,
10777142529fSAntoine Tenart 				  u16 vid)
10787142529fSAntoine Tenart {
10791c44ce56SVladimir Oltean 	return ocelot_vlan_vid_add(dev, vid, false, false);
10807142529fSAntoine Tenart }
10817142529fSAntoine Tenart 
10827142529fSAntoine Tenart static int ocelot_vlan_rx_kill_vid(struct net_device *dev, __be16 proto,
10837142529fSAntoine Tenart 				   u16 vid)
10847142529fSAntoine Tenart {
10857142529fSAntoine Tenart 	return ocelot_vlan_vid_del(dev, vid);
10867142529fSAntoine Tenart }
10877142529fSAntoine Tenart 
10887142529fSAntoine Tenart static int ocelot_set_features(struct net_device *dev,
10897142529fSAntoine Tenart 			       netdev_features_t features)
10907142529fSAntoine Tenart {
10917142529fSAntoine Tenart 	netdev_features_t changed = dev->features ^ features;
1092004d44f6SVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(dev);
1093004d44f6SVladimir Oltean 	struct ocelot *ocelot = priv->port.ocelot;
1094004d44f6SVladimir Oltean 	int port = priv->chip_port;
10957142529fSAntoine Tenart 
10962c1d029aSJoergen Andreasen 	if ((dev->features & NETIF_F_HW_TC) > (features & NETIF_F_HW_TC) &&
1097004d44f6SVladimir Oltean 	    priv->tc.offload_cnt) {
10982c1d029aSJoergen Andreasen 		netdev_err(dev,
10992c1d029aSJoergen Andreasen 			   "Cannot disable HW TC offload while offloads active\n");
11002c1d029aSJoergen Andreasen 		return -EBUSY;
11012c1d029aSJoergen Andreasen 	}
11022c1d029aSJoergen Andreasen 
11037142529fSAntoine Tenart 	if (changed & NETIF_F_HW_VLAN_CTAG_FILTER)
1104f270dbfaSVladimir Oltean 		ocelot_vlan_mode(ocelot, port, features);
11057142529fSAntoine Tenart 
11067142529fSAntoine Tenart 	return 0;
11077142529fSAntoine Tenart }
11087142529fSAntoine Tenart 
1109751302c3SFlorian Fainelli static int ocelot_get_port_parent_id(struct net_device *dev,
1110751302c3SFlorian Fainelli 				     struct netdev_phys_item_id *ppid)
1111751302c3SFlorian Fainelli {
1112004d44f6SVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(dev);
1113004d44f6SVladimir Oltean 	struct ocelot *ocelot = priv->port.ocelot;
1114751302c3SFlorian Fainelli 
1115751302c3SFlorian Fainelli 	ppid->id_len = sizeof(ocelot->base_mac);
1116751302c3SFlorian Fainelli 	memcpy(&ppid->id, &ocelot->base_mac, ppid->id_len);
1117751302c3SFlorian Fainelli 
1118751302c3SFlorian Fainelli 	return 0;
1119751302c3SFlorian Fainelli }
1120751302c3SFlorian Fainelli 
1121f145922dSYangbo Lu int ocelot_hwstamp_get(struct ocelot *ocelot, int port, struct ifreq *ifr)
11224e3b0468SAntoine Tenart {
11234e3b0468SAntoine Tenart 	return copy_to_user(ifr->ifr_data, &ocelot->hwtstamp_config,
11244e3b0468SAntoine Tenart 			    sizeof(ocelot->hwtstamp_config)) ? -EFAULT : 0;
11254e3b0468SAntoine Tenart }
1126f145922dSYangbo Lu EXPORT_SYMBOL(ocelot_hwstamp_get);
11274e3b0468SAntoine Tenart 
1128f145922dSYangbo Lu int ocelot_hwstamp_set(struct ocelot *ocelot, int port, struct ifreq *ifr)
11294e3b0468SAntoine Tenart {
1130306fd44bSVladimir Oltean 	struct ocelot_port *ocelot_port = ocelot->ports[port];
11314e3b0468SAntoine Tenart 	struct hwtstamp_config cfg;
11324e3b0468SAntoine Tenart 
11334e3b0468SAntoine Tenart 	if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
11344e3b0468SAntoine Tenart 		return -EFAULT;
11354e3b0468SAntoine Tenart 
11364e3b0468SAntoine Tenart 	/* reserved for future extensions */
11374e3b0468SAntoine Tenart 	if (cfg.flags)
11384e3b0468SAntoine Tenart 		return -EINVAL;
11394e3b0468SAntoine Tenart 
11404e3b0468SAntoine Tenart 	/* Tx type sanity check */
11414e3b0468SAntoine Tenart 	switch (cfg.tx_type) {
11424e3b0468SAntoine Tenart 	case HWTSTAMP_TX_ON:
1143306fd44bSVladimir Oltean 		ocelot_port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
11444e3b0468SAntoine Tenart 		break;
11454e3b0468SAntoine Tenart 	case HWTSTAMP_TX_ONESTEP_SYNC:
11464e3b0468SAntoine Tenart 		/* IFH_REW_OP_ONE_STEP_PTP updates the correctional field, we
11474e3b0468SAntoine Tenart 		 * need to update the origin time.
11484e3b0468SAntoine Tenart 		 */
1149306fd44bSVladimir Oltean 		ocelot_port->ptp_cmd = IFH_REW_OP_ORIGIN_PTP;
11504e3b0468SAntoine Tenart 		break;
11514e3b0468SAntoine Tenart 	case HWTSTAMP_TX_OFF:
1152306fd44bSVladimir Oltean 		ocelot_port->ptp_cmd = 0;
11534e3b0468SAntoine Tenart 		break;
11544e3b0468SAntoine Tenart 	default:
11554e3b0468SAntoine Tenart 		return -ERANGE;
11564e3b0468SAntoine Tenart 	}
11574e3b0468SAntoine Tenart 
11584e3b0468SAntoine Tenart 	mutex_lock(&ocelot->ptp_lock);
11594e3b0468SAntoine Tenart 
11604e3b0468SAntoine Tenart 	switch (cfg.rx_filter) {
11614e3b0468SAntoine Tenart 	case HWTSTAMP_FILTER_NONE:
11624e3b0468SAntoine Tenart 		break;
11634e3b0468SAntoine Tenart 	case HWTSTAMP_FILTER_ALL:
11644e3b0468SAntoine Tenart 	case HWTSTAMP_FILTER_SOME:
11654e3b0468SAntoine Tenart 	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
11664e3b0468SAntoine Tenart 	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
11674e3b0468SAntoine Tenart 	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
11684e3b0468SAntoine Tenart 	case HWTSTAMP_FILTER_NTP_ALL:
11694e3b0468SAntoine Tenart 	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
11704e3b0468SAntoine Tenart 	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
11714e3b0468SAntoine Tenart 	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
11724e3b0468SAntoine Tenart 	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
11734e3b0468SAntoine Tenart 	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
11744e3b0468SAntoine Tenart 	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
11754e3b0468SAntoine Tenart 	case HWTSTAMP_FILTER_PTP_V2_EVENT:
11764e3b0468SAntoine Tenart 	case HWTSTAMP_FILTER_PTP_V2_SYNC:
11774e3b0468SAntoine Tenart 	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
11784e3b0468SAntoine Tenart 		cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
11794e3b0468SAntoine Tenart 		break;
11804e3b0468SAntoine Tenart 	default:
11814e3b0468SAntoine Tenart 		mutex_unlock(&ocelot->ptp_lock);
11824e3b0468SAntoine Tenart 		return -ERANGE;
11834e3b0468SAntoine Tenart 	}
11844e3b0468SAntoine Tenart 
11854e3b0468SAntoine Tenart 	/* Commit back the result & save it */
11864e3b0468SAntoine Tenart 	memcpy(&ocelot->hwtstamp_config, &cfg, sizeof(cfg));
11874e3b0468SAntoine Tenart 	mutex_unlock(&ocelot->ptp_lock);
11884e3b0468SAntoine Tenart 
11894e3b0468SAntoine Tenart 	return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
11904e3b0468SAntoine Tenart }
1191f145922dSYangbo Lu EXPORT_SYMBOL(ocelot_hwstamp_set);
11924e3b0468SAntoine Tenart 
11934e3b0468SAntoine Tenart static int ocelot_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
11944e3b0468SAntoine Tenart {
1195004d44f6SVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(dev);
1196004d44f6SVladimir Oltean 	struct ocelot *ocelot = priv->port.ocelot;
1197004d44f6SVladimir Oltean 	int port = priv->chip_port;
11984e3b0468SAntoine Tenart 
11994e3b0468SAntoine Tenart 	/* The function is only used for PTP operations for now */
12004e3b0468SAntoine Tenart 	if (!ocelot->ptp)
12014e3b0468SAntoine Tenart 		return -EOPNOTSUPP;
12024e3b0468SAntoine Tenart 
12034e3b0468SAntoine Tenart 	switch (cmd) {
12044e3b0468SAntoine Tenart 	case SIOCSHWTSTAMP:
1205306fd44bSVladimir Oltean 		return ocelot_hwstamp_set(ocelot, port, ifr);
12064e3b0468SAntoine Tenart 	case SIOCGHWTSTAMP:
1207306fd44bSVladimir Oltean 		return ocelot_hwstamp_get(ocelot, port, ifr);
12084e3b0468SAntoine Tenart 	default:
12094e3b0468SAntoine Tenart 		return -EOPNOTSUPP;
12104e3b0468SAntoine Tenart 	}
12114e3b0468SAntoine Tenart }
12124e3b0468SAntoine Tenart 
1213a556c76aSAlexandre Belloni static const struct net_device_ops ocelot_port_netdev_ops = {
1214a556c76aSAlexandre Belloni 	.ndo_open			= ocelot_port_open,
1215a556c76aSAlexandre Belloni 	.ndo_stop			= ocelot_port_stop,
1216a556c76aSAlexandre Belloni 	.ndo_start_xmit			= ocelot_port_xmit,
1217a556c76aSAlexandre Belloni 	.ndo_set_rx_mode		= ocelot_set_rx_mode,
1218a556c76aSAlexandre Belloni 	.ndo_get_phys_port_name		= ocelot_port_get_phys_port_name,
1219a556c76aSAlexandre Belloni 	.ndo_set_mac_address		= ocelot_port_set_mac_address,
1220a556c76aSAlexandre Belloni 	.ndo_get_stats64		= ocelot_get_stats64,
1221531ee1a6SVladimir Oltean 	.ndo_fdb_add			= ocelot_port_fdb_add,
1222531ee1a6SVladimir Oltean 	.ndo_fdb_del			= ocelot_port_fdb_del,
1223531ee1a6SVladimir Oltean 	.ndo_fdb_dump			= ocelot_port_fdb_dump,
12247142529fSAntoine Tenart 	.ndo_vlan_rx_add_vid		= ocelot_vlan_rx_add_vid,
12257142529fSAntoine Tenart 	.ndo_vlan_rx_kill_vid		= ocelot_vlan_rx_kill_vid,
12267142529fSAntoine Tenart 	.ndo_set_features		= ocelot_set_features,
1227751302c3SFlorian Fainelli 	.ndo_get_port_parent_id		= ocelot_get_port_parent_id,
12282c1d029aSJoergen Andreasen 	.ndo_setup_tc			= ocelot_setup_tc,
12294e3b0468SAntoine Tenart 	.ndo_do_ioctl			= ocelot_ioctl,
1230a556c76aSAlexandre Belloni };
1231a556c76aSAlexandre Belloni 
12325e256365SVladimir Oltean void ocelot_get_strings(struct ocelot *ocelot, int port, u32 sset, u8 *data)
1233a556c76aSAlexandre Belloni {
1234a556c76aSAlexandre Belloni 	int i;
1235a556c76aSAlexandre Belloni 
1236a556c76aSAlexandre Belloni 	if (sset != ETH_SS_STATS)
1237a556c76aSAlexandre Belloni 		return;
1238a556c76aSAlexandre Belloni 
1239a556c76aSAlexandre Belloni 	for (i = 0; i < ocelot->num_stats; i++)
1240a556c76aSAlexandre Belloni 		memcpy(data + i * ETH_GSTRING_LEN, ocelot->stats_layout[i].name,
1241a556c76aSAlexandre Belloni 		       ETH_GSTRING_LEN);
1242a556c76aSAlexandre Belloni }
12435e256365SVladimir Oltean EXPORT_SYMBOL(ocelot_get_strings);
1244a556c76aSAlexandre Belloni 
1245c7282d38SVladimir Oltean static void ocelot_port_get_strings(struct net_device *netdev, u32 sset,
1246c7282d38SVladimir Oltean 				    u8 *data)
1247c7282d38SVladimir Oltean {
1248c7282d38SVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(netdev);
1249c7282d38SVladimir Oltean 	struct ocelot *ocelot = priv->port.ocelot;
1250c7282d38SVladimir Oltean 	int port = priv->chip_port;
1251c7282d38SVladimir Oltean 
1252c7282d38SVladimir Oltean 	ocelot_get_strings(ocelot, port, sset, data);
1253c7282d38SVladimir Oltean }
1254c7282d38SVladimir Oltean 
12551e1caa97SClaudiu Manoil static void ocelot_update_stats(struct ocelot *ocelot)
1256a556c76aSAlexandre Belloni {
1257a556c76aSAlexandre Belloni 	int i, j;
1258a556c76aSAlexandre Belloni 
1259a556c76aSAlexandre Belloni 	mutex_lock(&ocelot->stats_lock);
1260a556c76aSAlexandre Belloni 
1261a556c76aSAlexandre Belloni 	for (i = 0; i < ocelot->num_phys_ports; i++) {
1262a556c76aSAlexandre Belloni 		/* Configure the port to read the stats from */
1263a556c76aSAlexandre Belloni 		ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(i), SYS_STAT_CFG);
1264a556c76aSAlexandre Belloni 
1265a556c76aSAlexandre Belloni 		for (j = 0; j < ocelot->num_stats; j++) {
1266a556c76aSAlexandre Belloni 			u32 val;
1267a556c76aSAlexandre Belloni 			unsigned int idx = i * ocelot->num_stats + j;
1268a556c76aSAlexandre Belloni 
1269a556c76aSAlexandre Belloni 			val = ocelot_read_rix(ocelot, SYS_COUNT_RX_OCTETS,
1270a556c76aSAlexandre Belloni 					      ocelot->stats_layout[j].offset);
1271a556c76aSAlexandre Belloni 
1272a556c76aSAlexandre Belloni 			if (val < (ocelot->stats[idx] & U32_MAX))
1273a556c76aSAlexandre Belloni 				ocelot->stats[idx] += (u64)1 << 32;
1274a556c76aSAlexandre Belloni 
1275a556c76aSAlexandre Belloni 			ocelot->stats[idx] = (ocelot->stats[idx] &
1276a556c76aSAlexandre Belloni 					      ~(u64)U32_MAX) + val;
1277a556c76aSAlexandre Belloni 		}
1278a556c76aSAlexandre Belloni 	}
1279a556c76aSAlexandre Belloni 
12801e1caa97SClaudiu Manoil 	mutex_unlock(&ocelot->stats_lock);
12811e1caa97SClaudiu Manoil }
12821e1caa97SClaudiu Manoil 
12831e1caa97SClaudiu Manoil static void ocelot_check_stats_work(struct work_struct *work)
12841e1caa97SClaudiu Manoil {
12851e1caa97SClaudiu Manoil 	struct delayed_work *del_work = to_delayed_work(work);
12861e1caa97SClaudiu Manoil 	struct ocelot *ocelot = container_of(del_work, struct ocelot,
12871e1caa97SClaudiu Manoil 					     stats_work);
12881e1caa97SClaudiu Manoil 
12891e1caa97SClaudiu Manoil 	ocelot_update_stats(ocelot);
12901e1caa97SClaudiu Manoil 
1291a556c76aSAlexandre Belloni 	queue_delayed_work(ocelot->stats_queue, &ocelot->stats_work,
1292a556c76aSAlexandre Belloni 			   OCELOT_STATS_CHECK_DELAY);
1293a556c76aSAlexandre Belloni }
1294a556c76aSAlexandre Belloni 
12955e256365SVladimir Oltean void ocelot_get_ethtool_stats(struct ocelot *ocelot, int port, u64 *data)
1296a556c76aSAlexandre Belloni {
1297a556c76aSAlexandre Belloni 	int i;
1298a556c76aSAlexandre Belloni 
1299a556c76aSAlexandre Belloni 	/* check and update now */
13001e1caa97SClaudiu Manoil 	ocelot_update_stats(ocelot);
1301a556c76aSAlexandre Belloni 
1302a556c76aSAlexandre Belloni 	/* Copy all counters */
1303a556c76aSAlexandre Belloni 	for (i = 0; i < ocelot->num_stats; i++)
1304004d44f6SVladimir Oltean 		*data++ = ocelot->stats[port * ocelot->num_stats + i];
1305a556c76aSAlexandre Belloni }
13065e256365SVladimir Oltean EXPORT_SYMBOL(ocelot_get_ethtool_stats);
1307a556c76aSAlexandre Belloni 
1308c7282d38SVladimir Oltean static void ocelot_port_get_ethtool_stats(struct net_device *dev,
1309c7282d38SVladimir Oltean 					  struct ethtool_stats *stats,
1310c7282d38SVladimir Oltean 					  u64 *data)
1311a556c76aSAlexandre Belloni {
1312004d44f6SVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(dev);
1313004d44f6SVladimir Oltean 	struct ocelot *ocelot = priv->port.ocelot;
1314c7282d38SVladimir Oltean 	int port = priv->chip_port;
1315a556c76aSAlexandre Belloni 
1316c7282d38SVladimir Oltean 	ocelot_get_ethtool_stats(ocelot, port, data);
1317c7282d38SVladimir Oltean }
1318c7282d38SVladimir Oltean 
13195e256365SVladimir Oltean int ocelot_get_sset_count(struct ocelot *ocelot, int port, int sset)
1320c7282d38SVladimir Oltean {
1321a556c76aSAlexandre Belloni 	if (sset != ETH_SS_STATS)
1322a556c76aSAlexandre Belloni 		return -EOPNOTSUPP;
1323c7282d38SVladimir Oltean 
1324a556c76aSAlexandre Belloni 	return ocelot->num_stats;
1325a556c76aSAlexandre Belloni }
13265e256365SVladimir Oltean EXPORT_SYMBOL(ocelot_get_sset_count);
1327a556c76aSAlexandre Belloni 
1328c7282d38SVladimir Oltean static int ocelot_port_get_sset_count(struct net_device *dev, int sset)
13294e3b0468SAntoine Tenart {
1330004d44f6SVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(dev);
1331004d44f6SVladimir Oltean 	struct ocelot *ocelot = priv->port.ocelot;
1332c7282d38SVladimir Oltean 	int port = priv->chip_port;
13334e3b0468SAntoine Tenart 
1334c7282d38SVladimir Oltean 	return ocelot_get_sset_count(ocelot, port, sset);
1335c7282d38SVladimir Oltean }
13364e3b0468SAntoine Tenart 
13375e256365SVladimir Oltean int ocelot_get_ts_info(struct ocelot *ocelot, int port,
1338c7282d38SVladimir Oltean 		       struct ethtool_ts_info *info)
1339c7282d38SVladimir Oltean {
13404e3b0468SAntoine Tenart 	info->phc_index = ocelot->ptp_clock ?
13414e3b0468SAntoine Tenart 			  ptp_clock_index(ocelot->ptp_clock) : -1;
13424e3b0468SAntoine Tenart 	info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE |
13434e3b0468SAntoine Tenart 				 SOF_TIMESTAMPING_RX_SOFTWARE |
13444e3b0468SAntoine Tenart 				 SOF_TIMESTAMPING_SOFTWARE |
13454e3b0468SAntoine Tenart 				 SOF_TIMESTAMPING_TX_HARDWARE |
13464e3b0468SAntoine Tenart 				 SOF_TIMESTAMPING_RX_HARDWARE |
13474e3b0468SAntoine Tenart 				 SOF_TIMESTAMPING_RAW_HARDWARE;
13484e3b0468SAntoine Tenart 	info->tx_types = BIT(HWTSTAMP_TX_OFF) | BIT(HWTSTAMP_TX_ON) |
13494e3b0468SAntoine Tenart 			 BIT(HWTSTAMP_TX_ONESTEP_SYNC);
13504e3b0468SAntoine Tenart 	info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) | BIT(HWTSTAMP_FILTER_ALL);
13514e3b0468SAntoine Tenart 
13524e3b0468SAntoine Tenart 	return 0;
13534e3b0468SAntoine Tenart }
13545e256365SVladimir Oltean EXPORT_SYMBOL(ocelot_get_ts_info);
13554e3b0468SAntoine Tenart 
1356c7282d38SVladimir Oltean static int ocelot_port_get_ts_info(struct net_device *dev,
1357c7282d38SVladimir Oltean 				   struct ethtool_ts_info *info)
1358c7282d38SVladimir Oltean {
1359c7282d38SVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(dev);
1360c7282d38SVladimir Oltean 	struct ocelot *ocelot = priv->port.ocelot;
1361c7282d38SVladimir Oltean 	int port = priv->chip_port;
1362c7282d38SVladimir Oltean 
1363c7282d38SVladimir Oltean 	if (!ocelot->ptp)
1364c7282d38SVladimir Oltean 		return ethtool_op_get_ts_info(dev, info);
1365c7282d38SVladimir Oltean 
1366c7282d38SVladimir Oltean 	return ocelot_get_ts_info(ocelot, port, info);
1367c7282d38SVladimir Oltean }
1368c7282d38SVladimir Oltean 
1369a556c76aSAlexandre Belloni static const struct ethtool_ops ocelot_ethtool_ops = {
1370c7282d38SVladimir Oltean 	.get_strings		= ocelot_port_get_strings,
1371c7282d38SVladimir Oltean 	.get_ethtool_stats	= ocelot_port_get_ethtool_stats,
1372c7282d38SVladimir Oltean 	.get_sset_count		= ocelot_port_get_sset_count,
1373dc96ee37SAlexandre Belloni 	.get_link_ksettings	= phy_ethtool_get_link_ksettings,
1374dc96ee37SAlexandre Belloni 	.set_link_ksettings	= phy_ethtool_set_link_ksettings,
1375c7282d38SVladimir Oltean 	.get_ts_info		= ocelot_port_get_ts_info,
1376a556c76aSAlexandre Belloni };
1377a556c76aSAlexandre Belloni 
13785e256365SVladimir Oltean void ocelot_bridge_stp_state_set(struct ocelot *ocelot, int port, u8 state)
1379a556c76aSAlexandre Belloni {
1380a556c76aSAlexandre Belloni 	u32 port_cfg;
13814bda1415SVladimir Oltean 	int p, i;
1382a556c76aSAlexandre Belloni 
13834bda1415SVladimir Oltean 	if (!(BIT(port) & ocelot->bridge_mask))
13844bda1415SVladimir Oltean 		return;
1385a556c76aSAlexandre Belloni 
13864bda1415SVladimir Oltean 	port_cfg = ocelot_read_gix(ocelot, ANA_PORT_PORT_CFG, port);
1387a556c76aSAlexandre Belloni 
1388a556c76aSAlexandre Belloni 	switch (state) {
1389a556c76aSAlexandre Belloni 	case BR_STATE_FORWARDING:
13904bda1415SVladimir Oltean 		ocelot->bridge_fwd_mask |= BIT(port);
1391a556c76aSAlexandre Belloni 		/* Fallthrough */
1392a556c76aSAlexandre Belloni 	case BR_STATE_LEARNING:
1393a556c76aSAlexandre Belloni 		port_cfg |= ANA_PORT_PORT_CFG_LEARN_ENA;
1394a556c76aSAlexandre Belloni 		break;
1395a556c76aSAlexandre Belloni 
1396a556c76aSAlexandre Belloni 	default:
1397a556c76aSAlexandre Belloni 		port_cfg &= ~ANA_PORT_PORT_CFG_LEARN_ENA;
13984bda1415SVladimir Oltean 		ocelot->bridge_fwd_mask &= ~BIT(port);
1399a556c76aSAlexandre Belloni 		break;
1400a556c76aSAlexandre Belloni 	}
1401a556c76aSAlexandre Belloni 
14024bda1415SVladimir Oltean 	ocelot_write_gix(ocelot, port_cfg, ANA_PORT_PORT_CFG, port);
1403a556c76aSAlexandre Belloni 
1404a556c76aSAlexandre Belloni 	/* Apply FWD mask. The loop is needed to add/remove the current port as
1405a556c76aSAlexandre Belloni 	 * a source for the other ports.
1406a556c76aSAlexandre Belloni 	 */
14074bda1415SVladimir Oltean 	for (p = 0; p < ocelot->num_phys_ports; p++) {
1408c9d2203bSVladimir Oltean 		if (p == ocelot->cpu || (ocelot->bridge_fwd_mask & BIT(p))) {
14094bda1415SVladimir Oltean 			unsigned long mask = ocelot->bridge_fwd_mask & ~BIT(p);
1410a556c76aSAlexandre Belloni 
1411a556c76aSAlexandre Belloni 			for (i = 0; i < ocelot->num_phys_ports; i++) {
1412a556c76aSAlexandre Belloni 				unsigned long bond_mask = ocelot->lags[i];
1413a556c76aSAlexandre Belloni 
1414a556c76aSAlexandre Belloni 				if (!bond_mask)
1415a556c76aSAlexandre Belloni 					continue;
1416a556c76aSAlexandre Belloni 
14174bda1415SVladimir Oltean 				if (bond_mask & BIT(p)) {
1418a556c76aSAlexandre Belloni 					mask &= ~bond_mask;
1419a556c76aSAlexandre Belloni 					break;
1420a556c76aSAlexandre Belloni 				}
1421a556c76aSAlexandre Belloni 			}
1422a556c76aSAlexandre Belloni 
1423c9d2203bSVladimir Oltean 			/* Avoid the NPI port from looping back to itself */
1424c9d2203bSVladimir Oltean 			if (p != ocelot->cpu)
1425c9d2203bSVladimir Oltean 				mask |= BIT(ocelot->cpu);
1426c9d2203bSVladimir Oltean 
1427c9d2203bSVladimir Oltean 			ocelot_write_rix(ocelot, mask,
14284bda1415SVladimir Oltean 					 ANA_PGID_PGID, PGID_SRC + p);
1429a556c76aSAlexandre Belloni 		} else {
1430a556c76aSAlexandre Belloni 			/* Only the CPU port, this is compatible with link
1431a556c76aSAlexandre Belloni 			 * aggregation.
1432a556c76aSAlexandre Belloni 			 */
1433a556c76aSAlexandre Belloni 			ocelot_write_rix(ocelot,
1434c9d2203bSVladimir Oltean 					 BIT(ocelot->cpu),
14354bda1415SVladimir Oltean 					 ANA_PGID_PGID, PGID_SRC + p);
14364bda1415SVladimir Oltean 		}
1437a556c76aSAlexandre Belloni 	}
1438a556c76aSAlexandre Belloni }
14395e256365SVladimir Oltean EXPORT_SYMBOL(ocelot_bridge_stp_state_set);
1440a556c76aSAlexandre Belloni 
14414bda1415SVladimir Oltean static void ocelot_port_attr_stp_state_set(struct ocelot *ocelot, int port,
14424bda1415SVladimir Oltean 					   struct switchdev_trans *trans,
14434bda1415SVladimir Oltean 					   u8 state)
1444a556c76aSAlexandre Belloni {
14454bda1415SVladimir Oltean 	if (switchdev_trans_ph_prepare(trans))
14464bda1415SVladimir Oltean 		return;
1447a556c76aSAlexandre Belloni 
14484bda1415SVladimir Oltean 	ocelot_bridge_stp_state_set(ocelot, port, state);
14494bda1415SVladimir Oltean }
14504bda1415SVladimir Oltean 
14515e256365SVladimir Oltean void ocelot_set_ageing_time(struct ocelot *ocelot, unsigned int msecs)
14524bda1415SVladimir Oltean {
14534bda1415SVladimir Oltean 	ocelot_write(ocelot, ANA_AUTOAGE_AGE_PERIOD(msecs / 2),
1454a556c76aSAlexandre Belloni 		     ANA_AUTOAGE);
1455a556c76aSAlexandre Belloni }
14565e256365SVladimir Oltean EXPORT_SYMBOL(ocelot_set_ageing_time);
1457a556c76aSAlexandre Belloni 
14584bda1415SVladimir Oltean static void ocelot_port_attr_ageing_set(struct ocelot *ocelot, int port,
14594bda1415SVladimir Oltean 					unsigned long ageing_clock_t)
1460a556c76aSAlexandre Belloni {
14614bda1415SVladimir Oltean 	unsigned long ageing_jiffies = clock_t_to_jiffies(ageing_clock_t);
14624bda1415SVladimir Oltean 	u32 ageing_time = jiffies_to_msecs(ageing_jiffies) / 1000;
1463a556c76aSAlexandre Belloni 
14644bda1415SVladimir Oltean 	ocelot_set_ageing_time(ocelot, ageing_time);
14654bda1415SVladimir Oltean }
14664bda1415SVladimir Oltean 
14674bda1415SVladimir Oltean static void ocelot_port_attr_mc_set(struct ocelot *ocelot, int port, bool mc)
14684bda1415SVladimir Oltean {
14694bda1415SVladimir Oltean 	u32 cpu_fwd_mcast = ANA_PORT_CPU_FWD_CFG_CPU_IGMP_REDIR_ENA |
1470a556c76aSAlexandre Belloni 			    ANA_PORT_CPU_FWD_CFG_CPU_MLD_REDIR_ENA |
1471a556c76aSAlexandre Belloni 			    ANA_PORT_CPU_FWD_CFG_CPU_IPMC_CTRL_COPY_ENA;
14724bda1415SVladimir Oltean 	u32 val = 0;
1473a556c76aSAlexandre Belloni 
14744bda1415SVladimir Oltean 	if (mc)
14754bda1415SVladimir Oltean 		val = cpu_fwd_mcast;
14764bda1415SVladimir Oltean 
14774bda1415SVladimir Oltean 	ocelot_rmw_gix(ocelot, val, cpu_fwd_mcast,
14784bda1415SVladimir Oltean 		       ANA_PORT_CPU_FWD_CFG, port);
1479a556c76aSAlexandre Belloni }
1480a556c76aSAlexandre Belloni 
1481a556c76aSAlexandre Belloni static int ocelot_port_attr_set(struct net_device *dev,
1482a556c76aSAlexandre Belloni 				const struct switchdev_attr *attr,
1483a556c76aSAlexandre Belloni 				struct switchdev_trans *trans)
1484a556c76aSAlexandre Belloni {
1485004d44f6SVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(dev);
1486004d44f6SVladimir Oltean 	struct ocelot *ocelot = priv->port.ocelot;
1487004d44f6SVladimir Oltean 	int port = priv->chip_port;
1488a556c76aSAlexandre Belloni 	int err = 0;
1489a556c76aSAlexandre Belloni 
1490a556c76aSAlexandre Belloni 	switch (attr->id) {
1491a556c76aSAlexandre Belloni 	case SWITCHDEV_ATTR_ID_PORT_STP_STATE:
14924bda1415SVladimir Oltean 		ocelot_port_attr_stp_state_set(ocelot, port, trans,
1493a556c76aSAlexandre Belloni 					       attr->u.stp_state);
1494a556c76aSAlexandre Belloni 		break;
1495a556c76aSAlexandre Belloni 	case SWITCHDEV_ATTR_ID_BRIDGE_AGEING_TIME:
14964bda1415SVladimir Oltean 		ocelot_port_attr_ageing_set(ocelot, port, attr->u.ageing_time);
1497a556c76aSAlexandre Belloni 		break;
14987142529fSAntoine Tenart 	case SWITCHDEV_ATTR_ID_BRIDGE_VLAN_FILTERING:
1499004d44f6SVladimir Oltean 		priv->vlan_aware = attr->u.vlan_filtering;
1500004d44f6SVladimir Oltean 		ocelot_port_vlan_filtering(ocelot, port, priv->vlan_aware);
15017142529fSAntoine Tenart 		break;
1502a556c76aSAlexandre Belloni 	case SWITCHDEV_ATTR_ID_BRIDGE_MC_DISABLED:
15034bda1415SVladimir Oltean 		ocelot_port_attr_mc_set(ocelot, port, !attr->u.mc_disabled);
1504a556c76aSAlexandre Belloni 		break;
1505a556c76aSAlexandre Belloni 	default:
1506a556c76aSAlexandre Belloni 		err = -EOPNOTSUPP;
1507a556c76aSAlexandre Belloni 		break;
1508a556c76aSAlexandre Belloni 	}
1509a556c76aSAlexandre Belloni 
1510a556c76aSAlexandre Belloni 	return err;
1511a556c76aSAlexandre Belloni }
1512a556c76aSAlexandre Belloni 
15137142529fSAntoine Tenart static int ocelot_port_obj_add_vlan(struct net_device *dev,
15147142529fSAntoine Tenart 				    const struct switchdev_obj_port_vlan *vlan,
15157142529fSAntoine Tenart 				    struct switchdev_trans *trans)
15167142529fSAntoine Tenart {
15177142529fSAntoine Tenart 	int ret;
15187142529fSAntoine Tenart 	u16 vid;
15197142529fSAntoine Tenart 
15207142529fSAntoine Tenart 	for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
15217142529fSAntoine Tenart 		ret = ocelot_vlan_vid_add(dev, vid,
15227142529fSAntoine Tenart 					  vlan->flags & BRIDGE_VLAN_INFO_PVID,
15237142529fSAntoine Tenart 					  vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED);
15247142529fSAntoine Tenart 		if (ret)
15257142529fSAntoine Tenart 			return ret;
15267142529fSAntoine Tenart 	}
15277142529fSAntoine Tenart 
15287142529fSAntoine Tenart 	return 0;
15297142529fSAntoine Tenart }
15307142529fSAntoine Tenart 
15317142529fSAntoine Tenart static int ocelot_port_vlan_del_vlan(struct net_device *dev,
15327142529fSAntoine Tenart 				     const struct switchdev_obj_port_vlan *vlan)
15337142529fSAntoine Tenart {
15347142529fSAntoine Tenart 	int ret;
15357142529fSAntoine Tenart 	u16 vid;
15367142529fSAntoine Tenart 
15377142529fSAntoine Tenart 	for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
15387142529fSAntoine Tenart 		ret = ocelot_vlan_vid_del(dev, vid);
15397142529fSAntoine Tenart 
15407142529fSAntoine Tenart 		if (ret)
15417142529fSAntoine Tenart 			return ret;
15427142529fSAntoine Tenart 	}
15437142529fSAntoine Tenart 
15447142529fSAntoine Tenart 	return 0;
15457142529fSAntoine Tenart }
15467142529fSAntoine Tenart 
1547a556c76aSAlexandre Belloni static struct ocelot_multicast *ocelot_multicast_get(struct ocelot *ocelot,
1548a556c76aSAlexandre Belloni 						     const unsigned char *addr,
1549a556c76aSAlexandre Belloni 						     u16 vid)
1550a556c76aSAlexandre Belloni {
1551a556c76aSAlexandre Belloni 	struct ocelot_multicast *mc;
1552a556c76aSAlexandre Belloni 
1553a556c76aSAlexandre Belloni 	list_for_each_entry(mc, &ocelot->multicast, list) {
1554a556c76aSAlexandre Belloni 		if (ether_addr_equal(mc->addr, addr) && mc->vid == vid)
1555a556c76aSAlexandre Belloni 			return mc;
1556a556c76aSAlexandre Belloni 	}
1557a556c76aSAlexandre Belloni 
1558a556c76aSAlexandre Belloni 	return NULL;
1559a556c76aSAlexandre Belloni }
1560a556c76aSAlexandre Belloni 
1561a556c76aSAlexandre Belloni static int ocelot_port_obj_add_mdb(struct net_device *dev,
1562a556c76aSAlexandre Belloni 				   const struct switchdev_obj_port_mdb *mdb,
1563a556c76aSAlexandre Belloni 				   struct switchdev_trans *trans)
1564a556c76aSAlexandre Belloni {
1565004d44f6SVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(dev);
1566004d44f6SVladimir Oltean 	struct ocelot_port *ocelot_port = &priv->port;
1567004d44f6SVladimir Oltean 	struct ocelot *ocelot = ocelot_port->ocelot;
1568a556c76aSAlexandre Belloni 	unsigned char addr[ETH_ALEN];
1569004d44f6SVladimir Oltean 	struct ocelot_multicast *mc;
1570004d44f6SVladimir Oltean 	int port = priv->chip_port;
1571a556c76aSAlexandre Belloni 	u16 vid = mdb->vid;
1572a556c76aSAlexandre Belloni 	bool new = false;
1573a556c76aSAlexandre Belloni 
1574a556c76aSAlexandre Belloni 	if (!vid)
1575004d44f6SVladimir Oltean 		vid = ocelot_port->pvid;
1576a556c76aSAlexandre Belloni 
1577a556c76aSAlexandre Belloni 	mc = ocelot_multicast_get(ocelot, mdb->addr, vid);
1578a556c76aSAlexandre Belloni 	if (!mc) {
1579a556c76aSAlexandre Belloni 		mc = devm_kzalloc(ocelot->dev, sizeof(*mc), GFP_KERNEL);
1580a556c76aSAlexandre Belloni 		if (!mc)
1581a556c76aSAlexandre Belloni 			return -ENOMEM;
1582a556c76aSAlexandre Belloni 
1583a556c76aSAlexandre Belloni 		memcpy(mc->addr, mdb->addr, ETH_ALEN);
1584a556c76aSAlexandre Belloni 		mc->vid = vid;
1585a556c76aSAlexandre Belloni 
1586a556c76aSAlexandre Belloni 		list_add_tail(&mc->list, &ocelot->multicast);
1587a556c76aSAlexandre Belloni 		new = true;
1588a556c76aSAlexandre Belloni 	}
1589a556c76aSAlexandre Belloni 
1590a556c76aSAlexandre Belloni 	memcpy(addr, mc->addr, ETH_ALEN);
1591a556c76aSAlexandre Belloni 	addr[0] = 0;
1592a556c76aSAlexandre Belloni 
1593a556c76aSAlexandre Belloni 	if (!new) {
1594a556c76aSAlexandre Belloni 		addr[2] = mc->ports << 0;
1595a556c76aSAlexandre Belloni 		addr[1] = mc->ports << 8;
1596a556c76aSAlexandre Belloni 		ocelot_mact_forget(ocelot, addr, vid);
1597a556c76aSAlexandre Belloni 	}
1598a556c76aSAlexandre Belloni 
1599004d44f6SVladimir Oltean 	mc->ports |= BIT(port);
1600a556c76aSAlexandre Belloni 	addr[2] = mc->ports << 0;
1601a556c76aSAlexandre Belloni 	addr[1] = mc->ports << 8;
1602a556c76aSAlexandre Belloni 
1603a556c76aSAlexandre Belloni 	return ocelot_mact_learn(ocelot, 0, addr, vid, ENTRYTYPE_MACv4);
1604a556c76aSAlexandre Belloni }
1605a556c76aSAlexandre Belloni 
1606a556c76aSAlexandre Belloni static int ocelot_port_obj_del_mdb(struct net_device *dev,
1607a556c76aSAlexandre Belloni 				   const struct switchdev_obj_port_mdb *mdb)
1608a556c76aSAlexandre Belloni {
1609004d44f6SVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(dev);
1610004d44f6SVladimir Oltean 	struct ocelot_port *ocelot_port = &priv->port;
1611004d44f6SVladimir Oltean 	struct ocelot *ocelot = ocelot_port->ocelot;
1612a556c76aSAlexandre Belloni 	unsigned char addr[ETH_ALEN];
1613004d44f6SVladimir Oltean 	struct ocelot_multicast *mc;
1614004d44f6SVladimir Oltean 	int port = priv->chip_port;
1615a556c76aSAlexandre Belloni 	u16 vid = mdb->vid;
1616a556c76aSAlexandre Belloni 
1617a556c76aSAlexandre Belloni 	if (!vid)
1618004d44f6SVladimir Oltean 		vid = ocelot_port->pvid;
1619a556c76aSAlexandre Belloni 
1620a556c76aSAlexandre Belloni 	mc = ocelot_multicast_get(ocelot, mdb->addr, vid);
1621a556c76aSAlexandre Belloni 	if (!mc)
1622a556c76aSAlexandre Belloni 		return -ENOENT;
1623a556c76aSAlexandre Belloni 
1624a556c76aSAlexandre Belloni 	memcpy(addr, mc->addr, ETH_ALEN);
1625a556c76aSAlexandre Belloni 	addr[2] = mc->ports << 0;
1626a556c76aSAlexandre Belloni 	addr[1] = mc->ports << 8;
1627a556c76aSAlexandre Belloni 	addr[0] = 0;
1628a556c76aSAlexandre Belloni 	ocelot_mact_forget(ocelot, addr, vid);
1629a556c76aSAlexandre Belloni 
1630004d44f6SVladimir Oltean 	mc->ports &= ~BIT(port);
1631a556c76aSAlexandre Belloni 	if (!mc->ports) {
1632a556c76aSAlexandre Belloni 		list_del(&mc->list);
1633a556c76aSAlexandre Belloni 		devm_kfree(ocelot->dev, mc);
1634a556c76aSAlexandre Belloni 		return 0;
1635a556c76aSAlexandre Belloni 	}
1636a556c76aSAlexandre Belloni 
1637a556c76aSAlexandre Belloni 	addr[2] = mc->ports << 0;
1638a556c76aSAlexandre Belloni 	addr[1] = mc->ports << 8;
1639a556c76aSAlexandre Belloni 
1640a556c76aSAlexandre Belloni 	return ocelot_mact_learn(ocelot, 0, addr, vid, ENTRYTYPE_MACv4);
1641a556c76aSAlexandre Belloni }
1642a556c76aSAlexandre Belloni 
1643a556c76aSAlexandre Belloni static int ocelot_port_obj_add(struct net_device *dev,
1644a556c76aSAlexandre Belloni 			       const struct switchdev_obj *obj,
164569213513SPetr Machata 			       struct switchdev_trans *trans,
164669213513SPetr Machata 			       struct netlink_ext_ack *extack)
1647a556c76aSAlexandre Belloni {
1648a556c76aSAlexandre Belloni 	int ret = 0;
1649a556c76aSAlexandre Belloni 
1650a556c76aSAlexandre Belloni 	switch (obj->id) {
16517142529fSAntoine Tenart 	case SWITCHDEV_OBJ_ID_PORT_VLAN:
16527142529fSAntoine Tenart 		ret = ocelot_port_obj_add_vlan(dev,
16537142529fSAntoine Tenart 					       SWITCHDEV_OBJ_PORT_VLAN(obj),
16547142529fSAntoine Tenart 					       trans);
16557142529fSAntoine Tenart 		break;
1656a556c76aSAlexandre Belloni 	case SWITCHDEV_OBJ_ID_PORT_MDB:
1657a556c76aSAlexandre Belloni 		ret = ocelot_port_obj_add_mdb(dev, SWITCHDEV_OBJ_PORT_MDB(obj),
1658a556c76aSAlexandre Belloni 					      trans);
1659a556c76aSAlexandre Belloni 		break;
1660a556c76aSAlexandre Belloni 	default:
1661a556c76aSAlexandre Belloni 		return -EOPNOTSUPP;
1662a556c76aSAlexandre Belloni 	}
1663a556c76aSAlexandre Belloni 
1664a556c76aSAlexandre Belloni 	return ret;
1665a556c76aSAlexandre Belloni }
1666a556c76aSAlexandre Belloni 
1667a556c76aSAlexandre Belloni static int ocelot_port_obj_del(struct net_device *dev,
1668a556c76aSAlexandre Belloni 			       const struct switchdev_obj *obj)
1669a556c76aSAlexandre Belloni {
1670a556c76aSAlexandre Belloni 	int ret = 0;
1671a556c76aSAlexandre Belloni 
1672a556c76aSAlexandre Belloni 	switch (obj->id) {
16737142529fSAntoine Tenart 	case SWITCHDEV_OBJ_ID_PORT_VLAN:
16747142529fSAntoine Tenart 		ret = ocelot_port_vlan_del_vlan(dev,
16757142529fSAntoine Tenart 						SWITCHDEV_OBJ_PORT_VLAN(obj));
16767142529fSAntoine Tenart 		break;
1677a556c76aSAlexandre Belloni 	case SWITCHDEV_OBJ_ID_PORT_MDB:
1678a556c76aSAlexandre Belloni 		ret = ocelot_port_obj_del_mdb(dev, SWITCHDEV_OBJ_PORT_MDB(obj));
1679a556c76aSAlexandre Belloni 		break;
1680a556c76aSAlexandre Belloni 	default:
1681a556c76aSAlexandre Belloni 		return -EOPNOTSUPP;
1682a556c76aSAlexandre Belloni 	}
1683a556c76aSAlexandre Belloni 
1684a556c76aSAlexandre Belloni 	return ret;
1685a556c76aSAlexandre Belloni }
1686a556c76aSAlexandre Belloni 
16875e256365SVladimir Oltean int ocelot_port_bridge_join(struct ocelot *ocelot, int port,
1688a556c76aSAlexandre Belloni 			    struct net_device *bridge)
1689a556c76aSAlexandre Belloni {
1690a556c76aSAlexandre Belloni 	if (!ocelot->bridge_mask) {
1691a556c76aSAlexandre Belloni 		ocelot->hw_bridge_dev = bridge;
1692a556c76aSAlexandre Belloni 	} else {
1693a556c76aSAlexandre Belloni 		if (ocelot->hw_bridge_dev != bridge)
1694a556c76aSAlexandre Belloni 			/* This is adding the port to a second bridge, this is
1695a556c76aSAlexandre Belloni 			 * unsupported */
1696a556c76aSAlexandre Belloni 			return -ENODEV;
1697a556c76aSAlexandre Belloni 	}
1698a556c76aSAlexandre Belloni 
1699f270dbfaSVladimir Oltean 	ocelot->bridge_mask |= BIT(port);
1700a556c76aSAlexandre Belloni 
1701a556c76aSAlexandre Belloni 	return 0;
1702a556c76aSAlexandre Belloni }
17035e256365SVladimir Oltean EXPORT_SYMBOL(ocelot_port_bridge_join);
1704a556c76aSAlexandre Belloni 
17055e256365SVladimir Oltean int ocelot_port_bridge_leave(struct ocelot *ocelot, int port,
1706a556c76aSAlexandre Belloni 			     struct net_device *bridge)
1707a556c76aSAlexandre Belloni {
170897bb69e1SVladimir Oltean 	ocelot->bridge_mask &= ~BIT(port);
1709a556c76aSAlexandre Belloni 
1710a556c76aSAlexandre Belloni 	if (!ocelot->bridge_mask)
1711a556c76aSAlexandre Belloni 		ocelot->hw_bridge_dev = NULL;
17127142529fSAntoine Tenart 
171397bb69e1SVladimir Oltean 	ocelot_port_vlan_filtering(ocelot, port, 0);
171497bb69e1SVladimir Oltean 	ocelot_port_set_pvid(ocelot, port, 0);
171597bb69e1SVladimir Oltean 	return ocelot_port_set_native_vlan(ocelot, port, 0);
1716a556c76aSAlexandre Belloni }
17175e256365SVladimir Oltean EXPORT_SYMBOL(ocelot_port_bridge_leave);
1718a556c76aSAlexandre Belloni 
1719dc96ee37SAlexandre Belloni static void ocelot_set_aggr_pgids(struct ocelot *ocelot)
1720dc96ee37SAlexandre Belloni {
1721dc96ee37SAlexandre Belloni 	int i, port, lag;
1722dc96ee37SAlexandre Belloni 
1723dc96ee37SAlexandre Belloni 	/* Reset destination and aggregation PGIDS */
1724dc96ee37SAlexandre Belloni 	for (port = 0; port < ocelot->num_phys_ports; port++)
1725dc96ee37SAlexandre Belloni 		ocelot_write_rix(ocelot, BIT(port), ANA_PGID_PGID, port);
1726dc96ee37SAlexandre Belloni 
1727dc96ee37SAlexandre Belloni 	for (i = PGID_AGGR; i < PGID_SRC; i++)
1728dc96ee37SAlexandre Belloni 		ocelot_write_rix(ocelot, GENMASK(ocelot->num_phys_ports - 1, 0),
1729dc96ee37SAlexandre Belloni 				 ANA_PGID_PGID, i);
1730dc96ee37SAlexandre Belloni 
1731dc96ee37SAlexandre Belloni 	/* Now, set PGIDs for each LAG */
1732dc96ee37SAlexandre Belloni 	for (lag = 0; lag < ocelot->num_phys_ports; lag++) {
1733dc96ee37SAlexandre Belloni 		unsigned long bond_mask;
1734dc96ee37SAlexandre Belloni 		int aggr_count = 0;
1735dc96ee37SAlexandre Belloni 		u8 aggr_idx[16];
1736dc96ee37SAlexandre Belloni 
1737dc96ee37SAlexandre Belloni 		bond_mask = ocelot->lags[lag];
1738dc96ee37SAlexandre Belloni 		if (!bond_mask)
1739dc96ee37SAlexandre Belloni 			continue;
1740dc96ee37SAlexandre Belloni 
1741dc96ee37SAlexandre Belloni 		for_each_set_bit(port, &bond_mask, ocelot->num_phys_ports) {
1742dc96ee37SAlexandre Belloni 			// Destination mask
1743dc96ee37SAlexandre Belloni 			ocelot_write_rix(ocelot, bond_mask,
1744dc96ee37SAlexandre Belloni 					 ANA_PGID_PGID, port);
1745dc96ee37SAlexandre Belloni 			aggr_idx[aggr_count] = port;
1746dc96ee37SAlexandre Belloni 			aggr_count++;
1747dc96ee37SAlexandre Belloni 		}
1748dc96ee37SAlexandre Belloni 
1749dc96ee37SAlexandre Belloni 		for (i = PGID_AGGR; i < PGID_SRC; i++) {
1750dc96ee37SAlexandre Belloni 			u32 ac;
1751dc96ee37SAlexandre Belloni 
1752dc96ee37SAlexandre Belloni 			ac = ocelot_read_rix(ocelot, ANA_PGID_PGID, i);
1753dc96ee37SAlexandre Belloni 			ac &= ~bond_mask;
1754dc96ee37SAlexandre Belloni 			ac |= BIT(aggr_idx[i % aggr_count]);
1755dc96ee37SAlexandre Belloni 			ocelot_write_rix(ocelot, ac, ANA_PGID_PGID, i);
1756dc96ee37SAlexandre Belloni 		}
1757dc96ee37SAlexandre Belloni 	}
1758dc96ee37SAlexandre Belloni }
1759dc96ee37SAlexandre Belloni 
1760dc96ee37SAlexandre Belloni static void ocelot_setup_lag(struct ocelot *ocelot, int lag)
1761dc96ee37SAlexandre Belloni {
1762dc96ee37SAlexandre Belloni 	unsigned long bond_mask = ocelot->lags[lag];
1763dc96ee37SAlexandre Belloni 	unsigned int p;
1764dc96ee37SAlexandre Belloni 
1765dc96ee37SAlexandre Belloni 	for_each_set_bit(p, &bond_mask, ocelot->num_phys_ports) {
1766dc96ee37SAlexandre Belloni 		u32 port_cfg = ocelot_read_gix(ocelot, ANA_PORT_PORT_CFG, p);
1767dc96ee37SAlexandre Belloni 
1768dc96ee37SAlexandre Belloni 		port_cfg &= ~ANA_PORT_PORT_CFG_PORTID_VAL_M;
1769dc96ee37SAlexandre Belloni 
1770dc96ee37SAlexandre Belloni 		/* Use lag port as logical port for port i */
1771dc96ee37SAlexandre Belloni 		ocelot_write_gix(ocelot, port_cfg |
1772dc96ee37SAlexandre Belloni 				 ANA_PORT_PORT_CFG_PORTID_VAL(lag),
1773dc96ee37SAlexandre Belloni 				 ANA_PORT_PORT_CFG, p);
1774dc96ee37SAlexandre Belloni 	}
1775dc96ee37SAlexandre Belloni }
1776dc96ee37SAlexandre Belloni 
1777f270dbfaSVladimir Oltean static int ocelot_port_lag_join(struct ocelot *ocelot, int port,
1778dc96ee37SAlexandre Belloni 				struct net_device *bond)
1779dc96ee37SAlexandre Belloni {
1780dc96ee37SAlexandre Belloni 	struct net_device *ndev;
1781dc96ee37SAlexandre Belloni 	u32 bond_mask = 0;
1782f270dbfaSVladimir Oltean 	int lag, lp;
1783dc96ee37SAlexandre Belloni 
1784dc96ee37SAlexandre Belloni 	rcu_read_lock();
1785dc96ee37SAlexandre Belloni 	for_each_netdev_in_bond_rcu(bond, ndev) {
1786004d44f6SVladimir Oltean 		struct ocelot_port_private *priv = netdev_priv(ndev);
1787dc96ee37SAlexandre Belloni 
1788004d44f6SVladimir Oltean 		bond_mask |= BIT(priv->chip_port);
1789dc96ee37SAlexandre Belloni 	}
1790dc96ee37SAlexandre Belloni 	rcu_read_unlock();
1791dc96ee37SAlexandre Belloni 
1792dc96ee37SAlexandre Belloni 	lp = __ffs(bond_mask);
1793dc96ee37SAlexandre Belloni 
1794dc96ee37SAlexandre Belloni 	/* If the new port is the lowest one, use it as the logical port from
1795dc96ee37SAlexandre Belloni 	 * now on
1796dc96ee37SAlexandre Belloni 	 */
1797f270dbfaSVladimir Oltean 	if (port == lp) {
1798f270dbfaSVladimir Oltean 		lag = port;
1799f270dbfaSVladimir Oltean 		ocelot->lags[port] = bond_mask;
1800f270dbfaSVladimir Oltean 		bond_mask &= ~BIT(port);
1801dc96ee37SAlexandre Belloni 		if (bond_mask) {
1802dc96ee37SAlexandre Belloni 			lp = __ffs(bond_mask);
1803dc96ee37SAlexandre Belloni 			ocelot->lags[lp] = 0;
1804dc96ee37SAlexandre Belloni 		}
1805dc96ee37SAlexandre Belloni 	} else {
1806dc96ee37SAlexandre Belloni 		lag = lp;
1807f270dbfaSVladimir Oltean 		ocelot->lags[lp] |= BIT(port);
1808dc96ee37SAlexandre Belloni 	}
1809dc96ee37SAlexandre Belloni 
1810dc96ee37SAlexandre Belloni 	ocelot_setup_lag(ocelot, lag);
1811dc96ee37SAlexandre Belloni 	ocelot_set_aggr_pgids(ocelot);
1812dc96ee37SAlexandre Belloni 
1813dc96ee37SAlexandre Belloni 	return 0;
1814dc96ee37SAlexandre Belloni }
1815dc96ee37SAlexandre Belloni 
1816f270dbfaSVladimir Oltean static void ocelot_port_lag_leave(struct ocelot *ocelot, int port,
1817dc96ee37SAlexandre Belloni 				  struct net_device *bond)
1818dc96ee37SAlexandre Belloni {
1819dc96ee37SAlexandre Belloni 	u32 port_cfg;
1820dc96ee37SAlexandre Belloni 	int i;
1821dc96ee37SAlexandre Belloni 
1822dc96ee37SAlexandre Belloni 	/* Remove port from any lag */
1823dc96ee37SAlexandre Belloni 	for (i = 0; i < ocelot->num_phys_ports; i++)
1824f270dbfaSVladimir Oltean 		ocelot->lags[i] &= ~BIT(port);
1825dc96ee37SAlexandre Belloni 
1826dc96ee37SAlexandre Belloni 	/* if it was the logical port of the lag, move the lag config to the
1827dc96ee37SAlexandre Belloni 	 * next port
1828dc96ee37SAlexandre Belloni 	 */
1829f270dbfaSVladimir Oltean 	if (ocelot->lags[port]) {
1830f270dbfaSVladimir Oltean 		int n = __ffs(ocelot->lags[port]);
1831dc96ee37SAlexandre Belloni 
1832f270dbfaSVladimir Oltean 		ocelot->lags[n] = ocelot->lags[port];
1833f270dbfaSVladimir Oltean 		ocelot->lags[port] = 0;
1834dc96ee37SAlexandre Belloni 
1835dc96ee37SAlexandre Belloni 		ocelot_setup_lag(ocelot, n);
1836dc96ee37SAlexandre Belloni 	}
1837dc96ee37SAlexandre Belloni 
1838f270dbfaSVladimir Oltean 	port_cfg = ocelot_read_gix(ocelot, ANA_PORT_PORT_CFG, port);
1839dc96ee37SAlexandre Belloni 	port_cfg &= ~ANA_PORT_PORT_CFG_PORTID_VAL_M;
1840f270dbfaSVladimir Oltean 	ocelot_write_gix(ocelot, port_cfg | ANA_PORT_PORT_CFG_PORTID_VAL(port),
1841f270dbfaSVladimir Oltean 			 ANA_PORT_PORT_CFG, port);
1842dc96ee37SAlexandre Belloni 
1843dc96ee37SAlexandre Belloni 	ocelot_set_aggr_pgids(ocelot);
1844dc96ee37SAlexandre Belloni }
1845dc96ee37SAlexandre Belloni 
1846a556c76aSAlexandre Belloni /* Checks if the net_device instance given to us originate from our driver. */
1847a556c76aSAlexandre Belloni static bool ocelot_netdevice_dev_check(const struct net_device *dev)
1848a556c76aSAlexandre Belloni {
1849a556c76aSAlexandre Belloni 	return dev->netdev_ops == &ocelot_port_netdev_ops;
1850a556c76aSAlexandre Belloni }
1851a556c76aSAlexandre Belloni 
1852a556c76aSAlexandre Belloni static int ocelot_netdevice_port_event(struct net_device *dev,
1853a556c76aSAlexandre Belloni 				       unsigned long event,
1854a556c76aSAlexandre Belloni 				       struct netdev_notifier_changeupper_info *info)
1855a556c76aSAlexandre Belloni {
1856004d44f6SVladimir Oltean 	struct ocelot_port_private *priv = netdev_priv(dev);
1857004d44f6SVladimir Oltean 	struct ocelot_port *ocelot_port = &priv->port;
1858f270dbfaSVladimir Oltean 	struct ocelot *ocelot = ocelot_port->ocelot;
1859004d44f6SVladimir Oltean 	int port = priv->chip_port;
1860a556c76aSAlexandre Belloni 	int err = 0;
1861a556c76aSAlexandre Belloni 
1862a556c76aSAlexandre Belloni 	switch (event) {
1863a556c76aSAlexandre Belloni 	case NETDEV_CHANGEUPPER:
1864a556c76aSAlexandre Belloni 		if (netif_is_bridge_master(info->upper_dev)) {
1865004d44f6SVladimir Oltean 			if (info->linking) {
1866f270dbfaSVladimir Oltean 				err = ocelot_port_bridge_join(ocelot, port,
1867a556c76aSAlexandre Belloni 							      info->upper_dev);
1868004d44f6SVladimir Oltean 			} else {
1869f270dbfaSVladimir Oltean 				err = ocelot_port_bridge_leave(ocelot, port,
1870a556c76aSAlexandre Belloni 							       info->upper_dev);
1871004d44f6SVladimir Oltean 				priv->vlan_aware = false;
1872004d44f6SVladimir Oltean 			}
1873a556c76aSAlexandre Belloni 		}
1874dc96ee37SAlexandre Belloni 		if (netif_is_lag_master(info->upper_dev)) {
1875dc96ee37SAlexandre Belloni 			if (info->linking)
1876f270dbfaSVladimir Oltean 				err = ocelot_port_lag_join(ocelot, port,
1877dc96ee37SAlexandre Belloni 							   info->upper_dev);
1878dc96ee37SAlexandre Belloni 			else
1879f270dbfaSVladimir Oltean 				ocelot_port_lag_leave(ocelot, port,
1880dc96ee37SAlexandre Belloni 						      info->upper_dev);
1881dc96ee37SAlexandre Belloni 		}
1882a556c76aSAlexandre Belloni 		break;
1883a556c76aSAlexandre Belloni 	default:
1884a556c76aSAlexandre Belloni 		break;
1885a556c76aSAlexandre Belloni 	}
1886a556c76aSAlexandre Belloni 
1887a556c76aSAlexandre Belloni 	return err;
1888a556c76aSAlexandre Belloni }
1889a556c76aSAlexandre Belloni 
1890a556c76aSAlexandre Belloni static int ocelot_netdevice_event(struct notifier_block *unused,
1891a556c76aSAlexandre Belloni 				  unsigned long event, void *ptr)
1892a556c76aSAlexandre Belloni {
1893a556c76aSAlexandre Belloni 	struct netdev_notifier_changeupper_info *info = ptr;
1894a556c76aSAlexandre Belloni 	struct net_device *dev = netdev_notifier_info_to_dev(ptr);
18952ac0e152SGeert Uytterhoeven 	int ret = 0;
1896a556c76aSAlexandre Belloni 
18977afb3e57SClaudiu Manoil 	if (!ocelot_netdevice_dev_check(dev))
18987afb3e57SClaudiu Manoil 		return 0;
18997afb3e57SClaudiu Manoil 
1900dc96ee37SAlexandre Belloni 	if (event == NETDEV_PRECHANGEUPPER &&
1901dc96ee37SAlexandre Belloni 	    netif_is_lag_master(info->upper_dev)) {
1902dc96ee37SAlexandre Belloni 		struct netdev_lag_upper_info *lag_upper_info = info->upper_info;
1903dc96ee37SAlexandre Belloni 		struct netlink_ext_ack *extack;
1904dc96ee37SAlexandre Belloni 
19053b3eed8eSClaudiu Manoil 		if (lag_upper_info &&
19063b3eed8eSClaudiu Manoil 		    lag_upper_info->tx_type != NETDEV_LAG_TX_TYPE_HASH) {
1907dc96ee37SAlexandre Belloni 			extack = netdev_notifier_info_to_extack(&info->info);
1908dc96ee37SAlexandre Belloni 			NL_SET_ERR_MSG_MOD(extack, "LAG device using unsupported Tx type");
1909dc96ee37SAlexandre Belloni 
1910dc96ee37SAlexandre Belloni 			ret = -EINVAL;
1911dc96ee37SAlexandre Belloni 			goto notify;
1912dc96ee37SAlexandre Belloni 		}
1913dc96ee37SAlexandre Belloni 	}
1914dc96ee37SAlexandre Belloni 
1915a556c76aSAlexandre Belloni 	if (netif_is_lag_master(dev)) {
1916a556c76aSAlexandre Belloni 		struct net_device *slave;
1917a556c76aSAlexandre Belloni 		struct list_head *iter;
1918a556c76aSAlexandre Belloni 
1919a556c76aSAlexandre Belloni 		netdev_for_each_lower_dev(dev, slave, iter) {
1920a556c76aSAlexandre Belloni 			ret = ocelot_netdevice_port_event(slave, event, info);
1921a556c76aSAlexandre Belloni 			if (ret)
1922a556c76aSAlexandre Belloni 				goto notify;
1923a556c76aSAlexandre Belloni 		}
1924a556c76aSAlexandre Belloni 	} else {
1925a556c76aSAlexandre Belloni 		ret = ocelot_netdevice_port_event(dev, event, info);
1926a556c76aSAlexandre Belloni 	}
1927a556c76aSAlexandre Belloni 
1928a556c76aSAlexandre Belloni notify:
1929a556c76aSAlexandre Belloni 	return notifier_from_errno(ret);
1930a556c76aSAlexandre Belloni }
1931a556c76aSAlexandre Belloni 
1932a556c76aSAlexandre Belloni struct notifier_block ocelot_netdevice_nb __read_mostly = {
1933a556c76aSAlexandre Belloni 	.notifier_call = ocelot_netdevice_event,
1934a556c76aSAlexandre Belloni };
1935a556c76aSAlexandre Belloni EXPORT_SYMBOL(ocelot_netdevice_nb);
1936a556c76aSAlexandre Belloni 
193756da64bcSFlorian Fainelli static int ocelot_switchdev_event(struct notifier_block *unused,
193856da64bcSFlorian Fainelli 				  unsigned long event, void *ptr)
193956da64bcSFlorian Fainelli {
194056da64bcSFlorian Fainelli 	struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
194156da64bcSFlorian Fainelli 	int err;
194256da64bcSFlorian Fainelli 
194356da64bcSFlorian Fainelli 	switch (event) {
194456da64bcSFlorian Fainelli 	case SWITCHDEV_PORT_ATTR_SET:
194556da64bcSFlorian Fainelli 		err = switchdev_handle_port_attr_set(dev, ptr,
194656da64bcSFlorian Fainelli 						     ocelot_netdevice_dev_check,
194756da64bcSFlorian Fainelli 						     ocelot_port_attr_set);
194856da64bcSFlorian Fainelli 		return notifier_from_errno(err);
194956da64bcSFlorian Fainelli 	}
195056da64bcSFlorian Fainelli 
195156da64bcSFlorian Fainelli 	return NOTIFY_DONE;
195256da64bcSFlorian Fainelli }
195356da64bcSFlorian Fainelli 
195456da64bcSFlorian Fainelli struct notifier_block ocelot_switchdev_nb __read_mostly = {
195556da64bcSFlorian Fainelli 	.notifier_call = ocelot_switchdev_event,
195656da64bcSFlorian Fainelli };
195756da64bcSFlorian Fainelli EXPORT_SYMBOL(ocelot_switchdev_nb);
195856da64bcSFlorian Fainelli 
19590e332c85SPetr Machata static int ocelot_switchdev_blocking_event(struct notifier_block *unused,
19600e332c85SPetr Machata 					   unsigned long event, void *ptr)
19610e332c85SPetr Machata {
19620e332c85SPetr Machata 	struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
19630e332c85SPetr Machata 	int err;
19640e332c85SPetr Machata 
19650e332c85SPetr Machata 	switch (event) {
19660e332c85SPetr Machata 		/* Blocking events. */
19670e332c85SPetr Machata 	case SWITCHDEV_PORT_OBJ_ADD:
19680e332c85SPetr Machata 		err = switchdev_handle_port_obj_add(dev, ptr,
19690e332c85SPetr Machata 						    ocelot_netdevice_dev_check,
19700e332c85SPetr Machata 						    ocelot_port_obj_add);
19710e332c85SPetr Machata 		return notifier_from_errno(err);
19720e332c85SPetr Machata 	case SWITCHDEV_PORT_OBJ_DEL:
19730e332c85SPetr Machata 		err = switchdev_handle_port_obj_del(dev, ptr,
19740e332c85SPetr Machata 						    ocelot_netdevice_dev_check,
19750e332c85SPetr Machata 						    ocelot_port_obj_del);
19760e332c85SPetr Machata 		return notifier_from_errno(err);
197756da64bcSFlorian Fainelli 	case SWITCHDEV_PORT_ATTR_SET:
197856da64bcSFlorian Fainelli 		err = switchdev_handle_port_attr_set(dev, ptr,
197956da64bcSFlorian Fainelli 						     ocelot_netdevice_dev_check,
198056da64bcSFlorian Fainelli 						     ocelot_port_attr_set);
198156da64bcSFlorian Fainelli 		return notifier_from_errno(err);
19820e332c85SPetr Machata 	}
19830e332c85SPetr Machata 
19840e332c85SPetr Machata 	return NOTIFY_DONE;
19850e332c85SPetr Machata }
19860e332c85SPetr Machata 
19870e332c85SPetr Machata struct notifier_block ocelot_switchdev_blocking_nb __read_mostly = {
19880e332c85SPetr Machata 	.notifier_call = ocelot_switchdev_blocking_event,
19890e332c85SPetr Machata };
19900e332c85SPetr Machata EXPORT_SYMBOL(ocelot_switchdev_blocking_nb);
19910e332c85SPetr Machata 
19924e3b0468SAntoine Tenart int ocelot_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts)
19934e3b0468SAntoine Tenart {
19944e3b0468SAntoine Tenart 	struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info);
19954e3b0468SAntoine Tenart 	unsigned long flags;
19964e3b0468SAntoine Tenart 	time64_t s;
19974e3b0468SAntoine Tenart 	u32 val;
19984e3b0468SAntoine Tenart 	s64 ns;
19994e3b0468SAntoine Tenart 
20004e3b0468SAntoine Tenart 	spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
20014e3b0468SAntoine Tenart 
20024e3b0468SAntoine Tenart 	val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
20034e3b0468SAntoine Tenart 	val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
20044e3b0468SAntoine Tenart 	val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE);
20054e3b0468SAntoine Tenart 	ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
20064e3b0468SAntoine Tenart 
20074e3b0468SAntoine Tenart 	s = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_MSB, TOD_ACC_PIN) & 0xffff;
20084e3b0468SAntoine Tenart 	s <<= 32;
20094e3b0468SAntoine Tenart 	s += ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
20104e3b0468SAntoine Tenart 	ns = ocelot_read_rix(ocelot, PTP_PIN_TOD_NSEC, TOD_ACC_PIN);
20114e3b0468SAntoine Tenart 
20124e3b0468SAntoine Tenart 	spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
20134e3b0468SAntoine Tenart 
20144e3b0468SAntoine Tenart 	/* Deal with negative values */
20154e3b0468SAntoine Tenart 	if (ns >= 0x3ffffff0 && ns <= 0x3fffffff) {
20164e3b0468SAntoine Tenart 		s--;
20174e3b0468SAntoine Tenart 		ns &= 0xf;
20184e3b0468SAntoine Tenart 		ns += 999999984;
20194e3b0468SAntoine Tenart 	}
20204e3b0468SAntoine Tenart 
20214e3b0468SAntoine Tenart 	set_normalized_timespec64(ts, s, ns);
20224e3b0468SAntoine Tenart 	return 0;
20234e3b0468SAntoine Tenart }
20244e3b0468SAntoine Tenart EXPORT_SYMBOL(ocelot_ptp_gettime64);
20254e3b0468SAntoine Tenart 
20264e3b0468SAntoine Tenart static int ocelot_ptp_settime64(struct ptp_clock_info *ptp,
20274e3b0468SAntoine Tenart 				const struct timespec64 *ts)
20284e3b0468SAntoine Tenart {
20294e3b0468SAntoine Tenart 	struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info);
20304e3b0468SAntoine Tenart 	unsigned long flags;
20314e3b0468SAntoine Tenart 	u32 val;
20324e3b0468SAntoine Tenart 
20334e3b0468SAntoine Tenart 	spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
20344e3b0468SAntoine Tenart 
20354e3b0468SAntoine Tenart 	val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
20364e3b0468SAntoine Tenart 	val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
20374e3b0468SAntoine Tenart 	val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_IDLE);
20384e3b0468SAntoine Tenart 
20394e3b0468SAntoine Tenart 	ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
20404e3b0468SAntoine Tenart 
20414e3b0468SAntoine Tenart 	ocelot_write_rix(ocelot, lower_32_bits(ts->tv_sec), PTP_PIN_TOD_SEC_LSB,
20424e3b0468SAntoine Tenart 			 TOD_ACC_PIN);
20434e3b0468SAntoine Tenart 	ocelot_write_rix(ocelot, upper_32_bits(ts->tv_sec), PTP_PIN_TOD_SEC_MSB,
20444e3b0468SAntoine Tenart 			 TOD_ACC_PIN);
20454e3b0468SAntoine Tenart 	ocelot_write_rix(ocelot, ts->tv_nsec, PTP_PIN_TOD_NSEC, TOD_ACC_PIN);
20464e3b0468SAntoine Tenart 
20474e3b0468SAntoine Tenart 	val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
20484e3b0468SAntoine Tenart 	val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
20494e3b0468SAntoine Tenart 	val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_LOAD);
20504e3b0468SAntoine Tenart 
20514e3b0468SAntoine Tenart 	ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
20524e3b0468SAntoine Tenart 
20534e3b0468SAntoine Tenart 	spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
20544e3b0468SAntoine Tenart 	return 0;
20554e3b0468SAntoine Tenart }
20564e3b0468SAntoine Tenart 
20574e3b0468SAntoine Tenart static int ocelot_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
20584e3b0468SAntoine Tenart {
20594e3b0468SAntoine Tenart 	if (delta > -(NSEC_PER_SEC / 2) && delta < (NSEC_PER_SEC / 2)) {
20604e3b0468SAntoine Tenart 		struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info);
20614e3b0468SAntoine Tenart 		unsigned long flags;
20624e3b0468SAntoine Tenart 		u32 val;
20634e3b0468SAntoine Tenart 
20644e3b0468SAntoine Tenart 		spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
20654e3b0468SAntoine Tenart 
20664e3b0468SAntoine Tenart 		val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
20674e3b0468SAntoine Tenart 		val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
20684e3b0468SAntoine Tenart 		val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_IDLE);
20694e3b0468SAntoine Tenart 
20704e3b0468SAntoine Tenart 		ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
20714e3b0468SAntoine Tenart 
20724e3b0468SAntoine Tenart 		ocelot_write_rix(ocelot, 0, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
20734e3b0468SAntoine Tenart 		ocelot_write_rix(ocelot, 0, PTP_PIN_TOD_SEC_MSB, TOD_ACC_PIN);
20744e3b0468SAntoine Tenart 		ocelot_write_rix(ocelot, delta, PTP_PIN_TOD_NSEC, TOD_ACC_PIN);
20754e3b0468SAntoine Tenart 
20764e3b0468SAntoine Tenart 		val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
20774e3b0468SAntoine Tenart 		val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
20784e3b0468SAntoine Tenart 		val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_DELTA);
20794e3b0468SAntoine Tenart 
20804e3b0468SAntoine Tenart 		ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
20814e3b0468SAntoine Tenart 
20824e3b0468SAntoine Tenart 		spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
20834e3b0468SAntoine Tenart 	} else {
20844e3b0468SAntoine Tenart 		/* Fall back using ocelot_ptp_settime64 which is not exact. */
20854e3b0468SAntoine Tenart 		struct timespec64 ts;
20864e3b0468SAntoine Tenart 		u64 now;
20874e3b0468SAntoine Tenart 
20884e3b0468SAntoine Tenart 		ocelot_ptp_gettime64(ptp, &ts);
20894e3b0468SAntoine Tenart 
20904e3b0468SAntoine Tenart 		now = ktime_to_ns(timespec64_to_ktime(ts));
20914e3b0468SAntoine Tenart 		ts = ns_to_timespec64(now + delta);
20924e3b0468SAntoine Tenart 
20934e3b0468SAntoine Tenart 		ocelot_ptp_settime64(ptp, &ts);
20944e3b0468SAntoine Tenart 	}
20954e3b0468SAntoine Tenart 	return 0;
20964e3b0468SAntoine Tenart }
20974e3b0468SAntoine Tenart 
20984e3b0468SAntoine Tenart static int ocelot_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
20994e3b0468SAntoine Tenart {
21004e3b0468SAntoine Tenart 	struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info);
21014e3b0468SAntoine Tenart 	u32 unit = 0, direction = 0;
21024e3b0468SAntoine Tenart 	unsigned long flags;
21034e3b0468SAntoine Tenart 	u64 adj = 0;
21044e3b0468SAntoine Tenart 
21054e3b0468SAntoine Tenart 	spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
21064e3b0468SAntoine Tenart 
21074e3b0468SAntoine Tenart 	if (!scaled_ppm)
21084e3b0468SAntoine Tenart 		goto disable_adj;
21094e3b0468SAntoine Tenart 
21104e3b0468SAntoine Tenart 	if (scaled_ppm < 0) {
21114e3b0468SAntoine Tenart 		direction = PTP_CFG_CLK_ADJ_CFG_DIR;
21124e3b0468SAntoine Tenart 		scaled_ppm = -scaled_ppm;
21134e3b0468SAntoine Tenart 	}
21144e3b0468SAntoine Tenart 
21154e3b0468SAntoine Tenart 	adj = PSEC_PER_SEC << 16;
21164e3b0468SAntoine Tenart 	do_div(adj, scaled_ppm);
21174e3b0468SAntoine Tenart 	do_div(adj, 1000);
21184e3b0468SAntoine Tenart 
21194e3b0468SAntoine Tenart 	/* If the adjustment value is too large, use ns instead */
21204e3b0468SAntoine Tenart 	if (adj >= (1L << 30)) {
21214e3b0468SAntoine Tenart 		unit = PTP_CFG_CLK_ADJ_FREQ_NS;
21224e3b0468SAntoine Tenart 		do_div(adj, 1000);
21234e3b0468SAntoine Tenart 	}
21244e3b0468SAntoine Tenart 
21254e3b0468SAntoine Tenart 	/* Still too big */
21264e3b0468SAntoine Tenart 	if (adj >= (1L << 30))
21274e3b0468SAntoine Tenart 		goto disable_adj;
21284e3b0468SAntoine Tenart 
21294e3b0468SAntoine Tenart 	ocelot_write(ocelot, unit | adj, PTP_CLK_CFG_ADJ_FREQ);
21304e3b0468SAntoine Tenart 	ocelot_write(ocelot, PTP_CFG_CLK_ADJ_CFG_ENA | direction,
21314e3b0468SAntoine Tenart 		     PTP_CLK_CFG_ADJ_CFG);
21324e3b0468SAntoine Tenart 
21334e3b0468SAntoine Tenart 	spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
21344e3b0468SAntoine Tenart 	return 0;
21354e3b0468SAntoine Tenart 
21364e3b0468SAntoine Tenart disable_adj:
21374e3b0468SAntoine Tenart 	ocelot_write(ocelot, 0, PTP_CLK_CFG_ADJ_CFG);
21384e3b0468SAntoine Tenart 
21394e3b0468SAntoine Tenart 	spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
21404e3b0468SAntoine Tenart 	return 0;
21414e3b0468SAntoine Tenart }
21424e3b0468SAntoine Tenart 
21434e3b0468SAntoine Tenart static struct ptp_clock_info ocelot_ptp_clock_info = {
21444e3b0468SAntoine Tenart 	.owner		= THIS_MODULE,
21454e3b0468SAntoine Tenart 	.name		= "ocelot ptp",
21464e3b0468SAntoine Tenart 	.max_adj	= 0x7fffffff,
21474e3b0468SAntoine Tenart 	.n_alarm	= 0,
21484e3b0468SAntoine Tenart 	.n_ext_ts	= 0,
21494e3b0468SAntoine Tenart 	.n_per_out	= 0,
21504e3b0468SAntoine Tenart 	.n_pins		= 0,
21514e3b0468SAntoine Tenart 	.pps		= 0,
21524e3b0468SAntoine Tenart 	.gettime64	= ocelot_ptp_gettime64,
21534e3b0468SAntoine Tenart 	.settime64	= ocelot_ptp_settime64,
21544e3b0468SAntoine Tenart 	.adjtime	= ocelot_ptp_adjtime,
21554e3b0468SAntoine Tenart 	.adjfine	= ocelot_ptp_adjfine,
21564e3b0468SAntoine Tenart };
21574e3b0468SAntoine Tenart 
21584e3b0468SAntoine Tenart static int ocelot_init_timestamp(struct ocelot *ocelot)
21594e3b0468SAntoine Tenart {
21604e3b0468SAntoine Tenart 	ocelot->ptp_info = ocelot_ptp_clock_info;
21614e3b0468SAntoine Tenart 	ocelot->ptp_clock = ptp_clock_register(&ocelot->ptp_info, ocelot->dev);
21624e3b0468SAntoine Tenart 	if (IS_ERR(ocelot->ptp_clock))
21634e3b0468SAntoine Tenart 		return PTR_ERR(ocelot->ptp_clock);
21644e3b0468SAntoine Tenart 	/* Check if PHC support is missing at the configuration level */
21654e3b0468SAntoine Tenart 	if (!ocelot->ptp_clock)
21664e3b0468SAntoine Tenart 		return 0;
21674e3b0468SAntoine Tenart 
21684e3b0468SAntoine Tenart 	ocelot_write(ocelot, SYS_PTP_CFG_PTP_STAMP_WID(30), SYS_PTP_CFG);
21694e3b0468SAntoine Tenart 	ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_LOW);
21704e3b0468SAntoine Tenart 	ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_HIGH);
21714e3b0468SAntoine Tenart 
21724e3b0468SAntoine Tenart 	ocelot_write(ocelot, PTP_CFG_MISC_PTP_EN, PTP_CFG_MISC);
21734e3b0468SAntoine Tenart 
21744e3b0468SAntoine Tenart 	/* There is no device reconfiguration, PTP Rx stamping is always
21754e3b0468SAntoine Tenart 	 * enabled.
21764e3b0468SAntoine Tenart 	 */
21774e3b0468SAntoine Tenart 	ocelot->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
21784e3b0468SAntoine Tenart 
21794e3b0468SAntoine Tenart 	return 0;
21804e3b0468SAntoine Tenart }
21814e3b0468SAntoine Tenart 
2182fa914e9cSVladimir Oltean static void ocelot_port_set_mtu(struct ocelot *ocelot, int port, size_t mtu)
218331350d7fSVladimir Oltean {
218431350d7fSVladimir Oltean 	struct ocelot_port *ocelot_port = ocelot->ports[port];
21855bc9d2e6SVladimir Oltean 	int atop_wm;
218631350d7fSVladimir Oltean 
2187fa914e9cSVladimir Oltean 	ocelot_port_writel(ocelot_port, mtu, DEV_MAC_MAXLEN_CFG);
2188fa914e9cSVladimir Oltean 
2189fa914e9cSVladimir Oltean 	/* Set Pause WM hysteresis
2190fa914e9cSVladimir Oltean 	 * 152 = 6 * mtu / OCELOT_BUFFER_CELL_SZ
2191fa914e9cSVladimir Oltean 	 * 101 = 4 * mtu / OCELOT_BUFFER_CELL_SZ
2192fa914e9cSVladimir Oltean 	 */
2193fa914e9cSVladimir Oltean 	ocelot_write_rix(ocelot, SYS_PAUSE_CFG_PAUSE_ENA |
2194fa914e9cSVladimir Oltean 			 SYS_PAUSE_CFG_PAUSE_STOP(101) |
2195fa914e9cSVladimir Oltean 			 SYS_PAUSE_CFG_PAUSE_START(152), SYS_PAUSE_CFG, port);
2196fa914e9cSVladimir Oltean 
2197fa914e9cSVladimir Oltean 	/* Tail dropping watermark */
2198fa914e9cSVladimir Oltean 	atop_wm = (ocelot->shared_queue_sz - 9 * mtu) / OCELOT_BUFFER_CELL_SZ;
2199fa914e9cSVladimir Oltean 	ocelot_write_rix(ocelot, ocelot_wm_enc(9 * mtu),
2200fa914e9cSVladimir Oltean 			 SYS_ATOP, port);
2201fa914e9cSVladimir Oltean 	ocelot_write(ocelot, ocelot_wm_enc(atop_wm), SYS_ATOP_TOT_CFG);
2202fa914e9cSVladimir Oltean }
2203fa914e9cSVladimir Oltean 
22045e256365SVladimir Oltean void ocelot_init_port(struct ocelot *ocelot, int port)
2205fa914e9cSVladimir Oltean {
2206fa914e9cSVladimir Oltean 	struct ocelot_port *ocelot_port = ocelot->ports[port];
2207fa914e9cSVladimir Oltean 
220831350d7fSVladimir Oltean 	INIT_LIST_HEAD(&ocelot_port->skbs);
220931350d7fSVladimir Oltean 
221031350d7fSVladimir Oltean 	/* Basic L2 initialization */
221131350d7fSVladimir Oltean 
22125bc9d2e6SVladimir Oltean 	/* Set MAC IFG Gaps
22135bc9d2e6SVladimir Oltean 	 * FDX: TX_IFG = 5, RX_IFG1 = RX_IFG2 = 0
22145bc9d2e6SVladimir Oltean 	 * !FDX: TX_IFG = 5, RX_IFG1 = RX_IFG2 = 5
22155bc9d2e6SVladimir Oltean 	 */
22165bc9d2e6SVladimir Oltean 	ocelot_port_writel(ocelot_port, DEV_MAC_IFG_CFG_TX_IFG(5),
22175bc9d2e6SVladimir Oltean 			   DEV_MAC_IFG_CFG);
22185bc9d2e6SVladimir Oltean 
22195bc9d2e6SVladimir Oltean 	/* Load seed (0) and set MAC HDX late collision  */
22205bc9d2e6SVladimir Oltean 	ocelot_port_writel(ocelot_port, DEV_MAC_HDX_CFG_LATE_COL_POS(67) |
22215bc9d2e6SVladimir Oltean 			   DEV_MAC_HDX_CFG_SEED_LOAD,
22225bc9d2e6SVladimir Oltean 			   DEV_MAC_HDX_CFG);
22235bc9d2e6SVladimir Oltean 	mdelay(1);
22245bc9d2e6SVladimir Oltean 	ocelot_port_writel(ocelot_port, DEV_MAC_HDX_CFG_LATE_COL_POS(67),
22255bc9d2e6SVladimir Oltean 			   DEV_MAC_HDX_CFG);
22265bc9d2e6SVladimir Oltean 
22275bc9d2e6SVladimir Oltean 	/* Set Max Length and maximum tags allowed */
2228fa914e9cSVladimir Oltean 	ocelot_port_set_mtu(ocelot, port, VLAN_ETH_FRAME_LEN);
22295bc9d2e6SVladimir Oltean 	ocelot_port_writel(ocelot_port, DEV_MAC_TAGS_CFG_TAG_ID(ETH_P_8021AD) |
22305bc9d2e6SVladimir Oltean 			   DEV_MAC_TAGS_CFG_VLAN_AWR_ENA |
22315bc9d2e6SVladimir Oltean 			   DEV_MAC_TAGS_CFG_VLAN_LEN_AWR_ENA,
22325bc9d2e6SVladimir Oltean 			   DEV_MAC_TAGS_CFG);
22335bc9d2e6SVladimir Oltean 
22345bc9d2e6SVladimir Oltean 	/* Set SMAC of Pause frame (00:00:00:00:00:00) */
22355bc9d2e6SVladimir Oltean 	ocelot_port_writel(ocelot_port, 0, DEV_MAC_FC_MAC_HIGH_CFG);
22365bc9d2e6SVladimir Oltean 	ocelot_port_writel(ocelot_port, 0, DEV_MAC_FC_MAC_LOW_CFG);
22375bc9d2e6SVladimir Oltean 
223831350d7fSVladimir Oltean 	/* Drop frames with multicast source address */
223931350d7fSVladimir Oltean 	ocelot_rmw_gix(ocelot, ANA_PORT_DROP_CFG_DROP_MC_SMAC_ENA,
224031350d7fSVladimir Oltean 		       ANA_PORT_DROP_CFG_DROP_MC_SMAC_ENA,
224131350d7fSVladimir Oltean 		       ANA_PORT_DROP_CFG, port);
224231350d7fSVladimir Oltean 
224331350d7fSVladimir Oltean 	/* Set default VLAN and tag type to 8021Q. */
224431350d7fSVladimir Oltean 	ocelot_rmw_gix(ocelot, REW_PORT_VLAN_CFG_PORT_TPID(ETH_P_8021Q),
224531350d7fSVladimir Oltean 		       REW_PORT_VLAN_CFG_PORT_TPID_M,
224631350d7fSVladimir Oltean 		       REW_PORT_VLAN_CFG, port);
224731350d7fSVladimir Oltean 
224831350d7fSVladimir Oltean 	/* Enable vcap lookups */
224931350d7fSVladimir Oltean 	ocelot_vcap_enable(ocelot, port);
225031350d7fSVladimir Oltean }
22515e256365SVladimir Oltean EXPORT_SYMBOL(ocelot_init_port);
225231350d7fSVladimir Oltean 
2253a556c76aSAlexandre Belloni int ocelot_probe_port(struct ocelot *ocelot, u8 port,
2254a556c76aSAlexandre Belloni 		      void __iomem *regs,
2255a556c76aSAlexandre Belloni 		      struct phy_device *phy)
2256a556c76aSAlexandre Belloni {
2257004d44f6SVladimir Oltean 	struct ocelot_port_private *priv;
2258a556c76aSAlexandre Belloni 	struct ocelot_port *ocelot_port;
2259a556c76aSAlexandre Belloni 	struct net_device *dev;
2260a556c76aSAlexandre Belloni 	int err;
2261a556c76aSAlexandre Belloni 
2262004d44f6SVladimir Oltean 	dev = alloc_etherdev(sizeof(struct ocelot_port_private));
2263a556c76aSAlexandre Belloni 	if (!dev)
2264a556c76aSAlexandre Belloni 		return -ENOMEM;
2265a556c76aSAlexandre Belloni 	SET_NETDEV_DEV(dev, ocelot->dev);
2266004d44f6SVladimir Oltean 	priv = netdev_priv(dev);
2267004d44f6SVladimir Oltean 	priv->dev = dev;
2268004d44f6SVladimir Oltean 	priv->phy = phy;
2269004d44f6SVladimir Oltean 	priv->chip_port = port;
2270004d44f6SVladimir Oltean 	ocelot_port = &priv->port;
2271a556c76aSAlexandre Belloni 	ocelot_port->ocelot = ocelot;
2272a556c76aSAlexandre Belloni 	ocelot_port->regs = regs;
2273a556c76aSAlexandre Belloni 	ocelot->ports[port] = ocelot_port;
2274a556c76aSAlexandre Belloni 
2275a556c76aSAlexandre Belloni 	dev->netdev_ops = &ocelot_port_netdev_ops;
2276a556c76aSAlexandre Belloni 	dev->ethtool_ops = &ocelot_ethtool_ops;
2277a556c76aSAlexandre Belloni 
22782c1d029aSJoergen Andreasen 	dev->hw_features |= NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_RXFCS |
22792c1d029aSJoergen Andreasen 		NETIF_F_HW_TC;
22802c1d029aSJoergen Andreasen 	dev->features |= NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_HW_TC;
22817142529fSAntoine Tenart 
2282a556c76aSAlexandre Belloni 	memcpy(dev->dev_addr, ocelot->base_mac, ETH_ALEN);
2283a556c76aSAlexandre Belloni 	dev->dev_addr[ETH_ALEN - 1] += port;
2284a556c76aSAlexandre Belloni 	ocelot_mact_learn(ocelot, PGID_CPU, dev->dev_addr, ocelot_port->pvid,
2285a556c76aSAlexandre Belloni 			  ENTRYTYPE_LOCKED);
2286a556c76aSAlexandre Belloni 
228731350d7fSVladimir Oltean 	ocelot_init_port(ocelot, port);
22884e3b0468SAntoine Tenart 
2289a556c76aSAlexandre Belloni 	err = register_netdev(dev);
2290a556c76aSAlexandre Belloni 	if (err) {
2291a556c76aSAlexandre Belloni 		dev_err(ocelot->dev, "register_netdev failed\n");
229231350d7fSVladimir Oltean 		free_netdev(dev);
2293a556c76aSAlexandre Belloni 	}
2294a556c76aSAlexandre Belloni 
2295a556c76aSAlexandre Belloni 	return err;
2296a556c76aSAlexandre Belloni }
2297a556c76aSAlexandre Belloni EXPORT_SYMBOL(ocelot_probe_port);
2298a556c76aSAlexandre Belloni 
229921468199SVladimir Oltean void ocelot_set_cpu_port(struct ocelot *ocelot, int cpu,
230021468199SVladimir Oltean 			 enum ocelot_tag_prefix injection,
230121468199SVladimir Oltean 			 enum ocelot_tag_prefix extraction)
230221468199SVladimir Oltean {
230321468199SVladimir Oltean 	/* Configure and enable the CPU port. */
230421468199SVladimir Oltean 	ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, cpu);
230521468199SVladimir Oltean 	ocelot_write_rix(ocelot, BIT(cpu), ANA_PGID_PGID, PGID_CPU);
230621468199SVladimir Oltean 	ocelot_write_gix(ocelot, ANA_PORT_PORT_CFG_RECV_ENA |
230721468199SVladimir Oltean 			 ANA_PORT_PORT_CFG_PORTID_VAL(cpu),
230821468199SVladimir Oltean 			 ANA_PORT_PORT_CFG, cpu);
230921468199SVladimir Oltean 
231021468199SVladimir Oltean 	/* If the CPU port is a physical port, set up the port in Node
231121468199SVladimir Oltean 	 * Processor Interface (NPI) mode. This is the mode through which
231221468199SVladimir Oltean 	 * frames can be injected from and extracted to an external CPU.
231321468199SVladimir Oltean 	 * Only one port can be an NPI at the same time.
231421468199SVladimir Oltean 	 */
231521468199SVladimir Oltean 	if (cpu < ocelot->num_phys_ports) {
2316ba551bc3SVladimir Oltean 		int mtu = VLAN_ETH_FRAME_LEN + OCELOT_TAG_LEN;
2317ba551bc3SVladimir Oltean 
231821468199SVladimir Oltean 		ocelot_write(ocelot, QSYS_EXT_CPU_CFG_EXT_CPUQ_MSK_M |
231921468199SVladimir Oltean 			     QSYS_EXT_CPU_CFG_EXT_CPU_PORT(cpu),
232021468199SVladimir Oltean 			     QSYS_EXT_CPU_CFG);
2321ba551bc3SVladimir Oltean 
2322ba551bc3SVladimir Oltean 		if (injection == OCELOT_TAG_PREFIX_SHORT)
2323ba551bc3SVladimir Oltean 			mtu += OCELOT_SHORT_PREFIX_LEN;
2324ba551bc3SVladimir Oltean 		else if (injection == OCELOT_TAG_PREFIX_LONG)
2325ba551bc3SVladimir Oltean 			mtu += OCELOT_LONG_PREFIX_LEN;
2326ba551bc3SVladimir Oltean 
2327ba551bc3SVladimir Oltean 		ocelot_port_set_mtu(ocelot, cpu, mtu);
232821468199SVladimir Oltean 	}
232921468199SVladimir Oltean 
233021468199SVladimir Oltean 	/* CPU port Injection/Extraction configuration */
233121468199SVladimir Oltean 	ocelot_write_rix(ocelot, QSYS_SWITCH_PORT_MODE_INGRESS_DROP_MODE |
233221468199SVladimir Oltean 			 QSYS_SWITCH_PORT_MODE_SCH_NEXT_CFG(1) |
233321468199SVladimir Oltean 			 QSYS_SWITCH_PORT_MODE_PORT_ENA,
233421468199SVladimir Oltean 			 QSYS_SWITCH_PORT_MODE, cpu);
233521468199SVladimir Oltean 	ocelot_write_rix(ocelot, SYS_PORT_MODE_INCL_XTR_HDR(extraction) |
233621468199SVladimir Oltean 			 SYS_PORT_MODE_INCL_INJ_HDR(injection),
233721468199SVladimir Oltean 			 SYS_PORT_MODE, cpu);
233821468199SVladimir Oltean 
233921468199SVladimir Oltean 	/* Configure the CPU port to be VLAN aware */
234021468199SVladimir Oltean 	ocelot_write_gix(ocelot, ANA_PORT_VLAN_CFG_VLAN_VID(0) |
234121468199SVladimir Oltean 				 ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
234221468199SVladimir Oltean 				 ANA_PORT_VLAN_CFG_VLAN_POP_CNT(1),
234321468199SVladimir Oltean 			 ANA_PORT_VLAN_CFG, cpu);
234421468199SVladimir Oltean 
234521468199SVladimir Oltean 	ocelot->cpu = cpu;
234621468199SVladimir Oltean }
234721468199SVladimir Oltean EXPORT_SYMBOL(ocelot_set_cpu_port);
234821468199SVladimir Oltean 
2349a556c76aSAlexandre Belloni int ocelot_init(struct ocelot *ocelot)
2350a556c76aSAlexandre Belloni {
2351a556c76aSAlexandre Belloni 	char queue_name[32];
235221468199SVladimir Oltean 	int i, ret;
235321468199SVladimir Oltean 	u32 port;
2354a556c76aSAlexandre Belloni 
23553a77b593SVladimir Oltean 	if (ocelot->ops->reset) {
23563a77b593SVladimir Oltean 		ret = ocelot->ops->reset(ocelot);
23573a77b593SVladimir Oltean 		if (ret) {
23583a77b593SVladimir Oltean 			dev_err(ocelot->dev, "Switch reset failed\n");
23593a77b593SVladimir Oltean 			return ret;
23603a77b593SVladimir Oltean 		}
23613a77b593SVladimir Oltean 	}
23623a77b593SVladimir Oltean 
2363dc96ee37SAlexandre Belloni 	ocelot->lags = devm_kcalloc(ocelot->dev, ocelot->num_phys_ports,
2364dc96ee37SAlexandre Belloni 				    sizeof(u32), GFP_KERNEL);
2365dc96ee37SAlexandre Belloni 	if (!ocelot->lags)
2366dc96ee37SAlexandre Belloni 		return -ENOMEM;
2367dc96ee37SAlexandre Belloni 
2368a556c76aSAlexandre Belloni 	ocelot->stats = devm_kcalloc(ocelot->dev,
2369a556c76aSAlexandre Belloni 				     ocelot->num_phys_ports * ocelot->num_stats,
2370a556c76aSAlexandre Belloni 				     sizeof(u64), GFP_KERNEL);
2371a556c76aSAlexandre Belloni 	if (!ocelot->stats)
2372a556c76aSAlexandre Belloni 		return -ENOMEM;
2373a556c76aSAlexandre Belloni 
2374a556c76aSAlexandre Belloni 	mutex_init(&ocelot->stats_lock);
23754e3b0468SAntoine Tenart 	mutex_init(&ocelot->ptp_lock);
23764e3b0468SAntoine Tenart 	spin_lock_init(&ocelot->ptp_clock_lock);
2377a556c76aSAlexandre Belloni 	snprintf(queue_name, sizeof(queue_name), "%s-stats",
2378a556c76aSAlexandre Belloni 		 dev_name(ocelot->dev));
2379a556c76aSAlexandre Belloni 	ocelot->stats_queue = create_singlethread_workqueue(queue_name);
2380a556c76aSAlexandre Belloni 	if (!ocelot->stats_queue)
2381a556c76aSAlexandre Belloni 		return -ENOMEM;
2382a556c76aSAlexandre Belloni 
23832b120ddeSClaudiu Manoil 	INIT_LIST_HEAD(&ocelot->multicast);
2384a556c76aSAlexandre Belloni 	ocelot_mact_init(ocelot);
2385a556c76aSAlexandre Belloni 	ocelot_vlan_init(ocelot);
2386b5962294SHoratiu Vultur 	ocelot_ace_init(ocelot);
2387a556c76aSAlexandre Belloni 
2388a556c76aSAlexandre Belloni 	for (port = 0; port < ocelot->num_phys_ports; port++) {
2389a556c76aSAlexandre Belloni 		/* Clear all counters (5 groups) */
2390a556c76aSAlexandre Belloni 		ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(port) |
2391a556c76aSAlexandre Belloni 				     SYS_STAT_CFG_STAT_CLEAR_SHOT(0x7f),
2392a556c76aSAlexandre Belloni 			     SYS_STAT_CFG);
2393a556c76aSAlexandre Belloni 	}
2394a556c76aSAlexandre Belloni 
2395a556c76aSAlexandre Belloni 	/* Only use S-Tag */
2396a556c76aSAlexandre Belloni 	ocelot_write(ocelot, ETH_P_8021AD, SYS_VLAN_ETYPE_CFG);
2397a556c76aSAlexandre Belloni 
2398a556c76aSAlexandre Belloni 	/* Aggregation mode */
2399a556c76aSAlexandre Belloni 	ocelot_write(ocelot, ANA_AGGR_CFG_AC_SMAC_ENA |
2400a556c76aSAlexandre Belloni 			     ANA_AGGR_CFG_AC_DMAC_ENA |
2401a556c76aSAlexandre Belloni 			     ANA_AGGR_CFG_AC_IP4_SIPDIP_ENA |
2402a556c76aSAlexandre Belloni 			     ANA_AGGR_CFG_AC_IP4_TCPUDP_ENA, ANA_AGGR_CFG);
2403a556c76aSAlexandre Belloni 
2404a556c76aSAlexandre Belloni 	/* Set MAC age time to default value. The entry is aged after
2405a556c76aSAlexandre Belloni 	 * 2*AGE_PERIOD
2406a556c76aSAlexandre Belloni 	 */
2407a556c76aSAlexandre Belloni 	ocelot_write(ocelot,
2408a556c76aSAlexandre Belloni 		     ANA_AUTOAGE_AGE_PERIOD(BR_DEFAULT_AGEING_TIME / 2 / HZ),
2409a556c76aSAlexandre Belloni 		     ANA_AUTOAGE);
2410a556c76aSAlexandre Belloni 
2411a556c76aSAlexandre Belloni 	/* Disable learning for frames discarded by VLAN ingress filtering */
2412a556c76aSAlexandre Belloni 	regmap_field_write(ocelot->regfields[ANA_ADVLEARN_VLAN_CHK], 1);
2413a556c76aSAlexandre Belloni 
2414a556c76aSAlexandre Belloni 	/* Setup frame ageing - fixed value "2 sec" - in 6.5 us units */
2415a556c76aSAlexandre Belloni 	ocelot_write(ocelot, SYS_FRM_AGING_AGE_TX_ENA |
2416a556c76aSAlexandre Belloni 		     SYS_FRM_AGING_MAX_AGE(307692), SYS_FRM_AGING);
2417a556c76aSAlexandre Belloni 
2418a556c76aSAlexandre Belloni 	/* Setup flooding PGIDs */
2419a556c76aSAlexandre Belloni 	ocelot_write_rix(ocelot, ANA_FLOODING_FLD_MULTICAST(PGID_MC) |
2420a556c76aSAlexandre Belloni 			 ANA_FLOODING_FLD_BROADCAST(PGID_MC) |
2421a556c76aSAlexandre Belloni 			 ANA_FLOODING_FLD_UNICAST(PGID_UC),
2422a556c76aSAlexandre Belloni 			 ANA_FLOODING, 0);
2423a556c76aSAlexandre Belloni 	ocelot_write(ocelot, ANA_FLOODING_IPMC_FLD_MC6_DATA(PGID_MCIPV6) |
2424a556c76aSAlexandre Belloni 		     ANA_FLOODING_IPMC_FLD_MC6_CTRL(PGID_MC) |
2425a556c76aSAlexandre Belloni 		     ANA_FLOODING_IPMC_FLD_MC4_DATA(PGID_MCIPV4) |
2426a556c76aSAlexandre Belloni 		     ANA_FLOODING_IPMC_FLD_MC4_CTRL(PGID_MC),
2427a556c76aSAlexandre Belloni 		     ANA_FLOODING_IPMC);
2428a556c76aSAlexandre Belloni 
2429a556c76aSAlexandre Belloni 	for (port = 0; port < ocelot->num_phys_ports; port++) {
2430a556c76aSAlexandre Belloni 		/* Transmit the frame to the local port. */
2431a556c76aSAlexandre Belloni 		ocelot_write_rix(ocelot, BIT(port), ANA_PGID_PGID, port);
2432a556c76aSAlexandre Belloni 		/* Do not forward BPDU frames to the front ports. */
2433a556c76aSAlexandre Belloni 		ocelot_write_gix(ocelot,
2434a556c76aSAlexandre Belloni 				 ANA_PORT_CPU_FWD_BPDU_CFG_BPDU_REDIR_ENA(0xffff),
2435a556c76aSAlexandre Belloni 				 ANA_PORT_CPU_FWD_BPDU_CFG,
2436a556c76aSAlexandre Belloni 				 port);
2437a556c76aSAlexandre Belloni 		/* Ensure bridging is disabled */
2438a556c76aSAlexandre Belloni 		ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_SRC + port);
2439a556c76aSAlexandre Belloni 	}
2440a556c76aSAlexandre Belloni 
2441a556c76aSAlexandre Belloni 	/* Allow broadcast MAC frames. */
2442a556c76aSAlexandre Belloni 	for (i = ocelot->num_phys_ports + 1; i < PGID_CPU; i++) {
2443a556c76aSAlexandre Belloni 		u32 val = ANA_PGID_PGID_PGID(GENMASK(ocelot->num_phys_ports - 1, 0));
2444a556c76aSAlexandre Belloni 
2445a556c76aSAlexandre Belloni 		ocelot_write_rix(ocelot, val, ANA_PGID_PGID, i);
2446a556c76aSAlexandre Belloni 	}
2447a556c76aSAlexandre Belloni 	ocelot_write_rix(ocelot,
2448a556c76aSAlexandre Belloni 			 ANA_PGID_PGID_PGID(GENMASK(ocelot->num_phys_ports, 0)),
2449a556c76aSAlexandre Belloni 			 ANA_PGID_PGID, PGID_MC);
2450a556c76aSAlexandre Belloni 	ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_MCIPV4);
2451a556c76aSAlexandre Belloni 	ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_MCIPV6);
2452a556c76aSAlexandre Belloni 
2453a556c76aSAlexandre Belloni 	/* Allow manual injection via DEVCPU_QS registers, and byte swap these
2454a556c76aSAlexandre Belloni 	 * registers endianness.
2455a556c76aSAlexandre Belloni 	 */
2456a556c76aSAlexandre Belloni 	ocelot_write_rix(ocelot, QS_INJ_GRP_CFG_BYTE_SWAP |
2457a556c76aSAlexandre Belloni 			 QS_INJ_GRP_CFG_MODE(1), QS_INJ_GRP_CFG, 0);
2458a556c76aSAlexandre Belloni 	ocelot_write_rix(ocelot, QS_XTR_GRP_CFG_BYTE_SWAP |
2459a556c76aSAlexandre Belloni 			 QS_XTR_GRP_CFG_MODE(1), QS_XTR_GRP_CFG, 0);
2460a556c76aSAlexandre Belloni 	ocelot_write(ocelot, ANA_CPUQ_CFG_CPUQ_MIRROR(2) |
2461a556c76aSAlexandre Belloni 		     ANA_CPUQ_CFG_CPUQ_LRN(2) |
2462a556c76aSAlexandre Belloni 		     ANA_CPUQ_CFG_CPUQ_MAC_COPY(2) |
2463a556c76aSAlexandre Belloni 		     ANA_CPUQ_CFG_CPUQ_SRC_COPY(2) |
2464a556c76aSAlexandre Belloni 		     ANA_CPUQ_CFG_CPUQ_LOCKED_PORTMOVE(2) |
2465a556c76aSAlexandre Belloni 		     ANA_CPUQ_CFG_CPUQ_ALLBRIDGE(6) |
2466a556c76aSAlexandre Belloni 		     ANA_CPUQ_CFG_CPUQ_IPMC_CTRL(6) |
2467a556c76aSAlexandre Belloni 		     ANA_CPUQ_CFG_CPUQ_IGMP(6) |
2468a556c76aSAlexandre Belloni 		     ANA_CPUQ_CFG_CPUQ_MLD(6), ANA_CPUQ_CFG);
2469a556c76aSAlexandre Belloni 	for (i = 0; i < 16; i++)
2470a556c76aSAlexandre Belloni 		ocelot_write_rix(ocelot, ANA_CPUQ_8021_CFG_CPUQ_GARP_VAL(6) |
2471a556c76aSAlexandre Belloni 				 ANA_CPUQ_8021_CFG_CPUQ_BPDU_VAL(6),
2472a556c76aSAlexandre Belloni 				 ANA_CPUQ_8021_CFG, i);
2473a556c76aSAlexandre Belloni 
24741e1caa97SClaudiu Manoil 	INIT_DELAYED_WORK(&ocelot->stats_work, ocelot_check_stats_work);
2475a556c76aSAlexandre Belloni 	queue_delayed_work(ocelot->stats_queue, &ocelot->stats_work,
2476a556c76aSAlexandre Belloni 			   OCELOT_STATS_CHECK_DELAY);
24774e3b0468SAntoine Tenart 
24784e3b0468SAntoine Tenart 	if (ocelot->ptp) {
24794e3b0468SAntoine Tenart 		ret = ocelot_init_timestamp(ocelot);
24804e3b0468SAntoine Tenart 		if (ret) {
24814e3b0468SAntoine Tenart 			dev_err(ocelot->dev,
24824e3b0468SAntoine Tenart 				"Timestamp initialization failed\n");
24834e3b0468SAntoine Tenart 			return ret;
24844e3b0468SAntoine Tenart 		}
24854e3b0468SAntoine Tenart 	}
24864e3b0468SAntoine Tenart 
2487a556c76aSAlexandre Belloni 	return 0;
2488a556c76aSAlexandre Belloni }
2489a556c76aSAlexandre Belloni EXPORT_SYMBOL(ocelot_init);
2490a556c76aSAlexandre Belloni 
2491a556c76aSAlexandre Belloni void ocelot_deinit(struct ocelot *ocelot)
2492a556c76aSAlexandre Belloni {
24934e3b0468SAntoine Tenart 	struct list_head *pos, *tmp;
24944e3b0468SAntoine Tenart 	struct ocelot_port *port;
24954e3b0468SAntoine Tenart 	struct ocelot_skb *entry;
24964e3b0468SAntoine Tenart 	int i;
24974e3b0468SAntoine Tenart 
2498c5d13969SClaudiu Manoil 	cancel_delayed_work(&ocelot->stats_work);
2499a556c76aSAlexandre Belloni 	destroy_workqueue(ocelot->stats_queue);
2500a556c76aSAlexandre Belloni 	mutex_destroy(&ocelot->stats_lock);
2501b5962294SHoratiu Vultur 	ocelot_ace_deinit();
25024e3b0468SAntoine Tenart 
25034e3b0468SAntoine Tenart 	for (i = 0; i < ocelot->num_phys_ports; i++) {
25044e3b0468SAntoine Tenart 		port = ocelot->ports[i];
25054e3b0468SAntoine Tenart 
25064e3b0468SAntoine Tenart 		list_for_each_safe(pos, tmp, &port->skbs) {
25074e3b0468SAntoine Tenart 			entry = list_entry(pos, struct ocelot_skb, head);
25084e3b0468SAntoine Tenart 
25094e3b0468SAntoine Tenart 			list_del(pos);
25104e3b0468SAntoine Tenart 			dev_kfree_skb_any(entry->skb);
25114e3b0468SAntoine Tenart 			kfree(entry);
25124e3b0468SAntoine Tenart 		}
25134e3b0468SAntoine Tenart 	}
2514a556c76aSAlexandre Belloni }
2515a556c76aSAlexandre Belloni EXPORT_SYMBOL(ocelot_deinit);
2516a556c76aSAlexandre Belloni 
2517a556c76aSAlexandre Belloni MODULE_LICENSE("Dual MIT/GPL");
2518