ice_sched.c

   1// SPDX-License-Identifier: GPL-2.0
   2/* Copyright (c) 2018, Intel Corporation. */
   3
   4#include <net/devlink.h>
   5#include "ice_sched.h"
   6
   7/**
   8 * ice_sched_add_root_node - Insert the Tx scheduler root node in SW DB
   9 * @pi: port information structure
  10 * @info: Scheduler element information from firmware
  11 *
  12 * This function inserts the root node of the scheduling tree topology
  13 * to the SW DB.
  14 */
  15static int
  16ice_sched_add_root_node(struct ice_port_info *pi,
  17			struct ice_aqc_txsched_elem_data *info)
  18{
  19	struct ice_sched_node *root;
  20	struct ice_hw *hw;
  21
  22	if (!pi)
  23		return -EINVAL;
  24
  25	hw = pi->hw;
  26
  27	root = devm_kzalloc(ice_hw_to_dev(hw), sizeof(*root), GFP_KERNEL);
  28	if (!root)
  29		return -ENOMEM;
  30
  31	root->children = devm_kcalloc(ice_hw_to_dev(hw), hw->max_children[0],
  32				      sizeof(*root->children), GFP_KERNEL);
  33	if (!root->children) {
  34		devm_kfree(ice_hw_to_dev(hw), root);
  35		return -ENOMEM;
  36	}
  37
  38	memcpy(&root->info, info, sizeof(*info));
  39	pi->root = root;
  40	return 0;
  41}
  42
  43/**
  44 * ice_sched_find_node_by_teid - Find the Tx scheduler node in SW DB
  45 * @start_node: pointer to the starting ice_sched_node struct in a sub-tree
  46 * @teid: node TEID to search
  47 *
  48 * This function searches for a node matching the TEID in the scheduling tree
  49 * from the SW DB. The search is recursive and is restricted by the number of
  50 * layers it has searched through; stopping at the max supported layer.
  51 *
  52 * This function needs to be called when holding the port_info->sched_lock
  53 */
  54struct ice_sched_node *
  55ice_sched_find_node_by_teid(struct ice_sched_node *start_node, u32 teid)
  56{
  57	u16 i;
  58
  59	/* The TEID is same as that of the start_node */
  60	if (ICE_TXSCHED_GET_NODE_TEID(start_node) == teid)
  61		return start_node;
  62
  63	/* The node has no children or is at the max layer */
  64	if (!start_node->num_children ||
  65	    start_node->tx_sched_layer >= ICE_AQC_TOPO_MAX_LEVEL_NUM ||
  66	    start_node->info.data.elem_type == ICE_AQC_ELEM_TYPE_LEAF)
  67		return NULL;
  68
  69	/* Check if TEID matches to any of the children nodes */
  70	for (i = 0; i < start_node->num_children; i++)
  71		if (ICE_TXSCHED_GET_NODE_TEID(start_node->children[i]) == teid)
  72			return start_node->children[i];
  73
  74	/* Search within each child's sub-tree */
  75	for (i = 0; i < start_node->num_children; i++) {
  76		struct ice_sched_node *tmp;
  77
  78		tmp = ice_sched_find_node_by_teid(start_node->children[i],
  79						  teid);
  80		if (tmp)
  81			return tmp;
  82	}
  83
  84	return NULL;
  85}
  86
  87/**
  88 * ice_sched_find_next_vsi_node - find the next node for a given VSI
  89 * @vsi_node: VSI support node to start search with
  90 *
  91 * Return: Next VSI support node, or NULL.
  92 *
  93 * The function returns a pointer to the next node from the VSI layer
  94 * assigned to the given VSI, or NULL if there is no such a node.
  95 */
  96static struct ice_sched_node *
  97ice_sched_find_next_vsi_node(struct ice_sched_node *vsi_node)
  98{
  99	unsigned int vsi_handle = vsi_node->vsi_handle;
 100
 101	while ((vsi_node = vsi_node->sibling) != NULL)
 102		if (vsi_node->vsi_handle == vsi_handle)
 103			break;
 104
 105	return vsi_node;
 106}
 107
 108/**
 109 * ice_aqc_send_sched_elem_cmd - send scheduling elements cmd
 110 * @hw: pointer to the HW struct
 111 * @cmd_opc: cmd opcode
 112 * @elems_req: number of elements to request
 113 * @buf: pointer to buffer
 114 * @buf_size: buffer size in bytes
 115 * @elems_resp: returns total number of elements response
 116 * @cd: pointer to command details structure or NULL
 117 *
 118 * This function sends a scheduling elements cmd (cmd_opc)
 119 */
 120static int
 121ice_aqc_send_sched_elem_cmd(struct ice_hw *hw, enum ice_adminq_opc cmd_opc,
 122			    u16 elems_req, void *buf, u16 buf_size,
 123			    u16 *elems_resp, struct ice_sq_cd *cd)
 124{
 125	struct ice_aqc_sched_elem_cmd *cmd;
 126	struct libie_aq_desc desc;
 127	int status;
 128
 129	cmd = libie_aq_raw(&desc);
 130	ice_fill_dflt_direct_cmd_desc(&desc, cmd_opc);
 131	cmd->num_elem_req = cpu_to_le16(elems_req);
 132	desc.flags |= cpu_to_le16(LIBIE_AQ_FLAG_RD);
 133	status = ice_aq_send_cmd(hw, &desc, buf, buf_size, cd);
 134	if (!status && elems_resp)
 135		*elems_resp = le16_to_cpu(cmd->num_elem_resp);
 136
 137	return status;
 138}
 139
 140/**
 141 * ice_aq_query_sched_elems - query scheduler elements
 142 * @hw: pointer to the HW struct
 143 * @elems_req: number of elements to query
 144 * @buf: pointer to buffer
 145 * @buf_size: buffer size in bytes
 146 * @elems_ret: returns total number of elements returned
 147 * @cd: pointer to command details structure or NULL
 148 *
 149 * Query scheduling elements (0x0404)
 150 */
 151int
 152ice_aq_query_sched_elems(struct ice_hw *hw, u16 elems_req,
 153			 struct ice_aqc_txsched_elem_data *buf, u16 buf_size,
 154			 u16 *elems_ret, struct ice_sq_cd *cd)
 155{
 156	return ice_aqc_send_sched_elem_cmd(hw, ice_aqc_opc_get_sched_elems,
 157					   elems_req, (void *)buf, buf_size,
 158					   elems_ret, cd);
 159}
 160
 161/**
 162 * ice_sched_add_node - Insert the Tx scheduler node in SW DB
 163 * @pi: port information structure
 164 * @layer: Scheduler layer of the node
 165 * @info: Scheduler element information from firmware
 166 * @prealloc_node: preallocated ice_sched_node struct for SW DB
 167 *
 168 * This function inserts a scheduler node to the SW DB.
 169 */
 170int
 171ice_sched_add_node(struct ice_port_info *pi, u8 layer,
 172		   struct ice_aqc_txsched_elem_data *info,
 173		   struct ice_sched_node *prealloc_node)
 174{
 175	struct ice_aqc_txsched_elem_data elem;
 176	struct ice_sched_node *parent;
 177	struct ice_sched_node *node;
 178	struct ice_hw *hw;
 179	int status;
 180
 181	if (!pi)
 182		return -EINVAL;
 183
 184	hw = pi->hw;
 185
 186	/* A valid parent node should be there */
 187	parent = ice_sched_find_node_by_teid(pi->root,
 188					     le32_to_cpu(info->parent_teid));
 189	if (!parent) {
 190		ice_debug(hw, ICE_DBG_SCHED, "Parent Node not found for parent_teid=0x%x\n",
 191			  le32_to_cpu(info->parent_teid));
 192		return -EINVAL;
 193	}
 194
 195	/* query the current node information from FW before adding it
 196	 * to the SW DB
 197	 */
 198	status = ice_sched_query_elem(hw, le32_to_cpu(info->node_teid), &elem);
 199	if (status)
 200		return status;
 201
 202	if (prealloc_node)
 203		node = prealloc_node;
 204	else
 205		node = devm_kzalloc(ice_hw_to_dev(hw), sizeof(*node), GFP_KERNEL);
 206	if (!node)
 207		return -ENOMEM;
 208	if (hw->max_children[layer]) {
 209		node->children = devm_kcalloc(ice_hw_to_dev(hw),
 210					      hw->max_children[layer],
 211					      sizeof(*node->children), GFP_KERNEL);
 212		if (!node->children) {
 213			devm_kfree(ice_hw_to_dev(hw), node);
 214			return -ENOMEM;
 215		}
 216	}
 217
 218	node->in_use = true;
 219	node->parent = parent;
 220	node->tx_sched_layer = layer;
 221	parent->children[parent->num_children++] = node;
 222	node->info = elem;
 223	return 0;
 224}
 225
 226/**
 227 * ice_aq_delete_sched_elems - delete scheduler elements
 228 * @hw: pointer to the HW struct
 229 * @grps_req: number of groups to delete
 230 * @buf: pointer to buffer
 231 * @buf_size: buffer size in bytes
 232 * @grps_del: returns total number of elements deleted
 233 * @cd: pointer to command details structure or NULL
 234 *
 235 * Delete scheduling elements (0x040F)
 236 */
 237static int
 238ice_aq_delete_sched_elems(struct ice_hw *hw, u16 grps_req,
 239			  struct ice_aqc_delete_elem *buf, u16 buf_size,
 240			  u16 *grps_del, struct ice_sq_cd *cd)
 241{
 242	return ice_aqc_send_sched_elem_cmd(hw, ice_aqc_opc_delete_sched_elems,
 243					   grps_req, (void *)buf, buf_size,
 244					   grps_del, cd);
 245}
 246
 247/**
 248 * ice_sched_remove_elems - remove nodes from HW
 249 * @hw: pointer to the HW struct
 250 * @parent: pointer to the parent node
 251 * @node_teid: node teid to be deleted
 252 *
 253 * This function remove nodes from HW
 254 */
 255static int
 256ice_sched_remove_elems(struct ice_hw *hw, struct ice_sched_node *parent,
 257		       u32 node_teid)
 258{
 259	DEFINE_RAW_FLEX(struct ice_aqc_delete_elem, buf, teid, 1);
 260	u16 buf_size = __struct_size(buf);
 261	u16 num_groups_removed = 0;
 262	int status;
 263
 264	buf->hdr.parent_teid = parent->info.node_teid;
 265	buf->hdr.num_elems = cpu_to_le16(1);
 266	buf->teid[0] = cpu_to_le32(node_teid);
 267
 268	status = ice_aq_delete_sched_elems(hw, 1, buf, buf_size,
 269					   &num_groups_removed, NULL);
 270	if (status || num_groups_removed != 1)
 271		ice_debug(hw, ICE_DBG_SCHED, "remove node failed FW error %d\n",
 272			  hw->adminq.sq_last_status);
 273
 274	return status;
 275}
 276
 277/**
 278 * ice_sched_get_first_node - get the first node of the given layer
 279 * @pi: port information structure
 280 * @parent: pointer the base node of the subtree
 281 * @layer: layer number
 282 *
 283 * This function retrieves the first node of the given layer from the subtree
 284 */
 285static struct ice_sched_node *
 286ice_sched_get_first_node(struct ice_port_info *pi,
 287			 struct ice_sched_node *parent, u8 layer)
 288{
 289	return pi->sib_head[parent->tc_num][layer];
 290}
 291
 292/**
 293 * ice_sched_get_tc_node - get pointer to TC node
 294 * @pi: port information structure
 295 * @tc: TC number
 296 *
 297 * This function returns the TC node pointer
 298 */
 299struct ice_sched_node *ice_sched_get_tc_node(struct ice_port_info *pi, u8 tc)
 300{
 301	u8 i;
 302
 303	if (!pi || !pi->root)
 304		return NULL;
 305	for (i = 0; i < pi->root->num_children; i++)
 306		if (pi->root->children[i]->tc_num == tc)
 307			return pi->root->children[i];
 308	return NULL;
 309}
 310
 311/**
 312 * ice_free_sched_node - Free a Tx scheduler node from SW DB
 313 * @pi: port information structure
 314 * @node: pointer to the ice_sched_node struct
 315 *
 316 * This function frees up a node from SW DB as well as from HW
 317 *
 318 * This function needs to be called with the port_info->sched_lock held
 319 */
 320void ice_free_sched_node(struct ice_port_info *pi, struct ice_sched_node *node)
 321{
 322	struct ice_sched_node *parent;
 323	struct ice_hw *hw = pi->hw;
 324	u8 i, j;
 325
 326	/* Free the children before freeing up the parent node
 327	 * The parent array is updated below and that shifts the nodes
 328	 * in the array. So always pick the first child if num children > 0
 329	 */
 330	while (node->num_children)
 331		ice_free_sched_node(pi, node->children[0]);
 332
 333	/* Leaf, TC and root nodes can't be deleted by SW */
 334	if (node->tx_sched_layer >= hw->sw_entry_point_layer &&
 335	    node->info.data.elem_type != ICE_AQC_ELEM_TYPE_TC &&
 336	    node->info.data.elem_type != ICE_AQC_ELEM_TYPE_ROOT_PORT &&
 337	    node->info.data.elem_type != ICE_AQC_ELEM_TYPE_LEAF) {
 338		u32 teid = le32_to_cpu(node->info.node_teid);
 339
 340		ice_sched_remove_elems(hw, node->parent, teid);
 341	}
 342	parent = node->parent;
 343	/* root has no parent */
 344	if (parent) {
 345		struct ice_sched_node *p;
 346
 347		/* update the parent */
 348		for (i = 0; i < parent->num_children; i++)
 349			if (parent->children[i] == node) {
 350				for (j = i + 1; j < parent->num_children; j++)
 351					parent->children[j - 1] =
 352						parent->children[j];
 353				parent->num_children--;
 354				break;
 355			}
 356
 357		p = ice_sched_get_first_node(pi, node, node->tx_sched_layer);
 358		while (p) {
 359			if (p->sibling == node) {
 360				p->sibling = node->sibling;
 361				break;
 362			}
 363			p = p->sibling;
 364		}
 365
 366		/* update the sibling head if head is getting removed */
 367		if (pi->sib_head[node->tc_num][node->tx_sched_layer] == node)
 368			pi->sib_head[node->tc_num][node->tx_sched_layer] =
 369				node->sibling;
 370	}
 371
 372	devm_kfree(ice_hw_to_dev(hw), node->children);
 373	kfree(node->name);
 374	xa_erase(&pi->sched_node_ids, node->id);
 375	devm_kfree(ice_hw_to_dev(hw), node);
 376}
 377
 378/**
 379 * ice_aq_get_dflt_topo - gets default scheduler topology
 380 * @hw: pointer to the HW struct
 381 * @lport: logical port number
 382 * @buf: pointer to buffer
 383 * @buf_size: buffer size in bytes
 384 * @num_branches: returns total number of queue to port branches
 385 * @cd: pointer to command details structure or NULL
 386 *
 387 * Get default scheduler topology (0x400)
 388 */
 389static int
 390ice_aq_get_dflt_topo(struct ice_hw *hw, u8 lport,
 391		     struct ice_aqc_get_topo_elem *buf, u16 buf_size,
 392		     u8 *num_branches, struct ice_sq_cd *cd)
 393{
 394	struct ice_aqc_get_topo *cmd;
 395	struct libie_aq_desc desc;
 396	int status;
 397
 398	cmd = libie_aq_raw(&desc);
 399	ice_fill_dflt_direct_cmd_desc(&desc, ice_aqc_opc_get_dflt_topo);
 400	cmd->port_num = lport;
 401	status = ice_aq_send_cmd(hw, &desc, buf, buf_size, cd);
 402	if (!status && num_branches)
 403		*num_branches = cmd->num_branches;
 404
 405	return status;
 406}
 407
 408/**
 409 * ice_aq_add_sched_elems - adds scheduling element
 410 * @hw: pointer to the HW struct
 411 * @grps_req: the number of groups that are requested to be added
 412 * @buf: pointer to buffer
 413 * @buf_size: buffer size in bytes
 414 * @grps_added: returns total number of groups added
 415 * @cd: pointer to command details structure or NULL
 416 *
 417 * Add scheduling elements (0x0401)
 418 */
 419static int
 420ice_aq_add_sched_elems(struct ice_hw *hw, u16 grps_req,
 421		       struct ice_aqc_add_elem *buf, u16 buf_size,
 422		       u16 *grps_added, struct ice_sq_cd *cd)
 423{
 424	return ice_aqc_send_sched_elem_cmd(hw, ice_aqc_opc_add_sched_elems,
 425					   grps_req, (void *)buf, buf_size,
 426					   grps_added, cd);
 427}
 428
 429/**
 430 * ice_aq_cfg_sched_elems - configures scheduler elements
 431 * @hw: pointer to the HW struct
 432 * @elems_req: number of elements to configure
 433 * @buf: pointer to buffer
 434 * @buf_size: buffer size in bytes
 435 * @elems_cfgd: returns total number of elements configured
 436 * @cd: pointer to command details structure or NULL
 437 *
 438 * Configure scheduling elements (0x0403)
 439 */
 440static int
 441ice_aq_cfg_sched_elems(struct ice_hw *hw, u16 elems_req,
 442		       struct ice_aqc_txsched_elem_data *buf, u16 buf_size,
 443		       u16 *elems_cfgd, struct ice_sq_cd *cd)
 444{
 445	return ice_aqc_send_sched_elem_cmd(hw, ice_aqc_opc_cfg_sched_elems,
 446					   elems_req, (void *)buf, buf_size,
 447					   elems_cfgd, cd);
 448}
 449
 450/**
 451 * ice_aq_move_sched_elems - move scheduler element (just 1 group)
 452 * @hw: pointer to the HW struct
 453 * @buf: pointer to buffer
 454 * @buf_size: buffer size in bytes
 455 * @grps_movd: returns total number of groups moved
 456 *
 457 * Move scheduling elements (0x0408)
 458 */
 459int
 460ice_aq_move_sched_elems(struct ice_hw *hw, struct ice_aqc_move_elem *buf,
 461			u16 buf_size, u16 *grps_movd)
 462{
 463	return ice_aqc_send_sched_elem_cmd(hw, ice_aqc_opc_move_sched_elems,
 464					   1, buf, buf_size, grps_movd, NULL);
 465}
 466
 467/**
 468 * ice_aq_suspend_sched_elems - suspend scheduler elements
 469 * @hw: pointer to the HW struct
 470 * @elems_req: number of elements to suspend
 471 * @buf: pointer to buffer
 472 * @buf_size: buffer size in bytes
 473 * @elems_ret: returns total number of elements suspended
 474 * @cd: pointer to command details structure or NULL
 475 *
 476 * Suspend scheduling elements (0x0409)
 477 */
 478static int
 479ice_aq_suspend_sched_elems(struct ice_hw *hw, u16 elems_req, __le32 *buf,
 480			   u16 buf_size, u16 *elems_ret, struct ice_sq_cd *cd)
 481{
 482	return ice_aqc_send_sched_elem_cmd(hw, ice_aqc_opc_suspend_sched_elems,
 483					   elems_req, (void *)buf, buf_size,
 484					   elems_ret, cd);
 485}
 486
 487/**
 488 * ice_aq_resume_sched_elems - resume scheduler elements
 489 * @hw: pointer to the HW struct
 490 * @elems_req: number of elements to resume
 491 * @buf: pointer to buffer
 492 * @buf_size: buffer size in bytes
 493 * @elems_ret: returns total number of elements resumed
 494 * @cd: pointer to command details structure or NULL
 495 *
 496 * resume scheduling elements (0x040A)
 497 */
 498static int
 499ice_aq_resume_sched_elems(struct ice_hw *hw, u16 elems_req, __le32 *buf,
 500			  u16 buf_size, u16 *elems_ret, struct ice_sq_cd *cd)
 501{
 502	return ice_aqc_send_sched_elem_cmd(hw, ice_aqc_opc_resume_sched_elems,
 503					   elems_req, (void *)buf, buf_size,
 504					   elems_ret, cd);
 505}
 506
 507/**
 508 * ice_aq_query_sched_res - query scheduler resource
 509 * @hw: pointer to the HW struct
 510 * @buf_size: buffer size in bytes
 511 * @buf: pointer to buffer
 512 * @cd: pointer to command details structure or NULL
 513 *
 514 * Query scheduler resource allocation (0x0412)
 515 */
 516static int
 517ice_aq_query_sched_res(struct ice_hw *hw, u16 buf_size,
 518		       struct ice_aqc_query_txsched_res_resp *buf,
 519		       struct ice_sq_cd *cd)
 520{
 521	struct libie_aq_desc desc;
 522
 523	ice_fill_dflt_direct_cmd_desc(&desc, ice_aqc_opc_query_sched_res);
 524	return ice_aq_send_cmd(hw, &desc, buf, buf_size, cd);
 525}
 526
 527/**
 528 * ice_sched_suspend_resume_elems - suspend or resume HW nodes
 529 * @hw: pointer to the HW struct
 530 * @num_nodes: number of nodes
 531 * @node_teids: array of node teids to be suspended or resumed
 532 * @suspend: true means suspend / false means resume
 533 *
 534 * This function suspends or resumes HW nodes
 535 */
 536int
 537ice_sched_suspend_resume_elems(struct ice_hw *hw, u8 num_nodes, u32 *node_teids,
 538			       bool suspend)
 539{
 540	u16 i, buf_size, num_elem_ret = 0;
 541	__le32 *buf;
 542	int status;
 543
 544	buf_size = sizeof(*buf) * num_nodes;
 545	buf = devm_kzalloc(ice_hw_to_dev(hw), buf_size, GFP_KERNEL);
 546	if (!buf)
 547		return -ENOMEM;
 548
 549	for (i = 0; i < num_nodes; i++)
 550		buf[i] = cpu_to_le32(node_teids[i]);
 551
 552	if (suspend)
 553		status = ice_aq_suspend_sched_elems(hw, num_nodes, buf,
 554						    buf_size, &num_elem_ret,
 555						    NULL);
 556	else
 557		status = ice_aq_resume_sched_elems(hw, num_nodes, buf,
 558						   buf_size, &num_elem_ret,
 559						   NULL);
 560	if (status || num_elem_ret != num_nodes)
 561		ice_debug(hw, ICE_DBG_SCHED, "suspend/resume failed\n");
 562
 563	devm_kfree(ice_hw_to_dev(hw), buf);
 564	return status;
 565}
 566
 567/**
 568 * ice_alloc_lan_q_ctx - allocate LAN queue contexts for the given VSI and TC
 569 * @hw: pointer to the HW struct
 570 * @vsi_handle: VSI handle
 571 * @tc: TC number
 572 * @new_numqs: number of queues
 573 */
 574static int
 575ice_alloc_lan_q_ctx(struct ice_hw *hw, u16 vsi_handle, u8 tc, u16 new_numqs)
 576{
 577	struct ice_vsi_ctx *vsi_ctx;
 578	struct ice_q_ctx *q_ctx;
 579	u16 idx;
 580
 581	vsi_ctx = ice_get_vsi_ctx(hw, vsi_handle);
 582	if (!vsi_ctx)
 583		return -EINVAL;
 584	/* allocate LAN queue contexts */
 585	if (!vsi_ctx->lan_q_ctx[tc]) {
 586		q_ctx = devm_kcalloc(ice_hw_to_dev(hw), new_numqs,
 587				     sizeof(*q_ctx), GFP_KERNEL);
 588		if (!q_ctx)
 589			return -ENOMEM;
 590
 591		for (idx = 0; idx < new_numqs; idx++) {
 592			q_ctx[idx].q_handle = ICE_INVAL_Q_HANDLE;
 593			q_ctx[idx].q_teid = ICE_INVAL_TEID;
 594		}
 595
 596		vsi_ctx->lan_q_ctx[tc] = q_ctx;
 597		vsi_ctx->num_lan_q_entries[tc] = new_numqs;
 598		return 0;
 599	}
 600	/* num queues are increased, update the queue contexts */
 601	if (new_numqs > vsi_ctx->num_lan_q_entries[tc]) {
 602		u16 prev_num = vsi_ctx->num_lan_q_entries[tc];
 603
 604		q_ctx = devm_kcalloc(ice_hw_to_dev(hw), new_numqs,
 605				     sizeof(*q_ctx), GFP_KERNEL);
 606		if (!q_ctx)
 607			return -ENOMEM;
 608
 609		memcpy(q_ctx, vsi_ctx->lan_q_ctx[tc],
 610		       prev_num * sizeof(*q_ctx));
 611		devm_kfree(ice_hw_to_dev(hw), vsi_ctx->lan_q_ctx[tc]);
 612
 613		for (idx = prev_num; idx < new_numqs; idx++) {
 614			q_ctx[idx].q_handle = ICE_INVAL_Q_HANDLE;
 615			q_ctx[idx].q_teid = ICE_INVAL_TEID;
 616		}
 617
 618		vsi_ctx->lan_q_ctx[tc] = q_ctx;
 619		vsi_ctx->num_lan_q_entries[tc] = new_numqs;
 620	}
 621	return 0;
 622}
 623
 624/**
 625 * ice_alloc_rdma_q_ctx - allocate RDMA queue contexts for the given VSI and TC
 626 * @hw: pointer to the HW struct
 627 * @vsi_handle: VSI handle
 628 * @tc: TC number
 629 * @new_numqs: number of queues
 630 */
 631static int
 632ice_alloc_rdma_q_ctx(struct ice_hw *hw, u16 vsi_handle, u8 tc, u16 new_numqs)
 633{
 634	struct ice_vsi_ctx *vsi_ctx;
 635	struct ice_q_ctx *q_ctx;
 636
 637	vsi_ctx = ice_get_vsi_ctx(hw, vsi_handle);
 638	if (!vsi_ctx)
 639		return -EINVAL;
 640	/* allocate RDMA queue contexts */
 641	if (!vsi_ctx->rdma_q_ctx[tc]) {
 642		vsi_ctx->rdma_q_ctx[tc] = devm_kcalloc(ice_hw_to_dev(hw),
 643						       new_numqs,
 644						       sizeof(*q_ctx),
 645						       GFP_KERNEL);
 646		if (!vsi_ctx->rdma_q_ctx[tc])
 647			return -ENOMEM;
 648		vsi_ctx->num_rdma_q_entries[tc] = new_numqs;
 649		return 0;
 650	}
 651	/* num queues are increased, update the queue contexts */
 652	if (new_numqs > vsi_ctx->num_rdma_q_entries[tc]) {
 653		u16 prev_num = vsi_ctx->num_rdma_q_entries[tc];
 654
 655		q_ctx = devm_kcalloc(ice_hw_to_dev(hw), new_numqs,
 656				     sizeof(*q_ctx), GFP_KERNEL);
 657		if (!q_ctx)
 658			return -ENOMEM;
 659		memcpy(q_ctx, vsi_ctx->rdma_q_ctx[tc],
 660		       prev_num * sizeof(*q_ctx));
 661		devm_kfree(ice_hw_to_dev(hw), vsi_ctx->rdma_q_ctx[tc]);
 662		vsi_ctx->rdma_q_ctx[tc] = q_ctx;
 663		vsi_ctx->num_rdma_q_entries[tc] = new_numqs;
 664	}
 665	return 0;
 666}
 667
 668/**
 669 * ice_aq_rl_profile - performs a rate limiting task
 670 * @hw: pointer to the HW struct
 671 * @opcode: opcode for add, query, or remove profile(s)
 672 * @num_profiles: the number of profiles
 673 * @buf: pointer to buffer
 674 * @buf_size: buffer size in bytes
 675 * @num_processed: number of processed add or remove profile(s) to return
 676 * @cd: pointer to command details structure
 677 *
 678 * RL profile function to add, query, or remove profile(s)
 679 */
 680static int
 681ice_aq_rl_profile(struct ice_hw *hw, enum ice_adminq_opc opcode,
 682		  u16 num_profiles, struct ice_aqc_rl_profile_elem *buf,
 683		  u16 buf_size, u16 *num_processed, struct ice_sq_cd *cd)
 684{
 685	struct ice_aqc_rl_profile *cmd;
 686	struct libie_aq_desc desc;
 687	int status;
 688
 689	cmd = libie_aq_raw(&desc);
 690
 691	ice_fill_dflt_direct_cmd_desc(&desc, opcode);
 692	desc.flags |= cpu_to_le16(LIBIE_AQ_FLAG_RD);
 693	cmd->num_profiles = cpu_to_le16(num_profiles);
 694	status = ice_aq_send_cmd(hw, &desc, buf, buf_size, cd);
 695	if (!status && num_processed)
 696		*num_processed = le16_to_cpu(cmd->num_processed);
 697	return status;
 698}
 699
 700/**
 701 * ice_aq_add_rl_profile - adds rate limiting profile(s)
 702 * @hw: pointer to the HW struct
 703 * @num_profiles: the number of profile(s) to be add
 704 * @buf: pointer to buffer
 705 * @buf_size: buffer size in bytes
 706 * @num_profiles_added: total number of profiles added to return
 707 * @cd: pointer to command details structure
 708 *
 709 * Add RL profile (0x0410)
 710 */
 711static int
 712ice_aq_add_rl_profile(struct ice_hw *hw, u16 num_profiles,
 713		      struct ice_aqc_rl_profile_elem *buf, u16 buf_size,
 714		      u16 *num_profiles_added, struct ice_sq_cd *cd)
 715{
 716	return ice_aq_rl_profile(hw, ice_aqc_opc_add_rl_profiles, num_profiles,
 717				 buf, buf_size, num_profiles_added, cd);
 718}
 719
 720/**
 721 * ice_aq_remove_rl_profile - removes RL profile(s)
 722 * @hw: pointer to the HW struct
 723 * @num_profiles: the number of profile(s) to remove
 724 * @buf: pointer to buffer
 725 * @buf_size: buffer size in bytes
 726 * @num_profiles_removed: total number of profiles removed to return
 727 * @cd: pointer to command details structure or NULL
 728 *
 729 * Remove RL profile (0x0415)
 730 */
 731static int
 732ice_aq_remove_rl_profile(struct ice_hw *hw, u16 num_profiles,
 733			 struct ice_aqc_rl_profile_elem *buf, u16 buf_size,
 734			 u16 *num_profiles_removed, struct ice_sq_cd *cd)
 735{
 736	return ice_aq_rl_profile(hw, ice_aqc_opc_remove_rl_profiles,
 737				 num_profiles, buf, buf_size,
 738				 num_profiles_removed, cd);
 739}
 740
 741/**
 742 * ice_sched_del_rl_profile - remove RL profile
 743 * @hw: pointer to the HW struct
 744 * @rl_info: rate limit profile information
 745 *
 746 * If the profile ID is not referenced anymore, it removes profile ID with
 747 * its associated parameters from HW DB,and locally. The caller needs to
 748 * hold scheduler lock.
 749 */
 750static int
 751ice_sched_del_rl_profile(struct ice_hw *hw,
 752			 struct ice_aqc_rl_profile_info *rl_info)
 753{
 754	struct ice_aqc_rl_profile_elem *buf;
 755	u16 num_profiles_removed;
 756	u16 num_profiles = 1;
 757	int status;
 758
 759	if (rl_info->prof_id_ref != 0)
 760		return -EBUSY;
 761
 762	/* Safe to remove profile ID */
 763	buf = &rl_info->profile;
 764	status = ice_aq_remove_rl_profile(hw, num_profiles, buf, sizeof(*buf),
 765					  &num_profiles_removed, NULL);
 766	if (status || num_profiles_removed != num_profiles)
 767		return -EIO;
 768
 769	/* Delete stale entry now */
 770	list_del(&rl_info->list_entry);
 771	devm_kfree(ice_hw_to_dev(hw), rl_info);
 772	return status;
 773}
 774
 775/**
 776 * ice_sched_clear_rl_prof - clears RL prof entries
 777 * @pi: port information structure
 778 *
 779 * This function removes all RL profile from HW as well as from SW DB.
 780 */
 781static void ice_sched_clear_rl_prof(struct ice_port_info *pi)
 782{
 783	u16 ln;
 784
 785	for (ln = 0; ln < pi->hw->num_tx_sched_layers; ln++) {
 786		struct ice_aqc_rl_profile_info *rl_prof_elem;
 787		struct ice_aqc_rl_profile_info *rl_prof_tmp;
 788
 789		list_for_each_entry_safe(rl_prof_elem, rl_prof_tmp,
 790					 &pi->rl_prof_list[ln], list_entry) {
 791			struct ice_hw *hw = pi->hw;
 792			int status;
 793
 794			rl_prof_elem->prof_id_ref = 0;
 795			status = ice_sched_del_rl_profile(hw, rl_prof_elem);
 796			if (status) {
 797				ice_debug(hw, ICE_DBG_SCHED, "Remove rl profile failed\n");
 798				/* On error, free mem required */
 799				list_del(&rl_prof_elem->list_entry);
 800				devm_kfree(ice_hw_to_dev(hw), rl_prof_elem);
 801			}
 802		}
 803	}
 804}
 805
 806/**
 807 * ice_sched_clear_agg - clears the aggregator related information
 808 * @hw: pointer to the hardware structure
 809 *
 810 * This function removes aggregator list and free up aggregator related memory
 811 * previously allocated.
 812 */
 813void ice_sched_clear_agg(struct ice_hw *hw)
 814{
 815	struct ice_sched_agg_info *agg_info;
 816	struct ice_sched_agg_info *atmp;
 817
 818	list_for_each_entry_safe(agg_info, atmp, &hw->agg_list, list_entry) {
 819		struct ice_sched_agg_vsi_info *agg_vsi_info;
 820		struct ice_sched_agg_vsi_info *vtmp;
 821
 822		list_for_each_entry_safe(agg_vsi_info, vtmp,
 823					 &agg_info->agg_vsi_list, list_entry) {
 824			list_del(&agg_vsi_info->list_entry);
 825			devm_kfree(ice_hw_to_dev(hw), agg_vsi_info);
 826		}
 827		list_del(&agg_info->list_entry);
 828		devm_kfree(ice_hw_to_dev(hw), agg_info);
 829	}
 830}
 831
 832/**
 833 * ice_sched_clear_tx_topo - clears the scheduler tree nodes
 834 * @pi: port information structure
 835 *
 836 * This function removes all the nodes from HW as well as from SW DB.
 837 */
 838static void ice_sched_clear_tx_topo(struct ice_port_info *pi)
 839{
 840	if (!pi)
 841		return;
 842	/* remove RL profiles related lists */
 843	ice_sched_clear_rl_prof(pi);
 844	if (pi->root) {
 845		ice_free_sched_node(pi, pi->root);
 846		pi->root = NULL;
 847	}
 848}
 849
 850/**
 851 * ice_sched_clear_port - clear the scheduler elements from SW DB for a port
 852 * @pi: port information structure
 853 *
 854 * Cleanup scheduling elements from SW DB
 855 */
 856void ice_sched_clear_port(struct ice_port_info *pi)
 857{
 858	if (!pi || pi->port_state != ICE_SCHED_PORT_STATE_READY)
 859		return;
 860
 861	pi->port_state = ICE_SCHED_PORT_STATE_INIT;
 862	mutex_lock(&pi->sched_lock);
 863	ice_sched_clear_tx_topo(pi);
 864	mutex_unlock(&pi->sched_lock);
 865	mutex_destroy(&pi->sched_lock);
 866}
 867
 868/**
 869 * ice_sched_cleanup_all - cleanup scheduler elements from SW DB for all ports
 870 * @hw: pointer to the HW struct
 871 *
 872 * Cleanup scheduling elements from SW DB for all the ports
 873 */
 874void ice_sched_cleanup_all(struct ice_hw *hw)
 875{
 876	if (!hw)
 877		return;
 878
 879	devm_kfree(ice_hw_to_dev(hw), hw->layer_info);
 880	hw->layer_info = NULL;
 881
 882	ice_sched_clear_port(hw->port_info);
 883
 884	hw->num_tx_sched_layers = 0;
 885	hw->num_tx_sched_phys_layers = 0;
 886	hw->flattened_layers = 0;
 887	hw->max_cgds = 0;
 888}
 889
 890/**
 891 * ice_sched_add_elems - add nodes to HW and SW DB
 892 * @pi: port information structure
 893 * @tc_node: pointer to the branch node
 894 * @parent: pointer to the parent node
 895 * @layer: layer number to add nodes
 896 * @num_nodes: number of nodes
 897 * @num_nodes_added: pointer to num nodes added
 898 * @first_node_teid: if new nodes are added then return the TEID of first node
 899 * @prealloc_nodes: preallocated nodes struct for software DB
 900 *
 901 * This function add nodes to HW as well as to SW DB for a given layer
 902 */
 903int
 904ice_sched_add_elems(struct ice_port_info *pi, struct ice_sched_node *tc_node,
 905		    struct ice_sched_node *parent, u8 layer, u16 num_nodes,
 906		    u16 *num_nodes_added, u32 *first_node_teid,
 907		    struct ice_sched_node **prealloc_nodes)
 908{
 909	struct ice_sched_node *prev, *new_node;
 910	struct ice_aqc_add_elem *buf;
 911	u16 i, num_groups_added = 0;
 912	struct ice_hw *hw = pi->hw;
 913	size_t buf_size;
 914	int status = 0;
 915	u32 teid;
 916
 917	buf_size = struct_size(buf, generic, num_nodes);
 918	buf = devm_kzalloc(ice_hw_to_dev(hw), buf_size, GFP_KERNEL);
 919	if (!buf)
 920		return -ENOMEM;
 921
 922	buf->hdr.parent_teid = parent->info.node_teid;
 923	buf->hdr.num_elems = cpu_to_le16(num_nodes);
 924	for (i = 0; i < num_nodes; i++) {
 925		buf->generic[i].parent_teid = parent->info.node_teid;
 926		buf->generic[i].data.elem_type = ICE_AQC_ELEM_TYPE_SE_GENERIC;
 927		buf->generic[i].data.valid_sections =
 928			ICE_AQC_ELEM_VALID_GENERIC | ICE_AQC_ELEM_VALID_CIR |
 929			ICE_AQC_ELEM_VALID_EIR;
 930		buf->generic[i].data.generic = 0;
 931		buf->generic[i].data.cir_bw.bw_profile_idx =
 932			cpu_to_le16(ICE_SCHED_DFLT_RL_PROF_ID);
 933		buf->generic[i].data.cir_bw.bw_alloc =
 934			cpu_to_le16(ICE_SCHED_DFLT_BW_WT);
 935		buf->generic[i].data.eir_bw.bw_profile_idx =
 936			cpu_to_le16(ICE_SCHED_DFLT_RL_PROF_ID);
 937		buf->generic[i].data.eir_bw.bw_alloc =
 938			cpu_to_le16(ICE_SCHED_DFLT_BW_WT);
 939	}
 940
 941	status = ice_aq_add_sched_elems(hw, 1, buf, buf_size,
 942					&num_groups_added, NULL);
 943	if (status || num_groups_added != 1) {
 944		ice_debug(hw, ICE_DBG_SCHED, "add node failed FW Error %d\n",
 945			  hw->adminq.sq_last_status);
 946		devm_kfree(ice_hw_to_dev(hw), buf);
 947		return -EIO;
 948	}
 949
 950	*num_nodes_added = num_nodes;
 951	/* add nodes to the SW DB */
 952	for (i = 0; i < num_nodes; i++) {
 953		if (prealloc_nodes)
 954			status = ice_sched_add_node(pi, layer, &buf->generic[i], prealloc_nodes[i]);
 955		else
 956			status = ice_sched_add_node(pi, layer, &buf->generic[i], NULL);
 957
 958		if (status) {
 959			ice_debug(hw, ICE_DBG_SCHED, "add nodes in SW DB failed status =%d\n",
 960				  status);
 961			break;
 962		}
 963
 964		teid = le32_to_cpu(buf->generic[i].node_teid);
 965		new_node = ice_sched_find_node_by_teid(parent, teid);
 966		if (!new_node) {
 967			ice_debug(hw, ICE_DBG_SCHED, "Node is missing for teid =%d\n", teid);
 968			break;
 969		}
 970
 971		new_node->sibling = NULL;
 972		new_node->tc_num = tc_node->tc_num;
 973		new_node->tx_weight = ICE_SCHED_DFLT_BW_WT;
 974		new_node->tx_share = ICE_SCHED_DFLT_BW;
 975		new_node->tx_max = ICE_SCHED_DFLT_BW;
 976		new_node->name = kzalloc(SCHED_NODE_NAME_MAX_LEN, GFP_KERNEL);
 977		if (!new_node->name)
 978			return -ENOMEM;
 979
 980		status = xa_alloc(&pi->sched_node_ids, &new_node->id, NULL, XA_LIMIT(0, UINT_MAX),
 981				  GFP_KERNEL);
 982		if (status) {
 983			ice_debug(hw, ICE_DBG_SCHED, "xa_alloc failed for sched node status =%d\n",
 984				  status);
 985			break;
 986		}
 987
 988		snprintf(new_node->name, SCHED_NODE_NAME_MAX_LEN, "node_%u", new_node->id);
 989
 990		/* add it to previous node sibling pointer */
 991		/* Note: siblings are not linked across branches */
 992		prev = ice_sched_get_first_node(pi, tc_node, layer);
 993		if (prev && prev != new_node) {
 994			while (prev->sibling)
 995				prev = prev->sibling;
 996			prev->sibling = new_node;
 997		}
 998
 999		/* initialize the sibling head */
1000		if (!pi->sib_head[tc_node->tc_num][layer])
1001			pi->sib_head[tc_node->tc_num][layer] = new_node;
1002
1003		if (i == 0)
1004			*first_node_teid = teid;
1005	}
1006
1007	devm_kfree(ice_hw_to_dev(hw), buf);
1008	return status;
1009}
1010
1011/**
1012 * ice_sched_add_nodes_to_hw_layer - Add nodes to HW layer
1013 * @pi: port information structure
1014 * @tc_node: pointer to TC node
1015 * @parent: pointer to parent node
1016 * @layer: layer number to add nodes
1017 * @num_nodes: number of nodes to be added
1018 * @first_node_teid: pointer to the first node TEID
1019 * @num_nodes_added: pointer to number of nodes added
1020 *
1021 * Add nodes into specific HW layer.
1022 */
1023static int
1024ice_sched_add_nodes_to_hw_layer(struct ice_port_info *pi,
1025				struct ice_sched_node *tc_node,
1026				struct ice_sched_node *parent, u8 layer,
1027				u16 num_nodes, u32 *first_node_teid,
1028				u16 *num_nodes_added)
1029{
1030	u16 max_child_nodes;
1031
1032	*num_nodes_added = 0;
1033
1034	if (!num_nodes)
1035		return 0;
1036
1037	if (!parent || layer < pi->hw->sw_entry_point_layer)
1038		return -EINVAL;
1039
1040	/* max children per node per layer */
1041	max_child_nodes = pi->hw->max_children[parent->tx_sched_layer];
1042
1043	/* current number of children + required nodes exceed max children */
1044	if ((parent->num_children + num_nodes) > max_child_nodes) {
1045		/* Fail if the parent is a TC node */
1046		if (parent == tc_node)
1047			return -EIO;
1048		return -ENOSPC;
1049	}
1050
1051	return ice_sched_add_elems(pi, tc_node, parent, layer, num_nodes,
1052				   num_nodes_added, first_node_teid, NULL);
1053}
1054
1055/**
1056 * ice_sched_add_nodes_to_layer - Add nodes to a given layer
1057 * @pi: port information structure
1058 * @tc_node: pointer to TC node
1059 * @parent: pointer to parent node
1060 * @layer: layer number to add nodes
1061 * @num_nodes: number of nodes to be added
1062 * @first_node_teid: pointer to the first node TEID
1063 * @num_nodes_added: pointer to number of nodes added
1064 *
1065 * This function add nodes to a given layer.
1066 */
1067int
1068ice_sched_add_nodes_to_layer(struct ice_port_info *pi,
1069			     struct ice_sched_node *tc_node,
1070			     struct ice_sched_node *parent, u8 layer,
1071			     u16 num_nodes, u32 *first_node_teid,
1072			     u16 *num_nodes_added)
1073{
1074	u32 *first_teid_ptr = first_node_teid;
1075	u16 new_num_nodes = num_nodes;
1076	int status = 0;
1077
1078	*num_nodes_added = 0;
1079	while (*num_nodes_added < num_nodes) {
1080		u16 max_child_nodes, num_added = 0;
1081		u32 temp;
1082
1083		status = ice_sched_add_nodes_to_hw_layer(pi, tc_node, parent,
1084							 layer,	new_num_nodes,
1085							 first_teid_ptr,
1086							 &num_added);
1087		if (!status)
1088			*num_nodes_added += num_added;
1089		/* added more nodes than requested ? */
1090		if (*num_nodes_added > num_nodes) {
1091			ice_debug(pi->hw, ICE_DBG_SCHED, "added extra nodes %d %d\n", num_nodes,
1092				  *num_nodes_added);
1093			status = -EIO;
1094			break;
1095		}
1096		/* break if all the nodes are added successfully */
1097		if (!status && (*num_nodes_added == num_nodes))
1098			break;
1099		/* break if the error is not max limit */
1100		if (status && status != -ENOSPC)
1101			break;
1102		/* Exceeded the max children */
1103		max_child_nodes = pi->hw->max_children[parent->tx_sched_layer];
1104		/* utilize all the spaces if the parent is not full */
1105		if (parent->num_children < max_child_nodes) {
1106			new_num_nodes = max_child_nodes - parent->num_children;
1107		} else {
1108			/* This parent is full,
1109			 * try the next available sibling.
1110			 */
1111			parent = ice_sched_find_next_vsi_node(parent);
1112			/* Don't modify the first node TEID memory if the
1113			 * first node was added already in the above call.
1114			 * Instead send some temp memory for all other
1115			 * recursive calls.
1116			 */
1117			if (num_added)
1118				first_teid_ptr = &temp;
1119
1120			new_num_nodes = num_nodes - *num_nodes_added;
1121		}
1122	}
1123	return status;
1124}
1125
1126/**
1127 * ice_sched_get_qgrp_layer - get the current queue group layer number
1128 * @hw: pointer to the HW struct
1129 *
1130 * This function returns the current queue group layer number
1131 */
1132static u8 ice_sched_get_qgrp_layer(struct ice_hw *hw)
1133{
1134	/* It's always total layers - 1, the array is 0 relative so -2 */
1135	return hw->num_tx_sched_layers - ICE_QGRP_LAYER_OFFSET;
1136}
1137
1138/**
1139 * ice_sched_get_vsi_layer - get the current VSI layer number
1140 * @hw: pointer to the HW struct
1141 *
1142 * This function returns the current VSI layer number
1143 */
1144u8 ice_sched_get_vsi_layer(struct ice_hw *hw)
1145{
1146	/* Num Layers       VSI layer
1147	 *     9               6
1148	 *     7               4
1149	 *     5 or less       sw_entry_point_layer
1150	 */
1151	/* calculate the VSI layer based on number of layers. */
1152	if (hw->num_tx_sched_layers == ICE_SCHED_9_LAYERS)
1153		return hw->num_tx_sched_layers - ICE_VSI_LAYER_OFFSET;
1154	else if (hw->num_tx_sched_layers == ICE_SCHED_5_LAYERS)
1155		/* qgroup and VSI layers are same */
1156		return hw->num_tx_sched_layers - ICE_QGRP_LAYER_OFFSET;
1157	return hw->sw_entry_point_layer;
1158}
1159
1160/**
1161 * ice_sched_get_agg_layer - get the current aggregator layer number
1162 * @hw: pointer to the HW struct
1163 *
1164 * This function returns the current aggregator layer number
1165 */
1166u8 ice_sched_get_agg_layer(struct ice_hw *hw)
1167{
1168	/* Num Layers       aggregator layer
1169	 *     9               4
1170	 *     7 or less       sw_entry_point_layer
1171	 */
1172	/* calculate the aggregator layer based on number of layers. */
1173	if (hw->num_tx_sched_layers == ICE_SCHED_9_LAYERS)
1174		return hw->num_tx_sched_layers - ICE_AGG_LAYER_OFFSET;
1175	else
1176		return hw->sw_entry_point_layer;
1177}
1178
1179/**
1180 * ice_rm_dflt_leaf_node - remove the default leaf node in the tree
1181 * @pi: port information structure
1182 *
1183 * This function removes the leaf node that was created by the FW
1184 * during initialization
1185 */
1186static void ice_rm_dflt_leaf_node(struct ice_port_info *pi)
1187{
1188	struct ice_sched_node *node;
1189
1190	node = pi->root;
1191	while (node) {
1192		if (!node->num_children)
1193			break;
1194		node = node->children[0];
1195	}
1196	if (node && node->info.data.elem_type == ICE_AQC_ELEM_TYPE_LEAF) {
1197		u32 teid = le32_to_cpu(node->info.node_teid);
1198		int status;
1199
1200		/* remove the default leaf node */
1201		status = ice_sched_remove_elems(pi->hw, node->parent, teid);
1202		if (!status)
1203			ice_free_sched_node(pi, node);
1204	}
1205}
1206
1207/**
1208 * ice_sched_rm_dflt_nodes - free the default nodes in the tree
1209 * @pi: port information structure
1210 *
1211 * This function frees all the nodes except root and TC that were created by
1212 * the FW during initialization
1213 */
1214static void ice_sched_rm_dflt_nodes(struct ice_port_info *pi)
1215{
1216	struct ice_sched_node *node;
1217
1218	ice_rm_dflt_leaf_node(pi);
1219
1220	/* remove the default nodes except TC and root nodes */
1221	node = pi->root;
1222	while (node) {
1223		if (node->tx_sched_layer >= pi->hw->sw_entry_point_layer &&
1224		    node->info.data.elem_type != ICE_AQC_ELEM_TYPE_TC &&
1225		    node->info.data.elem_type != ICE_AQC_ELEM_TYPE_ROOT_PORT) {
1226			ice_free_sched_node(pi, node);
1227			break;
1228		}
1229
1230		if (!node->num_children)
1231			break;
1232		node = node->children[0];
1233	}
1234}
1235
1236/**
1237 * ice_sched_init_port - Initialize scheduler by querying information from FW
1238 * @pi: port info structure for the tree to cleanup
1239 *
1240 * This function is the initial call to find the total number of Tx scheduler
1241 * resources, default topology created by firmware and storing the information
1242 * in SW DB.
1243 */
1244int ice_sched_init_port(struct ice_port_info *pi)
1245{
1246	struct ice_aqc_get_topo_elem *buf;
1247	struct ice_hw *hw;
1248	u8 num_branches;
1249	u16 num_elems;
1250	int status;
1251	u8 i, j;
1252
1253	if (!pi)
1254		return -EINVAL;
1255	hw = pi->hw;
1256
1257	/* Query the Default Topology from FW */
1258	buf = kzalloc(ICE_AQ_MAX_BUF_LEN, GFP_KERNEL);
1259	if (!buf)
1260		return -ENOMEM;
1261
1262	/* Query default scheduling tree topology */
1263	status = ice_aq_get_dflt_topo(hw, pi->lport, buf, ICE_AQ_MAX_BUF_LEN,
1264				      &num_branches, NULL);
1265	if (status)
1266		goto err_init_port;
1267
1268	/* num_branches should be between 1-8 */
1269	if (num_branches < 1 || num_branches > ICE_TXSCHED_MAX_BRANCHES) {
1270		ice_debug(hw, ICE_DBG_SCHED, "num_branches unexpected %d\n",
1271			  num_branches);
1272		status = -EINVAL;
1273		goto err_init_port;
1274	}
1275
1276	/* get the number of elements on the default/first branch */
1277	num_elems = le16_to_cpu(buf[0].hdr.num_elems);
1278
1279	/* num_elems should always be between 1-9 */
1280	if (num_elems < 1 || num_elems > ICE_AQC_TOPO_MAX_LEVEL_NUM) {
1281		ice_debug(hw, ICE_DBG_SCHED, "num_elems unexpected %d\n",
1282			  num_elems);
1283		status = -EINVAL;
1284		goto err_init_port;
1285	}
1286
1287	/* If the last node is a leaf node then the index of the queue group
1288	 * layer is two less than the number of elements.
1289	 */
1290	if (num_elems > 2 && buf[0].generic[num_elems - 1].data.elem_type ==
1291	    ICE_AQC_ELEM_TYPE_LEAF)
1292		pi->last_node_teid =
1293			le32_to_cpu(buf[0].generic[num_elems - 2].node_teid);
1294	else
1295		pi->last_node_teid =
1296			le32_to_cpu(buf[0].generic[num_elems - 1].node_teid);
1297
1298	/* Insert the Tx Sched root node */
1299	status = ice_sched_add_root_node(pi, &buf[0].generic[0]);
1300	if (status)
1301		goto err_init_port;
1302
1303	/* Parse the default tree and cache the information */
1304	for (i = 0; i < num_branches; i++) {
1305		num_elems = le16_to_cpu(buf[i].hdr.num_elems);
1306
1307		/* Skip root element as already inserted */
1308		for (j = 1; j < num_elems; j++) {
1309			/* update the sw entry point */
1310			if (buf[0].generic[j].data.elem_type ==
1311			    ICE_AQC_ELEM_TYPE_ENTRY_POINT)
1312				hw->sw_entry_point_layer = j;
1313
1314			status = ice_sched_add_node(pi, j, &buf[i].generic[j], NULL);
1315			if (status)
1316				goto err_init_port;
1317		}
1318	}
1319
1320	/* Remove the default nodes. */
1321	if (pi->root)
1322		ice_sched_rm_dflt_nodes(pi);
1323
1324	/* initialize the port for handling the scheduler tree */
1325	pi->port_state = ICE_SCHED_PORT_STATE_READY;
1326	mutex_init(&pi->sched_lock);
1327	for (i = 0; i < ICE_AQC_TOPO_MAX_LEVEL_NUM; i++)
1328		INIT_LIST_HEAD(&pi->rl_prof_list[i]);
1329
1330err_init_port:
1331	if (status && pi->root) {
1332		ice_free_sched_node(pi, pi->root);
1333		pi->root = NULL;
1334	}
1335
1336	kfree(buf);
1337	return status;
1338}
1339
1340/**
1341 * ice_sched_query_res_alloc - query the FW for num of logical sched layers
1342 * @hw: pointer to the HW struct
1343 *
1344 * query FW for allocated scheduler resources and store in HW struct
1345 */
1346int ice_sched_query_res_alloc(struct ice_hw *hw)
1347{
1348	struct ice_aqc_query_txsched_res_resp *buf;
1349	__le16 max_sibl;
1350	int status = 0;
1351	u16 i;
1352
1353	if (hw->layer_info)
1354		return status;
1355
1356	buf = devm_kzalloc(ice_hw_to_dev(hw), sizeof(*buf), GFP_KERNEL);
1357	if (!buf)
1358		return -ENOMEM;
1359
1360	status = ice_aq_query_sched_res(hw, sizeof(*buf), buf, NULL);
1361	if (status)
1362		goto sched_query_out;
1363
1364	hw->num_tx_sched_layers = le16_to_cpu(buf->sched_props.logical_levels);
1365	hw->num_tx_sched_phys_layers =
1366		le16_to_cpu(buf->sched_props.phys_levels);
1367	hw->flattened_layers = buf->sched_props.flattening_bitmap;
1368	hw->max_cgds = buf->sched_props.max_pf_cgds;
1369
1370	/* max sibling group size of current layer refers to the max children
1371	 * of the below layer node.
1372	 * layer 1 node max children will be layer 2 max sibling group size
1373	 * layer 2 node max children will be layer 3 max sibling group size
1374	 * and so on. This array will be populated from root (index 0) to
1375	 * qgroup layer 7. Leaf node has no children.
1376	 */
1377	for (i = 0; i < hw->num_tx_sched_layers - 1; i++) {
1378		max_sibl = buf->layer_props[i + 1].max_sibl_grp_sz;
1379		hw->max_children[i] = le16_to_cpu(max_sibl);
1380	}
1381
1382	hw->layer_info = devm_kmemdup(ice_hw_to_dev(hw), buf->layer_props,
1383				      (hw->num_tx_sched_layers *
1384				       sizeof(*hw->layer_info)),
1385				      GFP_KERNEL);
1386	if (!hw->layer_info) {
1387		status = -ENOMEM;
1388		goto sched_query_out;
1389	}
1390
1391sched_query_out:
1392	devm_kfree(ice_hw_to_dev(hw), buf);
1393	return status;
1394}
1395
1396/**
1397 * ice_sched_get_psm_clk_freq - determine the PSM clock frequency
1398 * @hw: pointer to the HW struct
1399 *
1400 * Determine the PSM clock frequency and store in HW struct
1401 */
1402void ice_sched_get_psm_clk_freq(struct ice_hw *hw)
1403{
1404	u32 val, clk_src;
1405
1406	val = rd32(hw, GLGEN_CLKSTAT_SRC);
1407	clk_src = FIELD_GET(GLGEN_CLKSTAT_SRC_PSM_CLK_SRC_M, val);
1408
1409#define PSM_CLK_SRC_367_MHZ 0x0
1410#define PSM_CLK_SRC_416_MHZ 0x1
1411#define PSM_CLK_SRC_446_MHZ 0x2
1412#define PSM_CLK_SRC_390_MHZ 0x3
1413
1414	switch (clk_src) {
1415	case PSM_CLK_SRC_367_MHZ:
1416		hw->psm_clk_freq = ICE_PSM_CLK_367MHZ_IN_HZ;
1417		break;
1418	case PSM_CLK_SRC_416_MHZ:
1419		hw->psm_clk_freq = ICE_PSM_CLK_416MHZ_IN_HZ;
1420		break;
1421	case PSM_CLK_SRC_446_MHZ:
1422		hw->psm_clk_freq = ICE_PSM_CLK_446MHZ_IN_HZ;
1423		break;
1424	case PSM_CLK_SRC_390_MHZ:
1425		hw->psm_clk_freq = ICE_PSM_CLK_390MHZ_IN_HZ;
1426		break;
1427	default:
1428		ice_debug(hw, ICE_DBG_SCHED, "PSM clk_src unexpected %u\n",
1429			  clk_src);
1430		/* fall back to a safe default */
1431		hw->psm_clk_freq = ICE_PSM_CLK_446MHZ_IN_HZ;
1432	}
1433}
1434
1435/**
1436 * ice_sched_find_node_in_subtree - Find node in part of base node subtree
1437 * @hw: pointer to the HW struct
1438 * @base: pointer to the base node
1439 * @node: pointer to the node to search
1440 *
1441 * This function checks whether a given node is part of the base node
1442 * subtree or not
1443 */
1444static bool
1445ice_sched_find_node_in_subtree(struct ice_hw *hw, struct ice_sched_node *base,
1446			       struct ice_sched_node *node)
1447{
1448	u8 i;
1449
1450	for (i = 0; i < base->num_children; i++) {
1451		struct ice_sched_node *child = base->children[i];
1452
1453		if (node == child)
1454			return true;
1455
1456		if (child->tx_sched_layer > node->tx_sched_layer)
1457			return false;
1458
1459		/* this recursion is intentional, and wouldn't
1460		 * go more than 8 calls
1461		 */
1462		if (ice_sched_find_node_in_subtree(hw, child, node))
1463			return true;
1464	}
1465	return false;
1466}
1467
1468/**
1469 * ice_sched_get_free_qgrp - Scan all queue group siblings and find a free node
1470 * @pi: port information structure
1471 * @vsi_node: software VSI handle
1472 * @qgrp_node: first queue group node identified for scanning
1473 * @owner: LAN or RDMA
1474 *
1475 * This function retrieves a free LAN or RDMA queue group node by scanning
1476 * qgrp_node and its siblings for the queue group with the fewest number
1477 * of queues currently assigned.
1478 */
1479static struct ice_sched_node *
1480ice_sched_get_free_qgrp(struct ice_port_info *pi,
1481			struct ice_sched_node *vsi_node,
1482			struct ice_sched_node *qgrp_node, u8 owner)
1483{
1484	struct ice_sched_node *min_qgrp;
1485	u8 min_children;
1486
1487	if (!qgrp_node)
1488		return qgrp_node;
1489	min_children = qgrp_node->num_children;
1490	if (!min_children)
1491		return qgrp_node;
1492	min_qgrp = qgrp_node;
1493	/* scan all queue groups until find a node which has less than the
1494	 * minimum number of children. This way all queue group nodes get
1495	 * equal number of shares and active. The bandwidth will be equally
1496	 * distributed across all queues.
1497	 */
1498	while (qgrp_node) {
1499		/* make sure the qgroup node is part of the VSI subtree */
1500		if (ice_sched_find_node_in_subtree(pi->hw, vsi_node, qgrp_node))
1501			if (qgrp_node->num_children < min_children &&
1502			    qgrp_node->owner == owner) {
1503				/* replace the new min queue group node */
1504				min_qgrp = qgrp_node;
1505				min_children = min_qgrp->num_children;
1506				/* break if it has no children, */
1507				if (!min_children)
1508					break;
1509			}
1510		qgrp_node = qgrp_node->sibling;
1511	}
1512	return min_qgrp;
1513}
1514
1515/**
1516 * ice_sched_get_free_qparent - Get a free LAN or RDMA queue group node
1517 * @pi: port information structure
1518 * @vsi_handle: software VSI handle
1519 * @tc: branch number
1520 * @owner: LAN or RDMA
1521 *
1522 * This function retrieves a free LAN or RDMA queue group node
1523 */
1524struct ice_sched_node *
1525ice_sched_get_free_qparent(struct ice_port_info *pi, u16 vsi_handle, u8 tc,
1526			   u8 owner)
1527{
1528	struct ice_sched_node *vsi_node, *qgrp_node;
1529	struct ice_vsi_ctx *vsi_ctx;
1530	u8 qgrp_layer, vsi_layer;
1531	u16 max_children;
1532
1533	qgrp_layer = ice_sched_get_qgrp_layer(pi->hw);
1534	vsi_layer = ice_sched_get_vsi_layer(pi->hw);
1535	max_children = pi->hw->max_children[qgrp_layer];
1536
1537	vsi_ctx = ice_get_vsi_ctx(pi->hw, vsi_handle);
1538	if (!vsi_ctx)
1539		return NULL;
1540	vsi_node = vsi_ctx->sched.vsi_node[tc];
1541	/* validate invalid VSI ID */
1542	if (!vsi_node)
1543		return NULL;
1544
1545	/* If the queue group and VSI layer are same then queues
1546	 * are all attached directly to VSI
1547	 */
1548	if (qgrp_layer == vsi_layer)
1549		return vsi_node;
1550
1551	/* get the first queue group node from VSI sub-tree */
1552	qgrp_node = ice_sched_get_first_node(pi, vsi_node, qgrp_layer);
1553	while (qgrp_node) {
1554		struct ice_sched_node *next_vsi_node;
1555
1556		/* make sure the qgroup node is part of the VSI subtree */
1557		if (ice_sched_find_node_in_subtree(pi->hw, vsi_node, qgrp_node))
1558			if (qgrp_node->num_children < max_children &&
1559			    qgrp_node->owner == owner)
1560				break;
1561		qgrp_node = qgrp_node->sibling;
1562		if (qgrp_node)
1563			continue;
1564
1565		next_vsi_node = ice_sched_find_next_vsi_node(vsi_node);
1566		if (!next_vsi_node)
1567			break;
1568
1569		vsi_node = next_vsi_node;
1570		qgrp_node = ice_sched_get_first_node(pi, vsi_node, qgrp_layer);
1571	}
1572
1573	/* Select the best queue group */
1574	return ice_sched_get_free_qgrp(pi, vsi_node, qgrp_node, owner);
1575}
1576
1577/**
1578 * ice_sched_get_vsi_node - Get a VSI node based on VSI ID
1579 * @pi: pointer to the port information structure
1580 * @tc_node: pointer to the TC node
1581 * @vsi_handle: software VSI handle
1582 *
1583 * This function retrieves a VSI node for a given VSI ID from a given
1584 * TC branch
1585 */
1586static struct ice_sched_node *
1587ice_sched_get_vsi_node(struct ice_port_info *pi, struct ice_sched_node *tc_node,
1588		       u16 vsi_handle)
1589{
1590	struct ice_sched_node *node;
1591	u8 vsi_layer;
1592
1593	vsi_layer = ice_sched_get_vsi_layer(pi->hw);
1594	node = ice_sched_get_first_node(pi, tc_node, vsi_layer);
1595
1596	/* Check whether it already exists */
1597	while (node) {
1598		if (node->vsi_handle == vsi_handle)
1599			return node;
1600		node = node->sibling;
1601	}
1602
1603	return node;
1604}
1605
1606/**
1607 * ice_sched_get_agg_node - Get an aggregator node based on aggregator ID
1608 * @pi: pointer to the port information structure
1609 * @tc_node: pointer to the TC node
1610 * @agg_id: aggregator ID
1611 *
1612 * This function retrieves an aggregator node for a given aggregator ID from
1613 * a given TC branch
1614 */
1615struct ice_sched_node *
1616ice_sched_get_agg_node(struct ice_port_info *pi, struct ice_sched_node *tc_node,
1617		       u32 agg_id)
1618{
1619	struct ice_sched_node *node;
1620	struct ice_hw *hw = pi->hw;
1621	u8 agg_layer;
1622
1623	if (!hw)
1624		return NULL;
1625	agg_layer = ice_sched_get_agg_layer(hw);
1626	node = ice_sched_get_first_node(pi, tc_node, agg_layer);
1627
1628	/* Check whether it already exists */
1629	while (node) {
1630		if (node->agg_id == agg_id)
1631			return node;
1632		node = node->sibling;
1633	}
1634
1635	return node;
1636}
1637
1638/**
1639 * ice_sched_calc_vsi_child_nodes - calculate number of VSI child nodes
1640 * @hw: pointer to the HW struct
1641 * @num_new_qs: number of new queues that will be added to the tree
1642 * @num_nodes: num nodes array
1643 *
1644 * This function calculates the number of VSI child nodes based on the
1645 * number of queues.
1646 */
1647static void
1648ice_sched_calc_vsi_child_nodes(struct ice_hw *hw, u16 num_new_qs, u16 *num_nodes)
1649{
1650	u16 num = num_new_qs;
1651	u8 i, qgl, vsil;
1652
1653	qgl = ice_sched_get_qgrp_layer(hw);
1654	vsil = ice_sched_get_vsi_layer(hw);
1655
1656	/* calculate num nodes from queue group to VSI layer */
1657	for (i = qgl; i > vsil; i--) {
1658		/* round to the next integer if there is a remainder */
1659		num = DIV_ROUND_UP(num, hw->max_children[i]);
1660
1661		/* need at least one node */
1662		num_nodes[i] = num ? num : 1;
1663	}
1664}
1665
1666/**
1667 * ice_sched_add_vsi_child_nodes - add VSI child nodes to tree
1668 * @pi: port information structure
1669 * @vsi_handle: software VSI handle
1670 * @tc_node: pointer to the TC node
1671 * @num_nodes: pointer to the num nodes that needs to be added per layer
1672 * @owner: node owner (LAN or RDMA)
1673 *
1674 * This function adds the VSI child nodes to tree. It gets called for
1675 * LAN and RDMA separately.
1676 */
1677static int
1678ice_sched_add_vsi_child_nodes(struct ice_port_info *pi, u16 vsi_handle,
1679			      struct ice_sched_node *tc_node, u16 *num_nodes,
1680			      u8 owner)
1681{
1682	struct ice_sched_node *parent, *node;
1683	struct ice_hw *hw = pi->hw;
1684	u32 first_node_teid;
1685	u16 num_added = 0;
1686	u8 i, qgl, vsil;
1687
1688	qgl = ice_sched_get_qgrp_layer(hw);
1689	vsil = ice_sched_get_vsi_layer(hw);
1690	parent = ice_sched_get_vsi_node(pi, tc_node, vsi_handle);
1691	for (i = vsil + 1; i <= qgl; i++) {
1692		int status;
1693
1694		if (!parent)
1695			return -EIO;
1696
1697		status = ice_sched_add_nodes_to_layer(pi, tc_node, parent, i,
1698						      num_nodes[i],
1699						      &first_node_teid,
1700						      &num_added);
1701		if (status || num_nodes[i] != num_added)
1702			return -EIO;
1703
1704		/* The newly added node can be a new parent for the next
1705		 * layer nodes
1706		 */
1707		if (num_added) {
1708			parent = ice_sched_find_node_by_teid(tc_node,
1709							     first_node_teid);
1710			node = parent;
1711			while (node) {
1712				node->owner = owner;
1713				node = node->sibling;
1714			}
1715		} else {
1716			parent = parent->children[0];
1717		}
1718	}
1719
1720	return 0;
1721}
1722
1723/**
1724 * ice_sched_calc_vsi_support_nodes - calculate number of VSI support nodes
1725 * @pi: pointer to the port info structure
1726 * @tc_node: pointer to TC node
1727 * @num_nodes: pointer to num nodes array
1728 *
1729 * This function calculates the number of supported nodes needed to add this
1730 * VSI into Tx tree including the VSI, parent and intermediate nodes in below
1731 * layers
1732 */
1733static void
1734ice_sched_calc_vsi_support_nodes(struct ice_port_info *pi,
1735				 struct ice_sched_node *tc_node, u16 *num_nodes)
1736{
1737	struct ice_sched_node *node;
1738	u8 vsil;
1739	int i;
1740
1741	vsil = ice_sched_get_vsi_layer(pi->hw);
1742	for (i = vsil; i >= pi->hw->sw_entry_point_layer; i--)
1743		/* Add intermediate nodes if TC has no children and
1744		 * need at least one node for VSI
1745		 */
1746		if (!tc_node->num_children || i == vsil) {
1747			num_nodes[i]++;
1748		} else {
1749			/* If intermediate nodes are reached max children
1750			 * then add a new one.
1751			 */
1752			node = ice_sched_get_first_node(pi, tc_node, (u8)i);
1753			/* scan all the siblings */
1754			while (node) {
1755				if (node->num_children < pi->hw->max_children[i])
1756					break;
1757				node = node->sibling;
1758			}
1759
1760			/* tree has one intermediate node to add this new VSI.
1761			 * So no need to calculate supported nodes for below
1762			 * layers.
1763			 */
1764			if (node)
1765				break;
1766			/* all the nodes are full, allocate a new one */
1767			num_nodes[i]++;
1768		}
1769}
1770
1771/**
1772 * ice_sched_add_vsi_support_nodes - add VSI supported nodes into Tx tree
1773 * @pi: port information structure
1774 * @vsi_handle: software VSI handle
1775 * @tc_node: pointer to TC node
1776 * @num_nodes: pointer to num nodes array
1777 *
1778 * This function adds the VSI supported nodes into Tx tree including the
1779 * VSI, its parent and intermediate nodes in below layers
1780 */
1781static int
1782ice_sched_add_vsi_support_nodes(struct ice_port_info *pi, u16 vsi_handle,
1783				struct ice_sched_node *tc_node, u16 *num_nodes)
1784{
1785	struct ice_sched_node *parent = tc_node;
1786	u32 first_node_teid;
1787	u16 num_added = 0;
1788	u8 i, vsil;
1789
1790	if (!pi)
1791		return -EINVAL;
1792
1793	vsil = ice_sched_get_vsi_layer(pi->hw);
1794	for (i = pi->hw->sw_entry_point_layer; i <= vsil; i++) {
1795		int status;
1796
1797		status = ice_sched_add_nodes_to_layer(pi, tc_node, parent,
1798						      i, num_nodes[i],
1799						      &first_node_teid,
1800						      &num_added);
1801		if (status || num_nodes[i] != num_added)
1802			return -EIO;
1803
1804		/* The newly added node can be a new parent for the next
1805		 * layer nodes
1806		 */
1807		if (num_added)
1808			parent = ice_sched_find_node_by_teid(tc_node,
1809							     first_node_teid);
1810		else
1811			parent = parent->children[0];
1812
1813		if (!parent)
1814			return -EIO;
1815
1816		/* Do not modify the VSI handle for already existing VSI nodes,
1817		 * (if no new VSI node was added to the tree).
1818		 * Assign the VSI handle only to newly added VSI nodes.
1819		 */
1820		if (i == vsil && num_added)
1821			parent->vsi_handle = vsi_handle;
1822	}
1823
1824	return 0;
1825}
1826
1827/**
1828 * ice_sched_add_vsi_to_topo - add a new VSI into tree
1829 * @pi: port information structure
1830 * @vsi_handle: software VSI handle
1831 * @tc: TC number
1832 *
1833 * This function adds a new VSI into scheduler tree
1834 */
1835static int
1836ice_sched_add_vsi_to_topo(struct ice_port_info *pi, u16 vsi_handle, u8 tc)
1837{
1838	u16 num_nodes[ICE_AQC_TOPO_MAX_LEVEL_NUM] = { 0 };
1839	struct ice_sched_node *tc_node;
1840
1841	tc_node = ice_sched_get_tc_node(pi, tc);
1842	if (!tc_node)
1843		return -EINVAL;
1844
1845	/* calculate number of supported nodes needed for this VSI */
1846	ice_sched_calc_vsi_support_nodes(pi, tc_node, num_nodes);
1847
1848	/* add VSI supported nodes to TC subtree */
1849	return ice_sched_add_vsi_support_nodes(pi, vsi_handle, tc_node,
1850					       num_nodes);
1851}
1852
1853/**
1854 * ice_sched_recalc_vsi_support_nodes - recalculate VSI support nodes count
1855 * @hw: pointer to the HW struct
1856 * @vsi_node: pointer to the leftmost VSI node that needs to be extended
1857 * @new_numqs: new number of queues that has to be handled by the VSI
1858 * @new_num_nodes: pointer to nodes count table to modify the VSI layer entry
1859 *
1860 * This function recalculates the number of supported nodes that need to
1861 * be added after adding more Tx queues for a given VSI.
1862 * The number of new VSI support nodes that shall be added will be saved
1863 * to the @new_num_nodes table for the VSI layer.
1864 */
1865static void
1866ice_sched_recalc_vsi_support_nodes(struct ice_hw *hw,
1867				   struct ice_sched_node *vsi_node,
1868				   unsigned int new_numqs, u16 *new_num_nodes)
1869{
1870	u32 vsi_nodes_cnt = 1;
1871	u32 max_queue_cnt = 1;
1872	u32 qgl, vsil;
1873
1874	qgl = ice_sched_get_qgrp_layer(hw);
1875	vsil = ice_sched_get_vsi_layer(hw);
1876
1877	for (u32 i = vsil; i <= qgl; i++)
1878		max_queue_cnt *= hw->max_children[i];
1879
1880	while ((vsi_node = ice_sched_find_next_vsi_node(vsi_node)) != NULL)
1881		vsi_nodes_cnt++;
1882
1883	if (new_numqs > (max_queue_cnt * vsi_nodes_cnt))
1884		new_num_nodes[vsil] = DIV_ROUND_UP(new_numqs, max_queue_cnt) -
1885				      vsi_nodes_cnt;
1886}
1887
1888/**
1889 * ice_sched_update_vsi_child_nodes - update VSI child nodes
1890 * @pi: port information structure
1891 * @vsi_handle: software VSI handle
1892 * @tc: TC number
1893 * @new_numqs: new number of max queues
1894 * @owner: owner of this subtree
1895 *
1896 * This function updates the VSI child nodes based on the number of queues
1897 */
1898static int
1899ice_sched_update_vsi_child_nodes(struct ice_port_info *pi, u16 vsi_handle,
1900				 u8 tc, u16 new_numqs, u8 owner)
1901{
1902	u16 new_num_nodes[ICE_AQC_TOPO_MAX_LEVEL_NUM] = { 0 };
1903	struct ice_sched_node *vsi_node;
1904	struct ice_sched_node *tc_node;
1905	struct ice_vsi_ctx *vsi_ctx;
1906	struct ice_hw *hw = pi->hw;
1907	u16 prev_numqs;
1908	int status = 0;
1909
1910	tc_node = ice_sched_get_tc_node(pi, tc);
1911	if (!tc_node)
1912		return -EIO;
1913
1914	vsi_node = ice_sched_get_vsi_node(pi, tc_node, vsi_handle);
1915	if (!vsi_node)
1916		return -EIO;
1917
1918	vsi_ctx = ice_get_vsi_ctx(hw, vsi_handle);
1919	if (!vsi_ctx)
1920		return -EINVAL;
1921
1922	if (owner == ICE_SCHED_NODE_OWNER_LAN)
1923		prev_numqs = vsi_ctx->sched.max_lanq[tc];
1924	else
1925		prev_numqs = vsi_ctx->sched.max_rdmaq[tc];
1926	/* num queues are not changed or less than the previous number */
1927	if (new_numqs <= prev_numqs)
1928		return status;
1929	if (owner == ICE_SCHED_NODE_OWNER_LAN) {
1930		status = ice_alloc_lan_q_ctx(hw, vsi_handle, tc, new_numqs);
1931		if (status)
1932			return status;
1933	} else {
1934		status = ice_alloc_rdma_q_ctx(hw, vsi_handle, tc, new_numqs);
1935		if (status)
1936			return status;
1937	}
1938
1939	ice_sched_recalc_vsi_support_nodes(hw, vsi_node,
1940					   new_numqs, new_num_nodes);
1941	ice_sched_calc_vsi_child_nodes(hw, new_numqs - prev_numqs,
1942				       new_num_nodes);
1943
1944	/* Never decrease the number of queues in the tree. Update the tree
1945	 * only if number of queues > previous number of queues. This may
1946	 * leave some extra nodes in the tree if number of queues < previous
1947	 * number but that wouldn't harm anything. Removing those extra nodes
1948	 * may complicate the code if those nodes are part of SRL or
1949	 * individually rate limited.
1950	 * Also, add the required VSI support nodes if the existing ones cannot
1951	 * handle the requested new number of queues.
1952	 */
1953	status = ice_sched_add_vsi_support_nodes(pi, vsi_handle, tc_node,
1954						 new_num_nodes);
1955	if (status)
1956		return status;
1957
1958	status = ice_sched_add_vsi_child_nodes(pi, vsi_handle, tc_node,
1959					       new_num_nodes, owner);
1960	if (status)
1961		return status;
1962	if (owner == ICE_SCHED_NODE_OWNER_LAN)
1963		vsi_ctx->sched.max_lanq[tc] = new_numqs;
1964	else
1965		vsi_ctx->sched.max_rdmaq[tc] = new_numqs;
1966
1967	return 0;
1968}
1969
1970/**
1971 * ice_sched_cfg_vsi - configure the new/existing VSI
1972 * @pi: port information structure
1973 * @vsi_handle: software VSI handle
1974 * @tc: TC number
1975 * @maxqs: max number of queues
1976 * @owner: LAN or RDMA
1977 * @enable: TC enabled or disabled
1978 *
1979 * This function adds/updates VSI nodes based on the number of queues. If TC is
1980 * enabled and VSI is in suspended state then resume the VSI back. If TC is
1981 * disabled then suspend the VSI if it is not already.
1982 */
1983int
1984ice_sched_cfg_vsi(struct ice_port_info *pi, u16 vsi_handle, u8 tc, u16 maxqs,
1985		  u8 owner, bool enable)
1986{
1987	struct ice_sched_node *vsi_node, *tc_node;
1988	struct ice_vsi_ctx *vsi_ctx;
1989	struct ice_hw *hw = pi->hw;
1990	int status = 0;
1991
1992	ice_debug(pi->hw, ICE_DBG_SCHED, "add/config VSI %d\n", vsi_handle);
1993	tc_node = ice_sched_get_tc_node(pi, tc);
1994	if (!tc_node)
1995		return -EINVAL;
1996	vsi_ctx = ice_get_vsi_ctx(hw, vsi_handle);
1997	if (!vsi_ctx)
1998		return -EINVAL;
1999	vsi_node = ice_sched_get_vsi_node(pi, tc_node, vsi_handle);
2000
2001	/* suspend the VSI if TC is not enabled */
2002	if (!enable) {
2003		if (vsi_node && vsi_node->in_use) {
2004			u32 teid = le32_to_cpu(vsi_node->info.node_teid);
2005
2006			status = ice_sched_suspend_resume_elems(hw, 1, &teid,
2007								true);
2008			if (!status)
2009				vsi_node->in_use = false;
2010		}
2011		return status;
2012	}
2013
2014	/* TC is enabled, if it is a new VSI then add it to the tree */
2015	if (!vsi_node) {
2016		status = ice_sched_add_vsi_to_topo(pi, vsi_handle, tc);
2017		if (status)
2018			return status;
2019
2020		vsi_node = ice_sched_get_vsi_node(pi, tc_node, vsi_handle);
2021		if (!vsi_node)
2022			return -EIO;
2023
2024		vsi_ctx->sched.vsi_node[tc] = vsi_node;
2025		vsi_node->in_use = true;
2026		/* invalidate the max queues whenever VSI gets added first time
2027		 * into the scheduler tree (boot or after reset). We need to
2028		 * recreate the child nodes all the time in these cases.
2029		 */
2030		vsi_ctx->sched.max_lanq[tc] = 0;
2031		vsi_ctx->sched.max_rdmaq[tc] = 0;
2032	}
2033
2034	/* update the VSI child nodes */
2035	status = ice_sched_update_vsi_child_nodes(pi, vsi_handle, tc, maxqs,
2036						  owner);
2037	if (status)
2038		return status;
2039
2040	/* TC is enabled, resume the VSI if it is in the suspend state */
2041	if (!vsi_node->in_use) {
2042		u32 teid = le32_to_cpu(vsi_node->info.node_teid);
2043
2044		status = ice_sched_suspend_resume_elems(hw, 1, &teid, false);
2045		if (!status)
2046			vsi_node->in_use = true;
2047	}
2048
2049	return status;
2050}
2051
2052/**
2053 * ice_sched_rm_agg_vsi_info - remove aggregator related VSI info entry
2054 * @pi: port information structure
2055 * @vsi_handle: software VSI handle
2056 *
2057 * This function removes single aggregator VSI info entry from
2058 * aggregator list.
2059 */
2060static void ice_sched_rm_agg_vsi_info(struct ice_port_info *pi, u16 vsi_handle)
2061{
2062	struct ice_sched_agg_info *agg_info;
2063	struct ice_sched_agg_info *atmp;
2064
2065	list_for_each_entry_safe(agg_info, atmp, &pi->hw->agg_list,
2066				 list_entry) {
2067		struct ice_sched_agg_vsi_info *agg_vsi_info;
2068		struct ice_sched_agg_vsi_info *vtmp;
2069
2070		list_for_each_entry_safe(agg_vsi_info, vtmp,
2071					 &agg_info->agg_vsi_list, list_entry)
2072			if (agg_vsi_info->vsi_handle == vsi_handle) {
2073				list_del(&agg_vsi_info->list_entry);
2074				devm_kfree(ice_hw_to_dev(pi->hw),
2075					   agg_vsi_info);
2076				return;
2077			}
2078	}
2079}
2080
2081/**
2082 * ice_sched_is_leaf_node_present - check for a leaf node in the sub-tree
2083 * @node: pointer to the sub-tree node
2084 *
2085 * This function checks for a leaf node presence in a given sub-tree node.
2086 */
2087static bool ice_sched_is_leaf_node_present(struct ice_sched_node *node)
2088{
2089	u8 i;
2090
2091	for (i = 0; i < node->num_children; i++)
2092		if (ice_sched_is_leaf_node_present(node->children[i]))
2093			return true;
2094	/* check for a leaf node */
2095	return (node->info.data.elem_type == ICE_AQC_ELEM_TYPE_LEAF);
2096}
2097
2098/**
2099 * ice_sched_rm_vsi_subtree - remove all nodes assigned to a given VSI
2100 * @pi: port information structure
2101 * @vsi_node: pointer to the leftmost node of the VSI to be removed
2102 * @owner: LAN or RDMA
2103 * @tc: TC number
2104 *
2105 * Return: Zero in case of success, or -EBUSY if the VSI has leaf nodes in TC.
2106 *
2107 * This function removes all the VSI support nodes associated with a given VSI
2108 * and its LAN or RDMA children nodes from the scheduler tree.
2109 */
2110static int
2111ice_sched_rm_vsi_subtree(struct ice_port_info *pi,
2112			 struct ice_sched_node *vsi_node, u8 owner, u8 tc)
2113{
2114	u16 vsi_handle = vsi_node->vsi_handle;
2115	bool all_vsi_nodes_removed = true;
2116	int j = 0;
2117
2118	while (vsi_node) {
2119		struct ice_sched_node *next_vsi_node;
2120
2121		if (ice_sched_is_leaf_node_present(vsi_node)) {
2122			ice_debug(pi->hw, ICE_DBG_SCHED, "VSI has leaf nodes in TC %d\n", tc);
2123			return -EBUSY;
2124		}
2125		while (j < vsi_node->num_children) {
2126			if (vsi_node->children[j]->owner == owner)
2127				ice_free_sched_node(pi, vsi_node->children[j]);
2128			else
2129				j++;
2130		}
2131
2132		next_vsi_node = ice_sched_find_next_vsi_node(vsi_node);
2133
2134		/* remove the VSI if it has no children */
2135		if (!vsi_node->num_children)
2136			ice_free_sched_node(pi, vsi_node);
2137		else
2138			all_vsi_nodes_removed = false;
2139
2140		vsi_node = next_vsi_node;
2141	}
2142
2143	/* clean up aggregator related VSI info if any */
2144	if (all_vsi_nodes_removed)
2145		ice_sched_rm_agg_vsi_info(pi, vsi_handle);
2146
2147	return 0;
2148}
2149
2150/**
2151 * ice_sched_rm_vsi_cfg - remove the VSI and its children nodes
2152 * @pi: port information structure
2153 * @vsi_handle: software VSI handle
2154 * @owner: LAN or RDMA
2155 *
2156 * This function removes the VSI and its LAN or RDMA children nodes from the
2157 * scheduler tree.
2158 */
2159static int
2160ice_sched_rm_vsi_cfg(struct ice_port_info *pi, u16 vsi_handle, u8 owner)
2161{
2162	struct ice_vsi_ctx *vsi_ctx;
2163	int status = -EINVAL;
2164	u8 i;
2165
2166	ice_debug(pi->hw, ICE_DBG_SCHED, "removing VSI %d\n", vsi_handle);
2167	if (!ice_is_vsi_valid(pi->hw, vsi_handle))
2168		return status;
2169	mutex_lock(&pi->sched_lock);
2170	vsi_ctx = ice_get_vsi_ctx(pi->hw, vsi_handle);
2171	if (!vsi_ctx)
2172		goto exit_sched_rm_vsi_cfg;
2173
2174	ice_for_each_traffic_class(i) {
2175		struct ice_sched_node *vsi_node, *tc_node;
2176
2177		tc_node = ice_sched_get_tc_node(pi, i);
2178		if (!tc_node)
2179			continue;
2180
2181		vsi_node = ice_sched_get_vsi_node(pi, tc_node, vsi_handle);
2182		if (!vsi_node)
2183			continue;
2184
2185		status = ice_sched_rm_vsi_subtree(pi, vsi_node, owner, i);
2186		if (status)
2187			goto exit_sched_rm_vsi_cfg;
2188
2189		vsi_ctx->sched.vsi_node[i] = NULL;
2190
2191		if (owner == ICE_SCHED_NODE_OWNER_LAN)
2192			vsi_ctx->sched.max_lanq[i] = 0;
2193		else
2194			vsi_ctx->sched.max_rdmaq[i] = 0;
2195	}
2196	status = 0;
2197
2198exit_sched_rm_vsi_cfg:
2199	mutex_unlock(&pi->sched_lock);
2200	return status;
2201}
2202
2203/**
2204 * ice_rm_vsi_lan_cfg - remove VSI and its LAN children nodes
2205 * @pi: port information structure
2206 * @vsi_handle: software VSI handle
2207 *
2208 * This function clears the VSI and its LAN children nodes from scheduler tree
2209 * for all TCs.
2210 */
2211int ice_rm_vsi_lan_cfg(struct ice_port_info *pi, u16 vsi_handle)
2212{
2213	return ice_sched_rm_vsi_cfg(pi, vsi_handle, ICE_SCHED_NODE_OWNER_LAN);
2214}
2215
2216/**
2217 * ice_rm_vsi_rdma_cfg - remove VSI and its RDMA children nodes
2218 * @pi: port information structure
2219 * @vsi_handle: software VSI handle
2220 *
2221 * This function clears the VSI and its RDMA children nodes from scheduler tree
2222 * for all TCs.
2223 */
2224int ice_rm_vsi_rdma_cfg(struct ice_port_info *pi, u16 vsi_handle)
2225{
2226	return ice_sched_rm_vsi_cfg(pi, vsi_handle, ICE_SCHED_NODE_OWNER_RDMA);
2227}
2228
2229/**
2230 * ice_get_agg_info - get the aggregator ID
2231 * @hw: pointer to the hardware structure
2232 * @agg_id: aggregator ID
2233 *
2234 * This function validates aggregator ID. The function returns info if
2235 * aggregator ID is present in list otherwise it returns null.
2236 */
2237static struct ice_sched_agg_info *
2238ice_get_agg_info(struct ice_hw *hw, u32 agg_id)
2239{
2240	struct ice_sched_agg_info *agg_info;
2241
2242	list_for_each_entry(agg_info, &hw->agg_list, list_entry)
2243		if (agg_info->agg_id == agg_id)
2244			return agg_info;
2245
2246	return NULL;
2247}
2248
2249/**
2250 * ice_sched_get_free_vsi_parent - Find a free parent node in aggregator subtree
2251 * @hw: pointer to the HW struct
2252 * @node: pointer to a child node
2253 * @num_nodes: num nodes count array
2254 *
2255 * This function walks through the aggregator subtree to find a free parent
2256 * node
2257 */
2258struct ice_sched_node *
2259ice_sched_get_free_vsi_parent(struct ice_hw *hw, struct ice_sched_node *node,
2260			      u16 *num_nodes)
2261{
2262	u8 l = node->tx_sched_layer;
2263	u8 vsil, i;
2264
2265	vsil = ice_sched_get_vsi_layer(hw);
2266
2267	/* Is it VSI parent layer ? */
2268	if (l == vsil - 1)
2269		return (node->num_children < hw->max_children[l]) ? node : NULL;
2270
2271	/* We have intermediate nodes. Let's walk through the subtree. If the
2272	 * intermediate node has space to add a new node then clear the count
2273	 */
2274	if (node->num_children < hw->max_children[l])
2275		num_nodes[l] = 0;
2276	/* The below recursive call is intentional and wouldn't go more than
2277	 * 2 or 3 iterations.
2278	 */
2279
2280	for (i = 0; i < node->num_children; i++) {
2281		struct ice_sched_node *parent;
2282
2283		parent = ice_sched_get_free_vsi_parent(hw, node->children[i],
2284						       num_nodes);
2285		if (parent)
2286			return parent;
2287	}
2288
2289	return NULL;
2290}
2291
2292/**
2293 * ice_sched_update_parent - update the new parent in SW DB
2294 * @new_parent: pointer to a new parent node
2295 * @node: pointer to a child node
2296 *
2297 * This function removes the child from the old parent and adds it to a new
2298 * parent
2299 */
2300void
2301ice_sched_update_parent(struct ice_sched_node *new_parent,
2302			struct ice_sched_node *node)
2303{
2304	struct ice_sched_node *old_parent;
2305	u8 i, j;
2306
2307	old_parent = node->parent;
2308
2309	/* update the old parent children */
2310	for (i = 0; i < old_parent->num_children; i++)
2311		if (old_parent->children[i] == node) {
2312			for (j = i + 1; j < old_parent->num_children; j++)
2313				old_parent->children[j - 1] =
2314					old_parent->children[j];
2315			old_parent->num_children--;
2316			break;
2317		}
2318
2319	/* now move the node to a new parent */
2320	new_parent->children[new_parent->num_children++] = node;
2321	node->parent = new_parent;
2322	node->info.parent_teid = new_parent->info.node_teid;
2323}
2324
2325/**
2326 * ice_sched_move_nodes - move child nodes to a given parent
2327 * @pi: port information structure
2328 * @parent: pointer to parent node
2329 * @num_items: number of child nodes to be moved
2330 * @list: pointer to child node teids
2331 *
2332 * This function move the child nodes to a given parent.
2333 */
2334int
2335ice_sched_move_nodes(struct ice_port_info *pi, struct ice_sched_node *parent,
2336		     u16 num_items, u32 *list)
2337{
2338	DEFINE_RAW_FLEX(struct ice_aqc_move_elem, buf, teid, 1);
2339	u16 buf_len = __struct_size(buf);
2340	struct ice_sched_node *node;
2341	u16 i, grps_movd = 0;
2342	struct ice_hw *hw;
2343	int status = 0;
2344
2345	hw = pi->hw;
2346
2347	if (!parent || !num_items)
2348		return -EINVAL;
2349
2350	/* Does parent have enough space */
2351	if (parent->num_children + num_items >
2352	    hw->max_children[parent->tx_sched_layer])
2353		return -ENOSPC;
2354
2355	for (i = 0; i < num_items; i++) {
2356		node = ice_sched_find_node_by_teid(pi->root, list[i]);
2357		if (!node) {
2358			status = -EINVAL;
2359			break;
2360		}
2361
2362		buf->hdr.src_parent_teid = node->info.parent_teid;
2363		buf->hdr.dest_parent_teid = parent->info.node_teid;
2364		buf->teid[0] = node->info.node_teid;
2365		buf->hdr.num_elems = cpu_to_le16(1);
2366		status = ice_aq_move_sched_elems(hw, buf, buf_len, &grps_movd);
2367		if (status && grps_movd != 1) {
2368			status = -EIO;
2369			break;
2370		}
2371
2372		/* update the SW DB */
2373		ice_sched_update_parent(parent, node);
2374	}
2375
2376	return status;
2377}
2378
2379/**
2380 * ice_sched_move_vsi_to_agg - move VSI to aggregator node
2381 * @pi: port information structure
2382 * @vsi_handle: software VSI handle
2383 * @agg_id: aggregator ID
2384 * @tc: TC number
2385 *
2386 * This function moves a VSI to an aggregator node or its subtree.
2387 * Intermediate nodes may be created if required.
2388 */
2389static int
2390ice_sched_move_vsi_to_agg(struct ice_port_info *pi, u16 vsi_handle, u32 agg_id,
2391			  u8 tc)
2392{
2393	struct ice_sched_node *vsi_node, *agg_node, *tc_node, *parent;
2394	u16 num_nodes[ICE_AQC_TOPO_MAX_LEVEL_NUM] = { 0 };
2395	u32 first_node_teid, vsi_teid;
2396	u16 num_nodes_added;
2397	u8 aggl, vsil, i;
2398	int status;
2399
2400	tc_node = ice_sched_get_tc_node(pi, tc);
2401	if (!tc_node)
2402		return -EIO;
2403
2404	agg_node = ice_sched_get_agg_node(pi, tc_node, agg_id);
2405	if (!agg_node)
2406		return -ENOENT;
2407
2408	vsi_node = ice_sched_get_vsi_node(pi, tc_node, vsi_handle);
2409	if (!vsi_node)
2410		return -ENOENT;
2411
2412	/* Is this VSI already part of given aggregator? */
2413	if (ice_sched_find_node_in_subtree(pi->hw, agg_node, vsi_node))
2414		return 0;
2415
2416	aggl = ice_sched_get_agg_layer(pi->hw);
2417	vsil = ice_sched_get_vsi_layer(pi->hw);
2418
2419	/* set intermediate node count to 1 between aggregator and VSI layers */
2420	for (i = aggl + 1; i < vsil; i++)
2421		num_nodes[i] = 1;
2422
2423	/* Check if the aggregator subtree has any free node to add the VSI */
2424	for (i = 0; i < agg_node->num_children; i++) {
2425		parent = ice_sched_get_free_vsi_parent(pi->hw,
2426						       agg_node->children[i],
2427						       num_nodes);
2428		if (parent)
2429			goto move_nodes;
2430	}
2431
2432	/* add new nodes */
2433	parent = agg_node;
2434	for (i = aggl + 1; i < vsil; i++) {
2435		status = ice_sched_add_nodes_to_layer(pi, tc_node, parent, i,
2436						      num_nodes[i],
2437						      &first_node_teid,
2438						      &num_nodes_added);
2439		if (status || num_nodes[i] != num_nodes_added)
2440			return -EIO;
2441
2442		/* The newly added node can be a new parent for the next
2443		 * layer nodes
2444		 */
2445		if (num_nodes_added)
2446			parent = ice_sched_find_node_by_teid(tc_node,
2447							     first_node_teid);
2448		else
2449			parent = parent->children[0];
2450
2451		if (!parent)
2452			return -EIO;
2453	}
2454
2455move_nodes:
2456	vsi_teid = le32_to_cpu(vsi_node->info.node_teid);
2457	return ice_sched_move_nodes(pi, parent, 1, &vsi_teid);
2458}
2459
2460/**
2461 * ice_move_all_vsi_to_dflt_agg - move all VSI(s) to default aggregator
2462 * @pi: port information structure
2463 * @agg_info: aggregator info
2464 * @tc: traffic class number
2465 * @rm_vsi_info: true or false
2466 *
2467 * This function move all the VSI(s) to the default aggregator and delete
2468 * aggregator VSI info based on passed in boolean parameter rm_vsi_info. The
2469 * caller holds the scheduler lock.
2470 */
2471static int
2472ice_move_all_vsi_to_dflt_agg(struct ice_port_info *pi,
2473			     struct ice_sched_agg_info *agg_info, u8 tc,
2474			     bool rm_vsi_info)
2475{
2476	struct ice_sched_agg_vsi_info *agg_vsi_info;
2477	struct ice_sched_agg_vsi_info *tmp;
2478	int status = 0;
2479
2480	list_for_each_entry_safe(agg_vsi_info, tmp, &agg_info->agg_vsi_list,
2481				 list_entry) {
2482		u16 vsi_handle = agg_vsi_info->vsi_handle;
2483
2484		/* Move VSI to default aggregator */
2485		if (!ice_is_tc_ena(agg_vsi_info->tc_bitmap[0], tc))
2486			continue;
2487
2488		status = ice_sched_move_vsi_to_agg(pi, vsi_handle,
2489						   ICE_DFLT_AGG_ID, tc);
2490		if (status)
2491			break;
2492
2493		clear_bit(tc, agg_vsi_info->tc_bitmap);
2494		if (rm_vsi_info && !agg_vsi_info->tc_bitmap[0]) {
2495			list_del(&agg_vsi_info->list_entry);
2496			devm_kfree(ice_hw_to_dev(pi->hw), agg_vsi_info);
2497		}
2498	}
2499
2500	return status;
2501}
2502
2503/**
2504 * ice_sched_is_agg_inuse - check whether the aggregator is in use or not
2505 * @pi: port information structure
2506 * @node: node pointer
2507 *
2508 * This function checks whether the aggregator is attached with any VSI or not.
2509 */
2510static bool
2511ice_sched_is_agg_inuse(struct ice_port_info *pi, struct ice_sched_node *node)
2512{
2513	u8 vsil, i;
2514
2515	vsil = ice_sched_get_vsi_layer(pi->hw);
2516	if (node->tx_sched_layer < vsil - 1) {
2517		for (i = 0; i < node->num_children; i++)
2518			if (ice_sched_is_agg_inuse(pi, node->children[i]))
2519				return true;
2520		return false;
2521	} else {
2522		return node->num_children ? true : false;
2523	}
2524}
2525
2526/**
2527 * ice_sched_rm_agg_cfg - remove the aggregator node
2528 * @pi: port information structure
2529 * @agg_id: aggregator ID
2530 * @tc: TC number
2531 *
2532 * This function removes the aggregator node and intermediate nodes if any
2533 * from the given TC
2534 */
2535static int
2536ice_sched_rm_agg_cfg(struct ice_port_info *pi, u32 agg_id, u8 tc)
2537{
2538	struct ice_sched_node *tc_node, *agg_node;
2539	struct ice_hw *hw = pi->hw;
2540
2541	tc_node = ice_sched_get_tc_node(pi, tc);
2542	if (!tc_node)
2543		return -EIO;
2544
2545	agg_node = ice_sched_get_agg_node(pi, tc_node, agg_id);
2546	if (!agg_node)
2547		return -ENOENT;
2548
2549	/* Can't remove the aggregator node if it has children */
2550	if (ice_sched_is_agg_inuse(pi, agg_node))
2551		return -EBUSY;
2552
2553	/* need to remove the whole subtree if aggregator node is the
2554	 * only child.
2555	 */
2556	while (agg_node->tx_sched_layer > hw->sw_entry_point_layer) {
2557		struct ice_sched_node *parent = agg_node->parent;
2558
2559		if (!parent)
2560			return -EIO;
2561
2562		if (parent->num_children > 1)
2563			break;
2564
2565		agg_node = parent;
2566	}
2567
2568	ice_free_sched_node(pi, agg_node);
2569	return 0;
2570}
2571
2572/**
2573 * ice_rm_agg_cfg_tc - remove aggregator configuration for TC
2574 * @pi: port information structure
2575 * @agg_info: aggregator ID
2576 * @tc: TC number
2577 * @rm_vsi_info: bool value true or false
2578 *
2579 * This function removes aggregator reference to VSI of given TC. It removes
2580 * the aggregator configuration completely for requested TC. The caller needs
2581 * to hold the scheduler lock.
2582 */
2583static int
2584ice_rm_agg_cfg_tc(struct ice_port_info *pi, struct ice_sched_agg_info *agg_info,
2585		  u8 tc, bool rm_vsi_info)
2586{
2587	int status = 0;
2588
2589	/* If nothing to remove - return success */
2590	if (!ice_is_tc_ena(agg_info->tc_bitmap[0], tc))
2591		goto exit_rm_agg_cfg_tc;
2592
2593	status = ice_move_all_vsi_to_dflt_agg(pi, agg_info, tc, rm_vsi_info);
2594	if (status)
2595		goto exit_rm_agg_cfg_tc;
2596
2597	/* Delete aggregator node(s) */
2598	status = ice_sched_rm_agg_cfg(pi, agg_info->agg_id, tc);
2599	if (status)
2600		goto exit_rm_agg_cfg_tc;
2601
2602	clear_bit(tc, agg_info->tc_bitmap);
2603exit_rm_agg_cfg_tc:
2604	return status;
2605}
2606
2607/**
2608 * ice_save_agg_tc_bitmap - save aggregator TC bitmap
2609 * @pi: port information structure
2610 * @agg_id: aggregator ID
2611 * @tc_bitmap: 8 bits TC bitmap
2612 *
2613 * Save aggregator TC bitmap. This function needs to be called with scheduler
2614 * lock held.
2615 */
2616static int
2617ice_save_agg_tc_bitmap(struct ice_port_info *pi, u32 agg_id,
2618		       unsigned long *tc_bitmap)
2619{
2620	struct ice_sched_agg_info *agg_info;
2621
2622	agg_info = ice_get_agg_info(pi->hw, agg_id);
2623	if (!agg_info)
2624		return -EINVAL;
2625	bitmap_copy(agg_info->replay_tc_bitmap, tc_bitmap,
2626		    ICE_MAX_TRAFFIC_CLASS);
2627	return 0;
2628}
2629
2630/**
2631 * ice_sched_add_agg_cfg - create an aggregator node
2632 * @pi: port information structure
2633 * @agg_id: aggregator ID
2634 * @tc: TC number
2635 *
2636 * This function creates an aggregator node and intermediate nodes if required
2637 * for the given TC
2638 */
2639static int
2640ice_sched_add_agg_cfg(struct ice_port_info *pi, u32 agg_id, u8 tc)
2641{
2642	struct ice_sched_node *parent, *agg_node, *tc_node;
2643	u16 num_nodes[ICE_AQC_TOPO_MAX_LEVEL_NUM] = { 0 };
2644	struct ice_hw *hw = pi->hw;
2645	u32 first_node_teid;
2646	u16 num_nodes_added;
2647	int status = 0;
2648	u8 i, aggl;
2649
2650	tc_node = ice_sched_get_tc_node(pi, tc);
2651	if (!tc_node)
2652		return -EIO;
2653
2654	agg_node = ice_sched_get_agg_node(pi, tc_node, agg_id);
2655	/* Does Agg node already exist ? */
2656	if (agg_node)
2657		return status;
2658
2659	aggl = ice_sched_get_agg_layer(hw);
2660
2661	/* need one node in Agg layer */
2662	num_nodes[aggl] = 1;
2663
2664	/* Check whether the intermediate nodes have space to add the
2665	 * new aggregator. If they are full, then SW needs to allocate a new
2666	 * intermediate node on those layers
2667	 */
2668	for (i = hw->sw_entry_point_layer; i < aggl; i++) {
2669		parent = ice_sched_get_first_node(pi, tc_node, i);
2670
2671		/* scan all the siblings */
2672		while (parent) {
2673			if (parent->num_children < hw->max_children[i])
2674				break;
2675			parent = parent->sibling;
2676		}
2677
2678		/* all the nodes are full, reserve one for this layer */
2679		if (!parent)
2680			num_nodes[i]++;
2681	}
2682
2683	/* add the aggregator node */
2684	parent = tc_node;
2685	for (i = hw->sw_entry_point_layer; i <= aggl; i++) {
2686		if (!parent)
2687			return -EIO;
2688
2689		status = ice_sched_add_nodes_to_layer(pi, tc_node, parent, i,
2690						      num_nodes[i],
2691						      &first_node_teid,
2692						      &num_nodes_added);
2693		if (status || num_nodes[i] != num_nodes_added)
2694			return -EIO;
2695
2696		/* The newly added node can be a new parent for the next
2697		 * layer nodes
2698		 */
2699		if (num_nodes_added) {
2700			parent = ice_sched_find_node_by_teid(tc_node,
2701							     first_node_teid);
2702			/* register aggregator ID with the aggregator node */
2703			if (parent && i == aggl)
2704				parent->agg_id = agg_id;
2705		} else {
2706			parent = parent->children[0];
2707		}
2708	}
2709
2710	return 0;
2711}
2712
2713/**
2714 * ice_sched_cfg_agg - configure aggregator node
2715 * @pi: port information structure
2716 * @agg_id: aggregator ID
2717 * @agg_type: aggregator type queue, VSI, or aggregator group
2718 * @tc_bitmap: bits TC bitmap
2719 *
2720 * It registers a unique aggregator node into scheduler services. It
2721 * allows a user to register with a unique ID to track it's resources.
2722 * The aggregator type determines if this is a queue group, VSI group
2723 * or aggregator group. It then creates the aggregator node(s) for requested
2724 * TC(s) or removes an existing aggregator node including its configuration
2725 * if indicated via tc_bitmap. Call ice_rm_agg_cfg to release aggregator
2726 * resources and remove aggregator ID.
2727 * This function needs to be called with scheduler lock held.
2728 */
2729static int
2730ice_sched_cfg_agg(struct ice_port_info *pi, u32 agg_id,
2731		  enum ice_agg_type agg_type, unsigned long *tc_bitmap)
2732{
2733	struct ice_sched_agg_info *agg_info;
2734	struct ice_hw *hw = pi->hw;
2735	int status = 0;
2736	u8 tc;
2737
2738	agg_info = ice_get_agg_info(hw, agg_id);
2739	if (!agg_info) {
2740		/* Create new entry for new aggregator ID */
2741		agg_info = devm_kzalloc(ice_hw_to_dev(hw), sizeof(*agg_info),
2742					GFP_KERNEL);
2743		if (!agg_info)
2744			return -ENOMEM;
2745
2746		agg_info->agg_id = agg_id;
2747		agg_info->agg_type = agg_type;
2748		agg_info->tc_bitmap[0] = 0;
2749
2750		/* Initialize the aggregator VSI list head */
2751		INIT_LIST_HEAD(&agg_info->agg_vsi_list);
2752
2753		/* Add new entry in aggregator list */
2754		list_add(&agg_info->list_entry, &hw->agg_list);
2755	}
2756	/* Create aggregator node(s) for requested TC(s) */
2757	ice_for_each_traffic_class(tc) {
2758		if (!ice_is_tc_ena(*tc_bitmap, tc)) {
2759			/* Delete aggregator cfg TC if it exists previously */
2760			status = ice_rm_agg_cfg_tc(pi, agg_info, tc, false);
2761			if (status)
2762				break;
2763			continue;
2764		}
2765
2766		/* Check if aggregator node for TC already exists */
2767		if (ice_is_tc_ena(agg_info->tc_bitmap[0], tc))
2768			continue;
2769
2770		/* Create new aggregator node for TC */
2771		status = ice_sched_add_agg_cfg(pi, agg_id, tc);
2772		if (status)
2773			break;
2774
2775		/* Save aggregator node's TC information */
2776		set_bit(tc, agg_info->tc_bitmap);
2777	}
2778
2779	return status;
2780}
2781
2782/**
2783 * ice_cfg_agg - config aggregator node
2784 * @pi: port information structure
2785 * @agg_id: aggregator ID
2786 * @agg_type: aggregator type queue, VSI, or aggregator group
2787 * @tc_bitmap: bits TC bitmap
2788 *
2789 * This function configures aggregator node(s).
2790 */
2791int
2792ice_cfg_agg(struct ice_port_info *pi, u32 agg_id, enum ice_agg_type agg_type,
2793	    u8 tc_bitmap)
2794{
2795	unsigned long bitmap = tc_bitmap;
2796	int status;
2797
2798	mutex_lock(&pi->sched_lock);
2799	status = ice_sched_cfg_agg(pi, agg_id, agg_type, &bitmap);
2800	if (!status)
2801		status = ice_save_agg_tc_bitmap(pi, agg_id, &bitmap);
2802	mutex_unlock(&pi->sched_lock);
2803	return status;
2804}
2805
2806/**
2807 * ice_get_agg_vsi_info - get the aggregator ID
2808 * @agg_info: aggregator info
2809 * @vsi_handle: software VSI handle
2810 *
2811 * The function returns aggregator VSI info based on VSI handle. This function
2812 * needs to be called with scheduler lock held.
2813 */
2814static struct ice_sched_agg_vsi_info *
2815ice_get_agg_vsi_info(struct ice_sched_agg_info *agg_info, u16 vsi_handle)
2816{
2817	struct ice_sched_agg_vsi_info *agg_vsi_info;
2818
2819	list_for_each_entry(agg_vsi_info, &agg_info->agg_vsi_list, list_entry)
2820		if (agg_vsi_info->vsi_handle == vsi_handle)
2821			return agg_vsi_info;
2822
2823	return NULL;
2824}
2825
2826/**
2827 * ice_get_vsi_agg_info - get the aggregator info of VSI
2828 * @hw: pointer to the hardware structure
2829 * @vsi_handle: Sw VSI handle
2830 *
2831 * The function returns aggregator info of VSI represented via vsi_handle. The
2832 * VSI has in this case a different aggregator than the default one. This
2833 * function needs to be called with scheduler lock held.
2834 */
2835static struct ice_sched_agg_info *
2836ice_get_vsi_agg_info(struct ice_hw *hw, u16 vsi_handle)
2837{
2838	struct ice_sched_agg_info *agg_info;
2839
2840	list_for_each_entry(agg_info, &hw->agg_list, list_entry) {
2841		struct ice_sched_agg_vsi_info *agg_vsi_info;
2842
2843		agg_vsi_info = ice_get_agg_vsi_info(agg_info, vsi_handle);
2844		if (agg_vsi_info)
2845			return agg_info;
2846	}
2847	return NULL;
2848}
2849
2850/**
2851 * ice_save_agg_vsi_tc_bitmap - save aggregator VSI TC bitmap
2852 * @pi: port information structure
2853 * @agg_id: aggregator ID
2854 * @vsi_handle: software VSI handle
2855 * @tc_bitmap: TC bitmap of enabled TC(s)
2856 *
2857 * Save VSI to aggregator TC bitmap. This function needs to call with scheduler
2858 * lock held.
2859 */
2860static int
2861ice_save_agg_vsi_tc_bitmap(struct ice_port_info *pi, u32 agg_id, u16 vsi_handle,
2862			   unsigned long *tc_bitmap)
2863{
2864	struct ice_sched_agg_vsi_info *agg_vsi_info;
2865	struct ice_sched_agg_info *agg_info;
2866
2867	agg_info = ice_get_agg_info(pi->hw, agg_id);
2868	if (!agg_info)
2869		return -EINVAL;
2870	/* check if entry already exist */
2871	agg_vsi_info = ice_get_agg_vsi_info(agg_info, vsi_handle);
2872	if (!agg_vsi_info)
2873		return -EINVAL;
2874	bitmap_copy(agg_vsi_info->replay_tc_bitmap, tc_bitmap,
2875		    ICE_MAX_TRAFFIC_CLASS);
2876	return 0;
2877}
2878
2879/**
2880 * ice_sched_assoc_vsi_to_agg - associate/move VSI to new/default aggregator
2881 * @pi: port information structure
2882 * @agg_id: aggregator ID
2883 * @vsi_handle: software VSI handle
2884 * @tc_bitmap: TC bitmap of enabled TC(s)
2885 *
2886 * This function moves VSI to a new or default aggregator node. If VSI is
2887 * already associated to the aggregator node then no operation is performed on
2888 * the tree. This function needs to be called with scheduler lock held.
2889 */
2890static int
2891ice_sched_assoc_vsi_to_agg(struct ice_port_info *pi, u32 agg_id,
2892			   u16 vsi_handle, unsigned long *tc_bitmap)
2893{
2894	struct ice_sched_agg_vsi_info *agg_vsi_info, *iter, *old_agg_vsi_info = NULL;
2895	struct ice_sched_agg_info *agg_info, *old_agg_info;
2896	struct ice_hw *hw = pi->hw;
2897	int status = 0;
2898	u8 tc;
2899
2900	if (!ice_is_vsi_valid(pi->hw, vsi_handle))
2901		return -EINVAL;
2902	agg_info = ice_get_agg_info(hw, agg_id);
2903	if (!agg_info)
2904		return -EINVAL;
2905	/* If the VSI is already part of another aggregator then update
2906	 * its VSI info list
2907	 */
2908	old_agg_info = ice_get_vsi_agg_info(hw, vsi_handle);
2909	if (old_agg_info && old_agg_info != agg_info) {
2910		struct ice_sched_agg_vsi_info *vtmp;
2911
2912		list_for_each_entry_safe(iter, vtmp,
2913					 &old_agg_info->agg_vsi_list,
2914					 list_entry)
2915			if (iter->vsi_handle == vsi_handle) {
2916				old_agg_vsi_info = iter;
2917				break;
2918			}
2919	}
2920
2921	/* check if entry already exist */
2922	agg_vsi_info = ice_get_agg_vsi_info(agg_info, vsi_handle);
2923	if (!agg_vsi_info) {
2924		/* Create new entry for VSI under aggregator list */
2925		agg_vsi_info = devm_kzalloc(ice_hw_to_dev(hw),
2926					    sizeof(*agg_vsi_info), GFP_KERNEL);
2927		if (!agg_vsi_info)
2928			return -EINVAL;
2929
2930		/* add VSI ID into the aggregator list */
2931		agg_vsi_info->vsi_handle = vsi_handle;
2932		list_add(&agg_vsi_info->list_entry, &agg_info->agg_vsi_list);
2933	}
2934	/* Move VSI node to new aggregator node for requested TC(s) */
2935	ice_for_each_traffic_class(tc) {
2936		if (!ice_is_tc_ena(*tc_bitmap, tc))
2937			continue;
2938
2939		/* Move VSI to new aggregator */
2940		status = ice_sched_move_vsi_to_agg(pi, vsi_handle, agg_id, tc);
2941		if (status)
2942			break;
2943
2944		set_bit(tc, agg_vsi_info->tc_bitmap);
2945		if (old_agg_vsi_info)
2946			clear_bit(tc, old_agg_vsi_info->tc_bitmap);
2947	}
2948	if (old_agg_vsi_info && !old_agg_vsi_info->tc_bitmap[0]) {
2949		list_del(&old_agg_vsi_info->list_entry);
2950		devm_kfree(ice_hw_to_dev(pi->hw), old_agg_vsi_info);
2951	}
2952	return status;
2953}
2954
2955/**
2956 * ice_sched_rm_unused_rl_prof - remove unused RL profile
2957 * @pi: port information structure
2958 *
2959 * This function removes unused rate limit profiles from the HW and
2960 * SW DB. The caller needs to hold scheduler lock.
2961 */
2962static void ice_sched_rm_unused_rl_prof(struct ice_port_info *pi)
2963{
2964	u16 ln;
2965
2966	for (ln = 0; ln < pi->hw->num_tx_sched_layers; ln++) {
2967		struct ice_aqc_rl_profile_info *rl_prof_elem;
2968		struct ice_aqc_rl_profile_info *rl_prof_tmp;
2969
2970		list_for_each_entry_safe(rl_prof_elem, rl_prof_tmp,
2971					 &pi->rl_prof_list[ln], list_entry) {
2972			if (!ice_sched_del_rl_profile(pi->hw, rl_prof_elem))
2973				ice_debug(pi->hw, ICE_DBG_SCHED, "Removed rl profile\n");
2974		}
2975	}
2976}
2977
2978/**
2979 * ice_sched_update_elem - update element
2980 * @hw: pointer to the HW struct
2981 * @node: pointer to node
2982 * @info: node info to update
2983 *
2984 * Update the HW DB, and local SW DB of node. Update the scheduling
2985 * parameters of node from argument info data buffer (Info->data buf) and
2986 * returns success or error on config sched element failure. The caller
2987 * needs to hold scheduler lock.
2988 */
2989static int
2990ice_sched_update_elem(struct ice_hw *hw, struct ice_sched_node *node,
2991		      struct ice_aqc_txsched_elem_data *info)
2992{
2993	struct ice_aqc_txsched_elem_data buf;
2994	u16 elem_cfgd = 0;
2995	u16 num_elems = 1;
2996	int status;
2997
2998	buf = *info;
2999	/* Parent TEID is reserved field in this aq call */
3000	buf.parent_teid = 0;
3001	/* Element type is reserved field in this aq call */
3002	buf.data.elem_type = 0;
3003	/* Flags is reserved field in this aq call */
3004	buf.data.flags = 0;
3005
3006	/* Update HW DB */
3007	/* Configure element node */
3008	status = ice_aq_cfg_sched_elems(hw, num_elems, &buf, sizeof(buf),
3009					&elem_cfgd, NULL);
3010	if (status || elem_cfgd != num_elems) {
3011		ice_debug(hw, ICE_DBG_SCHED, "Config sched elem error\n");
3012		return -EIO;
3013	}
3014
3015	/* Config success case */
3016	/* Now update local SW DB */
3017	/* Only copy the data portion of info buffer */
3018	node->info.data = info->data;
3019	return status;
3020}
3021
3022/**
3023 * ice_sched_cfg_node_bw_alloc - configure node BW weight/alloc params
3024 * @hw: pointer to the HW struct
3025 * @node: sched node to configure
3026 * @rl_type: rate limit type CIR, EIR, or shared
3027 * @bw_alloc: BW weight/allocation
3028 *
3029 * This function configures node element's BW allocation.
3030 */
3031static int
3032ice_sched_cfg_node_bw_alloc(struct ice_hw *hw, struct ice_sched_node *node,
3033			    enum ice_rl_type rl_type, u16 bw_alloc)
3034{
3035	struct ice_aqc_txsched_elem_data buf;
3036	struct ice_aqc_txsched_elem *data;
3037
3038	buf = node->info;
3039	data = &buf.data;
3040	if (rl_type == ICE_MIN_BW) {
3041		data->valid_sections |= ICE_AQC_ELEM_VALID_CIR;
3042		data->cir_bw.bw_alloc = cpu_to_le16(bw_alloc);
3043	} else if (rl_type == ICE_MAX_BW) {
3044		data->valid_sections |= ICE_AQC_ELEM_VALID_EIR;
3045		data->eir_bw.bw_alloc = cpu_to_le16(bw_alloc);
3046	} else {
3047		return -EINVAL;
3048	}
3049
3050	/* Configure element */
3051	return ice_sched_update_elem(hw, node, &buf);
3052}
3053
3054/**
3055 * ice_move_vsi_to_agg - moves VSI to new or default aggregator
3056 * @pi: port information structure
3057 * @agg_id: aggregator ID
3058 * @vsi_handle: software VSI handle
3059 * @tc_bitmap: TC bitmap of enabled TC(s)
3060 *
3061 * Move or associate VSI to a new or default aggregator node.
3062 */
3063int
3064ice_move_vsi_to_agg(struct ice_port_info *pi, u32 agg_id, u16 vsi_handle,
3065		    u8 tc_bitmap)
3066{
3067	unsigned long bitmap = tc_bitmap;
3068	int status;
3069
3070	mutex_lock(&pi->sched_lock);
3071	status = ice_sched_assoc_vsi_to_agg(pi, agg_id, vsi_handle,
3072					    (unsigned long *)&bitmap);
3073	if (!status)
3074		status = ice_save_agg_vsi_tc_bitmap(pi, agg_id, vsi_handle,
3075						    (unsigned long *)&bitmap);
3076	mutex_unlock(&pi->sched_lock);
3077	return status;
3078}
3079
3080/**
3081 * ice_set_clear_cir_bw - set or clear CIR BW
3082 * @bw_t_info: bandwidth type information structure
3083 * @bw: bandwidth in Kbps - Kilo bits per sec
3084 *
3085 * Save or clear CIR bandwidth (BW) in the passed param bw_t_info.
3086 */
3087static void ice_set_clear_cir_bw(struct ice_bw_type_info *bw_t_info, u32 bw)
3088{
3089	if (bw == ICE_SCHED_DFLT_BW) {
3090		clear_bit(ICE_BW_TYPE_CIR, bw_t_info->bw_t_bitmap);
3091		bw_t_info->cir_bw.bw = 0;
3092	} else {
3093		/* Save type of BW information */
3094		set_bit(ICE_BW_TYPE_CIR, bw_t_info->bw_t_bitmap);
3095		bw_t_info->cir_bw.bw = bw;
3096	}
3097}
3098
3099/**
3100 * ice_set_clear_eir_bw - set or clear EIR BW
3101 * @bw_t_info: bandwidth type information structure
3102 * @bw: bandwidth in Kbps - Kilo bits per sec
3103 *
3104 * Save or clear EIR bandwidth (BW) in the passed param bw_t_info.
3105 */
3106static void ice_set_clear_eir_bw(struct ice_bw_type_info *bw_t_info, u32 bw)
3107{
3108	if (bw == ICE_SCHED_DFLT_BW) {
3109		clear_bit(ICE_BW_TYPE_EIR, bw_t_info->bw_t_bitmap);
3110		bw_t_info->eir_bw.bw = 0;
3111	} else {
3112		/* EIR BW and Shared BW profiles are mutually exclusive and
3113		 * hence only one of them may be set for any given element.
3114		 * First clear earlier saved shared BW information.
3115		 */
3116		clear_bit(ICE_BW_TYPE_SHARED, bw_t_info->bw_t_bitmap);
3117		bw_t_info->shared_bw = 0;
3118		/* save EIR BW information */
3119		set_bit(ICE_BW_TYPE_EIR, bw_t_info->bw_t_bitmap);
3120		bw_t_info->eir_bw.bw = bw;
3121	}
3122}
3123
3124/**
3125 * ice_set_clear_shared_bw - set or clear shared BW
3126 * @bw_t_info: bandwidth type information structure
3127 * @bw: bandwidth in Kbps - Kilo bits per sec
3128 *
3129 * Save or clear shared bandwidth (BW) in the passed param bw_t_info.
3130 */
3131static void ice_set_clear_shared_bw(struct ice_bw_type_info *bw_t_info, u32 bw)
3132{
3133	if (bw == ICE_SCHED_DFLT_BW) {
3134		clear_bit(ICE_BW_TYPE_SHARED, bw_t_info->bw_t_bitmap);
3135		bw_t_info->shared_bw = 0;
3136	} else {
3137		/* EIR BW and Shared BW profiles are mutually exclusive and
3138		 * hence only one of them may be set for any given element.
3139		 * First clear earlier saved EIR BW information.
3140		 */
3141		clear_bit(ICE_BW_TYPE_EIR, bw_t_info->bw_t_bitmap);
3142		bw_t_info->eir_bw.bw = 0;
3143		/* save shared BW information */
3144		set_bit(ICE_BW_TYPE_SHARED, bw_t_info->bw_t_bitmap);
3145		bw_t_info->shared_bw = bw;
3146	}
3147}
3148
3149/**
3150 * ice_sched_save_vsi_bw - save VSI node's BW information
3151 * @pi: port information structure
3152 * @vsi_handle: sw VSI handle
3153 * @tc: traffic class
3154 * @rl_type: rate limit type min, max, or shared
3155 * @bw: bandwidth in Kbps - Kilo bits per sec
3156 *
3157 * Save BW information of VSI type node for post replay use.
3158 */
3159static int
3160ice_sched_save_vsi_bw(struct ice_port_info *pi, u16 vsi_handle, u8 tc,
3161		      enum ice_rl_type rl_type, u32 bw)
3162{
3163	struct ice_vsi_ctx *vsi_ctx;
3164
3165	if (!ice_is_vsi_valid(pi->hw, vsi_handle))
3166		return -EINVAL;
3167	vsi_ctx = ice_get_vsi_ctx(pi->hw, vsi_handle);
3168	if (!vsi_ctx)
3169		return -EINVAL;
3170	switch (rl_type) {
3171	case ICE_MIN_BW:
3172		ice_set_clear_cir_bw(&vsi_ctx->sched.bw_t_info[tc], bw);
3173		break;
3174	case ICE_MAX_BW:
3175		ice_set_clear_eir_bw(&vsi_ctx->sched.bw_t_info[tc], bw);
3176		break;
3177	case ICE_SHARED_BW:
3178		ice_set_clear_shared_bw(&vsi_ctx->sched.bw_t_info[tc], bw);
3179		break;
3180	default:
3181		return -EINVAL;
3182	}
3183	return 0;
3184}
3185
3186/**
3187 * ice_sched_calc_wakeup - calculate RL profile wakeup parameter
3188 * @hw: pointer to the HW struct
3189 * @bw: bandwidth in Kbps
3190 *
3191 * This function calculates the wakeup parameter of RL profile.
3192 */
3193static u16 ice_sched_calc_wakeup(struct ice_hw *hw, s32 bw)
3194{
3195	s64 bytes_per_sec, wakeup_int, wakeup_a, wakeup_b, wakeup_f;
3196	s32 wakeup_f_int;
3197	u16 wakeup = 0;
3198
3199	/* Get the wakeup integer value */
3200	bytes_per_sec = div64_long(((s64)bw * 1000), BITS_PER_BYTE);
3201	wakeup_int = div64_long(hw->psm_clk_freq, bytes_per_sec);
3202	if (wakeup_int > 63) {
3203		wakeup = (u16)((1 << 15) | wakeup_int);
3204	} else {
3205		/* Calculate fraction value up to 4 decimals
3206		 * Convert Integer value to a constant multiplier
3207		 */
3208		wakeup_b = (s64)ICE_RL_PROF_MULTIPLIER * wakeup_int;
3209		wakeup_a = div64_long((s64)ICE_RL_PROF_MULTIPLIER *
3210					   hw->psm_clk_freq, bytes_per_sec);
3211
3212		/* Get Fraction value */
3213		wakeup_f = wakeup_a - wakeup_b;
3214
3215		/* Round up the Fractional value via Ceil(Fractional value) */
3216		if (wakeup_f > div64_long(ICE_RL_PROF_MULTIPLIER, 2))
3217			wakeup_f += 1;
3218
3219		wakeup_f_int = (s32)div64_long(wakeup_f * ICE_RL_PROF_FRACTION,
3220					       ICE_RL_PROF_MULTIPLIER);
3221		wakeup |= (u16)(wakeup_int << 9);
3222		wakeup |= (u16)(0x1ff & wakeup_f_int);
3223	}
3224
3225	return wakeup;
3226}
3227
3228/**
3229 * ice_sched_bw_to_rl_profile - convert BW to profile parameters
3230 * @hw: pointer to the HW struct
3231 * @bw: bandwidth in Kbps
3232 * @profile: profile parameters to return
3233 *
3234 * This function converts the BW to profile structure format.
3235 */
3236static int
3237ice_sched_bw_to_rl_profile(struct ice_hw *hw, u32 bw,
3238			   struct ice_aqc_rl_profile_elem *profile)
3239{
3240	s64 bytes_per_sec, ts_rate, mv_tmp;
3241	int status = -EINVAL;
3242	bool found = false;
3243	s32 encode = 0;
3244	s64 mv = 0;
3245	s32 i;
3246
3247	/* Bw settings range is from 0.5Mb/sec to 100Gb/sec */
3248	if (bw < ICE_SCHED_MIN_BW || bw > ICE_SCHED_MAX_BW)
3249		return status;
3250
3251	/* Bytes per second from Kbps */
3252	bytes_per_sec = div64_long(((s64)bw * 1000), BITS_PER_BYTE);
3253
3254	/* encode is 6 bits but really useful are 5 bits */
3255	for (i = 0; i < 64; i++) {
3256		u64 pow_result = BIT_ULL(i);
3257
3258		ts_rate = div64_long((s64)hw->psm_clk_freq,
3259				     pow_result * ICE_RL_PROF_TS_MULTIPLIER);
3260		if (ts_rate <= 0)
3261			continue;
3262
3263		/* Multiplier value */
3264		mv_tmp = div64_long(bytes_per_sec * ICE_RL_PROF_MULTIPLIER,
3265				    ts_rate);
3266
3267		/* Round to the nearest ICE_RL_PROF_MULTIPLIER */
3268		mv = round_up_64bit(mv_tmp, ICE_RL_PROF_MULTIPLIER);
3269
3270		/* First multiplier value greater than the given
3271		 * accuracy bytes
3272		 */
3273		if (mv > ICE_RL_PROF_ACCURACY_BYTES) {
3274			encode = i;
3275			found = true;
3276			break;
3277		}
3278	}
3279	if (found) {
3280		u16 wm;
3281
3282		wm = ice_sched_calc_wakeup(hw, bw);
3283		profile->rl_multiply = cpu_to_le16(mv);
3284		profile->wake_up_calc = cpu_to_le16(wm);
3285		profile->rl_encode = cpu_to_le16(encode);
3286		status = 0;
3287	} else {
3288		status = -ENOENT;
3289	}
3290
3291	return status;
3292}
3293
3294/**
3295 * ice_sched_add_rl_profile - add RL profile
3296 * @pi: port information structure
3297 * @rl_type: type of rate limit BW - min, max, or shared
3298 * @bw: bandwidth in Kbps - Kilo bits per sec
3299 * @layer_num: specifies in which layer to create profile
3300 *
3301 * This function first checks the existing list for corresponding BW
3302 * parameter. If it exists, it returns the associated profile otherwise
3303 * it creates a new rate limit profile for requested BW, and adds it to
3304 * the HW DB and local list. It returns the new profile or null on error.
3305 * The caller needs to hold the scheduler lock.
3306 */
3307static struct ice_aqc_rl_profile_info *
3308ice_sched_add_rl_profile(struct ice_port_info *pi,
3309			 enum ice_rl_type rl_type, u32 bw, u8 layer_num)
3310{
3311	struct ice_aqc_rl_profile_info *rl_prof_elem;
3312	u16 profiles_added = 0, num_profiles = 1;
3313	struct ice_aqc_rl_profile_elem *buf;
3314	struct ice_hw *hw;
3315	u8 profile_type;
3316	int status;
3317
3318	if (!pi || layer_num >= pi->hw->num_tx_sched_layers)
3319		return NULL;
3320	switch (rl_type) {
3321	case ICE_MIN_BW:
3322		profile_type = ICE_AQC_RL_PROFILE_TYPE_CIR;
3323		break;
3324	case ICE_MAX_BW:
3325		profile_type = ICE_AQC_RL_PROFILE_TYPE_EIR;
3326		break;
3327	case ICE_SHARED_BW:
3328		profile_type = ICE_AQC_RL_PROFILE_TYPE_SRL;
3329		break;
3330	default:
3331		return NULL;
3332	}
3333
3334	hw = pi->hw;
3335	list_for_each_entry(rl_prof_elem, &pi->rl_prof_list[layer_num],
3336			    list_entry)
3337		if ((rl_prof_elem->profile.flags & ICE_AQC_RL_PROFILE_TYPE_M) ==
3338		    profile_type && rl_prof_elem->bw == bw)
3339			/* Return existing profile ID info */
3340			return rl_prof_elem;
3341
3342	/* Create new profile ID */
3343	rl_prof_elem = devm_kzalloc(ice_hw_to_dev(hw), sizeof(*rl_prof_elem),
3344				    GFP_KERNEL);
3345
3346	if (!rl_prof_elem)
3347		return NULL;
3348
3349	status = ice_sched_bw_to_rl_profile(hw, bw, &rl_prof_elem->profile);
3350	if (status)
3351		goto exit_add_rl_prof;
3352
3353	rl_prof_elem->bw = bw;
3354	/* layer_num is zero relative, and fw expects level from 1 to 9 */
3355	rl_prof_elem->profile.level = layer_num + 1;
3356	rl_prof_elem->profile.flags = profile_type;
3357	rl_prof_elem->profile.max_burst_size = cpu_to_le16(hw->max_burst_size);
3358
3359	/* Create new entry in HW DB */
3360	buf = &rl_prof_elem->profile;
3361	status = ice_aq_add_rl_profile(hw, num_profiles, buf, sizeof(*buf),
3362				       &profiles_added, NULL);
3363	if (status || profiles_added != num_profiles)
3364		goto exit_add_rl_prof;
3365
3366	/* Good entry - add in the list */
3367	rl_prof_elem->prof_id_ref = 0;
3368	list_add(&rl_prof_elem->list_entry, &pi->rl_prof_list[layer_num]);
3369	return rl_prof_elem;
3370
3371exit_add_rl_prof:
3372	devm_kfree(ice_hw_to_dev(hw), rl_prof_elem);
3373	return NULL;
3374}
3375
3376/**
3377 * ice_sched_cfg_node_bw_lmt - configure node sched params
3378 * @hw: pointer to the HW struct
3379 * @node: sched node to configure
3380 * @rl_type: rate limit type CIR, EIR, or shared
3381 * @rl_prof_id: rate limit profile ID
3382 *
3383 * This function configures node element's BW limit.
3384 */
3385static int
3386ice_sched_cfg_node_bw_lmt(struct ice_hw *hw, struct ice_sched_node *node,
3387			  enum ice_rl_type rl_type, u16 rl_prof_id)
3388{
3389	struct ice_aqc_txsched_elem_data buf;
3390	struct ice_aqc_txsched_elem *data;
3391
3392	buf = node->info;
3393	data = &buf.data;
3394	switch (rl_type) {
3395	case ICE_MIN_BW:
3396		data->valid_sections |= ICE_AQC_ELEM_VALID_CIR;
3397		data->cir_bw.bw_profile_idx = cpu_to_le16(rl_prof_id);
3398		break;
3399	case ICE_MAX_BW:
3400		/* EIR BW and Shared BW profiles are mutually exclusive and
3401		 * hence only one of them may be set for any given element
3402		 */
3403		if (data->valid_sections & ICE_AQC_ELEM_VALID_SHARED)
3404			return -EIO;
3405		data->valid_sections |= ICE_AQC_ELEM_VALID_EIR;
3406		data->eir_bw.bw_profile_idx = cpu_to_le16(rl_prof_id);
3407		break;
3408	case ICE_SHARED_BW:
3409		/* Check for removing shared BW */
3410		if (rl_prof_id == ICE_SCHED_NO_SHARED_RL_PROF_ID) {
3411			/* remove shared profile */
3412			data->valid_sections &= ~ICE_AQC_ELEM_VALID_SHARED;
3413			data->srl_id = 0; /* clear SRL field */
3414
3415			/* enable back EIR to default profile */
3416			data->valid_sections |= ICE_AQC_ELEM_VALID_EIR;
3417			data->eir_bw.bw_profile_idx =
3418				cpu_to_le16(ICE_SCHED_DFLT_RL_PROF_ID);
3419			break;
3420		}
3421		/* EIR BW and Shared BW profiles are mutually exclusive and
3422		 * hence only one of them may be set for any given element
3423		 */
3424		if ((data->valid_sections & ICE_AQC_ELEM_VALID_EIR) &&
3425		    (le16_to_cpu(data->eir_bw.bw_profile_idx) !=
3426			    ICE_SCHED_DFLT_RL_PROF_ID))
3427			return -EIO;
3428		/* EIR BW is set to default, disable it */
3429		data->valid_sections &= ~ICE_AQC_ELEM_VALID_EIR;
3430		/* Okay to enable shared BW now */
3431		data->valid_sections |= ICE_AQC_ELEM_VALID_SHARED;
3432		data->srl_id = cpu_to_le16(rl_prof_id);
3433		break;
3434	default:
3435		/* Unknown rate limit type */
3436		return -EINVAL;
3437	}
3438
3439	/* Configure element */
3440	return ice_sched_update_elem(hw, node, &buf);
3441}
3442
3443/**
3444 * ice_sched_get_node_rl_prof_id - get node's rate limit profile ID
3445 * @node: sched node
3446 * @rl_type: rate limit type
3447 *
3448 * If existing profile matches, it returns the corresponding rate
3449 * limit profile ID, otherwise it returns an invalid ID as error.
3450 */
3451static u16
3452ice_sched_get_node_rl_prof_id(struct ice_sched_node *node,
3453			      enum ice_rl_type rl_type)
3454{
3455	u16 rl_prof_id = ICE_SCHED_INVAL_PROF_ID;
3456	struct ice_aqc_txsched_elem *data;
3457
3458	data = &node->info.data;
3459	switch (rl_type) {
3460	case ICE_MIN_BW:
3461		if (data->valid_sections & ICE_AQC_ELEM_VALID_CIR)
3462			rl_prof_id = le16_to_cpu(data->cir_bw.bw_profile_idx);
3463		break;
3464	case ICE_MAX_BW:
3465		if (data->valid_sections & ICE_AQC_ELEM_VALID_EIR)
3466			rl_prof_id = le16_to_cpu(data->eir_bw.bw_profile_idx);
3467		break;
3468	case ICE_SHARED_BW:
3469		if (data->valid_sections & ICE_AQC_ELEM_VALID_SHARED)
3470			rl_prof_id = le16_to_cpu(data->srl_id);
3471		break;
3472	default:
3473		break;
3474	}
3475
3476	return rl_prof_id;
3477}
3478
3479/**
3480 * ice_sched_get_rl_prof_layer - selects rate limit profile creation layer
3481 * @pi: port information structure
3482 * @rl_type: type of rate limit BW - min, max, or shared
3483 * @layer_index: layer index
3484 *
3485 * This function returns requested profile creation layer.
3486 */
3487static u8
3488ice_sched_get_rl_prof_layer(struct ice_port_info *pi, enum ice_rl_type rl_type,
3489			    u8 layer_index)
3490{
3491	struct ice_hw *hw = pi->hw;
3492
3493	if (layer_index >= hw->num_tx_sched_layers)
3494		return ICE_SCHED_INVAL_LAYER_NUM;
3495	switch (rl_type) {
3496	case ICE_MIN_BW:
3497		if (hw->layer_info[layer_index].max_cir_rl_profiles)
3498			return layer_index;
3499		break;
3500	case ICE_MAX_BW:
3501		if (hw->layer_info[layer_index].max_eir_rl_profiles)
3502			return layer_index;
3503		break;
3504	case ICE_SHARED_BW:
3505		/* if current layer doesn't support SRL profile creation
3506		 * then try a layer up or down.
3507		 */
3508		if (hw->layer_info[layer_index].max_srl_profiles)
3509			return layer_index;
3510		else if (layer_index < hw->num_tx_sched_layers - 1 &&
3511			 hw->layer_info[layer_index + 1].max_srl_profiles)
3512			return layer_index + 1;
3513		else if (layer_index > 0 &&
3514			 hw->layer_info[layer_index - 1].max_srl_profiles)
3515			return layer_index - 1;
3516		break;
3517	default:
3518		break;
3519	}
3520	return ICE_SCHED_INVAL_LAYER_NUM;
3521}
3522
3523/**
3524 * ice_sched_get_srl_node - get shared rate limit node
3525 * @node: tree node
3526 * @srl_layer: shared rate limit layer
3527 *
3528 * This function returns SRL node to be used for shared rate limit purpose.
3529 * The caller needs to hold scheduler lock.
3530 */
3531static struct ice_sched_node *
3532ice_sched_get_srl_node(struct ice_sched_node *node, u8 srl_layer)
3533{
3534	if (srl_layer > node->tx_sched_layer)
3535		return node->children[0];
3536	else if (srl_layer < node->tx_sched_layer)
3537		/* Node can't be created without a parent. It will always
3538		 * have a valid parent except root node.
3539		 */
3540		return node->parent;
3541	else
3542		return node;
3543}
3544
3545/**
3546 * ice_sched_rm_rl_profile - remove RL profile ID
3547 * @pi: port information structure
3548 * @layer_num: layer number where profiles are saved
3549 * @profile_type: profile type like EIR, CIR, or SRL
3550 * @profile_id: profile ID to remove
3551 *
3552 * This function removes rate limit profile from layer 'layer_num' of type
3553 * 'profile_type' and profile ID as 'profile_id'. The caller needs to hold
3554 * scheduler lock.
3555 */
3556static int
3557ice_sched_rm_rl_profile(struct ice_port_info *pi, u8 layer_num, u8 profile_type,
3558			u16 profile_id)
3559{
3560	struct ice_aqc_rl_profile_info *rl_prof_elem;
3561	int status = 0;
3562
3563	if (layer_num >= pi->hw->num_tx_sched_layers)
3564		return -EINVAL;
3565	/* Check the existing list for RL profile */
3566	list_for_each_entry(rl_prof_elem, &pi->rl_prof_list[layer_num],
3567			    list_entry)
3568		if ((rl_prof_elem->profile.flags & ICE_AQC_RL_PROFILE_TYPE_M) ==
3569		    profile_type &&
3570		    le16_to_cpu(rl_prof_elem->profile.profile_id) ==
3571		    profile_id) {
3572			if (rl_prof_elem->prof_id_ref)
3573				rl_prof_elem->prof_id_ref--;
3574
3575			/* Remove old profile ID from database */
3576			status = ice_sched_del_rl_profile(pi->hw, rl_prof_elem);
3577			if (status && status != -EBUSY)
3578				ice_debug(pi->hw, ICE_DBG_SCHED, "Remove rl profile failed\n");
3579			break;
3580		}
3581	if (status == -EBUSY)
3582		status = 0;
3583	return status;
3584}
3585
3586/**
3587 * ice_sched_set_node_bw_dflt - set node's bandwidth limit to default
3588 * @pi: port information structure
3589 * @node: pointer to node structure
3590 * @rl_type: rate limit type min, max, or shared
3591 * @layer_num: layer number where RL profiles are saved
3592 *
3593 * This function configures node element's BW rate limit profile ID of
3594 * type CIR, EIR, or SRL to default. This function needs to be called
3595 * with the scheduler lock held.
3596 */
3597static int
3598ice_sched_set_node_bw_dflt(struct ice_port_info *pi,
3599			   struct ice_sched_node *node,
3600			   enum ice_rl_type rl_type, u8 layer_num)
3601{
3602	struct ice_hw *hw;
3603	u8 profile_type;
3604	u16 rl_prof_id;
3605	u16 old_id;
3606	int status;
3607
3608	hw = pi->hw;
3609	switch (rl_type) {
3610	case ICE_MIN_BW:
3611		profile_type = ICE_AQC_RL_PROFILE_TYPE_CIR;
3612		rl_prof_id = ICE_SCHED_DFLT_RL_PROF_ID;
3613		break;
3614	case ICE_MAX_BW:
3615		profile_type = ICE_AQC_RL_PROFILE_TYPE_EIR;
3616		rl_prof_id = ICE_SCHED_DFLT_RL_PROF_ID;
3617		break;
3618	case ICE_SHARED_BW:
3619		profile_type = ICE_AQC_RL_PROFILE_TYPE_SRL;
3620		/* No SRL is configured for default case */
3621		rl_prof_id = ICE_SCHED_NO_SHARED_RL_PROF_ID;
3622		break;
3623	default:
3624		return -EINVAL;
3625	}
3626	/* Save existing RL prof ID for later clean up */
3627	old_id = ice_sched_get_node_rl_prof_id(node, rl_type);
3628	/* Configure BW scheduling parameters */
3629	status = ice_sched_cfg_node_bw_lmt(hw, node, rl_type, rl_prof_id);
3630	if (status)
3631		return status;
3632
3633	/* Remove stale RL profile ID */
3634	if (old_id == ICE_SCHED_DFLT_RL_PROF_ID ||
3635	    old_id == ICE_SCHED_INVAL_PROF_ID)
3636		return 0;
3637
3638	return ice_sched_rm_rl_profile(pi, layer_num, profile_type, old_id);
3639}
3640
3641/**
3642 * ice_sched_set_eir_srl_excl - set EIR/SRL exclusiveness
3643 * @pi: port information structure
3644 * @node: pointer to node structure
3645 * @layer_num: layer number where rate limit profiles are saved
3646 * @rl_type: rate limit type min, max, or shared
3647 * @bw: bandwidth value
3648 *
3649 * This function prepares node element's bandwidth to SRL or EIR exclusively.
3650 * EIR BW and Shared BW profiles are mutually exclusive and hence only one of
3651 * them may be set for any given element. This function needs to be called
3652 * with the scheduler lock held.
3653 */
3654static int
3655ice_sched_set_eir_srl_excl(struct ice_port_info *pi,
3656			   struct ice_sched_node *node,
3657			   u8 layer_num, enum ice_rl_type rl_type, u32 bw)
3658{
3659	if (rl_type == ICE_SHARED_BW) {
3660		/* SRL node passed in this case, it may be different node */
3661		if (bw == ICE_SCHED_DFLT_BW)
3662			/* SRL being removed, ice_sched_cfg_node_bw_lmt()
3663			 * enables EIR to default. EIR is not set in this
3664			 * case, so no additional action is required.
3665			 */
3666			return 0;
3667
3668		/* SRL being configured, set EIR to default here.
3669		 * ice_sched_cfg_node_bw_lmt() disables EIR when it
3670		 * configures SRL
3671		 */
3672		return ice_sched_set_node_bw_dflt(pi, node, ICE_MAX_BW,
3673						  layer_num);
3674	} else if (rl_type == ICE_MAX_BW &&
3675		   node->info.data.valid_sections & ICE_AQC_ELEM_VALID_SHARED) {
3676		/* Remove Shared profile. Set default shared BW call
3677		 * removes shared profile for a node.
3678		 */
3679		return ice_sched_set_node_bw_dflt(pi, node,
3680						  ICE_SHARED_BW,
3681						  layer_num);
3682	}
3683	return 0;
3684}
3685
3686/**
3687 * ice_sched_set_node_bw - set node's bandwidth
3688 * @pi: port information structure
3689 * @node: tree node
3690 * @rl_type: rate limit type min, max, or shared
3691 * @bw: bandwidth in Kbps - Kilo bits per sec
3692 * @layer_num: layer number
3693 *
3694 * This function adds new profile corresponding to requested BW, configures
3695 * node's RL profile ID of type CIR, EIR, or SRL, and removes old profile
3696 * ID from local database. The caller needs to hold scheduler lock.
3697 */
3698int
3699ice_sched_set_node_bw(struct ice_port_info *pi, struct ice_sched_node *node,
3700		      enum ice_rl_type rl_type, u32 bw, u8 layer_num)
3701{
3702	struct ice_aqc_rl_profile_info *rl_prof_info;
3703	struct ice_hw *hw = pi->hw;
3704	u16 old_id, rl_prof_id;
3705	int status = -EINVAL;
3706
3707	rl_prof_info = ice_sched_add_rl_profile(pi, rl_type, bw, layer_num);
3708	if (!rl_prof_info)
3709		return status;
3710
3711	rl_prof_id = le16_to_cpu(rl_prof_info->profile.profile_id);
3712
3713	/* Save existing RL prof ID for later clean up */
3714	old_id = ice_sched_get_node_rl_prof_id(node, rl_type);
3715	/* Configure BW scheduling parameters */
3716	status = ice_sched_cfg_node_bw_lmt(hw, node, rl_type, rl_prof_id);
3717	if (status)
3718		return status;
3719
3720	/* New changes has been applied */
3721	/* Increment the profile ID reference count */
3722	rl_prof_info->prof_id_ref++;
3723
3724	/* Check for old ID removal */
3725	if ((old_id == ICE_SCHED_DFLT_RL_PROF_ID && rl_type != ICE_SHARED_BW) ||
3726	    old_id == ICE_SCHED_INVAL_PROF_ID || old_id == rl_prof_id)
3727		return 0;
3728
3729	return ice_sched_rm_rl_profile(pi, layer_num,
3730				       rl_prof_info->profile.flags &
3731				       ICE_AQC_RL_PROFILE_TYPE_M, old_id);
3732}
3733
3734/**
3735 * ice_sched_set_node_priority - set node's priority
3736 * @pi: port information structure
3737 * @node: tree node
3738 * @priority: number 0-7 representing priority among siblings
3739 *
3740 * This function sets priority of a node among it's siblings.
3741 */
3742int
3743ice_sched_set_node_priority(struct ice_port_info *pi, struct ice_sched_node *node,
3744			    u16 priority)
3745{
3746	struct ice_aqc_txsched_elem_data buf;
3747	struct ice_aqc_txsched_elem *data;
3748
3749	buf = node->info;
3750	data = &buf.data;
3751
3752	data->valid_sections |= ICE_AQC_ELEM_VALID_GENERIC;
3753	data->generic |= FIELD_PREP(ICE_AQC_ELEM_GENERIC_PRIO_M, priority);
3754
3755	return ice_sched_update_elem(pi->hw, node, &buf);
3756}
3757
3758/**
3759 * ice_sched_set_node_weight - set node's weight
3760 * @pi: port information structure
3761 * @node: tree node
3762 * @weight: number 1-200 representing weight for WFQ
3763 *
3764 * This function sets weight of the node for WFQ algorithm.
3765 */
3766int
3767ice_sched_set_node_weight(struct ice_port_info *pi, struct ice_sched_node *node, u16 weight)
3768{
3769	struct ice_aqc_txsched_elem_data buf;
3770	struct ice_aqc_txsched_elem *data;
3771
3772	buf = node->info;
3773	data = &buf.data;
3774
3775	data->valid_sections = ICE_AQC_ELEM_VALID_CIR | ICE_AQC_ELEM_VALID_EIR |
3776			       ICE_AQC_ELEM_VALID_GENERIC;
3777	data->cir_bw.bw_alloc = cpu_to_le16(weight);
3778	data->eir_bw.bw_alloc = cpu_to_le16(weight);
3779
3780	data->generic |= FIELD_PREP(ICE_AQC_ELEM_GENERIC_SP_M, 0x0);
3781
3782	return ice_sched_update_elem(pi->hw, node, &buf);
3783}
3784
3785/**
3786 * ice_sched_set_node_bw_lmt - set node's BW limit
3787 * @pi: port information structure
3788 * @node: tree node
3789 * @rl_type: rate limit type min, max, or shared
3790 * @bw: bandwidth in Kbps - Kilo bits per sec
3791 *
3792 * It updates node's BW limit parameters like BW RL profile ID of type CIR,
3793 * EIR, or SRL. The caller needs to hold scheduler lock.
3794 */
3795int
3796ice_sched_set_node_bw_lmt(struct ice_port_info *pi, struct ice_sched_node *node,
3797			  enum ice_rl_type rl_type, u32 bw)
3798{
3799	struct ice_sched_node *cfg_node = node;
3800	int status;
3801
3802	struct ice_hw *hw;
3803	u8 layer_num;
3804
3805	if (!pi)
3806		return -EINVAL;
3807	hw = pi->hw;
3808	/* Remove unused RL profile IDs from HW and SW DB */
3809	ice_sched_rm_unused_rl_prof(pi);
3810	layer_num = ice_sched_get_rl_prof_layer(pi, rl_type,
3811						node->tx_sched_layer);
3812	if (layer_num >= hw->num_tx_sched_layers)
3813		return -EINVAL;
3814
3815	if (rl_type == ICE_SHARED_BW) {
3816		/* SRL node may be different */
3817		cfg_node = ice_sched_get_srl_node(node, layer_num);
3818		if (!cfg_node)
3819			return -EIO;
3820	}
3821	/* EIR BW and Shared BW profiles are mutually exclusive and
3822	 * hence only one of them may be set for any given element
3823	 */
3824	status = ice_sched_set_eir_srl_excl(pi, cfg_node, layer_num, rl_type,
3825					    bw);
3826	if (status)
3827		return status;
3828	if (bw == ICE_SCHED_DFLT_BW)
3829		return ice_sched_set_node_bw_dflt(pi, cfg_node, rl_type,
3830						  layer_num);
3831	return ice_sched_set_node_bw(pi, cfg_node, rl_type, bw, layer_num);
3832}
3833
3834/**
3835 * ice_sched_set_node_bw_dflt_lmt - set node's BW limit to default
3836 * @pi: port information structure
3837 * @node: pointer to node structure
3838 * @rl_type: rate limit type min, max, or shared
3839 *
3840 * This function configures node element's BW rate limit profile ID of
3841 * type CIR, EIR, or SRL to default. This function needs to be called
3842 * with the scheduler lock held.
3843 */
3844static int
3845ice_sched_set_node_bw_dflt_lmt(struct ice_port_info *pi,
3846			       struct ice_sched_node *node,
3847			       enum ice_rl_type rl_type)
3848{
3849	return ice_sched_set_node_bw_lmt(pi, node, rl_type,
3850					 ICE_SCHED_DFLT_BW);
3851}
3852
3853/**
3854 * ice_sched_validate_srl_node - Check node for SRL applicability
3855 * @node: sched node to configure
3856 * @sel_layer: selected SRL layer
3857 *
3858 * This function checks if the SRL can be applied to a selected layer node on
3859 * behalf of the requested node (first argument). This function needs to be
3860 * called with scheduler lock held.
3861 */
3862static int
3863ice_sched_validate_srl_node(struct ice_sched_node *node, u8 sel_layer)
3864{
3865	/* SRL profiles are not available on all layers. Check if the
3866	 * SRL profile can be applied to a node above or below the
3867	 * requested node. SRL configuration is possible only if the
3868	 * selected layer's node has single child.
3869	 */
3870	if (sel_layer == node->tx_sched_layer ||
3871	    ((sel_layer == node->tx_sched_layer + 1) &&
3872	    node->num_children == 1) ||
3873	    ((sel_layer == node->tx_sched_layer - 1) &&
3874	    (node->parent && node->parent->num_children == 1)))
3875		return 0;
3876
3877	return -EIO;
3878}
3879
3880/**
3881 * ice_sched_save_q_bw - save queue node's BW information
3882 * @q_ctx: queue context structure
3883 * @rl_type: rate limit type min, max, or shared
3884 * @bw: bandwidth in Kbps - Kilo bits per sec
3885 *
3886 * Save BW information of queue type node for post replay use.
3887 */
3888static int
3889ice_sched_save_q_bw(struct ice_q_ctx *q_ctx, enum ice_rl_type rl_type, u32 bw)
3890{
3891	switch (rl_type) {
3892	case ICE_MIN_BW:
3893		ice_set_clear_cir_bw(&q_ctx->bw_t_info, bw);
3894		break;
3895	case ICE_MAX_BW:
3896		ice_set_clear_eir_bw(&q_ctx->bw_t_info, bw);
3897		break;
3898	case ICE_SHARED_BW:
3899		ice_set_clear_shared_bw(&q_ctx->bw_t_info, bw);
3900		break;
3901	default:
3902		return -EINVAL;
3903	}
3904	return 0;
3905}
3906
3907/**
3908 * ice_sched_set_q_bw_lmt - sets queue BW limit
3909 * @pi: port information structure
3910 * @vsi_handle: sw VSI handle
3911 * @tc: traffic class
3912 * @q_handle: software queue handle
3913 * @rl_type: min, max, or shared
3914 * @bw: bandwidth in Kbps
3915 *
3916 * This function sets BW limit of queue scheduling node.
3917 */
3918static int
3919ice_sched_set_q_bw_lmt(struct ice_port_info *pi, u16 vsi_handle, u8 tc,
3920		       u16 q_handle, enum ice_rl_type rl_type, u32 bw)
3921{
3922	struct ice_sched_node *node;
3923	struct ice_q_ctx *q_ctx;
3924	int status = -EINVAL;
3925
3926	if (!ice_is_vsi_valid(pi->hw, vsi_handle))
3927		return -EINVAL;
3928	mutex_lock(&pi->sched_lock);
3929	q_ctx = ice_get_lan_q_ctx(pi->hw, vsi_handle, tc, q_handle);
3930	if (!q_ctx)
3931		goto exit_q_bw_lmt;
3932	node = ice_sched_find_node_by_teid(pi->root, q_ctx->q_teid);
3933	if (!node) {
3934		ice_debug(pi->hw, ICE_DBG_SCHED, "Wrong q_teid\n");
3935		goto exit_q_bw_lmt;
3936	}
3937
3938	/* Return error if it is not a leaf node */
3939	if (node->info.data.elem_type != ICE_AQC_ELEM_TYPE_LEAF)
3940		goto exit_q_bw_lmt;
3941
3942	/* SRL bandwidth layer selection */
3943	if (rl_type == ICE_SHARED_BW) {
3944		u8 sel_layer; /* selected layer */
3945
3946		sel_layer = ice_sched_get_rl_prof_layer(pi, rl_type,
3947							node->tx_sched_layer);
3948		if (sel_layer >= pi->hw->num_tx_sched_layers) {
3949			status = -EINVAL;
3950			goto exit_q_bw_lmt;
3951		}
3952		status = ice_sched_validate_srl_node(node, sel_layer);
3953		if (status)
3954			goto exit_q_bw_lmt;
3955	}
3956
3957	if (bw == ICE_SCHED_DFLT_BW)
3958		status = ice_sched_set_node_bw_dflt_lmt(pi, node, rl_type);
3959	else
3960		status = ice_sched_set_node_bw_lmt(pi, node, rl_type, bw);
3961
3962	if (!status)
3963		status = ice_sched_save_q_bw(q_ctx, rl_type, bw);
3964
3965exit_q_bw_lmt:
3966	mutex_unlock(&pi->sched_lock);
3967	return status;
3968}
3969
3970/**
3971 * ice_cfg_q_bw_lmt - configure queue BW limit
3972 * @pi: port information structure
3973 * @vsi_handle: sw VSI handle
3974 * @tc: traffic class
3975 * @q_handle: software queue handle
3976 * @rl_type: min, max, or shared
3977 * @bw: bandwidth in Kbps
3978 *
3979 * This function configures BW limit of queue scheduling node.
3980 */
3981int
3982ice_cfg_q_bw_lmt(struct ice_port_info *pi, u16 vsi_handle, u8 tc,
3983		 u16 q_handle, enum ice_rl_type rl_type, u32 bw)
3984{
3985	return ice_sched_set_q_bw_lmt(pi, vsi_handle, tc, q_handle, rl_type,
3986				      bw);
3987}
3988
3989/**
3990 * ice_cfg_q_bw_dflt_lmt - configure queue BW default limit
3991 * @pi: port information structure
3992 * @vsi_handle: sw VSI handle
3993 * @tc: traffic class
3994 * @q_handle: software queue handle
3995 * @rl_type: min, max, or shared
3996 *
3997 * This function configures BW default limit of queue scheduling node.
3998 */
3999int
4000ice_cfg_q_bw_dflt_lmt(struct ice_port_info *pi, u16 vsi_handle, u8 tc,
4001		      u16 q_handle, enum ice_rl_type rl_type)
4002{
4003	return ice_sched_set_q_bw_lmt(pi, vsi_handle, tc, q_handle, rl_type,
4004				      ICE_SCHED_DFLT_BW);
4005}
4006
4007/**
4008 * ice_sched_get_node_by_id_type - get node from ID type
4009 * @pi: port information structure
4010 * @id: identifier
4011 * @agg_type: type of aggregator
4012 * @tc: traffic class
4013 *
4014 * This function returns node identified by ID of type aggregator, and
4015 * based on traffic class (TC). This function needs to be called with
4016 * the scheduler lock held.
4017 */
4018static struct ice_sched_node *
4019ice_sched_get_node_by_id_type(struct ice_port_info *pi, u32 id,
4020			      enum ice_agg_type agg_type, u8 tc)
4021{
4022	struct ice_sched_node *node = NULL;
4023
4024	switch (agg_type) {
4025	case ICE_AGG_TYPE_VSI: {
4026		struct ice_vsi_ctx *vsi_ctx;
4027		u16 vsi_handle = (u16)id;
4028
4029		if (!ice_is_vsi_valid(pi->hw, vsi_handle))
4030			break;
4031		/* Get sched_vsi_info */
4032		vsi_ctx = ice_get_vsi_ctx(pi->hw, vsi_handle);
4033		if (!vsi_ctx)
4034			break;
4035		node = vsi_ctx->sched.vsi_node[tc];
4036		break;
4037	}
4038
4039	case ICE_AGG_TYPE_AGG: {
4040		struct ice_sched_node *tc_node;
4041
4042		tc_node = ice_sched_get_tc_node(pi, tc);
4043		if (tc_node)
4044			node = ice_sched_get_agg_node(pi, tc_node, id);
4045		break;
4046	}
4047
4048	default:
4049		break;
4050	}
4051
4052	return node;
4053}
4054
4055/**
4056 * ice_sched_set_node_bw_lmt_per_tc - set node BW limit per TC
4057 * @pi: port information structure
4058 * @id: ID (software VSI handle or AGG ID)
4059 * @agg_type: aggregator type (VSI or AGG type node)
4060 * @tc: traffic class
4061 * @rl_type: min or max
4062 * @bw: bandwidth in Kbps
4063 *
4064 * This function sets BW limit of VSI or Aggregator scheduling node
4065 * based on TC information from passed in argument BW.
4066 */
4067static int
4068ice_sched_set_node_bw_lmt_per_tc(struct ice_port_info *pi, u32 id,
4069				 enum ice_agg_type agg_type, u8 tc,
4070				 enum ice_rl_type rl_type, u32 bw)
4071{
4072	struct ice_sched_node *node;
4073	int status = -EINVAL;
4074
4075	if (!pi)
4076		return status;
4077
4078	if (rl_type == ICE_UNKNOWN_BW)
4079		return status;
4080
4081	mutex_lock(&pi->sched_lock);
4082	node = ice_sched_get_node_by_id_type(pi, id, agg_type, tc);
4083	if (!node) {
4084		ice_debug(pi->hw, ICE_DBG_SCHED, "Wrong id, agg type, or tc\n");
4085		goto exit_set_node_bw_lmt_per_tc;
4086	}
4087	if (bw == ICE_SCHED_DFLT_BW)
4088		status = ice_sched_set_node_bw_dflt_lmt(pi, node, rl_type);
4089	else
4090		status = ice_sched_set_node_bw_lmt(pi, node, rl_type, bw);
4091
4092exit_set_node_bw_lmt_per_tc:
4093	mutex_unlock(&pi->sched_lock);
4094	return status;
4095}
4096
4097/**
4098 * ice_cfg_vsi_bw_lmt_per_tc - configure VSI BW limit per TC
4099 * @pi: port information structure
4100 * @vsi_handle: software VSI handle
4101 * @tc: traffic class
4102 * @rl_type: min or max
4103 * @bw: bandwidth in Kbps
4104 *
4105 * This function configures BW limit of VSI scheduling node based on TC
4106 * information.
4107 */
4108int
4109ice_cfg_vsi_bw_lmt_per_tc(struct ice_port_info *pi, u16 vsi_handle, u8 tc,
4110			  enum ice_rl_type rl_type, u32 bw)
4111{
4112	int status;
4113
4114	status = ice_sched_set_node_bw_lmt_per_tc(pi, vsi_handle,
4115						  ICE_AGG_TYPE_VSI,
4116						  tc, rl_type, bw);
4117	if (!status) {
4118		mutex_lock(&pi->sched_lock);
4119		status = ice_sched_save_vsi_bw(pi, vsi_handle, tc, rl_type, bw);
4120		mutex_unlock(&pi->sched_lock);
4121	}
4122	return status;
4123}
4124
4125/**
4126 * ice_cfg_vsi_bw_dflt_lmt_per_tc - configure default VSI BW limit per TC
4127 * @pi: port information structure
4128 * @vsi_handle: software VSI handle
4129 * @tc: traffic class
4130 * @rl_type: min or max
4131 *
4132 * This function configures default BW limit of VSI scheduling node based on TC
4133 * information.
4134 */
4135int
4136ice_cfg_vsi_bw_dflt_lmt_per_tc(struct ice_port_info *pi, u16 vsi_handle, u8 tc,
4137			       enum ice_rl_type rl_type)
4138{
4139	int status;
4140
4141	status = ice_sched_set_node_bw_lmt_per_tc(pi, vsi_handle,
4142						  ICE_AGG_TYPE_VSI,
4143						  tc, rl_type,
4144						  ICE_SCHED_DFLT_BW);
4145	if (!status) {
4146		mutex_lock(&pi->sched_lock);
4147		status = ice_sched_save_vsi_bw(pi, vsi_handle, tc, rl_type,
4148					       ICE_SCHED_DFLT_BW);
4149		mutex_unlock(&pi->sched_lock);
4150	}
4151	return status;
4152}
4153
4154/**
4155 * ice_cfg_rl_burst_size - Set burst size value
4156 * @hw: pointer to the HW struct
4157 * @bytes: burst size in bytes
4158 *
4159 * This function configures/set the burst size to requested new value. The new
4160 * burst size value is used for future rate limit calls. It doesn't change the
4161 * existing or previously created RL profiles.
4162 */
4163int ice_cfg_rl_burst_size(struct ice_hw *hw, u32 bytes)
4164{
4165	u16 burst_size_to_prog;
4166
4167	if (bytes < ICE_MIN_BURST_SIZE_ALLOWED ||
4168	    bytes > ICE_MAX_BURST_SIZE_ALLOWED)
4169		return -EINVAL;
4170	if (ice_round_to_num(bytes, 64) <=
4171	    ICE_MAX_BURST_SIZE_64_BYTE_GRANULARITY) {
4172		/* 64 byte granularity case */
4173		/* Disable MSB granularity bit */
4174		burst_size_to_prog = ICE_64_BYTE_GRANULARITY;
4175		/* round number to nearest 64 byte granularity */
4176		bytes = ice_round_to_num(bytes, 64);
4177		/* The value is in 64 byte chunks */
4178		burst_size_to_prog |= (u16)(bytes / 64);
4179	} else {
4180		/* k bytes granularity case */
4181		/* Enable MSB granularity bit */
4182		burst_size_to_prog = ICE_KBYTE_GRANULARITY;
4183		/* round number to nearest 1024 granularity */
4184		bytes = ice_round_to_num(bytes, 1024);
4185		/* check rounding doesn't go beyond allowed */
4186		if (bytes > ICE_MAX_BURST_SIZE_KBYTE_GRANULARITY)
4187			bytes = ICE_MAX_BURST_SIZE_KBYTE_GRANULARITY;
4188		/* The value is in k bytes */
4189		burst_size_to_prog |= (u16)(bytes / 1024);
4190	}
4191	hw->max_burst_size = burst_size_to_prog;
4192	return 0;
4193}
4194
4195/**
4196 * ice_sched_replay_node_prio - re-configure node priority
4197 * @hw: pointer to the HW struct
4198 * @node: sched node to configure
4199 * @priority: priority value
4200 *
4201 * This function configures node element's priority value. It
4202 * needs to be called with scheduler lock held.
4203 */
4204static int
4205ice_sched_replay_node_prio(struct ice_hw *hw, struct ice_sched_node *node,
4206			   u8 priority)
4207{
4208	struct ice_aqc_txsched_elem_data buf;
4209	struct ice_aqc_txsched_elem *data;
4210	int status;
4211
4212	buf = node->info;
4213	data = &buf.data;
4214	data->valid_sections |= ICE_AQC_ELEM_VALID_GENERIC;
4215	data->generic = priority;
4216
4217	/* Configure element */
4218	status = ice_sched_update_elem(hw, node, &buf);
4219	return status;
4220}
4221
4222/**
4223 * ice_sched_replay_node_bw - replay node(s) BW
4224 * @hw: pointer to the HW struct
4225 * @node: sched node to configure
4226 * @bw_t_info: BW type information
4227 *
4228 * This function restores node's BW from bw_t_info. The caller needs
4229 * to hold the scheduler lock.
4230 */
4231static int
4232ice_sched_replay_node_bw(struct ice_hw *hw, struct ice_sched_node *node,
4233			 struct ice_bw_type_info *bw_t_info)
4234{
4235	struct ice_port_info *pi = hw->port_info;
4236	int status = -EINVAL;
4237	u16 bw_alloc;
4238
4239	if (!node)
4240		return status;
4241	if (bitmap_empty(bw_t_info->bw_t_bitmap, ICE_BW_TYPE_CNT))
4242		return 0;
4243	if (test_bit(ICE_BW_TYPE_PRIO, bw_t_info->bw_t_bitmap)) {
4244		status = ice_sched_replay_node_prio(hw, node,
4245						    bw_t_info->generic);
4246		if (status)
4247			return status;
4248	}
4249	if (test_bit(ICE_BW_TYPE_CIR, bw_t_info->bw_t_bitmap)) {
4250		status = ice_sched_set_node_bw_lmt(pi, node, ICE_MIN_BW,
4251						   bw_t_info->cir_bw.bw);
4252		if (status)
4253			return status;
4254	}
4255	if (test_bit(ICE_BW_TYPE_CIR_WT, bw_t_info->bw_t_bitmap)) {
4256		bw_alloc = bw_t_info->cir_bw.bw_alloc;
4257		status = ice_sched_cfg_node_bw_alloc(hw, node, ICE_MIN_BW,
4258						     bw_alloc);
4259		if (status)
4260			return status;
4261	}
4262	if (test_bit(ICE_BW_TYPE_EIR, bw_t_info->bw_t_bitmap)) {
4263		status = ice_sched_set_node_bw_lmt(pi, node, ICE_MAX_BW,
4264						   bw_t_info->eir_bw.bw);
4265		if (status)
4266			return status;
4267	}
4268	if (test_bit(ICE_BW_TYPE_EIR_WT, bw_t_info->bw_t_bitmap)) {
4269		bw_alloc = bw_t_info->eir_bw.bw_alloc;
4270		status = ice_sched_cfg_node_bw_alloc(hw, node, ICE_MAX_BW,
4271						     bw_alloc);
4272		if (status)
4273			return status;
4274	}
4275	if (test_bit(ICE_BW_TYPE_SHARED, bw_t_info->bw_t_bitmap))
4276		status = ice_sched_set_node_bw_lmt(pi, node, ICE_SHARED_BW,
4277						   bw_t_info->shared_bw);
4278	return status;
4279}
4280
4281/**
4282 * ice_sched_get_ena_tc_bitmap - get enabled TC bitmap
4283 * @pi: port info struct
4284 * @tc_bitmap: 8 bits TC bitmap to check
4285 * @ena_tc_bitmap: 8 bits enabled TC bitmap to return
4286 *
4287 * This function returns enabled TC bitmap in variable ena_tc_bitmap. Some TCs
4288 * may be missing, it returns enabled TCs. This function needs to be called with
4289 * scheduler lock held.
4290 */
4291static void
4292ice_sched_get_ena_tc_bitmap(struct ice_port_info *pi,
4293			    unsigned long *tc_bitmap,
4294			    unsigned long *ena_tc_bitmap)
4295{
4296	u8 tc;
4297
4298	/* Some TC(s) may be missing after reset, adjust for replay */
4299	ice_for_each_traffic_class(tc)
4300		if (ice_is_tc_ena(*tc_bitmap, tc) &&
4301		    (ice_sched_get_tc_node(pi, tc)))
4302			set_bit(tc, ena_tc_bitmap);
4303}
4304
4305/**
4306 * ice_sched_replay_agg - recreate aggregator node(s)
4307 * @hw: pointer to the HW struct
4308 *
4309 * This function recreate aggregator type nodes which are not replayed earlier.
4310 * It also replay aggregator BW information. These aggregator nodes are not
4311 * associated with VSI type node yet.
4312 */
4313void ice_sched_replay_agg(struct ice_hw *hw)
4314{
4315	struct ice_port_info *pi = hw->port_info;
4316	struct ice_sched_agg_info *agg_info;
4317
4318	mutex_lock(&pi->sched_lock);
4319	list_for_each_entry(agg_info, &hw->agg_list, list_entry)
4320		/* replay aggregator (re-create aggregator node) */
4321		if (!bitmap_equal(agg_info->tc_bitmap, agg_info->replay_tc_bitmap,
4322				  ICE_MAX_TRAFFIC_CLASS)) {
4323			DECLARE_BITMAP(replay_bitmap, ICE_MAX_TRAFFIC_CLASS);
4324			int status;
4325
4326			bitmap_zero(replay_bitmap, ICE_MAX_TRAFFIC_CLASS);
4327			ice_sched_get_ena_tc_bitmap(pi,
4328						    agg_info->replay_tc_bitmap,
4329						    replay_bitmap);
4330			status = ice_sched_cfg_agg(hw->port_info,
4331						   agg_info->agg_id,
4332						   ICE_AGG_TYPE_AGG,
4333						   replay_bitmap);
4334			if (status) {
4335				dev_info(ice_hw_to_dev(hw),
4336					 "Replay agg id[%d] failed\n",
4337					 agg_info->agg_id);
4338				/* Move on to next one */
4339				continue;
4340			}
4341		}
4342	mutex_unlock(&pi->sched_lock);
4343}
4344
4345/**
4346 * ice_sched_replay_agg_vsi_preinit - Agg/VSI replay pre initialization
4347 * @hw: pointer to the HW struct
4348 *
4349 * This function initialize aggregator(s) TC bitmap to zero. A required
4350 * preinit step for replaying aggregators.
4351 */
4352void ice_sched_replay_agg_vsi_preinit(struct ice_hw *hw)
4353{
4354	struct ice_port_info *pi = hw->port_info;
4355	struct ice_sched_agg_info *agg_info;
4356
4357	mutex_lock(&pi->sched_lock);
4358	list_for_each_entry(agg_info, &hw->agg_list, list_entry) {
4359		struct ice_sched_agg_vsi_info *agg_vsi_info;
4360
4361		agg_info->tc_bitmap[0] = 0;
4362		list_for_each_entry(agg_vsi_info, &agg_info->agg_vsi_list,
4363				    list_entry)
4364			agg_vsi_info->tc_bitmap[0] = 0;
4365	}
4366	mutex_unlock(&pi->sched_lock);
4367}
4368
4369/**
4370 * ice_sched_replay_vsi_agg - replay aggregator & VSI to aggregator node(s)
4371 * @hw: pointer to the HW struct
4372 * @vsi_handle: software VSI handle
4373 *
4374 * This function replays aggregator node, VSI to aggregator type nodes, and
4375 * their node bandwidth information. This function needs to be called with
4376 * scheduler lock held.
4377 */
4378static int ice_sched_replay_vsi_agg(struct ice_hw *hw, u16 vsi_handle)
4379{
4380	DECLARE_BITMAP(replay_bitmap, ICE_MAX_TRAFFIC_CLASS);
4381	struct ice_sched_agg_vsi_info *agg_vsi_info;
4382	struct ice_port_info *pi = hw->port_info;
4383	struct ice_sched_agg_info *agg_info;
4384	int status;
4385
4386	bitmap_zero(replay_bitmap, ICE_MAX_TRAFFIC_CLASS);
4387	if (!ice_is_vsi_valid(hw, vsi_handle))
4388		return -EINVAL;
4389	agg_info = ice_get_vsi_agg_info(hw, vsi_handle);
4390	if (!agg_info)
4391		return 0; /* Not present in list - default Agg case */
4392	agg_vsi_info = ice_get_agg_vsi_info(agg_info, vsi_handle);
4393	if (!agg_vsi_info)
4394		return 0; /* Not present in list - default Agg case */
4395	ice_sched_get_ena_tc_bitmap(pi, agg_info->replay_tc_bitmap,
4396				    replay_bitmap);
4397	/* Replay aggregator node associated to vsi_handle */
4398	status = ice_sched_cfg_agg(hw->port_info, agg_info->agg_id,
4399				   ICE_AGG_TYPE_AGG, replay_bitmap);
4400	if (status)
4401		return status;
4402
4403	bitmap_zero(replay_bitmap, ICE_MAX_TRAFFIC_CLASS);
4404	ice_sched_get_ena_tc_bitmap(pi, agg_vsi_info->replay_tc_bitmap,
4405				    replay_bitmap);
4406	/* Move this VSI (vsi_handle) to above aggregator */
4407	return ice_sched_assoc_vsi_to_agg(pi, agg_info->agg_id, vsi_handle,
4408					  replay_bitmap);
4409}
4410
4411/**
4412 * ice_replay_vsi_agg - replay VSI to aggregator node
4413 * @hw: pointer to the HW struct
4414 * @vsi_handle: software VSI handle
4415 *
4416 * This function replays association of VSI to aggregator type nodes, and
4417 * node bandwidth information.
4418 */
4419int ice_replay_vsi_agg(struct ice_hw *hw, u16 vsi_handle)
4420{
4421	struct ice_port_info *pi = hw->port_info;
4422	int status;
4423
4424	mutex_lock(&pi->sched_lock);
4425	status = ice_sched_replay_vsi_agg(hw, vsi_handle);
4426	mutex_unlock(&pi->sched_lock);
4427	return status;
4428}
4429
4430/**
4431 * ice_sched_replay_q_bw - replay queue type node BW
4432 * @pi: port information structure
4433 * @q_ctx: queue context structure
4434 *
4435 * This function replays queue type node bandwidth. This function needs to be
4436 * called with scheduler lock held.
4437 */
4438int ice_sched_replay_q_bw(struct ice_port_info *pi, struct ice_q_ctx *q_ctx)
4439{
4440	struct ice_sched_node *q_node;
4441
4442	/* Following also checks the presence of node in tree */
4443	q_node = ice_sched_find_node_by_teid(pi->root, q_ctx->q_teid);
4444	if (!q_node)
4445		return -EINVAL;
4446	return ice_sched_replay_node_bw(pi->hw, q_node, &q_ctx->bw_t_info);
4447}