1
2
3
4
5
6
7#include <net/pkt_cls.h>
8#include <net/tc_act/tc_gact.h>
9#include "common.h"
10#include "dwmac4.h"
11#include "dwmac5.h"
12#include "stmmac.h"
13
14static void tc_fill_all_pass_entry(struct stmmac_tc_entry *entry)
15{
16 memset(entry, 0, sizeof(*entry));
17 entry->in_use = true;
18 entry->is_last = true;
19 entry->is_frag = false;
20 entry->prio = ~0x0;
21 entry->handle = 0;
22 entry->val.match_data = 0x0;
23 entry->val.match_en = 0x0;
24 entry->val.af = 1;
25 entry->val.dma_ch_no = 0x0;
26}
27
28static struct stmmac_tc_entry *tc_find_entry(struct stmmac_priv *priv,
29 struct tc_cls_u32_offload *cls,
30 bool free)
31{
32 struct stmmac_tc_entry *entry, *first = NULL, *dup = NULL;
33 u32 loc = cls->knode.handle;
34 int i;
35
36 for (i = 0; i < priv->tc_entries_max; i++) {
37 entry = &priv->tc_entries[i];
38 if (!entry->in_use && !first && free)
39 first = entry;
40 if (entry->handle == loc && !free)
41 dup = entry;
42 }
43
44 if (dup)
45 return dup;
46 if (first) {
47 first->handle = loc;
48 first->in_use = true;
49
50
51 memset(&first->val, 0, sizeof(first->val));
52 }
53
54 return first;
55}
56
57static int tc_fill_actions(struct stmmac_tc_entry *entry,
58 struct stmmac_tc_entry *frag,
59 struct tc_cls_u32_offload *cls)
60{
61 struct stmmac_tc_entry *action_entry = entry;
62 const struct tc_action *act;
63 struct tcf_exts *exts;
64 int i;
65
66 exts = cls->knode.exts;
67 if (!tcf_exts_has_actions(exts))
68 return -EINVAL;
69 if (frag)
70 action_entry = frag;
71
72 tcf_exts_for_each_action(i, act, exts) {
73
74 if (is_tcf_gact_ok(act)) {
75 action_entry->val.af = 1;
76 break;
77 }
78
79 if (is_tcf_gact_shot(act)) {
80 action_entry->val.rf = 1;
81 break;
82 }
83
84
85 return -EINVAL;
86 }
87
88 return 0;
89}
90
91static int tc_fill_entry(struct stmmac_priv *priv,
92 struct tc_cls_u32_offload *cls)
93{
94 struct stmmac_tc_entry *entry, *frag = NULL;
95 struct tc_u32_sel *sel = cls->knode.sel;
96 u32 off, data, mask, real_off, rem;
97 u32 prio = cls->common.prio;
98 int ret;
99
100
101 if (sel->nkeys <= 0 || sel->nkeys > 1)
102 return -EINVAL;
103
104 off = sel->keys[0].off << sel->offshift;
105 data = sel->keys[0].val;
106 mask = sel->keys[0].mask;
107
108 switch (ntohs(cls->common.protocol)) {
109 case ETH_P_ALL:
110 break;
111 case ETH_P_IP:
112 off += ETH_HLEN;
113 break;
114 default:
115 return -EINVAL;
116 }
117
118 if (off > priv->tc_off_max)
119 return -EINVAL;
120
121 real_off = off / 4;
122 rem = off % 4;
123
124 entry = tc_find_entry(priv, cls, true);
125 if (!entry)
126 return -EINVAL;
127
128 if (rem) {
129 frag = tc_find_entry(priv, cls, true);
130 if (!frag) {
131 ret = -EINVAL;
132 goto err_unuse;
133 }
134
135 entry->frag_ptr = frag;
136 entry->val.match_en = (mask << (rem * 8)) &
137 GENMASK(31, rem * 8);
138 entry->val.match_data = (data << (rem * 8)) &
139 GENMASK(31, rem * 8);
140 entry->val.frame_offset = real_off;
141 entry->prio = prio;
142
143 frag->val.match_en = (mask >> (rem * 8)) &
144 GENMASK(rem * 8 - 1, 0);
145 frag->val.match_data = (data >> (rem * 8)) &
146 GENMASK(rem * 8 - 1, 0);
147 frag->val.frame_offset = real_off + 1;
148 frag->prio = prio;
149 frag->is_frag = true;
150 } else {
151 entry->frag_ptr = NULL;
152 entry->val.match_en = mask;
153 entry->val.match_data = data;
154 entry->val.frame_offset = real_off;
155 entry->prio = prio;
156 }
157
158 ret = tc_fill_actions(entry, frag, cls);
159 if (ret)
160 goto err_unuse;
161
162 return 0;
163
164err_unuse:
165 if (frag)
166 frag->in_use = false;
167 entry->in_use = false;
168 return ret;
169}
170
171static void tc_unfill_entry(struct stmmac_priv *priv,
172 struct tc_cls_u32_offload *cls)
173{
174 struct stmmac_tc_entry *entry;
175
176 entry = tc_find_entry(priv, cls, false);
177 if (!entry)
178 return;
179
180 entry->in_use = false;
181 if (entry->frag_ptr) {
182 entry = entry->frag_ptr;
183 entry->is_frag = false;
184 entry->in_use = false;
185 }
186}
187
188static int tc_config_knode(struct stmmac_priv *priv,
189 struct tc_cls_u32_offload *cls)
190{
191 int ret;
192
193 ret = tc_fill_entry(priv, cls);
194 if (ret)
195 return ret;
196
197 ret = stmmac_rxp_config(priv, priv->hw->pcsr, priv->tc_entries,
198 priv->tc_entries_max);
199 if (ret)
200 goto err_unfill;
201
202 return 0;
203
204err_unfill:
205 tc_unfill_entry(priv, cls);
206 return ret;
207}
208
209static int tc_delete_knode(struct stmmac_priv *priv,
210 struct tc_cls_u32_offload *cls)
211{
212 int ret;
213
214
215 tc_unfill_entry(priv, cls);
216
217 ret = stmmac_rxp_config(priv, priv->hw->pcsr, priv->tc_entries,
218 priv->tc_entries_max);
219 if (ret)
220 return ret;
221
222 return 0;
223}
224
225static int tc_setup_cls_u32(struct stmmac_priv *priv,
226 struct tc_cls_u32_offload *cls)
227{
228 switch (cls->command) {
229 case TC_CLSU32_REPLACE_KNODE:
230 tc_unfill_entry(priv, cls);
231
232 case TC_CLSU32_NEW_KNODE:
233 return tc_config_knode(priv, cls);
234 case TC_CLSU32_DELETE_KNODE:
235 return tc_delete_knode(priv, cls);
236 default:
237 return -EOPNOTSUPP;
238 }
239}
240
241static int tc_init(struct stmmac_priv *priv)
242{
243 struct dma_features *dma_cap = &priv->dma_cap;
244 unsigned int count;
245
246 if (!dma_cap->frpsel)
247 return -EINVAL;
248
249 switch (dma_cap->frpbs) {
250 case 0x0:
251 priv->tc_off_max = 64;
252 break;
253 case 0x1:
254 priv->tc_off_max = 128;
255 break;
256 case 0x2:
257 priv->tc_off_max = 256;
258 break;
259 default:
260 return -EINVAL;
261 }
262
263 switch (dma_cap->frpes) {
264 case 0x0:
265 count = 64;
266 break;
267 case 0x1:
268 count = 128;
269 break;
270 case 0x2:
271 count = 256;
272 break;
273 default:
274 return -EINVAL;
275 }
276
277
278 priv->tc_entries_max = count;
279 priv->tc_entries = devm_kcalloc(priv->device,
280 count, sizeof(*priv->tc_entries), GFP_KERNEL);
281 if (!priv->tc_entries)
282 return -ENOMEM;
283
284 tc_fill_all_pass_entry(&priv->tc_entries[count - 1]);
285
286 dev_info(priv->device, "Enabling HW TC (entries=%d, max_off=%d)\n",
287 priv->tc_entries_max, priv->tc_off_max);
288 return 0;
289}
290
291static int tc_setup_cbs(struct stmmac_priv *priv,
292 struct tc_cbs_qopt_offload *qopt)
293{
294 u32 tx_queues_count = priv->plat->tx_queues_to_use;
295 u32 queue = qopt->queue;
296 u32 ptr, speed_div;
297 u32 mode_to_use;
298 u64 value;
299 int ret;
300
301
302 if (queue <= 0 || queue >= tx_queues_count)
303 return -EINVAL;
304 if (priv->speed != SPEED_100 && priv->speed != SPEED_1000)
305 return -EOPNOTSUPP;
306
307 mode_to_use = priv->plat->tx_queues_cfg[queue].mode_to_use;
308 if (mode_to_use == MTL_QUEUE_DCB && qopt->enable) {
309 ret = stmmac_dma_qmode(priv, priv->ioaddr, queue, MTL_QUEUE_AVB);
310 if (ret)
311 return ret;
312
313 priv->plat->tx_queues_cfg[queue].mode_to_use = MTL_QUEUE_AVB;
314 } else if (!qopt->enable) {
315 return stmmac_dma_qmode(priv, priv->ioaddr, queue, MTL_QUEUE_DCB);
316 }
317
318
319 ptr = (priv->speed == SPEED_100) ? 4 : 8;
320 speed_div = (priv->speed == SPEED_100) ? 100000 : 1000000;
321
322
323 value = div_s64(qopt->idleslope * 1024ll * ptr, speed_div);
324 priv->plat->tx_queues_cfg[queue].idle_slope = value & GENMASK(31, 0);
325
326 value = div_s64(-qopt->sendslope * 1024ll * ptr, speed_div);
327 priv->plat->tx_queues_cfg[queue].send_slope = value & GENMASK(31, 0);
328
329 value = qopt->hicredit * 1024ll * 8;
330 priv->plat->tx_queues_cfg[queue].high_credit = value & GENMASK(31, 0);
331
332 value = qopt->locredit * 1024ll * 8;
333 priv->plat->tx_queues_cfg[queue].low_credit = value & GENMASK(31, 0);
334
335 ret = stmmac_config_cbs(priv, priv->hw,
336 priv->plat->tx_queues_cfg[queue].send_slope,
337 priv->plat->tx_queues_cfg[queue].idle_slope,
338 priv->plat->tx_queues_cfg[queue].high_credit,
339 priv->plat->tx_queues_cfg[queue].low_credit,
340 queue);
341 if (ret)
342 return ret;
343
344 dev_info(priv->device, "CBS queue %d: send %d, idle %d, hi %d, lo %d\n",
345 queue, qopt->sendslope, qopt->idleslope,
346 qopt->hicredit, qopt->locredit);
347 return 0;
348}
349
350const struct stmmac_tc_ops dwmac510_tc_ops = {
351 .init = tc_init,
352 .setup_cls_u32 = tc_setup_cls_u32,
353 .setup_cbs = tc_setup_cbs,
354};
355
