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