// SPDX-License-Identifier: GPL-2.0
/* Marvell RVU Ethernet driver
 *
 * Copyright (C) 2023 Marvell.
 *
 */
#include <linux/netdevice.h>
#include <linux/etherdevice.h>
#include <linux/inetdevice.h>
#include <linux/bitfield.h>

#include "otx2_common.h"
#include "cn10k.h"
#include "qos.h"

#define OTX2_QOS_QID_INNER              0xFFFFU
#define OTX2_QOS_QID_NONE               0xFFFEU
#define OTX2_QOS_ROOT_CLASSID           0xFFFFFFFF
#define OTX2_QOS_CLASS_NONE             0
#define OTX2_QOS_DEFAULT_PRIO           0xF
#define OTX2_QOS_INVALID_SQ             0xFFFF
#define OTX2_QOS_INVALID_TXSCHQ_IDX     0xFFFF
#define CN10K_MAX_RR_WEIGHT             GENMASK_ULL(13, 0)
#define OTX2_MAX_RR_QUANTUM             GENMASK_ULL(23, 0)

static void otx2_qos_update_tx_netdev_queues(struct otx2_nic *pfvf)
{
        struct otx2_hw *hw = &pfvf->hw;
        int tx_queues, qos_txqs, err;

        qos_txqs = bitmap_weight(pfvf->qos.qos_sq_bmap,
                                 OTX2_QOS_MAX_LEAF_NODES);

        tx_queues = hw->tx_queues + qos_txqs;

        err = netif_set_real_num_tx_queues(pfvf->netdev, tx_queues);
        if (err) {
                netdev_err(pfvf->netdev,
                           "Failed to set no of Tx queues: %d\n", tx_queues);
                return;
        }
}

static void otx2_qos_get_regaddr(struct otx2_qos_node *node,
                                 struct nix_txschq_config *cfg,
                                 int index)
{
        if (node->level == NIX_TXSCH_LVL_SMQ) {
                cfg->reg[index++] = NIX_AF_MDQX_PARENT(node->schq);
                cfg->reg[index++] = NIX_AF_MDQX_SCHEDULE(node->schq);
                cfg->reg[index++] = NIX_AF_MDQX_PIR(node->schq);
                cfg->reg[index]   = NIX_AF_MDQX_CIR(node->schq);
        } else if (node->level == NIX_TXSCH_LVL_TL4) {
                cfg->reg[index++] = NIX_AF_TL4X_PARENT(node->schq);
                cfg->reg[index++] = NIX_AF_TL4X_SCHEDULE(node->schq);
                cfg->reg[index++] = NIX_AF_TL4X_PIR(node->schq);
                cfg->reg[index]   = NIX_AF_TL4X_CIR(node->schq);
        } else if (node->level == NIX_TXSCH_LVL_TL3) {
                cfg->reg[index++] = NIX_AF_TL3X_PARENT(node->schq);
                cfg->reg[index++] = NIX_AF_TL3X_SCHEDULE(node->schq);
                cfg->reg[index++] = NIX_AF_TL3X_PIR(node->schq);
                cfg->reg[index]   = NIX_AF_TL3X_CIR(node->schq);
        } else if (node->level == NIX_TXSCH_LVL_TL2) {
                cfg->reg[index++] = NIX_AF_TL2X_PARENT(node->schq);
                cfg->reg[index++] = NIX_AF_TL2X_SCHEDULE(node->schq);
                cfg->reg[index++] = NIX_AF_TL2X_PIR(node->schq);
                cfg->reg[index]   = NIX_AF_TL2X_CIR(node->schq);
        }
}

static int otx2_qos_quantum_to_dwrr_weight(struct otx2_nic *pfvf, u32 quantum)
{
        u32 weight;

        weight = quantum / pfvf->hw.dwrr_mtu;
        if (quantum % pfvf->hw.dwrr_mtu)
                weight += 1;

        return weight;
}

static void otx2_config_sched_shaping(struct otx2_nic *pfvf,
                                      struct otx2_qos_node *node,
                                      struct nix_txschq_config *cfg,
                                      int *num_regs)
{
        u32 rr_weight;
        u32 quantum;
        u64 maxrate;

        otx2_qos_get_regaddr(node, cfg, *num_regs);

        /* configure parent txschq */
        cfg->regval[*num_regs] = node->parent->schq << 16;
        (*num_regs)++;

        /* configure prio/quantum */
        if (node->qid == OTX2_QOS_QID_NONE) {
                cfg->regval[*num_regs] =  node->prio << 24 |
                                          mtu_to_dwrr_weight(pfvf, pfvf->tx_max_pktlen);
                (*num_regs)++;
                return;
        }

        /* configure priority/quantum  */
        if (node->is_static) {
                cfg->regval[*num_regs] =
                        (node->schq - node->parent->prio_anchor) << 24;
        } else {
                quantum = node->quantum ?
                          node->quantum : pfvf->tx_max_pktlen;
                rr_weight = otx2_qos_quantum_to_dwrr_weight(pfvf, quantum);
                cfg->regval[*num_regs] = node->parent->child_dwrr_prio << 24 |
                                         rr_weight;
        }
        (*num_regs)++;

        /* configure PIR */
        maxrate = (node->rate > node->ceil) ? node->rate : node->ceil;

        cfg->regval[*num_regs] =
                otx2_get_txschq_rate_regval(pfvf, maxrate, 65536);
        (*num_regs)++;

        /* Don't configure CIR when both CIR+PIR not supported
         * On 96xx, CIR + PIR + RED_ALGO=STALL causes deadlock
         */
        if (!test_bit(QOS_CIR_PIR_SUPPORT, &pfvf->hw.cap_flag))
                return;

        cfg->regval[*num_regs] =
                otx2_get_txschq_rate_regval(pfvf, node->rate, 65536);
        (*num_regs)++;
}

static void __otx2_qos_txschq_cfg(struct otx2_nic *pfvf,
                                  struct otx2_qos_node *node,
                                  struct nix_txschq_config *cfg)
{
        struct otx2_hw *hw = &pfvf->hw;
        int num_regs = 0;
        u8 level;

        level = node->level;

        /* program txschq registers */
        if (level == NIX_TXSCH_LVL_SMQ) {
                cfg->reg[num_regs] = NIX_AF_SMQX_CFG(node->schq);
                cfg->regval[num_regs] = ((u64)pfvf->tx_max_pktlen << 8) |
                                        OTX2_MIN_MTU;
                cfg->regval[num_regs] |= (0x20ULL << 51) | (0x80ULL << 39) |
                                         (0x2ULL << 36);
                num_regs++;

                otx2_config_sched_shaping(pfvf, node, cfg, &num_regs);
        } else if (level == NIX_TXSCH_LVL_TL4) {
                otx2_config_sched_shaping(pfvf, node, cfg, &num_regs);
        } else if (level == NIX_TXSCH_LVL_TL3) {
                /* configure link cfg */
                if (level == pfvf->qos.link_cfg_lvl) {
                        cfg->reg[num_regs] = NIX_AF_TL3_TL2X_LINKX_CFG(node->schq, hw->tx_link);
                        cfg->regval[num_regs] = BIT_ULL(13) | BIT_ULL(12);
                        num_regs++;
                }

                otx2_config_sched_shaping(pfvf, node, cfg, &num_regs);
        } else if (level == NIX_TXSCH_LVL_TL2) {
                /* configure parent txschq */
                cfg->reg[num_regs] = NIX_AF_TL2X_PARENT(node->schq);
                cfg->regval[num_regs] = (u64)hw->tx_link << 16;
                num_regs++;

                /* configure link cfg */
                if (level == pfvf->qos.link_cfg_lvl) {
                        cfg->reg[num_regs] = NIX_AF_TL3_TL2X_LINKX_CFG(node->schq, hw->tx_link);
                        cfg->regval[num_regs] = BIT_ULL(13) | BIT_ULL(12);
                        num_regs++;
                }

                /* check if node is root */
                if (node->qid == OTX2_QOS_QID_INNER && !node->parent) {
                        cfg->reg[num_regs] = NIX_AF_TL2X_SCHEDULE(node->schq);
                        cfg->regval[num_regs] =  (u64)hw->txschq_aggr_lvl_rr_prio << 24 |
                                                 mtu_to_dwrr_weight(pfvf,
                                                                    pfvf->tx_max_pktlen);
                        num_regs++;
                        goto txschq_cfg_out;
                }

                otx2_config_sched_shaping(pfvf, node, cfg, &num_regs);
        }

txschq_cfg_out:
        cfg->num_regs = num_regs;
}

static int otx2_qos_txschq_set_parent_topology(struct otx2_nic *pfvf,
                                               struct otx2_qos_node *parent)
{
        struct mbox *mbox = &pfvf->mbox;
        struct nix_txschq_config *cfg;
        int rc;

        if (parent->level == NIX_TXSCH_LVL_MDQ)
                return 0;

        mutex_lock(&mbox->lock);

        cfg = otx2_mbox_alloc_msg_nix_txschq_cfg(&pfvf->mbox);
        if (!cfg) {
                mutex_unlock(&mbox->lock);
                return -ENOMEM;
        }

        cfg->lvl = parent->level;

        if (parent->level == NIX_TXSCH_LVL_TL4)
                cfg->reg[0] = NIX_AF_TL4X_TOPOLOGY(parent->schq);
        else if (parent->level == NIX_TXSCH_LVL_TL3)
                cfg->reg[0] = NIX_AF_TL3X_TOPOLOGY(parent->schq);
        else if (parent->level == NIX_TXSCH_LVL_TL2)
                cfg->reg[0] = NIX_AF_TL2X_TOPOLOGY(parent->schq);
        else if (parent->level == NIX_TXSCH_LVL_TL1)
                cfg->reg[0] = NIX_AF_TL1X_TOPOLOGY(parent->schq);

        cfg->regval[0] = (u64)parent->prio_anchor << 32;
        cfg->regval[0] |= ((parent->child_dwrr_prio != OTX2_QOS_DEFAULT_PRIO) ?
                            parent->child_dwrr_prio : 0)  << 1;
        cfg->num_regs++;

        rc = otx2_sync_mbox_msg(&pfvf->mbox);

        mutex_unlock(&mbox->lock);

        return rc;
}

static void otx2_qos_free_hw_node_schq(struct otx2_nic *pfvf,
                                       struct otx2_qos_node *parent)
{
        struct otx2_qos_node *node;

        list_for_each_entry_reverse(node, &parent->child_schq_list, list)
                otx2_txschq_free_one(pfvf, node->level, node->schq);
}

static void otx2_qos_free_hw_node(struct otx2_nic *pfvf,
                                  struct otx2_qos_node *parent)
{
        struct otx2_qos_node *node, *tmp;

        list_for_each_entry_safe(node, tmp, &parent->child_list, list) {
                otx2_qos_free_hw_node(pfvf, node);
                otx2_qos_free_hw_node_schq(pfvf, node);
                otx2_txschq_free_one(pfvf, node->level, node->schq);
        }
}

static void otx2_qos_free_hw_cfg(struct otx2_nic *pfvf,
                                 struct otx2_qos_node *node)
{
        mutex_lock(&pfvf->qos.qos_lock);

        /* free child node hw mappings */
        otx2_qos_free_hw_node(pfvf, node);
        otx2_qos_free_hw_node_schq(pfvf, node);

        /* free node hw mappings */
        otx2_txschq_free_one(pfvf, node->level, node->schq);

        mutex_unlock(&pfvf->qos.qos_lock);
}

static void otx2_qos_sw_node_delete(struct otx2_nic *pfvf,
                                    struct otx2_qos_node *node)
{
        hash_del_rcu(&node->hlist);

        if (node->qid != OTX2_QOS_QID_INNER && node->qid != OTX2_QOS_QID_NONE) {
                __clear_bit(node->qid, pfvf->qos.qos_sq_bmap);
                otx2_qos_update_tx_netdev_queues(pfvf);
        }

        list_del(&node->list);
        kfree(node);
}

static void otx2_qos_free_sw_node_schq(struct otx2_nic *pfvf,
                                       struct otx2_qos_node *parent)
{
        struct otx2_qos_node *node, *tmp;

        list_for_each_entry_safe(node, tmp, &parent->child_schq_list, list) {
                list_del(&node->list);
                kfree(node);
        }
}

static void __otx2_qos_free_sw_node(struct otx2_nic *pfvf,
                                    struct otx2_qos_node *parent)
{
        struct otx2_qos_node *node, *tmp;

        list_for_each_entry_safe(node, tmp, &parent->child_list, list) {
                __otx2_qos_free_sw_node(pfvf, node);
                otx2_qos_free_sw_node_schq(pfvf, node);
                otx2_qos_sw_node_delete(pfvf, node);
        }
}

static void otx2_qos_free_sw_node(struct otx2_nic *pfvf,
                                  struct otx2_qos_node *node)
{
        mutex_lock(&pfvf->qos.qos_lock);

        __otx2_qos_free_sw_node(pfvf, node);
        otx2_qos_free_sw_node_schq(pfvf, node);
        otx2_qos_sw_node_delete(pfvf, node);

        mutex_unlock(&pfvf->qos.qos_lock);
}

static void otx2_qos_destroy_node(struct otx2_nic *pfvf,
                                  struct otx2_qos_node *node)
{
        otx2_qos_free_hw_cfg(pfvf, node);
        otx2_qos_free_sw_node(pfvf, node);
}

static void otx2_qos_fill_cfg_schq(struct otx2_qos_node *parent,
                                   struct otx2_qos_cfg *cfg)
{
        struct otx2_qos_node *node;

        list_for_each_entry(node, &parent->child_schq_list, list)
                cfg->schq[node->level]++;
}

static void otx2_qos_fill_cfg_tl(struct otx2_qos_node *parent,
                                 struct otx2_qos_cfg *cfg)
{
        struct otx2_qos_node *node;

        list_for_each_entry(node, &parent->child_list, list) {
                otx2_qos_fill_cfg_tl(node, cfg);
                otx2_qos_fill_cfg_schq(node, cfg);
        }

        /* Assign the required number of transmit schedular queues under the
         * given class
         */
        cfg->schq_contig[parent->level - 1] += parent->child_dwrr_cnt +
                                               parent->max_static_prio + 1;
}

static void otx2_qos_prepare_txschq_cfg(struct otx2_nic *pfvf,
                                        struct otx2_qos_node *parent,
                                        struct otx2_qos_cfg *cfg)
{
        mutex_lock(&pfvf->qos.qos_lock);
        otx2_qos_fill_cfg_tl(parent, cfg);
        mutex_unlock(&pfvf->qos.qos_lock);
}

static void otx2_qos_read_txschq_cfg_schq(struct otx2_qos_node *parent,
                                          struct otx2_qos_cfg *cfg)
{
        struct otx2_qos_node *node;
        int cnt;

        list_for_each_entry(node, &parent->child_schq_list, list) {
                cnt = cfg->dwrr_node_pos[node->level];
                cfg->schq_list[node->level][cnt] = node->schq;
                cfg->schq[node->level]++;
                cfg->dwrr_node_pos[node->level]++;
        }
}

static void otx2_qos_read_txschq_cfg_tl(struct otx2_qos_node *parent,
                                        struct otx2_qos_cfg *cfg)
{
        struct otx2_qos_node *node;
        int cnt;

        list_for_each_entry(node, &parent->child_list, list) {
                otx2_qos_read_txschq_cfg_tl(node, cfg);
                cnt = cfg->static_node_pos[node->level];
                cfg->schq_contig_list[node->level][cnt] = node->schq;
                cfg->schq_index_used[node->level][cnt] = true;
                cfg->schq_contig[node->level]++;
                cfg->static_node_pos[node->level]++;
                otx2_qos_read_txschq_cfg_schq(node, cfg);
        }
}

static void otx2_qos_read_txschq_cfg(struct otx2_nic *pfvf,
                                     struct otx2_qos_node *node,
                                     struct otx2_qos_cfg *cfg)
{
        mutex_lock(&pfvf->qos.qos_lock);
        otx2_qos_read_txschq_cfg_tl(node, cfg);
        mutex_unlock(&pfvf->qos.qos_lock);
}

static struct otx2_qos_node *
otx2_qos_alloc_root(struct otx2_nic *pfvf)
{
        struct otx2_qos_node *node;

        node = kzalloc_obj(*node);
        if (!node)
                return ERR_PTR(-ENOMEM);

        node->parent = NULL;
        if (!is_otx2_vf(pfvf->pcifunc)) {
                node->level = NIX_TXSCH_LVL_TL1;
        } else {
                node->level = NIX_TXSCH_LVL_TL2;
                node->child_dwrr_prio = OTX2_QOS_DEFAULT_PRIO;
        }

        WRITE_ONCE(node->qid, OTX2_QOS_QID_INNER);
        node->classid = OTX2_QOS_ROOT_CLASSID;

        hash_add_rcu(pfvf->qos.qos_hlist, &node->hlist, node->classid);
        list_add_tail(&node->list, &pfvf->qos.qos_tree);
        INIT_LIST_HEAD(&node->child_list);
        INIT_LIST_HEAD(&node->child_schq_list);

        return node;
}

static int otx2_qos_add_child_node(struct otx2_qos_node *parent,
                                   struct otx2_qos_node *node)
{
        struct list_head *head = &parent->child_list;
        struct otx2_qos_node *tmp_node;
        struct list_head *tmp;

        if (node->prio > parent->max_static_prio)
                parent->max_static_prio = node->prio;

        for (tmp = head->next; tmp != head; tmp = tmp->next) {
                tmp_node = list_entry(tmp, struct otx2_qos_node, list);
                if (tmp_node->prio == node->prio &&
                    tmp_node->is_static)
                        return -EEXIST;
                if (tmp_node->prio > node->prio) {
                        list_add_tail(&node->list, tmp);
                        return 0;
                }
        }

        list_add_tail(&node->list, head);
        return 0;
}

static int otx2_qos_alloc_txschq_node(struct otx2_nic *pfvf,
                                      struct otx2_qos_node *node)
{
        struct otx2_qos_node *txschq_node, *parent, *tmp;
        int lvl;

        parent = node;
        for (lvl = node->level - 1; lvl >= NIX_TXSCH_LVL_MDQ; lvl--) {
                txschq_node = kzalloc_obj(*txschq_node);
                if (!txschq_node)
                        goto err_out;

                txschq_node->parent = parent;
                txschq_node->level = lvl;
                txschq_node->classid = OTX2_QOS_CLASS_NONE;
                WRITE_ONCE(txschq_node->qid, OTX2_QOS_QID_NONE);
                txschq_node->rate = 0;
                txschq_node->ceil = 0;
                txschq_node->prio = 0;
                txschq_node->quantum = 0;
                txschq_node->is_static = true;
                txschq_node->child_dwrr_prio = OTX2_QOS_DEFAULT_PRIO;
                txschq_node->txschq_idx = OTX2_QOS_INVALID_TXSCHQ_IDX;

                mutex_lock(&pfvf->qos.qos_lock);
                list_add_tail(&txschq_node->list, &node->child_schq_list);
                mutex_unlock(&pfvf->qos.qos_lock);

                INIT_LIST_HEAD(&txschq_node->child_list);
                INIT_LIST_HEAD(&txschq_node->child_schq_list);
                parent = txschq_node;
        }

        return 0;

err_out:
        list_for_each_entry_safe(txschq_node, tmp, &node->child_schq_list,
                                 list) {
                list_del(&txschq_node->list);
                kfree(txschq_node);
        }
        return -ENOMEM;
}

static struct otx2_qos_node *
otx2_qos_sw_create_leaf_node(struct otx2_nic *pfvf,
                             struct otx2_qos_node *parent,
                             u16 classid, u32 prio, u64 rate, u64 ceil,
                             u32 quantum, u16 qid, bool static_cfg)
{
        struct otx2_qos_node *node;
        int err;

        node = kzalloc_obj(*node);
        if (!node)
                return ERR_PTR(-ENOMEM);

        node->parent = parent;
        node->level = parent->level - 1;
        node->classid = classid;
        WRITE_ONCE(node->qid, qid);

        node->rate = otx2_convert_rate(rate);
        node->ceil = otx2_convert_rate(ceil);
        node->prio = prio;
        node->quantum = quantum;
        node->is_static = static_cfg;
        node->child_dwrr_prio = OTX2_QOS_DEFAULT_PRIO;
        node->txschq_idx = OTX2_QOS_INVALID_TXSCHQ_IDX;

        __set_bit(qid, pfvf->qos.qos_sq_bmap);

        hash_add_rcu(pfvf->qos.qos_hlist, &node->hlist, classid);

        mutex_lock(&pfvf->qos.qos_lock);
        err = otx2_qos_add_child_node(parent, node);
        if (err) {
                mutex_unlock(&pfvf->qos.qos_lock);
                return ERR_PTR(err);
        }
        mutex_unlock(&pfvf->qos.qos_lock);

        INIT_LIST_HEAD(&node->child_list);
        INIT_LIST_HEAD(&node->child_schq_list);

        err = otx2_qos_alloc_txschq_node(pfvf, node);
        if (err) {
                otx2_qos_sw_node_delete(pfvf, node);
                return ERR_PTR(-ENOMEM);
        }

        return node;
}

static struct otx2_qos_node
*otx2_sw_node_find_by_qid(struct otx2_nic *pfvf, u16 qid)
{
        struct otx2_qos_node *node = NULL;
        int bkt;

        hash_for_each(pfvf->qos.qos_hlist, bkt, node, hlist) {
                if (node->qid == qid)
                        break;
        }

        return node;
}

static struct otx2_qos_node *
otx2_sw_node_find(struct otx2_nic *pfvf, u32 classid)
{
        struct otx2_qos_node *node = NULL;

        hash_for_each_possible(pfvf->qos.qos_hlist, node, hlist, classid) {
                if (node->classid == classid)
                        break;
        }

        return node;
}

static struct otx2_qos_node *
otx2_sw_node_find_rcu(struct otx2_nic *pfvf, u32 classid)
{
        struct otx2_qos_node *node = NULL;

        hash_for_each_possible_rcu(pfvf->qos.qos_hlist, node, hlist, classid) {
                if (node->classid == classid)
                        break;
        }

        return node;
}

int otx2_get_txq_by_classid(struct otx2_nic *pfvf, u16 classid)
{
        struct otx2_qos_node *node;
        u16 qid;
        int res;

        node = otx2_sw_node_find_rcu(pfvf, classid);
        if (!node) {
                res = -ENOENT;
                goto out;
        }
        qid = READ_ONCE(node->qid);
        if (qid == OTX2_QOS_QID_INNER) {
                res = -EINVAL;
                goto out;
        }
        res = pfvf->hw.tx_queues + qid;
out:
        return res;
}

static int
otx2_qos_txschq_config(struct otx2_nic *pfvf, struct otx2_qos_node *node)
{
        struct mbox *mbox = &pfvf->mbox;
        struct nix_txschq_config *req;
        int rc;

        mutex_lock(&mbox->lock);

        req = otx2_mbox_alloc_msg_nix_txschq_cfg(&pfvf->mbox);
        if (!req) {
                mutex_unlock(&mbox->lock);
                return -ENOMEM;
        }

        req->lvl = node->level;
        __otx2_qos_txschq_cfg(pfvf, node, req);

        rc = otx2_sync_mbox_msg(&pfvf->mbox);

        mutex_unlock(&mbox->lock);

        return rc;
}

static int otx2_qos_txschq_alloc(struct otx2_nic *pfvf,
                                 struct otx2_qos_cfg *cfg)
{
        struct nix_txsch_alloc_req *req;
        struct nix_txsch_alloc_rsp *rsp;
        struct mbox *mbox = &pfvf->mbox;
        int lvl, rc, schq;

        mutex_lock(&mbox->lock);
        req = otx2_mbox_alloc_msg_nix_txsch_alloc(&pfvf->mbox);
        if (!req) {
                mutex_unlock(&mbox->lock);
                return -ENOMEM;
        }

        for (lvl = 0; lvl < NIX_TXSCH_LVL_CNT; lvl++) {
                req->schq[lvl] = cfg->schq[lvl];
                req->schq_contig[lvl] = cfg->schq_contig[lvl];
        }

        rc = otx2_sync_mbox_msg(&pfvf->mbox);
        if (rc) {
                mutex_unlock(&mbox->lock);
                return rc;
        }

        rsp = (struct nix_txsch_alloc_rsp *)
              otx2_mbox_get_rsp(&pfvf->mbox.mbox, 0, &req->hdr);

        if (IS_ERR(rsp)) {
                rc = PTR_ERR(rsp);
                goto out;
        }

        for (lvl = 0; lvl < NIX_TXSCH_LVL_CNT; lvl++) {
                for (schq = 0; schq < rsp->schq_contig[lvl]; schq++) {
                        cfg->schq_contig_list[lvl][schq] =
                                rsp->schq_contig_list[lvl][schq];
                }
        }

        for (lvl = 0; lvl < NIX_TXSCH_LVL_CNT; lvl++) {
                for (schq = 0; schq < rsp->schq[lvl]; schq++) {
                        cfg->schq_list[lvl][schq] =
                                rsp->schq_list[lvl][schq];
                }
        }

        pfvf->qos.link_cfg_lvl = rsp->link_cfg_lvl;
        pfvf->hw.txschq_aggr_lvl_rr_prio = rsp->aggr_lvl_rr_prio;

out:
        mutex_unlock(&mbox->lock);
        return rc;
}

static void otx2_qos_free_unused_txschq(struct otx2_nic *pfvf,
                                        struct otx2_qos_cfg *cfg)
{
        int lvl, idx, schq;

        for (lvl = 0; lvl < NIX_TXSCH_LVL_CNT; lvl++) {
                for (idx = 0; idx < cfg->schq_contig[lvl]; idx++) {
                        if (!cfg->schq_index_used[lvl][idx]) {
                                schq = cfg->schq_contig_list[lvl][idx];
                                otx2_txschq_free_one(pfvf, lvl, schq);
                        }
                }
        }
}

static void otx2_qos_txschq_fill_cfg_schq(struct otx2_nic *pfvf,
                                          struct otx2_qos_node *node,
                                          struct otx2_qos_cfg *cfg)
{
        struct otx2_qos_node *tmp;
        int cnt;

        list_for_each_entry(tmp, &node->child_schq_list, list) {
                cnt = cfg->dwrr_node_pos[tmp->level];
                tmp->schq = cfg->schq_list[tmp->level][cnt];
                cfg->dwrr_node_pos[tmp->level]++;
        }
}

static void otx2_qos_txschq_fill_cfg_tl(struct otx2_nic *pfvf,
                                        struct otx2_qos_node *node,
                                        struct otx2_qos_cfg *cfg)
{
        struct otx2_qos_node *tmp;
        int cnt;

        list_for_each_entry(tmp, &node->child_list, list) {
                otx2_qos_txschq_fill_cfg_tl(pfvf, tmp, cfg);
                cnt = cfg->static_node_pos[tmp->level];
                tmp->schq = cfg->schq_contig_list[tmp->level][tmp->txschq_idx];
                cfg->schq_index_used[tmp->level][tmp->txschq_idx] = true;
                if (cnt == 0)
                        node->prio_anchor =
                                cfg->schq_contig_list[tmp->level][0];
                cfg->static_node_pos[tmp->level]++;
                otx2_qos_txschq_fill_cfg_schq(pfvf, tmp, cfg);
        }
}

static void otx2_qos_txschq_fill_cfg(struct otx2_nic *pfvf,
                                     struct otx2_qos_node *node,
                                     struct otx2_qos_cfg *cfg)
{
        mutex_lock(&pfvf->qos.qos_lock);
        otx2_qos_txschq_fill_cfg_tl(pfvf, node, cfg);
        otx2_qos_txschq_fill_cfg_schq(pfvf, node, cfg);
        otx2_qos_free_unused_txschq(pfvf, cfg);
        mutex_unlock(&pfvf->qos.qos_lock);
}

static void __otx2_qos_assign_base_idx_tl(struct otx2_nic *pfvf,
                                          struct otx2_qos_node *tmp,
                                          unsigned long *child_idx_bmap,
                                          int child_cnt)
{
        int idx;

        if (tmp->txschq_idx != OTX2_QOS_INVALID_TXSCHQ_IDX)
                return;

        /* assign static nodes 1:1 prio mapping first, then remaining nodes */
        for (idx = 0; idx < child_cnt; idx++) {
                if (tmp->is_static && tmp->prio == idx &&
                    !test_bit(idx, child_idx_bmap)) {
                        tmp->txschq_idx = idx;
                        set_bit(idx, child_idx_bmap);
                        return;
                } else if (!tmp->is_static && idx >= tmp->prio &&
                           !test_bit(idx, child_idx_bmap)) {
                        tmp->txschq_idx = idx;
                        set_bit(idx, child_idx_bmap);
                        return;
                }
        }
}

static int otx2_qos_assign_base_idx_tl(struct otx2_nic *pfvf,
                                       struct otx2_qos_node *node)
{
        unsigned long *child_idx_bmap;
        struct otx2_qos_node *tmp;
        int child_cnt;

        list_for_each_entry(tmp, &node->child_list, list)
                tmp->txschq_idx = OTX2_QOS_INVALID_TXSCHQ_IDX;

        /* allocate child index array */
        child_cnt = node->child_dwrr_cnt + node->max_static_prio + 1;
        child_idx_bmap = kcalloc(BITS_TO_LONGS(child_cnt),
                                 sizeof(unsigned long),
                                 GFP_KERNEL);
        if (!child_idx_bmap)
                return -ENOMEM;

        list_for_each_entry(tmp, &node->child_list, list)
                otx2_qos_assign_base_idx_tl(pfvf, tmp);

        /* assign base index of static priority children first */
        list_for_each_entry(tmp, &node->child_list, list) {
                if (!tmp->is_static)
                        continue;
                __otx2_qos_assign_base_idx_tl(pfvf, tmp, child_idx_bmap,
                                              child_cnt);
        }

        /* assign base index of dwrr priority children */
        list_for_each_entry(tmp, &node->child_list, list)
                __otx2_qos_assign_base_idx_tl(pfvf, tmp, child_idx_bmap,
                                              child_cnt);

        kfree(child_idx_bmap);

        return 0;
}

static int otx2_qos_assign_base_idx(struct otx2_nic *pfvf,
                                    struct otx2_qos_node *node)
{
        int ret = 0;

        mutex_lock(&pfvf->qos.qos_lock);
        ret = otx2_qos_assign_base_idx_tl(pfvf, node);
        mutex_unlock(&pfvf->qos.qos_lock);

        return ret;
}

static int otx2_qos_txschq_push_cfg_schq(struct otx2_nic *pfvf,
                                         struct otx2_qos_node *node,
                                         struct otx2_qos_cfg *cfg)
{
        struct otx2_qos_node *tmp;
        int ret;

        list_for_each_entry(tmp, &node->child_schq_list, list) {
                ret = otx2_qos_txschq_config(pfvf, tmp);
                if (ret)
                        return -EIO;
                ret = otx2_qos_txschq_set_parent_topology(pfvf, tmp->parent);
                if (ret)
                        return -EIO;
        }

        return 0;
}

static int otx2_qos_txschq_push_cfg_tl(struct otx2_nic *pfvf,
                                       struct otx2_qos_node *node,
                                       struct otx2_qos_cfg *cfg)
{
        struct otx2_qos_node *tmp;
        int ret;

        list_for_each_entry(tmp, &node->child_list, list) {
                ret = otx2_qos_txschq_push_cfg_tl(pfvf, tmp, cfg);
                if (ret)
                        return -EIO;
                ret = otx2_qos_txschq_config(pfvf, tmp);
                if (ret)
                        return -EIO;
                ret = otx2_qos_txschq_push_cfg_schq(pfvf, tmp, cfg);
                if (ret)
                        return -EIO;
        }

        ret = otx2_qos_txschq_set_parent_topology(pfvf, node);
        if (ret)
                return -EIO;

        return 0;
}

static int otx2_qos_txschq_push_cfg(struct otx2_nic *pfvf,
                                    struct otx2_qos_node *node,
                                    struct otx2_qos_cfg *cfg)
{
        int ret;

        mutex_lock(&pfvf->qos.qos_lock);
        ret = otx2_qos_txschq_push_cfg_tl(pfvf, node, cfg);
        if (ret)
                goto out;
        ret = otx2_qos_txschq_push_cfg_schq(pfvf, node, cfg);
out:
        mutex_unlock(&pfvf->qos.qos_lock);
        return ret;
}

static int otx2_qos_txschq_update_config(struct otx2_nic *pfvf,
                                         struct otx2_qos_node *node,
                                         struct otx2_qos_cfg *cfg)
{
        otx2_qos_txschq_fill_cfg(pfvf, node, cfg);

        return otx2_qos_txschq_push_cfg(pfvf, node, cfg);
}

static int otx2_qos_txschq_update_root_cfg(struct otx2_nic *pfvf,
                                           struct otx2_qos_node *root,
                                           struct otx2_qos_cfg *cfg)
{
        root->schq = cfg->schq_list[root->level][0];
        return otx2_qos_txschq_config(pfvf, root);
}

static void otx2_qos_free_cfg(struct otx2_nic *pfvf, struct otx2_qos_cfg *cfg)
{
        int lvl, idx, schq;

        for (lvl = 0; lvl < NIX_TXSCH_LVL_CNT; lvl++) {
                for (idx = 0; idx < cfg->schq[lvl]; idx++) {
                        schq = cfg->schq_list[lvl][idx];
                        otx2_txschq_free_one(pfvf, lvl, schq);
                }
        }

        for (lvl = 0; lvl < NIX_TXSCH_LVL_CNT; lvl++) {
                for (idx = 0; idx < cfg->schq_contig[lvl]; idx++) {
                        if (cfg->schq_index_used[lvl][idx]) {
                                schq = cfg->schq_contig_list[lvl][idx];
                                otx2_txschq_free_one(pfvf, lvl, schq);
                        }
                }
        }
}

static void otx2_qos_enadis_sq(struct otx2_nic *pfvf,
                               struct otx2_qos_node *node,
                               u16 qid)
{
        if (pfvf->qos.qid_to_sqmap[qid] != OTX2_QOS_INVALID_SQ)
                otx2_qos_disable_sq(pfvf, qid);

        pfvf->qos.qid_to_sqmap[qid] = node->schq;
        otx2_qos_txschq_config(pfvf, node);
        otx2_qos_enable_sq(pfvf, qid);
}

static void otx2_qos_update_smq_schq(struct otx2_nic *pfvf,
                                     struct otx2_qos_node *node,
                                     bool action)
{
        struct otx2_qos_node *tmp;

        if (node->qid == OTX2_QOS_QID_INNER)
                return;

        list_for_each_entry(tmp, &node->child_schq_list, list) {
                if (tmp->level == NIX_TXSCH_LVL_MDQ) {
                        if (action == QOS_SMQ_FLUSH)
                                otx2_smq_flush(pfvf, tmp->schq);
                        else
                                otx2_qos_enadis_sq(pfvf, tmp, node->qid);
                }
        }
}

static void __otx2_qos_update_smq(struct otx2_nic *pfvf,
                                  struct otx2_qos_node *node,
                                  bool action)
{
        struct otx2_qos_node *tmp;

        list_for_each_entry(tmp, &node->child_list, list) {
                __otx2_qos_update_smq(pfvf, tmp, action);
                if (tmp->qid == OTX2_QOS_QID_INNER)
                        continue;
                if (tmp->level == NIX_TXSCH_LVL_MDQ) {
                        if (action == QOS_SMQ_FLUSH)
                                otx2_smq_flush(pfvf, tmp->schq);
                        else
                                otx2_qos_enadis_sq(pfvf, tmp, tmp->qid);
                } else {
                        otx2_qos_update_smq_schq(pfvf, tmp, action);
                }
        }
}

static void otx2_qos_update_smq(struct otx2_nic *pfvf,
                                struct otx2_qos_node *node,
                                bool action)
{
        mutex_lock(&pfvf->qos.qos_lock);
        __otx2_qos_update_smq(pfvf, node, action);
        otx2_qos_update_smq_schq(pfvf, node, action);
        mutex_unlock(&pfvf->qos.qos_lock);
}

static int otx2_qos_push_txschq_cfg(struct otx2_nic *pfvf,
                                    struct otx2_qos_node *node,
                                    struct otx2_qos_cfg *cfg)
{
        int ret;

        ret = otx2_qos_txschq_alloc(pfvf, cfg);
        if (ret)
                return -ENOSPC;

        ret = otx2_qos_assign_base_idx(pfvf, node);
        if (ret)
                return -ENOMEM;

        if (!(pfvf->netdev->flags & IFF_UP)) {
                otx2_qos_txschq_fill_cfg(pfvf, node, cfg);
                return 0;
        }

        ret = otx2_qos_txschq_update_config(pfvf, node, cfg);
        if (ret) {
                otx2_qos_free_cfg(pfvf, cfg);
                return -EIO;
        }

        otx2_qos_update_smq(pfvf, node, QOS_CFG_SQ);

        return 0;
}

static int otx2_qos_update_tree(struct otx2_nic *pfvf,
                                struct otx2_qos_node *node,
                                struct otx2_qos_cfg *cfg)
{
        otx2_qos_prepare_txschq_cfg(pfvf, node->parent, cfg);
        return otx2_qos_push_txschq_cfg(pfvf, node->parent, cfg);
}

static int otx2_qos_root_add(struct otx2_nic *pfvf, u16 htb_maj_id, u16 htb_defcls,
                             struct netlink_ext_ack *extack)
{
        struct otx2_qos_cfg *new_cfg;
        struct otx2_qos_node *root;
        int err;

        netdev_dbg(pfvf->netdev,
                   "TC_HTB_CREATE: handle=0x%x defcls=0x%x\n",
                   htb_maj_id, htb_defcls);

        root = otx2_qos_alloc_root(pfvf);
        if (IS_ERR(root)) {
                err = PTR_ERR(root);
                return err;
        }

        /* allocate txschq queue */
        new_cfg = kzalloc_obj(*new_cfg);
        if (!new_cfg) {
                NL_SET_ERR_MSG_MOD(extack, "Memory allocation error");
                err = -ENOMEM;
                goto free_root_node;
        }
        /* allocate htb root node */
        new_cfg->schq[root->level] = 1;
        err = otx2_qos_txschq_alloc(pfvf, new_cfg);
        if (err) {
                NL_SET_ERR_MSG_MOD(extack, "Error allocating txschq");
                goto free_root_node;
        }

        /* Update TL1 RR PRIO */
        if (root->level == NIX_TXSCH_LVL_TL1) {
                root->child_dwrr_prio = pfvf->hw.txschq_aggr_lvl_rr_prio;
                netdev_dbg(pfvf->netdev,
                           "TL1 DWRR Priority %d\n", root->child_dwrr_prio);
        }

        if (!(pfvf->netdev->flags & IFF_UP) ||
            root->level == NIX_TXSCH_LVL_TL1) {
                root->schq = new_cfg->schq_list[root->level][0];
                goto out;
        }

        /* update the txschq configuration in hw */
        err = otx2_qos_txschq_update_root_cfg(pfvf, root, new_cfg);
        if (err) {
                NL_SET_ERR_MSG_MOD(extack,
                                   "Error updating txschq configuration");
                goto txschq_free;
        }

out:
        WRITE_ONCE(pfvf->qos.defcls, htb_defcls);
        /* Pairs with smp_load_acquire() in ndo_select_queue */
        smp_store_release(&pfvf->qos.maj_id, htb_maj_id);
        kfree(new_cfg);
        return 0;

txschq_free:
        otx2_qos_free_cfg(pfvf, new_cfg);
free_root_node:
        kfree(new_cfg);
        otx2_qos_sw_node_delete(pfvf, root);
        return err;
}

static int otx2_qos_root_destroy(struct otx2_nic *pfvf)
{
        struct otx2_qos_node *root;

        netdev_dbg(pfvf->netdev, "TC_HTB_DESTROY\n");

        /* find root node */
        root = otx2_sw_node_find(pfvf, OTX2_QOS_ROOT_CLASSID);
        if (!root)
                return -ENOENT;

        /* free the hw mappings */
        otx2_qos_destroy_node(pfvf, root);

        return 0;
}

static int otx2_qos_validate_quantum(struct otx2_nic *pfvf, u32 quantum)
{
        u32 rr_weight = otx2_qos_quantum_to_dwrr_weight(pfvf, quantum);
        int err = 0;

        /* Max Round robin weight supported by octeontx2 and CN10K
         * is different. Validate accordingly
         */
        if (is_dev_otx2(pfvf->pdev))
                err = (rr_weight > OTX2_MAX_RR_QUANTUM) ? -EINVAL : 0;
        else if (rr_weight > CN10K_MAX_RR_WEIGHT)
                err = -EINVAL;

        return err;
}

static int otx2_qos_validate_dwrr_cfg(struct otx2_qos_node *parent,
                                      struct netlink_ext_ack *extack,
                                      struct otx2_nic *pfvf,
                                      u64 prio, u64 quantum)
{
        int err;

        err = otx2_qos_validate_quantum(pfvf, quantum);
        if (err) {
                NL_SET_ERR_MSG_MOD(extack, "Unsupported quantum value");
                return err;
        }

        if (parent->child_dwrr_prio == OTX2_QOS_DEFAULT_PRIO) {
                parent->child_dwrr_prio = prio;
        } else if (prio != parent->child_dwrr_prio) {
                NL_SET_ERR_MSG_MOD(extack, "Only one DWRR group is allowed");
                return -EOPNOTSUPP;
        }

        return 0;
}

static int otx2_qos_validate_configuration(struct otx2_qos_node *parent,
                                           struct netlink_ext_ack *extack,
                                           struct otx2_nic *pfvf,
                                           u64 prio, bool static_cfg)
{
        if (prio == parent->child_dwrr_prio && static_cfg) {
                NL_SET_ERR_MSG_MOD(extack, "DWRR child group with same priority exists");
                return -EEXIST;
        }

        if (static_cfg && test_bit(prio, parent->prio_bmap)) {
                NL_SET_ERR_MSG_MOD(extack,
                                   "Static priority child with same priority exists");
                return -EEXIST;
        }

        return 0;
}

static void otx2_reset_dwrr_prio(struct otx2_qos_node *parent, u64 prio)
{
        /* For PF, root node dwrr priority is static */
        if (parent->level == NIX_TXSCH_LVL_TL1)
                return;

        if (parent->child_dwrr_prio != OTX2_QOS_DEFAULT_PRIO) {
                parent->child_dwrr_prio = OTX2_QOS_DEFAULT_PRIO;
                clear_bit(prio, parent->prio_bmap);
        }
}

static bool is_qos_node_dwrr(struct otx2_qos_node *parent,
                             struct otx2_nic *pfvf,
                             u64 prio)
{
        struct otx2_qos_node *node;
        bool ret = false;

        if (parent->child_dwrr_prio == prio)
                return true;

        mutex_lock(&pfvf->qos.qos_lock);
        list_for_each_entry(node, &parent->child_list, list) {
                if (prio == node->prio) {
                        if (parent->child_dwrr_prio != OTX2_QOS_DEFAULT_PRIO &&
                            parent->child_dwrr_prio != prio)
                                continue;

                        if (otx2_qos_validate_quantum(pfvf, node->quantum)) {
                                netdev_err(pfvf->netdev,
                                           "Unsupported quantum value for existing classid=0x%x quantum=%d prio=%d",
                                            node->classid, node->quantum,
                                            node->prio);
                                break;
                        }
                        /* mark old node as dwrr */
                        node->is_static = false;
                        parent->child_dwrr_cnt++;
                        parent->child_static_cnt--;
                        ret = true;
                        break;
                }
        }
        mutex_unlock(&pfvf->qos.qos_lock);

        return ret;
}

static int otx2_qos_leaf_alloc_queue(struct otx2_nic *pfvf, u16 classid,
                                     u32 parent_classid, u64 rate, u64 ceil,
                                     u64 prio, u32 quantum,
                                     struct netlink_ext_ack *extack)
{
        struct otx2_qos_cfg *old_cfg, *new_cfg;
        struct otx2_qos_node *node, *parent;
        int qid, ret, err;
        bool static_cfg;

        netdev_dbg(pfvf->netdev,
                   "TC_HTB_LEAF_ALLOC_QUEUE: classid=0x%x parent_classid=0x%x rate=%lld ceil=%lld prio=%lld quantum=%d\n",
                   classid, parent_classid, rate, ceil, prio, quantum);

        if (prio > OTX2_QOS_MAX_PRIO) {
                NL_SET_ERR_MSG_MOD(extack, "Valid priority range 0 to 7");
                ret = -EOPNOTSUPP;
                goto out;
        }

        if (!quantum || quantum > INT_MAX) {
                NL_SET_ERR_MSG_MOD(extack, "Invalid quantum, range 1 - 2147483647 bytes");
                ret = -EOPNOTSUPP;
                goto out;
        }

        /* get parent node */
        parent = otx2_sw_node_find(pfvf, parent_classid);
        if (!parent) {
                NL_SET_ERR_MSG_MOD(extack, "parent node not found");
                ret = -ENOENT;
                goto out;
        }
        if (parent->level == NIX_TXSCH_LVL_MDQ) {
                NL_SET_ERR_MSG_MOD(extack, "HTB qos max levels reached");
                ret = -EOPNOTSUPP;
                goto out;
        }

        static_cfg = !is_qos_node_dwrr(parent, pfvf, prio);
        ret = otx2_qos_validate_configuration(parent, extack, pfvf, prio,
                                              static_cfg);
        if (ret)
                goto out;

        if (!static_cfg) {
                ret = otx2_qos_validate_dwrr_cfg(parent, extack, pfvf, prio,
                                                 quantum);
                if (ret)
                        goto out;
        }

        if (static_cfg)
                parent->child_static_cnt++;
        else
                parent->child_dwrr_cnt++;

        set_bit(prio, parent->prio_bmap);

        /* read current txschq configuration */
        old_cfg = kzalloc_obj(*old_cfg);
        if (!old_cfg) {
                NL_SET_ERR_MSG_MOD(extack, "Memory allocation error");
                ret = -ENOMEM;
                goto reset_prio;
        }
        otx2_qos_read_txschq_cfg(pfvf, parent, old_cfg);

        /* allocate a new sq */
        qid = otx2_qos_get_qid(pfvf);
        if (qid < 0) {
                NL_SET_ERR_MSG_MOD(extack, "Reached max supported QOS SQ's");
                ret = -ENOMEM;
                goto free_old_cfg;
        }

        /* Actual SQ mapping will be updated after SMQ alloc */
        pfvf->qos.qid_to_sqmap[qid] = OTX2_QOS_INVALID_SQ;

        /* allocate and initialize a new child node */
        node = otx2_qos_sw_create_leaf_node(pfvf, parent, classid, prio, rate,
                                            ceil, quantum, qid, static_cfg);
        if (IS_ERR(node)) {
                NL_SET_ERR_MSG_MOD(extack, "Unable to allocate leaf node");
                ret = PTR_ERR(node);
                goto free_old_cfg;
        }

        /* push new txschq config to hw */
        new_cfg = kzalloc_obj(*new_cfg);
        if (!new_cfg) {
                NL_SET_ERR_MSG_MOD(extack, "Memory allocation error");
                ret = -ENOMEM;
                goto free_node;
        }
        ret = otx2_qos_update_tree(pfvf, node, new_cfg);
        if (ret) {
                NL_SET_ERR_MSG_MOD(extack, "HTB HW configuration error");
                kfree(new_cfg);
                otx2_qos_sw_node_delete(pfvf, node);
                /* restore the old qos tree */
                err = otx2_qos_txschq_update_config(pfvf, parent, old_cfg);
                if (err) {
                        netdev_err(pfvf->netdev,
                                   "Failed to restore txcshq configuration");
                        goto free_old_cfg;
                }

                otx2_qos_update_smq(pfvf, parent, QOS_CFG_SQ);
                goto free_old_cfg;
        }

        /* update tx_real_queues */
        otx2_qos_update_tx_netdev_queues(pfvf);

        /* free new txschq config */
        kfree(new_cfg);

        /* free old txschq config */
        otx2_qos_free_cfg(pfvf, old_cfg);
        kfree(old_cfg);

        return pfvf->hw.tx_queues + qid;

free_node:
        otx2_qos_sw_node_delete(pfvf, node);
free_old_cfg:
        kfree(old_cfg);
reset_prio:
        if (static_cfg)
                parent->child_static_cnt--;
        else
                parent->child_dwrr_cnt--;

        clear_bit(prio, parent->prio_bmap);
out:
        return ret;
}

static int otx2_qos_leaf_to_inner(struct otx2_nic *pfvf, u16 classid,
                                  u16 child_classid, u64 rate, u64 ceil, u64 prio,
                                  u32 quantum, struct netlink_ext_ack *extack)
{
        struct otx2_qos_cfg *old_cfg, *new_cfg;
        struct otx2_qos_node *node, *child;
        bool static_cfg;
        int ret, err;
        u16 qid;

        netdev_dbg(pfvf->netdev,
                   "TC_HTB_LEAF_TO_INNER classid %04x, child %04x, rate %llu, ceil %llu\n",
                   classid, child_classid, rate, ceil);

        if (prio > OTX2_QOS_MAX_PRIO) {
                NL_SET_ERR_MSG_MOD(extack, "Valid priority range 0 to 7");
                ret = -EOPNOTSUPP;
                goto out;
        }

        if (!quantum || quantum > INT_MAX) {
                NL_SET_ERR_MSG_MOD(extack, "Invalid quantum, range 1 - 2147483647 bytes");
                ret = -EOPNOTSUPP;
                goto out;
        }

        /* find node related to classid */
        node = otx2_sw_node_find(pfvf, classid);
        if (!node) {
                NL_SET_ERR_MSG_MOD(extack, "HTB node not found");
                ret = -ENOENT;
                goto out;
        }
        /* check max qos txschq level */
        if (node->level == NIX_TXSCH_LVL_MDQ) {
                NL_SET_ERR_MSG_MOD(extack, "HTB qos level not supported");
                ret = -EOPNOTSUPP;
                goto out;
        }

        static_cfg = !is_qos_node_dwrr(node, pfvf, prio);
        if (!static_cfg) {
                ret = otx2_qos_validate_dwrr_cfg(node, extack, pfvf, prio,
                                                 quantum);
                if (ret)
                        goto out;
        }

        if (static_cfg)
                node->child_static_cnt++;
        else
                node->child_dwrr_cnt++;

        set_bit(prio, node->prio_bmap);

        /* store the qid to assign to leaf node */
        qid = node->qid;

        /* read current txschq configuration */
        old_cfg = kzalloc_obj(*old_cfg);
        if (!old_cfg) {
                NL_SET_ERR_MSG_MOD(extack, "Memory allocation error");
                ret = -ENOMEM;
                goto reset_prio;
        }
        otx2_qos_read_txschq_cfg(pfvf, node, old_cfg);

        /* delete the txschq nodes allocated for this node */
        otx2_qos_disable_sq(pfvf, qid);
        otx2_qos_free_hw_node_schq(pfvf, node);
        otx2_qos_free_sw_node_schq(pfvf, node);
        pfvf->qos.qid_to_sqmap[qid] = OTX2_QOS_INVALID_SQ;

        /* mark this node as htb inner node */
        WRITE_ONCE(node->qid, OTX2_QOS_QID_INNER);

        /* allocate and initialize a new child node */
        child = otx2_qos_sw_create_leaf_node(pfvf, node, child_classid,
                                             prio, rate, ceil, quantum,
                                             qid, static_cfg);
        if (IS_ERR(child)) {
                NL_SET_ERR_MSG_MOD(extack, "Unable to allocate leaf node");
                ret = PTR_ERR(child);
                goto free_old_cfg;
        }

        /* push new txschq config to hw */
        new_cfg = kzalloc_obj(*new_cfg);
        if (!new_cfg) {
                NL_SET_ERR_MSG_MOD(extack, "Memory allocation error");
                ret = -ENOMEM;
                goto free_node;
        }
        ret = otx2_qos_update_tree(pfvf, child, new_cfg);
        if (ret) {
                NL_SET_ERR_MSG_MOD(extack, "HTB HW configuration error");
                kfree(new_cfg);
                otx2_qos_sw_node_delete(pfvf, child);
                /* restore the old qos tree */
                WRITE_ONCE(node->qid, qid);
                err = otx2_qos_alloc_txschq_node(pfvf, node);
                if (err) {
                        netdev_err(pfvf->netdev,
                                   "Failed to restore old leaf node");
                        goto free_old_cfg;
                }
                err = otx2_qos_txschq_update_config(pfvf, node, old_cfg);
                if (err) {
                        netdev_err(pfvf->netdev,
                                   "Failed to restore txcshq configuration");
                        goto free_old_cfg;
                }
                otx2_qos_update_smq(pfvf, node, QOS_CFG_SQ);
                goto free_old_cfg;
        }

        /* free new txschq config */
        kfree(new_cfg);

        /* free old txschq config */
        otx2_qos_free_cfg(pfvf, old_cfg);
        kfree(old_cfg);

        return 0;

free_node:
        otx2_qos_sw_node_delete(pfvf, child);
free_old_cfg:
        kfree(old_cfg);
reset_prio:
        if (static_cfg)
                node->child_static_cnt--;
        else
                node->child_dwrr_cnt--;
        clear_bit(prio, node->prio_bmap);
out:
        return ret;
}

static int otx2_qos_cur_leaf_nodes(struct otx2_nic *pfvf)
{
        int last = find_last_bit(pfvf->qos.qos_sq_bmap, pfvf->hw.tc_tx_queues);

        return last ==  pfvf->hw.tc_tx_queues ? 0 : last + 1;
}

static void otx2_reset_qdisc(struct net_device *dev, u16 qid)
{
        struct netdev_queue *dev_queue = netdev_get_tx_queue(dev, qid);
        struct Qdisc *qdisc = rtnl_dereference(dev_queue->qdisc_sleeping);

        if (!qdisc)
                return;

        spin_lock_bh(qdisc_lock(qdisc));
        qdisc_reset(qdisc);
        spin_unlock_bh(qdisc_lock(qdisc));
}

static void otx2_cfg_smq(struct otx2_nic *pfvf, struct otx2_qos_node *node,
                         int qid)
{
        struct otx2_qos_node *tmp;

        list_for_each_entry(tmp, &node->child_schq_list, list)
                if (tmp->level == NIX_TXSCH_LVL_MDQ) {
                        otx2_qos_txschq_config(pfvf, tmp);
                        pfvf->qos.qid_to_sqmap[qid] = tmp->schq;
                }
}

static int otx2_qos_leaf_del(struct otx2_nic *pfvf, u16 *classid,
                             struct netlink_ext_ack *extack)
{
        struct otx2_qos_node *node, *parent;
        int dwrr_del_node = false;
        u16 qid, moved_qid;
        u64 prio;

        netdev_dbg(pfvf->netdev, "TC_HTB_LEAF_DEL classid %04x\n", *classid);

        /* find node related to classid */
        node = otx2_sw_node_find(pfvf, *classid);
        if (!node) {
                NL_SET_ERR_MSG_MOD(extack, "HTB node not found");
                return -ENOENT;
        }
        parent = node->parent;
        prio   = node->prio;
        qid    = node->qid;

        if (!node->is_static)
                dwrr_del_node = true;

        otx2_qos_disable_sq(pfvf, node->qid);

        otx2_qos_destroy_node(pfvf, node);
        pfvf->qos.qid_to_sqmap[qid] = OTX2_QOS_INVALID_SQ;

        if (dwrr_del_node) {
                parent->child_dwrr_cnt--;
        } else {
                parent->child_static_cnt--;
                clear_bit(prio, parent->prio_bmap);
        }

        /* Reset DWRR priority if all dwrr nodes are deleted */
        if (!parent->child_dwrr_cnt)
                otx2_reset_dwrr_prio(parent, prio);

        if (!parent->child_static_cnt)
                parent->max_static_prio = 0;

        moved_qid = otx2_qos_cur_leaf_nodes(pfvf);

        /* last node just deleted */
        if (moved_qid == 0 || moved_qid == qid)
                return 0;

        moved_qid--;

        node = otx2_sw_node_find_by_qid(pfvf, moved_qid);
        if (!node)
                return 0;

        /* stop traffic to the old queue and disable
         * SQ associated with it
         */
        node->qid =  OTX2_QOS_QID_INNER;
        __clear_bit(moved_qid, pfvf->qos.qos_sq_bmap);
        otx2_qos_disable_sq(pfvf, moved_qid);

        otx2_reset_qdisc(pfvf->netdev, pfvf->hw.tx_queues + moved_qid);

        /* enable SQ associated with qid and
         * update the node
         */
        otx2_cfg_smq(pfvf, node, qid);

        otx2_qos_enable_sq(pfvf, qid);
        __set_bit(qid, pfvf->qos.qos_sq_bmap);
        node->qid = qid;

        *classid = node->classid;
        return 0;
}

static int otx2_qos_leaf_del_last(struct otx2_nic *pfvf, u16 classid, bool force,
                                  struct netlink_ext_ack *extack)
{
        struct otx2_qos_node *node, *parent;
        struct otx2_qos_cfg *new_cfg;
        int dwrr_del_node = false;
        u64 prio;
        int err;
        u16 qid;

        netdev_dbg(pfvf->netdev,
                   "TC_HTB_LEAF_DEL_LAST classid %04x\n", classid);

        /* find node related to classid */
        node = otx2_sw_node_find(pfvf, classid);
        if (!node) {
                NL_SET_ERR_MSG_MOD(extack, "HTB node not found");
                return -ENOENT;
        }

        /* save qid for use by parent */
        qid = node->qid;
        prio = node->prio;

        parent = otx2_sw_node_find(pfvf, node->parent->classid);
        if (!parent) {
                NL_SET_ERR_MSG_MOD(extack, "parent node not found");
                return -ENOENT;
        }

        if (!node->is_static)
                dwrr_del_node = true;

        WRITE_ONCE(node->qid, OTX2_QOS_QID_INNER);
        /* destroy the leaf node */
        otx2_qos_disable_sq(pfvf, qid);
        otx2_qos_destroy_node(pfvf, node);
        pfvf->qos.qid_to_sqmap[qid] = OTX2_QOS_INVALID_SQ;

        if (dwrr_del_node) {
                parent->child_dwrr_cnt--;
        } else {
                parent->child_static_cnt--;
                clear_bit(prio, parent->prio_bmap);
        }

        /* Reset DWRR priority if all dwrr nodes are deleted */
        if (!parent->child_dwrr_cnt)
                otx2_reset_dwrr_prio(parent, prio);

        if (!parent->child_static_cnt)
                parent->max_static_prio = 0;

        /* create downstream txschq entries to parent */
        err = otx2_qos_alloc_txschq_node(pfvf, parent);
        if (err) {
                NL_SET_ERR_MSG_MOD(extack, "HTB failed to create txsch configuration");
                return err;
        }
        WRITE_ONCE(parent->qid, qid);
        __set_bit(qid, pfvf->qos.qos_sq_bmap);

        /* push new txschq config to hw */
        new_cfg = kzalloc_obj(*new_cfg);
        if (!new_cfg) {
                NL_SET_ERR_MSG_MOD(extack, "Memory allocation error");
                return -ENOMEM;
        }
        /* fill txschq cfg and push txschq cfg to hw */
        otx2_qos_fill_cfg_schq(parent, new_cfg);
        err = otx2_qos_push_txschq_cfg(pfvf, parent, new_cfg);
        if (err) {
                NL_SET_ERR_MSG_MOD(extack, "HTB HW configuration error");
                kfree(new_cfg);
                return err;
        }
        kfree(new_cfg);

        return 0;
}

void otx2_clean_qos_queues(struct otx2_nic *pfvf)
{
        struct otx2_qos_node *root;

        root = otx2_sw_node_find(pfvf, OTX2_QOS_ROOT_CLASSID);
        if (!root)
                return;

        otx2_qos_update_smq(pfvf, root, QOS_SMQ_FLUSH);
}

void otx2_qos_config_txschq(struct otx2_nic *pfvf)
{
        struct otx2_qos_node *root;
        int err;

        root = otx2_sw_node_find(pfvf, OTX2_QOS_ROOT_CLASSID);
        if (!root)
                return;

        if (root->level != NIX_TXSCH_LVL_TL1) {
                err = otx2_qos_txschq_config(pfvf, root);
                if (err) {
                        netdev_err(pfvf->netdev, "Error update txschq configuration\n");
                        goto root_destroy;
                }
        }

        err = otx2_qos_txschq_push_cfg_tl(pfvf, root, NULL);
        if (err) {
                netdev_err(pfvf->netdev, "Error update txschq configuration\n");
                goto root_destroy;
        }

        otx2_qos_update_smq(pfvf, root, QOS_CFG_SQ);
        return;

root_destroy:
        netdev_err(pfvf->netdev, "Failed to update Scheduler/Shaping config in Hardware\n");
        /* Free resources allocated */
        otx2_qos_root_destroy(pfvf);
}

int otx2_setup_tc_htb(struct net_device *ndev, struct tc_htb_qopt_offload *htb)
{
        struct otx2_nic *pfvf = netdev_priv(ndev);
        int res;

        switch (htb->command) {
        case TC_HTB_CREATE:
                return otx2_qos_root_add(pfvf, htb->parent_classid,
                                         htb->classid, htb->extack);
        case TC_HTB_DESTROY:
                return otx2_qos_root_destroy(pfvf);
        case TC_HTB_LEAF_ALLOC_QUEUE:
                res = otx2_qos_leaf_alloc_queue(pfvf, htb->classid,
                                                htb->parent_classid,
                                                htb->rate, htb->ceil,
                                                htb->prio, htb->quantum,
                                                htb->extack);
                if (res < 0)
                        return res;
                htb->qid = res;
                return 0;
        case TC_HTB_LEAF_TO_INNER:
                return otx2_qos_leaf_to_inner(pfvf, htb->parent_classid,
                                              htb->classid, htb->rate,
                                              htb->ceil, htb->prio,
                                              htb->quantum, htb->extack);
        case TC_HTB_LEAF_DEL:
                return otx2_qos_leaf_del(pfvf, &htb->classid, htb->extack);
        case TC_HTB_LEAF_DEL_LAST:
        case TC_HTB_LEAF_DEL_LAST_FORCE:
                return otx2_qos_leaf_del_last(pfvf, htb->classid,
                                htb->command == TC_HTB_LEAF_DEL_LAST_FORCE,
                                              htb->extack);
        case TC_HTB_LEAF_QUERY_QUEUE:
                res = otx2_get_txq_by_classid(pfvf, htb->classid);
                htb->qid = res;
                return 0;
        case TC_HTB_NODE_MODIFY:
                fallthrough;
        default:
                return -EOPNOTSUPP;
        }
}