~hc/RK356X_SDK_RELEASE.git

..	..	@@ -4,36 +4,13 @@
4	4	*
5	5	* Copyright (c) 2017 Microsemi Corporation
6	6	*/
7		-#include <linux/etherdevice.h>
8		-#include <linux/ethtool.h>
9	7	#include <linux/if_bridge.h>
10		-#include <linux/if_ether.h>
11		-#include <linux/if_vlan.h>
12		-#include <linux/interrupt.h>
13		-#include <linux/kernel.h>
14		-#include <linux/module.h>
15		-#include <linux/netdevice.h>
16		-#include <linux/phy.h>
17		-#include <linux/skbuff.h>
18		-#include <net/arp.h>
19		-#include <net/netevent.h>
20		-#include <net/rtnetlink.h>
21		-#include <net/switchdev.h>
22		-
	8	+#include <soc/mscc/ocelot_vcap.h>
23	9	#include "ocelot.h"
	10	+#include "ocelot_vcap.h"
24	11
25		-/* MAC table entry types.
26		- * ENTRYTYPE_NORMAL is subject to aging.
27		- * ENTRYTYPE_LOCKED is not subject to aging.
28		- * ENTRYTYPE_MACv4 is not subject to aging. For IPv4 multicast.
29		- * ENTRYTYPE_MACv6 is not subject to aging. For IPv6 multicast.
30		- */
31		-enum macaccess_entry_type {
32		- ENTRYTYPE_NORMAL = 0,
33		- ENTRYTYPE_LOCKED,
34		- ENTRYTYPE_MACv4,
35		- ENTRYTYPE_MACv6,
36		-};
	12	+#define TABLE_UPDATE_SLEEP_US 10
	13	+#define TABLE_UPDATE_TIMEOUT_US 100000
37	14
38	15	struct ocelot_mact_entry {
39	16	u8 mac[ETH_ALEN];
..	..	@@ -41,23 +18,20 @@
41	18	enum macaccess_entry_type type;
42	19	};
43	20
	21	+static inline u32 ocelot_mact_read_macaccess(struct ocelot *ocelot)
	22	+{
	23	+ return ocelot_read(ocelot, ANA_TABLES_MACACCESS);
	24	+}
	25	+
44	26	static inline int ocelot_mact_wait_for_completion(struct ocelot *ocelot)
45	27	{
46		- unsigned int val, timeout = 10;
	28	+ u32 val;
47	29
48		- /* Wait for the issued mac table command to be completed, or timeout.
49		- * When the command read from ANA_TABLES_MACACCESS is
50		- * MACACCESS_CMD_IDLE, the issued command completed successfully.
51		- */
52		- do {
53		- val = ocelot_read(ocelot, ANA_TABLES_MACACCESS);
54		- val &= ANA_TABLES_MACACCESS_MAC_TABLE_CMD_M;
55		- } while (val != MACACCESS_CMD_IDLE && timeout--);
56		-
57		- if (!timeout)
58		- return -ETIMEDOUT;
59		-
60		- return 0;
	30	+ return readx_poll_timeout(ocelot_mact_read_macaccess,
	31	+ ocelot, val,
	32	+ (val & ANA_TABLES_MACACCESS_MAC_TABLE_CMD_M) ==
	33	+ MACACCESS_CMD_IDLE,
	34	+ TABLE_UPDATE_SLEEP_US, TABLE_UPDATE_TIMEOUT_US);
61	35	}
62	36
63	37	static void ocelot_mact_select(struct ocelot *ocelot,
..	..	@@ -82,26 +56,38 @@
82	56
83	57	}
84	58
85		-static int ocelot_mact_learn(struct ocelot *ocelot, int port,
86		- const unsigned char mac[ETH_ALEN],
87		- unsigned int vid,
88		- enum macaccess_entry_type type)
	59	+int ocelot_mact_learn(struct ocelot *ocelot, int port,
	60	+ const unsigned char mac[ETH_ALEN],
	61	+ unsigned int vid, enum macaccess_entry_type type)
89	62	{
	63	+ u32 cmd = ANA_TABLES_MACACCESS_VALID \|
	64	+ ANA_TABLES_MACACCESS_DEST_IDX(port) \|
	65	+ ANA_TABLES_MACACCESS_ENTRYTYPE(type) \|
	66	+ ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_LEARN);
	67	+ unsigned int mc_ports;
	68	+
	69	+ /* Set MAC_CPU_COPY if the CPU port is used by a multicast entry */
	70	+ if (type == ENTRYTYPE_MACv4)
	71	+ mc_ports = (mac[1] << 8) \| mac[2];
	72	+ else if (type == ENTRYTYPE_MACv6)
	73	+ mc_ports = (mac[0] << 8) \| mac[1];
	74	+ else
	75	+ mc_ports = 0;
	76	+
	77	+ if (mc_ports & BIT(ocelot->num_phys_ports))
	78	+ cmd \|= ANA_TABLES_MACACCESS_MAC_CPU_COPY;
	79	+
90	80	ocelot_mact_select(ocelot, mac, vid);
91	81
92	82	/* Issue a write command */
93		- ocelot_write(ocelot, ANA_TABLES_MACACCESS_VALID \|
94		- ANA_TABLES_MACACCESS_DEST_IDX(port) \|
95		- ANA_TABLES_MACACCESS_ENTRYTYPE(type) \|
96		- ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_LEARN),
97		- ANA_TABLES_MACACCESS);
	83	+ ocelot_write(ocelot, cmd, ANA_TABLES_MACACCESS);
98	84
99	85	return ocelot_mact_wait_for_completion(ocelot);
100	86	}
	87	+EXPORT_SYMBOL(ocelot_mact_learn);
101	88
102		-static int ocelot_mact_forget(struct ocelot *ocelot,
103		- const unsigned char mac[ETH_ALEN],
104		- unsigned int vid)
	89	+int ocelot_mact_forget(struct ocelot *ocelot,
	90	+ const unsigned char mac[ETH_ALEN], unsigned int vid)
105	91	{
106	92	ocelot_mact_select(ocelot, mac, vid);
107	93
..	..	@@ -112,6 +98,7 @@
112	98
113	99	return ocelot_mact_wait_for_completion(ocelot);
114	100	}
	101	+EXPORT_SYMBOL(ocelot_mact_forget);
115	102
116	103	static void ocelot_mact_init(struct ocelot *ocelot)
117	104	{
..	..	@@ -129,23 +116,35 @@
129	116	ocelot_write(ocelot, MACACCESS_CMD_INIT, ANA_TABLES_MACACCESS);
130	117	}
131	118
	119	+static void ocelot_vcap_enable(struct ocelot *ocelot, int port)
	120	+{
	121	+ ocelot_write_gix(ocelot, ANA_PORT_VCAP_S2_CFG_S2_ENA \|
	122	+ ANA_PORT_VCAP_S2_CFG_S2_IP6_CFG(0xa),
	123	+ ANA_PORT_VCAP_S2_CFG, port);
	124	+
	125	+ ocelot_write_gix(ocelot, ANA_PORT_VCAP_CFG_S1_ENA,
	126	+ ANA_PORT_VCAP_CFG, port);
	127	+
	128	+ ocelot_rmw_gix(ocelot, REW_PORT_CFG_ES0_EN,
	129	+ REW_PORT_CFG_ES0_EN,
	130	+ REW_PORT_CFG, port);
	131	+}
	132	+
	133	+static inline u32 ocelot_vlant_read_vlanaccess(struct ocelot *ocelot)
	134	+{
	135	+ return ocelot_read(ocelot, ANA_TABLES_VLANACCESS);
	136	+}
	137	+
132	138	static inline int ocelot_vlant_wait_for_completion(struct ocelot *ocelot)
133	139	{
134		- unsigned int val, timeout = 10;
	140	+ u32 val;
135	141
136		- /* Wait for the issued vlan table command to be completed, or timeout.
137		- * When the command read from ANA_TABLES_VLANACCESS is
138		- * VLANACCESS_CMD_IDLE, the issued command completed successfully.
139		- */
140		- do {
141		- val = ocelot_read(ocelot, ANA_TABLES_VLANACCESS);
142		- val &= ANA_TABLES_VLANACCESS_VLAN_TBL_CMD_M;
143		- } while (val != ANA_TABLES_VLANACCESS_CMD_IDLE && timeout--);
144		-
145		- if (!timeout)
146		- return -ETIMEDOUT;
147		-
148		- return 0;
	142	+ return readx_poll_timeout(ocelot_vlant_read_vlanaccess,
	143	+ ocelot,
	144	+ val,
	145	+ (val & ANA_TABLES_VLANACCESS_VLAN_TBL_CMD_M) ==
	146	+ ANA_TABLES_VLANACCESS_CMD_IDLE,
	147	+ TABLE_UPDATE_SLEEP_US, TABLE_UPDATE_TIMEOUT_US);
149	148	}
150	149
151	150	static int ocelot_vlant_set_mask(struct ocelot *ocelot, u16 vid, u32 mask)
..	..	@@ -161,147 +160,159 @@
161	160	return ocelot_vlant_wait_for_completion(ocelot);
162	161	}
163	162
164		-static void ocelot_vlan_mode(struct ocelot_port *port,
165		- netdev_features_t features)
	163	+static int ocelot_port_set_native_vlan(struct ocelot *ocelot, int port,
	164	+ u16 vid)
166	165	{
167		- struct ocelot *ocelot = port->ocelot;
168		- u8 p = port->chip_port;
169		- u32 val;
	166	+ struct ocelot_port *ocelot_port = ocelot->ports[port];
	167	+ u32 val = 0;
170	168
171		- /* Filtering */
172		- val = ocelot_read(ocelot, ANA_VLANMASK);
173		- if (features & NETIF_F_HW_VLAN_CTAG_FILTER)
174		- val \|= BIT(p);
175		- else
176		- val &= ~BIT(p);
177		- ocelot_write(ocelot, val, ANA_VLANMASK);
178		-}
	169	+ if (ocelot_port->vid != vid) {
	170	+ /* Always permit deleting the native VLAN (vid = 0) */
	171	+ if (ocelot_port->vid && vid) {
	172	+ dev_err(ocelot->dev,
	173	+ "Port already has a native VLAN: %d\n",
	174	+ ocelot_port->vid);
	175	+ return -EBUSY;
	176	+ }
	177	+ ocelot_port->vid = vid;
	178	+ }
179	179
180		-static void ocelot_vlan_port_apply(struct ocelot *ocelot,
181		- struct ocelot_port *port)
182		-{
183		- u32 val;
	180	+ ocelot_rmw_gix(ocelot, REW_PORT_VLAN_CFG_PORT_VID(vid),
	181	+ REW_PORT_VLAN_CFG_PORT_VID_M,
	182	+ REW_PORT_VLAN_CFG, port);
184	183
185		- /* Ingress clasification (ANA_PORT_VLAN_CFG) */
186		- /* Default vlan to clasify for untagged frames (may be zero) */
187		- val = ANA_PORT_VLAN_CFG_VLAN_VID(port->pvid);
188		- if (port->vlan_aware)
189		- val \|= ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA \|
190		- ANA_PORT_VLAN_CFG_VLAN_POP_CNT(1);
191		-
192		- ocelot_rmw_gix(ocelot, val,
193		- ANA_PORT_VLAN_CFG_VLAN_VID_M \|
194		- ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA \|
195		- ANA_PORT_VLAN_CFG_VLAN_POP_CNT_M,
196		- ANA_PORT_VLAN_CFG, port->chip_port);
197		-
198		- /* Drop frames with multicast source address */
199		- val = ANA_PORT_DROP_CFG_DROP_MC_SMAC_ENA;
200		- if (port->vlan_aware && !port->vid)
	184	+ if (ocelot_port->vlan_aware && !ocelot_port->vid)
201	185	/* If port is vlan-aware and tagged, drop untagged and priority
202	186	* tagged frames.
203	187	*/
204		- val \|= ANA_PORT_DROP_CFG_DROP_UNTAGGED_ENA \|
	188	+ val = ANA_PORT_DROP_CFG_DROP_UNTAGGED_ENA \|
	189	+ ANA_PORT_DROP_CFG_DROP_PRIO_S_TAGGED_ENA \|
	190	+ ANA_PORT_DROP_CFG_DROP_PRIO_C_TAGGED_ENA;
	191	+ ocelot_rmw_gix(ocelot, val,
	192	+ ANA_PORT_DROP_CFG_DROP_UNTAGGED_ENA \|
205	193	ANA_PORT_DROP_CFG_DROP_PRIO_S_TAGGED_ENA \|
206		- ANA_PORT_DROP_CFG_DROP_PRIO_C_TAGGED_ENA;
207		- ocelot_write_gix(ocelot, val, ANA_PORT_DROP_CFG, port->chip_port);
	194	+ ANA_PORT_DROP_CFG_DROP_PRIO_C_TAGGED_ENA,
	195	+ ANA_PORT_DROP_CFG, port);
208	196
209		- /* Egress configuration (REW_TAG_CFG): VLAN tag type to 8021Q. */
210		- val = REW_TAG_CFG_TAG_TPID_CFG(0);
211		-
212		- if (port->vlan_aware) {
213		- if (port->vid)
	197	+ if (ocelot_port->vlan_aware) {
	198	+ if (ocelot_port->vid)
214	199	/* Tag all frames except when VID == DEFAULT_VLAN */
215		- val \|= REW_TAG_CFG_TAG_CFG(1);
	200	+ val = REW_TAG_CFG_TAG_CFG(1);
216	201	else
217	202	/* Tag all frames */
218		- val \|= REW_TAG_CFG_TAG_CFG(3);
	203	+ val = REW_TAG_CFG_TAG_CFG(3);
	204	+ } else {
	205	+ /* Port tagging disabled. */
	206	+ val = REW_TAG_CFG_TAG_CFG(0);
219	207	}
220	208	ocelot_rmw_gix(ocelot, val,
221		- REW_TAG_CFG_TAG_TPID_CFG_M \|
222	209	REW_TAG_CFG_TAG_CFG_M,
223		- REW_TAG_CFG, port->chip_port);
	210	+ REW_TAG_CFG, port);
224	211
225		- /* Set default VLAN and tag type to 8021Q. */
226		- val = REW_PORT_VLAN_CFG_PORT_TPID(ETH_P_8021Q) \|
227		- REW_PORT_VLAN_CFG_PORT_VID(port->vid);
228		- ocelot_rmw_gix(ocelot, val,
229		- REW_PORT_VLAN_CFG_PORT_TPID_M \|
230		- REW_PORT_VLAN_CFG_PORT_VID_M,
231		- REW_PORT_VLAN_CFG, port->chip_port);
	212	+ return 0;
232	213	}
233	214
234		-static int ocelot_vlan_vid_add(struct net_device *dev, u16 vid, bool pvid,
235		- bool untagged)
	215	+int ocelot_port_vlan_filtering(struct ocelot *ocelot, int port,
	216	+ bool vlan_aware, struct switchdev_trans *trans)
236	217	{
237		- struct ocelot_port *port = netdev_priv(dev);
238		- struct ocelot *ocelot = port->ocelot;
	218	+ struct ocelot_port *ocelot_port = ocelot->ports[port];
	219	+ u32 val;
	220	+
	221	+ if (switchdev_trans_ph_prepare(trans)) {
	222	+ struct ocelot_vcap_block *block = &ocelot->block[VCAP_IS1];
	223	+ struct ocelot_vcap_filter *filter;
	224	+
	225	+ list_for_each_entry(filter, &block->rules, list) {
	226	+ if (filter->ingress_port_mask & BIT(port) &&
	227	+ filter->action.vid_replace_ena) {
	228	+ dev_err(ocelot->dev,
	229	+ "Cannot change VLAN state with vlan modify rules active\n");
	230	+ return -EBUSY;
	231	+ }
	232	+ }
	233	+
	234	+ return 0;
	235	+ }
	236	+
	237	+ ocelot_port->vlan_aware = vlan_aware;
	238	+
	239	+ if (vlan_aware)
	240	+ val = ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA \|
	241	+ ANA_PORT_VLAN_CFG_VLAN_POP_CNT(1);
	242	+ else
	243	+ val = 0;
	244	+ ocelot_rmw_gix(ocelot, val,
	245	+ ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA \|
	246	+ ANA_PORT_VLAN_CFG_VLAN_POP_CNT_M,
	247	+ ANA_PORT_VLAN_CFG, port);
	248	+
	249	+ ocelot_port_set_native_vlan(ocelot, port, ocelot_port->vid);
	250	+
	251	+ return 0;
	252	+}
	253	+EXPORT_SYMBOL(ocelot_port_vlan_filtering);
	254	+
	255	+/* Default vlan to clasify for untagged frames (may be zero) */
	256	+static void ocelot_port_set_pvid(struct ocelot *ocelot, int port, u16 pvid)
	257	+{
	258	+ struct ocelot_port *ocelot_port = ocelot->ports[port];
	259	+
	260	+ ocelot_rmw_gix(ocelot,
	261	+ ANA_PORT_VLAN_CFG_VLAN_VID(pvid),
	262	+ ANA_PORT_VLAN_CFG_VLAN_VID_M,
	263	+ ANA_PORT_VLAN_CFG, port);
	264	+
	265	+ ocelot_port->pvid = pvid;
	266	+}
	267	+
	268	+int ocelot_vlan_add(struct ocelot *ocelot, int port, u16 vid, bool pvid,
	269	+ bool untagged)
	270	+{
239	271	int ret;
240	272
241		- /* Add the port MAC address to with the right VLAN information */
242		- ocelot_mact_learn(ocelot, PGID_CPU, dev->dev_addr, vid,
243		- ENTRYTYPE_LOCKED);
244		-
245	273	/* Make the port a member of the VLAN */
246		- ocelot->vlan_mask[vid] \|= BIT(port->chip_port);
	274	+ ocelot->vlan_mask[vid] \|= BIT(port);
247	275	ret = ocelot_vlant_set_mask(ocelot, vid, ocelot->vlan_mask[vid]);
248	276	if (ret)
249	277	return ret;
250	278
251	279	/* Default ingress vlan classification */
252	280	if (pvid)
253		- port->pvid = vid;
	281	+ ocelot_port_set_pvid(ocelot, port, vid);
254	282
255	283	/* Untagged egress vlan clasification */
256		- if (untagged && port->vid != vid) {
257		- if (port->vid) {
258		- dev_err(ocelot->dev,
259		- "Port already has a native VLAN: %d\n",
260		- port->vid);
261		- return -EBUSY;
262		- }
263		- port->vid = vid;
	284	+ if (untagged) {
	285	+ ret = ocelot_port_set_native_vlan(ocelot, port, vid);
	286	+ if (ret)
	287	+ return ret;
264	288	}
265		-
266		- ocelot_vlan_port_apply(ocelot, port);
267	289
268	290	return 0;
269	291	}
	292	+EXPORT_SYMBOL(ocelot_vlan_add);
270	293
271		-static int ocelot_vlan_vid_del(struct net_device *dev, u16 vid)
	294	+int ocelot_vlan_del(struct ocelot *ocelot, int port, u16 vid)
272	295	{
273		- struct ocelot_port *port = netdev_priv(dev);
274		- struct ocelot *ocelot = port->ocelot;
	296	+ struct ocelot_port *ocelot_port = ocelot->ports[port];
275	297	int ret;
276	298
277		- /* 8021q removes VID 0 on module unload for all interfaces
278		- * with VLAN filtering feature. We need to keep it to receive
279		- * untagged traffic.
280		- */
281		- if (vid == 0)
282		- return 0;
283		-
284		- /* Del the port MAC address to with the right VLAN information */
285		- ocelot_mact_forget(ocelot, dev->dev_addr, vid);
286		-
287	299	/* Stop the port from being a member of the vlan */
288		- ocelot->vlan_mask[vid] &= ~BIT(port->chip_port);
	300	+ ocelot->vlan_mask[vid] &= ~BIT(port);
289	301	ret = ocelot_vlant_set_mask(ocelot, vid, ocelot->vlan_mask[vid]);
290	302	if (ret)
291	303	return ret;
292	304
293	305	/* Ingress */
294		- if (port->pvid == vid)
295		- port->pvid = 0;
	306	+ if (ocelot_port->pvid == vid)
	307	+ ocelot_port_set_pvid(ocelot, port, 0);
296	308
297	309	/* Egress */
298		- if (port->vid == vid)
299		- port->vid = 0;
300		-
301		- ocelot_vlan_port_apply(ocelot, port);
	310	+ if (ocelot_port->vid == vid)
	311	+ ocelot_port_set_native_vlan(ocelot, port, 0);
302	312
303	313	return 0;
304	314	}
	315	+EXPORT_SYMBOL(ocelot_vlan_del);
305	316
306	317	static void ocelot_vlan_init(struct ocelot *ocelot)
307	318	{
..	..	@@ -325,16 +336,11 @@
325	336	ocelot->vlan_mask[0] = GENMASK(ocelot->num_phys_ports - 1, 0);
326	337	ocelot_vlant_set_mask(ocelot, 0, ocelot->vlan_mask[0]);
327	338
328		- /* Configure the CPU port to be VLAN aware */
329		- ocelot_write_gix(ocelot, ANA_PORT_VLAN_CFG_VLAN_VID(0) \|
330		- ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA \|
331		- ANA_PORT_VLAN_CFG_VLAN_POP_CNT(1),
332		- ANA_PORT_VLAN_CFG, ocelot->num_phys_ports);
333		-
334	339	/* Set vlan ingress filter mask to all ports but the CPU port by
335	340	* default.
336	341	*/
337		- ocelot_write(ocelot, GENMASK(9, 0), ANA_VLANMASK);
	342	+ ocelot_write(ocelot, GENMASK(ocelot->num_phys_ports - 1, 0),
	343	+ ANA_VLANMASK);
338	344
339	345	for (port = 0; port < ocelot->num_phys_ports; port++) {
340	346	ocelot_write_gix(ocelot, 0, REW_PORT_VLAN_CFG, port);
..	..	@@ -342,26 +348,72 @@
342	348	}
343	349	}
344	350
345		-/* Watermark encode
346		- * Bit 8: Unit; 0:1, 1:16
347		- * Bit 7-0: Value to be multiplied with unit
348		- */
349		-static u16 ocelot_wm_enc(u16 value)
	351	+static u32 ocelot_read_eq_avail(struct ocelot *ocelot, int port)
350	352	{
351		- if (value >= BIT(8))
352		- return BIT(8) \| (value / 16);
353		-
354		- return value;
	353	+ return ocelot_read_rix(ocelot, QSYS_SW_STATUS, port);
355	354	}
356	355
357		-static void ocelot_port_adjust_link(struct net_device *dev)
	356	+int ocelot_port_flush(struct ocelot *ocelot, int port)
358	357	{
359		- struct ocelot_port *port = netdev_priv(dev);
360		- struct ocelot *ocelot = port->ocelot;
361		- u8 p = port->chip_port;
362		- int speed, atop_wm, mode = 0;
	358	+ unsigned int pause_ena;
	359	+ int err, val;
363	360
364		- switch (dev->phydev->speed) {
	361	+ /* Disable dequeuing from the egress queues */
	362	+ ocelot_rmw_rix(ocelot, QSYS_PORT_MODE_DEQUEUE_DIS,
	363	+ QSYS_PORT_MODE_DEQUEUE_DIS,
	364	+ QSYS_PORT_MODE, port);
	365	+
	366	+ /* Disable flow control */
	367	+ ocelot_fields_read(ocelot, port, SYS_PAUSE_CFG_PAUSE_ENA, &pause_ena);
	368	+ ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_ENA, 0);
	369	+
	370	+ /* Disable priority flow control */
	371	+ ocelot_fields_write(ocelot, port,
	372	+ QSYS_SWITCH_PORT_MODE_TX_PFC_ENA, 0);
	373	+
	374	+ /* Wait at least the time it takes to receive a frame of maximum length
	375	+ * at the port.
	376	+ * Worst-case delays for 10 kilobyte jumbo frames are:
	377	+ * 8 ms on a 10M port
	378	+ * 800 μs on a 100M port
	379	+ * 80 μs on a 1G port
	380	+ * 32 μs on a 2.5G port
	381	+ */
	382	+ usleep_range(8000, 10000);
	383	+
	384	+ /* Disable half duplex backpressure. */
	385	+ ocelot_rmw_rix(ocelot, 0, SYS_FRONT_PORT_MODE_HDX_MODE,
	386	+ SYS_FRONT_PORT_MODE, port);
	387	+
	388	+ /* Flush the queues associated with the port. */
	389	+ ocelot_rmw_gix(ocelot, REW_PORT_CFG_FLUSH_ENA, REW_PORT_CFG_FLUSH_ENA,
	390	+ REW_PORT_CFG, port);
	391	+
	392	+ /* Enable dequeuing from the egress queues. */
	393	+ ocelot_rmw_rix(ocelot, 0, QSYS_PORT_MODE_DEQUEUE_DIS, QSYS_PORT_MODE,
	394	+ port);
	395	+
	396	+ /* Wait until flushing is complete. */
	397	+ err = read_poll_timeout(ocelot_read_eq_avail, val, !val,
	398	+ 100, 2000000, false, ocelot, port);
	399	+
	400	+ /* Clear flushing again. */
	401	+ ocelot_rmw_gix(ocelot, 0, REW_PORT_CFG_FLUSH_ENA, REW_PORT_CFG, port);
	402	+
	403	+ /* Re-enable flow control */
	404	+ ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_ENA, pause_ena);
	405	+
	406	+ return err;
	407	+}
	408	+EXPORT_SYMBOL(ocelot_port_flush);
	409	+
	410	+void ocelot_adjust_link(struct ocelot *ocelot, int port,
	411	+ struct phy_device *phydev)
	412	+{
	413	+ struct ocelot_port *ocelot_port = ocelot->ports[port];
	414	+ int speed, mode = 0;
	415	+
	416	+ switch (phydev->speed) {
365	417	case SPEED_10:
366	418	speed = OCELOT_SPEED_10;
367	419	break;
..	..	@@ -377,87 +429,54 @@
377	429	mode = DEV_MAC_MODE_CFG_GIGA_MODE_ENA;
378	430	break;
379	431	default:
380		- netdev_err(dev, "Unsupported PHY speed: %d\n",
381		- dev->phydev->speed);
	432	+ dev_err(ocelot->dev, "Unsupported PHY speed on port %d: %d\n",
	433	+ port, phydev->speed);
382	434	return;
383	435	}
384	436
385		- phy_print_status(dev->phydev);
	437	+ phy_print_status(phydev);
386	438
387		- if (!dev->phydev->link)
	439	+ if (!phydev->link)
388	440	return;
389	441
390	442	/* Only full duplex supported for now */
391		- ocelot_port_writel(port, DEV_MAC_MODE_CFG_FDX_ENA \|
	443	+ ocelot_port_writel(ocelot_port, DEV_MAC_MODE_CFG_FDX_ENA \|
392	444	mode, DEV_MAC_MODE_CFG);
393	445
394		- /* Set MAC IFG Gaps
395		- * FDX: TX_IFG = 5, RX_IFG1 = RX_IFG2 = 0
396		- * !FDX: TX_IFG = 5, RX_IFG1 = RX_IFG2 = 5
397		- */
398		- ocelot_port_writel(port, DEV_MAC_IFG_CFG_TX_IFG(5), DEV_MAC_IFG_CFG);
399		-
400		- /* Load seed (0) and set MAC HDX late collision */
401		- ocelot_port_writel(port, DEV_MAC_HDX_CFG_LATE_COL_POS(67) \|
402		- DEV_MAC_HDX_CFG_SEED_LOAD,
403		- DEV_MAC_HDX_CFG);
404		- mdelay(1);
405		- ocelot_port_writel(port, DEV_MAC_HDX_CFG_LATE_COL_POS(67),
406		- DEV_MAC_HDX_CFG);
407		-
408	446	/* Disable HDX fast control */
409		- ocelot_port_writel(port, DEV_PORT_MISC_HDX_FAST_DIS, DEV_PORT_MISC);
	447	+ ocelot_port_writel(ocelot_port, DEV_PORT_MISC_HDX_FAST_DIS,
	448	+ DEV_PORT_MISC);
410	449
411	450	/* SGMII only for now */
412		- ocelot_port_writel(port, PCS1G_MODE_CFG_SGMII_MODE_ENA, PCS1G_MODE_CFG);
413		- ocelot_port_writel(port, PCS1G_SD_CFG_SD_SEL, PCS1G_SD_CFG);
	451	+ ocelot_port_writel(ocelot_port, PCS1G_MODE_CFG_SGMII_MODE_ENA,
	452	+ PCS1G_MODE_CFG);
	453	+ ocelot_port_writel(ocelot_port, PCS1G_SD_CFG_SD_SEL, PCS1G_SD_CFG);
414	454
415	455	/* Enable PCS */
416		- ocelot_port_writel(port, PCS1G_CFG_PCS_ENA, PCS1G_CFG);
	456	+ ocelot_port_writel(ocelot_port, PCS1G_CFG_PCS_ENA, PCS1G_CFG);
417	457
418	458	/* No aneg on SGMII */
419		- ocelot_port_writel(port, 0, PCS1G_ANEG_CFG);
	459	+ ocelot_port_writel(ocelot_port, 0, PCS1G_ANEG_CFG);
420	460
421	461	/* No loopback */
422		- ocelot_port_writel(port, 0, PCS1G_LB_CFG);
423		-
424		- /* Set Max Length and maximum tags allowed */
425		- ocelot_port_writel(port, VLAN_ETH_FRAME_LEN, DEV_MAC_MAXLEN_CFG);
426		- ocelot_port_writel(port, DEV_MAC_TAGS_CFG_TAG_ID(ETH_P_8021AD) \|
427		- DEV_MAC_TAGS_CFG_VLAN_AWR_ENA \|
428		- DEV_MAC_TAGS_CFG_VLAN_LEN_AWR_ENA,
429		- DEV_MAC_TAGS_CFG);
	462	+ ocelot_port_writel(ocelot_port, 0, PCS1G_LB_CFG);
430	463
431	464	/* Enable MAC module */
432		- ocelot_port_writel(port, DEV_MAC_ENA_CFG_RX_ENA \|
	465	+ ocelot_port_writel(ocelot_port, DEV_MAC_ENA_CFG_RX_ENA \|
433	466	DEV_MAC_ENA_CFG_TX_ENA, DEV_MAC_ENA_CFG);
434	467
435	468	/* Take MAC, Port, Phy (intern) and PCS (SGMII/Serdes) clock out of
436	469	* reset */
437		- ocelot_port_writel(port, DEV_CLOCK_CFG_LINK_SPEED(speed),
	470	+ ocelot_port_writel(ocelot_port, DEV_CLOCK_CFG_LINK_SPEED(speed),
438	471	DEV_CLOCK_CFG);
439		-
440		- /* Set SMAC of Pause frame (00:00:00:00:00:00) */
441		- ocelot_port_writel(port, 0, DEV_MAC_FC_MAC_HIGH_CFG);
442		- ocelot_port_writel(port, 0, DEV_MAC_FC_MAC_LOW_CFG);
443	472
444	473	/* No PFC */
445	474	ocelot_write_gix(ocelot, ANA_PFC_PFC_CFG_FC_LINK_SPEED(speed),
446		- ANA_PFC_PFC_CFG, p);
447		-
448		- /* Set Pause WM hysteresis
449		- * 152 = 6 * VLAN_ETH_FRAME_LEN / OCELOT_BUFFER_CELL_SZ
450		- * 101 = 4 * VLAN_ETH_FRAME_LEN / OCELOT_BUFFER_CELL_SZ
451		- */
452		- ocelot_write_rix(ocelot, SYS_PAUSE_CFG_PAUSE_ENA \|
453		- SYS_PAUSE_CFG_PAUSE_STOP(101) \|
454		- SYS_PAUSE_CFG_PAUSE_START(152), SYS_PAUSE_CFG, p);
	475	+ ANA_PFC_PFC_CFG, port);
455	476
456	477	/* Core: Enable port for frame transfer */
457		- ocelot_write_rix(ocelot, QSYS_SWITCH_PORT_MODE_INGRESS_DROP_MODE \|
458		- QSYS_SWITCH_PORT_MODE_SCH_NEXT_CFG(1) \|
459		- QSYS_SWITCH_PORT_MODE_PORT_ENA,
460		- QSYS_SWITCH_PORT_MODE, p);
	478	+ ocelot_fields_write(ocelot, port,
	479	+ QSYS_SWITCH_PORT_MODE_PORT_ENA, 1);
461	480
462	481	/* Flow control */
463	482	ocelot_write_rix(ocelot, SYS_MAC_FC_CFG_PAUSE_VAL_CFG(0xffff) \|
..	..	@@ -465,273 +484,149 @@
465	484	SYS_MAC_FC_CFG_ZERO_PAUSE_ENA \|
466	485	SYS_MAC_FC_CFG_FC_LATENCY_CFG(0x7) \|
467	486	SYS_MAC_FC_CFG_FC_LINK_SPEED(speed),
468		- SYS_MAC_FC_CFG, p);
469		- ocelot_write_rix(ocelot, 0, ANA_POL_FLOWC, p);
470		-
471		- /* Tail dropping watermark */
472		- atop_wm = (ocelot->shared_queue_sz - 9 * VLAN_ETH_FRAME_LEN) / OCELOT_BUFFER_CELL_SZ;
473		- ocelot_write_rix(ocelot, ocelot_wm_enc(9 * VLAN_ETH_FRAME_LEN),
474		- SYS_ATOP, p);
475		- ocelot_write(ocelot, ocelot_wm_enc(atop_wm), SYS_ATOP_TOT_CFG);
	487	+ SYS_MAC_FC_CFG, port);
	488	+ ocelot_write_rix(ocelot, 0, ANA_POL_FLOWC, port);
476	489	}
	490	+EXPORT_SYMBOL(ocelot_adjust_link);
477	491
478		-static int ocelot_port_open(struct net_device *dev)
	492	+void ocelot_port_enable(struct ocelot *ocelot, int port,
	493	+ struct phy_device *phy)
479	494	{
480		- struct ocelot_port *port = netdev_priv(dev);
481		- struct ocelot *ocelot = port->ocelot;
482		- int err;
483		-
484	495	/* Enable receiving frames on the port, and activate auto-learning of
485	496	* MAC addresses.
486	497	*/
487	498	ocelot_write_gix(ocelot, ANA_PORT_PORT_CFG_LEARNAUTO \|
488	499	ANA_PORT_PORT_CFG_RECV_ENA \|
489		- ANA_PORT_PORT_CFG_PORTID_VAL(port->chip_port),
490		- ANA_PORT_PORT_CFG, port->chip_port);
491		-
492		- err = phy_connect_direct(dev, port->phy, &ocelot_port_adjust_link,
493		- PHY_INTERFACE_MODE_NA);
494		- if (err) {
495		- netdev_err(dev, "Could not attach to PHY\n");
496		- return err;
497		- }
498		-
499		- dev->phydev = port->phy;
500		-
501		- phy_attached_info(port->phy);
502		- phy_start(port->phy);
503		- return 0;
	500	+ ANA_PORT_PORT_CFG_PORTID_VAL(port),
	501	+ ANA_PORT_PORT_CFG, port);
504	502	}
	503	+EXPORT_SYMBOL(ocelot_port_enable);
505	504
506		-static int ocelot_port_stop(struct net_device *dev)
	505	+void ocelot_port_disable(struct ocelot *ocelot, int port)
507	506	{
508		- struct ocelot_port *port = netdev_priv(dev);
	507	+ struct ocelot_port *ocelot_port = ocelot->ports[port];
509	508
510		- phy_disconnect(port->phy);
511		-
512		- dev->phydev = NULL;
513		-
514		- ocelot_port_writel(port, 0, DEV_MAC_ENA_CFG);
515		- ocelot_rmw_rix(port->ocelot, 0, QSYS_SWITCH_PORT_MODE_PORT_ENA,
516		- QSYS_SWITCH_PORT_MODE, port->chip_port);
517		- return 0;
	509	+ ocelot_port_writel(ocelot_port, 0, DEV_MAC_ENA_CFG);
	510	+ ocelot_fields_write(ocelot, port, QSYS_SWITCH_PORT_MODE_PORT_ENA, 0);
518	511	}
	512	+EXPORT_SYMBOL(ocelot_port_disable);
519	513
520		-/* Generate the IFH for frame injection
521		- *
522		- * The IFH is a 128bit-value
523		- * bit 127: bypass the analyzer processing
524		- * bit 56-67: destination mask
525		- * bit 28-29: pop_cnt: 3 disables all rewriting of the frame
526		- * bit 20-27: cpu extraction queue mask
527		- * bit 16: tag type 0: C-tag, 1: S-tag
528		- * bit 0-11: VID
529		- */
530		-static int ocelot_gen_ifh(u32 ifh, struct frame_info info)
	514	+void ocelot_port_add_txtstamp_skb(struct ocelot *ocelot, int port,
	515	+ struct sk_buff *clone)
531	516	{
532		- ifh[0] = IFH_INJ_BYPASS;
533		- ifh[1] = (0xf00 & info->port) >> 8;
534		- ifh[2] = (0xff & info->port) << 24;
535		- ifh[3] = (info->tag_type << 16) \| info->vid;
	517	+ struct ocelot_port *ocelot_port = ocelot->ports[port];
536	518
537		- return 0;
	519	+ spin_lock(&ocelot_port->ts_id_lock);
	520	+
	521	+ skb_shinfo(clone)->tx_flags \|= SKBTX_IN_PROGRESS;
	522	+ /* Store timestamp ID in cb[0] of sk_buff */
	523	+ clone->cb[0] = ocelot_port->ts_id;
	524	+ ocelot_port->ts_id = (ocelot_port->ts_id + 1) % 4;
	525	+ skb_queue_tail(&ocelot_port->tx_skbs, clone);
	526	+
	527	+ spin_unlock(&ocelot_port->ts_id_lock);
538	528	}
	529	+EXPORT_SYMBOL(ocelot_port_add_txtstamp_skb);
539	530
540		-static int ocelot_port_xmit(struct sk_buff skb, struct net_device dev)
	531	+static void ocelot_get_hwtimestamp(struct ocelot *ocelot,
	532	+ struct timespec64 *ts)
541	533	{
542		- struct ocelot_port *port = netdev_priv(dev);
543		- struct ocelot *ocelot = port->ocelot;
544		- u32 val, ifh[IFH_LEN];
545		- struct frame_info info = {};
546		- u8 grp = 0; /* Send everything on CPU group 0 */
547		- unsigned int i, count, last;
548		-
549		- val = ocelot_read(ocelot, QS_INJ_STATUS);
550		- if (!(val & QS_INJ_STATUS_FIFO_RDY(BIT(grp))) \|\|
551		- (val & QS_INJ_STATUS_WMARK_REACHED(BIT(grp))))
552		- return NETDEV_TX_BUSY;
553		-
554		- ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) \|
555		- QS_INJ_CTRL_SOF, QS_INJ_CTRL, grp);
556		-
557		- info.port = BIT(port->chip_port);
558		- info.tag_type = IFH_TAG_TYPE_C;
559		- info.vid = skb_vlan_tag_get(skb);
560		- ocelot_gen_ifh(ifh, &info);
561		-
562		- for (i = 0; i < IFH_LEN; i++)
563		- ocelot_write_rix(ocelot, (__force u32)cpu_to_be32(ifh[i]),
564		- QS_INJ_WR, grp);
565		-
566		- count = (skb->len + 3) / 4;
567		- last = skb->len % 4;
568		- for (i = 0; i < count; i++) {
569		- ocelot_write_rix(ocelot, ((u32 *)skb->data)[i], QS_INJ_WR, grp);
570		- }
571		-
572		- /* Add padding */
573		- while (i < (OCELOT_BUFFER_CELL_SZ / 4)) {
574		- ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
575		- i++;
576		- }
577		-
578		- /* Indicate EOF and valid bytes in last word */
579		- ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) \|
580		- QS_INJ_CTRL_VLD_BYTES(skb->len < OCELOT_BUFFER_CELL_SZ ? 0 : last) \|
581		- QS_INJ_CTRL_EOF,
582		- QS_INJ_CTRL, grp);
583		-
584		- /* Add dummy CRC */
585		- ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
586		- skb_tx_timestamp(skb);
587		-
588		- dev->stats.tx_packets++;
589		- dev->stats.tx_bytes += skb->len;
590		- dev_kfree_skb_any(skb);
591		-
592		- return NETDEV_TX_OK;
593		-}
594		-
595		-static void ocelot_mact_mc_reset(struct ocelot_port *port)
596		-{
597		- struct ocelot *ocelot = port->ocelot;
598		- struct netdev_hw_addr ha, n;
599		-
600		- /* Free and forget all the MAC addresses stored in the port private mc
601		- * list. These are mc addresses that were previously added by calling
602		- * ocelot_mact_mc_add().
603		- */
604		- list_for_each_entry_safe(ha, n, &port->mc, list) {
605		- ocelot_mact_forget(ocelot, ha->addr, port->pvid);
606		- list_del(&ha->list);
607		- kfree(ha);
608		- }
609		-}
610		-
611		-static int ocelot_mact_mc_add(struct ocelot_port *port,
612		- struct netdev_hw_addr *hw_addr)
613		-{
614		- struct ocelot *ocelot = port->ocelot;
615		- struct netdev_hw_addr ha = kzalloc(sizeof(ha), GFP_ATOMIC);
616		-
617		- if (!ha)
618		- return -ENOMEM;
619		-
620		- memcpy(ha, hw_addr, sizeof(*ha));
621		- list_add_tail(&ha->list, &port->mc);
622		-
623		- ocelot_mact_learn(ocelot, PGID_CPU, ha->addr, port->pvid,
624		- ENTRYTYPE_LOCKED);
625		-
626		- return 0;
627		-}
628		-
629		-static void ocelot_set_rx_mode(struct net_device *dev)
630		-{
631		- struct ocelot_port *port = netdev_priv(dev);
632		- struct ocelot *ocelot = port->ocelot;
633		- struct netdev_hw_addr *ha;
634		- int i;
	534	+ unsigned long flags;
635	535	u32 val;
636	536
637		- /* This doesn't handle promiscuous mode because the bridge core is
638		- * setting IFF_PROMISC on all slave interfaces and all frames would be
639		- * forwarded to the CPU port.
640		- */
641		- val = GENMASK(ocelot->num_phys_ports - 1, 0);
642		- for (i = ocelot->num_phys_ports + 1; i < PGID_CPU; i++)
643		- ocelot_write_rix(ocelot, val, ANA_PGID_PGID, i);
	537	+ spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
644	538
645		- /* Handle the device multicast addresses. First remove all the
646		- * previously installed addresses and then add the latest ones to the
647		- * mac table.
648		- */
649		- ocelot_mact_mc_reset(port);
650		- netdev_for_each_mc_addr(ha, dev)
651		- ocelot_mact_mc_add(port, ha);
	539	+ /* Read current PTP time to get seconds */
	540	+ val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
	541	+
	542	+ val &= ~(PTP_PIN_CFG_SYNC \| PTP_PIN_CFG_ACTION_MASK \| PTP_PIN_CFG_DOM);
	543	+ val \|= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE);
	544	+ ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
	545	+ ts->tv_sec = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
	546	+
	547	+ /* Read packet HW timestamp from FIFO */
	548	+ val = ocelot_read(ocelot, SYS_PTP_TXSTAMP);
	549	+ ts->tv_nsec = SYS_PTP_TXSTAMP_PTP_TXSTAMP(val);
	550	+
	551	+ /* Sec has incremented since the ts was registered */
	552	+ if ((ts->tv_sec & 0x1) != !!(val & SYS_PTP_TXSTAMP_PTP_TXSTAMP_SEC))
	553	+ ts->tv_sec--;
	554	+
	555	+ spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
652	556	}
653	557
654		-static int ocelot_port_get_phys_port_name(struct net_device *dev,
655		- char *buf, size_t len)
	558	+void ocelot_get_txtstamp(struct ocelot *ocelot)
656	559	{
657		- struct ocelot_port *port = netdev_priv(dev);
658		- int ret;
	560	+ int budget = OCELOT_PTP_QUEUE_SZ;
659	561
660		- ret = snprintf(buf, len, "p%d", port->chip_port);
661		- if (ret >= len)
662		- return -EINVAL;
	562	+ while (budget--) {
	563	+ struct sk_buff skb, skb_tmp, *skb_match = NULL;
	564	+ struct skb_shared_hwtstamps shhwtstamps;
	565	+ struct ocelot_port *port;
	566	+ struct timespec64 ts;
	567	+ unsigned long flags;
	568	+ u32 val, id, txport;
663	569
664		- return 0;
	570	+ val = ocelot_read(ocelot, SYS_PTP_STATUS);
	571	+
	572	+ /* Check if a timestamp can be retrieved */
	573	+ if (!(val & SYS_PTP_STATUS_PTP_MESS_VLD))
	574	+ break;
	575	+
	576	+ WARN_ON(val & SYS_PTP_STATUS_PTP_OVFL);
	577	+
	578	+ /* Retrieve the ts ID and Tx port */
	579	+ id = SYS_PTP_STATUS_PTP_MESS_ID_X(val);
	580	+ txport = SYS_PTP_STATUS_PTP_MESS_TXPORT_X(val);
	581	+
	582	+ /* Retrieve its associated skb */
	583	+ port = ocelot->ports[txport];
	584	+
	585	+ spin_lock_irqsave(&port->tx_skbs.lock, flags);
	586	+
	587	+ skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
	588	+ if (skb->cb[0] != id)
	589	+ continue;
	590	+ __skb_unlink(skb, &port->tx_skbs);
	591	+ skb_match = skb;
	592	+ break;
	593	+ }
	594	+
	595	+ spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
	596	+
	597	+ if (WARN_ON(!skb_match))
	598	+ continue;
	599	+
	600	+ /* Get the h/w timestamp */
	601	+ ocelot_get_hwtimestamp(ocelot, &ts);
	602	+
	603	+ /* Set the timestamp into the skb */
	604	+ memset(&shhwtstamps, 0, sizeof(shhwtstamps));
	605	+ shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
	606	+ skb_complete_tx_timestamp(skb_match, &shhwtstamps);
	607	+
	608	+ /* Next ts */
	609	+ ocelot_write(ocelot, SYS_PTP_NXT_PTP_NXT, SYS_PTP_NXT);
	610	+ }
665	611	}
	612	+EXPORT_SYMBOL(ocelot_get_txtstamp);
666	613
667		-static int ocelot_port_set_mac_address(struct net_device dev, void p)
	614	+int ocelot_fdb_add(struct ocelot *ocelot, int port,
	615	+ const unsigned char *addr, u16 vid)
668	616	{
669		- struct ocelot_port *port = netdev_priv(dev);
670		- struct ocelot *ocelot = port->ocelot;
671		- const struct sockaddr *addr = p;
	617	+ struct ocelot_port *ocelot_port = ocelot->ports[port];
	618	+ int pgid = port;
672	619
673		- /* Learn the new net device MAC address in the mac table. */
674		- ocelot_mact_learn(ocelot, PGID_CPU, addr->sa_data, port->pvid,
675		- ENTRYTYPE_LOCKED);
676		- /* Then forget the previous one. */
677		- ocelot_mact_forget(ocelot, dev->dev_addr, port->pvid);
678		-
679		- ether_addr_copy(dev->dev_addr, addr->sa_data);
680		- return 0;
681		-}
682		-
683		-static void ocelot_get_stats64(struct net_device *dev,
684		- struct rtnl_link_stats64 *stats)
685		-{
686		- struct ocelot_port *port = netdev_priv(dev);
687		- struct ocelot *ocelot = port->ocelot;
688		-
689		- /* Configure the port to read the stats from */
690		- ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(port->chip_port),
691		- SYS_STAT_CFG);
692		-
693		- /* Get Rx stats */
694		- stats->rx_bytes = ocelot_read(ocelot, SYS_COUNT_RX_OCTETS);
695		- stats->rx_packets = ocelot_read(ocelot, SYS_COUNT_RX_SHORTS) +
696		- ocelot_read(ocelot, SYS_COUNT_RX_FRAGMENTS) +
697		- ocelot_read(ocelot, SYS_COUNT_RX_JABBERS) +
698		- ocelot_read(ocelot, SYS_COUNT_RX_LONGS) +
699		- ocelot_read(ocelot, SYS_COUNT_RX_64) +
700		- ocelot_read(ocelot, SYS_COUNT_RX_65_127) +
701		- ocelot_read(ocelot, SYS_COUNT_RX_128_255) +
702		- ocelot_read(ocelot, SYS_COUNT_RX_256_1023) +
703		- ocelot_read(ocelot, SYS_COUNT_RX_1024_1526) +
704		- ocelot_read(ocelot, SYS_COUNT_RX_1527_MAX);
705		- stats->multicast = ocelot_read(ocelot, SYS_COUNT_RX_MULTICAST);
706		- stats->rx_dropped = dev->stats.rx_dropped;
707		-
708		- /* Get Tx stats */
709		- stats->tx_bytes = ocelot_read(ocelot, SYS_COUNT_TX_OCTETS);
710		- stats->tx_packets = ocelot_read(ocelot, SYS_COUNT_TX_64) +
711		- ocelot_read(ocelot, SYS_COUNT_TX_65_127) +
712		- ocelot_read(ocelot, SYS_COUNT_TX_128_511) +
713		- ocelot_read(ocelot, SYS_COUNT_TX_512_1023) +
714		- ocelot_read(ocelot, SYS_COUNT_TX_1024_1526) +
715		- ocelot_read(ocelot, SYS_COUNT_TX_1527_MAX);
716		- stats->tx_dropped = ocelot_read(ocelot, SYS_COUNT_TX_DROPS) +
717		- ocelot_read(ocelot, SYS_COUNT_TX_AGING);
718		- stats->collisions = ocelot_read(ocelot, SYS_COUNT_TX_COLLISION);
719		-}
720		-
721		-static int ocelot_fdb_add(struct ndmsg ndm, struct nlattr tb[],
722		- struct net_device dev, const unsigned char addr,
723		- u16 vid, u16 flags)
724		-{
725		- struct ocelot_port *port = netdev_priv(dev);
726		- struct ocelot *ocelot = port->ocelot;
	620	+ if (port == ocelot->npi)
	621	+ pgid = PGID_CPU;
727	622
728	623	if (!vid) {
729		- if (!port->vlan_aware)
	624	+ if (!ocelot_port->vlan_aware)
730	625	/* If the bridge is not VLAN aware and no VID was
731	626	* provided, set it to pvid to ensure the MAC entry
732	627	* matches incoming untagged packets
733	628	*/
734		- vid = port->pvid;
	629	+ vid = ocelot_port->pvid;
735	630	else
736	631	/* If the bridge is VLAN aware a VID must be provided as
737	632	* otherwise the learnt entry wouldn't match any frame.
..	..	@@ -739,30 +634,21 @@
739	634	return -EINVAL;
740	635	}
741	636
742		- return ocelot_mact_learn(ocelot, port->chip_port, addr, vid,
743		- ENTRYTYPE_LOCKED);
	637	+ return ocelot_mact_learn(ocelot, pgid, addr, vid, ENTRYTYPE_LOCKED);
744	638	}
	639	+EXPORT_SYMBOL(ocelot_fdb_add);
745	640
746		-static int ocelot_fdb_del(struct ndmsg ndm, struct nlattr tb[],
747		- struct net_device *dev,
748		- const unsigned char *addr, u16 vid)
	641	+int ocelot_fdb_del(struct ocelot *ocelot, int port,
	642	+ const unsigned char *addr, u16 vid)
749	643	{
750		- struct ocelot_port *port = netdev_priv(dev);
751		- struct ocelot *ocelot = port->ocelot;
752		-
753	644	return ocelot_mact_forget(ocelot, addr, vid);
754	645	}
	646	+EXPORT_SYMBOL(ocelot_fdb_del);
755	647
756		-struct ocelot_dump_ctx {
757		- struct net_device *dev;
758		- struct sk_buff *skb;
759		- struct netlink_callback *cb;
760		- int idx;
761		-};
762		-
763		-static int ocelot_fdb_do_dump(struct ocelot_mact_entry *entry,
764		- struct ocelot_dump_ctx *dump)
	648	+int ocelot_port_fdb_do_dump(const unsigned char *addr, u16 vid,
	649	+ bool is_static, void *data)
765	650	{
	651	+ struct ocelot_dump_ctx *dump = data;
766	652	u32 portid = NETLINK_CB(dump->cb->skb).portid;
767	653	u32 seq = dump->cb->nlh->nlmsg_seq;
768	654	struct nlmsghdr *nlh;
..	..	@@ -783,12 +669,12 @@
783	669	ndm->ndm_flags = NTF_SELF;
784	670	ndm->ndm_type = 0;
785	671	ndm->ndm_ifindex = dump->dev->ifindex;
786		- ndm->ndm_state = NUD_REACHABLE;
	672	+ ndm->ndm_state = is_static ? NUD_NOARP : NUD_REACHABLE;
787	673
788		- if (nla_put(dump->skb, NDA_LLADDR, ETH_ALEN, entry->mac))
	674	+ if (nla_put(dump->skb, NDA_LLADDR, ETH_ALEN, addr))
789	675	goto nla_put_failure;
790	676
791		- if (entry->vid && nla_put_u16(dump->skb, NDA_VLAN, entry->vid))
	677	+ if (vid && nla_put_u16(dump->skb, NDA_VLAN, vid))
792	678	goto nla_put_failure;
793	679
794	680	nlmsg_end(dump->skb, nlh);
..	..	@@ -801,13 +687,13 @@
801	687	nlmsg_cancel(dump->skb, nlh);
802	688	return -EMSGSIZE;
803	689	}
	690	+EXPORT_SYMBOL(ocelot_port_fdb_do_dump);
804	691
805		-static inline int ocelot_mact_read(struct ocelot_port *port, int row, int col,
806		- struct ocelot_mact_entry *entry)
	692	+static int ocelot_mact_read(struct ocelot *ocelot, int port, int row, int col,
	693	+ struct ocelot_mact_entry *entry)
807	694	{
808		- struct ocelot *ocelot = port->ocelot;
809		- char mac[ETH_ALEN];
810	695	u32 val, dst, macl, mach;
	696	+ char mac[ETH_ALEN];
811	697
812	698	/* Set row and column to read from */
813	699	ocelot_field_write(ocelot, ANA_TABLES_MACTINDX_M_INDEX, row);
..	..	@@ -830,7 +716,7 @@
830	716	* do not report it.
831	717	*/
832	718	dst = (val & ANA_TABLES_MACACCESS_DEST_IDX_M) >> 3;
833		- if (dst != port->chip_port)
	719	+ if (dst != port)
834	720	return -EINVAL;
835	721
836	722	/* Get the entry's MAC address and VLAN id */
..	..	@@ -850,90 +736,107 @@
850	736	return 0;
851	737	}
852	738
853		-static int ocelot_fdb_dump(struct sk_buff skb, struct netlink_callback cb,
854		- struct net_device *dev,
855		- struct net_device filter_dev, int idx)
	739	+int ocelot_fdb_dump(struct ocelot *ocelot, int port,
	740	+ dsa_fdb_dump_cb_t cb, void data)
856	741	{
857		- struct ocelot_port *port = netdev_priv(dev);
858		- int i, j, ret = 0;
859		- struct ocelot_dump_ctx dump = {
860		- .dev = dev,
861		- .skb = skb,
862		- .cb = cb,
863		- .idx = *idx,
864		- };
	742	+ int i, j;
865	743
866		- struct ocelot_mact_entry entry;
867		-
868		- /* Loop through all the mac tables entries. There are 1024 rows of 4
869		- * entries.
870		- */
871		- for (i = 0; i < 1024; i++) {
	744	+ /* Loop through all the mac tables entries. */
	745	+ for (i = 0; i < ocelot->num_mact_rows; i++) {
872	746	for (j = 0; j < 4; j++) {
873		- ret = ocelot_mact_read(port, i, j, &entry);
	747	+ struct ocelot_mact_entry entry;
	748	+ bool is_static;
	749	+ int ret;
	750	+
	751	+ ret = ocelot_mact_read(ocelot, port, i, j, &entry);
874	752	/* If the entry is invalid (wrong port, invalid...),
875	753	* skip it.
876	754	*/
877	755	if (ret == -EINVAL)
878	756	continue;
879	757	else if (ret)
880		- goto end;
	758	+ return ret;
881	759
882		- ret = ocelot_fdb_do_dump(&entry, &dump);
	760	+ is_static = (entry.type == ENTRYTYPE_LOCKED);
	761	+
	762	+ ret = cb(entry.mac, entry.vid, is_static, data);
883	763	if (ret)
884		- goto end;
	764	+ return ret;
885	765	}
886	766	}
887	767
888		-end:
889		- *idx = dump.idx;
890		- return ret;
891		-}
892		-
893		-static int ocelot_vlan_rx_add_vid(struct net_device *dev, __be16 proto,
894		- u16 vid)
895		-{
896		- return ocelot_vlan_vid_add(dev, vid, false, false);
897		-}
898		-
899		-static int ocelot_vlan_rx_kill_vid(struct net_device *dev, __be16 proto,
900		- u16 vid)
901		-{
902		- return ocelot_vlan_vid_del(dev, vid);
903		-}
904		-
905		-static int ocelot_set_features(struct net_device *dev,
906		- netdev_features_t features)
907		-{
908		- struct ocelot_port *port = netdev_priv(dev);
909		- netdev_features_t changed = dev->features ^ features;
910		-
911		- if (changed & NETIF_F_HW_VLAN_CTAG_FILTER)
912		- ocelot_vlan_mode(port, features);
913		-
914	768	return 0;
915	769	}
	770	+EXPORT_SYMBOL(ocelot_fdb_dump);
916	771
917		-static const struct net_device_ops ocelot_port_netdev_ops = {
918		- .ndo_open = ocelot_port_open,
919		- .ndo_stop = ocelot_port_stop,
920		- .ndo_start_xmit = ocelot_port_xmit,
921		- .ndo_set_rx_mode = ocelot_set_rx_mode,
922		- .ndo_get_phys_port_name = ocelot_port_get_phys_port_name,
923		- .ndo_set_mac_address = ocelot_port_set_mac_address,
924		- .ndo_get_stats64 = ocelot_get_stats64,
925		- .ndo_fdb_add = ocelot_fdb_add,
926		- .ndo_fdb_del = ocelot_fdb_del,
927		- .ndo_fdb_dump = ocelot_fdb_dump,
928		- .ndo_vlan_rx_add_vid = ocelot_vlan_rx_add_vid,
929		- .ndo_vlan_rx_kill_vid = ocelot_vlan_rx_kill_vid,
930		- .ndo_set_features = ocelot_set_features,
931		-};
932		-
933		-static void ocelot_get_strings(struct net_device netdev, u32 sset, u8 data)
	772	+int ocelot_hwstamp_get(struct ocelot ocelot, int port, struct ifreq ifr)
934	773	{
935		- struct ocelot_port *port = netdev_priv(netdev);
936		- struct ocelot *ocelot = port->ocelot;
	774	+ return copy_to_user(ifr->ifr_data, &ocelot->hwtstamp_config,
	775	+ sizeof(ocelot->hwtstamp_config)) ? -EFAULT : 0;
	776	+}
	777	+EXPORT_SYMBOL(ocelot_hwstamp_get);
	778	+
	779	+int ocelot_hwstamp_set(struct ocelot ocelot, int port, struct ifreq ifr)
	780	+{
	781	+ struct ocelot_port *ocelot_port = ocelot->ports[port];
	782	+ struct hwtstamp_config cfg;
	783	+
	784	+ if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
	785	+ return -EFAULT;
	786	+
	787	+ /* reserved for future extensions */
	788	+ if (cfg.flags)
	789	+ return -EINVAL;
	790	+
	791	+ /* Tx type sanity check */
	792	+ switch (cfg.tx_type) {
	793	+ case HWTSTAMP_TX_ON:
	794	+ ocelot_port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
	795	+ break;
	796	+ case HWTSTAMP_TX_ONESTEP_SYNC:
	797	+ /* IFH_REW_OP_ONE_STEP_PTP updates the correctional field, we
	798	+ * need to update the origin time.
	799	+ */
	800	+ ocelot_port->ptp_cmd = IFH_REW_OP_ORIGIN_PTP;
	801	+ break;
	802	+ case HWTSTAMP_TX_OFF:
	803	+ ocelot_port->ptp_cmd = 0;
	804	+ break;
	805	+ default:
	806	+ return -ERANGE;
	807	+ }
	808	+
	809	+ mutex_lock(&ocelot->ptp_lock);
	810	+
	811	+ switch (cfg.rx_filter) {
	812	+ case HWTSTAMP_FILTER_NONE:
	813	+ break;
	814	+ case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
	815	+ case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
	816	+ case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
	817	+ case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
	818	+ case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
	819	+ case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
	820	+ case HWTSTAMP_FILTER_PTP_V2_EVENT:
	821	+ case HWTSTAMP_FILTER_PTP_V2_SYNC:
	822	+ case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
	823	+ cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
	824	+ break;
	825	+ default:
	826	+ mutex_unlock(&ocelot->ptp_lock);
	827	+ return -ERANGE;
	828	+ }
	829	+
	830	+ /* Commit back the result & save it */
	831	+ memcpy(&ocelot->hwtstamp_config, &cfg, sizeof(cfg));
	832	+ mutex_unlock(&ocelot->ptp_lock);
	833	+
	834	+ return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
	835	+}
	836	+EXPORT_SYMBOL(ocelot_hwstamp_set);
	837	+
	838	+void ocelot_get_strings(struct ocelot ocelot, int port, u32 sset, u8 data)
	839	+{
937	840	int i;
938	841
939	842	if (sset != ETH_SS_STATS)
..	..	@@ -943,14 +846,12 @@
943	846	memcpy(data + i * ETH_GSTRING_LEN, ocelot->stats_layout[i].name,
944	847	ETH_GSTRING_LEN);
945	848	}
	849	+EXPORT_SYMBOL(ocelot_get_strings);
946	850
947		-static void ocelot_check_stats(struct work_struct *work)
	851	+/* Caller must hold &ocelot->stats_lock */
	852	+static void ocelot_update_stats(struct ocelot *ocelot)
948	853	{
949		- struct delayed_work *del_work = to_delayed_work(work);
950		- struct ocelot *ocelot = container_of(del_work, struct ocelot, stats_work);
951	854	int i, j;
952		-
953		- mutex_lock(&ocelot->stats_lock);
954	855
955	856	for (i = 0; i < ocelot->num_phys_ports; i++) {
956	857	/* Configure the port to read the stats from */
..	..	@@ -970,106 +871,108 @@
970	871	~(u64)U32_MAX) + val;
971	872	}
972	873	}
973		-
974		- cancel_delayed_work(&ocelot->stats_work);
975		- queue_delayed_work(ocelot->stats_queue, &ocelot->stats_work,
976		- OCELOT_STATS_CHECK_DELAY);
977		-
978		- mutex_unlock(&ocelot->stats_lock);
979	874	}
980	875
981		-static void ocelot_get_ethtool_stats(struct net_device *dev,
982		- struct ethtool_stats stats, u64 data)
	876	+static void ocelot_check_stats_work(struct work_struct *work)
983	877	{
984		- struct ocelot_port *port = netdev_priv(dev);
985		- struct ocelot *ocelot = port->ocelot;
	878	+ struct delayed_work *del_work = to_delayed_work(work);
	879	+ struct ocelot *ocelot = container_of(del_work, struct ocelot,
	880	+ stats_work);
	881	+
	882	+ mutex_lock(&ocelot->stats_lock);
	883	+ ocelot_update_stats(ocelot);
	884	+ mutex_unlock(&ocelot->stats_lock);
	885	+
	886	+ queue_delayed_work(ocelot->stats_queue, &ocelot->stats_work,
	887	+ OCELOT_STATS_CHECK_DELAY);
	888	+}
	889	+
	890	+void ocelot_get_ethtool_stats(struct ocelot ocelot, int port, u64 data)
	891	+{
986	892	int i;
987	893
	894	+ mutex_lock(&ocelot->stats_lock);
	895	+
988	896	/* check and update now */
989		- ocelot_check_stats(&ocelot->stats_work.work);
	897	+ ocelot_update_stats(ocelot);
990	898
991	899	/* Copy all counters */
992	900	for (i = 0; i < ocelot->num_stats; i++)
993		- data++ = ocelot->stats[port->chip_port ocelot->num_stats + i];
	901	+ data++ = ocelot->stats[port ocelot->num_stats + i];
	902	+
	903	+ mutex_unlock(&ocelot->stats_lock);
994	904	}
	905	+EXPORT_SYMBOL(ocelot_get_ethtool_stats);
995	906
996		-static int ocelot_get_sset_count(struct net_device *dev, int sset)
	907	+int ocelot_get_sset_count(struct ocelot *ocelot, int port, int sset)
997	908	{
998		- struct ocelot_port *port = netdev_priv(dev);
999		- struct ocelot *ocelot = port->ocelot;
1000		-
1001	909	if (sset != ETH_SS_STATS)
1002	910	return -EOPNOTSUPP;
	911	+
1003	912	return ocelot->num_stats;
1004	913	}
	914	+EXPORT_SYMBOL(ocelot_get_sset_count);
1005	915
1006		-static const struct ethtool_ops ocelot_ethtool_ops = {
1007		- .get_strings = ocelot_get_strings,
1008		- .get_ethtool_stats = ocelot_get_ethtool_stats,
1009		- .get_sset_count = ocelot_get_sset_count,
1010		- .get_link_ksettings = phy_ethtool_get_link_ksettings,
1011		- .set_link_ksettings = phy_ethtool_set_link_ksettings,
1012		-};
1013		-
1014		-static int ocelot_port_attr_get(struct net_device *dev,
1015		- struct switchdev_attr *attr)
	916	+int ocelot_get_ts_info(struct ocelot *ocelot, int port,
	917	+ struct ethtool_ts_info *info)
1016	918	{
1017		- struct ocelot_port *ocelot_port = netdev_priv(dev);
1018		- struct ocelot *ocelot = ocelot_port->ocelot;
1019		-
1020		- switch (attr->id) {
1021		- case SWITCHDEV_ATTR_ID_PORT_PARENT_ID:
1022		- attr->u.ppid.id_len = sizeof(ocelot->base_mac);
1023		- memcpy(&attr->u.ppid.id, &ocelot->base_mac,
1024		- attr->u.ppid.id_len);
1025		- break;
1026		- default:
1027		- return -EOPNOTSUPP;
	919	+ info->phc_index = ocelot->ptp_clock ?
	920	+ ptp_clock_index(ocelot->ptp_clock) : -1;
	921	+ if (info->phc_index == -1) {
	922	+ info->so_timestamping \|= SOF_TIMESTAMPING_TX_SOFTWARE \|
	923	+ SOF_TIMESTAMPING_RX_SOFTWARE \|
	924	+ SOF_TIMESTAMPING_SOFTWARE;
	925	+ return 0;
1028	926	}
	927	+ info->so_timestamping \|= SOF_TIMESTAMPING_TX_SOFTWARE \|
	928	+ SOF_TIMESTAMPING_RX_SOFTWARE \|
	929	+ SOF_TIMESTAMPING_SOFTWARE \|
	930	+ SOF_TIMESTAMPING_TX_HARDWARE \|
	931	+ SOF_TIMESTAMPING_RX_HARDWARE \|
	932	+ SOF_TIMESTAMPING_RAW_HARDWARE;
	933	+ info->tx_types = BIT(HWTSTAMP_TX_OFF) \| BIT(HWTSTAMP_TX_ON) \|
	934	+ BIT(HWTSTAMP_TX_ONESTEP_SYNC);
	935	+ info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) \|
	936	+ BIT(HWTSTAMP_FILTER_PTP_V2_EVENT) \|
	937	+ BIT(HWTSTAMP_FILTER_PTP_V2_L2_EVENT) \|
	938	+ BIT(HWTSTAMP_FILTER_PTP_V2_L4_EVENT);
1029	939
1030	940	return 0;
1031	941	}
	942	+EXPORT_SYMBOL(ocelot_get_ts_info);
1032	943
1033		-static int ocelot_port_attr_stp_state_set(struct ocelot_port *ocelot_port,
1034		- struct switchdev_trans *trans,
1035		- u8 state)
	944	+void ocelot_bridge_stp_state_set(struct ocelot *ocelot, int port, u8 state)
1036	945	{
1037		- struct ocelot *ocelot = ocelot_port->ocelot;
1038	946	u32 port_cfg;
1039		- int port, i;
	947	+ int p, i;
1040	948
1041		- if (switchdev_trans_ph_prepare(trans))
1042		- return 0;
	949	+ if (!(BIT(port) & ocelot->bridge_mask))
	950	+ return;
1043	951
1044		- if (!(BIT(ocelot_port->chip_port) & ocelot->bridge_mask))
1045		- return 0;
1046		-
1047		- port_cfg = ocelot_read_gix(ocelot, ANA_PORT_PORT_CFG,
1048		- ocelot_port->chip_port);
	952	+ port_cfg = ocelot_read_gix(ocelot, ANA_PORT_PORT_CFG, port);
1049	953
1050	954	switch (state) {
1051	955	case BR_STATE_FORWARDING:
1052		- ocelot->bridge_fwd_mask \|= BIT(ocelot_port->chip_port);
1053		- /* Fallthrough */
	956	+ ocelot->bridge_fwd_mask \|= BIT(port);
	957	+ fallthrough;
1054	958	case BR_STATE_LEARNING:
1055	959	port_cfg \|= ANA_PORT_PORT_CFG_LEARN_ENA;
1056	960	break;
1057	961
1058	962	default:
1059	963	port_cfg &= ~ANA_PORT_PORT_CFG_LEARN_ENA;
1060		- ocelot->bridge_fwd_mask &= ~BIT(ocelot_port->chip_port);
	964	+ ocelot->bridge_fwd_mask &= ~BIT(port);
1061	965	break;
1062	966	}
1063	967
1064		- ocelot_write_gix(ocelot, port_cfg, ANA_PORT_PORT_CFG,
1065		- ocelot_port->chip_port);
	968	+ ocelot_write_gix(ocelot, port_cfg, ANA_PORT_PORT_CFG, port);
1066	969
1067	970	/* Apply FWD mask. The loop is needed to add/remove the current port as
1068	971	* a source for the other ports.
1069	972	*/
1070		- for (port = 0; port < ocelot->num_phys_ports; port++) {
1071		- if (ocelot->bridge_fwd_mask & BIT(port)) {
1072		- unsigned long mask = ocelot->bridge_fwd_mask & ~BIT(port);
	973	+ for (p = 0; p < ocelot->num_phys_ports; p++) {
	974	+ if (ocelot->bridge_fwd_mask & BIT(p)) {
	975	+ unsigned long mask = ocelot->bridge_fwd_mask & ~BIT(p);
1073	976
1074	977	for (i = 0; i < ocelot->num_phys_ports; i++) {
1075	978	unsigned long bond_mask = ocelot->lags[i];
..	..	@@ -1077,120 +980,35 @@
1077	980	if (!bond_mask)
1078	981	continue;
1079	982
1080		- if (bond_mask & BIT(port)) {
	983	+ if (bond_mask & BIT(p)) {
1081	984	mask &= ~bond_mask;
1082	985	break;
1083	986	}
1084	987	}
1085	988
1086		- ocelot_write_rix(ocelot,
1087		- BIT(ocelot->num_phys_ports) \| mask,
1088		- ANA_PGID_PGID, PGID_SRC + port);
	989	+ ocelot_write_rix(ocelot, mask,
	990	+ ANA_PGID_PGID, PGID_SRC + p);
1089	991	} else {
1090		- /* Only the CPU port, this is compatible with link
1091		- * aggregation.
1092		- */
1093		- ocelot_write_rix(ocelot,
1094		- BIT(ocelot->num_phys_ports),
1095		- ANA_PGID_PGID, PGID_SRC + port);
	992	+ ocelot_write_rix(ocelot, 0,
	993	+ ANA_PGID_PGID, PGID_SRC + p);
1096	994	}
1097	995	}
1098		-
1099		- return 0;
1100	996	}
	997	+EXPORT_SYMBOL(ocelot_bridge_stp_state_set);
1101	998
1102		-static void ocelot_port_attr_ageing_set(struct ocelot_port *ocelot_port,
1103		- unsigned long ageing_clock_t)
	999	+void ocelot_set_ageing_time(struct ocelot *ocelot, unsigned int msecs)
1104	1000	{
1105		- struct ocelot *ocelot = ocelot_port->ocelot;
1106		- unsigned long ageing_jiffies = clock_t_to_jiffies(ageing_clock_t);
1107		- u32 ageing_time = jiffies_to_msecs(ageing_jiffies) / 1000;
	1001	+ unsigned int age_period = ANA_AUTOAGE_AGE_PERIOD(msecs / 2000);
1108	1002
1109		- ocelot_write(ocelot, ANA_AUTOAGE_AGE_PERIOD(ageing_time / 2),
1110		- ANA_AUTOAGE);
	1003	+ /* Setting AGE_PERIOD to zero effectively disables automatic aging,
	1004	+ * which is clearly not what our intention is. So avoid that.
	1005	+ */
	1006	+ if (!age_period)
	1007	+ age_period = 1;
	1008	+
	1009	+ ocelot_rmw(ocelot, age_period, ANA_AUTOAGE_AGE_PERIOD_M, ANA_AUTOAGE);
1111	1010	}
1112		-
1113		-static void ocelot_port_attr_mc_set(struct ocelot_port *port, bool mc)
1114		-{
1115		- struct ocelot *ocelot = port->ocelot;
1116		- u32 val = ocelot_read_gix(ocelot, ANA_PORT_CPU_FWD_CFG,
1117		- port->chip_port);
1118		-
1119		- if (mc)
1120		- val \|= ANA_PORT_CPU_FWD_CFG_CPU_IGMP_REDIR_ENA \|
1121		- ANA_PORT_CPU_FWD_CFG_CPU_MLD_REDIR_ENA \|
1122		- ANA_PORT_CPU_FWD_CFG_CPU_IPMC_CTRL_COPY_ENA;
1123		- else
1124		- val &= ~(ANA_PORT_CPU_FWD_CFG_CPU_IGMP_REDIR_ENA \|
1125		- ANA_PORT_CPU_FWD_CFG_CPU_MLD_REDIR_ENA \|
1126		- ANA_PORT_CPU_FWD_CFG_CPU_IPMC_CTRL_COPY_ENA);
1127		-
1128		- ocelot_write_gix(ocelot, val, ANA_PORT_CPU_FWD_CFG, port->chip_port);
1129		-}
1130		-
1131		-static int ocelot_port_attr_set(struct net_device *dev,
1132		- const struct switchdev_attr *attr,
1133		- struct switchdev_trans *trans)
1134		-{
1135		- struct ocelot_port *ocelot_port = netdev_priv(dev);
1136		- int err = 0;
1137		-
1138		- switch (attr->id) {
1139		- case SWITCHDEV_ATTR_ID_PORT_STP_STATE:
1140		- ocelot_port_attr_stp_state_set(ocelot_port, trans,
1141		- attr->u.stp_state);
1142		- break;
1143		- case SWITCHDEV_ATTR_ID_BRIDGE_AGEING_TIME:
1144		- ocelot_port_attr_ageing_set(ocelot_port, attr->u.ageing_time);
1145		- break;
1146		- case SWITCHDEV_ATTR_ID_BRIDGE_VLAN_FILTERING:
1147		- ocelot_port->vlan_aware = attr->u.vlan_filtering;
1148		- ocelot_vlan_port_apply(ocelot_port->ocelot, ocelot_port);
1149		- break;
1150		- case SWITCHDEV_ATTR_ID_BRIDGE_MC_DISABLED:
1151		- ocelot_port_attr_mc_set(ocelot_port, !attr->u.mc_disabled);
1152		- break;
1153		- default:
1154		- err = -EOPNOTSUPP;
1155		- break;
1156		- }
1157		-
1158		- return err;
1159		-}
1160		-
1161		-static int ocelot_port_obj_add_vlan(struct net_device *dev,
1162		- const struct switchdev_obj_port_vlan *vlan,
1163		- struct switchdev_trans *trans)
1164		-{
1165		- int ret;
1166		- u16 vid;
1167		-
1168		- for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
1169		- ret = ocelot_vlan_vid_add(dev, vid,
1170		- vlan->flags & BRIDGE_VLAN_INFO_PVID,
1171		- vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED);
1172		- if (ret)
1173		- return ret;
1174		- }
1175		-
1176		- return 0;
1177		-}
1178		-
1179		-static int ocelot_port_vlan_del_vlan(struct net_device *dev,
1180		- const struct switchdev_obj_port_vlan *vlan)
1181		-{
1182		- int ret;
1183		- u16 vid;
1184		-
1185		- for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
1186		- ret = ocelot_vlan_vid_del(dev, vid);
1187		-
1188		- if (ret)
1189		- return ret;
1190		- }
1191		-
1192		- return 0;
1193		-}
	1011	+EXPORT_SYMBOL(ocelot_set_ageing_time);
1194	1012
1195	1013	static struct ocelot_multicast ocelot_multicast_get(struct ocelot ocelot,
1196	1014	const unsigned char *addr,
..	..	@@ -1206,139 +1024,156 @@
1206	1024	return NULL;
1207	1025	}
1208	1026
1209		-static int ocelot_port_obj_add_mdb(struct net_device *dev,
1210		- const struct switchdev_obj_port_mdb *mdb,
1211		- struct switchdev_trans *trans)
	1027	+static enum macaccess_entry_type ocelot_classify_mdb(const unsigned char *addr)
1212	1028	{
1213		- struct ocelot_port *port = netdev_priv(dev);
1214		- struct ocelot *ocelot = port->ocelot;
1215		- struct ocelot_multicast *mc;
	1029	+ if (addr[0] == 0x01 && addr[1] == 0x00 && addr[2] == 0x5e)
	1030	+ return ENTRYTYPE_MACv4;
	1031	+ if (addr[0] == 0x33 && addr[1] == 0x33)
	1032	+ return ENTRYTYPE_MACv6;
	1033	+ return ENTRYTYPE_NORMAL;
	1034	+}
	1035	+
	1036	+static int ocelot_mdb_get_pgid(struct ocelot *ocelot,
	1037	+ enum macaccess_entry_type entry_type)
	1038	+{
	1039	+ int pgid;
	1040	+
	1041	+ /* According to VSC7514 datasheet 3.9.1.5 IPv4 Multicast Entries and
	1042	+ * 3.9.1.6 IPv6 Multicast Entries, "Instead of a lookup in the
	1043	+ * destination mask table (PGID), the destination set is programmed as
	1044	+ * part of the entry MAC address.", and the DEST_IDX is set to 0.
	1045	+ */
	1046	+ if (entry_type == ENTRYTYPE_MACv4 \|\|
	1047	+ entry_type == ENTRYTYPE_MACv6)
	1048	+ return 0;
	1049	+
	1050	+ for_each_nonreserved_multicast_dest_pgid(ocelot, pgid) {
	1051	+ struct ocelot_multicast *mc;
	1052	+ bool used = false;
	1053	+
	1054	+ list_for_each_entry(mc, &ocelot->multicast, list) {
	1055	+ if (mc->pgid == pgid) {
	1056	+ used = true;
	1057	+ break;
	1058	+ }
	1059	+ }
	1060	+
	1061	+ if (!used)
	1062	+ return pgid;
	1063	+ }
	1064	+
	1065	+ return -1;
	1066	+}
	1067	+
	1068	+static void ocelot_encode_ports_to_mdb(unsigned char *addr,
	1069	+ struct ocelot_multicast *mc,
	1070	+ enum macaccess_entry_type entry_type)
	1071	+{
	1072	+ memcpy(addr, mc->addr, ETH_ALEN);
	1073	+
	1074	+ if (entry_type == ENTRYTYPE_MACv4) {
	1075	+ addr[0] = 0;
	1076	+ addr[1] = mc->ports >> 8;
	1077	+ addr[2] = mc->ports & 0xff;
	1078	+ } else if (entry_type == ENTRYTYPE_MACv6) {
	1079	+ addr[0] = mc->ports >> 8;
	1080	+ addr[1] = mc->ports & 0xff;
	1081	+ }
	1082	+}
	1083	+
	1084	+int ocelot_port_mdb_add(struct ocelot *ocelot, int port,
	1085	+ const struct switchdev_obj_port_mdb *mdb)
	1086	+{
	1087	+ struct ocelot_port *ocelot_port = ocelot->ports[port];
	1088	+ enum macaccess_entry_type entry_type;
1216	1089	unsigned char addr[ETH_ALEN];
	1090	+ struct ocelot_multicast *mc;
1217	1091	u16 vid = mdb->vid;
1218	1092	bool new = false;
1219	1093
	1094	+ if (port == ocelot->npi)
	1095	+ port = ocelot->num_phys_ports;
	1096	+
1220	1097	if (!vid)
1221		- vid = port->pvid;
	1098	+ vid = ocelot_port->pvid;
	1099	+
	1100	+ entry_type = ocelot_classify_mdb(mdb->addr);
1222	1101
1223	1102	mc = ocelot_multicast_get(ocelot, mdb->addr, vid);
1224	1103	if (!mc) {
	1104	+ int pgid = ocelot_mdb_get_pgid(ocelot, entry_type);
	1105	+
	1106	+ if (pgid < 0) {
	1107	+ dev_err(ocelot->dev,
	1108	+ "No more PGIDs available for mdb %pM vid %d\n",
	1109	+ mdb->addr, vid);
	1110	+ return -ENOSPC;
	1111	+ }
	1112	+
1225	1113	mc = devm_kzalloc(ocelot->dev, sizeof(*mc), GFP_KERNEL);
1226	1114	if (!mc)
1227	1115	return -ENOMEM;
1228	1116
1229	1117	memcpy(mc->addr, mdb->addr, ETH_ALEN);
1230	1118	mc->vid = vid;
	1119	+ mc->pgid = pgid;
1231	1120
1232	1121	list_add_tail(&mc->list, &ocelot->multicast);
1233	1122	new = true;
1234	1123	}
1235	1124
1236		- memcpy(addr, mc->addr, ETH_ALEN);
1237		- addr[0] = 0;
1238		-
1239	1125	if (!new) {
1240		- addr[2] = mc->ports << 0;
1241		- addr[1] = mc->ports << 8;
	1126	+ ocelot_encode_ports_to_mdb(addr, mc, entry_type);
1242	1127	ocelot_mact_forget(ocelot, addr, vid);
1243	1128	}
1244	1129
1245		- mc->ports \|= BIT(port->chip_port);
1246		- addr[2] = mc->ports << 0;
1247		- addr[1] = mc->ports << 8;
	1130	+ mc->ports \|= BIT(port);
	1131	+ ocelot_encode_ports_to_mdb(addr, mc, entry_type);
1248	1132
1249		- return ocelot_mact_learn(ocelot, 0, addr, vid, ENTRYTYPE_MACv4);
	1133	+ return ocelot_mact_learn(ocelot, mc->pgid, addr, vid, entry_type);
1250	1134	}
	1135	+EXPORT_SYMBOL(ocelot_port_mdb_add);
1251	1136
1252		-static int ocelot_port_obj_del_mdb(struct net_device *dev,
1253		- const struct switchdev_obj_port_mdb *mdb)
	1137	+int ocelot_port_mdb_del(struct ocelot *ocelot, int port,
	1138	+ const struct switchdev_obj_port_mdb *mdb)
1254	1139	{
1255		- struct ocelot_port *port = netdev_priv(dev);
1256		- struct ocelot *ocelot = port->ocelot;
1257		- struct ocelot_multicast *mc;
	1140	+ struct ocelot_port *ocelot_port = ocelot->ports[port];
	1141	+ enum macaccess_entry_type entry_type;
1258	1142	unsigned char addr[ETH_ALEN];
	1143	+ struct ocelot_multicast *mc;
1259	1144	u16 vid = mdb->vid;
1260	1145
	1146	+ if (port == ocelot->npi)
	1147	+ port = ocelot->num_phys_ports;
	1148	+
1261	1149	if (!vid)
1262		- vid = port->pvid;
	1150	+ vid = ocelot_port->pvid;
1263	1151
1264	1152	mc = ocelot_multicast_get(ocelot, mdb->addr, vid);
1265	1153	if (!mc)
1266	1154	return -ENOENT;
1267	1155
1268		- memcpy(addr, mc->addr, ETH_ALEN);
1269		- addr[2] = mc->ports << 0;
1270		- addr[1] = mc->ports << 8;
1271		- addr[0] = 0;
	1156	+ entry_type = ocelot_classify_mdb(mdb->addr);
	1157	+
	1158	+ ocelot_encode_ports_to_mdb(addr, mc, entry_type);
1272	1159	ocelot_mact_forget(ocelot, addr, vid);
1273	1160
1274		- mc->ports &= ~BIT(port->chip_port);
	1161	+ mc->ports &= ~BIT(port);
1275	1162	if (!mc->ports) {
1276	1163	list_del(&mc->list);
1277	1164	devm_kfree(ocelot->dev, mc);
1278	1165	return 0;
1279	1166	}
1280	1167
1281		- addr[2] = mc->ports << 0;
1282		- addr[1] = mc->ports << 8;
	1168	+ ocelot_encode_ports_to_mdb(addr, mc, entry_type);
1283	1169
1284		- return ocelot_mact_learn(ocelot, 0, addr, vid, ENTRYTYPE_MACv4);
	1170	+ return ocelot_mact_learn(ocelot, mc->pgid, addr, vid, entry_type);
1285	1171	}
	1172	+EXPORT_SYMBOL(ocelot_port_mdb_del);
1286	1173
1287		-static int ocelot_port_obj_add(struct net_device *dev,
1288		- const struct switchdev_obj *obj,
1289		- struct switchdev_trans *trans)
	1174	+int ocelot_port_bridge_join(struct ocelot *ocelot, int port,
	1175	+ struct net_device *bridge)
1290	1176	{
1291		- int ret = 0;
1292		-
1293		- switch (obj->id) {
1294		- case SWITCHDEV_OBJ_ID_PORT_VLAN:
1295		- ret = ocelot_port_obj_add_vlan(dev,
1296		- SWITCHDEV_OBJ_PORT_VLAN(obj),
1297		- trans);
1298		- break;
1299		- case SWITCHDEV_OBJ_ID_PORT_MDB:
1300		- ret = ocelot_port_obj_add_mdb(dev, SWITCHDEV_OBJ_PORT_MDB(obj),
1301		- trans);
1302		- break;
1303		- default:
1304		- return -EOPNOTSUPP;
1305		- }
1306		-
1307		- return ret;
1308		-}
1309		-
1310		-static int ocelot_port_obj_del(struct net_device *dev,
1311		- const struct switchdev_obj *obj)
1312		-{
1313		- int ret = 0;
1314		-
1315		- switch (obj->id) {
1316		- case SWITCHDEV_OBJ_ID_PORT_VLAN:
1317		- ret = ocelot_port_vlan_del_vlan(dev,
1318		- SWITCHDEV_OBJ_PORT_VLAN(obj));
1319		- break;
1320		- case SWITCHDEV_OBJ_ID_PORT_MDB:
1321		- ret = ocelot_port_obj_del_mdb(dev, SWITCHDEV_OBJ_PORT_MDB(obj));
1322		- break;
1323		- default:
1324		- return -EOPNOTSUPP;
1325		- }
1326		-
1327		- return ret;
1328		-}
1329		-
1330		-static const struct switchdev_ops ocelot_port_switchdev_ops = {
1331		- .switchdev_port_attr_get = ocelot_port_attr_get,
1332		- .switchdev_port_attr_set = ocelot_port_attr_set,
1333		- .switchdev_port_obj_add = ocelot_port_obj_add,
1334		- .switchdev_port_obj_del = ocelot_port_obj_del,
1335		-};
1336		-
1337		-static int ocelot_port_bridge_join(struct ocelot_port *ocelot_port,
1338		- struct net_device *bridge)
1339		-{
1340		- struct ocelot *ocelot = ocelot_port->ocelot;
1341		-
1342	1177	if (!ocelot->bridge_mask) {
1343	1178	ocelot->hw_bridge_dev = bridge;
1344	1179	} else {
..	..	@@ -1348,36 +1183,47 @@
1348	1183	return -ENODEV;
1349	1184	}
1350	1185
1351		- ocelot->bridge_mask \|= BIT(ocelot_port->chip_port);
	1186	+ ocelot->bridge_mask \|= BIT(port);
1352	1187
1353	1188	return 0;
1354	1189	}
	1190	+EXPORT_SYMBOL(ocelot_port_bridge_join);
1355	1191
1356		-static void ocelot_port_bridge_leave(struct ocelot_port *ocelot_port,
1357		- struct net_device *bridge)
	1192	+int ocelot_port_bridge_leave(struct ocelot *ocelot, int port,
	1193	+ struct net_device *bridge)
1358	1194	{
1359		- struct ocelot *ocelot = ocelot_port->ocelot;
	1195	+ struct switchdev_trans trans;
	1196	+ int ret;
1360	1197
1361		- ocelot->bridge_mask &= ~BIT(ocelot_port->chip_port);
	1198	+ ocelot->bridge_mask &= ~BIT(port);
1362	1199
1363	1200	if (!ocelot->bridge_mask)
1364	1201	ocelot->hw_bridge_dev = NULL;
1365	1202
1366		- /* Clear bridge vlan settings before calling ocelot_vlan_port_apply */
1367		- ocelot_port->vlan_aware = 0;
1368		- ocelot_port->pvid = 0;
1369		- ocelot_port->vid = 0;
	1203	+ trans.ph_prepare = true;
	1204	+ ret = ocelot_port_vlan_filtering(ocelot, port, false, &trans);
	1205	+ if (ret)
	1206	+ return ret;
	1207	+
	1208	+ trans.ph_prepare = false;
	1209	+ ret = ocelot_port_vlan_filtering(ocelot, port, false, &trans);
	1210	+ if (ret)
	1211	+ return ret;
	1212	+
	1213	+ ocelot_port_set_pvid(ocelot, port, 0);
	1214	+ return ocelot_port_set_native_vlan(ocelot, port, 0);
1370	1215	}
	1216	+EXPORT_SYMBOL(ocelot_port_bridge_leave);
1371	1217
1372	1218	static void ocelot_set_aggr_pgids(struct ocelot *ocelot)
1373	1219	{
1374	1220	int i, port, lag;
1375	1221
1376	1222	/* Reset destination and aggregation PGIDS */
1377		- for (port = 0; port < ocelot->num_phys_ports; port++)
	1223	+ for_each_unicast_dest_pgid(ocelot, port)
1378	1224	ocelot_write_rix(ocelot, BIT(port), ANA_PGID_PGID, port);
1379	1225
1380		- for (i = PGID_AGGR; i < PGID_SRC; i++)
	1226	+ for_each_aggr_pgid(ocelot, i)
1381	1227	ocelot_write_rix(ocelot, GENMASK(ocelot->num_phys_ports - 1, 0),
1382	1228	ANA_PGID_PGID, i);
1383	1229
..	..	@@ -1399,7 +1245,7 @@
1399	1245	aggr_count++;
1400	1246	}
1401	1247
1402		- for (i = PGID_AGGR; i < PGID_SRC; i++) {
	1248	+ for_each_aggr_pgid(ocelot, i) {
1403	1249	u32 ac;
1404	1250
1405	1251	ac = ocelot_read_rix(ocelot, ANA_PGID_PGID, i);
..	..	@@ -1427,20 +1273,18 @@
1427	1273	}
1428	1274	}
1429	1275
1430		-static int ocelot_port_lag_join(struct ocelot_port *ocelot_port,
1431		- struct net_device *bond)
	1276	+int ocelot_port_lag_join(struct ocelot *ocelot, int port,
	1277	+ struct net_device *bond)
1432	1278	{
1433		- struct ocelot *ocelot = ocelot_port->ocelot;
1434		- int p = ocelot_port->chip_port;
1435		- int lag, lp;
1436	1279	struct net_device *ndev;
1437	1280	u32 bond_mask = 0;
	1281	+ int lag, lp;
1438	1282
1439	1283	rcu_read_lock();
1440	1284	for_each_netdev_in_bond_rcu(bond, ndev) {
1441		- struct ocelot_port *port = netdev_priv(ndev);
	1285	+ struct ocelot_port_private *priv = netdev_priv(ndev);
1442	1286
1443		- bond_mask \|= BIT(port->chip_port);
	1287	+ bond_mask \|= BIT(priv->chip_port);
1444	1288	}
1445	1289	rcu_read_unlock();
1446	1290
..	..	@@ -1449,17 +1293,17 @@
1449	1293	/* If the new port is the lowest one, use it as the logical port from
1450	1294	* now on
1451	1295	*/
1452		- if (p == lp) {
1453		- lag = p;
1454		- ocelot->lags[p] = bond_mask;
1455		- bond_mask &= ~BIT(p);
	1296	+ if (port == lp) {
	1297	+ lag = port;
	1298	+ ocelot->lags[port] = bond_mask;
	1299	+ bond_mask &= ~BIT(port);
1456	1300	if (bond_mask) {
1457	1301	lp = __ffs(bond_mask);
1458	1302	ocelot->lags[lp] = 0;
1459	1303	}
1460	1304	} else {
1461	1305	lag = lp;
1462		- ocelot->lags[lp] \|= BIT(p);
	1306	+ ocelot->lags[lp] \|= BIT(port);
1463	1307	}
1464	1308
1465	1309	ocelot_setup_lag(ocelot, lag);
..	..	@@ -1467,181 +1311,198 @@
1467	1311
1468	1312	return 0;
1469	1313	}
	1314	+EXPORT_SYMBOL(ocelot_port_lag_join);
1470	1315
1471		-static void ocelot_port_lag_leave(struct ocelot_port *ocelot_port,
1472		- struct net_device *bond)
	1316	+void ocelot_port_lag_leave(struct ocelot *ocelot, int port,
	1317	+ struct net_device *bond)
1473	1318	{
1474		- struct ocelot *ocelot = ocelot_port->ocelot;
1475		- int p = ocelot_port->chip_port;
1476	1319	u32 port_cfg;
1477	1320	int i;
1478	1321
1479	1322	/* Remove port from any lag */
1480	1323	for (i = 0; i < ocelot->num_phys_ports; i++)
1481		- ocelot->lags[i] &= ~BIT(ocelot_port->chip_port);
	1324	+ ocelot->lags[i] &= ~BIT(port);
1482	1325
1483	1326	/* if it was the logical port of the lag, move the lag config to the
1484	1327	* next port
1485	1328	*/
1486		- if (ocelot->lags[p]) {
1487		- int n = __ffs(ocelot->lags[p]);
	1329	+ if (ocelot->lags[port]) {
	1330	+ int n = __ffs(ocelot->lags[port]);
1488	1331
1489		- ocelot->lags[n] = ocelot->lags[p];
1490		- ocelot->lags[p] = 0;
	1332	+ ocelot->lags[n] = ocelot->lags[port];
	1333	+ ocelot->lags[port] = 0;
1491	1334
1492	1335	ocelot_setup_lag(ocelot, n);
1493	1336	}
1494	1337
1495		- port_cfg = ocelot_read_gix(ocelot, ANA_PORT_PORT_CFG, p);
	1338	+ port_cfg = ocelot_read_gix(ocelot, ANA_PORT_PORT_CFG, port);
1496	1339	port_cfg &= ~ANA_PORT_PORT_CFG_PORTID_VAL_M;
1497		- ocelot_write_gix(ocelot, port_cfg \| ANA_PORT_PORT_CFG_PORTID_VAL(p),
1498		- ANA_PORT_PORT_CFG, p);
	1340	+ ocelot_write_gix(ocelot, port_cfg \| ANA_PORT_PORT_CFG_PORTID_VAL(port),
	1341	+ ANA_PORT_PORT_CFG, port);
1499	1342
1500	1343	ocelot_set_aggr_pgids(ocelot);
1501	1344	}
	1345	+EXPORT_SYMBOL(ocelot_port_lag_leave);
1502	1346
1503		-/* Checks if the net_device instance given to us originate from our driver. */
1504		-static bool ocelot_netdevice_dev_check(const struct net_device *dev)
	1347	+/* Configure the maximum SDU (L2 payload) on RX to the value specified in @sdu.
	1348	+ * The length of VLAN tags is accounted for automatically via DEV_MAC_TAGS_CFG.
	1349	+ * In the special case that it's the NPI port that we're configuring, the
	1350	+ * length of the tag and optional prefix needs to be accounted for privately,
	1351	+ * in order to be able to sustain communication at the requested @sdu.
	1352	+ */
	1353	+void ocelot_port_set_maxlen(struct ocelot *ocelot, int port, size_t sdu)
1505	1354	{
1506		- return dev->netdev_ops == &ocelot_port_netdev_ops;
	1355	+ struct ocelot_port *ocelot_port = ocelot->ports[port];
	1356	+ int maxlen = sdu + ETH_HLEN + ETH_FCS_LEN;
	1357	+ int pause_start, pause_stop;
	1358	+ int atop, atop_tot;
	1359	+
	1360	+ if (port == ocelot->npi) {
	1361	+ maxlen += OCELOT_TAG_LEN;
	1362	+
	1363	+ if (ocelot->inj_prefix == OCELOT_TAG_PREFIX_SHORT)
	1364	+ maxlen += OCELOT_SHORT_PREFIX_LEN;
	1365	+ else if (ocelot->inj_prefix == OCELOT_TAG_PREFIX_LONG)
	1366	+ maxlen += OCELOT_LONG_PREFIX_LEN;
	1367	+ }
	1368	+
	1369	+ ocelot_port_writel(ocelot_port, maxlen, DEV_MAC_MAXLEN_CFG);
	1370	+
	1371	+ /* Set Pause watermark hysteresis */
	1372	+ pause_start = 6 * maxlen / OCELOT_BUFFER_CELL_SZ;
	1373	+ pause_stop = 4 * maxlen / OCELOT_BUFFER_CELL_SZ;
	1374	+ ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_START,
	1375	+ pause_start);
	1376	+ ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_STOP,
	1377	+ pause_stop);
	1378	+
	1379	+ /* Tail dropping watermarks */
	1380	+ atop_tot = (ocelot->shared_queue_sz - 9 * maxlen) /
	1381	+ OCELOT_BUFFER_CELL_SZ;
	1382	+ atop = (9 * maxlen) / OCELOT_BUFFER_CELL_SZ;
	1383	+ ocelot_write_rix(ocelot, ocelot->ops->wm_enc(atop), SYS_ATOP, port);
	1384	+ ocelot_write(ocelot, ocelot->ops->wm_enc(atop_tot), SYS_ATOP_TOT_CFG);
1507	1385	}
	1386	+EXPORT_SYMBOL(ocelot_port_set_maxlen);
1508	1387
1509		-static int ocelot_netdevice_port_event(struct net_device *dev,
1510		- unsigned long event,
1511		- struct netdev_notifier_changeupper_info *info)
	1388	+int ocelot_get_max_mtu(struct ocelot *ocelot, int port)
1512	1389	{
1513		- struct ocelot_port *ocelot_port = netdev_priv(dev);
1514		- int err = 0;
	1390	+ int max_mtu = 65535 - ETH_HLEN - ETH_FCS_LEN;
1515	1391
1516		- switch (event) {
1517		- case NETDEV_CHANGEUPPER:
1518		- if (netif_is_bridge_master(info->upper_dev)) {
1519		- if (info->linking)
1520		- err = ocelot_port_bridge_join(ocelot_port,
1521		- info->upper_dev);
1522		- else
1523		- ocelot_port_bridge_leave(ocelot_port,
1524		- info->upper_dev);
	1392	+ if (port == ocelot->npi) {
	1393	+ max_mtu -= OCELOT_TAG_LEN;
1525	1394
1526		- ocelot_vlan_port_apply(ocelot_port->ocelot,
1527		- ocelot_port);
1528		- }
1529		- if (netif_is_lag_master(info->upper_dev)) {
1530		- if (info->linking)
1531		- err = ocelot_port_lag_join(ocelot_port,
1532		- info->upper_dev);
1533		- else
1534		- ocelot_port_lag_leave(ocelot_port,
1535		- info->upper_dev);
1536		- }
1537		- break;
1538		- default:
1539		- break;
	1395	+ if (ocelot->inj_prefix == OCELOT_TAG_PREFIX_SHORT)
	1396	+ max_mtu -= OCELOT_SHORT_PREFIX_LEN;
	1397	+ else if (ocelot->inj_prefix == OCELOT_TAG_PREFIX_LONG)
	1398	+ max_mtu -= OCELOT_LONG_PREFIX_LEN;
1540	1399	}
1541	1400
1542		- return err;
	1401	+ return max_mtu;
1543	1402	}
	1403	+EXPORT_SYMBOL(ocelot_get_max_mtu);
1544	1404
1545		-static int ocelot_netdevice_event(struct notifier_block *unused,
1546		- unsigned long event, void *ptr)
	1405	+void ocelot_init_port(struct ocelot *ocelot, int port)
1547	1406	{
1548		- struct netdev_notifier_changeupper_info *info = ptr;
1549		- struct net_device *dev = netdev_notifier_info_to_dev(ptr);
1550		- int ret = 0;
	1407	+ struct ocelot_port *ocelot_port = ocelot->ports[port];
1551	1408
1552		- if (event == NETDEV_PRECHANGEUPPER &&
1553		- ocelot_netdevice_dev_check(dev) &&
1554		- netif_is_lag_master(info->upper_dev)) {
1555		- struct netdev_lag_upper_info *lag_upper_info = info->upper_info;
1556		- struct netlink_ext_ack *extack;
1557		-
1558		- if (lag_upper_info &&
1559		- lag_upper_info->tx_type != NETDEV_LAG_TX_TYPE_HASH) {
1560		- extack = netdev_notifier_info_to_extack(&info->info);
1561		- NL_SET_ERR_MSG_MOD(extack, "LAG device using unsupported Tx type");
1562		-
1563		- ret = -EINVAL;
1564		- goto notify;
1565		- }
1566		- }
1567		-
1568		- if (netif_is_lag_master(dev)) {
1569		- struct net_device *slave;
1570		- struct list_head *iter;
1571		-
1572		- netdev_for_each_lower_dev(dev, slave, iter) {
1573		- ret = ocelot_netdevice_port_event(slave, event, info);
1574		- if (ret)
1575		- goto notify;
1576		- }
1577		- } else {
1578		- ret = ocelot_netdevice_port_event(dev, event, info);
1579		- }
1580		-
1581		-notify:
1582		- return notifier_from_errno(ret);
1583		-}
1584		-
1585		-struct notifier_block ocelot_netdevice_nb __read_mostly = {
1586		- .notifier_call = ocelot_netdevice_event,
1587		-};
1588		-EXPORT_SYMBOL(ocelot_netdevice_nb);
1589		-
1590		-int ocelot_probe_port(struct ocelot *ocelot, u8 port,
1591		- void __iomem *regs,
1592		- struct phy_device *phy)
1593		-{
1594		- struct ocelot_port *ocelot_port;
1595		- struct net_device *dev;
1596		- int err;
1597		-
1598		- dev = alloc_etherdev(sizeof(struct ocelot_port));
1599		- if (!dev)
1600		- return -ENOMEM;
1601		- SET_NETDEV_DEV(dev, ocelot->dev);
1602		- ocelot_port = netdev_priv(dev);
1603		- ocelot_port->dev = dev;
1604		- ocelot_port->ocelot = ocelot;
1605		- ocelot_port->regs = regs;
1606		- ocelot_port->chip_port = port;
1607		- ocelot_port->phy = phy;
1608		- INIT_LIST_HEAD(&ocelot_port->mc);
1609		- ocelot->ports[port] = ocelot_port;
1610		-
1611		- dev->netdev_ops = &ocelot_port_netdev_ops;
1612		- dev->ethtool_ops = &ocelot_ethtool_ops;
1613		- dev->switchdev_ops = &ocelot_port_switchdev_ops;
1614		-
1615		- dev->hw_features \|= NETIF_F_HW_VLAN_CTAG_FILTER;
1616		- dev->features \|= NETIF_F_HW_VLAN_CTAG_FILTER;
1617		-
1618		- memcpy(dev->dev_addr, ocelot->base_mac, ETH_ALEN);
1619		- dev->dev_addr[ETH_ALEN - 1] += port;
1620		- ocelot_mact_learn(ocelot, PGID_CPU, dev->dev_addr, ocelot_port->pvid,
1621		- ENTRYTYPE_LOCKED);
1622		-
1623		- err = register_netdev(dev);
1624		- if (err) {
1625		- dev_err(ocelot->dev, "register_netdev failed\n");
1626		- goto err_register_netdev;
1627		- }
	1409	+ skb_queue_head_init(&ocelot_port->tx_skbs);
	1410	+ spin_lock_init(&ocelot_port->ts_id_lock);
1628	1411
1629	1412	/* Basic L2 initialization */
1630		- ocelot_vlan_port_apply(ocelot, ocelot_port);
1631	1413
1632		- return 0;
	1414	+ /* Set MAC IFG Gaps
	1415	+ * FDX: TX_IFG = 5, RX_IFG1 = RX_IFG2 = 0
	1416	+ * !FDX: TX_IFG = 5, RX_IFG1 = RX_IFG2 = 5
	1417	+ */
	1418	+ ocelot_port_writel(ocelot_port, DEV_MAC_IFG_CFG_TX_IFG(5),
	1419	+ DEV_MAC_IFG_CFG);
1633	1420
1634		-err_register_netdev:
1635		- free_netdev(dev);
1636		- return err;
	1421	+ /* Load seed (0) and set MAC HDX late collision */
	1422	+ ocelot_port_writel(ocelot_port, DEV_MAC_HDX_CFG_LATE_COL_POS(67) \|
	1423	+ DEV_MAC_HDX_CFG_SEED_LOAD,
	1424	+ DEV_MAC_HDX_CFG);
	1425	+ mdelay(1);
	1426	+ ocelot_port_writel(ocelot_port, DEV_MAC_HDX_CFG_LATE_COL_POS(67),
	1427	+ DEV_MAC_HDX_CFG);
	1428	+
	1429	+ /* Set Max Length and maximum tags allowed */
	1430	+ ocelot_port_set_maxlen(ocelot, port, ETH_DATA_LEN);
	1431	+ ocelot_port_writel(ocelot_port, DEV_MAC_TAGS_CFG_TAG_ID(ETH_P_8021AD) \|
	1432	+ DEV_MAC_TAGS_CFG_VLAN_AWR_ENA \|
	1433	+ DEV_MAC_TAGS_CFG_VLAN_DBL_AWR_ENA \|
	1434	+ DEV_MAC_TAGS_CFG_VLAN_LEN_AWR_ENA,
	1435	+ DEV_MAC_TAGS_CFG);
	1436	+
	1437	+ /* Set SMAC of Pause frame (00:00:00:00:00:00) */
	1438	+ ocelot_port_writel(ocelot_port, 0, DEV_MAC_FC_MAC_HIGH_CFG);
	1439	+ ocelot_port_writel(ocelot_port, 0, DEV_MAC_FC_MAC_LOW_CFG);
	1440	+
	1441	+ /* Enable transmission of pause frames */
	1442	+ ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_ENA, 1);
	1443	+
	1444	+ /* Drop frames with multicast source address */
	1445	+ ocelot_rmw_gix(ocelot, ANA_PORT_DROP_CFG_DROP_MC_SMAC_ENA,
	1446	+ ANA_PORT_DROP_CFG_DROP_MC_SMAC_ENA,
	1447	+ ANA_PORT_DROP_CFG, port);
	1448	+
	1449	+ /* Set default VLAN and tag type to 8021Q. */
	1450	+ ocelot_rmw_gix(ocelot, REW_PORT_VLAN_CFG_PORT_TPID(ETH_P_8021Q),
	1451	+ REW_PORT_VLAN_CFG_PORT_TPID_M,
	1452	+ REW_PORT_VLAN_CFG, port);
	1453	+
	1454	+ /* Enable vcap lookups */
	1455	+ ocelot_vcap_enable(ocelot, port);
1637	1456	}
1638		-EXPORT_SYMBOL(ocelot_probe_port);
	1457	+EXPORT_SYMBOL(ocelot_init_port);
	1458	+
	1459	+/* Configure and enable the CPU port module, which is a set of queues
	1460	+ * accessible through register MMIO, frame DMA or Ethernet (in case
	1461	+ * NPI mode is used).
	1462	+ */
	1463	+static void ocelot_cpu_port_init(struct ocelot *ocelot)
	1464	+{
	1465	+ int cpu = ocelot->num_phys_ports;
	1466	+
	1467	+ /* The unicast destination PGID for the CPU port module is unused */
	1468	+ ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, cpu);
	1469	+ /* Instead set up a multicast destination PGID for traffic copied to
	1470	+ * the CPU. Whitelisted MAC addresses like the port netdevice MAC
	1471	+ * addresses will be copied to the CPU via this PGID.
	1472	+ */
	1473	+ ocelot_write_rix(ocelot, BIT(cpu), ANA_PGID_PGID, PGID_CPU);
	1474	+ ocelot_write_gix(ocelot, ANA_PORT_PORT_CFG_RECV_ENA \|
	1475	+ ANA_PORT_PORT_CFG_PORTID_VAL(cpu),
	1476	+ ANA_PORT_PORT_CFG, cpu);
	1477	+
	1478	+ /* Enable CPU port module */
	1479	+ ocelot_fields_write(ocelot, cpu, QSYS_SWITCH_PORT_MODE_PORT_ENA, 1);
	1480	+ /* CPU port Injection/Extraction configuration */
	1481	+ ocelot_fields_write(ocelot, cpu, SYS_PORT_MODE_INCL_XTR_HDR,
	1482	+ ocelot->xtr_prefix);
	1483	+ ocelot_fields_write(ocelot, cpu, SYS_PORT_MODE_INCL_INJ_HDR,
	1484	+ ocelot->inj_prefix);
	1485	+
	1486	+ /* Configure the CPU port to be VLAN aware */
	1487	+ ocelot_write_gix(ocelot, ANA_PORT_VLAN_CFG_VLAN_VID(0) \|
	1488	+ ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA \|
	1489	+ ANA_PORT_VLAN_CFG_VLAN_POP_CNT(1),
	1490	+ ANA_PORT_VLAN_CFG, cpu);
	1491	+}
1639	1492
1640	1493	int ocelot_init(struct ocelot *ocelot)
1641	1494	{
1642		- u32 port;
1643		- int i, cpu = ocelot->num_phys_ports;
1644	1495	char queue_name[32];
	1496	+ int i, ret;
	1497	+ u32 port;
	1498	+
	1499	+ if (ocelot->ops->reset) {
	1500	+ ret = ocelot->ops->reset(ocelot);
	1501	+ if (ret) {
	1502	+ dev_err(ocelot->dev, "Switch reset failed\n");
	1503	+ return ret;
	1504	+ }
	1505	+ }
1645	1506
1646	1507	ocelot->lags = devm_kcalloc(ocelot->dev, ocelot->num_phys_ports,
1647	1508	sizeof(u32), GFP_KERNEL);
..	..	@@ -1655,14 +1516,19 @@
1655	1516	return -ENOMEM;
1656	1517
1657	1518	mutex_init(&ocelot->stats_lock);
	1519	+ mutex_init(&ocelot->ptp_lock);
	1520	+ spin_lock_init(&ocelot->ptp_clock_lock);
1658	1521	snprintf(queue_name, sizeof(queue_name), "%s-stats",
1659	1522	dev_name(ocelot->dev));
1660	1523	ocelot->stats_queue = create_singlethread_workqueue(queue_name);
1661	1524	if (!ocelot->stats_queue)
1662	1525	return -ENOMEM;
1663	1526
	1527	+ INIT_LIST_HEAD(&ocelot->multicast);
1664	1528	ocelot_mact_init(ocelot);
1665	1529	ocelot_vlan_init(ocelot);
	1530	+ ocelot_vcap_init(ocelot);
	1531	+ ocelot_cpu_port_init(ocelot);
1666	1532
1667	1533	for (port = 0; port < ocelot->num_phys_ports; port++) {
1668	1534	/* Clear all counters (5 groups) */
..	..	@@ -1695,10 +1561,11 @@
1695	1561	SYS_FRM_AGING_MAX_AGE(307692), SYS_FRM_AGING);
1696	1562
1697	1563	/* Setup flooding PGIDs */
1698		- ocelot_write_rix(ocelot, ANA_FLOODING_FLD_MULTICAST(PGID_MC) \|
1699		- ANA_FLOODING_FLD_BROADCAST(PGID_MC) \|
1700		- ANA_FLOODING_FLD_UNICAST(PGID_UC),
1701		- ANA_FLOODING, 0);
	1564	+ for (i = 0; i < ocelot->num_flooding_pgids; i++)
	1565	+ ocelot_write_rix(ocelot, ANA_FLOODING_FLD_MULTICAST(PGID_MC) \|
	1566	+ ANA_FLOODING_FLD_BROADCAST(PGID_MC) \|
	1567	+ ANA_FLOODING_FLD_UNICAST(PGID_UC),
	1568	+ ANA_FLOODING, i);
1702	1569	ocelot_write(ocelot, ANA_FLOODING_IPMC_FLD_MC6_DATA(PGID_MCIPV6) \|
1703	1570	ANA_FLOODING_IPMC_FLD_MC6_CTRL(PGID_MC) \|
1704	1571	ANA_FLOODING_IPMC_FLD_MC4_DATA(PGID_MCIPV4) \|
..	..	@@ -1717,15 +1584,8 @@
1717	1584	ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_SRC + port);
1718	1585	}
1719	1586
1720		- /* Configure and enable the CPU port. */
1721		- ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, cpu);
1722		- ocelot_write_rix(ocelot, BIT(cpu), ANA_PGID_PGID, PGID_CPU);
1723		- ocelot_write_gix(ocelot, ANA_PORT_PORT_CFG_RECV_ENA \|
1724		- ANA_PORT_PORT_CFG_PORTID_VAL(cpu),
1725		- ANA_PORT_PORT_CFG, cpu);
1726		-
1727	1587	/* Allow broadcast MAC frames. */
1728		- for (i = ocelot->num_phys_ports + 1; i < PGID_CPU; i++) {
	1588	+ for_each_nonreserved_multicast_dest_pgid(ocelot, i) {
1729	1589	u32 val = ANA_PGID_PGID_PGID(GENMASK(ocelot->num_phys_ports - 1, 0));
1730	1590
1731	1591	ocelot_write_rix(ocelot, val, ANA_PGID_PGID, i);
..	..	@@ -1733,16 +1593,13 @@
1733	1593	ocelot_write_rix(ocelot,
1734	1594	ANA_PGID_PGID_PGID(GENMASK(ocelot->num_phys_ports, 0)),
1735	1595	ANA_PGID_PGID, PGID_MC);
1736		- ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_MCIPV4);
1737		- ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_MCIPV6);
	1596	+ ocelot_write_rix(ocelot,
	1597	+ ANA_PGID_PGID_PGID(GENMASK(ocelot->num_phys_ports, 0)),
	1598	+ ANA_PGID_PGID, PGID_MCIPV4);
	1599	+ ocelot_write_rix(ocelot,
	1600	+ ANA_PGID_PGID_PGID(GENMASK(ocelot->num_phys_ports, 0)),
	1601	+ ANA_PGID_PGID, PGID_MCIPV6);
1738	1602
1739		- /* CPU port Injection/Extraction configuration */
1740		- ocelot_write_rix(ocelot, QSYS_SWITCH_PORT_MODE_INGRESS_DROP_MODE \|
1741		- QSYS_SWITCH_PORT_MODE_SCH_NEXT_CFG(1) \|
1742		- QSYS_SWITCH_PORT_MODE_PORT_ENA,
1743		- QSYS_SWITCH_PORT_MODE, cpu);
1744		- ocelot_write_rix(ocelot, SYS_PORT_MODE_INCL_XTR_HDR(1) \|
1745		- SYS_PORT_MODE_INCL_INJ_HDR(1), SYS_PORT_MODE, cpu);
1746	1603	/* Allow manual injection via DEVCPU_QS registers, and byte swap these
1747	1604	* registers endianness.
1748	1605	*/
..	..	@@ -1764,9 +1621,10 @@
1764	1621	ANA_CPUQ_8021_CFG_CPUQ_BPDU_VAL(6),
1765	1622	ANA_CPUQ_8021_CFG, i);
1766	1623
1767		- INIT_DELAYED_WORK(&ocelot->stats_work, ocelot_check_stats);
	1624	+ INIT_DELAYED_WORK(&ocelot->stats_work, ocelot_check_stats_work);
1768	1625	queue_delayed_work(ocelot->stats_queue, &ocelot->stats_work,
1769	1626	OCELOT_STATS_CHECK_DELAY);
	1627	+
1770	1628	return 0;
1771	1629	}
1772	1630	EXPORT_SYMBOL(ocelot_init);
..	..	@@ -1779,4 +1637,12 @@
1779	1637	}
1780	1638	EXPORT_SYMBOL(ocelot_deinit);
1781	1639
	1640	+void ocelot_deinit_port(struct ocelot *ocelot, int port)
	1641	+{
	1642	+ struct ocelot_port *ocelot_port = ocelot->ports[port];
	1643	+
	1644	+ skb_queue_purge(&ocelot_port->tx_skbs);
	1645	+}
	1646	+EXPORT_SYMBOL(ocelot_deinit_port);
	1647	+
1782	1648	MODULE_LICENSE("Dual MIT/GPL");