qos.c

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