1 // SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
2 /* Copyright 2019 NXP */
3 
4 #include "enetc.h"
5 
6 #include <net/pkt_sched.h>
7 #include <linux/math64.h>
8 #include <linux/refcount.h>
9 #include <net/pkt_cls.h>
10 #include <net/tc_act/tc_gate.h>
11 
12 static u16 enetc_get_max_gcl_len(struct enetc_hw *hw)
13 {
14 	return enetc_rd(hw, ENETC_QBV_PTGCAPR_OFFSET)
15 		& ENETC_QBV_MAX_GCL_LEN_MASK;
16 }
17 
18 void enetc_sched_speed_set(struct enetc_ndev_priv *priv, int speed)
19 {
20 	u32 old_speed = priv->speed;
21 	u32 pspeed;
22 
23 	if (speed == old_speed)
24 		return;
25 
26 	switch (speed) {
27 	case SPEED_1000:
28 		pspeed = ENETC_PMR_PSPEED_1000M;
29 		break;
30 	case SPEED_2500:
31 		pspeed = ENETC_PMR_PSPEED_2500M;
32 		break;
33 	case SPEED_100:
34 		pspeed = ENETC_PMR_PSPEED_100M;
35 		break;
36 	case SPEED_10:
37 	default:
38 		pspeed = ENETC_PMR_PSPEED_10M;
39 	}
40 
41 	priv->speed = speed;
42 	enetc_port_wr(&priv->si->hw, ENETC_PMR,
43 		      (enetc_port_rd(&priv->si->hw, ENETC_PMR)
44 		      & (~ENETC_PMR_PSPEED_MASK))
45 		      | pspeed);
46 }
47 
48 static int enetc_setup_taprio(struct net_device *ndev,
49 			      struct tc_taprio_qopt_offload *admin_conf)
50 {
51 	struct enetc_ndev_priv *priv = netdev_priv(ndev);
52 	struct enetc_cbd cbd = {.cmd = 0};
53 	struct tgs_gcl_conf *gcl_config;
54 	struct tgs_gcl_data *gcl_data;
55 	struct gce *gce;
56 	dma_addr_t dma;
57 	u16 data_size;
58 	u16 gcl_len;
59 	u32 tge;
60 	int err;
61 	int i;
62 
63 	if (admin_conf->num_entries > enetc_get_max_gcl_len(&priv->si->hw))
64 		return -EINVAL;
65 	gcl_len = admin_conf->num_entries;
66 
67 	tge = enetc_rd(&priv->si->hw, ENETC_QBV_PTGCR_OFFSET);
68 	if (!admin_conf->enable) {
69 		enetc_wr(&priv->si->hw,
70 			 ENETC_QBV_PTGCR_OFFSET,
71 			 tge & (~ENETC_QBV_TGE));
72 		return 0;
73 	}
74 
75 	if (admin_conf->cycle_time > U32_MAX ||
76 	    admin_conf->cycle_time_extension > U32_MAX)
77 		return -EINVAL;
78 
79 	/* Configure the (administrative) gate control list using the
80 	 * control BD descriptor.
81 	 */
82 	gcl_config = &cbd.gcl_conf;
83 
84 	data_size = struct_size(gcl_data, entry, gcl_len);
85 	gcl_data = kzalloc(data_size, __GFP_DMA | GFP_KERNEL);
86 	if (!gcl_data)
87 		return -ENOMEM;
88 
89 	gce = (struct gce *)(gcl_data + 1);
90 
91 	/* Set all gates open as default */
92 	gcl_config->atc = 0xff;
93 	gcl_config->acl_len = cpu_to_le16(gcl_len);
94 
95 	gcl_data->btl = cpu_to_le32(lower_32_bits(admin_conf->base_time));
96 	gcl_data->bth = cpu_to_le32(upper_32_bits(admin_conf->base_time));
97 	gcl_data->ct = cpu_to_le32(admin_conf->cycle_time);
98 	gcl_data->cte = cpu_to_le32(admin_conf->cycle_time_extension);
99 
100 	for (i = 0; i < gcl_len; i++) {
101 		struct tc_taprio_sched_entry *temp_entry;
102 		struct gce *temp_gce = gce + i;
103 
104 		temp_entry = &admin_conf->entries[i];
105 
106 		temp_gce->gate = (u8)temp_entry->gate_mask;
107 		temp_gce->period = cpu_to_le32(temp_entry->interval);
108 	}
109 
110 	cbd.length = cpu_to_le16(data_size);
111 	cbd.status_flags = 0;
112 
113 	dma = dma_map_single(&priv->si->pdev->dev, gcl_data,
114 			     data_size, DMA_TO_DEVICE);
115 	if (dma_mapping_error(&priv->si->pdev->dev, dma)) {
116 		netdev_err(priv->si->ndev, "DMA mapping failed!\n");
117 		kfree(gcl_data);
118 		return -ENOMEM;
119 	}
120 
121 	cbd.addr[0] = cpu_to_le32(lower_32_bits(dma));
122 	cbd.addr[1] = cpu_to_le32(upper_32_bits(dma));
123 	cbd.cls = BDCR_CMD_PORT_GCL;
124 	cbd.status_flags = 0;
125 
126 	enetc_wr(&priv->si->hw, ENETC_QBV_PTGCR_OFFSET,
127 		 tge | ENETC_QBV_TGE);
128 
129 	err = enetc_send_cmd(priv->si, &cbd);
130 	if (err)
131 		enetc_wr(&priv->si->hw,
132 			 ENETC_QBV_PTGCR_OFFSET,
133 			 tge & (~ENETC_QBV_TGE));
134 
135 	dma_unmap_single(&priv->si->pdev->dev, dma, data_size, DMA_TO_DEVICE);
136 	kfree(gcl_data);
137 
138 	return err;
139 }
140 
141 int enetc_setup_tc_taprio(struct net_device *ndev, void *type_data)
142 {
143 	struct tc_taprio_qopt_offload *taprio = type_data;
144 	struct enetc_ndev_priv *priv = netdev_priv(ndev);
145 	int err;
146 	int i;
147 
148 	/* TSD and Qbv are mutually exclusive in hardware */
149 	for (i = 0; i < priv->num_tx_rings; i++)
150 		if (priv->tx_ring[i]->tsd_enable)
151 			return -EBUSY;
152 
153 	for (i = 0; i < priv->num_tx_rings; i++)
154 		enetc_set_bdr_prio(&priv->si->hw,
155 				   priv->tx_ring[i]->index,
156 				   taprio->enable ? i : 0);
157 
158 	err = enetc_setup_taprio(ndev, taprio);
159 
160 	if (err)
161 		for (i = 0; i < priv->num_tx_rings; i++)
162 			enetc_set_bdr_prio(&priv->si->hw,
163 					   priv->tx_ring[i]->index,
164 					   taprio->enable ? 0 : i);
165 
166 	return err;
167 }
168 
169 static u32 enetc_get_cbs_enable(struct enetc_hw *hw, u8 tc)
170 {
171 	return enetc_port_rd(hw, ENETC_PTCCBSR0(tc)) & ENETC_CBSE;
172 }
173 
174 static u8 enetc_get_cbs_bw(struct enetc_hw *hw, u8 tc)
175 {
176 	return enetc_port_rd(hw, ENETC_PTCCBSR0(tc)) & ENETC_CBS_BW_MASK;
177 }
178 
179 int enetc_setup_tc_cbs(struct net_device *ndev, void *type_data)
180 {
181 	struct enetc_ndev_priv *priv = netdev_priv(ndev);
182 	struct tc_cbs_qopt_offload *cbs = type_data;
183 	u32 port_transmit_rate = priv->speed;
184 	u8 tc_nums = netdev_get_num_tc(ndev);
185 	struct enetc_si *si = priv->si;
186 	u32 hi_credit_bit, hi_credit_reg;
187 	u32 max_interference_size;
188 	u32 port_frame_max_size;
189 	u8 tc = cbs->queue;
190 	u8 prio_top, prio_next;
191 	int bw_sum = 0;
192 	u8 bw;
193 
194 	prio_top = netdev_get_prio_tc_map(ndev, tc_nums - 1);
195 	prio_next = netdev_get_prio_tc_map(ndev, tc_nums - 2);
196 
197 	/* Support highest prio and second prio tc in cbs mode */
198 	if (tc != prio_top && tc != prio_next)
199 		return -EOPNOTSUPP;
200 
201 	if (!cbs->enable) {
202 		/* Make sure the other TC that are numerically
203 		 * lower than this TC have been disabled.
204 		 */
205 		if (tc == prio_top &&
206 		    enetc_get_cbs_enable(&si->hw, prio_next)) {
207 			dev_err(&ndev->dev,
208 				"Disable TC%d before disable TC%d\n",
209 				prio_next, tc);
210 			return -EINVAL;
211 		}
212 
213 		enetc_port_wr(&si->hw, ENETC_PTCCBSR1(tc), 0);
214 		enetc_port_wr(&si->hw, ENETC_PTCCBSR0(tc), 0);
215 
216 		return 0;
217 	}
218 
219 	if (cbs->idleslope - cbs->sendslope != port_transmit_rate * 1000L ||
220 	    cbs->idleslope < 0 || cbs->sendslope > 0)
221 		return -EOPNOTSUPP;
222 
223 	port_frame_max_size = ndev->mtu + VLAN_ETH_HLEN + ETH_FCS_LEN;
224 
225 	bw = cbs->idleslope / (port_transmit_rate * 10UL);
226 
227 	/* Make sure the other TC that are numerically
228 	 * higher than this TC have been enabled.
229 	 */
230 	if (tc == prio_next) {
231 		if (!enetc_get_cbs_enable(&si->hw, prio_top)) {
232 			dev_err(&ndev->dev,
233 				"Enable TC%d first before enable TC%d\n",
234 				prio_top, prio_next);
235 			return -EINVAL;
236 		}
237 		bw_sum += enetc_get_cbs_bw(&si->hw, prio_top);
238 	}
239 
240 	if (bw_sum + bw >= 100) {
241 		dev_err(&ndev->dev,
242 			"The sum of all CBS Bandwidth can't exceed 100\n");
243 		return -EINVAL;
244 	}
245 
246 	enetc_port_rd(&si->hw, ENETC_PTCMSDUR(tc));
247 
248 	/* For top prio TC, the max_interfrence_size is maxSizedFrame.
249 	 *
250 	 * For next prio TC, the max_interfrence_size is calculated as below:
251 	 *
252 	 *      max_interference_size = M0 + Ma + Ra * M0 / (R0 - Ra)
253 	 *
254 	 *	- RA: idleSlope for AVB Class A
255 	 *	- R0: port transmit rate
256 	 *	- M0: maximum sized frame for the port
257 	 *	- MA: maximum sized frame for AVB Class A
258 	 */
259 
260 	if (tc == prio_top) {
261 		max_interference_size = port_frame_max_size * 8;
262 	} else {
263 		u32 m0, ma, r0, ra;
264 
265 		m0 = port_frame_max_size * 8;
266 		ma = enetc_port_rd(&si->hw, ENETC_PTCMSDUR(prio_top)) * 8;
267 		ra = enetc_get_cbs_bw(&si->hw, prio_top) *
268 			port_transmit_rate * 10000ULL;
269 		r0 = port_transmit_rate * 1000000ULL;
270 		max_interference_size = m0 + ma +
271 			(u32)div_u64((u64)ra * m0, r0 - ra);
272 	}
273 
274 	/* hiCredit bits calculate by:
275 	 *
276 	 * maxSizedFrame * (idleSlope/portTxRate)
277 	 */
278 	hi_credit_bit = max_interference_size * bw / 100;
279 
280 	/* hiCredit bits to hiCredit register need to calculated as:
281 	 *
282 	 * (enetClockFrequency / portTransmitRate) * 100
283 	 */
284 	hi_credit_reg = (u32)div_u64((ENETC_CLK * 100ULL) * hi_credit_bit,
285 				     port_transmit_rate * 1000000ULL);
286 
287 	enetc_port_wr(&si->hw, ENETC_PTCCBSR1(tc), hi_credit_reg);
288 
289 	/* Set bw register and enable this traffic class */
290 	enetc_port_wr(&si->hw, ENETC_PTCCBSR0(tc), bw | ENETC_CBSE);
291 
292 	return 0;
293 }
294 
295 int enetc_setup_tc_txtime(struct net_device *ndev, void *type_data)
296 {
297 	struct enetc_ndev_priv *priv = netdev_priv(ndev);
298 	struct tc_etf_qopt_offload *qopt = type_data;
299 	u8 tc_nums = netdev_get_num_tc(ndev);
300 	int tc;
301 
302 	if (!tc_nums)
303 		return -EOPNOTSUPP;
304 
305 	tc = qopt->queue;
306 
307 	if (tc < 0 || tc >= priv->num_tx_rings)
308 		return -EINVAL;
309 
310 	/* Do not support TXSTART and TX CSUM offload simutaniously */
311 	if (ndev->features & NETIF_F_CSUM_MASK)
312 		return -EBUSY;
313 
314 	/* TSD and Qbv are mutually exclusive in hardware */
315 	if (enetc_rd(&priv->si->hw, ENETC_QBV_PTGCR_OFFSET) & ENETC_QBV_TGE)
316 		return -EBUSY;
317 
318 	priv->tx_ring[tc]->tsd_enable = qopt->enable;
319 	enetc_port_wr(&priv->si->hw, ENETC_PTCTSDR(tc),
320 		      qopt->enable ? ENETC_TSDE : 0);
321 
322 	return 0;
323 }
324 
325 enum streamid_type {
326 	STREAMID_TYPE_RESERVED = 0,
327 	STREAMID_TYPE_NULL,
328 	STREAMID_TYPE_SMAC,
329 };
330 
331 enum streamid_vlan_tagged {
332 	STREAMID_VLAN_RESERVED = 0,
333 	STREAMID_VLAN_TAGGED,
334 	STREAMID_VLAN_UNTAGGED,
335 	STREAMID_VLAN_ALL,
336 };
337 
338 #define ENETC_PSFP_WILDCARD -1
339 #define HANDLE_OFFSET 100
340 
341 enum forward_type {
342 	FILTER_ACTION_TYPE_PSFP = BIT(0),
343 	FILTER_ACTION_TYPE_ACL = BIT(1),
344 	FILTER_ACTION_TYPE_BOTH = GENMASK(1, 0),
345 };
346 
347 /* This is for limit output type for input actions */
348 struct actions_fwd {
349 	u64 actions;
350 	u64 keys;	/* include the must needed keys */
351 	enum forward_type output;
352 };
353 
354 struct psfp_streamfilter_counters {
355 	u64 matching_frames_count;
356 	u64 passing_frames_count;
357 	u64 not_passing_frames_count;
358 	u64 passing_sdu_count;
359 	u64 not_passing_sdu_count;
360 	u64 red_frames_count;
361 };
362 
363 struct enetc_streamid {
364 	u32 index;
365 	union {
366 		u8 src_mac[6];
367 		u8 dst_mac[6];
368 	};
369 	u8 filtertype;
370 	u16 vid;
371 	u8 tagged;
372 	s32 handle;
373 };
374 
375 struct enetc_psfp_filter {
376 	u32 index;
377 	s32 handle;
378 	s8 prio;
379 	u32 maxsdu;
380 	u32 gate_id;
381 	s32 meter_id;
382 	refcount_t refcount;
383 	struct hlist_node node;
384 };
385 
386 struct enetc_psfp_gate {
387 	u32 index;
388 	s8 init_ipv;
389 	u64 basetime;
390 	u64 cycletime;
391 	u64 cycletimext;
392 	u32 num_entries;
393 	refcount_t refcount;
394 	struct hlist_node node;
395 	struct action_gate_entry entries[];
396 };
397 
398 /* Only enable the green color frame now
399  * Will add eir and ebs color blind, couple flag etc when
400  * policing action add more offloading parameters
401  */
402 struct enetc_psfp_meter {
403 	u32 index;
404 	u32 cir;
405 	u32 cbs;
406 	refcount_t refcount;
407 	struct hlist_node node;
408 };
409 
410 #define ENETC_PSFP_FLAGS_FMI BIT(0)
411 
412 struct enetc_stream_filter {
413 	struct enetc_streamid sid;
414 	u32 sfi_index;
415 	u32 sgi_index;
416 	u32 flags;
417 	u32 fmi_index;
418 	struct flow_stats stats;
419 	struct hlist_node node;
420 };
421 
422 struct enetc_psfp {
423 	unsigned long dev_bitmap;
424 	unsigned long *psfp_sfi_bitmap;
425 	struct hlist_head stream_list;
426 	struct hlist_head psfp_filter_list;
427 	struct hlist_head psfp_gate_list;
428 	struct hlist_head psfp_meter_list;
429 	spinlock_t psfp_lock; /* spinlock for the struct enetc_psfp r/w */
430 };
431 
432 static struct actions_fwd enetc_act_fwd[] = {
433 	{
434 		BIT(FLOW_ACTION_GATE),
435 		BIT(FLOW_DISSECTOR_KEY_ETH_ADDRS),
436 		FILTER_ACTION_TYPE_PSFP
437 	},
438 	{
439 		BIT(FLOW_ACTION_POLICE) |
440 		BIT(FLOW_ACTION_GATE),
441 		BIT(FLOW_DISSECTOR_KEY_ETH_ADDRS),
442 		FILTER_ACTION_TYPE_PSFP
443 	},
444 	/* example for ACL actions */
445 	{
446 		BIT(FLOW_ACTION_DROP),
447 		0,
448 		FILTER_ACTION_TYPE_ACL
449 	}
450 };
451 
452 static struct enetc_psfp epsfp = {
453 	.psfp_sfi_bitmap = NULL,
454 };
455 
456 static LIST_HEAD(enetc_block_cb_list);
457 
458 /* Stream Identity Entry Set Descriptor */
459 static int enetc_streamid_hw_set(struct enetc_ndev_priv *priv,
460 				 struct enetc_streamid *sid,
461 				 u8 enable)
462 {
463 	struct enetc_cbd cbd = {.cmd = 0};
464 	struct streamid_data *si_data;
465 	struct streamid_conf *si_conf;
466 	u16 data_size;
467 	dma_addr_t dma;
468 	int port;
469 	int err;
470 
471 	port = enetc_pf_to_port(priv->si->pdev);
472 	if (port < 0)
473 		return -EINVAL;
474 
475 	if (sid->index >= priv->psfp_cap.max_streamid)
476 		return -EINVAL;
477 
478 	if (sid->filtertype != STREAMID_TYPE_NULL &&
479 	    sid->filtertype != STREAMID_TYPE_SMAC)
480 		return -EOPNOTSUPP;
481 
482 	/* Disable operation before enable */
483 	cbd.index = cpu_to_le16((u16)sid->index);
484 	cbd.cls = BDCR_CMD_STREAM_IDENTIFY;
485 	cbd.status_flags = 0;
486 
487 	data_size = sizeof(struct streamid_data);
488 	si_data = kzalloc(data_size, __GFP_DMA | GFP_KERNEL);
489 	if (!si_data)
490 		return -ENOMEM;
491 	cbd.length = cpu_to_le16(data_size);
492 
493 	dma = dma_map_single(&priv->si->pdev->dev, si_data,
494 			     data_size, DMA_FROM_DEVICE);
495 	if (dma_mapping_error(&priv->si->pdev->dev, dma)) {
496 		netdev_err(priv->si->ndev, "DMA mapping failed!\n");
497 		err = -ENOMEM;
498 		goto out;
499 	}
500 
501 	cbd.addr[0] = cpu_to_le32(lower_32_bits(dma));
502 	cbd.addr[1] = cpu_to_le32(upper_32_bits(dma));
503 	eth_broadcast_addr(si_data->dmac);
504 	si_data->vid_vidm_tg = (ENETC_CBDR_SID_VID_MASK
505 			       + ((0x3 << 14) | ENETC_CBDR_SID_VIDM));
506 
507 	si_conf = &cbd.sid_set;
508 	/* Only one port supported for one entry, set itself */
509 	si_conf->iports = cpu_to_le32(1 << port);
510 	si_conf->id_type = 1;
511 	si_conf->oui[2] = 0x0;
512 	si_conf->oui[1] = 0x80;
513 	si_conf->oui[0] = 0xC2;
514 
515 	err = enetc_send_cmd(priv->si, &cbd);
516 	if (err)
517 		goto out;
518 
519 	if (!enable)
520 		goto out;
521 
522 	/* Enable the entry overwrite again incase space flushed by hardware */
523 	memset(&cbd, 0, sizeof(cbd));
524 
525 	cbd.index = cpu_to_le16((u16)sid->index);
526 	cbd.cmd = 0;
527 	cbd.cls = BDCR_CMD_STREAM_IDENTIFY;
528 	cbd.status_flags = 0;
529 
530 	si_conf->en = 0x80;
531 	si_conf->stream_handle = cpu_to_le32(sid->handle);
532 	si_conf->iports = cpu_to_le32(1 << port);
533 	si_conf->id_type = sid->filtertype;
534 	si_conf->oui[2] = 0x0;
535 	si_conf->oui[1] = 0x80;
536 	si_conf->oui[0] = 0xC2;
537 
538 	memset(si_data, 0, data_size);
539 
540 	cbd.length = cpu_to_le16(data_size);
541 
542 	cbd.addr[0] = cpu_to_le32(lower_32_bits(dma));
543 	cbd.addr[1] = cpu_to_le32(upper_32_bits(dma));
544 
545 	/* VIDM default to be 1.
546 	 * VID Match. If set (b1) then the VID must match, otherwise
547 	 * any VID is considered a match. VIDM setting is only used
548 	 * when TG is set to b01.
549 	 */
550 	if (si_conf->id_type == STREAMID_TYPE_NULL) {
551 		ether_addr_copy(si_data->dmac, sid->dst_mac);
552 		si_data->vid_vidm_tg = (sid->vid & ENETC_CBDR_SID_VID_MASK) +
553 				       ((((u16)(sid->tagged) & 0x3) << 14)
554 				       | ENETC_CBDR_SID_VIDM);
555 	} else if (si_conf->id_type == STREAMID_TYPE_SMAC) {
556 		ether_addr_copy(si_data->smac, sid->src_mac);
557 		si_data->vid_vidm_tg = (sid->vid & ENETC_CBDR_SID_VID_MASK) +
558 				       ((((u16)(sid->tagged) & 0x3) << 14)
559 				       | ENETC_CBDR_SID_VIDM);
560 	}
561 
562 	err = enetc_send_cmd(priv->si, &cbd);
563 out:
564 	if (!dma_mapping_error(&priv->si->pdev->dev, dma))
565 		dma_unmap_single(&priv->si->pdev->dev, dma, data_size, DMA_FROM_DEVICE);
566 
567 	kfree(si_data);
568 
569 	return err;
570 }
571 
572 /* Stream Filter Instance Set Descriptor */
573 static int enetc_streamfilter_hw_set(struct enetc_ndev_priv *priv,
574 				     struct enetc_psfp_filter *sfi,
575 				     u8 enable)
576 {
577 	struct enetc_cbd cbd = {.cmd = 0};
578 	struct sfi_conf *sfi_config;
579 	int port;
580 
581 	port = enetc_pf_to_port(priv->si->pdev);
582 	if (port < 0)
583 		return -EINVAL;
584 
585 	cbd.index = cpu_to_le16(sfi->index);
586 	cbd.cls = BDCR_CMD_STREAM_FILTER;
587 	cbd.status_flags = 0x80;
588 	cbd.length = cpu_to_le16(1);
589 
590 	sfi_config = &cbd.sfi_conf;
591 	if (!enable)
592 		goto exit;
593 
594 	sfi_config->en = 0x80;
595 
596 	if (sfi->handle >= 0) {
597 		sfi_config->stream_handle =
598 			cpu_to_le32(sfi->handle);
599 		sfi_config->sthm |= 0x80;
600 	}
601 
602 	sfi_config->sg_inst_table_index = cpu_to_le16(sfi->gate_id);
603 	sfi_config->input_ports = cpu_to_le32(1 << port);
604 
605 	/* The priority value which may be matched against the
606 	 * frame’s priority value to determine a match for this entry.
607 	 */
608 	if (sfi->prio >= 0)
609 		sfi_config->multi |= (sfi->prio & 0x7) | 0x8;
610 
611 	/* Filter Type. Identifies the contents of the MSDU/FM_INST_INDEX
612 	 * field as being either an MSDU value or an index into the Flow
613 	 * Meter Instance table.
614 	 */
615 	if (sfi->maxsdu) {
616 		sfi_config->msdu =
617 		cpu_to_le16(sfi->maxsdu);
618 		sfi_config->multi |= 0x40;
619 	}
620 
621 	if (sfi->meter_id >= 0) {
622 		sfi_config->fm_inst_table_index = cpu_to_le16(sfi->meter_id);
623 		sfi_config->multi |= 0x80;
624 	}
625 
626 exit:
627 	return enetc_send_cmd(priv->si, &cbd);
628 }
629 
630 static int enetc_streamcounter_hw_get(struct enetc_ndev_priv *priv,
631 				      u32 index,
632 				      struct psfp_streamfilter_counters *cnt)
633 {
634 	struct enetc_cbd cbd = { .cmd = 2 };
635 	struct sfi_counter_data *data_buf;
636 	dma_addr_t dma;
637 	u16 data_size;
638 	int err;
639 
640 	cbd.index = cpu_to_le16((u16)index);
641 	cbd.cmd = 2;
642 	cbd.cls = BDCR_CMD_STREAM_FILTER;
643 	cbd.status_flags = 0;
644 
645 	data_size = sizeof(struct sfi_counter_data);
646 	data_buf = kzalloc(data_size, __GFP_DMA | GFP_KERNEL);
647 	if (!data_buf)
648 		return -ENOMEM;
649 
650 	dma = dma_map_single(&priv->si->pdev->dev, data_buf,
651 			     data_size, DMA_FROM_DEVICE);
652 	if (dma_mapping_error(&priv->si->pdev->dev, dma)) {
653 		netdev_err(priv->si->ndev, "DMA mapping failed!\n");
654 		err = -ENOMEM;
655 		goto exit;
656 	}
657 	cbd.addr[0] = cpu_to_le32(lower_32_bits(dma));
658 	cbd.addr[1] = cpu_to_le32(upper_32_bits(dma));
659 
660 	cbd.length = cpu_to_le16(data_size);
661 
662 	err = enetc_send_cmd(priv->si, &cbd);
663 	if (err)
664 		goto exit;
665 
666 	cnt->matching_frames_count = ((u64)data_buf->matchh << 32) +
667 				     data_buf->matchl;
668 
669 	cnt->not_passing_sdu_count = ((u64)data_buf->msdu_droph << 32) +
670 				     data_buf->msdu_dropl;
671 
672 	cnt->passing_sdu_count = cnt->matching_frames_count
673 				- cnt->not_passing_sdu_count;
674 
675 	cnt->not_passing_frames_count =
676 				((u64)data_buf->stream_gate_droph << 32) +
677 				data_buf->stream_gate_dropl;
678 
679 	cnt->passing_frames_count = cnt->matching_frames_count -
680 				    cnt->not_passing_sdu_count -
681 				    cnt->not_passing_frames_count;
682 
683 	cnt->red_frames_count =	((u64)data_buf->flow_meter_droph << 32)	+
684 				data_buf->flow_meter_dropl;
685 
686 exit:
687 	kfree(data_buf);
688 	return err;
689 }
690 
691 static u64 get_ptp_now(struct enetc_hw *hw)
692 {
693 	u64 now_lo, now_hi, now;
694 
695 	now_lo = enetc_rd(hw, ENETC_SICTR0);
696 	now_hi = enetc_rd(hw, ENETC_SICTR1);
697 	now = now_lo | now_hi << 32;
698 
699 	return now;
700 }
701 
702 static int get_start_ns(u64 now, u64 cycle, u64 *start)
703 {
704 	u64 n;
705 
706 	if (!cycle)
707 		return -EFAULT;
708 
709 	n = div64_u64(now, cycle);
710 
711 	*start = (n + 1) * cycle;
712 
713 	return 0;
714 }
715 
716 /* Stream Gate Instance Set Descriptor */
717 static int enetc_streamgate_hw_set(struct enetc_ndev_priv *priv,
718 				   struct enetc_psfp_gate *sgi,
719 				   u8 enable)
720 {
721 	struct enetc_cbd cbd = { .cmd = 0 };
722 	struct sgi_table *sgi_config;
723 	struct sgcl_conf *sgcl_config;
724 	struct sgcl_data *sgcl_data;
725 	struct sgce *sgce;
726 	dma_addr_t dma;
727 	u16 data_size;
728 	int err, i;
729 	u64 now;
730 
731 	cbd.index = cpu_to_le16(sgi->index);
732 	cbd.cmd = 0;
733 	cbd.cls = BDCR_CMD_STREAM_GCL;
734 	cbd.status_flags = 0x80;
735 
736 	/* disable */
737 	if (!enable)
738 		return enetc_send_cmd(priv->si, &cbd);
739 
740 	if (!sgi->num_entries)
741 		return 0;
742 
743 	if (sgi->num_entries > priv->psfp_cap.max_psfp_gatelist ||
744 	    !sgi->cycletime)
745 		return -EINVAL;
746 
747 	/* enable */
748 	sgi_config = &cbd.sgi_table;
749 
750 	/* Keep open before gate list start */
751 	sgi_config->ocgtst = 0x80;
752 
753 	sgi_config->oipv = (sgi->init_ipv < 0) ?
754 				0x0 : ((sgi->init_ipv & 0x7) | 0x8);
755 
756 	sgi_config->en = 0x80;
757 
758 	/* Basic config */
759 	err = enetc_send_cmd(priv->si, &cbd);
760 	if (err)
761 		return -EINVAL;
762 
763 	memset(&cbd, 0, sizeof(cbd));
764 
765 	cbd.index = cpu_to_le16(sgi->index);
766 	cbd.cmd = 1;
767 	cbd.cls = BDCR_CMD_STREAM_GCL;
768 	cbd.status_flags = 0;
769 
770 	sgcl_config = &cbd.sgcl_conf;
771 
772 	sgcl_config->acl_len = (sgi->num_entries - 1) & 0x3;
773 
774 	data_size = struct_size(sgcl_data, sgcl, sgi->num_entries);
775 
776 	sgcl_data = kzalloc(data_size, __GFP_DMA | GFP_KERNEL);
777 	if (!sgcl_data)
778 		return -ENOMEM;
779 
780 	cbd.length = cpu_to_le16(data_size);
781 
782 	dma = dma_map_single(&priv->si->pdev->dev,
783 			     sgcl_data, data_size,
784 			     DMA_FROM_DEVICE);
785 	if (dma_mapping_error(&priv->si->pdev->dev, dma)) {
786 		netdev_err(priv->si->ndev, "DMA mapping failed!\n");
787 		kfree(sgcl_data);
788 		return -ENOMEM;
789 	}
790 
791 	cbd.addr[0] = cpu_to_le32(lower_32_bits(dma));
792 	cbd.addr[1] = cpu_to_le32(upper_32_bits(dma));
793 
794 	sgce = &sgcl_data->sgcl[0];
795 
796 	sgcl_config->agtst = 0x80;
797 
798 	sgcl_data->ct = sgi->cycletime;
799 	sgcl_data->cte = sgi->cycletimext;
800 
801 	if (sgi->init_ipv >= 0)
802 		sgcl_config->aipv = (sgi->init_ipv & 0x7) | 0x8;
803 
804 	for (i = 0; i < sgi->num_entries; i++) {
805 		struct action_gate_entry *from = &sgi->entries[i];
806 		struct sgce *to = &sgce[i];
807 
808 		if (from->gate_state)
809 			to->multi |= 0x10;
810 
811 		if (from->ipv >= 0)
812 			to->multi |= ((from->ipv & 0x7) << 5) | 0x08;
813 
814 		if (from->maxoctets >= 0) {
815 			to->multi |= 0x01;
816 			to->msdu[0] = from->maxoctets & 0xFF;
817 			to->msdu[1] = (from->maxoctets >> 8) & 0xFF;
818 			to->msdu[2] = (from->maxoctets >> 16) & 0xFF;
819 		}
820 
821 		to->interval = from->interval;
822 	}
823 
824 	/* If basetime is less than now, calculate start time */
825 	now = get_ptp_now(&priv->si->hw);
826 
827 	if (sgi->basetime < now) {
828 		u64 start;
829 
830 		err = get_start_ns(now, sgi->cycletime, &start);
831 		if (err)
832 			goto exit;
833 		sgcl_data->btl = lower_32_bits(start);
834 		sgcl_data->bth = upper_32_bits(start);
835 	} else {
836 		u32 hi, lo;
837 
838 		hi = upper_32_bits(sgi->basetime);
839 		lo = lower_32_bits(sgi->basetime);
840 		sgcl_data->bth = hi;
841 		sgcl_data->btl = lo;
842 	}
843 
844 	err = enetc_send_cmd(priv->si, &cbd);
845 
846 exit:
847 	kfree(sgcl_data);
848 
849 	return err;
850 }
851 
852 static int enetc_flowmeter_hw_set(struct enetc_ndev_priv *priv,
853 				  struct enetc_psfp_meter *fmi,
854 				  u8 enable)
855 {
856 	struct enetc_cbd cbd = { .cmd = 0 };
857 	struct fmi_conf *fmi_config;
858 	u64 temp = 0;
859 
860 	cbd.index = cpu_to_le16((u16)fmi->index);
861 	cbd.cls = BDCR_CMD_FLOW_METER;
862 	cbd.status_flags = 0x80;
863 
864 	if (!enable)
865 		return enetc_send_cmd(priv->si, &cbd);
866 
867 	fmi_config = &cbd.fmi_conf;
868 	fmi_config->en = 0x80;
869 
870 	if (fmi->cir) {
871 		temp = (u64)8000 * fmi->cir;
872 		temp = div_u64(temp, 3725);
873 	}
874 
875 	fmi_config->cir = cpu_to_le32((u32)temp);
876 	fmi_config->cbs = cpu_to_le32(fmi->cbs);
877 
878 	/* Default for eir ebs disable */
879 	fmi_config->eir = 0;
880 	fmi_config->ebs = 0;
881 
882 	/* Default:
883 	 * mark red disable
884 	 * drop on yellow disable
885 	 * color mode disable
886 	 * couple flag disable
887 	 */
888 	fmi_config->conf = 0;
889 
890 	return enetc_send_cmd(priv->si, &cbd);
891 }
892 
893 static struct enetc_stream_filter *enetc_get_stream_by_index(u32 index)
894 {
895 	struct enetc_stream_filter *f;
896 
897 	hlist_for_each_entry(f, &epsfp.stream_list, node)
898 		if (f->sid.index == index)
899 			return f;
900 
901 	return NULL;
902 }
903 
904 static struct enetc_psfp_gate *enetc_get_gate_by_index(u32 index)
905 {
906 	struct enetc_psfp_gate *g;
907 
908 	hlist_for_each_entry(g, &epsfp.psfp_gate_list, node)
909 		if (g->index == index)
910 			return g;
911 
912 	return NULL;
913 }
914 
915 static struct enetc_psfp_filter *enetc_get_filter_by_index(u32 index)
916 {
917 	struct enetc_psfp_filter *s;
918 
919 	hlist_for_each_entry(s, &epsfp.psfp_filter_list, node)
920 		if (s->index == index)
921 			return s;
922 
923 	return NULL;
924 }
925 
926 static struct enetc_psfp_meter *enetc_get_meter_by_index(u32 index)
927 {
928 	struct enetc_psfp_meter *m;
929 
930 	hlist_for_each_entry(m, &epsfp.psfp_meter_list, node)
931 		if (m->index == index)
932 			return m;
933 
934 	return NULL;
935 }
936 
937 static struct enetc_psfp_filter
938 	*enetc_psfp_check_sfi(struct enetc_psfp_filter *sfi)
939 {
940 	struct enetc_psfp_filter *s;
941 
942 	hlist_for_each_entry(s, &epsfp.psfp_filter_list, node)
943 		if (s->gate_id == sfi->gate_id &&
944 		    s->prio == sfi->prio &&
945 		    s->maxsdu == sfi->maxsdu &&
946 		    s->meter_id == sfi->meter_id)
947 			return s;
948 
949 	return NULL;
950 }
951 
952 static int enetc_get_free_index(struct enetc_ndev_priv *priv)
953 {
954 	u32 max_size = priv->psfp_cap.max_psfp_filter;
955 	unsigned long index;
956 
957 	index = find_first_zero_bit(epsfp.psfp_sfi_bitmap, max_size);
958 	if (index == max_size)
959 		return -1;
960 
961 	return index;
962 }
963 
964 static void stream_filter_unref(struct enetc_ndev_priv *priv, u32 index)
965 {
966 	struct enetc_psfp_filter *sfi;
967 	u8 z;
968 
969 	sfi = enetc_get_filter_by_index(index);
970 	WARN_ON(!sfi);
971 	z = refcount_dec_and_test(&sfi->refcount);
972 
973 	if (z) {
974 		enetc_streamfilter_hw_set(priv, sfi, false);
975 		hlist_del(&sfi->node);
976 		kfree(sfi);
977 		clear_bit(index, epsfp.psfp_sfi_bitmap);
978 	}
979 }
980 
981 static void stream_gate_unref(struct enetc_ndev_priv *priv, u32 index)
982 {
983 	struct enetc_psfp_gate *sgi;
984 	u8 z;
985 
986 	sgi = enetc_get_gate_by_index(index);
987 	WARN_ON(!sgi);
988 	z = refcount_dec_and_test(&sgi->refcount);
989 	if (z) {
990 		enetc_streamgate_hw_set(priv, sgi, false);
991 		hlist_del(&sgi->node);
992 		kfree(sgi);
993 	}
994 }
995 
996 static void flow_meter_unref(struct enetc_ndev_priv *priv, u32 index)
997 {
998 	struct enetc_psfp_meter *fmi;
999 	u8 z;
1000 
1001 	fmi = enetc_get_meter_by_index(index);
1002 	WARN_ON(!fmi);
1003 	z = refcount_dec_and_test(&fmi->refcount);
1004 	if (z) {
1005 		enetc_flowmeter_hw_set(priv, fmi, false);
1006 		hlist_del(&fmi->node);
1007 		kfree(fmi);
1008 	}
1009 }
1010 
1011 static void remove_one_chain(struct enetc_ndev_priv *priv,
1012 			     struct enetc_stream_filter *filter)
1013 {
1014 	if (filter->flags & ENETC_PSFP_FLAGS_FMI)
1015 		flow_meter_unref(priv, filter->fmi_index);
1016 
1017 	stream_gate_unref(priv, filter->sgi_index);
1018 	stream_filter_unref(priv, filter->sfi_index);
1019 
1020 	hlist_del(&filter->node);
1021 	kfree(filter);
1022 }
1023 
1024 static int enetc_psfp_hw_set(struct enetc_ndev_priv *priv,
1025 			     struct enetc_streamid *sid,
1026 			     struct enetc_psfp_filter *sfi,
1027 			     struct enetc_psfp_gate *sgi,
1028 			     struct enetc_psfp_meter *fmi)
1029 {
1030 	int err;
1031 
1032 	err = enetc_streamid_hw_set(priv, sid, true);
1033 	if (err)
1034 		return err;
1035 
1036 	if (sfi) {
1037 		err = enetc_streamfilter_hw_set(priv, sfi, true);
1038 		if (err)
1039 			goto revert_sid;
1040 	}
1041 
1042 	err = enetc_streamgate_hw_set(priv, sgi, true);
1043 	if (err)
1044 		goto revert_sfi;
1045 
1046 	if (fmi) {
1047 		err = enetc_flowmeter_hw_set(priv, fmi, true);
1048 		if (err)
1049 			goto revert_sgi;
1050 	}
1051 
1052 	return 0;
1053 
1054 revert_sgi:
1055 	enetc_streamgate_hw_set(priv, sgi, false);
1056 revert_sfi:
1057 	if (sfi)
1058 		enetc_streamfilter_hw_set(priv, sfi, false);
1059 revert_sid:
1060 	enetc_streamid_hw_set(priv, sid, false);
1061 	return err;
1062 }
1063 
1064 static struct actions_fwd *enetc_check_flow_actions(u64 acts,
1065 						    unsigned int inputkeys)
1066 {
1067 	int i;
1068 
1069 	for (i = 0; i < ARRAY_SIZE(enetc_act_fwd); i++)
1070 		if (acts == enetc_act_fwd[i].actions &&
1071 		    inputkeys & enetc_act_fwd[i].keys)
1072 			return &enetc_act_fwd[i];
1073 
1074 	return NULL;
1075 }
1076 
1077 static int enetc_psfp_parse_clsflower(struct enetc_ndev_priv *priv,
1078 				      struct flow_cls_offload *f)
1079 {
1080 	struct flow_action_entry *entryg = NULL, *entryp = NULL;
1081 	struct flow_rule *rule = flow_cls_offload_flow_rule(f);
1082 	struct netlink_ext_ack *extack = f->common.extack;
1083 	struct enetc_stream_filter *filter, *old_filter;
1084 	struct enetc_psfp_meter *fmi = NULL, *old_fmi;
1085 	struct enetc_psfp_filter *sfi, *old_sfi;
1086 	struct enetc_psfp_gate *sgi, *old_sgi;
1087 	struct flow_action_entry *entry;
1088 	struct action_gate_entry *e;
1089 	u8 sfi_overwrite = 0;
1090 	int entries_size;
1091 	int i, err;
1092 
1093 	if (f->common.chain_index >= priv->psfp_cap.max_streamid) {
1094 		NL_SET_ERR_MSG_MOD(extack, "No Stream identify resource!");
1095 		return -ENOSPC;
1096 	}
1097 
1098 	flow_action_for_each(i, entry, &rule->action)
1099 		if (entry->id == FLOW_ACTION_GATE)
1100 			entryg = entry;
1101 		else if (entry->id == FLOW_ACTION_POLICE)
1102 			entryp = entry;
1103 
1104 	/* Not support without gate action */
1105 	if (!entryg)
1106 		return -EINVAL;
1107 
1108 	filter = kzalloc(sizeof(*filter), GFP_KERNEL);
1109 	if (!filter)
1110 		return -ENOMEM;
1111 
1112 	filter->sid.index = f->common.chain_index;
1113 
1114 	if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_ETH_ADDRS)) {
1115 		struct flow_match_eth_addrs match;
1116 
1117 		flow_rule_match_eth_addrs(rule, &match);
1118 
1119 		if (!is_zero_ether_addr(match.mask->dst) &&
1120 		    !is_zero_ether_addr(match.mask->src)) {
1121 			NL_SET_ERR_MSG_MOD(extack,
1122 					   "Cannot match on both source and destination MAC");
1123 			err = -EINVAL;
1124 			goto free_filter;
1125 		}
1126 
1127 		if (!is_zero_ether_addr(match.mask->dst)) {
1128 			if (!is_broadcast_ether_addr(match.mask->dst)) {
1129 				NL_SET_ERR_MSG_MOD(extack,
1130 						   "Masked matching on destination MAC not supported");
1131 				err = -EINVAL;
1132 				goto free_filter;
1133 			}
1134 			ether_addr_copy(filter->sid.dst_mac, match.key->dst);
1135 			filter->sid.filtertype = STREAMID_TYPE_NULL;
1136 		}
1137 
1138 		if (!is_zero_ether_addr(match.mask->src)) {
1139 			if (!is_broadcast_ether_addr(match.mask->src)) {
1140 				NL_SET_ERR_MSG_MOD(extack,
1141 						   "Masked matching on source MAC not supported");
1142 				err = -EINVAL;
1143 				goto free_filter;
1144 			}
1145 			ether_addr_copy(filter->sid.src_mac, match.key->src);
1146 			filter->sid.filtertype = STREAMID_TYPE_SMAC;
1147 		}
1148 	} else {
1149 		NL_SET_ERR_MSG_MOD(extack, "Unsupported, must include ETH_ADDRS");
1150 		err = -EINVAL;
1151 		goto free_filter;
1152 	}
1153 
1154 	if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_VLAN)) {
1155 		struct flow_match_vlan match;
1156 
1157 		flow_rule_match_vlan(rule, &match);
1158 		if (match.mask->vlan_priority) {
1159 			if (match.mask->vlan_priority !=
1160 			    (VLAN_PRIO_MASK >> VLAN_PRIO_SHIFT)) {
1161 				NL_SET_ERR_MSG_MOD(extack, "Only full mask is supported for VLAN priority");
1162 				err = -EINVAL;
1163 				goto free_filter;
1164 			}
1165 		}
1166 
1167 		if (match.mask->vlan_id) {
1168 			if (match.mask->vlan_id != VLAN_VID_MASK) {
1169 				NL_SET_ERR_MSG_MOD(extack, "Only full mask is supported for VLAN id");
1170 				err = -EINVAL;
1171 				goto free_filter;
1172 			}
1173 
1174 			filter->sid.vid = match.key->vlan_id;
1175 			if (!filter->sid.vid)
1176 				filter->sid.tagged = STREAMID_VLAN_UNTAGGED;
1177 			else
1178 				filter->sid.tagged = STREAMID_VLAN_TAGGED;
1179 		}
1180 	} else {
1181 		filter->sid.tagged = STREAMID_VLAN_ALL;
1182 	}
1183 
1184 	/* parsing gate action */
1185 	if (entryg->gate.index >= priv->psfp_cap.max_psfp_gate) {
1186 		NL_SET_ERR_MSG_MOD(extack, "No Stream Gate resource!");
1187 		err = -ENOSPC;
1188 		goto free_filter;
1189 	}
1190 
1191 	if (entryg->gate.num_entries >= priv->psfp_cap.max_psfp_gatelist) {
1192 		NL_SET_ERR_MSG_MOD(extack, "No Stream Gate resource!");
1193 		err = -ENOSPC;
1194 		goto free_filter;
1195 	}
1196 
1197 	entries_size = struct_size(sgi, entries, entryg->gate.num_entries);
1198 	sgi = kzalloc(entries_size, GFP_KERNEL);
1199 	if (!sgi) {
1200 		err = -ENOMEM;
1201 		goto free_filter;
1202 	}
1203 
1204 	refcount_set(&sgi->refcount, 1);
1205 	sgi->index = entryg->gate.index;
1206 	sgi->init_ipv = entryg->gate.prio;
1207 	sgi->basetime = entryg->gate.basetime;
1208 	sgi->cycletime = entryg->gate.cycletime;
1209 	sgi->num_entries = entryg->gate.num_entries;
1210 
1211 	e = sgi->entries;
1212 	for (i = 0; i < entryg->gate.num_entries; i++) {
1213 		e[i].gate_state = entryg->gate.entries[i].gate_state;
1214 		e[i].interval = entryg->gate.entries[i].interval;
1215 		e[i].ipv = entryg->gate.entries[i].ipv;
1216 		e[i].maxoctets = entryg->gate.entries[i].maxoctets;
1217 	}
1218 
1219 	filter->sgi_index = sgi->index;
1220 
1221 	sfi = kzalloc(sizeof(*sfi), GFP_KERNEL);
1222 	if (!sfi) {
1223 		err = -ENOMEM;
1224 		goto free_gate;
1225 	}
1226 
1227 	refcount_set(&sfi->refcount, 1);
1228 	sfi->gate_id = sgi->index;
1229 	sfi->meter_id = ENETC_PSFP_WILDCARD;
1230 
1231 	/* Flow meter and max frame size */
1232 	if (entryp) {
1233 		if (entryp->police.rate_pkt_ps) {
1234 			NL_SET_ERR_MSG_MOD(extack, "QoS offload not support packets per second");
1235 			err = -EOPNOTSUPP;
1236 			goto free_sfi;
1237 		}
1238 		if (entryp->police.burst) {
1239 			fmi = kzalloc(sizeof(*fmi), GFP_KERNEL);
1240 			if (!fmi) {
1241 				err = -ENOMEM;
1242 				goto free_sfi;
1243 			}
1244 			refcount_set(&fmi->refcount, 1);
1245 			fmi->cir = entryp->police.rate_bytes_ps;
1246 			fmi->cbs = entryp->police.burst;
1247 			fmi->index = entryp->police.index;
1248 			filter->flags |= ENETC_PSFP_FLAGS_FMI;
1249 			filter->fmi_index = fmi->index;
1250 			sfi->meter_id = fmi->index;
1251 		}
1252 
1253 		if (entryp->police.mtu)
1254 			sfi->maxsdu = entryp->police.mtu;
1255 	}
1256 
1257 	/* prio ref the filter prio */
1258 	if (f->common.prio && f->common.prio <= BIT(3))
1259 		sfi->prio = f->common.prio - 1;
1260 	else
1261 		sfi->prio = ENETC_PSFP_WILDCARD;
1262 
1263 	old_sfi = enetc_psfp_check_sfi(sfi);
1264 	if (!old_sfi) {
1265 		int index;
1266 
1267 		index = enetc_get_free_index(priv);
1268 		if (sfi->handle < 0) {
1269 			NL_SET_ERR_MSG_MOD(extack, "No Stream Filter resource!");
1270 			err = -ENOSPC;
1271 			goto free_fmi;
1272 		}
1273 
1274 		sfi->index = index;
1275 		sfi->handle = index + HANDLE_OFFSET;
1276 		/* Update the stream filter handle also */
1277 		filter->sid.handle = sfi->handle;
1278 		filter->sfi_index = sfi->index;
1279 		sfi_overwrite = 0;
1280 	} else {
1281 		filter->sfi_index = old_sfi->index;
1282 		filter->sid.handle = old_sfi->handle;
1283 		sfi_overwrite = 1;
1284 	}
1285 
1286 	err = enetc_psfp_hw_set(priv, &filter->sid,
1287 				sfi_overwrite ? NULL : sfi, sgi, fmi);
1288 	if (err)
1289 		goto free_fmi;
1290 
1291 	spin_lock(&epsfp.psfp_lock);
1292 	if (filter->flags & ENETC_PSFP_FLAGS_FMI) {
1293 		old_fmi = enetc_get_meter_by_index(filter->fmi_index);
1294 		if (old_fmi) {
1295 			fmi->refcount = old_fmi->refcount;
1296 			refcount_set(&fmi->refcount,
1297 				     refcount_read(&old_fmi->refcount) + 1);
1298 			hlist_del(&old_fmi->node);
1299 			kfree(old_fmi);
1300 		}
1301 		hlist_add_head(&fmi->node, &epsfp.psfp_meter_list);
1302 	}
1303 
1304 	/* Remove the old node if exist and update with a new node */
1305 	old_sgi = enetc_get_gate_by_index(filter->sgi_index);
1306 	if (old_sgi) {
1307 		refcount_set(&sgi->refcount,
1308 			     refcount_read(&old_sgi->refcount) + 1);
1309 		hlist_del(&old_sgi->node);
1310 		kfree(old_sgi);
1311 	}
1312 
1313 	hlist_add_head(&sgi->node, &epsfp.psfp_gate_list);
1314 
1315 	if (!old_sfi) {
1316 		hlist_add_head(&sfi->node, &epsfp.psfp_filter_list);
1317 		set_bit(sfi->index, epsfp.psfp_sfi_bitmap);
1318 	} else {
1319 		kfree(sfi);
1320 		refcount_inc(&old_sfi->refcount);
1321 	}
1322 
1323 	old_filter = enetc_get_stream_by_index(filter->sid.index);
1324 	if (old_filter)
1325 		remove_one_chain(priv, old_filter);
1326 
1327 	filter->stats.lastused = jiffies;
1328 	hlist_add_head(&filter->node, &epsfp.stream_list);
1329 
1330 	spin_unlock(&epsfp.psfp_lock);
1331 
1332 	return 0;
1333 
1334 free_fmi:
1335 	kfree(fmi);
1336 free_sfi:
1337 	kfree(sfi);
1338 free_gate:
1339 	kfree(sgi);
1340 free_filter:
1341 	kfree(filter);
1342 
1343 	return err;
1344 }
1345 
1346 static int enetc_config_clsflower(struct enetc_ndev_priv *priv,
1347 				  struct flow_cls_offload *cls_flower)
1348 {
1349 	struct flow_rule *rule = flow_cls_offload_flow_rule(cls_flower);
1350 	struct netlink_ext_ack *extack = cls_flower->common.extack;
1351 	struct flow_dissector *dissector = rule->match.dissector;
1352 	struct flow_action *action = &rule->action;
1353 	struct flow_action_entry *entry;
1354 	struct actions_fwd *fwd;
1355 	u64 actions = 0;
1356 	int i, err;
1357 
1358 	if (!flow_action_has_entries(action)) {
1359 		NL_SET_ERR_MSG_MOD(extack, "At least one action is needed");
1360 		return -EINVAL;
1361 	}
1362 
1363 	flow_action_for_each(i, entry, action)
1364 		actions |= BIT(entry->id);
1365 
1366 	fwd = enetc_check_flow_actions(actions, dissector->used_keys);
1367 	if (!fwd) {
1368 		NL_SET_ERR_MSG_MOD(extack, "Unsupported filter type!");
1369 		return -EOPNOTSUPP;
1370 	}
1371 
1372 	if (fwd->output & FILTER_ACTION_TYPE_PSFP) {
1373 		err = enetc_psfp_parse_clsflower(priv, cls_flower);
1374 		if (err) {
1375 			NL_SET_ERR_MSG_MOD(extack, "Invalid PSFP inputs");
1376 			return err;
1377 		}
1378 	} else {
1379 		NL_SET_ERR_MSG_MOD(extack, "Unsupported actions");
1380 		return -EOPNOTSUPP;
1381 	}
1382 
1383 	return 0;
1384 }
1385 
1386 static int enetc_psfp_destroy_clsflower(struct enetc_ndev_priv *priv,
1387 					struct flow_cls_offload *f)
1388 {
1389 	struct enetc_stream_filter *filter;
1390 	struct netlink_ext_ack *extack = f->common.extack;
1391 	int err;
1392 
1393 	if (f->common.chain_index >= priv->psfp_cap.max_streamid) {
1394 		NL_SET_ERR_MSG_MOD(extack, "No Stream identify resource!");
1395 		return -ENOSPC;
1396 	}
1397 
1398 	filter = enetc_get_stream_by_index(f->common.chain_index);
1399 	if (!filter)
1400 		return -EINVAL;
1401 
1402 	err = enetc_streamid_hw_set(priv, &filter->sid, false);
1403 	if (err)
1404 		return err;
1405 
1406 	remove_one_chain(priv, filter);
1407 
1408 	return 0;
1409 }
1410 
1411 static int enetc_destroy_clsflower(struct enetc_ndev_priv *priv,
1412 				   struct flow_cls_offload *f)
1413 {
1414 	return enetc_psfp_destroy_clsflower(priv, f);
1415 }
1416 
1417 static int enetc_psfp_get_stats(struct enetc_ndev_priv *priv,
1418 				struct flow_cls_offload *f)
1419 {
1420 	struct psfp_streamfilter_counters counters = {};
1421 	struct enetc_stream_filter *filter;
1422 	struct flow_stats stats = {};
1423 	int err;
1424 
1425 	filter = enetc_get_stream_by_index(f->common.chain_index);
1426 	if (!filter)
1427 		return -EINVAL;
1428 
1429 	err = enetc_streamcounter_hw_get(priv, filter->sfi_index, &counters);
1430 	if (err)
1431 		return -EINVAL;
1432 
1433 	spin_lock(&epsfp.psfp_lock);
1434 	stats.pkts = counters.matching_frames_count +
1435 		     counters.not_passing_sdu_count -
1436 		     filter->stats.pkts;
1437 	stats.drops = counters.not_passing_frames_count +
1438 		      counters.not_passing_sdu_count +
1439 		      counters.red_frames_count -
1440 		      filter->stats.drops;
1441 	stats.lastused = filter->stats.lastused;
1442 	filter->stats.pkts += stats.pkts;
1443 	filter->stats.drops += stats.drops;
1444 	spin_unlock(&epsfp.psfp_lock);
1445 
1446 	flow_stats_update(&f->stats, 0x0, stats.pkts, stats.drops,
1447 			  stats.lastused, FLOW_ACTION_HW_STATS_DELAYED);
1448 
1449 	return 0;
1450 }
1451 
1452 static int enetc_setup_tc_cls_flower(struct enetc_ndev_priv *priv,
1453 				     struct flow_cls_offload *cls_flower)
1454 {
1455 	switch (cls_flower->command) {
1456 	case FLOW_CLS_REPLACE:
1457 		return enetc_config_clsflower(priv, cls_flower);
1458 	case FLOW_CLS_DESTROY:
1459 		return enetc_destroy_clsflower(priv, cls_flower);
1460 	case FLOW_CLS_STATS:
1461 		return enetc_psfp_get_stats(priv, cls_flower);
1462 	default:
1463 		return -EOPNOTSUPP;
1464 	}
1465 }
1466 
1467 static inline void clean_psfp_sfi_bitmap(void)
1468 {
1469 	bitmap_free(epsfp.psfp_sfi_bitmap);
1470 	epsfp.psfp_sfi_bitmap = NULL;
1471 }
1472 
1473 static void clean_stream_list(void)
1474 {
1475 	struct enetc_stream_filter *s;
1476 	struct hlist_node *tmp;
1477 
1478 	hlist_for_each_entry_safe(s, tmp, &epsfp.stream_list, node) {
1479 		hlist_del(&s->node);
1480 		kfree(s);
1481 	}
1482 }
1483 
1484 static void clean_sfi_list(void)
1485 {
1486 	struct enetc_psfp_filter *sfi;
1487 	struct hlist_node *tmp;
1488 
1489 	hlist_for_each_entry_safe(sfi, tmp, &epsfp.psfp_filter_list, node) {
1490 		hlist_del(&sfi->node);
1491 		kfree(sfi);
1492 	}
1493 }
1494 
1495 static void clean_sgi_list(void)
1496 {
1497 	struct enetc_psfp_gate *sgi;
1498 	struct hlist_node *tmp;
1499 
1500 	hlist_for_each_entry_safe(sgi, tmp, &epsfp.psfp_gate_list, node) {
1501 		hlist_del(&sgi->node);
1502 		kfree(sgi);
1503 	}
1504 }
1505 
1506 static void clean_psfp_all(void)
1507 {
1508 	/* Disable all list nodes and free all memory */
1509 	clean_sfi_list();
1510 	clean_sgi_list();
1511 	clean_stream_list();
1512 	epsfp.dev_bitmap = 0;
1513 	clean_psfp_sfi_bitmap();
1514 }
1515 
1516 int enetc_setup_tc_block_cb(enum tc_setup_type type, void *type_data,
1517 			    void *cb_priv)
1518 {
1519 	struct net_device *ndev = cb_priv;
1520 
1521 	if (!tc_can_offload(ndev))
1522 		return -EOPNOTSUPP;
1523 
1524 	switch (type) {
1525 	case TC_SETUP_CLSFLOWER:
1526 		return enetc_setup_tc_cls_flower(netdev_priv(ndev), type_data);
1527 	default:
1528 		return -EOPNOTSUPP;
1529 	}
1530 }
1531 
1532 int enetc_psfp_init(struct enetc_ndev_priv *priv)
1533 {
1534 	if (epsfp.psfp_sfi_bitmap)
1535 		return 0;
1536 
1537 	epsfp.psfp_sfi_bitmap = bitmap_zalloc(priv->psfp_cap.max_psfp_filter,
1538 					      GFP_KERNEL);
1539 	if (!epsfp.psfp_sfi_bitmap)
1540 		return -ENOMEM;
1541 
1542 	spin_lock_init(&epsfp.psfp_lock);
1543 
1544 	if (list_empty(&enetc_block_cb_list))
1545 		epsfp.dev_bitmap = 0;
1546 
1547 	return 0;
1548 }
1549 
1550 int enetc_psfp_clean(struct enetc_ndev_priv *priv)
1551 {
1552 	if (!list_empty(&enetc_block_cb_list))
1553 		return -EBUSY;
1554 
1555 	clean_psfp_all();
1556 
1557 	return 0;
1558 }
1559 
1560 int enetc_setup_tc_psfp(struct net_device *ndev, void *type_data)
1561 {
1562 	struct enetc_ndev_priv *priv = netdev_priv(ndev);
1563 	struct flow_block_offload *f = type_data;
1564 	int port, err;
1565 
1566 	err = flow_block_cb_setup_simple(f, &enetc_block_cb_list,
1567 					 enetc_setup_tc_block_cb,
1568 					 ndev, ndev, true);
1569 	if (err)
1570 		return err;
1571 
1572 	switch (f->command) {
1573 	case FLOW_BLOCK_BIND:
1574 		port = enetc_pf_to_port(priv->si->pdev);
1575 		if (port < 0)
1576 			return -EINVAL;
1577 
1578 		set_bit(port, &epsfp.dev_bitmap);
1579 		break;
1580 	case FLOW_BLOCK_UNBIND:
1581 		port = enetc_pf_to_port(priv->si->pdev);
1582 		if (port < 0)
1583 			return -EINVAL;
1584 
1585 		clear_bit(port, &epsfp.dev_bitmap);
1586 		if (!epsfp.dev_bitmap)
1587 			clean_psfp_all();
1588 		break;
1589 	}
1590 
1591 	return 0;
1592 }
1593