1
2
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
12static 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
18void 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
48static 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
80
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
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
141int 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
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
169static 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
174static 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
179int 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
198 if (tc != prio_top && tc != prio_next)
199 return -EOPNOTSUPP;
200
201 if (!cbs->enable) {
202
203
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
228
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
249
250
251
252
253
254
255
256
257
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
275
276
277
278 hi_credit_bit = max_interference_size * bw / 100;
279
280
281
282
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
290 enetc_port_wr(&si->hw, ENETC_PTCCBSR0(tc), bw | ENETC_CBSE);
291
292 return 0;
293}
294
295int 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
311 if (ndev->features & NETIF_F_CSUM_MASK)
312 return -EBUSY;
313
314
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
325enum streamid_type {
326 STREAMID_TYPE_RESERVED = 0,
327 STREAMID_TYPE_NULL,
328 STREAMID_TYPE_SMAC,
329};
330
331enum 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
341enum 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
348struct actions_fwd {
349 u64 actions;
350 u64 keys;
351 enum forward_type output;
352};
353
354struct 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
363struct 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
375struct 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
386struct 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
399
400
401
402struct 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
412struct 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
422struct 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;
430};
431
432static 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
445 {
446 BIT(FLOW_ACTION_DROP),
447 0,
448 FILTER_ACTION_TYPE_ACL
449 }
450};
451
452static struct enetc_psfp epsfp = {
453 .psfp_sfi_bitmap = NULL,
454};
455
456static LIST_HEAD(enetc_block_cb_list);
457
458
459static 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 err;
469
470 if (sid->index >= priv->psfp_cap.max_streamid)
471 return -EINVAL;
472
473 if (sid->filtertype != STREAMID_TYPE_NULL &&
474 sid->filtertype != STREAMID_TYPE_SMAC)
475 return -EOPNOTSUPP;
476
477
478 cbd.index = cpu_to_le16((u16)sid->index);
479 cbd.cls = BDCR_CMD_STREAM_IDENTIFY;
480 cbd.status_flags = 0;
481
482 data_size = sizeof(struct streamid_data);
483 si_data = kzalloc(data_size, __GFP_DMA | GFP_KERNEL);
484 cbd.length = cpu_to_le16(data_size);
485
486 dma = dma_map_single(&priv->si->pdev->dev, si_data,
487 data_size, DMA_FROM_DEVICE);
488 if (dma_mapping_error(&priv->si->pdev->dev, dma)) {
489 netdev_err(priv->si->ndev, "DMA mapping failed!\n");
490 kfree(si_data);
491 return -ENOMEM;
492 }
493
494 cbd.addr[0] = cpu_to_le32(lower_32_bits(dma));
495 cbd.addr[1] = cpu_to_le32(upper_32_bits(dma));
496 eth_broadcast_addr(si_data->dmac);
497 si_data->vid_vidm_tg = (ENETC_CBDR_SID_VID_MASK
498 + ((0x3 << 14) | ENETC_CBDR_SID_VIDM));
499
500 si_conf = &cbd.sid_set;
501
502 si_conf->iports = cpu_to_le32(1 << enetc_pf_to_port(priv->si->pdev));
503 si_conf->id_type = 1;
504 si_conf->oui[2] = 0x0;
505 si_conf->oui[1] = 0x80;
506 si_conf->oui[0] = 0xC2;
507
508 err = enetc_send_cmd(priv->si, &cbd);
509 if (err)
510 return -EINVAL;
511
512 if (!enable) {
513 kfree(si_data);
514 return 0;
515 }
516
517
518 memset(&cbd, 0, sizeof(cbd));
519
520 cbd.index = cpu_to_le16((u16)sid->index);
521 cbd.cmd = 0;
522 cbd.cls = BDCR_CMD_STREAM_IDENTIFY;
523 cbd.status_flags = 0;
524
525 si_conf->en = 0x80;
526 si_conf->stream_handle = cpu_to_le32(sid->handle);
527 si_conf->iports = cpu_to_le32(1 << enetc_pf_to_port(priv->si->pdev));
528 si_conf->id_type = sid->filtertype;
529 si_conf->oui[2] = 0x0;
530 si_conf->oui[1] = 0x80;
531 si_conf->oui[0] = 0xC2;
532
533 memset(si_data, 0, data_size);
534
535 cbd.length = cpu_to_le16(data_size);
536
537 cbd.addr[0] = cpu_to_le32(lower_32_bits(dma));
538 cbd.addr[1] = cpu_to_le32(upper_32_bits(dma));
539
540
541
542
543
544
545 if (si_conf->id_type == STREAMID_TYPE_NULL) {
546 ether_addr_copy(si_data->dmac, sid->dst_mac);
547 si_data->vid_vidm_tg = (sid->vid & ENETC_CBDR_SID_VID_MASK) +
548 ((((u16)(sid->tagged) & 0x3) << 14)
549 | ENETC_CBDR_SID_VIDM);
550 } else if (si_conf->id_type == STREAMID_TYPE_SMAC) {
551 ether_addr_copy(si_data->smac, sid->src_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 }
556
557 err = enetc_send_cmd(priv->si, &cbd);
558 kfree(si_data);
559
560 return err;
561}
562
563
564static int enetc_streamfilter_hw_set(struct enetc_ndev_priv *priv,
565 struct enetc_psfp_filter *sfi,
566 u8 enable)
567{
568 struct enetc_cbd cbd = {.cmd = 0};
569 struct sfi_conf *sfi_config;
570
571 cbd.index = cpu_to_le16(sfi->index);
572 cbd.cls = BDCR_CMD_STREAM_FILTER;
573 cbd.status_flags = 0x80;
574 cbd.length = cpu_to_le16(1);
575
576 sfi_config = &cbd.sfi_conf;
577 if (!enable)
578 goto exit;
579
580 sfi_config->en = 0x80;
581
582 if (sfi->handle >= 0) {
583 sfi_config->stream_handle =
584 cpu_to_le32(sfi->handle);
585 sfi_config->sthm |= 0x80;
586 }
587
588 sfi_config->sg_inst_table_index = cpu_to_le16(sfi->gate_id);
589 sfi_config->input_ports =
590 cpu_to_le32(1 << enetc_pf_to_port(priv->si->pdev));
591
592
593
594
595 if (sfi->prio >= 0)
596 sfi_config->multi |= (sfi->prio & 0x7) | 0x8;
597
598
599
600
601
602 if (sfi->maxsdu) {
603 sfi_config->msdu =
604 cpu_to_le16(sfi->maxsdu);
605 sfi_config->multi |= 0x40;
606 }
607
608 if (sfi->meter_id >= 0) {
609 sfi_config->fm_inst_table_index = cpu_to_le16(sfi->meter_id);
610 sfi_config->multi |= 0x80;
611 }
612
613exit:
614 return enetc_send_cmd(priv->si, &cbd);
615}
616
617static int enetc_streamcounter_hw_get(struct enetc_ndev_priv *priv,
618 u32 index,
619 struct psfp_streamfilter_counters *cnt)
620{
621 struct enetc_cbd cbd = { .cmd = 2 };
622 struct sfi_counter_data *data_buf;
623 dma_addr_t dma;
624 u16 data_size;
625 int err;
626
627 cbd.index = cpu_to_le16((u16)index);
628 cbd.cmd = 2;
629 cbd.cls = BDCR_CMD_STREAM_FILTER;
630 cbd.status_flags = 0;
631
632 data_size = sizeof(struct sfi_counter_data);
633 data_buf = kzalloc(data_size, __GFP_DMA | GFP_KERNEL);
634 if (!data_buf)
635 return -ENOMEM;
636
637 dma = dma_map_single(&priv->si->pdev->dev, data_buf,
638 data_size, DMA_FROM_DEVICE);
639 if (dma_mapping_error(&priv->si->pdev->dev, dma)) {
640 netdev_err(priv->si->ndev, "DMA mapping failed!\n");
641 err = -ENOMEM;
642 goto exit;
643 }
644 cbd.addr[0] = cpu_to_le32(lower_32_bits(dma));
645 cbd.addr[1] = cpu_to_le32(upper_32_bits(dma));
646
647 cbd.length = cpu_to_le16(data_size);
648
649 err = enetc_send_cmd(priv->si, &cbd);
650 if (err)
651 goto exit;
652
653 cnt->matching_frames_count = ((u64)data_buf->matchh << 32) +
654 data_buf->matchl;
655
656 cnt->not_passing_sdu_count = ((u64)data_buf->msdu_droph << 32) +
657 data_buf->msdu_dropl;
658
659 cnt->passing_sdu_count = cnt->matching_frames_count
660 - cnt->not_passing_sdu_count;
661
662 cnt->not_passing_frames_count =
663 ((u64)data_buf->stream_gate_droph << 32) +
664 data_buf->stream_gate_dropl;
665
666 cnt->passing_frames_count = cnt->matching_frames_count -
667 cnt->not_passing_sdu_count -
668 cnt->not_passing_frames_count;
669
670 cnt->red_frames_count = ((u64)data_buf->flow_meter_droph << 32) +
671 data_buf->flow_meter_dropl;
672
673exit:
674 kfree(data_buf);
675 return err;
676}
677
678static u64 get_ptp_now(struct enetc_hw *hw)
679{
680 u64 now_lo, now_hi, now;
681
682 now_lo = enetc_rd(hw, ENETC_SICTR0);
683 now_hi = enetc_rd(hw, ENETC_SICTR1);
684 now = now_lo | now_hi << 32;
685
686 return now;
687}
688
689static int get_start_ns(u64 now, u64 cycle, u64 *start)
690{
691 u64 n;
692
693 if (!cycle)
694 return -EFAULT;
695
696 n = div64_u64(now, cycle);
697
698 *start = (n + 1) * cycle;
699
700 return 0;
701}
702
703
704static int enetc_streamgate_hw_set(struct enetc_ndev_priv *priv,
705 struct enetc_psfp_gate *sgi,
706 u8 enable)
707{
708 struct enetc_cbd cbd = { .cmd = 0 };
709 struct sgi_table *sgi_config;
710 struct sgcl_conf *sgcl_config;
711 struct sgcl_data *sgcl_data;
712 struct sgce *sgce;
713 dma_addr_t dma;
714 u16 data_size;
715 int err, i;
716 u64 now;
717
718 cbd.index = cpu_to_le16(sgi->index);
719 cbd.cmd = 0;
720 cbd.cls = BDCR_CMD_STREAM_GCL;
721 cbd.status_flags = 0x80;
722
723
724 if (!enable)
725 return enetc_send_cmd(priv->si, &cbd);
726
727 if (!sgi->num_entries)
728 return 0;
729
730 if (sgi->num_entries > priv->psfp_cap.max_psfp_gatelist ||
731 !sgi->cycletime)
732 return -EINVAL;
733
734
735 sgi_config = &cbd.sgi_table;
736
737
738 sgi_config->ocgtst = 0x80;
739
740 sgi_config->oipv = (sgi->init_ipv < 0) ?
741 0x0 : ((sgi->init_ipv & 0x7) | 0x8);
742
743 sgi_config->en = 0x80;
744
745
746 err = enetc_send_cmd(priv->si, &cbd);
747 if (err)
748 return -EINVAL;
749
750 memset(&cbd, 0, sizeof(cbd));
751
752 cbd.index = cpu_to_le16(sgi->index);
753 cbd.cmd = 1;
754 cbd.cls = BDCR_CMD_STREAM_GCL;
755 cbd.status_flags = 0;
756
757 sgcl_config = &cbd.sgcl_conf;
758
759 sgcl_config->acl_len = (sgi->num_entries - 1) & 0x3;
760
761 data_size = struct_size(sgcl_data, sgcl, sgi->num_entries);
762
763 sgcl_data = kzalloc(data_size, __GFP_DMA | GFP_KERNEL);
764 if (!sgcl_data)
765 return -ENOMEM;
766
767 cbd.length = cpu_to_le16(data_size);
768
769 dma = dma_map_single(&priv->si->pdev->dev,
770 sgcl_data, data_size,
771 DMA_FROM_DEVICE);
772 if (dma_mapping_error(&priv->si->pdev->dev, dma)) {
773 netdev_err(priv->si->ndev, "DMA mapping failed!\n");
774 kfree(sgcl_data);
775 return -ENOMEM;
776 }
777
778 cbd.addr[0] = cpu_to_le32(lower_32_bits(dma));
779 cbd.addr[1] = cpu_to_le32(upper_32_bits(dma));
780
781 sgce = &sgcl_data->sgcl[0];
782
783 sgcl_config->agtst = 0x80;
784
785 sgcl_data->ct = sgi->cycletime;
786 sgcl_data->cte = sgi->cycletimext;
787
788 if (sgi->init_ipv >= 0)
789 sgcl_config->aipv = (sgi->init_ipv & 0x7) | 0x8;
790
791 for (i = 0; i < sgi->num_entries; i++) {
792 struct action_gate_entry *from = &sgi->entries[i];
793 struct sgce *to = &sgce[i];
794
795 if (from->gate_state)
796 to->multi |= 0x10;
797
798 if (from->ipv >= 0)
799 to->multi |= ((from->ipv & 0x7) << 5) | 0x08;
800
801 if (from->maxoctets >= 0) {
802 to->multi |= 0x01;
803 to->msdu[0] = from->maxoctets & 0xFF;
804 to->msdu[1] = (from->maxoctets >> 8) & 0xFF;
805 to->msdu[2] = (from->maxoctets >> 16) & 0xFF;
806 }
807
808 to->interval = from->interval;
809 }
810
811
812 now = get_ptp_now(&priv->si->hw);
813
814 if (sgi->basetime < now) {
815 u64 start;
816
817 err = get_start_ns(now, sgi->cycletime, &start);
818 if (err)
819 goto exit;
820 sgcl_data->btl = lower_32_bits(start);
821 sgcl_data->bth = upper_32_bits(start);
822 } else {
823 u32 hi, lo;
824
825 hi = upper_32_bits(sgi->basetime);
826 lo = lower_32_bits(sgi->basetime);
827 sgcl_data->bth = hi;
828 sgcl_data->btl = lo;
829 }
830
831 err = enetc_send_cmd(priv->si, &cbd);
832
833exit:
834 kfree(sgcl_data);
835
836 return err;
837}
838
839static int enetc_flowmeter_hw_set(struct enetc_ndev_priv *priv,
840 struct enetc_psfp_meter *fmi,
841 u8 enable)
842{
843 struct enetc_cbd cbd = { .cmd = 0 };
844 struct fmi_conf *fmi_config;
845 u64 temp = 0;
846
847 cbd.index = cpu_to_le16((u16)fmi->index);
848 cbd.cls = BDCR_CMD_FLOW_METER;
849 cbd.status_flags = 0x80;
850
851 if (!enable)
852 return enetc_send_cmd(priv->si, &cbd);
853
854 fmi_config = &cbd.fmi_conf;
855 fmi_config->en = 0x80;
856
857 if (fmi->cir) {
858 temp = (u64)8000 * fmi->cir;
859 temp = div_u64(temp, 3725);
860 }
861
862 fmi_config->cir = cpu_to_le32((u32)temp);
863 fmi_config->cbs = cpu_to_le32(fmi->cbs);
864
865
866 fmi_config->eir = 0;
867 fmi_config->ebs = 0;
868
869
870
871
872
873
874
875 fmi_config->conf = 0;
876
877 return enetc_send_cmd(priv->si, &cbd);
878}
879
880static struct enetc_stream_filter *enetc_get_stream_by_index(u32 index)
881{
882 struct enetc_stream_filter *f;
883
884 hlist_for_each_entry(f, &epsfp.stream_list, node)
885 if (f->sid.index == index)
886 return f;
887
888 return NULL;
889}
890
891static struct enetc_psfp_gate *enetc_get_gate_by_index(u32 index)
892{
893 struct enetc_psfp_gate *g;
894
895 hlist_for_each_entry(g, &epsfp.psfp_gate_list, node)
896 if (g->index == index)
897 return g;
898
899 return NULL;
900}
901
902static struct enetc_psfp_filter *enetc_get_filter_by_index(u32 index)
903{
904 struct enetc_psfp_filter *s;
905
906 hlist_for_each_entry(s, &epsfp.psfp_filter_list, node)
907 if (s->index == index)
908 return s;
909
910 return NULL;
911}
912
913static struct enetc_psfp_meter *enetc_get_meter_by_index(u32 index)
914{
915 struct enetc_psfp_meter *m;
916
917 hlist_for_each_entry(m, &epsfp.psfp_meter_list, node)
918 if (m->index == index)
919 return m;
920
921 return NULL;
922}
923
924static struct enetc_psfp_filter
925 *enetc_psfp_check_sfi(struct enetc_psfp_filter *sfi)
926{
927 struct enetc_psfp_filter *s;
928
929 hlist_for_each_entry(s, &epsfp.psfp_filter_list, node)
930 if (s->gate_id == sfi->gate_id &&
931 s->prio == sfi->prio &&
932 s->maxsdu == sfi->maxsdu &&
933 s->meter_id == sfi->meter_id)
934 return s;
935
936 return NULL;
937}
938
939static int enetc_get_free_index(struct enetc_ndev_priv *priv)
940{
941 u32 max_size = priv->psfp_cap.max_psfp_filter;
942 unsigned long index;
943
944 index = find_first_zero_bit(epsfp.psfp_sfi_bitmap, max_size);
945 if (index == max_size)
946 return -1;
947
948 return index;
949}
950
951static void stream_filter_unref(struct enetc_ndev_priv *priv, u32 index)
952{
953 struct enetc_psfp_filter *sfi;
954 u8 z;
955
956 sfi = enetc_get_filter_by_index(index);
957 WARN_ON(!sfi);
958 z = refcount_dec_and_test(&sfi->refcount);
959
960 if (z) {
961 enetc_streamfilter_hw_set(priv, sfi, false);
962 hlist_del(&sfi->node);
963 kfree(sfi);
964 clear_bit(index, epsfp.psfp_sfi_bitmap);
965 }
966}
967
968static void stream_gate_unref(struct enetc_ndev_priv *priv, u32 index)
969{
970 struct enetc_psfp_gate *sgi;
971 u8 z;
972
973 sgi = enetc_get_gate_by_index(index);
974 WARN_ON(!sgi);
975 z = refcount_dec_and_test(&sgi->refcount);
976 if (z) {
977 enetc_streamgate_hw_set(priv, sgi, false);
978 hlist_del(&sgi->node);
979 kfree(sgi);
980 }
981}
982
983static void flow_meter_unref(struct enetc_ndev_priv *priv, u32 index)
984{
985 struct enetc_psfp_meter *fmi;
986 u8 z;
987
988 fmi = enetc_get_meter_by_index(index);
989 WARN_ON(!fmi);
990 z = refcount_dec_and_test(&fmi->refcount);
991 if (z) {
992 enetc_flowmeter_hw_set(priv, fmi, false);
993 hlist_del(&fmi->node);
994 kfree(fmi);
995 }
996}
997
998static void remove_one_chain(struct enetc_ndev_priv *priv,
999 struct enetc_stream_filter *filter)
1000{
1001 if (filter->flags & ENETC_PSFP_FLAGS_FMI)
1002 flow_meter_unref(priv, filter->fmi_index);
1003
1004 stream_gate_unref(priv, filter->sgi_index);
1005 stream_filter_unref(priv, filter->sfi_index);
1006
1007 hlist_del(&filter->node);
1008 kfree(filter);
1009}
1010
1011static int enetc_psfp_hw_set(struct enetc_ndev_priv *priv,
1012 struct enetc_streamid *sid,
1013 struct enetc_psfp_filter *sfi,
1014 struct enetc_psfp_gate *sgi,
1015 struct enetc_psfp_meter *fmi)
1016{
1017 int err;
1018
1019 err = enetc_streamid_hw_set(priv, sid, true);
1020 if (err)
1021 return err;
1022
1023 if (sfi) {
1024 err = enetc_streamfilter_hw_set(priv, sfi, true);
1025 if (err)
1026 goto revert_sid;
1027 }
1028
1029 err = enetc_streamgate_hw_set(priv, sgi, true);
1030 if (err)
1031 goto revert_sfi;
1032
1033 if (fmi) {
1034 err = enetc_flowmeter_hw_set(priv, fmi, true);
1035 if (err)
1036 goto revert_sgi;
1037 }
1038
1039 return 0;
1040
1041revert_sgi:
1042 enetc_streamgate_hw_set(priv, sgi, false);
1043revert_sfi:
1044 if (sfi)
1045 enetc_streamfilter_hw_set(priv, sfi, false);
1046revert_sid:
1047 enetc_streamid_hw_set(priv, sid, false);
1048 return err;
1049}
1050
1051static struct actions_fwd *enetc_check_flow_actions(u64 acts,
1052 unsigned int inputkeys)
1053{
1054 int i;
1055
1056 for (i = 0; i < ARRAY_SIZE(enetc_act_fwd); i++)
1057 if (acts == enetc_act_fwd[i].actions &&
1058 inputkeys & enetc_act_fwd[i].keys)
1059 return &enetc_act_fwd[i];
1060
1061 return NULL;
1062}
1063
1064static int enetc_psfp_parse_clsflower(struct enetc_ndev_priv *priv,
1065 struct flow_cls_offload *f)
1066{
1067 struct flow_action_entry *entryg = NULL, *entryp = NULL;
1068 struct flow_rule *rule = flow_cls_offload_flow_rule(f);
1069 struct netlink_ext_ack *extack = f->common.extack;
1070 struct enetc_stream_filter *filter, *old_filter;
1071 struct enetc_psfp_meter *fmi = NULL, *old_fmi;
1072 struct enetc_psfp_filter *sfi, *old_sfi;
1073 struct enetc_psfp_gate *sgi, *old_sgi;
1074 struct flow_action_entry *entry;
1075 struct action_gate_entry *e;
1076 u8 sfi_overwrite = 0;
1077 int entries_size;
1078 int i, err;
1079
1080 if (f->common.chain_index >= priv->psfp_cap.max_streamid) {
1081 NL_SET_ERR_MSG_MOD(extack, "No Stream identify resource!");
1082 return -ENOSPC;
1083 }
1084
1085 flow_action_for_each(i, entry, &rule->action)
1086 if (entry->id == FLOW_ACTION_GATE)
1087 entryg = entry;
1088 else if (entry->id == FLOW_ACTION_POLICE)
1089 entryp = entry;
1090
1091
1092 if (!entryg)
1093 return -EINVAL;
1094
1095 filter = kzalloc(sizeof(*filter), GFP_KERNEL);
1096 if (!filter)
1097 return -ENOMEM;
1098
1099 filter->sid.index = f->common.chain_index;
1100
1101 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_ETH_ADDRS)) {
1102 struct flow_match_eth_addrs match;
1103
1104 flow_rule_match_eth_addrs(rule, &match);
1105
1106 if (!is_zero_ether_addr(match.mask->dst) &&
1107 !is_zero_ether_addr(match.mask->src)) {
1108 NL_SET_ERR_MSG_MOD(extack,
1109 "Cannot match on both source and destination MAC");
1110 err = -EINVAL;
1111 goto free_filter;
1112 }
1113
1114 if (!is_zero_ether_addr(match.mask->dst)) {
1115 if (!is_broadcast_ether_addr(match.mask->dst)) {
1116 NL_SET_ERR_MSG_MOD(extack,
1117 "Masked matching on destination MAC not supported");
1118 err = -EINVAL;
1119 goto free_filter;
1120 }
1121 ether_addr_copy(filter->sid.dst_mac, match.key->dst);
1122 filter->sid.filtertype = STREAMID_TYPE_NULL;
1123 }
1124
1125 if (!is_zero_ether_addr(match.mask->src)) {
1126 if (!is_broadcast_ether_addr(match.mask->src)) {
1127 NL_SET_ERR_MSG_MOD(extack,
1128 "Masked matching on source MAC not supported");
1129 err = -EINVAL;
1130 goto free_filter;
1131 }
1132 ether_addr_copy(filter->sid.src_mac, match.key->src);
1133 filter->sid.filtertype = STREAMID_TYPE_SMAC;
1134 }
1135 } else {
1136 NL_SET_ERR_MSG_MOD(extack, "Unsupported, must include ETH_ADDRS");
1137 err = -EINVAL;
1138 goto free_filter;
1139 }
1140
1141 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_VLAN)) {
1142 struct flow_match_vlan match;
1143
1144 flow_rule_match_vlan(rule, &match);
1145 if (match.mask->vlan_priority) {
1146 if (match.mask->vlan_priority !=
1147 (VLAN_PRIO_MASK >> VLAN_PRIO_SHIFT)) {
1148 NL_SET_ERR_MSG_MOD(extack, "Only full mask is supported for VLAN priority");
1149 err = -EINVAL;
1150 goto free_filter;
1151 }
1152 }
1153
1154 if (match.mask->vlan_id) {
1155 if (match.mask->vlan_id != VLAN_VID_MASK) {
1156 NL_SET_ERR_MSG_MOD(extack, "Only full mask is supported for VLAN id");
1157 err = -EINVAL;
1158 goto free_filter;
1159 }
1160
1161 filter->sid.vid = match.key->vlan_id;
1162 if (!filter->sid.vid)
1163 filter->sid.tagged = STREAMID_VLAN_UNTAGGED;
1164 else
1165 filter->sid.tagged = STREAMID_VLAN_TAGGED;
1166 }
1167 } else {
1168 filter->sid.tagged = STREAMID_VLAN_ALL;
1169 }
1170
1171
1172 if (entryg->gate.index >= priv->psfp_cap.max_psfp_gate) {
1173 NL_SET_ERR_MSG_MOD(extack, "No Stream Gate resource!");
1174 err = -ENOSPC;
1175 goto free_filter;
1176 }
1177
1178 if (entryg->gate.num_entries >= priv->psfp_cap.max_psfp_gatelist) {
1179 NL_SET_ERR_MSG_MOD(extack, "No Stream Gate resource!");
1180 err = -ENOSPC;
1181 goto free_filter;
1182 }
1183
1184 entries_size = struct_size(sgi, entries, entryg->gate.num_entries);
1185 sgi = kzalloc(entries_size, GFP_KERNEL);
1186 if (!sgi) {
1187 err = -ENOMEM;
1188 goto free_filter;
1189 }
1190
1191 refcount_set(&sgi->refcount, 1);
1192 sgi->index = entryg->gate.index;
1193 sgi->init_ipv = entryg->gate.prio;
1194 sgi->basetime = entryg->gate.basetime;
1195 sgi->cycletime = entryg->gate.cycletime;
1196 sgi->num_entries = entryg->gate.num_entries;
1197
1198 e = sgi->entries;
1199 for (i = 0; i < entryg->gate.num_entries; i++) {
1200 e[i].gate_state = entryg->gate.entries[i].gate_state;
1201 e[i].interval = entryg->gate.entries[i].interval;
1202 e[i].ipv = entryg->gate.entries[i].ipv;
1203 e[i].maxoctets = entryg->gate.entries[i].maxoctets;
1204 }
1205
1206 filter->sgi_index = sgi->index;
1207
1208 sfi = kzalloc(sizeof(*sfi), GFP_KERNEL);
1209 if (!sfi) {
1210 err = -ENOMEM;
1211 goto free_gate;
1212 }
1213
1214 refcount_set(&sfi->refcount, 1);
1215 sfi->gate_id = sgi->index;
1216 sfi->meter_id = ENETC_PSFP_WILDCARD;
1217
1218
1219 if (entryp) {
1220 if (entryp->police.rate_pkt_ps) {
1221 NL_SET_ERR_MSG_MOD(extack, "QoS offload not support packets per second");
1222 err = -EOPNOTSUPP;
1223 goto free_sfi;
1224 }
1225 if (entryp->police.burst) {
1226 fmi = kzalloc(sizeof(*fmi), GFP_KERNEL);
1227 if (!fmi) {
1228 err = -ENOMEM;
1229 goto free_sfi;
1230 }
1231 refcount_set(&fmi->refcount, 1);
1232 fmi->cir = entryp->police.rate_bytes_ps;
1233 fmi->cbs = entryp->police.burst;
1234 fmi->index = entryp->police.index;
1235 filter->flags |= ENETC_PSFP_FLAGS_FMI;
1236 filter->fmi_index = fmi->index;
1237 sfi->meter_id = fmi->index;
1238 }
1239
1240 if (entryp->police.mtu)
1241 sfi->maxsdu = entryp->police.mtu;
1242 }
1243
1244
1245 if (f->common.prio && f->common.prio <= BIT(3))
1246 sfi->prio = f->common.prio - 1;
1247 else
1248 sfi->prio = ENETC_PSFP_WILDCARD;
1249
1250 old_sfi = enetc_psfp_check_sfi(sfi);
1251 if (!old_sfi) {
1252 int index;
1253
1254 index = enetc_get_free_index(priv);
1255 if (sfi->handle < 0) {
1256 NL_SET_ERR_MSG_MOD(extack, "No Stream Filter resource!");
1257 err = -ENOSPC;
1258 goto free_fmi;
1259 }
1260
1261 sfi->index = index;
1262 sfi->handle = index + HANDLE_OFFSET;
1263
1264 filter->sid.handle = sfi->handle;
1265 filter->sfi_index = sfi->index;
1266 sfi_overwrite = 0;
1267 } else {
1268 filter->sfi_index = old_sfi->index;
1269 filter->sid.handle = old_sfi->handle;
1270 sfi_overwrite = 1;
1271 }
1272
1273 err = enetc_psfp_hw_set(priv, &filter->sid,
1274 sfi_overwrite ? NULL : sfi, sgi, fmi);
1275 if (err)
1276 goto free_fmi;
1277
1278 spin_lock(&epsfp.psfp_lock);
1279 if (filter->flags & ENETC_PSFP_FLAGS_FMI) {
1280 old_fmi = enetc_get_meter_by_index(filter->fmi_index);
1281 if (old_fmi) {
1282 fmi->refcount = old_fmi->refcount;
1283 refcount_set(&fmi->refcount,
1284 refcount_read(&old_fmi->refcount) + 1);
1285 hlist_del(&old_fmi->node);
1286 kfree(old_fmi);
1287 }
1288 hlist_add_head(&fmi->node, &epsfp.psfp_meter_list);
1289 }
1290
1291
1292 old_sgi = enetc_get_gate_by_index(filter->sgi_index);
1293 if (old_sgi) {
1294 refcount_set(&sgi->refcount,
1295 refcount_read(&old_sgi->refcount) + 1);
1296 hlist_del(&old_sgi->node);
1297 kfree(old_sgi);
1298 }
1299
1300 hlist_add_head(&sgi->node, &epsfp.psfp_gate_list);
1301
1302 if (!old_sfi) {
1303 hlist_add_head(&sfi->node, &epsfp.psfp_filter_list);
1304 set_bit(sfi->index, epsfp.psfp_sfi_bitmap);
1305 } else {
1306 kfree(sfi);
1307 refcount_inc(&old_sfi->refcount);
1308 }
1309
1310 old_filter = enetc_get_stream_by_index(filter->sid.index);
1311 if (old_filter)
1312 remove_one_chain(priv, old_filter);
1313
1314 filter->stats.lastused = jiffies;
1315 hlist_add_head(&filter->node, &epsfp.stream_list);
1316
1317 spin_unlock(&epsfp.psfp_lock);
1318
1319 return 0;
1320
1321free_fmi:
1322 kfree(fmi);
1323free_sfi:
1324 kfree(sfi);
1325free_gate:
1326 kfree(sgi);
1327free_filter:
1328 kfree(filter);
1329
1330 return err;
1331}
1332
1333static int enetc_config_clsflower(struct enetc_ndev_priv *priv,
1334 struct flow_cls_offload *cls_flower)
1335{
1336 struct flow_rule *rule = flow_cls_offload_flow_rule(cls_flower);
1337 struct netlink_ext_ack *extack = cls_flower->common.extack;
1338 struct flow_dissector *dissector = rule->match.dissector;
1339 struct flow_action *action = &rule->action;
1340 struct flow_action_entry *entry;
1341 struct actions_fwd *fwd;
1342 u64 actions = 0;
1343 int i, err;
1344
1345 if (!flow_action_has_entries(action)) {
1346 NL_SET_ERR_MSG_MOD(extack, "At least one action is needed");
1347 return -EINVAL;
1348 }
1349
1350 flow_action_for_each(i, entry, action)
1351 actions |= BIT(entry->id);
1352
1353 fwd = enetc_check_flow_actions(actions, dissector->used_keys);
1354 if (!fwd) {
1355 NL_SET_ERR_MSG_MOD(extack, "Unsupported filter type!");
1356 return -EOPNOTSUPP;
1357 }
1358
1359 if (fwd->output & FILTER_ACTION_TYPE_PSFP) {
1360 err = enetc_psfp_parse_clsflower(priv, cls_flower);
1361 if (err) {
1362 NL_SET_ERR_MSG_MOD(extack, "Invalid PSFP inputs");
1363 return err;
1364 }
1365 } else {
1366 NL_SET_ERR_MSG_MOD(extack, "Unsupported actions");
1367 return -EOPNOTSUPP;
1368 }
1369
1370 return 0;
1371}
1372
1373static int enetc_psfp_destroy_clsflower(struct enetc_ndev_priv *priv,
1374 struct flow_cls_offload *f)
1375{
1376 struct enetc_stream_filter *filter;
1377 struct netlink_ext_ack *extack = f->common.extack;
1378 int err;
1379
1380 if (f->common.chain_index >= priv->psfp_cap.max_streamid) {
1381 NL_SET_ERR_MSG_MOD(extack, "No Stream identify resource!");
1382 return -ENOSPC;
1383 }
1384
1385 filter = enetc_get_stream_by_index(f->common.chain_index);
1386 if (!filter)
1387 return -EINVAL;
1388
1389 err = enetc_streamid_hw_set(priv, &filter->sid, false);
1390 if (err)
1391 return err;
1392
1393 remove_one_chain(priv, filter);
1394
1395 return 0;
1396}
1397
1398static int enetc_destroy_clsflower(struct enetc_ndev_priv *priv,
1399 struct flow_cls_offload *f)
1400{
1401 return enetc_psfp_destroy_clsflower(priv, f);
1402}
1403
1404static int enetc_psfp_get_stats(struct enetc_ndev_priv *priv,
1405 struct flow_cls_offload *f)
1406{
1407 struct psfp_streamfilter_counters counters = {};
1408 struct enetc_stream_filter *filter;
1409 struct flow_stats stats = {};
1410 int err;
1411
1412 filter = enetc_get_stream_by_index(f->common.chain_index);
1413 if (!filter)
1414 return -EINVAL;
1415
1416 err = enetc_streamcounter_hw_get(priv, filter->sfi_index, &counters);
1417 if (err)
1418 return -EINVAL;
1419
1420 spin_lock(&epsfp.psfp_lock);
1421 stats.pkts = counters.matching_frames_count +
1422 counters.not_passing_sdu_count -
1423 filter->stats.pkts;
1424 stats.drops = counters.not_passing_frames_count +
1425 counters.not_passing_sdu_count +
1426 counters.red_frames_count -
1427 filter->stats.drops;
1428 stats.lastused = filter->stats.lastused;
1429 filter->stats.pkts += stats.pkts;
1430 filter->stats.drops += stats.drops;
1431 spin_unlock(&epsfp.psfp_lock);
1432
1433 flow_stats_update(&f->stats, 0x0, stats.pkts, stats.drops,
1434 stats.lastused, FLOW_ACTION_HW_STATS_DELAYED);
1435
1436 return 0;
1437}
1438
1439static int enetc_setup_tc_cls_flower(struct enetc_ndev_priv *priv,
1440 struct flow_cls_offload *cls_flower)
1441{
1442 switch (cls_flower->command) {
1443 case FLOW_CLS_REPLACE:
1444 return enetc_config_clsflower(priv, cls_flower);
1445 case FLOW_CLS_DESTROY:
1446 return enetc_destroy_clsflower(priv, cls_flower);
1447 case FLOW_CLS_STATS:
1448 return enetc_psfp_get_stats(priv, cls_flower);
1449 default:
1450 return -EOPNOTSUPP;
1451 }
1452}
1453
1454static inline void clean_psfp_sfi_bitmap(void)
1455{
1456 bitmap_free(epsfp.psfp_sfi_bitmap);
1457 epsfp.psfp_sfi_bitmap = NULL;
1458}
1459
1460static void clean_stream_list(void)
1461{
1462 struct enetc_stream_filter *s;
1463 struct hlist_node *tmp;
1464
1465 hlist_for_each_entry_safe(s, tmp, &epsfp.stream_list, node) {
1466 hlist_del(&s->node);
1467 kfree(s);
1468 }
1469}
1470
1471static void clean_sfi_list(void)
1472{
1473 struct enetc_psfp_filter *sfi;
1474 struct hlist_node *tmp;
1475
1476 hlist_for_each_entry_safe(sfi, tmp, &epsfp.psfp_filter_list, node) {
1477 hlist_del(&sfi->node);
1478 kfree(sfi);
1479 }
1480}
1481
1482static void clean_sgi_list(void)
1483{
1484 struct enetc_psfp_gate *sgi;
1485 struct hlist_node *tmp;
1486
1487 hlist_for_each_entry_safe(sgi, tmp, &epsfp.psfp_gate_list, node) {
1488 hlist_del(&sgi->node);
1489 kfree(sgi);
1490 }
1491}
1492
1493static void clean_psfp_all(void)
1494{
1495
1496 clean_sfi_list();
1497 clean_sgi_list();
1498 clean_stream_list();
1499 epsfp.dev_bitmap = 0;
1500 clean_psfp_sfi_bitmap();
1501}
1502
1503int enetc_setup_tc_block_cb(enum tc_setup_type type, void *type_data,
1504 void *cb_priv)
1505{
1506 struct net_device *ndev = cb_priv;
1507
1508 if (!tc_can_offload(ndev))
1509 return -EOPNOTSUPP;
1510
1511 switch (type) {
1512 case TC_SETUP_CLSFLOWER:
1513 return enetc_setup_tc_cls_flower(netdev_priv(ndev), type_data);
1514 default:
1515 return -EOPNOTSUPP;
1516 }
1517}
1518
1519int enetc_psfp_init(struct enetc_ndev_priv *priv)
1520{
1521 if (epsfp.psfp_sfi_bitmap)
1522 return 0;
1523
1524 epsfp.psfp_sfi_bitmap = bitmap_zalloc(priv->psfp_cap.max_psfp_filter,
1525 GFP_KERNEL);
1526 if (!epsfp.psfp_sfi_bitmap)
1527 return -ENOMEM;
1528
1529 spin_lock_init(&epsfp.psfp_lock);
1530
1531 if (list_empty(&enetc_block_cb_list))
1532 epsfp.dev_bitmap = 0;
1533
1534 return 0;
1535}
1536
1537int enetc_psfp_clean(struct enetc_ndev_priv *priv)
1538{
1539 if (!list_empty(&enetc_block_cb_list))
1540 return -EBUSY;
1541
1542 clean_psfp_all();
1543
1544 return 0;
1545}
1546
1547int enetc_setup_tc_psfp(struct net_device *ndev, void *type_data)
1548{
1549 struct enetc_ndev_priv *priv = netdev_priv(ndev);
1550 struct flow_block_offload *f = type_data;
1551 int err;
1552
1553 err = flow_block_cb_setup_simple(f, &enetc_block_cb_list,
1554 enetc_setup_tc_block_cb,
1555 ndev, ndev, true);
1556 if (err)
1557 return err;
1558
1559 switch (f->command) {
1560 case FLOW_BLOCK_BIND:
1561 set_bit(enetc_pf_to_port(priv->si->pdev), &epsfp.dev_bitmap);
1562 break;
1563 case FLOW_BLOCK_UNBIND:
1564 clear_bit(enetc_pf_to_port(priv->si->pdev), &epsfp.dev_bitmap);
1565 if (!epsfp.dev_bitmap)
1566 clean_psfp_all();
1567 break;
1568 }
1569
1570 return 0;
1571}
1572
