~hc/RK356X_SDK_RELEASE.git

..	..	@@ -1,12 +1,8 @@
	1	+// SPDX-License-Identifier: GPL-2.0-or-later
1	2	/*
2	3	* Broadcom Starfighter 2 DSA switch CFP support
3	4	*
4	5	* Copyright (C) 2016, Broadcom
5		- *
6		- * This program is free software; you can redistribute it and/or modify
7		- * it under the terms of the GNU General Public License as published by
8		- * the Free Software Foundation; either version 2 of the License, or
9		- * (at your option) any later version.
10	6	*/
11	7
12	8	#include <linux/list.h>
..	..	@@ -16,9 +12,18 @@
16	12	#include <linux/netdevice.h>
17	13	#include <net/dsa.h>
18	14	#include <linux/bitmap.h>
	15	+#include <net/flow_offload.h>
	16	+#include <net/switchdev.h>
	17	+#include <uapi/linux/if_bridge.h>
19	18
20	19	#include "bcm_sf2.h"
21	20	#include "bcm_sf2_regs.h"
	21	+
	22	+struct cfp_rule {
	23	+ int port;
	24	+ struct ethtool_rx_flow_spec fs;
	25	+ struct list_head next;
	26	+};
22	27
23	28	struct cfp_udf_slice_layout {
24	29	u8 slices[UDFS_PER_SLICE];
..	..	@@ -123,12 +128,12 @@
123	128	return count;
124	129	}
125	130
126		-static inline u32 udf_upper_bits(unsigned int num_udf)
	131	+static inline u32 udf_upper_bits(int num_udf)
127	132	{
128	133	return GENMASK(num_udf - 1, 0) >> (UDFS_PER_SLICE - 1);
129	134	}
130	135
131		-static inline u32 udf_lower_bits(unsigned int num_udf)
	136	+static inline u32 udf_lower_bits(int num_udf)
132	137	{
133	138	return (u8)GENMASK(num_udf - 1, 0);
134	139	}
..	..	@@ -206,6 +211,7 @@
206	211
207	212	static int bcm_sf2_cfp_act_pol_set(struct bcm_sf2_priv *priv,
208	213	unsigned int rule_index,
	214	+ int src_port,
209	215	unsigned int port_num,
210	216	unsigned int queue_num,
211	217	bool fwd_map_change)
..	..	@@ -222,6 +228,10 @@
222	228	CHANGE_TC \| queue_num << NEW_TC_SHIFT;
223	229	else
224	230	reg = 0;
	231	+
	232	+ /* Enable looping back to the original port */
	233	+ if (src_port == port_num)
	234	+ reg \|= LOOP_BK_EN;
225	235
226	236	core_writel(priv, reg, CORE_ACT_POL_DATA0);
227	237
..	..	@@ -251,17 +261,29 @@
251	261	}
252	262
253	263	static void bcm_sf2_cfp_slice_ipv4(struct bcm_sf2_priv *priv,
254		- struct ethtool_tcpip4_spec *v4_spec,
255		- unsigned int slice_num,
	264	+ struct flow_dissector_key_ipv4_addrs *addrs,
	265	+ struct flow_dissector_key_ports *ports,
	266	+ const __be16 vlan_tci,
	267	+ unsigned int slice_num, u8 num_udf,
256	268	bool mask)
257	269	{
258	270	u32 reg, offset;
	271	+
	272	+ /* UDF_Valid[7:0] [31:24]
	273	+ * S-Tag [23:8]
	274	+ * C-Tag [7:0]
	275	+ */
	276	+ reg = udf_lower_bits(num_udf) << 24 \| be16_to_cpu(vlan_tci) >> 8;
	277	+ if (mask)
	278	+ core_writel(priv, reg, CORE_CFP_MASK_PORT(5));
	279	+ else
	280	+ core_writel(priv, reg, CORE_CFP_DATA_PORT(5));
259	281
260	282	/* C-Tag [31:24]
261	283	* UDF_n_A8 [23:8]
262	284	* UDF_n_A7 [7:0]
263	285	*/
264		- reg = 0;
	286	+ reg = (u32)(be16_to_cpu(vlan_tci) & 0xff) << 24;
265	287	if (mask)
266	288	offset = CORE_CFP_MASK_PORT(4);
267	289	else
..	..	@@ -272,7 +294,7 @@
272	294	* UDF_n_A6 [23:8]
273	295	* UDF_n_A5 [7:0]
274	296	*/
275		- reg = be16_to_cpu(v4_spec->pdst) >> 8;
	297	+ reg = be16_to_cpu(ports->dst) >> 8;
276	298	if (mask)
277	299	offset = CORE_CFP_MASK_PORT(3);
278	300	else
..	..	@@ -283,9 +305,9 @@
283	305	* UDF_n_A4 [23:8]
284	306	* UDF_n_A3 [7:0]
285	307	*/
286		- reg = (be16_to_cpu(v4_spec->pdst) & 0xff) << 24 \|
287		- (u32)be16_to_cpu(v4_spec->psrc) << 8 \|
288		- (be32_to_cpu(v4_spec->ip4dst) & 0x0000ff00) >> 8;
	308	+ reg = (be16_to_cpu(ports->dst) & 0xff) << 24 \|
	309	+ (u32)be16_to_cpu(ports->src) << 8 \|
	310	+ (be32_to_cpu(addrs->dst) & 0x0000ff00) >> 8;
289	311	if (mask)
290	312	offset = CORE_CFP_MASK_PORT(2);
291	313	else
..	..	@@ -296,9 +318,9 @@
296	318	* UDF_n_A2 [23:8]
297	319	* UDF_n_A1 [7:0]
298	320	*/
299		- reg = (u32)(be32_to_cpu(v4_spec->ip4dst) & 0xff) << 24 \|
300		- (u32)(be32_to_cpu(v4_spec->ip4dst) >> 16) << 8 \|
301		- (be32_to_cpu(v4_spec->ip4src) & 0x0000ff00) >> 8;
	321	+ reg = (u32)(be32_to_cpu(addrs->dst) & 0xff) << 24 \|
	322	+ (u32)(be32_to_cpu(addrs->dst) >> 16) << 8 \|
	323	+ (be32_to_cpu(addrs->src) & 0x0000ff00) >> 8;
302	324	if (mask)
303	325	offset = CORE_CFP_MASK_PORT(1);
304	326	else
..	..	@@ -311,8 +333,8 @@
311	333	* Slice ID [3:2]
312	334	* Slice valid [1:0]
313	335	*/
314		- reg = (u32)(be32_to_cpu(v4_spec->ip4src) & 0xff) << 24 \|
315		- (u32)(be32_to_cpu(v4_spec->ip4src) >> 16) << 8 \|
	336	+ reg = (u32)(be32_to_cpu(addrs->src) & 0xff) << 24 \|
	337	+ (u32)(be32_to_cpu(addrs->src) >> 16) << 8 \|
316	338	SLICE_NUM(slice_num) \| SLICE_VALID;
317	339	if (mask)
318	340	offset = CORE_CFP_MASK_PORT(0);
..	..	@@ -326,9 +348,14 @@
326	348	unsigned int queue_num,
327	349	struct ethtool_rx_flow_spec *fs)
328	350	{
329		- struct ethtool_tcpip4_spec v4_spec, v4_m_spec;
	351	+ __be16 vlan_tci = 0, vlan_m_tci = htons(0xffff);
	352	+ struct ethtool_rx_flow_spec_input input = {};
330	353	const struct cfp_udf_layout *layout;
331	354	unsigned int slice_num, rule_index;
	355	+ struct ethtool_rx_flow_rule *flow;
	356	+ struct flow_match_ipv4_addrs ipv4;
	357	+ struct flow_match_ports ports;
	358	+ struct flow_match_ip ip;
332	359	u8 ip_proto, ip_frag;
333	360	u8 num_udf;
334	361	u32 reg;
..	..	@@ -337,19 +364,21 @@
337	364	switch (fs->flow_type & ~FLOW_EXT) {
338	365	case TCP_V4_FLOW:
339	366	ip_proto = IPPROTO_TCP;
340		- v4_spec = &fs->h_u.tcp_ip4_spec;
341		- v4_m_spec = &fs->m_u.tcp_ip4_spec;
342	367	break;
343	368	case UDP_V4_FLOW:
344	369	ip_proto = IPPROTO_UDP;
345		- v4_spec = &fs->h_u.udp_ip4_spec;
346		- v4_m_spec = &fs->m_u.udp_ip4_spec;
347	370	break;
348	371	default:
349	372	return -EINVAL;
350	373	}
351	374
352		- ip_frag = be32_to_cpu(fs->m_ext.data[0]);
	375	+ ip_frag = !!(be32_to_cpu(fs->h_ext.data[0]) & 1);
	376	+
	377	+ /* Extract VLAN TCI */
	378	+ if (fs->flow_type & FLOW_EXT) {
	379	+ vlan_tci = fs->h_ext.vlan_tci;
	380	+ vlan_m_tci = fs->m_ext.vlan_tci;
	381	+ }
353	382
354	383	/* Locate the first rule available */
355	384	if (fs->location == RX_CLS_LOC_ANY)
..	..	@@ -361,11 +390,22 @@
361	390	if (rule_index > bcm_sf2_cfp_rule_size(priv))
362	391	return -ENOSPC;
363	392
	393	+ input.fs = fs;
	394	+ flow = ethtool_rx_flow_rule_create(&input);
	395	+ if (IS_ERR(flow))
	396	+ return PTR_ERR(flow);
	397	+
	398	+ flow_rule_match_ipv4_addrs(flow->rule, &ipv4);
	399	+ flow_rule_match_ports(flow->rule, &ports);
	400	+ flow_rule_match_ip(flow->rule, &ip);
	401	+
364	402	layout = &udf_tcpip4_layout;
365	403	/* We only use one UDF slice for now */
366	404	slice_num = bcm_sf2_get_slice_number(layout, 0);
367		- if (slice_num == UDF_NUM_SLICES)
368		- return -EINVAL;
	405	+ if (slice_num == UDF_NUM_SLICES) {
	406	+ ret = -EINVAL;
	407	+ goto out_err_flow_rule;
	408	+ }
369	409
370	410	num_udf = bcm_sf2_get_num_udf_slices(layout->udfs[slice_num].slices);
371	411
..	..	@@ -392,7 +432,7 @@
392	432	* Reserved [1]
393	433	* UDF_Valid[8] [0]
394	434	*/
395		- core_writel(priv, v4_spec->tos << IPTOS_SHIFT \|
	435	+ core_writel(priv, ip.key->tos << IPTOS_SHIFT \|
396	436	ip_proto << IPPROTO_SHIFT \| ip_frag << IP_FRAG_SHIFT \|
397	437	udf_upper_bits(num_udf),
398	438	CORE_CFP_DATA_PORT(6));
..	..	@@ -401,18 +441,11 @@
401	441	core_writel(priv, layout->udfs[slice_num].mask_value \|
402	442	udf_upper_bits(num_udf), CORE_CFP_MASK_PORT(6));
403	443
404		- /* UDF_Valid[7:0] [31:24]
405		- * S-Tag [23:8]
406		- * C-Tag [7:0]
407		- */
408		- core_writel(priv, udf_lower_bits(num_udf) << 24, CORE_CFP_DATA_PORT(5));
409		-
410		- /* Mask all but valid UDFs */
411		- core_writel(priv, udf_lower_bits(num_udf) << 24, CORE_CFP_MASK_PORT(5));
412		-
413	444	/* Program the match and the mask */
414		- bcm_sf2_cfp_slice_ipv4(priv, v4_spec, slice_num, false);
415		- bcm_sf2_cfp_slice_ipv4(priv, v4_m_spec, SLICE_NUM_MASK, true);
	445	+ bcm_sf2_cfp_slice_ipv4(priv, ipv4.key, ports.key, vlan_tci,
	446	+ slice_num, num_udf, false);
	447	+ bcm_sf2_cfp_slice_ipv4(priv, ipv4.mask, ports.mask, vlan_m_tci,
	448	+ SLICE_NUM_MASK, num_udf, true);
416	449
417	450	/* Insert into TCAM now */
418	451	bcm_sf2_cfp_rule_addr_set(priv, rule_index);
..	..	@@ -420,14 +453,14 @@
420	453	ret = bcm_sf2_cfp_op(priv, OP_SEL_WRITE \| TCAM_SEL);
421	454	if (ret) {
422	455	pr_err("TCAM entry at addr %d failed\n", rule_index);
423		- return ret;
	456	+ goto out_err_flow_rule;
424	457	}
425	458
426	459	/* Insert into Action and policer RAMs now */
427		- ret = bcm_sf2_cfp_act_pol_set(priv, rule_index, port_num,
	460	+ ret = bcm_sf2_cfp_act_pol_set(priv, rule_index, port, port_num,
428	461	queue_num, true);
429	462	if (ret)
430		- return ret;
	463	+ goto out_err_flow_rule;
431	464
432	465	/* Turn on CFP for this rule now */
433	466	reg = core_readl(priv, CORE_CFP_CTL_REG);
..	..	@@ -440,14 +473,29 @@
440	473	fs->location = rule_index;
441	474
442	475	return 0;
	476	+
	477	+out_err_flow_rule:
	478	+ ethtool_rx_flow_rule_destroy(flow);
	479	+ return ret;
443	480	}
444	481
445	482	static void bcm_sf2_cfp_slice_ipv6(struct bcm_sf2_priv *priv,
446	483	const __be32 *ip6_addr, const __be16 port,
447		- unsigned int slice_num,
	484	+ const __be16 vlan_tci,
	485	+ unsigned int slice_num, u32 udf_bits,
448	486	bool mask)
449	487	{
450	488	u32 reg, tmp, val, offset;
	489	+
	490	+ /* UDF_Valid[7:0] [31:24]
	491	+ * S-Tag [23:8]
	492	+ * C-Tag [7:0]
	493	+ */
	494	+ reg = udf_bits << 24 \| be16_to_cpu(vlan_tci) >> 8;
	495	+ if (mask)
	496	+ core_writel(priv, reg, CORE_CFP_MASK_PORT(5));
	497	+ else
	498	+ core_writel(priv, reg, CORE_CFP_DATA_PORT(5));
451	499
452	500	/* C-Tag [31:24]
453	501	* UDF_n_B8 [23:8] (port)
..	..	@@ -455,6 +503,7 @@
455	503	*/
456	504	reg = be32_to_cpu(ip6_addr[3]);
457	505	val = (u32)be16_to_cpu(port) << 8 \| ((reg >> 8) & 0xff);
	506	+ val \|= (u32)(be16_to_cpu(vlan_tci) & 0xff) << 24;
458	507	if (mask)
459	508	offset = CORE_CFP_MASK_PORT(4);
460	509	else
..	..	@@ -515,14 +564,78 @@
515	564	core_writel(priv, reg, offset);
516	565	}
517	566
	567	+static struct cfp_rule bcm_sf2_cfp_rule_find(struct bcm_sf2_priv priv,
	568	+ int port, u32 location)
	569	+{
	570	+ struct cfp_rule *rule;
	571	+
	572	+ list_for_each_entry(rule, &priv->cfp.rules_list, next) {
	573	+ if (rule->port == port && rule->fs.location == location)
	574	+ return rule;
	575	+ }
	576	+
	577	+ return NULL;
	578	+}
	579	+
	580	+static int bcm_sf2_cfp_rule_cmp(struct bcm_sf2_priv *priv, int port,
	581	+ struct ethtool_rx_flow_spec *fs)
	582	+{
	583	+ struct cfp_rule *rule = NULL;
	584	+ size_t fs_size = 0;
	585	+ int ret = 1;
	586	+
	587	+ if (list_empty(&priv->cfp.rules_list))
	588	+ return ret;
	589	+
	590	+ list_for_each_entry(rule, &priv->cfp.rules_list, next) {
	591	+ ret = 1;
	592	+ if (rule->port != port)
	593	+ continue;
	594	+
	595	+ if (rule->fs.flow_type != fs->flow_type \|\|
	596	+ rule->fs.ring_cookie != fs->ring_cookie \|\|
	597	+ rule->fs.h_ext.data[0] != fs->h_ext.data[0])
	598	+ continue;
	599	+
	600	+ switch (fs->flow_type & ~FLOW_EXT) {
	601	+ case TCP_V6_FLOW:
	602	+ case UDP_V6_FLOW:
	603	+ fs_size = sizeof(struct ethtool_tcpip6_spec);
	604	+ break;
	605	+ case TCP_V4_FLOW:
	606	+ case UDP_V4_FLOW:
	607	+ fs_size = sizeof(struct ethtool_tcpip4_spec);
	608	+ break;
	609	+ default:
	610	+ continue;
	611	+ }
	612	+
	613	+ ret = memcmp(&rule->fs.h_u, &fs->h_u, fs_size);
	614	+ ret \|= memcmp(&rule->fs.m_u, &fs->m_u, fs_size);
	615	+ /* Compare VLAN TCI values as well */
	616	+ if (rule->fs.flow_type & FLOW_EXT) {
	617	+ ret \|= rule->fs.h_ext.vlan_tci != fs->h_ext.vlan_tci;
	618	+ ret \|= rule->fs.m_ext.vlan_tci != fs->m_ext.vlan_tci;
	619	+ }
	620	+ if (ret == 0)
	621	+ break;
	622	+ }
	623	+
	624	+ return ret;
	625	+}
	626	+
518	627	static int bcm_sf2_cfp_ipv6_rule_set(struct bcm_sf2_priv *priv, int port,
519	628	unsigned int port_num,
520	629	unsigned int queue_num,
521	630	struct ethtool_rx_flow_spec *fs)
522	631	{
523		- struct ethtool_tcpip6_spec v6_spec, v6_m_spec;
	632	+ __be16 vlan_tci = 0, vlan_m_tci = htons(0xffff);
	633	+ struct ethtool_rx_flow_spec_input input = {};
524	634	unsigned int slice_num, rule_index[2];
525	635	const struct cfp_udf_layout *layout;
	636	+ struct ethtool_rx_flow_rule *flow;
	637	+ struct flow_match_ipv6_addrs ipv6;
	638	+ struct flow_match_ports ports;
526	639	u8 ip_proto, ip_frag;
527	640	int ret = 0;
528	641	u8 num_udf;
..	..	@@ -531,19 +644,21 @@
531	644	switch (fs->flow_type & ~FLOW_EXT) {
532	645	case TCP_V6_FLOW:
533	646	ip_proto = IPPROTO_TCP;
534		- v6_spec = &fs->h_u.tcp_ip6_spec;
535		- v6_m_spec = &fs->m_u.tcp_ip6_spec;
536	647	break;
537	648	case UDP_V6_FLOW:
538	649	ip_proto = IPPROTO_UDP;
539		- v6_spec = &fs->h_u.udp_ip6_spec;
540		- v6_m_spec = &fs->m_u.udp_ip6_spec;
541	650	break;
542	651	default:
543	652	return -EINVAL;
544	653	}
545	654
546		- ip_frag = be32_to_cpu(fs->m_ext.data[0]);
	655	+ ip_frag = !!(be32_to_cpu(fs->h_ext.data[0]) & 1);
	656	+
	657	+ /* Extract VLAN TCI */
	658	+ if (fs->flow_type & FLOW_EXT) {
	659	+ vlan_tci = fs->h_ext.vlan_tci;
	660	+ vlan_m_tci = fs->m_ext.vlan_tci;
	661	+ }
547	662
548	663	layout = &udf_tcpip6_layout;
549	664	slice_num = bcm_sf2_get_slice_number(layout, 0);
..	..	@@ -584,6 +699,15 @@
584	699	goto out_err;
585	700	}
586	701
	702	+ input.fs = fs;
	703	+ flow = ethtool_rx_flow_rule_create(&input);
	704	+ if (IS_ERR(flow)) {
	705	+ ret = PTR_ERR(flow);
	706	+ goto out_err;
	707	+ }
	708	+ flow_rule_match_ipv6_addrs(flow->rule, &ipv6);
	709	+ flow_rule_match_ports(flow->rule, &ports);
	710	+
587	711	/* Apply the UDF layout for this filter */
588	712	bcm_sf2_cfp_udf_set(priv, layout, slice_num);
589	713
..	..	@@ -617,20 +741,13 @@
617	741	reg = layout->udfs[slice_num].mask_value \| udf_upper_bits(num_udf);
618	742	core_writel(priv, reg, CORE_CFP_MASK_PORT(6));
619	743
620		- /* UDF_Valid[7:0] [31:24]
621		- * S-Tag [23:8]
622		- * C-Tag [7:0]
623		- */
624		- core_writel(priv, udf_lower_bits(num_udf) << 24, CORE_CFP_DATA_PORT(5));
625		-
626		- /* Mask all but valid UDFs */
627		- core_writel(priv, udf_lower_bits(num_udf) << 24, CORE_CFP_MASK_PORT(5));
628		-
629	744	/* Slice the IPv6 source address and port */
630		- bcm_sf2_cfp_slice_ipv6(priv, v6_spec->ip6src, v6_spec->psrc,
631		- slice_num, false);
632		- bcm_sf2_cfp_slice_ipv6(priv, v6_m_spec->ip6src, v6_m_spec->psrc,
633		- SLICE_NUM_MASK, true);
	745	+ bcm_sf2_cfp_slice_ipv6(priv, ipv6.key->src.in6_u.u6_addr32,
	746	+ ports.key->src, vlan_tci, slice_num,
	747	+ udf_lower_bits(num_udf), false);
	748	+ bcm_sf2_cfp_slice_ipv6(priv, ipv6.mask->src.in6_u.u6_addr32,
	749	+ ports.mask->src, vlan_m_tci, SLICE_NUM_MASK,
	750	+ udf_lower_bits(num_udf), true);
634	751
635	752	/* Insert into TCAM now because we need to insert a second rule */
636	753	bcm_sf2_cfp_rule_addr_set(priv, rule_index[0]);
..	..	@@ -638,20 +755,20 @@
638	755	ret = bcm_sf2_cfp_op(priv, OP_SEL_WRITE \| TCAM_SEL);
639	756	if (ret) {
640	757	pr_err("TCAM entry at addr %d failed\n", rule_index[0]);
641		- goto out_err;
	758	+ goto out_err_flow_rule;
642	759	}
643	760
644	761	/* Insert into Action and policer RAMs now */
645		- ret = bcm_sf2_cfp_act_pol_set(priv, rule_index[0], port_num,
	762	+ ret = bcm_sf2_cfp_act_pol_set(priv, rule_index[0], port, port_num,
646	763	queue_num, false);
647	764	if (ret)
648		- goto out_err;
	765	+ goto out_err_flow_rule;
649	766
650	767	/* Now deal with the second slice to chain this rule */
651	768	slice_num = bcm_sf2_get_slice_number(layout, slice_num + 1);
652	769	if (slice_num == UDF_NUM_SLICES) {
653	770	ret = -EINVAL;
654		- goto out_err;
	771	+ goto out_err_flow_rule;
655	772	}
656	773
657	774	num_udf = bcm_sf2_get_num_udf_slices(layout->udfs[slice_num].slices);
..	..	@@ -681,16 +798,12 @@
681	798	udf_lower_bits(num_udf) << 8;
682	799	core_writel(priv, reg, CORE_CFP_MASK_PORT(6));
683	800
684		- /* Don't care */
685		- core_writel(priv, 0, CORE_CFP_DATA_PORT(5));
686		-
687		- /* Mask all */
688		- core_writel(priv, 0, CORE_CFP_MASK_PORT(5));
689		-
690		- bcm_sf2_cfp_slice_ipv6(priv, v6_spec->ip6dst, v6_spec->pdst, slice_num,
691		- false);
692		- bcm_sf2_cfp_slice_ipv6(priv, v6_m_spec->ip6dst, v6_m_spec->pdst,
693		- SLICE_NUM_MASK, true);
	801	+ bcm_sf2_cfp_slice_ipv6(priv, ipv6.key->dst.in6_u.u6_addr32,
	802	+ ports.key->dst, 0, slice_num,
	803	+ 0, false);
	804	+ bcm_sf2_cfp_slice_ipv6(priv, ipv6.mask->dst.in6_u.u6_addr32,
	805	+ ports.key->dst, 0, SLICE_NUM_MASK,
	806	+ 0, true);
694	807
695	808	/* Insert into TCAM now */
696	809	bcm_sf2_cfp_rule_addr_set(priv, rule_index[1]);
..	..	@@ -698,16 +811,16 @@
698	811	ret = bcm_sf2_cfp_op(priv, OP_SEL_WRITE \| TCAM_SEL);
699	812	if (ret) {
700	813	pr_err("TCAM entry at addr %d failed\n", rule_index[1]);
701		- goto out_err;
	814	+ goto out_err_flow_rule;
702	815	}
703	816
704	817	/* Insert into Action and policer RAMs now, set chain ID to
705	818	* the one we are chained to
706	819	*/
707		- ret = bcm_sf2_cfp_act_pol_set(priv, rule_index[1], port_num,
	820	+ ret = bcm_sf2_cfp_act_pol_set(priv, rule_index[1], port, port_num,
708	821	queue_num, true);
709	822	if (ret)
710		- goto out_err;
	823	+ goto out_err_flow_rule;
711	824
712	825	/* Turn on CFP for this rule now */
713	826	reg = core_readl(priv, CORE_CFP_CTL_REG);
..	..	@@ -723,32 +836,23 @@
723	836
724	837	return ret;
725	838
	839	+out_err_flow_rule:
	840	+ ethtool_rx_flow_rule_destroy(flow);
726	841	out_err:
727	842	clear_bit(rule_index[1], priv->cfp.used);
728	843	return ret;
729	844	}
730	845
731		-static int bcm_sf2_cfp_rule_set(struct dsa_switch *ds, int port,
732		- struct ethtool_rx_flow_spec *fs)
	846	+static int bcm_sf2_cfp_rule_insert(struct dsa_switch *ds, int port,
	847	+ struct ethtool_rx_flow_spec *fs)
733	848	{
734	849	struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
735		- s8 cpu_port = ds->ports[port].cpu_dp->index;
	850	+ s8 cpu_port = dsa_to_port(ds, port)->cpu_dp->index;
736	851	__u64 ring_cookie = fs->ring_cookie;
	852	+ struct switchdev_obj_port_vlan vlan;
737	853	unsigned int queue_num, port_num;
738		- int ret = -EINVAL;
739		-
740		- /* Check for unsupported extensions */
741		- if ((fs->flow_type & FLOW_EXT) && (fs->m_ext.vlan_etype \|\|
742		- fs->m_ext.data[1]))
743		- return -EINVAL;
744		-
745		- if (fs->location != RX_CLS_LOC_ANY &&
746		- fs->location > bcm_sf2_cfp_rule_size(priv))
747		- return -EINVAL;
748		-
749		- if (fs->location != RX_CLS_LOC_ANY &&
750		- test_bit(fs->location, priv->cfp.used))
751		- return -EBUSY;
	854	+ u16 vid;
	855	+ int ret;
752	856
753	857	/* This rule is a Wake-on-LAN filter and we must specifically
754	858	* target the CPU port in order for it to be working.
..	..	@@ -767,6 +871,34 @@
767	871	dsa_is_cpu_port(ds, port_num)) \|\|
768	872	port_num >= priv->hw_params.num_ports)
769	873	return -EINVAL;
	874	+
	875	+ /* If the rule is matching a particular VLAN, make sure that we honor
	876	+ * the matching and have it tagged or untagged on the destination port,
	877	+ * we do this on egress with a VLAN entry. The egress tagging attribute
	878	+ * is expected to be provided in h_ext.data[1] bit 0. A 1 means untagged,
	879	+ * a 0 means tagged.
	880	+ */
	881	+ if (fs->flow_type & FLOW_EXT) {
	882	+ /* We cannot support matching multiple VLAN IDs yet */
	883	+ if ((be16_to_cpu(fs->m_ext.vlan_tci) & VLAN_VID_MASK) !=
	884	+ VLAN_VID_MASK)
	885	+ return -EINVAL;
	886	+
	887	+ vid = be16_to_cpu(fs->h_ext.vlan_tci) & VLAN_VID_MASK;
	888	+ vlan.vid_begin = vid;
	889	+ vlan.vid_end = vid;
	890	+ if (cpu_to_be32(fs->h_ext.data[1]) & 1)
	891	+ vlan.flags = BRIDGE_VLAN_INFO_UNTAGGED;
	892	+ else
	893	+ vlan.flags = 0;
	894	+
	895	+ ret = ds->ops->port_vlan_prepare(ds, port_num, &vlan);
	896	+ if (ret)
	897	+ return ret;
	898	+
	899	+ ds->ops->port_vlan_add(ds, port_num, &vlan);
	900	+ }
	901	+
770	902	/*
771	903	* We have a small oddity where Port 6 just does not have a
772	904	* valid bit here (so we substract by one).
..	..	@@ -787,8 +919,54 @@
787	919	queue_num, fs);
788	920	break;
789	921	default:
	922	+ ret = -EINVAL;
790	923	break;
791	924	}
	925	+
	926	+ return ret;
	927	+}
	928	+
	929	+static int bcm_sf2_cfp_rule_set(struct dsa_switch *ds, int port,
	930	+ struct ethtool_rx_flow_spec *fs)
	931	+{
	932	+ struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
	933	+ struct cfp_rule *rule = NULL;
	934	+ int ret = -EINVAL;
	935	+
	936	+ /* Check for unsupported extensions */
	937	+ if (fs->flow_type & FLOW_MAC_EXT)
	938	+ return -EINVAL;
	939	+
	940	+ if (fs->location != RX_CLS_LOC_ANY &&
	941	+ fs->location > bcm_sf2_cfp_rule_size(priv))
	942	+ return -EINVAL;
	943	+
	944	+ if ((fs->flow_type & FLOW_EXT) &&
	945	+ !(ds->ops->port_vlan_prepare \|\| ds->ops->port_vlan_add \|\|
	946	+ ds->ops->port_vlan_del))
	947	+ return -EOPNOTSUPP;
	948	+
	949	+ if (fs->location != RX_CLS_LOC_ANY &&
	950	+ test_bit(fs->location, priv->cfp.used))
	951	+ return -EBUSY;
	952	+
	953	+ ret = bcm_sf2_cfp_rule_cmp(priv, port, fs);
	954	+ if (ret == 0)
	955	+ return -EEXIST;
	956	+
	957	+ rule = kzalloc(sizeof(*rule), GFP_KERNEL);
	958	+ if (!rule)
	959	+ return -ENOMEM;
	960	+
	961	+ ret = bcm_sf2_cfp_rule_insert(ds, port, fs);
	962	+ if (ret) {
	963	+ kfree(rule);
	964	+ return ret;
	965	+ }
	966	+
	967	+ rule->port = port;
	968	+ memcpy(&rule->fs, fs, sizeof(*fs));
	969	+ list_add_tail(&rule->next, &priv->cfp.rules_list);
792	970
793	971	return ret;
794	972	}
..	..	@@ -830,10 +1008,26 @@
830	1008	return 0;
831	1009	}
832	1010
833		-static int bcm_sf2_cfp_rule_del(struct bcm_sf2_priv *priv, int port,
834		- u32 loc)
	1011	+static int bcm_sf2_cfp_rule_remove(struct bcm_sf2_priv *priv, int port,
	1012	+ u32 loc)
835	1013	{
836	1014	u32 next_loc = 0;
	1015	+ int ret;
	1016	+
	1017	+ ret = bcm_sf2_cfp_rule_del_one(priv, port, loc, &next_loc);
	1018	+ if (ret)
	1019	+ return ret;
	1020	+
	1021	+ /* If this was an IPv6 rule, delete is companion rule too */
	1022	+ if (next_loc)
	1023	+ ret = bcm_sf2_cfp_rule_del_one(priv, port, next_loc, NULL);
	1024	+
	1025	+ return ret;
	1026	+}
	1027	+
	1028	+static int bcm_sf2_cfp_rule_del(struct bcm_sf2_priv *priv, int port, u32 loc)
	1029	+{
	1030	+ struct cfp_rule *rule;
837	1031	int ret;
838	1032
839	1033	if (loc > bcm_sf2_cfp_rule_size(priv))
..	..	@@ -846,13 +1040,14 @@
846	1040	if (!test_bit(loc, priv->cfp.unique) \|\| loc == 0)
847	1041	return -EINVAL;
848	1042
849		- ret = bcm_sf2_cfp_rule_del_one(priv, port, loc, &next_loc);
850		- if (ret)
851		- return ret;
	1043	+ rule = bcm_sf2_cfp_rule_find(priv, port, loc);
	1044	+ if (!rule)
	1045	+ return -EINVAL;
852	1046
853		- /* If this was an IPv6 rule, delete is companion rule too */
854		- if (next_loc)
855		- ret = bcm_sf2_cfp_rule_del_one(priv, port, next_loc, NULL);
	1047	+ ret = bcm_sf2_cfp_rule_remove(priv, port, loc);
	1048	+
	1049	+ list_del(&rule->next);
	1050	+ kfree(rule);
856	1051
857	1052	return ret;
858	1053	}
..	..	@@ -870,304 +1065,16 @@
870	1065	flow->m_ext.data[1] ^= cpu_to_be32(~0);
871	1066	}
872	1067
873		-static int bcm_sf2_cfp_unslice_ipv4(struct bcm_sf2_priv *priv,
874		- struct ethtool_tcpip4_spec *v4_spec,
875		- bool mask)
876		-{
877		- u32 reg, offset, ipv4;
878		- u16 src_dst_port;
879		-
880		- if (mask)
881		- offset = CORE_CFP_MASK_PORT(3);
882		- else
883		- offset = CORE_CFP_DATA_PORT(3);
884		-
885		- reg = core_readl(priv, offset);
886		- /* src port [15:8] */
887		- src_dst_port = reg << 8;
888		-
889		- if (mask)
890		- offset = CORE_CFP_MASK_PORT(2);
891		- else
892		- offset = CORE_CFP_DATA_PORT(2);
893		-
894		- reg = core_readl(priv, offset);
895		- /* src port [7:0] */
896		- src_dst_port \|= (reg >> 24);
897		-
898		- v4_spec->pdst = cpu_to_be16(src_dst_port);
899		- v4_spec->psrc = cpu_to_be16((u16)(reg >> 8));
900		-
901		- /* IPv4 dst [15:8] */
902		- ipv4 = (reg & 0xff) << 8;
903		-
904		- if (mask)
905		- offset = CORE_CFP_MASK_PORT(1);
906		- else
907		- offset = CORE_CFP_DATA_PORT(1);
908		-
909		- reg = core_readl(priv, offset);
910		- /* IPv4 dst [31:16] */
911		- ipv4 \|= ((reg >> 8) & 0xffff) << 16;
912		- /* IPv4 dst [7:0] */
913		- ipv4 \|= (reg >> 24) & 0xff;
914		- v4_spec->ip4dst = cpu_to_be32(ipv4);
915		-
916		- /* IPv4 src [15:8] */
917		- ipv4 = (reg & 0xff) << 8;
918		-
919		- if (mask)
920		- offset = CORE_CFP_MASK_PORT(0);
921		- else
922		- offset = CORE_CFP_DATA_PORT(0);
923		- reg = core_readl(priv, offset);
924		-
925		- /* Once the TCAM is programmed, the mask reflects the slice number
926		- * being matched, don't bother checking it when reading back the
927		- * mask spec
928		- */
929		- if (!mask && !(reg & SLICE_VALID))
930		- return -EINVAL;
931		-
932		- /* IPv4 src [7:0] */
933		- ipv4 \|= (reg >> 24) & 0xff;
934		- /* IPv4 src [31:16] */
935		- ipv4 \|= ((reg >> 8) & 0xffff) << 16;
936		- v4_spec->ip4src = cpu_to_be32(ipv4);
937		-
938		- return 0;
939		-}
940		-
941		-static int bcm_sf2_cfp_ipv4_rule_get(struct bcm_sf2_priv *priv, int port,
942		- struct ethtool_rx_flow_spec *fs)
943		-{
944		- struct ethtool_tcpip4_spec v4_spec = NULL, v4_m_spec = NULL;
945		- u32 reg;
946		- int ret;
947		-
948		- reg = core_readl(priv, CORE_CFP_DATA_PORT(6));
949		-
950		- switch ((reg & IPPROTO_MASK) >> IPPROTO_SHIFT) {
951		- case IPPROTO_TCP:
952		- fs->flow_type = TCP_V4_FLOW;
953		- v4_spec = &fs->h_u.tcp_ip4_spec;
954		- v4_m_spec = &fs->m_u.tcp_ip4_spec;
955		- break;
956		- case IPPROTO_UDP:
957		- fs->flow_type = UDP_V4_FLOW;
958		- v4_spec = &fs->h_u.udp_ip4_spec;
959		- v4_m_spec = &fs->m_u.udp_ip4_spec;
960		- break;
961		- default:
962		- return -EINVAL;
963		- }
964		-
965		- fs->m_ext.data[0] = cpu_to_be32((reg >> IP_FRAG_SHIFT) & 1);
966		- v4_spec->tos = (reg >> IPTOS_SHIFT) & IPTOS_MASK;
967		-
968		- ret = bcm_sf2_cfp_unslice_ipv4(priv, v4_spec, false);
969		- if (ret)
970		- return ret;
971		-
972		- return bcm_sf2_cfp_unslice_ipv4(priv, v4_m_spec, true);
973		-}
974		-
975		-static int bcm_sf2_cfp_unslice_ipv6(struct bcm_sf2_priv *priv,
976		- __be32 ip6_addr, __be16 port,
977		- bool mask)
978		-{
979		- u32 reg, tmp, offset;
980		-
981		- /* C-Tag [31:24]
982		- * UDF_n_B8 [23:8] (port)
983		- * UDF_n_B7 (upper) [7:0] (addr[15:8])
984		- */
985		- if (mask)
986		- offset = CORE_CFP_MASK_PORT(4);
987		- else
988		- offset = CORE_CFP_DATA_PORT(4);
989		- reg = core_readl(priv, offset);
990		- *port = cpu_to_be32(reg) >> 8;
991		- tmp = (u32)(reg & 0xff) << 8;
992		-
993		- /* UDF_n_B7 (lower) [31:24] (addr[7:0])
994		- * UDF_n_B6 [23:8] (addr[31:16])
995		- * UDF_n_B5 (upper) [7:0] (addr[47:40])
996		- */
997		- if (mask)
998		- offset = CORE_CFP_MASK_PORT(3);
999		- else
1000		- offset = CORE_CFP_DATA_PORT(3);
1001		- reg = core_readl(priv, offset);
1002		- tmp \|= (reg >> 24) & 0xff;
1003		- tmp \|= (u32)((reg >> 8) << 16);
1004		- ip6_addr[3] = cpu_to_be32(tmp);
1005		- tmp = (u32)(reg & 0xff) << 8;
1006		-
1007		- /* UDF_n_B5 (lower) [31:24] (addr[39:32])
1008		- * UDF_n_B4 [23:8] (addr[63:48])
1009		- * UDF_n_B3 (upper) [7:0] (addr[79:72])
1010		- */
1011		- if (mask)
1012		- offset = CORE_CFP_MASK_PORT(2);
1013		- else
1014		- offset = CORE_CFP_DATA_PORT(2);
1015		- reg = core_readl(priv, offset);
1016		- tmp \|= (reg >> 24) & 0xff;
1017		- tmp \|= (u32)((reg >> 8) << 16);
1018		- ip6_addr[2] = cpu_to_be32(tmp);
1019		- tmp = (u32)(reg & 0xff) << 8;
1020		-
1021		- /* UDF_n_B3 (lower) [31:24] (addr[71:64])
1022		- * UDF_n_B2 [23:8] (addr[95:80])
1023		- * UDF_n_B1 (upper) [7:0] (addr[111:104])
1024		- */
1025		- if (mask)
1026		- offset = CORE_CFP_MASK_PORT(1);
1027		- else
1028		- offset = CORE_CFP_DATA_PORT(1);
1029		- reg = core_readl(priv, offset);
1030		- tmp \|= (reg >> 24) & 0xff;
1031		- tmp \|= (u32)((reg >> 8) << 16);
1032		- ip6_addr[1] = cpu_to_be32(tmp);
1033		- tmp = (u32)(reg & 0xff) << 8;
1034		-
1035		- /* UDF_n_B1 (lower) [31:24] (addr[103:96])
1036		- * UDF_n_B0 [23:8] (addr[127:112])
1037		- * Reserved [7:4]
1038		- * Slice ID [3:2]
1039		- * Slice valid [1:0]
1040		- */
1041		- if (mask)
1042		- offset = CORE_CFP_MASK_PORT(0);
1043		- else
1044		- offset = CORE_CFP_DATA_PORT(0);
1045		- reg = core_readl(priv, offset);
1046		- tmp \|= (reg >> 24) & 0xff;
1047		- tmp \|= (u32)((reg >> 8) << 16);
1048		- ip6_addr[0] = cpu_to_be32(tmp);
1049		-
1050		- if (!mask && !(reg & SLICE_VALID))
1051		- return -EINVAL;
1052		-
1053		- return 0;
1054		-}
1055		-
1056		-static int bcm_sf2_cfp_ipv6_rule_get(struct bcm_sf2_priv *priv, int port,
1057		- struct ethtool_rx_flow_spec *fs,
1058		- u32 next_loc)
1059		-{
1060		- struct ethtool_tcpip6_spec v6_spec = NULL, v6_m_spec = NULL;
1061		- u32 reg;
1062		- int ret;
1063		-
1064		- /* UDPv6 and TCPv6 both use ethtool_tcpip6_spec so we are fine
1065		- * assuming tcp_ip6_spec here being an union.
1066		- */
1067		- v6_spec = &fs->h_u.tcp_ip6_spec;
1068		- v6_m_spec = &fs->m_u.tcp_ip6_spec;
1069		-
1070		- /* Read the second half first */
1071		- ret = bcm_sf2_cfp_unslice_ipv6(priv, v6_spec->ip6dst, &v6_spec->pdst,
1072		- false);
1073		- if (ret)
1074		- return ret;
1075		-
1076		- ret = bcm_sf2_cfp_unslice_ipv6(priv, v6_m_spec->ip6dst,
1077		- &v6_m_spec->pdst, true);
1078		- if (ret)
1079		- return ret;
1080		-
1081		- /* Read last to avoid next entry clobbering the results during search
1082		- * operations. We would not have the port enabled for this rule, so
1083		- * don't bother checking it.
1084		- */
1085		- (void)core_readl(priv, CORE_CFP_DATA_PORT(7));
1086		-
1087		- /* The slice number is valid, so read the rule we are chained from now
1088		- * which is our first half.
1089		- */
1090		- bcm_sf2_cfp_rule_addr_set(priv, next_loc);
1091		- ret = bcm_sf2_cfp_op(priv, OP_SEL_READ \| TCAM_SEL);
1092		- if (ret)
1093		- return ret;
1094		-
1095		- reg = core_readl(priv, CORE_CFP_DATA_PORT(6));
1096		-
1097		- switch ((reg & IPPROTO_MASK) >> IPPROTO_SHIFT) {
1098		- case IPPROTO_TCP:
1099		- fs->flow_type = TCP_V6_FLOW;
1100		- break;
1101		- case IPPROTO_UDP:
1102		- fs->flow_type = UDP_V6_FLOW;
1103		- break;
1104		- default:
1105		- return -EINVAL;
1106		- }
1107		-
1108		- ret = bcm_sf2_cfp_unslice_ipv6(priv, v6_spec->ip6src, &v6_spec->psrc,
1109		- false);
1110		- if (ret)
1111		- return ret;
1112		-
1113		- return bcm_sf2_cfp_unslice_ipv6(priv, v6_m_spec->ip6src,
1114		- &v6_m_spec->psrc, true);
1115		-}
1116		-
1117	1068	static int bcm_sf2_cfp_rule_get(struct bcm_sf2_priv *priv, int port,
1118	1069	struct ethtool_rxnfc *nfc)
1119	1070	{
1120		- u32 reg, ipv4_or_chain_id;
1121		- unsigned int queue_num;
1122		- int ret;
	1071	+ struct cfp_rule *rule;
1123	1072
1124		- bcm_sf2_cfp_rule_addr_set(priv, nfc->fs.location);
1125		-
1126		- ret = bcm_sf2_cfp_op(priv, OP_SEL_READ \| ACT_POL_RAM);
1127		- if (ret)
1128		- return ret;
1129		-
1130		- reg = core_readl(priv, CORE_ACT_POL_DATA0);
1131		-
1132		- ret = bcm_sf2_cfp_op(priv, OP_SEL_READ \| TCAM_SEL);
1133		- if (ret)
1134		- return ret;
1135		-
1136		- /* Extract the destination port */
1137		- nfc->fs.ring_cookie = fls((reg >> DST_MAP_IB_SHIFT) &
1138		- DST_MAP_IB_MASK) - 1;
1139		-
1140		- /* There is no Port 6, so we compensate for that here */
1141		- if (nfc->fs.ring_cookie >= 6)
1142		- nfc->fs.ring_cookie++;
1143		- nfc->fs.ring_cookie *= SF2_NUM_EGRESS_QUEUES;
1144		-
1145		- /* Extract the destination queue */
1146		- queue_num = (reg >> NEW_TC_SHIFT) & NEW_TC_MASK;
1147		- nfc->fs.ring_cookie += queue_num;
1148		-
1149		- /* Extract the L3_FRAMING or CHAIN_ID */
1150		- reg = core_readl(priv, CORE_CFP_DATA_PORT(6));
1151		-
1152		- /* With IPv6 rules this would contain a non-zero chain ID since
1153		- * we reserve entry 0 and it cannot be used. So if we read 0 here
1154		- * this means an IPv4 rule.
1155		- */
1156		- ipv4_or_chain_id = (reg >> L3_FRAMING_SHIFT) & 0xff;
1157		- if (ipv4_or_chain_id == 0)
1158		- ret = bcm_sf2_cfp_ipv4_rule_get(priv, port, &nfc->fs);
1159		- else
1160		- ret = bcm_sf2_cfp_ipv6_rule_get(priv, port, &nfc->fs,
1161		- ipv4_or_chain_id);
1162		- if (ret)
1163		- return ret;
1164		-
1165		- /* Read last to avoid next entry clobbering the results during search
1166		- * operations
1167		- */
1168		- reg = core_readl(priv, CORE_CFP_DATA_PORT(7));
1169		- if (!(reg & 1 << port))
	1073	+ rule = bcm_sf2_cfp_rule_find(priv, port, nfc->fs.location);
	1074	+ if (!rule)
1170	1075	return -EINVAL;
	1076	+
	1077	+ memcpy(&nfc->fs, &rule->fs, sizeof(rule->fs));
1171	1078
1172	1079	bcm_sf2_invert_masks(&nfc->fs);
1173	1080
..	..	@@ -1199,7 +1106,7 @@
1199	1106	int bcm_sf2_get_rxnfc(struct dsa_switch *ds, int port,
1200	1107	struct ethtool_rxnfc nfc, u32 rule_locs)
1201	1108	{
1202		- struct net_device *p = ds->ports[port].cpu_dp->master;
	1109	+ struct net_device *p = dsa_to_port(ds, port)->cpu_dp->master;
1203	1110	struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
1204	1111	int ret = 0;
1205	1112
..	..	@@ -1242,7 +1149,7 @@
1242	1149	int bcm_sf2_set_rxnfc(struct dsa_switch *ds, int port,
1243	1150	struct ethtool_rxnfc *nfc)
1244	1151	{
1245		- struct net_device *p = ds->ports[port].cpu_dp->master;
	1152	+ struct net_device *p = dsa_to_port(ds, port)->cpu_dp->master;
1246	1153	struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
1247	1154	int ret = 0;
1248	1155
..	..	@@ -1305,3 +1212,139 @@
1305	1212
1306	1213	return 0;
1307	1214	}
	1215	+
	1216	+void bcm_sf2_cfp_exit(struct dsa_switch *ds)
	1217	+{
	1218	+ struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
	1219	+ struct cfp_rule rule, n;
	1220	+
	1221	+ if (list_empty(&priv->cfp.rules_list))
	1222	+ return;
	1223	+
	1224	+ list_for_each_entry_safe_reverse(rule, n, &priv->cfp.rules_list, next)
	1225	+ bcm_sf2_cfp_rule_del(priv, rule->port, rule->fs.location);
	1226	+}
	1227	+
	1228	+int bcm_sf2_cfp_resume(struct dsa_switch *ds)
	1229	+{
	1230	+ struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
	1231	+ struct cfp_rule *rule;
	1232	+ int ret = 0;
	1233	+ u32 reg;
	1234	+
	1235	+ if (list_empty(&priv->cfp.rules_list))
	1236	+ return ret;
	1237	+
	1238	+ reg = core_readl(priv, CORE_CFP_CTL_REG);
	1239	+ reg &= ~CFP_EN_MAP_MASK;
	1240	+ core_writel(priv, reg, CORE_CFP_CTL_REG);
	1241	+
	1242	+ ret = bcm_sf2_cfp_rst(priv);
	1243	+ if (ret)
	1244	+ return ret;
	1245	+
	1246	+ list_for_each_entry(rule, &priv->cfp.rules_list, next) {
	1247	+ ret = bcm_sf2_cfp_rule_remove(priv, rule->port,
	1248	+ rule->fs.location);
	1249	+ if (ret) {
	1250	+ dev_err(ds->dev, "failed to remove rule\n");
	1251	+ return ret;
	1252	+ }
	1253	+
	1254	+ ret = bcm_sf2_cfp_rule_insert(ds, rule->port, &rule->fs);
	1255	+ if (ret) {
	1256	+ dev_err(ds->dev, "failed to restore rule\n");
	1257	+ return ret;
	1258	+ }
	1259	+ }
	1260	+
	1261	+ return ret;
	1262	+}
	1263	+
	1264	+static const struct bcm_sf2_cfp_stat {
	1265	+ unsigned int offset;
	1266	+ unsigned int ram_loc;
	1267	+ const char *name;
	1268	+} bcm_sf2_cfp_stats[] = {
	1269	+ {
	1270	+ .offset = CORE_STAT_GREEN_CNTR,
	1271	+ .ram_loc = GREEN_STAT_RAM,
	1272	+ .name = "Green"
	1273	+ },
	1274	+ {
	1275	+ .offset = CORE_STAT_YELLOW_CNTR,
	1276	+ .ram_loc = YELLOW_STAT_RAM,
	1277	+ .name = "Yellow"
	1278	+ },
	1279	+ {
	1280	+ .offset = CORE_STAT_RED_CNTR,
	1281	+ .ram_loc = RED_STAT_RAM,
	1282	+ .name = "Red"
	1283	+ },
	1284	+};
	1285	+
	1286	+void bcm_sf2_cfp_get_strings(struct dsa_switch *ds, int port,
	1287	+ u32 stringset, uint8_t *data)
	1288	+{
	1289	+ struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
	1290	+ unsigned int s = ARRAY_SIZE(bcm_sf2_cfp_stats);
	1291	+ char buf[ETH_GSTRING_LEN];
	1292	+ unsigned int i, j, iter;
	1293	+
	1294	+ if (stringset != ETH_SS_STATS)
	1295	+ return;
	1296	+
	1297	+ for (i = 1; i < priv->num_cfp_rules; i++) {
	1298	+ for (j = 0; j < s; j++) {
	1299	+ snprintf(buf, sizeof(buf),
	1300	+ "CFP%03d_%sCntr",
	1301	+ i, bcm_sf2_cfp_stats[j].name);
	1302	+ iter = (i - 1) * s + j;
	1303	+ strlcpy(data + iter * ETH_GSTRING_LEN,
	1304	+ buf, ETH_GSTRING_LEN);
	1305	+ }
	1306	+ }
	1307	+}
	1308	+
	1309	+void bcm_sf2_cfp_get_ethtool_stats(struct dsa_switch *ds, int port,
	1310	+ uint64_t *data)
	1311	+{
	1312	+ struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
	1313	+ unsigned int s = ARRAY_SIZE(bcm_sf2_cfp_stats);
	1314	+ const struct bcm_sf2_cfp_stat *stat;
	1315	+ unsigned int i, j, iter;
	1316	+ struct cfp_rule *rule;
	1317	+ int ret;
	1318	+
	1319	+ mutex_lock(&priv->cfp.lock);
	1320	+ for (i = 1; i < priv->num_cfp_rules; i++) {
	1321	+ rule = bcm_sf2_cfp_rule_find(priv, port, i);
	1322	+ if (!rule)
	1323	+ continue;
	1324	+
	1325	+ for (j = 0; j < s; j++) {
	1326	+ stat = &bcm_sf2_cfp_stats[j];
	1327	+
	1328	+ bcm_sf2_cfp_rule_addr_set(priv, i);
	1329	+ ret = bcm_sf2_cfp_op(priv, stat->ram_loc \| OP_SEL_READ);
	1330	+ if (ret)
	1331	+ continue;
	1332	+
	1333	+ iter = (i - 1) * s + j;
	1334	+ data[iter] = core_readl(priv, stat->offset);
	1335	+ }
	1336	+
	1337	+ }
	1338	+ mutex_unlock(&priv->cfp.lock);
	1339	+}
	1340	+
	1341	+int bcm_sf2_cfp_get_sset_count(struct dsa_switch *ds, int port, int sset)
	1342	+{
	1343	+ struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
	1344	+
	1345	+ if (sset != ETH_SS_STATS)
	1346	+ return 0;
	1347	+
	1348	+ /* 3 counters per CFP rules */
	1349	+ return (priv->num_cfp_rules - 1) * ARRAY_SIZE(bcm_sf2_cfp_stats);
	1350	+}