Loading...
Note: File does not exist in v5.4.
1// SPDX-License-Identifier: GPL-2.0
2/* Marvell RVU Ethernet driver
3 *
4 * Copyright (C) 2023 Marvell.
5 *
6 */
7#include <linux/netdevice.h>
8#include <linux/etherdevice.h>
9#include <linux/inetdevice.h>
10#include <linux/bitfield.h>
11
12#include "otx2_common.h"
13#include "cn10k.h"
14#include "qos.h"
15
16#define OTX2_QOS_QID_INNER 0xFFFFU
17#define OTX2_QOS_QID_NONE 0xFFFEU
18#define OTX2_QOS_ROOT_CLASSID 0xFFFFFFFF
19#define OTX2_QOS_CLASS_NONE 0
20#define OTX2_QOS_DEFAULT_PRIO 0xF
21#define OTX2_QOS_INVALID_SQ 0xFFFF
22#define OTX2_QOS_INVALID_TXSCHQ_IDX 0xFFFF
23#define CN10K_MAX_RR_WEIGHT GENMASK_ULL(13, 0)
24#define OTX2_MAX_RR_QUANTUM GENMASK_ULL(23, 0)
25
26static void otx2_qos_update_tx_netdev_queues(struct otx2_nic *pfvf)
27{
28 struct otx2_hw *hw = &pfvf->hw;
29 int tx_queues, qos_txqs, err;
30
31 qos_txqs = bitmap_weight(pfvf->qos.qos_sq_bmap,
32 OTX2_QOS_MAX_LEAF_NODES);
33
34 tx_queues = hw->tx_queues + qos_txqs;
35
36 err = netif_set_real_num_tx_queues(pfvf->netdev, tx_queues);
37 if (err) {
38 netdev_err(pfvf->netdev,
39 "Failed to set no of Tx queues: %d\n", tx_queues);
40 return;
41 }
42}
43
44static void otx2_qos_get_regaddr(struct otx2_qos_node *node,
45 struct nix_txschq_config *cfg,
46 int index)
47{
48 if (node->level == NIX_TXSCH_LVL_SMQ) {
49 cfg->reg[index++] = NIX_AF_MDQX_PARENT(node->schq);
50 cfg->reg[index++] = NIX_AF_MDQX_SCHEDULE(node->schq);
51 cfg->reg[index++] = NIX_AF_MDQX_PIR(node->schq);
52 cfg->reg[index] = NIX_AF_MDQX_CIR(node->schq);
53 } else if (node->level == NIX_TXSCH_LVL_TL4) {
54 cfg->reg[index++] = NIX_AF_TL4X_PARENT(node->schq);
55 cfg->reg[index++] = NIX_AF_TL4X_SCHEDULE(node->schq);
56 cfg->reg[index++] = NIX_AF_TL4X_PIR(node->schq);
57 cfg->reg[index] = NIX_AF_TL4X_CIR(node->schq);
58 } else if (node->level == NIX_TXSCH_LVL_TL3) {
59 cfg->reg[index++] = NIX_AF_TL3X_PARENT(node->schq);
60 cfg->reg[index++] = NIX_AF_TL3X_SCHEDULE(node->schq);
61 cfg->reg[index++] = NIX_AF_TL3X_PIR(node->schq);
62 cfg->reg[index] = NIX_AF_TL3X_CIR(node->schq);
63 } else if (node->level == NIX_TXSCH_LVL_TL2) {
64 cfg->reg[index++] = NIX_AF_TL2X_PARENT(node->schq);
65 cfg->reg[index++] = NIX_AF_TL2X_SCHEDULE(node->schq);
66 cfg->reg[index++] = NIX_AF_TL2X_PIR(node->schq);
67 cfg->reg[index] = NIX_AF_TL2X_CIR(node->schq);
68 }
69}
70
71static int otx2_qos_quantum_to_dwrr_weight(struct otx2_nic *pfvf, u32 quantum)
72{
73 u32 weight;
74
75 weight = quantum / pfvf->hw.dwrr_mtu;
76 if (quantum % pfvf->hw.dwrr_mtu)
77 weight += 1;
78
79 return weight;
80}
81
82static void otx2_config_sched_shaping(struct otx2_nic *pfvf,
83 struct otx2_qos_node *node,
84 struct nix_txschq_config *cfg,
85 int *num_regs)
86{
87 u32 rr_weight;
88 u32 quantum;
89 u64 maxrate;
90
91 otx2_qos_get_regaddr(node, cfg, *num_regs);
92
93 /* configure parent txschq */
94 cfg->regval[*num_regs] = node->parent->schq << 16;
95 (*num_regs)++;
96
97 /* configure prio/quantum */
98 if (node->qid == OTX2_QOS_QID_NONE) {
99 cfg->regval[*num_regs] = node->prio << 24 |
100 mtu_to_dwrr_weight(pfvf, pfvf->tx_max_pktlen);
101 (*num_regs)++;
102 return;
103 }
104
105 /* configure priority/quantum */
106 if (node->is_static) {
107 cfg->regval[*num_regs] =
108 (node->schq - node->parent->prio_anchor) << 24;
109 } else {
110 quantum = node->quantum ?
111 node->quantum : pfvf->tx_max_pktlen;
112 rr_weight = otx2_qos_quantum_to_dwrr_weight(pfvf, quantum);
113 cfg->regval[*num_regs] = node->parent->child_dwrr_prio << 24 |
114 rr_weight;
115 }
116 (*num_regs)++;
117
118 /* configure PIR */
119 maxrate = (node->rate > node->ceil) ? node->rate : node->ceil;
120
121 cfg->regval[*num_regs] =
122 otx2_get_txschq_rate_regval(pfvf, maxrate, 65536);
123 (*num_regs)++;
124
125 /* Don't configure CIR when both CIR+PIR not supported
126 * On 96xx, CIR + PIR + RED_ALGO=STALL causes deadlock
127 */
128 if (!test_bit(QOS_CIR_PIR_SUPPORT, &pfvf->hw.cap_flag))
129 return;
130
131 cfg->regval[*num_regs] =
132 otx2_get_txschq_rate_regval(pfvf, node->rate, 65536);
133 (*num_regs)++;
134}
135
136static void __otx2_qos_txschq_cfg(struct otx2_nic *pfvf,
137 struct otx2_qos_node *node,
138 struct nix_txschq_config *cfg)
139{
140 struct otx2_hw *hw = &pfvf->hw;
141 int num_regs = 0;
142 u8 level;
143
144 level = node->level;
145
146 /* program txschq registers */
147 if (level == NIX_TXSCH_LVL_SMQ) {
148 cfg->reg[num_regs] = NIX_AF_SMQX_CFG(node->schq);
149 cfg->regval[num_regs] = ((u64)pfvf->tx_max_pktlen << 8) |
150 OTX2_MIN_MTU;
151 cfg->regval[num_regs] |= (0x20ULL << 51) | (0x80ULL << 39) |
152 (0x2ULL << 36);
153 num_regs++;
154
155 otx2_config_sched_shaping(pfvf, node, cfg, &num_regs);
156 } else if (level == NIX_TXSCH_LVL_TL4) {
157 otx2_config_sched_shaping(pfvf, node, cfg, &num_regs);
158 } else if (level == NIX_TXSCH_LVL_TL3) {
159 /* configure link cfg */
160 if (level == pfvf->qos.link_cfg_lvl) {
161 cfg->reg[num_regs] = NIX_AF_TL3_TL2X_LINKX_CFG(node->schq, hw->tx_link);
162 cfg->regval[num_regs] = BIT_ULL(13) | BIT_ULL(12);
163 num_regs++;
164 }
165
166 otx2_config_sched_shaping(pfvf, node, cfg, &num_regs);
167 } else if (level == NIX_TXSCH_LVL_TL2) {
168 /* configure link cfg */
169 if (level == pfvf->qos.link_cfg_lvl) {
170 cfg->reg[num_regs] = NIX_AF_TL3_TL2X_LINKX_CFG(node->schq, hw->tx_link);
171 cfg->regval[num_regs] = BIT_ULL(13) | BIT_ULL(12);
172 num_regs++;
173 }
174
175 /* check if node is root */
176 if (node->qid == OTX2_QOS_QID_INNER && !node->parent) {
177 cfg->reg[num_regs] = NIX_AF_TL2X_SCHEDULE(node->schq);
178 cfg->regval[num_regs] = (u64)hw->txschq_aggr_lvl_rr_prio << 24 |
179 mtu_to_dwrr_weight(pfvf,
180 pfvf->tx_max_pktlen);
181 num_regs++;
182 goto txschq_cfg_out;
183 }
184
185 otx2_config_sched_shaping(pfvf, node, cfg, &num_regs);
186 }
187
188txschq_cfg_out:
189 cfg->num_regs = num_regs;
190}
191
192static int otx2_qos_txschq_set_parent_topology(struct otx2_nic *pfvf,
193 struct otx2_qos_node *parent)
194{
195 struct mbox *mbox = &pfvf->mbox;
196 struct nix_txschq_config *cfg;
197 int rc;
198
199 if (parent->level == NIX_TXSCH_LVL_MDQ)
200 return 0;
201
202 mutex_lock(&mbox->lock);
203
204 cfg = otx2_mbox_alloc_msg_nix_txschq_cfg(&pfvf->mbox);
205 if (!cfg) {
206 mutex_unlock(&mbox->lock);
207 return -ENOMEM;
208 }
209
210 cfg->lvl = parent->level;
211
212 if (parent->level == NIX_TXSCH_LVL_TL4)
213 cfg->reg[0] = NIX_AF_TL4X_TOPOLOGY(parent->schq);
214 else if (parent->level == NIX_TXSCH_LVL_TL3)
215 cfg->reg[0] = NIX_AF_TL3X_TOPOLOGY(parent->schq);
216 else if (parent->level == NIX_TXSCH_LVL_TL2)
217 cfg->reg[0] = NIX_AF_TL2X_TOPOLOGY(parent->schq);
218 else if (parent->level == NIX_TXSCH_LVL_TL1)
219 cfg->reg[0] = NIX_AF_TL1X_TOPOLOGY(parent->schq);
220
221 cfg->regval[0] = (u64)parent->prio_anchor << 32;
222 cfg->regval[0] |= ((parent->child_dwrr_prio != OTX2_QOS_DEFAULT_PRIO) ?
223 parent->child_dwrr_prio : 0) << 1;
224 cfg->num_regs++;
225
226 rc = otx2_sync_mbox_msg(&pfvf->mbox);
227
228 mutex_unlock(&mbox->lock);
229
230 return rc;
231}
232
233static void otx2_qos_free_hw_node_schq(struct otx2_nic *pfvf,
234 struct otx2_qos_node *parent)
235{
236 struct otx2_qos_node *node;
237
238 list_for_each_entry_reverse(node, &parent->child_schq_list, list)
239 otx2_txschq_free_one(pfvf, node->level, node->schq);
240}
241
242static void otx2_qos_free_hw_node(struct otx2_nic *pfvf,
243 struct otx2_qos_node *parent)
244{
245 struct otx2_qos_node *node, *tmp;
246
247 list_for_each_entry_safe(node, tmp, &parent->child_list, list) {
248 otx2_qos_free_hw_node(pfvf, node);
249 otx2_qos_free_hw_node_schq(pfvf, node);
250 otx2_txschq_free_one(pfvf, node->level, node->schq);
251 }
252}
253
254static void otx2_qos_free_hw_cfg(struct otx2_nic *pfvf,
255 struct otx2_qos_node *node)
256{
257 mutex_lock(&pfvf->qos.qos_lock);
258
259 /* free child node hw mappings */
260 otx2_qos_free_hw_node(pfvf, node);
261 otx2_qos_free_hw_node_schq(pfvf, node);
262
263 /* free node hw mappings */
264 otx2_txschq_free_one(pfvf, node->level, node->schq);
265
266 mutex_unlock(&pfvf->qos.qos_lock);
267}
268
269static void otx2_qos_sw_node_delete(struct otx2_nic *pfvf,
270 struct otx2_qos_node *node)
271{
272 hash_del_rcu(&node->hlist);
273
274 if (node->qid != OTX2_QOS_QID_INNER && node->qid != OTX2_QOS_QID_NONE) {
275 __clear_bit(node->qid, pfvf->qos.qos_sq_bmap);
276 otx2_qos_update_tx_netdev_queues(pfvf);
277 }
278
279 list_del(&node->list);
280 kfree(node);
281}
282
283static void otx2_qos_free_sw_node_schq(struct otx2_nic *pfvf,
284 struct otx2_qos_node *parent)
285{
286 struct otx2_qos_node *node, *tmp;
287
288 list_for_each_entry_safe(node, tmp, &parent->child_schq_list, list) {
289 list_del(&node->list);
290 kfree(node);
291 }
292}
293
294static void __otx2_qos_free_sw_node(struct otx2_nic *pfvf,
295 struct otx2_qos_node *parent)
296{
297 struct otx2_qos_node *node, *tmp;
298
299 list_for_each_entry_safe(node, tmp, &parent->child_list, list) {
300 __otx2_qos_free_sw_node(pfvf, node);
301 otx2_qos_free_sw_node_schq(pfvf, node);
302 otx2_qos_sw_node_delete(pfvf, node);
303 }
304}
305
306static void otx2_qos_free_sw_node(struct otx2_nic *pfvf,
307 struct otx2_qos_node *node)
308{
309 mutex_lock(&pfvf->qos.qos_lock);
310
311 __otx2_qos_free_sw_node(pfvf, node);
312 otx2_qos_free_sw_node_schq(pfvf, node);
313 otx2_qos_sw_node_delete(pfvf, node);
314
315 mutex_unlock(&pfvf->qos.qos_lock);
316}
317
318static void otx2_qos_destroy_node(struct otx2_nic *pfvf,
319 struct otx2_qos_node *node)
320{
321 otx2_qos_free_hw_cfg(pfvf, node);
322 otx2_qos_free_sw_node(pfvf, node);
323}
324
325static void otx2_qos_fill_cfg_schq(struct otx2_qos_node *parent,
326 struct otx2_qos_cfg *cfg)
327{
328 struct otx2_qos_node *node;
329
330 list_for_each_entry(node, &parent->child_schq_list, list)
331 cfg->schq[node->level]++;
332}
333
334static void otx2_qos_fill_cfg_tl(struct otx2_qos_node *parent,
335 struct otx2_qos_cfg *cfg)
336{
337 struct otx2_qos_node *node;
338
339 list_for_each_entry(node, &parent->child_list, list) {
340 otx2_qos_fill_cfg_tl(node, cfg);
341 otx2_qos_fill_cfg_schq(node, cfg);
342 }
343
344 /* Assign the required number of transmit schedular queues under the
345 * given class
346 */
347 cfg->schq_contig[parent->level - 1] += parent->child_dwrr_cnt +
348 parent->max_static_prio + 1;
349}
350
351static void otx2_qos_prepare_txschq_cfg(struct otx2_nic *pfvf,
352 struct otx2_qos_node *parent,
353 struct otx2_qos_cfg *cfg)
354{
355 mutex_lock(&pfvf->qos.qos_lock);
356 otx2_qos_fill_cfg_tl(parent, cfg);
357 mutex_unlock(&pfvf->qos.qos_lock);
358}
359
360static void otx2_qos_read_txschq_cfg_schq(struct otx2_qos_node *parent,
361 struct otx2_qos_cfg *cfg)
362{
363 struct otx2_qos_node *node;
364 int cnt;
365
366 list_for_each_entry(node, &parent->child_schq_list, list) {
367 cnt = cfg->dwrr_node_pos[node->level];
368 cfg->schq_list[node->level][cnt] = node->schq;
369 cfg->schq[node->level]++;
370 cfg->dwrr_node_pos[node->level]++;
371 }
372}
373
374static void otx2_qos_read_txschq_cfg_tl(struct otx2_qos_node *parent,
375 struct otx2_qos_cfg *cfg)
376{
377 struct otx2_qos_node *node;
378 int cnt;
379
380 list_for_each_entry(node, &parent->child_list, list) {
381 otx2_qos_read_txschq_cfg_tl(node, cfg);
382 cnt = cfg->static_node_pos[node->level];
383 cfg->schq_contig_list[node->level][cnt] = node->schq;
384 cfg->schq_index_used[node->level][cnt] = true;
385 cfg->schq_contig[node->level]++;
386 cfg->static_node_pos[node->level]++;
387 otx2_qos_read_txschq_cfg_schq(node, cfg);
388 }
389}
390
391static void otx2_qos_read_txschq_cfg(struct otx2_nic *pfvf,
392 struct otx2_qos_node *node,
393 struct otx2_qos_cfg *cfg)
394{
395 mutex_lock(&pfvf->qos.qos_lock);
396 otx2_qos_read_txschq_cfg_tl(node, cfg);
397 mutex_unlock(&pfvf->qos.qos_lock);
398}
399
400static struct otx2_qos_node *
401otx2_qos_alloc_root(struct otx2_nic *pfvf)
402{
403 struct otx2_qos_node *node;
404
405 node = kzalloc(sizeof(*node), GFP_KERNEL);
406 if (!node)
407 return ERR_PTR(-ENOMEM);
408
409 node->parent = NULL;
410 if (!is_otx2_vf(pfvf->pcifunc)) {
411 node->level = NIX_TXSCH_LVL_TL1;
412 } else {
413 node->level = NIX_TXSCH_LVL_TL2;
414 node->child_dwrr_prio = OTX2_QOS_DEFAULT_PRIO;
415 }
416
417 WRITE_ONCE(node->qid, OTX2_QOS_QID_INNER);
418 node->classid = OTX2_QOS_ROOT_CLASSID;
419
420 hash_add_rcu(pfvf->qos.qos_hlist, &node->hlist, node->classid);
421 list_add_tail(&node->list, &pfvf->qos.qos_tree);
422 INIT_LIST_HEAD(&node->child_list);
423 INIT_LIST_HEAD(&node->child_schq_list);
424
425 return node;
426}
427
428static int otx2_qos_add_child_node(struct otx2_qos_node *parent,
429 struct otx2_qos_node *node)
430{
431 struct list_head *head = &parent->child_list;
432 struct otx2_qos_node *tmp_node;
433 struct list_head *tmp;
434
435 if (node->prio > parent->max_static_prio)
436 parent->max_static_prio = node->prio;
437
438 for (tmp = head->next; tmp != head; tmp = tmp->next) {
439 tmp_node = list_entry(tmp, struct otx2_qos_node, list);
440 if (tmp_node->prio == node->prio &&
441 tmp_node->is_static)
442 return -EEXIST;
443 if (tmp_node->prio > node->prio) {
444 list_add_tail(&node->list, tmp);
445 return 0;
446 }
447 }
448
449 list_add_tail(&node->list, head);
450 return 0;
451}
452
453static int otx2_qos_alloc_txschq_node(struct otx2_nic *pfvf,
454 struct otx2_qos_node *node)
455{
456 struct otx2_qos_node *txschq_node, *parent, *tmp;
457 int lvl;
458
459 parent = node;
460 for (lvl = node->level - 1; lvl >= NIX_TXSCH_LVL_MDQ; lvl--) {
461 txschq_node = kzalloc(sizeof(*txschq_node), GFP_KERNEL);
462 if (!txschq_node)
463 goto err_out;
464
465 txschq_node->parent = parent;
466 txschq_node->level = lvl;
467 txschq_node->classid = OTX2_QOS_CLASS_NONE;
468 WRITE_ONCE(txschq_node->qid, OTX2_QOS_QID_NONE);
469 txschq_node->rate = 0;
470 txschq_node->ceil = 0;
471 txschq_node->prio = 0;
472 txschq_node->quantum = 0;
473 txschq_node->is_static = true;
474 txschq_node->child_dwrr_prio = OTX2_QOS_DEFAULT_PRIO;
475 txschq_node->txschq_idx = OTX2_QOS_INVALID_TXSCHQ_IDX;
476
477 mutex_lock(&pfvf->qos.qos_lock);
478 list_add_tail(&txschq_node->list, &node->child_schq_list);
479 mutex_unlock(&pfvf->qos.qos_lock);
480
481 INIT_LIST_HEAD(&txschq_node->child_list);
482 INIT_LIST_HEAD(&txschq_node->child_schq_list);
483 parent = txschq_node;
484 }
485
486 return 0;
487
488err_out:
489 list_for_each_entry_safe(txschq_node, tmp, &node->child_schq_list,
490 list) {
491 list_del(&txschq_node->list);
492 kfree(txschq_node);
493 }
494 return -ENOMEM;
495}
496
497static struct otx2_qos_node *
498otx2_qos_sw_create_leaf_node(struct otx2_nic *pfvf,
499 struct otx2_qos_node *parent,
500 u16 classid, u32 prio, u64 rate, u64 ceil,
501 u32 quantum, u16 qid, bool static_cfg)
502{
503 struct otx2_qos_node *node;
504 int err;
505
506 node = kzalloc(sizeof(*node), GFP_KERNEL);
507 if (!node)
508 return ERR_PTR(-ENOMEM);
509
510 node->parent = parent;
511 node->level = parent->level - 1;
512 node->classid = classid;
513 WRITE_ONCE(node->qid, qid);
514
515 node->rate = otx2_convert_rate(rate);
516 node->ceil = otx2_convert_rate(ceil);
517 node->prio = prio;
518 node->quantum = quantum;
519 node->is_static = static_cfg;
520 node->child_dwrr_prio = OTX2_QOS_DEFAULT_PRIO;
521 node->txschq_idx = OTX2_QOS_INVALID_TXSCHQ_IDX;
522
523 __set_bit(qid, pfvf->qos.qos_sq_bmap);
524
525 hash_add_rcu(pfvf->qos.qos_hlist, &node->hlist, classid);
526
527 mutex_lock(&pfvf->qos.qos_lock);
528 err = otx2_qos_add_child_node(parent, node);
529 if (err) {
530 mutex_unlock(&pfvf->qos.qos_lock);
531 return ERR_PTR(err);
532 }
533 mutex_unlock(&pfvf->qos.qos_lock);
534
535 INIT_LIST_HEAD(&node->child_list);
536 INIT_LIST_HEAD(&node->child_schq_list);
537
538 err = otx2_qos_alloc_txschq_node(pfvf, node);
539 if (err) {
540 otx2_qos_sw_node_delete(pfvf, node);
541 return ERR_PTR(-ENOMEM);
542 }
543
544 return node;
545}
546
547static struct otx2_qos_node
548*otx2_sw_node_find_by_qid(struct otx2_nic *pfvf, u16 qid)
549{
550 struct otx2_qos_node *node = NULL;
551 int bkt;
552
553 hash_for_each(pfvf->qos.qos_hlist, bkt, node, hlist) {
554 if (node->qid == qid)
555 break;
556 }
557
558 return node;
559}
560
561static struct otx2_qos_node *
562otx2_sw_node_find(struct otx2_nic *pfvf, u32 classid)
563{
564 struct otx2_qos_node *node = NULL;
565
566 hash_for_each_possible(pfvf->qos.qos_hlist, node, hlist, classid) {
567 if (node->classid == classid)
568 break;
569 }
570
571 return node;
572}
573
574static struct otx2_qos_node *
575otx2_sw_node_find_rcu(struct otx2_nic *pfvf, u32 classid)
576{
577 struct otx2_qos_node *node = NULL;
578
579 hash_for_each_possible_rcu(pfvf->qos.qos_hlist, node, hlist, classid) {
580 if (node->classid == classid)
581 break;
582 }
583
584 return node;
585}
586
587int otx2_get_txq_by_classid(struct otx2_nic *pfvf, u16 classid)
588{
589 struct otx2_qos_node *node;
590 u16 qid;
591 int res;
592
593 node = otx2_sw_node_find_rcu(pfvf, classid);
594 if (!node) {
595 res = -ENOENT;
596 goto out;
597 }
598 qid = READ_ONCE(node->qid);
599 if (qid == OTX2_QOS_QID_INNER) {
600 res = -EINVAL;
601 goto out;
602 }
603 res = pfvf->hw.tx_queues + qid;
604out:
605 return res;
606}
607
608static int
609otx2_qos_txschq_config(struct otx2_nic *pfvf, struct otx2_qos_node *node)
610{
611 struct mbox *mbox = &pfvf->mbox;
612 struct nix_txschq_config *req;
613 int rc;
614
615 mutex_lock(&mbox->lock);
616
617 req = otx2_mbox_alloc_msg_nix_txschq_cfg(&pfvf->mbox);
618 if (!req) {
619 mutex_unlock(&mbox->lock);
620 return -ENOMEM;
621 }
622
623 req->lvl = node->level;
624 __otx2_qos_txschq_cfg(pfvf, node, req);
625
626 rc = otx2_sync_mbox_msg(&pfvf->mbox);
627
628 mutex_unlock(&mbox->lock);
629
630 return rc;
631}
632
633static int otx2_qos_txschq_alloc(struct otx2_nic *pfvf,
634 struct otx2_qos_cfg *cfg)
635{
636 struct nix_txsch_alloc_req *req;
637 struct nix_txsch_alloc_rsp *rsp;
638 struct mbox *mbox = &pfvf->mbox;
639 int lvl, rc, schq;
640
641 mutex_lock(&mbox->lock);
642 req = otx2_mbox_alloc_msg_nix_txsch_alloc(&pfvf->mbox);
643 if (!req) {
644 mutex_unlock(&mbox->lock);
645 return -ENOMEM;
646 }
647
648 for (lvl = 0; lvl < NIX_TXSCH_LVL_CNT; lvl++) {
649 req->schq[lvl] = cfg->schq[lvl];
650 req->schq_contig[lvl] = cfg->schq_contig[lvl];
651 }
652
653 rc = otx2_sync_mbox_msg(&pfvf->mbox);
654 if (rc) {
655 mutex_unlock(&mbox->lock);
656 return rc;
657 }
658
659 rsp = (struct nix_txsch_alloc_rsp *)
660 otx2_mbox_get_rsp(&pfvf->mbox.mbox, 0, &req->hdr);
661
662 if (IS_ERR(rsp)) {
663 rc = PTR_ERR(rsp);
664 goto out;
665 }
666
667 for (lvl = 0; lvl < NIX_TXSCH_LVL_CNT; lvl++) {
668 for (schq = 0; schq < rsp->schq_contig[lvl]; schq++) {
669 cfg->schq_contig_list[lvl][schq] =
670 rsp->schq_contig_list[lvl][schq];
671 }
672 }
673
674 for (lvl = 0; lvl < NIX_TXSCH_LVL_CNT; lvl++) {
675 for (schq = 0; schq < rsp->schq[lvl]; schq++) {
676 cfg->schq_list[lvl][schq] =
677 rsp->schq_list[lvl][schq];
678 }
679 }
680
681 pfvf->qos.link_cfg_lvl = rsp->link_cfg_lvl;
682 pfvf->hw.txschq_aggr_lvl_rr_prio = rsp->aggr_lvl_rr_prio;
683
684out:
685 mutex_unlock(&mbox->lock);
686 return rc;
687}
688
689static void otx2_qos_free_unused_txschq(struct otx2_nic *pfvf,
690 struct otx2_qos_cfg *cfg)
691{
692 int lvl, idx, schq;
693
694 for (lvl = 0; lvl < NIX_TXSCH_LVL_CNT; lvl++) {
695 for (idx = 0; idx < cfg->schq_contig[lvl]; idx++) {
696 if (!cfg->schq_index_used[lvl][idx]) {
697 schq = cfg->schq_contig_list[lvl][idx];
698 otx2_txschq_free_one(pfvf, lvl, schq);
699 }
700 }
701 }
702}
703
704static void otx2_qos_txschq_fill_cfg_schq(struct otx2_nic *pfvf,
705 struct otx2_qos_node *node,
706 struct otx2_qos_cfg *cfg)
707{
708 struct otx2_qos_node *tmp;
709 int cnt;
710
711 list_for_each_entry(tmp, &node->child_schq_list, list) {
712 cnt = cfg->dwrr_node_pos[tmp->level];
713 tmp->schq = cfg->schq_list[tmp->level][cnt];
714 cfg->dwrr_node_pos[tmp->level]++;
715 }
716}
717
718static void otx2_qos_txschq_fill_cfg_tl(struct otx2_nic *pfvf,
719 struct otx2_qos_node *node,
720 struct otx2_qos_cfg *cfg)
721{
722 struct otx2_qos_node *tmp;
723 int cnt;
724
725 list_for_each_entry(tmp, &node->child_list, list) {
726 otx2_qos_txschq_fill_cfg_tl(pfvf, tmp, cfg);
727 cnt = cfg->static_node_pos[tmp->level];
728 tmp->schq = cfg->schq_contig_list[tmp->level][tmp->txschq_idx];
729 cfg->schq_index_used[tmp->level][tmp->txschq_idx] = true;
730 if (cnt == 0)
731 node->prio_anchor =
732 cfg->schq_contig_list[tmp->level][0];
733 cfg->static_node_pos[tmp->level]++;
734 otx2_qos_txschq_fill_cfg_schq(pfvf, tmp, cfg);
735 }
736}
737
738static void otx2_qos_txschq_fill_cfg(struct otx2_nic *pfvf,
739 struct otx2_qos_node *node,
740 struct otx2_qos_cfg *cfg)
741{
742 mutex_lock(&pfvf->qos.qos_lock);
743 otx2_qos_txschq_fill_cfg_tl(pfvf, node, cfg);
744 otx2_qos_txschq_fill_cfg_schq(pfvf, node, cfg);
745 otx2_qos_free_unused_txschq(pfvf, cfg);
746 mutex_unlock(&pfvf->qos.qos_lock);
747}
748
749static void __otx2_qos_assign_base_idx_tl(struct otx2_nic *pfvf,
750 struct otx2_qos_node *tmp,
751 unsigned long *child_idx_bmap,
752 int child_cnt)
753{
754 int idx;
755
756 if (tmp->txschq_idx != OTX2_QOS_INVALID_TXSCHQ_IDX)
757 return;
758
759 /* assign static nodes 1:1 prio mapping first, then remaining nodes */
760 for (idx = 0; idx < child_cnt; idx++) {
761 if (tmp->is_static && tmp->prio == idx &&
762 !test_bit(idx, child_idx_bmap)) {
763 tmp->txschq_idx = idx;
764 set_bit(idx, child_idx_bmap);
765 return;
766 } else if (!tmp->is_static && idx >= tmp->prio &&
767 !test_bit(idx, child_idx_bmap)) {
768 tmp->txschq_idx = idx;
769 set_bit(idx, child_idx_bmap);
770 return;
771 }
772 }
773}
774
775static int otx2_qos_assign_base_idx_tl(struct otx2_nic *pfvf,
776 struct otx2_qos_node *node)
777{
778 unsigned long *child_idx_bmap;
779 struct otx2_qos_node *tmp;
780 int child_cnt;
781
782 list_for_each_entry(tmp, &node->child_list, list)
783 tmp->txschq_idx = OTX2_QOS_INVALID_TXSCHQ_IDX;
784
785 /* allocate child index array */
786 child_cnt = node->child_dwrr_cnt + node->max_static_prio + 1;
787 child_idx_bmap = kcalloc(BITS_TO_LONGS(child_cnt),
788 sizeof(unsigned long),
789 GFP_KERNEL);
790 if (!child_idx_bmap)
791 return -ENOMEM;
792
793 list_for_each_entry(tmp, &node->child_list, list)
794 otx2_qos_assign_base_idx_tl(pfvf, tmp);
795
796 /* assign base index of static priority children first */
797 list_for_each_entry(tmp, &node->child_list, list) {
798 if (!tmp->is_static)
799 continue;
800 __otx2_qos_assign_base_idx_tl(pfvf, tmp, child_idx_bmap,
801 child_cnt);
802 }
803
804 /* assign base index of dwrr priority children */
805 list_for_each_entry(tmp, &node->child_list, list)
806 __otx2_qos_assign_base_idx_tl(pfvf, tmp, child_idx_bmap,
807 child_cnt);
808
809 kfree(child_idx_bmap);
810
811 return 0;
812}
813
814static int otx2_qos_assign_base_idx(struct otx2_nic *pfvf,
815 struct otx2_qos_node *node)
816{
817 int ret = 0;
818
819 mutex_lock(&pfvf->qos.qos_lock);
820 ret = otx2_qos_assign_base_idx_tl(pfvf, node);
821 mutex_unlock(&pfvf->qos.qos_lock);
822
823 return ret;
824}
825
826static int otx2_qos_txschq_push_cfg_schq(struct otx2_nic *pfvf,
827 struct otx2_qos_node *node,
828 struct otx2_qos_cfg *cfg)
829{
830 struct otx2_qos_node *tmp;
831 int ret;
832
833 list_for_each_entry(tmp, &node->child_schq_list, list) {
834 ret = otx2_qos_txschq_config(pfvf, tmp);
835 if (ret)
836 return -EIO;
837 ret = otx2_qos_txschq_set_parent_topology(pfvf, tmp->parent);
838 if (ret)
839 return -EIO;
840 }
841
842 return 0;
843}
844
845static int otx2_qos_txschq_push_cfg_tl(struct otx2_nic *pfvf,
846 struct otx2_qos_node *node,
847 struct otx2_qos_cfg *cfg)
848{
849 struct otx2_qos_node *tmp;
850 int ret;
851
852 list_for_each_entry(tmp, &node->child_list, list) {
853 ret = otx2_qos_txschq_push_cfg_tl(pfvf, tmp, cfg);
854 if (ret)
855 return -EIO;
856 ret = otx2_qos_txschq_config(pfvf, tmp);
857 if (ret)
858 return -EIO;
859 ret = otx2_qos_txschq_push_cfg_schq(pfvf, tmp, cfg);
860 if (ret)
861 return -EIO;
862 }
863
864 ret = otx2_qos_txschq_set_parent_topology(pfvf, node);
865 if (ret)
866 return -EIO;
867
868 return 0;
869}
870
871static int otx2_qos_txschq_push_cfg(struct otx2_nic *pfvf,
872 struct otx2_qos_node *node,
873 struct otx2_qos_cfg *cfg)
874{
875 int ret;
876
877 mutex_lock(&pfvf->qos.qos_lock);
878 ret = otx2_qos_txschq_push_cfg_tl(pfvf, node, cfg);
879 if (ret)
880 goto out;
881 ret = otx2_qos_txschq_push_cfg_schq(pfvf, node, cfg);
882out:
883 mutex_unlock(&pfvf->qos.qos_lock);
884 return ret;
885}
886
887static int otx2_qos_txschq_update_config(struct otx2_nic *pfvf,
888 struct otx2_qos_node *node,
889 struct otx2_qos_cfg *cfg)
890{
891 otx2_qos_txschq_fill_cfg(pfvf, node, cfg);
892
893 return otx2_qos_txschq_push_cfg(pfvf, node, cfg);
894}
895
896static int otx2_qos_txschq_update_root_cfg(struct otx2_nic *pfvf,
897 struct otx2_qos_node *root,
898 struct otx2_qos_cfg *cfg)
899{
900 root->schq = cfg->schq_list[root->level][0];
901 return otx2_qos_txschq_config(pfvf, root);
902}
903
904static void otx2_qos_free_cfg(struct otx2_nic *pfvf, struct otx2_qos_cfg *cfg)
905{
906 int lvl, idx, schq;
907
908 for (lvl = 0; lvl < NIX_TXSCH_LVL_CNT; lvl++) {
909 for (idx = 0; idx < cfg->schq[lvl]; idx++) {
910 schq = cfg->schq_list[lvl][idx];
911 otx2_txschq_free_one(pfvf, lvl, schq);
912 }
913 }
914
915 for (lvl = 0; lvl < NIX_TXSCH_LVL_CNT; lvl++) {
916 for (idx = 0; idx < cfg->schq_contig[lvl]; idx++) {
917 if (cfg->schq_index_used[lvl][idx]) {
918 schq = cfg->schq_contig_list[lvl][idx];
919 otx2_txschq_free_one(pfvf, lvl, schq);
920 }
921 }
922 }
923}
924
925static void otx2_qos_enadis_sq(struct otx2_nic *pfvf,
926 struct otx2_qos_node *node,
927 u16 qid)
928{
929 if (pfvf->qos.qid_to_sqmap[qid] != OTX2_QOS_INVALID_SQ)
930 otx2_qos_disable_sq(pfvf, qid);
931
932 pfvf->qos.qid_to_sqmap[qid] = node->schq;
933 otx2_qos_txschq_config(pfvf, node);
934 otx2_qos_enable_sq(pfvf, qid);
935}
936
937static void otx2_qos_update_smq_schq(struct otx2_nic *pfvf,
938 struct otx2_qos_node *node,
939 bool action)
940{
941 struct otx2_qos_node *tmp;
942
943 if (node->qid == OTX2_QOS_QID_INNER)
944 return;
945
946 list_for_each_entry(tmp, &node->child_schq_list, list) {
947 if (tmp->level == NIX_TXSCH_LVL_MDQ) {
948 if (action == QOS_SMQ_FLUSH)
949 otx2_smq_flush(pfvf, tmp->schq);
950 else
951 otx2_qos_enadis_sq(pfvf, tmp, node->qid);
952 }
953 }
954}
955
956static void __otx2_qos_update_smq(struct otx2_nic *pfvf,
957 struct otx2_qos_node *node,
958 bool action)
959{
960 struct otx2_qos_node *tmp;
961
962 list_for_each_entry(tmp, &node->child_list, list) {
963 __otx2_qos_update_smq(pfvf, tmp, action);
964 if (tmp->qid == OTX2_QOS_QID_INNER)
965 continue;
966 if (tmp->level == NIX_TXSCH_LVL_MDQ) {
967 if (action == QOS_SMQ_FLUSH)
968 otx2_smq_flush(pfvf, tmp->schq);
969 else
970 otx2_qos_enadis_sq(pfvf, tmp, tmp->qid);
971 } else {
972 otx2_qos_update_smq_schq(pfvf, tmp, action);
973 }
974 }
975}
976
977static void otx2_qos_update_smq(struct otx2_nic *pfvf,
978 struct otx2_qos_node *node,
979 bool action)
980{
981 mutex_lock(&pfvf->qos.qos_lock);
982 __otx2_qos_update_smq(pfvf, node, action);
983 otx2_qos_update_smq_schq(pfvf, node, action);
984 mutex_unlock(&pfvf->qos.qos_lock);
985}
986
987static int otx2_qos_push_txschq_cfg(struct otx2_nic *pfvf,
988 struct otx2_qos_node *node,
989 struct otx2_qos_cfg *cfg)
990{
991 int ret;
992
993 ret = otx2_qos_txschq_alloc(pfvf, cfg);
994 if (ret)
995 return -ENOSPC;
996
997 ret = otx2_qos_assign_base_idx(pfvf, node);
998 if (ret)
999 return -ENOMEM;
1000
1001 if (!(pfvf->netdev->flags & IFF_UP)) {
1002 otx2_qos_txschq_fill_cfg(pfvf, node, cfg);
1003 return 0;
1004 }
1005
1006 ret = otx2_qos_txschq_update_config(pfvf, node, cfg);
1007 if (ret) {
1008 otx2_qos_free_cfg(pfvf, cfg);
1009 return -EIO;
1010 }
1011
1012 otx2_qos_update_smq(pfvf, node, QOS_CFG_SQ);
1013
1014 return 0;
1015}
1016
1017static int otx2_qos_update_tree(struct otx2_nic *pfvf,
1018 struct otx2_qos_node *node,
1019 struct otx2_qos_cfg *cfg)
1020{
1021 otx2_qos_prepare_txschq_cfg(pfvf, node->parent, cfg);
1022 return otx2_qos_push_txschq_cfg(pfvf, node->parent, cfg);
1023}
1024
1025static int otx2_qos_root_add(struct otx2_nic *pfvf, u16 htb_maj_id, u16 htb_defcls,
1026 struct netlink_ext_ack *extack)
1027{
1028 struct otx2_qos_cfg *new_cfg;
1029 struct otx2_qos_node *root;
1030 int err;
1031
1032 netdev_dbg(pfvf->netdev,
1033 "TC_HTB_CREATE: handle=0x%x defcls=0x%x\n",
1034 htb_maj_id, htb_defcls);
1035
1036 root = otx2_qos_alloc_root(pfvf);
1037 if (IS_ERR(root)) {
1038 err = PTR_ERR(root);
1039 return err;
1040 }
1041
1042 /* allocate txschq queue */
1043 new_cfg = kzalloc(sizeof(*new_cfg), GFP_KERNEL);
1044 if (!new_cfg) {
1045 NL_SET_ERR_MSG_MOD(extack, "Memory allocation error");
1046 err = -ENOMEM;
1047 goto free_root_node;
1048 }
1049 /* allocate htb root node */
1050 new_cfg->schq[root->level] = 1;
1051 err = otx2_qos_txschq_alloc(pfvf, new_cfg);
1052 if (err) {
1053 NL_SET_ERR_MSG_MOD(extack, "Error allocating txschq");
1054 goto free_root_node;
1055 }
1056
1057 /* Update TL1 RR PRIO */
1058 if (root->level == NIX_TXSCH_LVL_TL1) {
1059 root->child_dwrr_prio = pfvf->hw.txschq_aggr_lvl_rr_prio;
1060 netdev_dbg(pfvf->netdev,
1061 "TL1 DWRR Priority %d\n", root->child_dwrr_prio);
1062 }
1063
1064 if (!(pfvf->netdev->flags & IFF_UP) ||
1065 root->level == NIX_TXSCH_LVL_TL1) {
1066 root->schq = new_cfg->schq_list[root->level][0];
1067 goto out;
1068 }
1069
1070 /* update the txschq configuration in hw */
1071 err = otx2_qos_txschq_update_root_cfg(pfvf, root, new_cfg);
1072 if (err) {
1073 NL_SET_ERR_MSG_MOD(extack,
1074 "Error updating txschq configuration");
1075 goto txschq_free;
1076 }
1077
1078out:
1079 WRITE_ONCE(pfvf->qos.defcls, htb_defcls);
1080 /* Pairs with smp_load_acquire() in ndo_select_queue */
1081 smp_store_release(&pfvf->qos.maj_id, htb_maj_id);
1082 kfree(new_cfg);
1083 return 0;
1084
1085txschq_free:
1086 otx2_qos_free_cfg(pfvf, new_cfg);
1087free_root_node:
1088 kfree(new_cfg);
1089 otx2_qos_sw_node_delete(pfvf, root);
1090 return err;
1091}
1092
1093static int otx2_qos_root_destroy(struct otx2_nic *pfvf)
1094{
1095 struct otx2_qos_node *root;
1096
1097 netdev_dbg(pfvf->netdev, "TC_HTB_DESTROY\n");
1098
1099 /* find root node */
1100 root = otx2_sw_node_find(pfvf, OTX2_QOS_ROOT_CLASSID);
1101 if (!root)
1102 return -ENOENT;
1103
1104 /* free the hw mappings */
1105 otx2_qos_destroy_node(pfvf, root);
1106
1107 return 0;
1108}
1109
1110static int otx2_qos_validate_quantum(struct otx2_nic *pfvf, u32 quantum)
1111{
1112 u32 rr_weight = otx2_qos_quantum_to_dwrr_weight(pfvf, quantum);
1113 int err = 0;
1114
1115 /* Max Round robin weight supported by octeontx2 and CN10K
1116 * is different. Validate accordingly
1117 */
1118 if (is_dev_otx2(pfvf->pdev))
1119 err = (rr_weight > OTX2_MAX_RR_QUANTUM) ? -EINVAL : 0;
1120 else if (rr_weight > CN10K_MAX_RR_WEIGHT)
1121 err = -EINVAL;
1122
1123 return err;
1124}
1125
1126static int otx2_qos_validate_dwrr_cfg(struct otx2_qos_node *parent,
1127 struct netlink_ext_ack *extack,
1128 struct otx2_nic *pfvf,
1129 u64 prio, u64 quantum)
1130{
1131 int err;
1132
1133 err = otx2_qos_validate_quantum(pfvf, quantum);
1134 if (err) {
1135 NL_SET_ERR_MSG_MOD(extack, "Unsupported quantum value");
1136 return err;
1137 }
1138
1139 if (parent->child_dwrr_prio == OTX2_QOS_DEFAULT_PRIO) {
1140 parent->child_dwrr_prio = prio;
1141 } else if (prio != parent->child_dwrr_prio) {
1142 NL_SET_ERR_MSG_MOD(extack, "Only one DWRR group is allowed");
1143 return -EOPNOTSUPP;
1144 }
1145
1146 return 0;
1147}
1148
1149static int otx2_qos_validate_configuration(struct otx2_qos_node *parent,
1150 struct netlink_ext_ack *extack,
1151 struct otx2_nic *pfvf,
1152 u64 prio, bool static_cfg)
1153{
1154 if (prio == parent->child_dwrr_prio && static_cfg) {
1155 NL_SET_ERR_MSG_MOD(extack, "DWRR child group with same priority exists");
1156 return -EEXIST;
1157 }
1158
1159 if (static_cfg && test_bit(prio, parent->prio_bmap)) {
1160 NL_SET_ERR_MSG_MOD(extack,
1161 "Static priority child with same priority exists");
1162 return -EEXIST;
1163 }
1164
1165 return 0;
1166}
1167
1168static void otx2_reset_dwrr_prio(struct otx2_qos_node *parent, u64 prio)
1169{
1170 /* For PF, root node dwrr priority is static */
1171 if (parent->level == NIX_TXSCH_LVL_TL1)
1172 return;
1173
1174 if (parent->child_dwrr_prio != OTX2_QOS_DEFAULT_PRIO) {
1175 parent->child_dwrr_prio = OTX2_QOS_DEFAULT_PRIO;
1176 clear_bit(prio, parent->prio_bmap);
1177 }
1178}
1179
1180static bool is_qos_node_dwrr(struct otx2_qos_node *parent,
1181 struct otx2_nic *pfvf,
1182 u64 prio)
1183{
1184 struct otx2_qos_node *node;
1185 bool ret = false;
1186
1187 if (parent->child_dwrr_prio == prio)
1188 return true;
1189
1190 mutex_lock(&pfvf->qos.qos_lock);
1191 list_for_each_entry(node, &parent->child_list, list) {
1192 if (prio == node->prio) {
1193 if (parent->child_dwrr_prio != OTX2_QOS_DEFAULT_PRIO &&
1194 parent->child_dwrr_prio != prio)
1195 continue;
1196
1197 if (otx2_qos_validate_quantum(pfvf, node->quantum)) {
1198 netdev_err(pfvf->netdev,
1199 "Unsupported quantum value for existing classid=0x%x quantum=%d prio=%d",
1200 node->classid, node->quantum,
1201 node->prio);
1202 break;
1203 }
1204 /* mark old node as dwrr */
1205 node->is_static = false;
1206 parent->child_dwrr_cnt++;
1207 parent->child_static_cnt--;
1208 ret = true;
1209 break;
1210 }
1211 }
1212 mutex_unlock(&pfvf->qos.qos_lock);
1213
1214 return ret;
1215}
1216
1217static int otx2_qos_leaf_alloc_queue(struct otx2_nic *pfvf, u16 classid,
1218 u32 parent_classid, u64 rate, u64 ceil,
1219 u64 prio, u32 quantum,
1220 struct netlink_ext_ack *extack)
1221{
1222 struct otx2_qos_cfg *old_cfg, *new_cfg;
1223 struct otx2_qos_node *node, *parent;
1224 int qid, ret, err;
1225 bool static_cfg;
1226
1227 netdev_dbg(pfvf->netdev,
1228 "TC_HTB_LEAF_ALLOC_QUEUE: classid=0x%x parent_classid=0x%x rate=%lld ceil=%lld prio=%lld quantum=%d\n",
1229 classid, parent_classid, rate, ceil, prio, quantum);
1230
1231 if (prio > OTX2_QOS_MAX_PRIO) {
1232 NL_SET_ERR_MSG_MOD(extack, "Valid priority range 0 to 7");
1233 ret = -EOPNOTSUPP;
1234 goto out;
1235 }
1236
1237 if (!quantum || quantum > INT_MAX) {
1238 NL_SET_ERR_MSG_MOD(extack, "Invalid quantum, range 1 - 2147483647 bytes");
1239 ret = -EOPNOTSUPP;
1240 goto out;
1241 }
1242
1243 /* get parent node */
1244 parent = otx2_sw_node_find(pfvf, parent_classid);
1245 if (!parent) {
1246 NL_SET_ERR_MSG_MOD(extack, "parent node not found");
1247 ret = -ENOENT;
1248 goto out;
1249 }
1250 if (parent->level == NIX_TXSCH_LVL_MDQ) {
1251 NL_SET_ERR_MSG_MOD(extack, "HTB qos max levels reached");
1252 ret = -EOPNOTSUPP;
1253 goto out;
1254 }
1255
1256 static_cfg = !is_qos_node_dwrr(parent, pfvf, prio);
1257 ret = otx2_qos_validate_configuration(parent, extack, pfvf, prio,
1258 static_cfg);
1259 if (ret)
1260 goto out;
1261
1262 if (!static_cfg) {
1263 ret = otx2_qos_validate_dwrr_cfg(parent, extack, pfvf, prio,
1264 quantum);
1265 if (ret)
1266 goto out;
1267 }
1268
1269 if (static_cfg)
1270 parent->child_static_cnt++;
1271 else
1272 parent->child_dwrr_cnt++;
1273
1274 set_bit(prio, parent->prio_bmap);
1275
1276 /* read current txschq configuration */
1277 old_cfg = kzalloc(sizeof(*old_cfg), GFP_KERNEL);
1278 if (!old_cfg) {
1279 NL_SET_ERR_MSG_MOD(extack, "Memory allocation error");
1280 ret = -ENOMEM;
1281 goto reset_prio;
1282 }
1283 otx2_qos_read_txschq_cfg(pfvf, parent, old_cfg);
1284
1285 /* allocate a new sq */
1286 qid = otx2_qos_get_qid(pfvf);
1287 if (qid < 0) {
1288 NL_SET_ERR_MSG_MOD(extack, "Reached max supported QOS SQ's");
1289 ret = -ENOMEM;
1290 goto free_old_cfg;
1291 }
1292
1293 /* Actual SQ mapping will be updated after SMQ alloc */
1294 pfvf->qos.qid_to_sqmap[qid] = OTX2_QOS_INVALID_SQ;
1295
1296 /* allocate and initialize a new child node */
1297 node = otx2_qos_sw_create_leaf_node(pfvf, parent, classid, prio, rate,
1298 ceil, quantum, qid, static_cfg);
1299 if (IS_ERR(node)) {
1300 NL_SET_ERR_MSG_MOD(extack, "Unable to allocate leaf node");
1301 ret = PTR_ERR(node);
1302 goto free_old_cfg;
1303 }
1304
1305 /* push new txschq config to hw */
1306 new_cfg = kzalloc(sizeof(*new_cfg), GFP_KERNEL);
1307 if (!new_cfg) {
1308 NL_SET_ERR_MSG_MOD(extack, "Memory allocation error");
1309 ret = -ENOMEM;
1310 goto free_node;
1311 }
1312 ret = otx2_qos_update_tree(pfvf, node, new_cfg);
1313 if (ret) {
1314 NL_SET_ERR_MSG_MOD(extack, "HTB HW configuration error");
1315 kfree(new_cfg);
1316 otx2_qos_sw_node_delete(pfvf, node);
1317 /* restore the old qos tree */
1318 err = otx2_qos_txschq_update_config(pfvf, parent, old_cfg);
1319 if (err) {
1320 netdev_err(pfvf->netdev,
1321 "Failed to restore txcshq configuration");
1322 goto free_old_cfg;
1323 }
1324
1325 otx2_qos_update_smq(pfvf, parent, QOS_CFG_SQ);
1326 goto free_old_cfg;
1327 }
1328
1329 /* update tx_real_queues */
1330 otx2_qos_update_tx_netdev_queues(pfvf);
1331
1332 /* free new txschq config */
1333 kfree(new_cfg);
1334
1335 /* free old txschq config */
1336 otx2_qos_free_cfg(pfvf, old_cfg);
1337 kfree(old_cfg);
1338
1339 return pfvf->hw.tx_queues + qid;
1340
1341free_node:
1342 otx2_qos_sw_node_delete(pfvf, node);
1343free_old_cfg:
1344 kfree(old_cfg);
1345reset_prio:
1346 if (static_cfg)
1347 parent->child_static_cnt--;
1348 else
1349 parent->child_dwrr_cnt--;
1350
1351 clear_bit(prio, parent->prio_bmap);
1352out:
1353 return ret;
1354}
1355
1356static int otx2_qos_leaf_to_inner(struct otx2_nic *pfvf, u16 classid,
1357 u16 child_classid, u64 rate, u64 ceil, u64 prio,
1358 u32 quantum, struct netlink_ext_ack *extack)
1359{
1360 struct otx2_qos_cfg *old_cfg, *new_cfg;
1361 struct otx2_qos_node *node, *child;
1362 bool static_cfg;
1363 int ret, err;
1364 u16 qid;
1365
1366 netdev_dbg(pfvf->netdev,
1367 "TC_HTB_LEAF_TO_INNER classid %04x, child %04x, rate %llu, ceil %llu\n",
1368 classid, child_classid, rate, ceil);
1369
1370 if (prio > OTX2_QOS_MAX_PRIO) {
1371 NL_SET_ERR_MSG_MOD(extack, "Valid priority range 0 to 7");
1372 ret = -EOPNOTSUPP;
1373 goto out;
1374 }
1375
1376 if (!quantum || quantum > INT_MAX) {
1377 NL_SET_ERR_MSG_MOD(extack, "Invalid quantum, range 1 - 2147483647 bytes");
1378 ret = -EOPNOTSUPP;
1379 goto out;
1380 }
1381
1382 /* find node related to classid */
1383 node = otx2_sw_node_find(pfvf, classid);
1384 if (!node) {
1385 NL_SET_ERR_MSG_MOD(extack, "HTB node not found");
1386 ret = -ENOENT;
1387 goto out;
1388 }
1389 /* check max qos txschq level */
1390 if (node->level == NIX_TXSCH_LVL_MDQ) {
1391 NL_SET_ERR_MSG_MOD(extack, "HTB qos level not supported");
1392 ret = -EOPNOTSUPP;
1393 goto out;
1394 }
1395
1396 static_cfg = !is_qos_node_dwrr(node, pfvf, prio);
1397 if (!static_cfg) {
1398 ret = otx2_qos_validate_dwrr_cfg(node, extack, pfvf, prio,
1399 quantum);
1400 if (ret)
1401 goto out;
1402 }
1403
1404 if (static_cfg)
1405 node->child_static_cnt++;
1406 else
1407 node->child_dwrr_cnt++;
1408
1409 set_bit(prio, node->prio_bmap);
1410
1411 /* store the qid to assign to leaf node */
1412 qid = node->qid;
1413
1414 /* read current txschq configuration */
1415 old_cfg = kzalloc(sizeof(*old_cfg), GFP_KERNEL);
1416 if (!old_cfg) {
1417 NL_SET_ERR_MSG_MOD(extack, "Memory allocation error");
1418 ret = -ENOMEM;
1419 goto reset_prio;
1420 }
1421 otx2_qos_read_txschq_cfg(pfvf, node, old_cfg);
1422
1423 /* delete the txschq nodes allocated for this node */
1424 otx2_qos_disable_sq(pfvf, qid);
1425 otx2_qos_free_hw_node_schq(pfvf, node);
1426 otx2_qos_free_sw_node_schq(pfvf, node);
1427 pfvf->qos.qid_to_sqmap[qid] = OTX2_QOS_INVALID_SQ;
1428
1429 /* mark this node as htb inner node */
1430 WRITE_ONCE(node->qid, OTX2_QOS_QID_INNER);
1431
1432 /* allocate and initialize a new child node */
1433 child = otx2_qos_sw_create_leaf_node(pfvf, node, child_classid,
1434 prio, rate, ceil, quantum,
1435 qid, static_cfg);
1436 if (IS_ERR(child)) {
1437 NL_SET_ERR_MSG_MOD(extack, "Unable to allocate leaf node");
1438 ret = PTR_ERR(child);
1439 goto free_old_cfg;
1440 }
1441
1442 /* push new txschq config to hw */
1443 new_cfg = kzalloc(sizeof(*new_cfg), GFP_KERNEL);
1444 if (!new_cfg) {
1445 NL_SET_ERR_MSG_MOD(extack, "Memory allocation error");
1446 ret = -ENOMEM;
1447 goto free_node;
1448 }
1449 ret = otx2_qos_update_tree(pfvf, child, new_cfg);
1450 if (ret) {
1451 NL_SET_ERR_MSG_MOD(extack, "HTB HW configuration error");
1452 kfree(new_cfg);
1453 otx2_qos_sw_node_delete(pfvf, child);
1454 /* restore the old qos tree */
1455 WRITE_ONCE(node->qid, qid);
1456 err = otx2_qos_alloc_txschq_node(pfvf, node);
1457 if (err) {
1458 netdev_err(pfvf->netdev,
1459 "Failed to restore old leaf node");
1460 goto free_old_cfg;
1461 }
1462 err = otx2_qos_txschq_update_config(pfvf, node, old_cfg);
1463 if (err) {
1464 netdev_err(pfvf->netdev,
1465 "Failed to restore txcshq configuration");
1466 goto free_old_cfg;
1467 }
1468 otx2_qos_update_smq(pfvf, node, QOS_CFG_SQ);
1469 goto free_old_cfg;
1470 }
1471
1472 /* free new txschq config */
1473 kfree(new_cfg);
1474
1475 /* free old txschq config */
1476 otx2_qos_free_cfg(pfvf, old_cfg);
1477 kfree(old_cfg);
1478
1479 return 0;
1480
1481free_node:
1482 otx2_qos_sw_node_delete(pfvf, child);
1483free_old_cfg:
1484 kfree(old_cfg);
1485reset_prio:
1486 if (static_cfg)
1487 node->child_static_cnt--;
1488 else
1489 node->child_dwrr_cnt--;
1490 clear_bit(prio, node->prio_bmap);
1491out:
1492 return ret;
1493}
1494
1495static int otx2_qos_cur_leaf_nodes(struct otx2_nic *pfvf)
1496{
1497 int last = find_last_bit(pfvf->qos.qos_sq_bmap, pfvf->hw.tc_tx_queues);
1498
1499 return last == pfvf->hw.tc_tx_queues ? 0 : last + 1;
1500}
1501
1502static void otx2_reset_qdisc(struct net_device *dev, u16 qid)
1503{
1504 struct netdev_queue *dev_queue = netdev_get_tx_queue(dev, qid);
1505 struct Qdisc *qdisc = rtnl_dereference(dev_queue->qdisc_sleeping);
1506
1507 if (!qdisc)
1508 return;
1509
1510 spin_lock_bh(qdisc_lock(qdisc));
1511 qdisc_reset(qdisc);
1512 spin_unlock_bh(qdisc_lock(qdisc));
1513}
1514
1515static void otx2_cfg_smq(struct otx2_nic *pfvf, struct otx2_qos_node *node,
1516 int qid)
1517{
1518 struct otx2_qos_node *tmp;
1519
1520 list_for_each_entry(tmp, &node->child_schq_list, list)
1521 if (tmp->level == NIX_TXSCH_LVL_MDQ) {
1522 otx2_qos_txschq_config(pfvf, tmp);
1523 pfvf->qos.qid_to_sqmap[qid] = tmp->schq;
1524 }
1525}
1526
1527static int otx2_qos_leaf_del(struct otx2_nic *pfvf, u16 *classid,
1528 struct netlink_ext_ack *extack)
1529{
1530 struct otx2_qos_node *node, *parent;
1531 int dwrr_del_node = false;
1532 u16 qid, moved_qid;
1533 u64 prio;
1534
1535 netdev_dbg(pfvf->netdev, "TC_HTB_LEAF_DEL classid %04x\n", *classid);
1536
1537 /* find node related to classid */
1538 node = otx2_sw_node_find(pfvf, *classid);
1539 if (!node) {
1540 NL_SET_ERR_MSG_MOD(extack, "HTB node not found");
1541 return -ENOENT;
1542 }
1543 parent = node->parent;
1544 prio = node->prio;
1545 qid = node->qid;
1546
1547 if (!node->is_static)
1548 dwrr_del_node = true;
1549
1550 otx2_qos_disable_sq(pfvf, node->qid);
1551
1552 otx2_qos_destroy_node(pfvf, node);
1553 pfvf->qos.qid_to_sqmap[qid] = OTX2_QOS_INVALID_SQ;
1554
1555 if (dwrr_del_node) {
1556 parent->child_dwrr_cnt--;
1557 } else {
1558 parent->child_static_cnt--;
1559 clear_bit(prio, parent->prio_bmap);
1560 }
1561
1562 /* Reset DWRR priority if all dwrr nodes are deleted */
1563 if (!parent->child_dwrr_cnt)
1564 otx2_reset_dwrr_prio(parent, prio);
1565
1566 if (!parent->child_static_cnt)
1567 parent->max_static_prio = 0;
1568
1569 moved_qid = otx2_qos_cur_leaf_nodes(pfvf);
1570
1571 /* last node just deleted */
1572 if (moved_qid == 0 || moved_qid == qid)
1573 return 0;
1574
1575 moved_qid--;
1576
1577 node = otx2_sw_node_find_by_qid(pfvf, moved_qid);
1578 if (!node)
1579 return 0;
1580
1581 /* stop traffic to the old queue and disable
1582 * SQ associated with it
1583 */
1584 node->qid = OTX2_QOS_QID_INNER;
1585 __clear_bit(moved_qid, pfvf->qos.qos_sq_bmap);
1586 otx2_qos_disable_sq(pfvf, moved_qid);
1587
1588 otx2_reset_qdisc(pfvf->netdev, pfvf->hw.tx_queues + moved_qid);
1589
1590 /* enable SQ associated with qid and
1591 * update the node
1592 */
1593 otx2_cfg_smq(pfvf, node, qid);
1594
1595 otx2_qos_enable_sq(pfvf, qid);
1596 __set_bit(qid, pfvf->qos.qos_sq_bmap);
1597 node->qid = qid;
1598
1599 *classid = node->classid;
1600 return 0;
1601}
1602
1603static int otx2_qos_leaf_del_last(struct otx2_nic *pfvf, u16 classid, bool force,
1604 struct netlink_ext_ack *extack)
1605{
1606 struct otx2_qos_node *node, *parent;
1607 struct otx2_qos_cfg *new_cfg;
1608 int dwrr_del_node = false;
1609 u64 prio;
1610 int err;
1611 u16 qid;
1612
1613 netdev_dbg(pfvf->netdev,
1614 "TC_HTB_LEAF_DEL_LAST classid %04x\n", classid);
1615
1616 /* find node related to classid */
1617 node = otx2_sw_node_find(pfvf, classid);
1618 if (!node) {
1619 NL_SET_ERR_MSG_MOD(extack, "HTB node not found");
1620 return -ENOENT;
1621 }
1622
1623 /* save qid for use by parent */
1624 qid = node->qid;
1625 prio = node->prio;
1626
1627 parent = otx2_sw_node_find(pfvf, node->parent->classid);
1628 if (!parent) {
1629 NL_SET_ERR_MSG_MOD(extack, "parent node not found");
1630 return -ENOENT;
1631 }
1632
1633 if (!node->is_static)
1634 dwrr_del_node = true;
1635
1636 /* destroy the leaf node */
1637 otx2_qos_disable_sq(pfvf, qid);
1638 otx2_qos_destroy_node(pfvf, node);
1639 pfvf->qos.qid_to_sqmap[qid] = OTX2_QOS_INVALID_SQ;
1640
1641 if (dwrr_del_node) {
1642 parent->child_dwrr_cnt--;
1643 } else {
1644 parent->child_static_cnt--;
1645 clear_bit(prio, parent->prio_bmap);
1646 }
1647
1648 /* Reset DWRR priority if all dwrr nodes are deleted */
1649 if (!parent->child_dwrr_cnt)
1650 otx2_reset_dwrr_prio(parent, prio);
1651
1652 if (!parent->child_static_cnt)
1653 parent->max_static_prio = 0;
1654
1655 /* create downstream txschq entries to parent */
1656 err = otx2_qos_alloc_txschq_node(pfvf, parent);
1657 if (err) {
1658 NL_SET_ERR_MSG_MOD(extack, "HTB failed to create txsch configuration");
1659 return err;
1660 }
1661 WRITE_ONCE(parent->qid, qid);
1662 __set_bit(qid, pfvf->qos.qos_sq_bmap);
1663
1664 /* push new txschq config to hw */
1665 new_cfg = kzalloc(sizeof(*new_cfg), GFP_KERNEL);
1666 if (!new_cfg) {
1667 NL_SET_ERR_MSG_MOD(extack, "Memory allocation error");
1668 return -ENOMEM;
1669 }
1670 /* fill txschq cfg and push txschq cfg to hw */
1671 otx2_qos_fill_cfg_schq(parent, new_cfg);
1672 err = otx2_qos_push_txschq_cfg(pfvf, parent, new_cfg);
1673 if (err) {
1674 NL_SET_ERR_MSG_MOD(extack, "HTB HW configuration error");
1675 kfree(new_cfg);
1676 return err;
1677 }
1678 kfree(new_cfg);
1679
1680 /* update tx_real_queues */
1681 otx2_qos_update_tx_netdev_queues(pfvf);
1682
1683 return 0;
1684}
1685
1686void otx2_clean_qos_queues(struct otx2_nic *pfvf)
1687{
1688 struct otx2_qos_node *root;
1689
1690 root = otx2_sw_node_find(pfvf, OTX2_QOS_ROOT_CLASSID);
1691 if (!root)
1692 return;
1693
1694 otx2_qos_update_smq(pfvf, root, QOS_SMQ_FLUSH);
1695}
1696
1697void otx2_qos_config_txschq(struct otx2_nic *pfvf)
1698{
1699 struct otx2_qos_node *root;
1700 int err;
1701
1702 root = otx2_sw_node_find(pfvf, OTX2_QOS_ROOT_CLASSID);
1703 if (!root)
1704 return;
1705
1706 if (root->level != NIX_TXSCH_LVL_TL1) {
1707 err = otx2_qos_txschq_config(pfvf, root);
1708 if (err) {
1709 netdev_err(pfvf->netdev, "Error update txschq configuration\n");
1710 goto root_destroy;
1711 }
1712 }
1713
1714 err = otx2_qos_txschq_push_cfg_tl(pfvf, root, NULL);
1715 if (err) {
1716 netdev_err(pfvf->netdev, "Error update txschq configuration\n");
1717 goto root_destroy;
1718 }
1719
1720 otx2_qos_update_smq(pfvf, root, QOS_CFG_SQ);
1721 return;
1722
1723root_destroy:
1724 netdev_err(pfvf->netdev, "Failed to update Scheduler/Shaping config in Hardware\n");
1725 /* Free resources allocated */
1726 otx2_qos_root_destroy(pfvf);
1727}
1728
1729int otx2_setup_tc_htb(struct net_device *ndev, struct tc_htb_qopt_offload *htb)
1730{
1731 struct otx2_nic *pfvf = netdev_priv(ndev);
1732 int res;
1733
1734 switch (htb->command) {
1735 case TC_HTB_CREATE:
1736 return otx2_qos_root_add(pfvf, htb->parent_classid,
1737 htb->classid, htb->extack);
1738 case TC_HTB_DESTROY:
1739 return otx2_qos_root_destroy(pfvf);
1740 case TC_HTB_LEAF_ALLOC_QUEUE:
1741 res = otx2_qos_leaf_alloc_queue(pfvf, htb->classid,
1742 htb->parent_classid,
1743 htb->rate, htb->ceil,
1744 htb->prio, htb->quantum,
1745 htb->extack);
1746 if (res < 0)
1747 return res;
1748 htb->qid = res;
1749 return 0;
1750 case TC_HTB_LEAF_TO_INNER:
1751 return otx2_qos_leaf_to_inner(pfvf, htb->parent_classid,
1752 htb->classid, htb->rate,
1753 htb->ceil, htb->prio,
1754 htb->quantum, htb->extack);
1755 case TC_HTB_LEAF_DEL:
1756 return otx2_qos_leaf_del(pfvf, &htb->classid, htb->extack);
1757 case TC_HTB_LEAF_DEL_LAST:
1758 case TC_HTB_LEAF_DEL_LAST_FORCE:
1759 return otx2_qos_leaf_del_last(pfvf, htb->classid,
1760 htb->command == TC_HTB_LEAF_DEL_LAST_FORCE,
1761 htb->extack);
1762 case TC_HTB_LEAF_QUERY_QUEUE:
1763 res = otx2_get_txq_by_classid(pfvf, htb->classid);
1764 htb->qid = res;
1765 return 0;
1766 case TC_HTB_NODE_MODIFY:
1767 fallthrough;
1768 default:
1769 return -EOPNOTSUPP;
1770 }
1771}