drivers/net/ethernet/intel/ice/ice_sched.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /* Copyright (c) 2018, Intel Corporation. */
   3
   4 #include "ice_sched.h"
   5
   6 /**
   7  * ice_sched_add_root_node - Insert the Tx scheduler root node in SW DB
   8  * @pi: port information structure
   9  * @info: Scheduler element information from firmware
  10  *
  11  * This function inserts the root node of the scheduling tree topology
  12  * to the SW DB.
  13  */
  14 static enum ice_status
  15 ice_sched_add_root_node(struct ice_port_info *pi,
  16                         struct ice_aqc_txsched_elem_data *info)
  17 {
  18         struct ice_sched_node *root;
  19         struct ice_hw *hw;
  20
  21         if (!pi)
  22                 return ICE_ERR_PARAM;
  23
  24         hw = pi->hw;
  25
  26         root = devm_kzalloc(ice_hw_to_dev(hw), sizeof(*root), GFP_KERNEL);
  27         if (!root)
  28                 return ICE_ERR_NO_MEMORY;
  29
  30         /* coverity[suspicious_sizeof] */
  31         root->children = devm_kcalloc(ice_hw_to_dev(hw), hw->max_children[0],
  32                                       sizeof(*root), GFP_KERNEL);
  33         if (!root->children) {
  34                 devm_kfree(ice_hw_to_dev(hw), root);
  35                 return ICE_ERR_NO_MEMORY;
  36         }
  37
  38         memcpy(&root->info, info, sizeof(*info));
  39         pi->root = root;
  40         return 0;
  41 }
  42
  43 /**
  44  * ice_sched_find_node_by_teid - Find the Tx scheduler node in SW DB
  45  * @start_node: pointer to the starting ice_sched_node struct in a sub-tree
  46  * @teid: node TEID to search
  47  *
  48  * This function searches for a node matching the TEID in the scheduling tree
  49  * from the SW DB. The search is recursive and is restricted by the number of
  50  * layers it has searched through; stopping at the max supported layer.
  51  *
  52  * This function needs to be called when holding the port_info->sched_lock
  53  */
  54 struct ice_sched_node *
  55 ice_sched_find_node_by_teid(struct ice_sched_node *start_node, u32 teid)
  56 {
  57         u16 i;
  58
  59         /* The TEID is same as that of the start_node */
  60         if (ICE_TXSCHED_GET_NODE_TEID(start_node) == teid)
  61                 return start_node;
  62
  63         /* The node has no children or is at the max layer */
  64         if (!start_node->num_children ||
  65             start_node->tx_sched_layer >= ICE_AQC_TOPO_MAX_LEVEL_NUM ||
  66             start_node->info.data.elem_type == ICE_AQC_ELEM_TYPE_LEAF)
  67                 return NULL;
  68
  69         /* Check if TEID matches to any of the children nodes */
  70         for (i = 0; i < start_node->num_children; i++)
  71                 if (ICE_TXSCHED_GET_NODE_TEID(start_node->children[i]) == teid)
  72                         return start_node->children[i];
  73
  74         /* Search within each child's sub-tree */
  75         for (i = 0; i < start_node->num_children; i++) {
  76                 struct ice_sched_node *tmp;
  77
  78                 tmp = ice_sched_find_node_by_teid(start_node->children[i],
  79                                                   teid);
  80                 if (tmp)
  81                         return tmp;
  82         }
  83
  84         return NULL;
  85 }
  86
  87 /**
  88  * ice_aqc_send_sched_elem_cmd - send scheduling elements cmd
  89  * @hw: pointer to the HW struct
  90  * @cmd_opc: cmd opcode
  91  * @elems_req: number of elements to request
  92  * @buf: pointer to buffer
  93  * @buf_size: buffer size in bytes
  94  * @elems_resp: returns total number of elements response
  95  * @cd: pointer to command details structure or NULL
  96  *
  97  * This function sends a scheduling elements cmd (cmd_opc)
  98  */
  99 static enum ice_status
 100 ice_aqc_send_sched_elem_cmd(struct ice_hw *hw, enum ice_adminq_opc cmd_opc,
 101                             u16 elems_req, void *buf, u16 buf_size,
 102                             u16 *elems_resp, struct ice_sq_cd *cd)
 103 {
 104         struct ice_aqc_sched_elem_cmd *cmd;
 105         struct ice_aq_desc desc;
 106         enum ice_status status;
 107
 108         cmd = &desc.params.sched_elem_cmd;
 109         ice_fill_dflt_direct_cmd_desc(&desc, cmd_opc);
 110         cmd->num_elem_req = cpu_to_le16(elems_req);
 111         desc.flags |= cpu_to_le16(ICE_AQ_FLAG_RD);
 112         status = ice_aq_send_cmd(hw, &desc, buf, buf_size, cd);
 113         if (!status && elems_resp)
 114                 *elems_resp = le16_to_cpu(cmd->num_elem_resp);
 115
 116         return status;
 117 }
 118
 119 /**
 120  * ice_aq_query_sched_elems - query scheduler elements
 121  * @hw: pointer to the HW struct
 122  * @elems_req: number of elements to query
 123  * @buf: pointer to buffer
 124  * @buf_size: buffer size in bytes
 125  * @elems_ret: returns total number of elements returned
 126  * @cd: pointer to command details structure or NULL
 127  *
 128  * Query scheduling elements (0x0404)
 129  */
 130 enum ice_status
 131 ice_aq_query_sched_elems(struct ice_hw *hw, u16 elems_req,
 132                          struct ice_aqc_get_elem *buf, u16 buf_size,
 133                          u16 *elems_ret, struct ice_sq_cd *cd)
 134 {
 135         return ice_aqc_send_sched_elem_cmd(hw, ice_aqc_opc_get_sched_elems,
 136                                            elems_req, (void *)buf, buf_size,
 137                                            elems_ret, cd);
 138 }
 139
 140 /**
 141  * ice_sched_add_node - Insert the Tx scheduler node in SW DB
 142  * @pi: port information structure
 143  * @layer: Scheduler layer of the node
 144  * @info: Scheduler element information from firmware
 145  *
 146  * This function inserts a scheduler node to the SW DB.
 147  */
 148 enum ice_status
 149 ice_sched_add_node(struct ice_port_info *pi, u8 layer,
 150                    struct ice_aqc_txsched_elem_data *info)
 151 {
 152         struct ice_sched_node *parent;
 153         struct ice_aqc_get_elem elem;
 154         struct ice_sched_node *node;
 155         enum ice_status status;
 156         struct ice_hw *hw;
 157
 158         if (!pi)
 159                 return ICE_ERR_PARAM;
 160
 161         hw = pi->hw;
 162
 163         /* A valid parent node should be there */
 164         parent = ice_sched_find_node_by_teid(pi->root,
 165                                              le32_to_cpu(info->parent_teid));
 166         if (!parent) {
 167                 ice_debug(hw, ICE_DBG_SCHED,
 168                           "Parent Node not found for parent_teid=0x%x\n",
 169                           le32_to_cpu(info->parent_teid));
 170                 return ICE_ERR_PARAM;
 171         }
 172
 173         /* query the current node information from FW  before additing it
 174          * to the SW DB
 175          */
 176         status = ice_sched_query_elem(hw, le32_to_cpu(info->node_teid), &elem);
 177         if (status)
 178                 return status;
 179
 180         node = devm_kzalloc(ice_hw_to_dev(hw), sizeof(*node), GFP_KERNEL);
 181         if (!node)
 182                 return ICE_ERR_NO_MEMORY;
 183         if (hw->max_children[layer]) {
 184                 /* coverity[suspicious_sizeof] */
 185                 node->children = devm_kcalloc(ice_hw_to_dev(hw),
 186                                               hw->max_children[layer],
 187                                               sizeof(*node), GFP_KERNEL);
 188                 if (!node->children) {
 189                         devm_kfree(ice_hw_to_dev(hw), node);
 190                         return ICE_ERR_NO_MEMORY;
 191                 }
 192         }
 193
 194         node->in_use = true;
 195         node->parent = parent;
 196         node->tx_sched_layer = layer;
 197         parent->children[parent->num_children++] = node;
 198         memcpy(&node->info, &elem.generic[0], sizeof(node->info));
 199         return 0;
 200 }
 201
 202 /**
 203  * ice_aq_delete_sched_elems - delete scheduler elements
 204  * @hw: pointer to the HW struct
 205  * @grps_req: number of groups to delete
 206  * @buf: pointer to buffer
 207  * @buf_size: buffer size in bytes
 208  * @grps_del: returns total number of elements deleted
 209  * @cd: pointer to command details structure or NULL
 210  *
 211  * Delete scheduling elements (0x040F)
 212  */
 213 static enum ice_status
 214 ice_aq_delete_sched_elems(struct ice_hw *hw, u16 grps_req,
 215                           struct ice_aqc_delete_elem *buf, u16 buf_size,
 216                           u16 *grps_del, struct ice_sq_cd *cd)
 217 {
 218         return ice_aqc_send_sched_elem_cmd(hw, ice_aqc_opc_delete_sched_elems,
 219                                            grps_req, (void *)buf, buf_size,
 220                                            grps_del, cd);
 221 }
 222
 223 /**
 224  * ice_sched_remove_elems - remove nodes from HW
 225  * @hw: pointer to the HW struct
 226  * @parent: pointer to the parent node
 227  * @num_nodes: number of nodes
 228  * @node_teids: array of node teids to be deleted
 229  *
 230  * This function remove nodes from HW
 231  */
 232 static enum ice_status
 233 ice_sched_remove_elems(struct ice_hw *hw, struct ice_sched_node *parent,
 234                        u16 num_nodes, u32 *node_teids)
 235 {
 236         struct ice_aqc_delete_elem *buf;
 237         u16 i, num_groups_removed = 0;
 238         enum ice_status status;
 239         u16 buf_size;
 240
 241         buf_size = sizeof(*buf) + sizeof(u32) * (num_nodes - 1);
 242         buf = devm_kzalloc(ice_hw_to_dev(hw), buf_size, GFP_KERNEL);
 243         if (!buf)
 244                 return ICE_ERR_NO_MEMORY;
 245
 246         buf->hdr.parent_teid = parent->info.node_teid;
 247         buf->hdr.num_elems = cpu_to_le16(num_nodes);
 248         for (i = 0; i < num_nodes; i++)
 249                 buf->teid[i] = cpu_to_le32(node_teids[i]);
 250
 251         status = ice_aq_delete_sched_elems(hw, 1, buf, buf_size,
 252                                            &num_groups_removed, NULL);
 253         if (status || num_groups_removed != 1)
 254                 ice_debug(hw, ICE_DBG_SCHED, "remove node failed FW error %d\n",
 255                           hw->adminq.sq_last_status);
 256
 257         devm_kfree(ice_hw_to_dev(hw), buf);
 258         return status;
 259 }
 260
 261 /**
 262  * ice_sched_get_first_node - get the first node of the given layer
 263  * @hw: pointer to the HW struct
 264  * @parent: pointer the base node of the subtree
 265  * @layer: layer number
 266  *
 267  * This function retrieves the first node of the given layer from the subtree
 268  */
 269 static struct ice_sched_node *
 270 ice_sched_get_first_node(struct ice_hw *hw, struct ice_sched_node *parent,
 271                          u8 layer)
 272 {
 273         u8 i;
 274
 275         if (layer < hw->sw_entry_point_layer)
 276                 return NULL;
 277         for (i = 0; i < parent->num_children; i++) {
 278                 struct ice_sched_node *node = parent->children[i];
 279
 280                 if (node) {
 281                         if (node->tx_sched_layer == layer)
 282                                 return node;
 283                         /* this recursion is intentional, and wouldn't
 284                          * go more than 9 calls
 285                          */
 286                         return ice_sched_get_first_node(hw, node, layer);
 287                 }
 288         }
 289         return NULL;
 290 }
 291
 292 /**
 293  * ice_sched_get_tc_node - get pointer to TC node
 294  * @pi: port information structure
 295  * @tc: TC number
 296  *
 297  * This function returns the TC node pointer
 298  */
 299 struct ice_sched_node *ice_sched_get_tc_node(struct ice_port_info *pi, u8 tc)
 300 {
 301         u8 i;
 302
 303         if (!pi)
 304                 return NULL;
 305         for (i = 0; i < pi->root->num_children; i++)
 306                 if (pi->root->children[i]->tc_num == tc)
 307                         return pi->root->children[i];
 308         return NULL;
 309 }
 310
 311 /**
 312  * ice_free_sched_node - Free a Tx scheduler node from SW DB
 313  * @pi: port information structure
 314  * @node: pointer to the ice_sched_node struct
 315  *
 316  * This function frees up a node from SW DB as well as from HW
 317  *
 318  * This function needs to be called with the port_info->sched_lock held
 319  */
 320 void ice_free_sched_node(struct ice_port_info *pi, struct ice_sched_node *node)
 321 {
 322         struct ice_sched_node *parent;
 323         struct ice_hw *hw = pi->hw;
 324         u8 i, j;
 325
 326         /* Free the children before freeing up the parent node
 327          * The parent array is updated below and that shifts the nodes
 328          * in the array. So always pick the first child if num children > 0
 329          */
 330         while (node->num_children)
 331                 ice_free_sched_node(pi, node->children[0]);
 332
 333         /* Leaf, TC and root nodes can't be deleted by SW */
 334         if (node->tx_sched_layer >= hw->sw_entry_point_layer &&
 335             node->info.data.elem_type != ICE_AQC_ELEM_TYPE_TC &&
 336             node->info.data.elem_type != ICE_AQC_ELEM_TYPE_ROOT_PORT &&
 337             node->info.data.elem_type != ICE_AQC_ELEM_TYPE_LEAF) {
 338                 u32 teid = le32_to_cpu(node->info.node_teid);
 339
 340                 ice_sched_remove_elems(hw, node->parent, 1, &teid);
 341         }
 342         parent = node->parent;
 343         /* root has no parent */
 344         if (parent) {
 345                 struct ice_sched_node *p, *tc_node;
 346
 347                 /* update the parent */
 348                 for (i = 0; i < parent->num_children; i++)
 349                         if (parent->children[i] == node) {
 350                                 for (j = i + 1; j < parent->num_children; j++)
 351                                         parent->children[j - 1] =
 352                                                 parent->children[j];
 353                                 parent->num_children--;
 354                                 break;
 355                         }
 356
 357                 /* search for previous sibling that points to this node and
 358                  * remove the reference
 359                  */
 360                 tc_node = ice_sched_get_tc_node(pi, node->tc_num);
 361                 if (!tc_node) {
 362                         ice_debug(hw, ICE_DBG_SCHED,
 363                                   "Invalid TC number %d\n", node->tc_num);
 364                         goto err_exit;
 365                 }
 366                 p = ice_sched_get_first_node(hw, tc_node, node->tx_sched_layer);
 367                 while (p) {
 368                         if (p->sibling == node) {
 369                                 p->sibling = node->sibling;
 370                                 break;
 371                         }
 372                         p = p->sibling;
 373                 }
 374         }
 375 err_exit:
 376         /* leaf nodes have no children */
 377         if (node->children)
 378                 devm_kfree(ice_hw_to_dev(hw), node->children);
 379         devm_kfree(ice_hw_to_dev(hw), node);
 380 }
 381
 382 /**
 383  * ice_aq_get_dflt_topo - gets default scheduler topology
 384  * @hw: pointer to the HW struct
 385  * @lport: logical port number
 386  * @buf: pointer to buffer
 387  * @buf_size: buffer size in bytes
 388  * @num_branches: returns total number of queue to port branches
 389  * @cd: pointer to command details structure or NULL
 390  *
 391  * Get default scheduler topology (0x400)
 392  */
 393 static enum ice_status
 394 ice_aq_get_dflt_topo(struct ice_hw *hw, u8 lport,
 395                      struct ice_aqc_get_topo_elem *buf, u16 buf_size,
 396                      u8 *num_branches, struct ice_sq_cd *cd)
 397 {
 398         struct ice_aqc_get_topo *cmd;
 399         struct ice_aq_desc desc;
 400         enum ice_status status;
 401
 402         cmd = &desc.params.get_topo;
 403         ice_fill_dflt_direct_cmd_desc(&desc, ice_aqc_opc_get_dflt_topo);
 404         cmd->port_num = lport;
 405         status = ice_aq_send_cmd(hw, &desc, buf, buf_size, cd);
 406         if (!status && num_branches)
 407                 *num_branches = cmd->num_branches;
 408
 409         return status;
 410 }
 411
 412 /**
 413  * ice_aq_add_sched_elems - adds scheduling element
 414  * @hw: pointer to the HW struct
 415  * @grps_req: the number of groups that are requested to be added
 416  * @buf: pointer to buffer
 417  * @buf_size: buffer size in bytes
 418  * @grps_added: returns total number of groups added
 419  * @cd: pointer to command details structure or NULL
 420  *
 421  * Add scheduling elements (0x0401)
 422  */
 423 static enum ice_status
 424 ice_aq_add_sched_elems(struct ice_hw *hw, u16 grps_req,
 425                        struct ice_aqc_add_elem *buf, u16 buf_size,
 426                        u16 *grps_added, struct ice_sq_cd *cd)
 427 {
 428         return ice_aqc_send_sched_elem_cmd(hw, ice_aqc_opc_add_sched_elems,
 429                                            grps_req, (void *)buf, buf_size,
 430                                            grps_added, cd);
 431 }
 432
 433 /**
 434  * ice_aq_suspend_sched_elems - suspend scheduler elements
 435  * @hw: pointer to the HW struct
 436  * @elems_req: number of elements to suspend
 437  * @buf: pointer to buffer
 438  * @buf_size: buffer size in bytes
 439  * @elems_ret: returns total number of elements suspended
 440  * @cd: pointer to command details structure or NULL
 441  *
 442  * Suspend scheduling elements (0x0409)
 443  */
 444 static enum ice_status
 445 ice_aq_suspend_sched_elems(struct ice_hw *hw, u16 elems_req,
 446                            struct ice_aqc_suspend_resume_elem *buf,
 447                            u16 buf_size, u16 *elems_ret, struct ice_sq_cd *cd)
 448 {
 449         return ice_aqc_send_sched_elem_cmd(hw, ice_aqc_opc_suspend_sched_elems,
 450                                            elems_req, (void *)buf, buf_size,
 451                                            elems_ret, cd);
 452 }
 453
 454 /**
 455  * ice_aq_resume_sched_elems - resume scheduler elements
 456  * @hw: pointer to the HW struct
 457  * @elems_req: number of elements to resume
 458  * @buf: pointer to buffer
 459  * @buf_size: buffer size in bytes
 460  * @elems_ret: returns total number of elements resumed
 461  * @cd: pointer to command details structure or NULL
 462  *
 463  * resume scheduling elements (0x040A)
 464  */
 465 static enum ice_status
 466 ice_aq_resume_sched_elems(struct ice_hw *hw, u16 elems_req,
 467                           struct ice_aqc_suspend_resume_elem *buf,
 468                           u16 buf_size, u16 *elems_ret, struct ice_sq_cd *cd)
 469 {
 470         return ice_aqc_send_sched_elem_cmd(hw, ice_aqc_opc_resume_sched_elems,
 471                                            elems_req, (void *)buf, buf_size,
 472                                            elems_ret, cd);
 473 }
 474
 475 /**
 476  * ice_aq_query_sched_res - query scheduler resource
 477  * @hw: pointer to the HW struct
 478  * @buf_size: buffer size in bytes
 479  * @buf: pointer to buffer
 480  * @cd: pointer to command details structure or NULL
 481  *
 482  * Query scheduler resource allocation (0x0412)
 483  */
 484 static enum ice_status
 485 ice_aq_query_sched_res(struct ice_hw *hw, u16 buf_size,
 486                        struct ice_aqc_query_txsched_res_resp *buf,
 487                        struct ice_sq_cd *cd)
 488 {
 489         struct ice_aq_desc desc;
 490
 491         ice_fill_dflt_direct_cmd_desc(&desc, ice_aqc_opc_query_sched_res);
 492         return ice_aq_send_cmd(hw, &desc, buf, buf_size, cd);
 493 }
 494
 495 /**
 496  * ice_sched_suspend_resume_elems - suspend or resume HW nodes
 497  * @hw: pointer to the HW struct
 498  * @num_nodes: number of nodes
 499  * @node_teids: array of node teids to be suspended or resumed
 500  * @suspend: true means suspend / false means resume
 501  *
 502  * This function suspends or resumes HW nodes
 503  */
 504 static enum ice_status
 505 ice_sched_suspend_resume_elems(struct ice_hw *hw, u8 num_nodes, u32 *node_teids,
 506                                bool suspend)
 507 {
 508         struct ice_aqc_suspend_resume_elem *buf;
 509         u16 i, buf_size, num_elem_ret = 0;
 510         enum ice_status status;
 511
 512         buf_size = sizeof(*buf) * num_nodes;
 513         buf = devm_kzalloc(ice_hw_to_dev(hw), buf_size, GFP_KERNEL);
 514         if (!buf)
 515                 return ICE_ERR_NO_MEMORY;
 516
 517         for (i = 0; i < num_nodes; i++)
 518                 buf->teid[i] = cpu_to_le32(node_teids[i]);
 519
 520         if (suspend)
 521                 status = ice_aq_suspend_sched_elems(hw, num_nodes, buf,
 522                                                     buf_size, &num_elem_ret,
 523                                                     NULL);
 524         else
 525                 status = ice_aq_resume_sched_elems(hw, num_nodes, buf,
 526                                                    buf_size, &num_elem_ret,
 527                                                    NULL);
 528         if (status || num_elem_ret != num_nodes)
 529                 ice_debug(hw, ICE_DBG_SCHED, "suspend/resume failed\n");
 530
 531         devm_kfree(ice_hw_to_dev(hw), buf);
 532         return status;
 533 }
 534
 535 /**
 536  * ice_alloc_lan_q_ctx - allocate LAN queue contexts for the given VSI and TC
 537  * @hw: pointer to the HW struct
 538  * @vsi_handle: VSI handle
 539  * @tc: TC number
 540  * @new_numqs: number of queues
 541  */
 542 static enum ice_status
 543 ice_alloc_lan_q_ctx(struct ice_hw *hw, u16 vsi_handle, u8 tc, u16 new_numqs)
 544 {
 545         struct ice_vsi_ctx *vsi_ctx;
 546         struct ice_q_ctx *q_ctx;
 547
 548         vsi_ctx = ice_get_vsi_ctx(hw, vsi_handle);
 549         if (!vsi_ctx)
 550                 return ICE_ERR_PARAM;
 551         /* allocate LAN queue contexts */
 552         if (!vsi_ctx->lan_q_ctx[tc]) {
 553                 vsi_ctx->lan_q_ctx[tc] = devm_kcalloc(ice_hw_to_dev(hw),
 554                                                       new_numqs,
 555                                                       sizeof(*q_ctx),
 556                                                       GFP_KERNEL);
 557                 if (!vsi_ctx->lan_q_ctx[tc])
 558                         return ICE_ERR_NO_MEMORY;
 559                 vsi_ctx->num_lan_q_entries[tc] = new_numqs;
 560                 return 0;
 561         }
 562         /* num queues are increased, update the queue contexts */
 563         if (new_numqs > vsi_ctx->num_lan_q_entries[tc]) {
 564                 u16 prev_num = vsi_ctx->num_lan_q_entries[tc];
 565
 566                 q_ctx = devm_kcalloc(ice_hw_to_dev(hw), new_numqs,
 567                                      sizeof(*q_ctx), GFP_KERNEL);
 568                 if (!q_ctx)
 569                         return ICE_ERR_NO_MEMORY;
 570                 memcpy(q_ctx, vsi_ctx->lan_q_ctx[tc],
 571                        prev_num * sizeof(*q_ctx));
 572                 devm_kfree(ice_hw_to_dev(hw), vsi_ctx->lan_q_ctx[tc]);
 573                 vsi_ctx->lan_q_ctx[tc] = q_ctx;
 574                 vsi_ctx->num_lan_q_entries[tc] = new_numqs;
 575         }
 576         return 0;
 577 }
 578
 579 /**
 580  * ice_sched_clear_agg - clears the aggregator related information
 581  * @hw: pointer to the hardware structure
 582  *
 583  * This function removes aggregator list and free up aggregator related memory
 584  * previously allocated.
 585  */
 586 void ice_sched_clear_agg(struct ice_hw *hw)
 587 {
 588         struct ice_sched_agg_info *agg_info;
 589         struct ice_sched_agg_info *atmp;
 590
 591         list_for_each_entry_safe(agg_info, atmp, &hw->agg_list, list_entry) {
 592                 struct ice_sched_agg_vsi_info *agg_vsi_info;
 593                 struct ice_sched_agg_vsi_info *vtmp;
 594
 595                 list_for_each_entry_safe(agg_vsi_info, vtmp,
 596                                          &agg_info->agg_vsi_list, list_entry) {
 597                         list_del(&agg_vsi_info->list_entry);
 598                         devm_kfree(ice_hw_to_dev(hw), agg_vsi_info);
 599                 }
 600                 list_del(&agg_info->list_entry);
 601                 devm_kfree(ice_hw_to_dev(hw), agg_info);
 602         }
 603 }
 604
 605 /**
 606  * ice_sched_clear_tx_topo - clears the scheduler tree nodes
 607  * @pi: port information structure
 608  *
 609  * This function removes all the nodes from HW as well as from SW DB.
 610  */
 611 static void ice_sched_clear_tx_topo(struct ice_port_info *pi)
 612 {
 613         if (!pi)
 614                 return;
 615         if (pi->root) {
 616                 ice_free_sched_node(pi, pi->root);
 617                 pi->root = NULL;
 618         }
 619 }
 620
 621 /**
 622  * ice_sched_clear_port - clear the scheduler elements from SW DB for a port
 623  * @pi: port information structure
 624  *
 625  * Cleanup scheduling elements from SW DB
 626  */
 627 void ice_sched_clear_port(struct ice_port_info *pi)
 628 {
 629         if (!pi || pi->port_state != ICE_SCHED_PORT_STATE_READY)
 630                 return;
 631
 632         pi->port_state = ICE_SCHED_PORT_STATE_INIT;
 633         mutex_lock(&pi->sched_lock);
 634         ice_sched_clear_tx_topo(pi);
 635         mutex_unlock(&pi->sched_lock);
 636         mutex_destroy(&pi->sched_lock);
 637 }
 638
 639 /**
 640  * ice_sched_cleanup_all - cleanup scheduler elements from SW DB for all ports
 641  * @hw: pointer to the HW struct
 642  *
 643  * Cleanup scheduling elements from SW DB for all the ports
 644  */
 645 void ice_sched_cleanup_all(struct ice_hw *hw)
 646 {
 647         if (!hw)
 648                 return;
 649
 650         if (hw->layer_info) {
 651                 devm_kfree(ice_hw_to_dev(hw), hw->layer_info);
 652                 hw->layer_info = NULL;
 653         }
 654
 655         if (hw->port_info)
 656                 ice_sched_clear_port(hw->port_info);
 657
 658         hw->num_tx_sched_layers = 0;
 659         hw->num_tx_sched_phys_layers = 0;
 660         hw->flattened_layers = 0;
 661         hw->max_cgds = 0;
 662 }
 663
 664 /**
 665  * ice_sched_add_elems - add nodes to HW and SW DB
 666  * @pi: port information structure
 667  * @tc_node: pointer to the branch node
 668  * @parent: pointer to the parent node
 669  * @layer: layer number to add nodes
 670  * @num_nodes: number of nodes
 671  * @num_nodes_added: pointer to num nodes added
 672  * @first_node_teid: if new nodes are added then return the TEID of first node
 673  *
 674  * This function add nodes to HW as well as to SW DB for a given layer
 675  */
 676 static enum ice_status
 677 ice_sched_add_elems(struct ice_port_info *pi, struct ice_sched_node *tc_node,
 678                     struct ice_sched_node *parent, u8 layer, u16 num_nodes,
 679                     u16 *num_nodes_added, u32 *first_node_teid)
 680 {
 681         struct ice_sched_node *prev, *new_node;
 682         struct ice_aqc_add_elem *buf;
 683         u16 i, num_groups_added = 0;
 684         enum ice_status status = 0;
 685         struct ice_hw *hw = pi->hw;
 686         u16 buf_size;
 687         u32 teid;
 688
 689         buf_size = sizeof(*buf) + sizeof(*buf->generic) * (num_nodes - 1);
 690         buf = devm_kzalloc(ice_hw_to_dev(hw), buf_size, GFP_KERNEL);
 691         if (!buf)
 692                 return ICE_ERR_NO_MEMORY;
 693
 694         buf->hdr.parent_teid = parent->info.node_teid;
 695         buf->hdr.num_elems = cpu_to_le16(num_nodes);
 696         for (i = 0; i < num_nodes; i++) {
 697                 buf->generic[i].parent_teid = parent->info.node_teid;
 698                 buf->generic[i].data.elem_type = ICE_AQC_ELEM_TYPE_SE_GENERIC;
 699                 buf->generic[i].data.valid_sections =
 700                         ICE_AQC_ELEM_VALID_GENERIC | ICE_AQC_ELEM_VALID_CIR |
 701                         ICE_AQC_ELEM_VALID_EIR;
 702                 buf->generic[i].data.generic = 0;
 703                 buf->generic[i].data.cir_bw.bw_profile_idx =
 704                         cpu_to_le16(ICE_SCHED_DFLT_RL_PROF_ID);
 705                 buf->generic[i].data.cir_bw.bw_alloc =
 706                         cpu_to_le16(ICE_SCHED_DFLT_BW_WT);
 707                 buf->generic[i].data.eir_bw.bw_profile_idx =
 708                         cpu_to_le16(ICE_SCHED_DFLT_RL_PROF_ID);
 709                 buf->generic[i].data.eir_bw.bw_alloc =
 710                         cpu_to_le16(ICE_SCHED_DFLT_BW_WT);
 711         }
 712
 713         status = ice_aq_add_sched_elems(hw, 1, buf, buf_size,
 714                                         &num_groups_added, NULL);
 715         if (status || num_groups_added != 1) {
 716                 ice_debug(hw, ICE_DBG_SCHED, "add node failed FW Error %d\n",
 717                           hw->adminq.sq_last_status);
 718                 devm_kfree(ice_hw_to_dev(hw), buf);
 719                 return ICE_ERR_CFG;
 720         }
 721
 722         *num_nodes_added = num_nodes;
 723         /* add nodes to the SW DB */
 724         for (i = 0; i < num_nodes; i++) {
 725                 status = ice_sched_add_node(pi, layer, &buf->generic[i]);
 726                 if (status) {
 727                         ice_debug(hw, ICE_DBG_SCHED,
 728                                   "add nodes in SW DB failed status =%d\n",
 729                                   status);
 730                         break;
 731                 }
 732
 733                 teid = le32_to_cpu(buf->generic[i].node_teid);
 734                 new_node = ice_sched_find_node_by_teid(parent, teid);
 735                 if (!new_node) {
 736                         ice_debug(hw, ICE_DBG_SCHED,
 737                                   "Node is missing for teid =%d\n", teid);
 738                         break;
 739                 }
 740
 741                 new_node->sibling = NULL;
 742                 new_node->tc_num = tc_node->tc_num;
 743
 744                 /* add it to previous node sibling pointer */
 745                 /* Note: siblings are not linked across branches */
 746                 prev = ice_sched_get_first_node(hw, tc_node, layer);
 747                 if (prev && prev != new_node) {
 748                         while (prev->sibling)
 749                                 prev = prev->sibling;
 750                         prev->sibling = new_node;
 751                 }
 752
 753                 if (i == 0)
 754                         *first_node_teid = teid;
 755         }
 756
 757         devm_kfree(ice_hw_to_dev(hw), buf);
 758         return status;
 759 }
 760
 761 /**
 762  * ice_sched_add_nodes_to_layer - Add nodes to a given layer
 763  * @pi: port information structure
 764  * @tc_node: pointer to TC node
 765  * @parent: pointer to parent node
 766  * @layer: layer number to add nodes
 767  * @num_nodes: number of nodes to be added
 768  * @first_node_teid: pointer to the first node TEID
 769  * @num_nodes_added: pointer to number of nodes added
 770  *
 771  * This function add nodes to a given layer.
 772  */
 773 static enum ice_status
 774 ice_sched_add_nodes_to_layer(struct ice_port_info *pi,
 775                              struct ice_sched_node *tc_node,
 776                              struct ice_sched_node *parent, u8 layer,
 777                              u16 num_nodes, u32 *first_node_teid,
 778                              u16 *num_nodes_added)
 779 {
 780         u32 *first_teid_ptr = first_node_teid;
 781         u16 new_num_nodes, max_child_nodes;
 782         enum ice_status status = 0;
 783         struct ice_hw *hw = pi->hw;
 784         u16 num_added = 0;
 785         u32 temp;
 786
 787         *num_nodes_added = 0;
 788
 789         if (!num_nodes)
 790                 return status;
 791
 792         if (!parent || layer < hw->sw_entry_point_layer)
 793                 return ICE_ERR_PARAM;
 794
 795         /* max children per node per layer */
 796         max_child_nodes = hw->max_children[parent->tx_sched_layer];
 797
 798         /* current number of children + required nodes exceed max children ? */
 799         if ((parent->num_children + num_nodes) > max_child_nodes) {
 800                 /* Fail if the parent is a TC node */
 801                 if (parent == tc_node)
 802                         return ICE_ERR_CFG;
 803
 804                 /* utilize all the spaces if the parent is not full */
 805                 if (parent->num_children < max_child_nodes) {
 806                         new_num_nodes = max_child_nodes - parent->num_children;
 807                         /* this recursion is intentional, and wouldn't
 808                          * go more than 2 calls
 809                          */
 810                         status = ice_sched_add_nodes_to_layer(pi, tc_node,
 811                                                               parent, layer,
 812                                                               new_num_nodes,
 813                                                               first_node_teid,
 814                                                               &num_added);
 815                         if (status)
 816                                 return status;
 817
 818                         *num_nodes_added += num_added;
 819                 }
 820                 /* Don't modify the first node TEID memory if the first node was
 821                  * added already in the above call. Instead send some temp
 822                  * memory for all other recursive calls.
 823                  */
 824                 if (num_added)
 825                         first_teid_ptr = &temp;
 826
 827                 new_num_nodes = num_nodes - num_added;
 828
 829                 /* This parent is full, try the next sibling */
 830                 parent = parent->sibling;
 831
 832                 /* this recursion is intentional, for 1024 queues
 833                  * per VSI, it goes max of 16 iterations.
 834                  * 1024 / 8 = 128 layer 8 nodes
 835                  * 128 /8 = 16 (add 8 nodes per iteration)
 836                  */
 837                 status = ice_sched_add_nodes_to_layer(pi, tc_node, parent,
 838                                                       layer, new_num_nodes,
 839                                                       first_teid_ptr,
 840                                                       &num_added);
 841                 *num_nodes_added += num_added;
 842                 return status;
 843         }
 844
 845         status = ice_sched_add_elems(pi, tc_node, parent, layer, num_nodes,
 846                                      num_nodes_added, first_node_teid);
 847         return status;
 848 }
 849
 850 /**
 851  * ice_sched_get_qgrp_layer - get the current queue group layer number
 852  * @hw: pointer to the HW struct
 853  *
 854  * This function returns the current queue group layer number
 855  */
 856 static u8 ice_sched_get_qgrp_layer(struct ice_hw *hw)
 857 {
 858         /* It's always total layers - 1, the array is 0 relative so -2 */
 859         return hw->num_tx_sched_layers - ICE_QGRP_LAYER_OFFSET;
 860 }
 861
 862 /**
 863  * ice_sched_get_vsi_layer - get the current VSI layer number
 864  * @hw: pointer to the HW struct
 865  *
 866  * This function returns the current VSI layer number
 867  */
 868 static u8 ice_sched_get_vsi_layer(struct ice_hw *hw)
 869 {
 870         /* Num Layers       VSI layer
 871          *     9               6
 872          *     7               4
 873          *     5 or less       sw_entry_point_layer
 874          */
 875         /* calculate the VSI layer based on number of layers. */
 876         if (hw->num_tx_sched_layers > ICE_VSI_LAYER_OFFSET + 1) {
 877                 u8 layer = hw->num_tx_sched_layers - ICE_VSI_LAYER_OFFSET;
 878
 879                 if (layer > hw->sw_entry_point_layer)
 880                         return layer;
 881         }
 882         return hw->sw_entry_point_layer;
 883 }
 884
 885 /**
 886  * ice_rm_dflt_leaf_node - remove the default leaf node in the tree
 887  * @pi: port information structure
 888  *
 889  * This function removes the leaf node that was created by the FW
 890  * during initialization
 891  */
 892 static void ice_rm_dflt_leaf_node(struct ice_port_info *pi)
 893 {
 894         struct ice_sched_node *node;
 895
 896         node = pi->root;
 897         while (node) {
 898                 if (!node->num_children)
 899                         break;
 900                 node = node->children[0];
 901         }
 902         if (node && node->info.data.elem_type == ICE_AQC_ELEM_TYPE_LEAF) {
 903                 u32 teid = le32_to_cpu(node->info.node_teid);
 904                 enum ice_status status;
 905
 906                 /* remove the default leaf node */
 907                 status = ice_sched_remove_elems(pi->hw, node->parent, 1, &teid);
 908                 if (!status)
 909                         ice_free_sched_node(pi, node);
 910         }
 911 }
 912
 913 /**
 914  * ice_sched_rm_dflt_nodes - free the default nodes in the tree
 915  * @pi: port information structure
 916  *
 917  * This function frees all the nodes except root and TC that were created by
 918  * the FW during initialization
 919  */
 920 static void ice_sched_rm_dflt_nodes(struct ice_port_info *pi)
 921 {
 922         struct ice_sched_node *node;
 923
 924         ice_rm_dflt_leaf_node(pi);
 925
 926         /* remove the default nodes except TC and root nodes */
 927         node = pi->root;
 928         while (node) {
 929                 if (node->tx_sched_layer >= pi->hw->sw_entry_point_layer &&
 930                     node->info.data.elem_type != ICE_AQC_ELEM_TYPE_TC &&
 931                     node->info.data.elem_type != ICE_AQC_ELEM_TYPE_ROOT_PORT) {
 932                         ice_free_sched_node(pi, node);
 933                         break;
 934                 }
 935
 936                 if (!node->num_children)
 937                         break;
 938                 node = node->children[0];
 939         }
 940 }
 941
 942 /**
 943  * ice_sched_init_port - Initialize scheduler by querying information from FW
 944  * @pi: port info structure for the tree to cleanup
 945  *
 946  * This function is the initial call to find the total number of Tx scheduler
 947  * resources, default topology created by firmware and storing the information
 948  * in SW DB.
 949  */
 950 enum ice_status ice_sched_init_port(struct ice_port_info *pi)
 951 {
 952         struct ice_aqc_get_topo_elem *buf;
 953         enum ice_status status;
 954         struct ice_hw *hw;
 955         u8 num_branches;
 956         u16 num_elems;
 957         u8 i, j;
 958
 959         if (!pi)
 960                 return ICE_ERR_PARAM;
 961         hw = pi->hw;
 962
 963         /* Query the Default Topology from FW */
 964         buf = devm_kzalloc(ice_hw_to_dev(hw), ICE_AQ_MAX_BUF_LEN, GFP_KERNEL);
 965         if (!buf)
 966                 return ICE_ERR_NO_MEMORY;
 967
 968         /* Query default scheduling tree topology */
 969         status = ice_aq_get_dflt_topo(hw, pi->lport, buf, ICE_AQ_MAX_BUF_LEN,
 970                                       &num_branches, NULL);
 971         if (status)
 972                 goto err_init_port;
 973
 974         /* num_branches should be between 1-8 */
 975         if (num_branches < 1 || num_branches > ICE_TXSCHED_MAX_BRANCHES) {
 976                 ice_debug(hw, ICE_DBG_SCHED, "num_branches unexpected %d\n",
 977                           num_branches);
 978                 status = ICE_ERR_PARAM;
 979                 goto err_init_port;
 980         }
 981
 982         /* get the number of elements on the default/first branch */
 983         num_elems = le16_to_cpu(buf[0].hdr.num_elems);
 984
 985         /* num_elems should always be between 1-9 */
 986         if (num_elems < 1 || num_elems > ICE_AQC_TOPO_MAX_LEVEL_NUM) {
 987                 ice_debug(hw, ICE_DBG_SCHED, "num_elems unexpected %d\n",
 988                           num_elems);
 989                 status = ICE_ERR_PARAM;
 990                 goto err_init_port;
 991         }
 992
 993         /* If the last node is a leaf node then the index of the queue group
 994          * layer is two less than the number of elements.
 995          */
 996         if (num_elems > 2 && buf[0].generic[num_elems - 1].data.elem_type ==
 997             ICE_AQC_ELEM_TYPE_LEAF)
 998                 pi->last_node_teid =
 999                         le32_to_cpu(buf[0].generic[num_elems - 2].node_teid);
1000         else
1001                 pi->last_node_teid =
1002                         le32_to_cpu(buf[0].generic[num_elems - 1].node_teid);
1003
1004         /* Insert the Tx Sched root node */
1005         status = ice_sched_add_root_node(pi, &buf[0].generic[0]);
1006         if (status)
1007                 goto err_init_port;
1008
1009         /* Parse the default tree and cache the information */
1010         for (i = 0; i < num_branches; i++) {
1011                 num_elems = le16_to_cpu(buf[i].hdr.num_elems);
1012
1013                 /* Skip root element as already inserted */
1014                 for (j = 1; j < num_elems; j++) {
1015                         /* update the sw entry point */
1016                         if (buf[0].generic[j].data.elem_type ==
1017                             ICE_AQC_ELEM_TYPE_ENTRY_POINT)
1018                                 hw->sw_entry_point_layer = j;
1019
1020                         status = ice_sched_add_node(pi, j, &buf[i].generic[j]);
1021                         if (status)
1022                                 goto err_init_port;
1023                 }
1024         }
1025
1026         /* Remove the default nodes. */
1027         if (pi->root)
1028                 ice_sched_rm_dflt_nodes(pi);
1029
1030         /* initialize the port for handling the scheduler tree */
1031         pi->port_state = ICE_SCHED_PORT_STATE_READY;
1032         mutex_init(&pi->sched_lock);
1033
1034 err_init_port:
1035         if (status && pi->root) {
1036                 ice_free_sched_node(pi, pi->root);
1037                 pi->root = NULL;
1038         }
1039
1040         devm_kfree(ice_hw_to_dev(hw), buf);
1041         return status;
1042 }
1043
1044 /**
1045  * ice_sched_query_res_alloc - query the FW for num of logical sched layers
1046  * @hw: pointer to the HW struct
1047  *
1048  * query FW for allocated scheduler resources and store in HW struct
1049  */
1050 enum ice_status ice_sched_query_res_alloc(struct ice_hw *hw)
1051 {
1052         struct ice_aqc_query_txsched_res_resp *buf;
1053         enum ice_status status = 0;
1054         __le16 max_sibl;
1055         u8 i;
1056
1057         if (hw->layer_info)
1058                 return status;
1059
1060         buf = devm_kzalloc(ice_hw_to_dev(hw), sizeof(*buf), GFP_KERNEL);
1061         if (!buf)
1062                 return ICE_ERR_NO_MEMORY;
1063
1064         status = ice_aq_query_sched_res(hw, sizeof(*buf), buf, NULL);
1065         if (status)
1066                 goto sched_query_out;
1067
1068         hw->num_tx_sched_layers = le16_to_cpu(buf->sched_props.logical_levels);
1069         hw->num_tx_sched_phys_layers =
1070                 le16_to_cpu(buf->sched_props.phys_levels);
1071         hw->flattened_layers = buf->sched_props.flattening_bitmap;
1072         hw->max_cgds = buf->sched_props.max_pf_cgds;
1073
1074         /* max sibling group size of current layer refers to the max children
1075          * of the below layer node.
1076          * layer 1 node max children will be layer 2 max sibling group size
1077          * layer 2 node max children will be layer 3 max sibling group size
1078          * and so on. This array will be populated from root (index 0) to
1079          * qgroup layer 7. Leaf node has no children.
1080          */
1081         for (i = 0; i < hw->num_tx_sched_layers; i++) {
1082                 max_sibl = buf->layer_props[i].max_sibl_grp_sz;
1083                 hw->max_children[i] = le16_to_cpu(max_sibl);
1084         }
1085
1086         hw->layer_info = devm_kmemdup(ice_hw_to_dev(hw), buf->layer_props,
1087                                       (hw->num_tx_sched_layers *
1088                                        sizeof(*hw->layer_info)),
1089                                       GFP_KERNEL);
1090         if (!hw->layer_info) {
1091                 status = ICE_ERR_NO_MEMORY;
1092                 goto sched_query_out;
1093         }
1094
1095 sched_query_out:
1096         devm_kfree(ice_hw_to_dev(hw), buf);
1097         return status;
1098 }
1099
1100 /**
1101  * ice_sched_find_node_in_subtree - Find node in part of base node subtree
1102  * @hw: pointer to the HW struct
1103  * @base: pointer to the base node
1104  * @node: pointer to the node to search
1105  *
1106  * This function checks whether a given node is part of the base node
1107  * subtree or not
1108  */
1109 static bool
1110 ice_sched_find_node_in_subtree(struct ice_hw *hw, struct ice_sched_node *base,
1111                                struct ice_sched_node *node)
1112 {
1113         u8 i;
1114
1115         for (i = 0; i < base->num_children; i++) {
1116                 struct ice_sched_node *child = base->children[i];
1117
1118                 if (node == child)
1119                         return true;
1120
1121                 if (child->tx_sched_layer > node->tx_sched_layer)
1122                         return false;
1123
1124                 /* this recursion is intentional, and wouldn't
1125                  * go more than 8 calls
1126                  */
1127                 if (ice_sched_find_node_in_subtree(hw, child, node))
1128                         return true;
1129         }
1130         return false;
1131 }
1132
1133 /**
1134  * ice_sched_get_free_qparent - Get a free LAN or RDMA queue group node
1135  * @pi: port information structure
1136  * @vsi_handle: software VSI handle
1137  * @tc: branch number
1138  * @owner: LAN or RDMA
1139  *
1140  * This function retrieves a free LAN or RDMA queue group node
1141  */
1142 struct ice_sched_node *
1143 ice_sched_get_free_qparent(struct ice_port_info *pi, u16 vsi_handle, u8 tc,
1144                            u8 owner)
1145 {
1146         struct ice_sched_node *vsi_node, *qgrp_node = NULL;
1147         struct ice_vsi_ctx *vsi_ctx;
1148         u16 max_children;
1149         u8 qgrp_layer;
1150
1151         qgrp_layer = ice_sched_get_qgrp_layer(pi->hw);
1152         max_children = pi->hw->max_children[qgrp_layer];
1153
1154         vsi_ctx = ice_get_vsi_ctx(pi->hw, vsi_handle);
1155         if (!vsi_ctx)
1156                 return NULL;
1157         vsi_node = vsi_ctx->sched.vsi_node[tc];
1158         /* validate invalid VSI ID */
1159         if (!vsi_node)
1160                 goto lan_q_exit;
1161
1162         /* get the first queue group node from VSI sub-tree */
1163         qgrp_node = ice_sched_get_first_node(pi->hw, vsi_node, qgrp_layer);
1164         while (qgrp_node) {
1165                 /* make sure the qgroup node is part of the VSI subtree */
1166                 if (ice_sched_find_node_in_subtree(pi->hw, vsi_node, qgrp_node))
1167                         if (qgrp_node->num_children < max_children &&
1168                             qgrp_node->owner == owner)
1169                                 break;
1170                 qgrp_node = qgrp_node->sibling;
1171         }
1172
1173 lan_q_exit:
1174         return qgrp_node;
1175 }
1176
1177 /**
1178  * ice_sched_get_vsi_node - Get a VSI node based on VSI ID
1179  * @hw: pointer to the HW struct
1180  * @tc_node: pointer to the TC node
1181  * @vsi_handle: software VSI handle
1182  *
1183  * This function retrieves a VSI node for a given VSI ID from a given
1184  * TC branch
1185  */
1186 static struct ice_sched_node *
1187 ice_sched_get_vsi_node(struct ice_hw *hw, struct ice_sched_node *tc_node,
1188                        u16 vsi_handle)
1189 {
1190         struct ice_sched_node *node;
1191         u8 vsi_layer;
1192
1193         vsi_layer = ice_sched_get_vsi_layer(hw);
1194         node = ice_sched_get_first_node(hw, tc_node, vsi_layer);
1195
1196         /* Check whether it already exists */
1197         while (node) {
1198                 if (node->vsi_handle == vsi_handle)
1199                         return node;
1200                 node = node->sibling;
1201         }
1202
1203         return node;
1204 }
1205
1206 /**
1207  * ice_sched_calc_vsi_child_nodes - calculate number of VSI child nodes
1208  * @hw: pointer to the HW struct
1209  * @num_qs: number of queues
1210  * @num_nodes: num nodes array
1211  *
1212  * This function calculates the number of VSI child nodes based on the
1213  * number of queues.
1214  */
1215 static void
1216 ice_sched_calc_vsi_child_nodes(struct ice_hw *hw, u16 num_qs, u16 *num_nodes)
1217 {
1218         u16 num = num_qs;
1219         u8 i, qgl, vsil;
1220
1221         qgl = ice_sched_get_qgrp_layer(hw);
1222         vsil = ice_sched_get_vsi_layer(hw);
1223
1224         /* calculate num nodes from queue group to VSI layer */
1225         for (i = qgl; i > vsil; i--) {
1226                 /* round to the next integer if there is a remainder */
1227                 num = DIV_ROUND_UP(num, hw->max_children[i]);
1228
1229                 /* need at least one node */
1230                 num_nodes[i] = num ? num : 1;
1231         }
1232 }
1233
1234 /**
1235  * ice_sched_add_vsi_child_nodes - add VSI child nodes to tree
1236  * @pi: port information structure
1237  * @vsi_handle: software VSI handle
1238  * @tc_node: pointer to the TC node
1239  * @num_nodes: pointer to the num nodes that needs to be added per layer
1240  * @owner: node owner (LAN or RDMA)
1241  *
1242  * This function adds the VSI child nodes to tree. It gets called for
1243  * LAN and RDMA separately.
1244  */
1245 static enum ice_status
1246 ice_sched_add_vsi_child_nodes(struct ice_port_info *pi, u16 vsi_handle,
1247                               struct ice_sched_node *tc_node, u16 *num_nodes,
1248                               u8 owner)
1249 {
1250         struct ice_sched_node *parent, *node;
1251         struct ice_hw *hw = pi->hw;
1252         enum ice_status status;
1253         u32 first_node_teid;
1254         u16 num_added = 0;
1255         u8 i, qgl, vsil;
1256
1257         qgl = ice_sched_get_qgrp_layer(hw);
1258         vsil = ice_sched_get_vsi_layer(hw);
1259         parent = ice_sched_get_vsi_node(hw, tc_node, vsi_handle);
1260         for (i = vsil + 1; i <= qgl; i++) {
1261                 if (!parent)
1262                         return ICE_ERR_CFG;
1263
1264                 status = ice_sched_add_nodes_to_layer(pi, tc_node, parent, i,
1265                                                       num_nodes[i],
1266                                                       &first_node_teid,
1267                                                       &num_added);
1268                 if (status || num_nodes[i] != num_added)
1269                         return ICE_ERR_CFG;
1270
1271                 /* The newly added node can be a new parent for the next
1272                  * layer nodes
1273                  */
1274                 if (num_added) {
1275                         parent = ice_sched_find_node_by_teid(tc_node,
1276                                                              first_node_teid);
1277                         node = parent;
1278                         while (node) {
1279                                 node->owner = owner;
1280                                 node = node->sibling;
1281                         }
1282                 } else {
1283                         parent = parent->children[0];
1284                 }
1285         }
1286
1287         return 0;
1288 }
1289
1290 /**
1291  * ice_sched_calc_vsi_support_nodes - calculate number of VSI support nodes
1292  * @hw: pointer to the HW struct
1293  * @tc_node: pointer to TC node
1294  * @num_nodes: pointer to num nodes array
1295  *
1296  * This function calculates the number of supported nodes needed to add this
1297  * VSI into Tx tree including the VSI, parent and intermediate nodes in below
1298  * layers
1299  */
1300 static void
1301 ice_sched_calc_vsi_support_nodes(struct ice_hw *hw,
1302                                  struct ice_sched_node *tc_node, u16 *num_nodes)
1303 {
1304         struct ice_sched_node *node;
1305         u8 vsil;
1306         int i;
1307
1308         vsil = ice_sched_get_vsi_layer(hw);
1309         for (i = vsil; i >= hw->sw_entry_point_layer; i--)
1310                 /* Add intermediate nodes if TC has no children and
1311                  * need at least one node for VSI
1312                  */
1313                 if (!tc_node->num_children || i == vsil) {
1314                         num_nodes[i]++;
1315                 } else {
1316                         /* If intermediate nodes are reached max children
1317                          * then add a new one.
1318                          */
1319                         node = ice_sched_get_first_node(hw, tc_node, (u8)i);
1320                         /* scan all the siblings */
1321                         while (node) {
1322                                 if (node->num_children < hw->max_children[i])
1323                                         break;
1324                                 node = node->sibling;
1325                         }
1326
1327                         /* tree has one intermediate node to add this new VSI.
1328                          * So no need to calculate supported nodes for below
1329                          * layers.
1330                          */
1331                         if (node)
1332                                 break;
1333                         /* all the nodes are full, allocate a new one */
1334                         num_nodes[i]++;
1335                 }
1336 }
1337
1338 /**
1339  * ice_sched_add_vsi_support_nodes - add VSI supported nodes into Tx tree
1340  * @pi: port information structure
1341  * @vsi_handle: software VSI handle
1342  * @tc_node: pointer to TC node
1343  * @num_nodes: pointer to num nodes array
1344  *
1345  * This function adds the VSI supported nodes into Tx tree including the
1346  * VSI, its parent and intermediate nodes in below layers
1347  */
1348 static enum ice_status
1349 ice_sched_add_vsi_support_nodes(struct ice_port_info *pi, u16 vsi_handle,
1350                                 struct ice_sched_node *tc_node, u16 *num_nodes)
1351 {
1352         struct ice_sched_node *parent = tc_node;
1353         enum ice_status status;
1354         u32 first_node_teid;
1355         u16 num_added = 0;
1356         u8 i, vsil;
1357
1358         if (!pi)
1359                 return ICE_ERR_PARAM;
1360
1361         vsil = ice_sched_get_vsi_layer(pi->hw);
1362         for (i = pi->hw->sw_entry_point_layer; i <= vsil; i++) {
1363                 status = ice_sched_add_nodes_to_layer(pi, tc_node, parent,
1364                                                       i, num_nodes[i],
1365                                                       &first_node_teid,
1366                                                       &num_added);
1367                 if (status || num_nodes[i] != num_added)
1368                         return ICE_ERR_CFG;
1369
1370                 /* The newly added node can be a new parent for the next
1371                  * layer nodes
1372                  */
1373                 if (num_added)
1374                         parent = ice_sched_find_node_by_teid(tc_node,
1375                                                              first_node_teid);
1376                 else
1377                         parent = parent->children[0];
1378
1379                 if (!parent)
1380                         return ICE_ERR_CFG;
1381
1382                 if (i == vsil)
1383                         parent->vsi_handle = vsi_handle;
1384         }
1385
1386         return 0;
1387 }
1388
1389 /**
1390  * ice_sched_add_vsi_to_topo - add a new VSI into tree
1391  * @pi: port information structure
1392  * @vsi_handle: software VSI handle
1393  * @tc: TC number
1394  *
1395  * This function adds a new VSI into scheduler tree
1396  */
1397 static enum ice_status
1398 ice_sched_add_vsi_to_topo(struct ice_port_info *pi, u16 vsi_handle, u8 tc)
1399 {
1400         u16 num_nodes[ICE_AQC_TOPO_MAX_LEVEL_NUM] = { 0 };
1401         struct ice_sched_node *tc_node;
1402         struct ice_hw *hw = pi->hw;
1403
1404         tc_node = ice_sched_get_tc_node(pi, tc);
1405         if (!tc_node)
1406                 return ICE_ERR_PARAM;
1407
1408         /* calculate number of supported nodes needed for this VSI */
1409         ice_sched_calc_vsi_support_nodes(hw, tc_node, num_nodes);
1410
1411         /* add VSI supported nodes to TC subtree */
1412         return ice_sched_add_vsi_support_nodes(pi, vsi_handle, tc_node,
1413                                                num_nodes);
1414 }
1415
1416 /**
1417  * ice_sched_update_vsi_child_nodes - update VSI child nodes
1418  * @pi: port information structure
1419  * @vsi_handle: software VSI handle
1420  * @tc: TC number
1421  * @new_numqs: new number of max queues
1422  * @owner: owner of this subtree
1423  *
1424  * This function updates the VSI child nodes based on the number of queues
1425  */
1426 static enum ice_status
1427 ice_sched_update_vsi_child_nodes(struct ice_port_info *pi, u16 vsi_handle,
1428                                  u8 tc, u16 new_numqs, u8 owner)
1429 {
1430         u16 new_num_nodes[ICE_AQC_TOPO_MAX_LEVEL_NUM] = { 0 };
1431         struct ice_sched_node *vsi_node;
1432         struct ice_sched_node *tc_node;
1433         struct ice_vsi_ctx *vsi_ctx;
1434         enum ice_status status = 0;
1435         struct ice_hw *hw = pi->hw;
1436         u16 prev_numqs;
1437
1438         tc_node = ice_sched_get_tc_node(pi, tc);
1439         if (!tc_node)
1440                 return ICE_ERR_CFG;
1441
1442         vsi_node = ice_sched_get_vsi_node(hw, tc_node, vsi_handle);
1443         if (!vsi_node)
1444                 return ICE_ERR_CFG;
1445
1446         vsi_ctx = ice_get_vsi_ctx(hw, vsi_handle);
1447         if (!vsi_ctx)
1448                 return ICE_ERR_PARAM;
1449
1450         prev_numqs = vsi_ctx->sched.max_lanq[tc];
1451         /* num queues are not changed or less than the previous number */
1452         if (new_numqs <= prev_numqs)
1453                 return status;
1454         status = ice_alloc_lan_q_ctx(hw, vsi_handle, tc, new_numqs);
1455         if (status)
1456                 return status;
1457
1458         if (new_numqs)
1459                 ice_sched_calc_vsi_child_nodes(hw, new_numqs, new_num_nodes);
1460         /* Keep the max number of queue configuration all the time. Update the
1461          * tree only if number of queues > previous number of queues. This may
1462          * leave some extra nodes in the tree if number of queues < previous
1463          * number but that wouldn't harm anything. Removing those extra nodes
1464          * may complicate the code if those nodes are part of SRL or
1465          * individually rate limited.
1466          */
1467         status = ice_sched_add_vsi_child_nodes(pi, vsi_handle, tc_node,
1468                                                new_num_nodes, owner);
1469         if (status)
1470                 return status;
1471         vsi_ctx->sched.max_lanq[tc] = new_numqs;
1472
1473         return 0;
1474 }
1475
1476 /**
1477  * ice_sched_cfg_vsi - configure the new/existing VSI
1478  * @pi: port information structure
1479  * @vsi_handle: software VSI handle
1480  * @tc: TC number
1481  * @maxqs: max number of queues
1482  * @owner: LAN or RDMA
1483  * @enable: TC enabled or disabled
1484  *
1485  * This function adds/updates VSI nodes based on the number of queues. If TC is
1486  * enabled and VSI is in suspended state then resume the VSI back. If TC is
1487  * disabled then suspend the VSI if it is not already.
1488  */
1489 enum ice_status
1490 ice_sched_cfg_vsi(struct ice_port_info *pi, u16 vsi_handle, u8 tc, u16 maxqs,
1491                   u8 owner, bool enable)
1492 {
1493         struct ice_sched_node *vsi_node, *tc_node;
1494         struct ice_vsi_ctx *vsi_ctx;
1495         enum ice_status status = 0;
1496         struct ice_hw *hw = pi->hw;
1497
1498         ice_debug(pi->hw, ICE_DBG_SCHED, "add/config VSI %d\n", vsi_handle);
1499         tc_node = ice_sched_get_tc_node(pi, tc);
1500         if (!tc_node)
1501                 return ICE_ERR_PARAM;
1502         vsi_ctx = ice_get_vsi_ctx(hw, vsi_handle);
1503         if (!vsi_ctx)
1504                 return ICE_ERR_PARAM;
1505         vsi_node = ice_sched_get_vsi_node(hw, tc_node, vsi_handle);
1506
1507         /* suspend the VSI if TC is not enabled */
1508         if (!enable) {
1509                 if (vsi_node && vsi_node->in_use) {
1510                         u32 teid = le32_to_cpu(vsi_node->info.node_teid);
1511
1512                         status = ice_sched_suspend_resume_elems(hw, 1, &teid,
1513                                                                 true);
1514                         if (!status)
1515                                 vsi_node->in_use = false;
1516                 }
1517                 return status;
1518         }
1519
1520         /* TC is enabled, if it is a new VSI then add it to the tree */
1521         if (!vsi_node) {
1522                 status = ice_sched_add_vsi_to_topo(pi, vsi_handle, tc);
1523                 if (status)
1524                         return status;
1525
1526                 vsi_node = ice_sched_get_vsi_node(hw, tc_node, vsi_handle);
1527                 if (!vsi_node)
1528                         return ICE_ERR_CFG;
1529
1530                 vsi_ctx->sched.vsi_node[tc] = vsi_node;
1531                 vsi_node->in_use = true;
1532                 /* invalidate the max queues whenever VSI gets added first time
1533                  * into the scheduler tree (boot or after reset). We need to
1534                  * recreate the child nodes all the time in these cases.
1535                  */
1536                 vsi_ctx->sched.max_lanq[tc] = 0;
1537         }
1538
1539         /* update the VSI child nodes */
1540         status = ice_sched_update_vsi_child_nodes(pi, vsi_handle, tc, maxqs,
1541                                                   owner);
1542         if (status)
1543                 return status;
1544
1545         /* TC is enabled, resume the VSI if it is in the suspend state */
1546         if (!vsi_node->in_use) {
1547                 u32 teid = le32_to_cpu(vsi_node->info.node_teid);
1548
1549                 status = ice_sched_suspend_resume_elems(hw, 1, &teid, false);
1550                 if (!status)
1551                         vsi_node->in_use = true;
1552         }
1553
1554         return status;
1555 }
1556
1557 /**
1558  * ice_sched_rm_agg_vsi_entry - remove aggregator related VSI info entry
1559  * @pi: port information structure
1560  * @vsi_handle: software VSI handle
1561  *
1562  * This function removes single aggregator VSI info entry from
1563  * aggregator list.
1564  */
1565 static void
1566 ice_sched_rm_agg_vsi_info(struct ice_port_info *pi, u16 vsi_handle)
1567 {
1568         struct ice_sched_agg_info *agg_info;
1569         struct ice_sched_agg_info *atmp;
1570
1571         list_for_each_entry_safe(agg_info, atmp, &pi->hw->agg_list,
1572                                  list_entry) {
1573                 struct ice_sched_agg_vsi_info *agg_vsi_info;
1574                 struct ice_sched_agg_vsi_info *vtmp;
1575
1576                 list_for_each_entry_safe(agg_vsi_info, vtmp,
1577                                          &agg_info->agg_vsi_list, list_entry)
1578                         if (agg_vsi_info->vsi_handle == vsi_handle) {
1579                                 list_del(&agg_vsi_info->list_entry);
1580                                 devm_kfree(ice_hw_to_dev(pi->hw),
1581                                            agg_vsi_info);
1582                                 return;
1583                         }
1584         }
1585 }
1586
1587 /**
1588  * ice_sched_is_leaf_node_present - check for a leaf node in the sub-tree
1589  * @node: pointer to the sub-tree node
1590  *
1591  * This function checks for a leaf node presence in a given sub-tree node.
1592  */
1593 static bool ice_sched_is_leaf_node_present(struct ice_sched_node *node)
1594 {
1595         u8 i;
1596
1597         for (i = 0; i < node->num_children; i++)
1598                 if (ice_sched_is_leaf_node_present(node->children[i]))
1599                         return true;
1600         /* check for a leaf node */
1601         return (node->info.data.elem_type == ICE_AQC_ELEM_TYPE_LEAF);
1602 }
1603
1604 /**
1605  * ice_sched_rm_vsi_cfg - remove the VSI and its children nodes
1606  * @pi: port information structure
1607  * @vsi_handle: software VSI handle
1608  * @owner: LAN or RDMA
1609  *
1610  * This function removes the VSI and its LAN or RDMA children nodes from the
1611  * scheduler tree.
1612  */
1613 static enum ice_status
1614 ice_sched_rm_vsi_cfg(struct ice_port_info *pi, u16 vsi_handle, u8 owner)
1615 {
1616         enum ice_status status = ICE_ERR_PARAM;
1617         struct ice_vsi_ctx *vsi_ctx;
1618         u8 i;
1619
1620         ice_debug(pi->hw, ICE_DBG_SCHED, "removing VSI %d\n", vsi_handle);
1621         if (!ice_is_vsi_valid(pi->hw, vsi_handle))
1622                 return status;
1623         mutex_lock(&pi->sched_lock);
1624         vsi_ctx = ice_get_vsi_ctx(pi->hw, vsi_handle);
1625         if (!vsi_ctx)
1626                 goto exit_sched_rm_vsi_cfg;
1627
1628         ice_for_each_traffic_class(i) {
1629                 struct ice_sched_node *vsi_node, *tc_node;
1630                 u8 j = 0;
1631
1632                 tc_node = ice_sched_get_tc_node(pi, i);
1633                 if (!tc_node)
1634                         continue;
1635
1636                 vsi_node = ice_sched_get_vsi_node(pi->hw, tc_node, vsi_handle);
1637                 if (!vsi_node)
1638                         continue;
1639
1640                 if (ice_sched_is_leaf_node_present(vsi_node)) {
1641                         ice_debug(pi->hw, ICE_DBG_SCHED,
1642                                   "VSI has leaf nodes in TC %d\n", i);
1643                         status = ICE_ERR_IN_USE;
1644                         goto exit_sched_rm_vsi_cfg;
1645                 }
1646                 while (j < vsi_node->num_children) {
1647                         if (vsi_node->children[j]->owner == owner) {
1648                                 ice_free_sched_node(pi, vsi_node->children[j]);
1649
1650                                 /* reset the counter again since the num
1651                                  * children will be updated after node removal
1652                                  */
1653                                 j = 0;
1654                         } else {
1655                                 j++;
1656                         }
1657                 }
1658                 /* remove the VSI if it has no children */
1659                 if (!vsi_node->num_children) {
1660                         ice_free_sched_node(pi, vsi_node);
1661                         vsi_ctx->sched.vsi_node[i] = NULL;
1662
1663                         /* clean up aggregator related VSI info if any */
1664                         ice_sched_rm_agg_vsi_info(pi, vsi_handle);
1665                 }
1666                 if (owner == ICE_SCHED_NODE_OWNER_LAN)
1667                         vsi_ctx->sched.max_lanq[i] = 0;
1668         }
1669         status = 0;
1670
1671 exit_sched_rm_vsi_cfg:
1672         mutex_unlock(&pi->sched_lock);
1673         return status;
1674 }
1675
1676 /**
1677  * ice_rm_vsi_lan_cfg - remove VSI and its LAN children nodes
1678  * @pi: port information structure
1679  * @vsi_handle: software VSI handle
1680  *
1681  * This function clears the VSI and its LAN children nodes from scheduler tree
1682  * for all TCs.
1683  */
1684 enum ice_status ice_rm_vsi_lan_cfg(struct ice_port_info *pi, u16 vsi_handle)
1685 {
1686         return ice_sched_rm_vsi_cfg(pi, vsi_handle, ICE_SCHED_NODE_OWNER_LAN);
1687 }