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