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