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