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 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
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_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
enetc_setup_tc_taprio(struct net_device * ndev,void * type_data)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
enetc_get_cbs_enable(struct enetc_hw * hw,u8 tc)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
enetc_get_cbs_bw(struct enetc_hw * hw,u8 tc)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
enetc_setup_tc_cbs(struct net_device * ndev,void * type_data)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
enetc_setup_tc_txtime(struct net_device * ndev,void * type_data)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 */
enetc_streamid_hw_set(struct enetc_ndev_priv * priv,struct enetc_streamid * sid,u8 enable)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 */
enetc_streamfilter_hw_set(struct enetc_ndev_priv * priv,struct enetc_psfp_filter * sfi,u8 enable)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
enetc_streamcounter_hw_get(struct enetc_ndev_priv * priv,u32 index,struct psfp_streamfilter_counters * cnt)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
get_ptp_now(struct enetc_hw * hw)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
get_start_ns(u64 now,u64 cycle,u64 * start)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 */
enetc_streamgate_hw_set(struct enetc_ndev_priv * priv,struct enetc_psfp_gate * sgi,u8 enable)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
enetc_flowmeter_hw_set(struct enetc_ndev_priv * priv,struct enetc_psfp_meter * fmi,u8 enable)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
enetc_get_stream_by_index(u32 index)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
enetc_get_gate_by_index(u32 index)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
enetc_get_filter_by_index(u32 index)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
enetc_get_meter_by_index(u32 index)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
enetc_psfp_check_sfi(struct enetc_psfp_filter * sfi)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
enetc_get_free_index(struct enetc_ndev_priv * priv)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
stream_filter_unref(struct enetc_ndev_priv * priv,u32 index)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
stream_gate_unref(struct enetc_ndev_priv * priv,u32 index)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
flow_meter_unref(struct enetc_ndev_priv * priv,u32 index)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
remove_one_chain(struct enetc_ndev_priv * priv,struct enetc_stream_filter * filter)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
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)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
enetc_check_flow_actions(u64 acts,unsigned int inputkeys)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
enetc_psfp_parse_clsflower(struct enetc_ndev_priv * priv,struct flow_cls_offload * f)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
enetc_config_clsflower(struct enetc_ndev_priv * priv,struct flow_cls_offload * cls_flower)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
enetc_psfp_destroy_clsflower(struct enetc_ndev_priv * priv,struct flow_cls_offload * f)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
enetc_destroy_clsflower(struct enetc_ndev_priv * priv,struct flow_cls_offload * f)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
enetc_psfp_get_stats(struct enetc_ndev_priv * priv,struct flow_cls_offload * f)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
enetc_setup_tc_cls_flower(struct enetc_ndev_priv * priv,struct flow_cls_offload * cls_flower)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
clean_psfp_sfi_bitmap(void)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
clean_stream_list(void)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
clean_sfi_list(void)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
clean_sgi_list(void)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
clean_psfp_all(void)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
enetc_setup_tc_block_cb(enum tc_setup_type type,void * type_data,void * cb_priv)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
enetc_psfp_init(struct enetc_ndev_priv * priv)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
enetc_psfp_clean(struct enetc_ndev_priv * priv)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
enetc_setup_tc_psfp(struct net_device * ndev,void * type_data)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