drivers/net/ethernet/marvell/octeontx2/nic/qos.c

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