core/en/htb.c

0001 // SPDX-License-Identifier: GPL-2.0 OR Linux-OpenIB
0002 /* Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES. All rights reserved. */
0003
0004 #include <net/pkt_cls.h>
0005 #include "htb.h"
0006 #include "en.h"
0007 #include "../qos.h"
0008
0009 struct mlx5e_qos_node {
0010     struct hlist_node hnode;
0011     struct mlx5e_qos_node *parent;
0012     u64 rate;
0013     u32 bw_share;
0014     u32 max_average_bw;
0015     u32 hw_id;
0016     u32 classid; /* 16-bit, except root. */
0017     u16 qid;
0018 };
0019
0020 struct mlx5e_htb {
0021     DECLARE_HASHTABLE(qos_tc2node, order_base_2(MLX5E_QOS_MAX_LEAF_NODES));
0022     DECLARE_BITMAP(qos_used_qids, MLX5E_QOS_MAX_LEAF_NODES);
0023     struct mlx5_core_dev *mdev;
0024     struct net_device *netdev;
0025     struct mlx5e_priv *priv;
0026     struct mlx5e_selq *selq;
0027 };
0028
0029 #define MLX5E_QOS_QID_INNER 0xffff
0030 #define MLX5E_HTB_CLASSID_ROOT 0xffffffff
0031
0032 /* Software representation of the QoS tree */
0033
0034 int mlx5e_htb_enumerate_leaves(struct mlx5e_htb *htb, mlx5e_fp_htb_enumerate callback, void *data)
0035 {
0036     struct mlx5e_qos_node *node = NULL;
0037     int bkt, err;
0038
0039     hash_for_each(htb->qos_tc2node, bkt, node, hnode) {
0040         if (node->qid == MLX5E_QOS_QID_INNER)
0041             continue;
0042         err = callback(data, node->qid, node->hw_id);
0043         if (err)
0044             return err;
0045     }
0046     return 0;
0047 }
0048
0049 int mlx5e_htb_cur_leaf_nodes(struct mlx5e_htb *htb)
0050 {
0051     int last;
0052
0053     last = find_last_bit(htb->qos_used_qids, mlx5e_qos_max_leaf_nodes(htb->mdev));
0054     return last == mlx5e_qos_max_leaf_nodes(htb->mdev) ? 0 : last + 1;
0055 }
0056
0057 static int mlx5e_htb_find_unused_qos_qid(struct mlx5e_htb *htb)
0058 {
0059     int size = mlx5e_qos_max_leaf_nodes(htb->mdev);
0060     struct mlx5e_priv *priv = htb->priv;
0061     int res;
0062
0063     WARN_ONCE(!mutex_is_locked(&priv->state_lock), "%s: state_lock is not held\n", __func__);
0064     res = find_first_zero_bit(htb->qos_used_qids, size);
0065
0066     return res == size ? -ENOSPC : res;
0067 }
0068
0069 static struct mlx5e_qos_node *
0070 mlx5e_htb_node_create_leaf(struct mlx5e_htb *htb, u16 classid, u16 qid,
0071                struct mlx5e_qos_node *parent)
0072 {
0073     struct mlx5e_qos_node *node;
0074
0075     node = kzalloc(sizeof(*node), GFP_KERNEL);
0076     if (!node)
0077         return ERR_PTR(-ENOMEM);
0078
0079     node->parent = parent;
0080
0081     node->qid = qid;
0082     __set_bit(qid, htb->qos_used_qids);
0083
0084     node->classid = classid;
0085     hash_add_rcu(htb->qos_tc2node, &node->hnode, classid);
0086
0087     mlx5e_update_tx_netdev_queues(htb->priv);
0088
0089     return node;
0090 }
0091
0092 static struct mlx5e_qos_node *mlx5e_htb_node_create_root(struct mlx5e_htb *htb)
0093 {
0094     struct mlx5e_qos_node *node;
0095
0096     node = kzalloc(sizeof(*node), GFP_KERNEL);
0097     if (!node)
0098         return ERR_PTR(-ENOMEM);
0099
0100     node->qid = MLX5E_QOS_QID_INNER;
0101     node->classid = MLX5E_HTB_CLASSID_ROOT;
0102     hash_add_rcu(htb->qos_tc2node, &node->hnode, node->classid);
0103
0104     return node;
0105 }
0106
0107 static struct mlx5e_qos_node *mlx5e_htb_node_find(struct mlx5e_htb *htb, u32 classid)
0108 {
0109     struct mlx5e_qos_node *node = NULL;
0110
0111     hash_for_each_possible(htb->qos_tc2node, node, hnode, classid) {
0112         if (node->classid == classid)
0113             break;
0114     }
0115
0116     return node;
0117 }
0118
0119 static struct mlx5e_qos_node *mlx5e_htb_node_find_rcu(struct mlx5e_htb *htb, u32 classid)
0120 {
0121     struct mlx5e_qos_node *node = NULL;
0122
0123     hash_for_each_possible_rcu(htb->qos_tc2node, node, hnode, classid) {
0124         if (node->classid == classid)
0125             break;
0126     }
0127
0128     return node;
0129 }
0130
0131 static void mlx5e_htb_node_delete(struct mlx5e_htb *htb, struct mlx5e_qos_node *node)
0132 {
0133     hash_del_rcu(&node->hnode);
0134     if (node->qid != MLX5E_QOS_QID_INNER) {
0135         __clear_bit(node->qid, htb->qos_used_qids);
0136         mlx5e_update_tx_netdev_queues(htb->priv);
0137     }
0138     /* Make sure this qid is no longer selected by mlx5e_select_queue, so
0139      * that mlx5e_reactivate_qos_sq can safely restart the netdev TX queue.
0140      */
0141     synchronize_net();
0142     kfree(node);
0143 }
0144
0145 /* TX datapath API */
0146
0147 int mlx5e_htb_get_txq_by_classid(struct mlx5e_htb *htb, u16 classid)
0148 {
0149     struct mlx5e_qos_node *node;
0150     u16 qid;
0151     int res;
0152
0153     rcu_read_lock();
0154
0155     node = mlx5e_htb_node_find_rcu(htb, classid);
0156     if (!node) {
0157         res = -ENOENT;
0158         goto out;
0159     }
0160     qid = READ_ONCE(node->qid);
0161     if (qid == MLX5E_QOS_QID_INNER) {
0162         res = -EINVAL;
0163         goto out;
0164     }
0165     res = mlx5e_qid_from_qos(&htb->priv->channels, qid);
0166
0167 out:
0168     rcu_read_unlock();
0169     return res;
0170 }
0171
0172 /* HTB TC handlers */
0173
0174 static int
0175 mlx5e_htb_root_add(struct mlx5e_htb *htb, u16 htb_maj_id, u16 htb_defcls,
0176            struct netlink_ext_ack *extack)
0177 {
0178     struct mlx5e_priv *priv = htb->priv;
0179     struct mlx5e_qos_node *root;
0180     bool opened;
0181     int err;
0182
0183     qos_dbg(htb->mdev, "TC_HTB_CREATE handle %04x:, default :%04x\n", htb_maj_id, htb_defcls);
0184
0185     mlx5e_selq_prepare_htb(htb->selq, htb_maj_id, htb_defcls);
0186
0187     opened = test_bit(MLX5E_STATE_OPENED, &priv->state);
0188     if (opened) {
0189         err = mlx5e_qos_alloc_queues(priv, &priv->channels);
0190         if (err)
0191             goto err_cancel_selq;
0192     }
0193
0194     root = mlx5e_htb_node_create_root(htb);
0195     if (IS_ERR(root)) {
0196         err = PTR_ERR(root);
0197         goto err_free_queues;
0198     }
0199
0200     err = mlx5_qos_create_root_node(htb->mdev, &root->hw_id);
0201     if (err) {
0202         NL_SET_ERR_MSG_MOD(extack, "Firmware error. Try upgrading firmware.");
0203         goto err_sw_node_delete;
0204     }
0205
0206     mlx5e_selq_apply(htb->selq);
0207
0208     return 0;
0209
0210 err_sw_node_delete:
0211     mlx5e_htb_node_delete(htb, root);
0212
0213 err_free_queues:
0214     if (opened)
0215         mlx5e_qos_close_all_queues(&priv->channels);
0216 err_cancel_selq:
0217     mlx5e_selq_cancel(htb->selq);
0218     return err;
0219 }
0220
0221 static int mlx5e_htb_root_del(struct mlx5e_htb *htb)
0222 {
0223     struct mlx5e_priv *priv = htb->priv;
0224     struct mlx5e_qos_node *root;
0225     int err;
0226
0227     qos_dbg(htb->mdev, "TC_HTB_DESTROY\n");
0228
0229     /* Wait until real_num_tx_queues is updated for mlx5e_select_queue,
0230      * so that we can safely switch to its non-HTB non-PTP fastpath.
0231      */
0232     synchronize_net();
0233
0234     mlx5e_selq_prepare_htb(htb->selq, 0, 0);
0235     mlx5e_selq_apply(htb->selq);
0236
0237     root = mlx5e_htb_node_find(htb, MLX5E_HTB_CLASSID_ROOT);
0238     if (!root) {
0239         qos_err(htb->mdev, "Failed to find the root node in the QoS tree\n");
0240         return -ENOENT;
0241     }
0242     err = mlx5_qos_destroy_node(htb->mdev, root->hw_id);
0243     if (err)
0244         qos_err(htb->mdev, "Failed to destroy root node %u, err = %d\n",
0245             root->hw_id, err);
0246     mlx5e_htb_node_delete(htb, root);
0247
0248     mlx5e_qos_deactivate_all_queues(&priv->channels);
0249     mlx5e_qos_close_all_queues(&priv->channels);
0250
0251     return err;
0252 }
0253
0254 static int mlx5e_htb_convert_rate(struct mlx5e_htb *htb, u64 rate,
0255                   struct mlx5e_qos_node *parent, u32 *bw_share)
0256 {
0257     u64 share = 0;
0258
0259     while (parent->classid != MLX5E_HTB_CLASSID_ROOT && !parent->max_average_bw)
0260         parent = parent->parent;
0261
0262     if (parent->max_average_bw)
0263         share = div64_u64(div_u64(rate * 100, BYTES_IN_MBIT),
0264                   parent->max_average_bw);
0265     else
0266         share = 101;
0267
0268     *bw_share = share == 0 ? 1 : share > 100 ? 0 : share;
0269
0270     qos_dbg(htb->mdev, "Convert: rate %llu, parent ceil %llu -> bw_share %u\n",
0271         rate, (u64)parent->max_average_bw * BYTES_IN_MBIT, *bw_share);
0272
0273     return 0;
0274 }
0275
0276 static void mlx5e_htb_convert_ceil(struct mlx5e_htb *htb, u64 ceil, u32 *max_average_bw)
0277 {
0278     /* Hardware treats 0 as "unlimited", set at least 1. */
0279     *max_average_bw = max_t(u32, div_u64(ceil, BYTES_IN_MBIT), 1);
0280
0281     qos_dbg(htb->mdev, "Convert: ceil %llu -> max_average_bw %u\n",
0282         ceil, *max_average_bw);
0283 }
0284
0285 int
0286 mlx5e_htb_leaf_alloc_queue(struct mlx5e_htb *htb, u16 classid,
0287                u32 parent_classid, u64 rate, u64 ceil,
0288                struct netlink_ext_ack *extack)
0289 {
0290     struct mlx5e_qos_node *node, *parent;
0291     struct mlx5e_priv *priv = htb->priv;
0292     int qid;
0293     int err;
0294
0295     qos_dbg(htb->mdev, "TC_HTB_LEAF_ALLOC_QUEUE classid %04x, parent %04x, rate %llu, ceil %llu\n",
0296         classid, parent_classid, rate, ceil);
0297
0298     qid = mlx5e_htb_find_unused_qos_qid(htb);
0299     if (qid < 0) {
0300         NL_SET_ERR_MSG_MOD(extack, "Maximum amount of leaf classes is reached.");
0301         return qid;
0302     }
0303
0304     parent = mlx5e_htb_node_find(htb, parent_classid);
0305     if (!parent)
0306         return -EINVAL;
0307
0308     node = mlx5e_htb_node_create_leaf(htb, classid, qid, parent);
0309     if (IS_ERR(node))
0310         return PTR_ERR(node);
0311
0312     node->rate = rate;
0313     mlx5e_htb_convert_rate(htb, rate, node->parent, &node->bw_share);
0314     mlx5e_htb_convert_ceil(htb, ceil, &node->max_average_bw);
0315
0316     err = mlx5_qos_create_leaf_node(htb->mdev, node->parent->hw_id,
0317                     node->bw_share, node->max_average_bw,
0318                     &node->hw_id);
0319     if (err) {
0320         NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating a leaf node.");
0321         qos_err(htb->mdev, "Failed to create a leaf node (class %04x), err = %d\n",
0322             classid, err);
0323         mlx5e_htb_node_delete(htb, node);
0324         return err;
0325     }
0326
0327     if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
0328         err = mlx5e_open_qos_sq(priv, &priv->channels, node->qid, node->hw_id);
0329         if (err) {
0330             NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
0331             qos_warn(htb->mdev, "Failed to create a QoS SQ (class %04x), err = %d\n",
0332                  classid, err);
0333         } else {
0334             mlx5e_activate_qos_sq(priv, node->qid, node->hw_id);
0335         }
0336     }
0337
0338     return mlx5e_qid_from_qos(&priv->channels, node->qid);
0339 }
0340
0341 int
0342 mlx5e_htb_leaf_to_inner(struct mlx5e_htb *htb, u16 classid, u16 child_classid,
0343             u64 rate, u64 ceil, struct netlink_ext_ack *extack)
0344 {
0345     struct mlx5e_qos_node *node, *child;
0346     struct mlx5e_priv *priv = htb->priv;
0347     int err, tmp_err;
0348     u32 new_hw_id;
0349     u16 qid;
0350
0351     qos_dbg(htb->mdev, "TC_HTB_LEAF_TO_INNER classid %04x, upcoming child %04x, rate %llu, ceil %llu\n",
0352         classid, child_classid, rate, ceil);
0353
0354     node = mlx5e_htb_node_find(htb, classid);
0355     if (!node)
0356         return -ENOENT;
0357
0358     err = mlx5_qos_create_inner_node(htb->mdev, node->parent->hw_id,
0359                      node->bw_share, node->max_average_bw,
0360                      &new_hw_id);
0361     if (err) {
0362         NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating an inner node.");
0363         qos_err(htb->mdev, "Failed to create an inner node (class %04x), err = %d\n",
0364             classid, err);
0365         return err;
0366     }
0367
0368     /* Intentionally reuse the qid for the upcoming first child. */
0369     child = mlx5e_htb_node_create_leaf(htb, child_classid, node->qid, node);
0370     if (IS_ERR(child)) {
0371         err = PTR_ERR(child);
0372         goto err_destroy_hw_node;
0373     }
0374
0375     child->rate = rate;
0376     mlx5e_htb_convert_rate(htb, rate, node, &child->bw_share);
0377     mlx5e_htb_convert_ceil(htb, ceil, &child->max_average_bw);
0378
0379     err = mlx5_qos_create_leaf_node(htb->mdev, new_hw_id, child->bw_share,
0380                     child->max_average_bw, &child->hw_id);
0381     if (err) {
0382         NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating a leaf node.");
0383         qos_err(htb->mdev, "Failed to create a leaf node (class %04x), err = %d\n",
0384             classid, err);
0385         goto err_delete_sw_node;
0386     }
0387
0388     /* No fail point. */
0389
0390     qid = node->qid;
0391     /* Pairs with mlx5e_htb_get_txq_by_classid. */
0392     WRITE_ONCE(node->qid, MLX5E_QOS_QID_INNER);
0393
0394     if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
0395         mlx5e_deactivate_qos_sq(priv, qid);
0396         mlx5e_close_qos_sq(priv, qid);
0397     }
0398
0399     err = mlx5_qos_destroy_node(htb->mdev, node->hw_id);
0400     if (err) /* Not fatal. */
0401         qos_warn(htb->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
0402              node->hw_id, classid, err);
0403
0404     node->hw_id = new_hw_id;
0405
0406     if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
0407         err = mlx5e_open_qos_sq(priv, &priv->channels, child->qid, child->hw_id);
0408         if (err) {
0409             NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
0410             qos_warn(htb->mdev, "Failed to create a QoS SQ (class %04x), err = %d\n",
0411                  classid, err);
0412         } else {
0413             mlx5e_activate_qos_sq(priv, child->qid, child->hw_id);
0414         }
0415     }
0416
0417     return 0;
0418
0419 err_delete_sw_node:
0420     child->qid = MLX5E_QOS_QID_INNER;
0421     mlx5e_htb_node_delete(htb, child);
0422
0423 err_destroy_hw_node:
0424     tmp_err = mlx5_qos_destroy_node(htb->mdev, new_hw_id);
0425     if (tmp_err) /* Not fatal. */
0426         qos_warn(htb->mdev, "Failed to roll back creation of an inner node %u (class %04x), err = %d\n",
0427              new_hw_id, classid, tmp_err);
0428     return err;
0429 }
0430
0431 static struct mlx5e_qos_node *mlx5e_htb_node_find_by_qid(struct mlx5e_htb *htb, u16 qid)
0432 {
0433     struct mlx5e_qos_node *node = NULL;
0434     int bkt;
0435
0436     hash_for_each(htb->qos_tc2node, bkt, node, hnode)
0437         if (node->qid == qid)
0438             break;
0439
0440     return node;
0441 }
0442
0443 int mlx5e_htb_leaf_del(struct mlx5e_htb *htb, u16 *classid,
0444                struct netlink_ext_ack *extack)
0445 {
0446     struct mlx5e_priv *priv = htb->priv;
0447     struct mlx5e_qos_node *node;
0448     struct netdev_queue *txq;
0449     u16 qid, moved_qid;
0450     bool opened;
0451     int err;
0452
0453     qos_dbg(htb->mdev, "TC_HTB_LEAF_DEL classid %04x\n", *classid);
0454
0455     node = mlx5e_htb_node_find(htb, *classid);
0456     if (!node)
0457         return -ENOENT;
0458
0459     /* Store qid for reuse. */
0460     qid = node->qid;
0461
0462     opened = test_bit(MLX5E_STATE_OPENED, &priv->state);
0463     if (opened) {
0464         txq = netdev_get_tx_queue(htb->netdev,
0465                       mlx5e_qid_from_qos(&priv->channels, qid));
0466         mlx5e_deactivate_qos_sq(priv, qid);
0467         mlx5e_close_qos_sq(priv, qid);
0468     }
0469
0470     err = mlx5_qos_destroy_node(htb->mdev, node->hw_id);
0471     if (err) /* Not fatal. */
0472         qos_warn(htb->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
0473              node->hw_id, *classid, err);
0474
0475     mlx5e_htb_node_delete(htb, node);
0476
0477     moved_qid = mlx5e_htb_cur_leaf_nodes(htb);
0478
0479     if (moved_qid == 0) {
0480         /* The last QoS SQ was just destroyed. */
0481         if (opened)
0482             mlx5e_reactivate_qos_sq(priv, qid, txq);
0483         return 0;
0484     }
0485     moved_qid--;
0486
0487     if (moved_qid < qid) {
0488         /* The highest QoS SQ was just destroyed. */
0489         WARN(moved_qid != qid - 1, "Gaps in queue numeration: destroyed queue %u, the highest queue is %u",
0490              qid, moved_qid);
0491         if (opened)
0492             mlx5e_reactivate_qos_sq(priv, qid, txq);
0493         return 0;
0494     }
0495
0496     WARN(moved_qid == qid, "Can't move node with qid %u to itself", qid);
0497     qos_dbg(htb->mdev, "Moving QoS SQ %u to %u\n", moved_qid, qid);
0498
0499     node = mlx5e_htb_node_find_by_qid(htb, moved_qid);
0500     WARN(!node, "Could not find a node with qid %u to move to queue %u",
0501          moved_qid, qid);
0502
0503     /* Stop traffic to the old queue. */
0504     WRITE_ONCE(node->qid, MLX5E_QOS_QID_INNER);
0505     __clear_bit(moved_qid, priv->htb->qos_used_qids);
0506
0507     if (opened) {
0508         txq = netdev_get_tx_queue(htb->netdev,
0509                       mlx5e_qid_from_qos(&priv->channels, moved_qid));
0510         mlx5e_deactivate_qos_sq(priv, moved_qid);
0511         mlx5e_close_qos_sq(priv, moved_qid);
0512     }
0513
0514     /* Prevent packets from the old class from getting into the new one. */
0515     mlx5e_reset_qdisc(htb->netdev, moved_qid);
0516
0517     __set_bit(qid, htb->qos_used_qids);
0518     WRITE_ONCE(node->qid, qid);
0519
0520     if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
0521         err = mlx5e_open_qos_sq(priv, &priv->channels, node->qid, node->hw_id);
0522         if (err) {
0523             NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
0524             qos_warn(htb->mdev, "Failed to create a QoS SQ (class %04x) while moving qid %u to %u, err = %d\n",
0525                  node->classid, moved_qid, qid, err);
0526         } else {
0527             mlx5e_activate_qos_sq(priv, node->qid, node->hw_id);
0528         }
0529     }
0530
0531     mlx5e_update_tx_netdev_queues(priv);
0532     if (opened)
0533         mlx5e_reactivate_qos_sq(priv, moved_qid, txq);
0534
0535     *classid = node->classid;
0536     return 0;
0537 }
0538
0539 int
0540 mlx5e_htb_leaf_del_last(struct mlx5e_htb *htb, u16 classid, bool force,
0541             struct netlink_ext_ack *extack)
0542 {
0543     struct mlx5e_qos_node *node, *parent;
0544     struct mlx5e_priv *priv = htb->priv;
0545     u32 old_hw_id, new_hw_id;
0546     int err, saved_err = 0;
0547     u16 qid;
0548
0549     qos_dbg(htb->mdev, "TC_HTB_LEAF_DEL_LAST%s classid %04x\n",
0550         force ? "_FORCE" : "", classid);
0551
0552     node = mlx5e_htb_node_find(htb, classid);
0553     if (!node)
0554         return -ENOENT;
0555
0556     err = mlx5_qos_create_leaf_node(htb->mdev, node->parent->parent->hw_id,
0557                     node->parent->bw_share,
0558                     node->parent->max_average_bw,
0559                     &new_hw_id);
0560     if (err) {
0561         NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating a leaf node.");
0562         qos_err(htb->mdev, "Failed to create a leaf node (class %04x), err = %d\n",
0563             classid, err);
0564         if (!force)
0565             return err;
0566         saved_err = err;
0567     }
0568
0569     /* Store qid for reuse and prevent clearing the bit. */
0570     qid = node->qid;
0571     /* Pairs with mlx5e_htb_get_txq_by_classid. */
0572     WRITE_ONCE(node->qid, MLX5E_QOS_QID_INNER);
0573
0574     if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
0575         mlx5e_deactivate_qos_sq(priv, qid);
0576         mlx5e_close_qos_sq(priv, qid);
0577     }
0578
0579     /* Prevent packets from the old class from getting into the new one. */
0580     mlx5e_reset_qdisc(htb->netdev, qid);
0581
0582     err = mlx5_qos_destroy_node(htb->mdev, node->hw_id);
0583     if (err) /* Not fatal. */
0584         qos_warn(htb->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
0585              node->hw_id, classid, err);
0586
0587     parent = node->parent;
0588     mlx5e_htb_node_delete(htb, node);
0589
0590     node = parent;
0591     WRITE_ONCE(node->qid, qid);
0592
0593     /* Early return on error in force mode. Parent will still be an inner
0594      * node to be deleted by a following delete operation.
0595      */
0596     if (saved_err)
0597         return saved_err;
0598
0599     old_hw_id = node->hw_id;
0600     node->hw_id = new_hw_id;
0601
0602     if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
0603         err = mlx5e_open_qos_sq(priv, &priv->channels, node->qid, node->hw_id);
0604         if (err) {
0605             NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
0606             qos_warn(htb->mdev, "Failed to create a QoS SQ (class %04x), err = %d\n",
0607                  classid, err);
0608         } else {
0609             mlx5e_activate_qos_sq(priv, node->qid, node->hw_id);
0610         }
0611     }
0612
0613     err = mlx5_qos_destroy_node(htb->mdev, old_hw_id);
0614     if (err) /* Not fatal. */
0615         qos_warn(htb->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
0616              node->hw_id, classid, err);
0617
0618     return 0;
0619 }
0620
0621 static int
0622 mlx5e_htb_update_children(struct mlx5e_htb *htb, struct mlx5e_qos_node *node,
0623               struct netlink_ext_ack *extack)
0624 {
0625     struct mlx5e_qos_node *child;
0626     int err = 0;
0627     int bkt;
0628
0629     hash_for_each(htb->qos_tc2node, bkt, child, hnode) {
0630         u32 old_bw_share = child->bw_share;
0631         int err_one;
0632
0633         if (child->parent != node)
0634             continue;
0635
0636         mlx5e_htb_convert_rate(htb, child->rate, node, &child->bw_share);
0637         if (child->bw_share == old_bw_share)
0638             continue;
0639
0640         err_one = mlx5_qos_update_node(htb->mdev, child->hw_id, child->bw_share,
0641                            child->max_average_bw, child->hw_id);
0642         if (!err && err_one) {
0643             err = err_one;
0644
0645             NL_SET_ERR_MSG_MOD(extack, "Firmware error when modifying a child node.");
0646             qos_err(htb->mdev, "Failed to modify a child node (class %04x), err = %d\n",
0647                 node->classid, err);
0648         }
0649     }
0650
0651     return err;
0652 }
0653
0654 int
0655 mlx5e_htb_node_modify(struct mlx5e_htb *htb, u16 classid, u64 rate, u64 ceil,
0656               struct netlink_ext_ack *extack)
0657 {
0658     u32 bw_share, max_average_bw;
0659     struct mlx5e_qos_node *node;
0660     bool ceil_changed = false;
0661     int err;
0662
0663     qos_dbg(htb->mdev, "TC_HTB_LEAF_MODIFY classid %04x, rate %llu, ceil %llu\n",
0664         classid, rate, ceil);
0665
0666     node = mlx5e_htb_node_find(htb, classid);
0667     if (!node)
0668         return -ENOENT;
0669
0670     node->rate = rate;
0671     mlx5e_htb_convert_rate(htb, rate, node->parent, &bw_share);
0672     mlx5e_htb_convert_ceil(htb, ceil, &max_average_bw);
0673
0674     err = mlx5_qos_update_node(htb->mdev, node->parent->hw_id, bw_share,
0675                    max_average_bw, node->hw_id);
0676     if (err) {
0677         NL_SET_ERR_MSG_MOD(extack, "Firmware error when modifying a node.");
0678         qos_err(htb->mdev, "Failed to modify a node (class %04x), err = %d\n",
0679             classid, err);
0680         return err;
0681     }
0682
0683     if (max_average_bw != node->max_average_bw)
0684         ceil_changed = true;
0685
0686     node->bw_share = bw_share;
0687     node->max_average_bw = max_average_bw;
0688
0689     if (ceil_changed)
0690         err = mlx5e_htb_update_children(htb, node, extack);
0691
0692     return err;
0693 }
0694
0695 struct mlx5e_htb *mlx5e_htb_alloc(void)
0696 {
0697     return kvzalloc(sizeof(struct mlx5e_htb), GFP_KERNEL);
0698 }
0699
0700 void mlx5e_htb_free(struct mlx5e_htb *htb)
0701 {
0702     kvfree(htb);
0703 }
0704
0705 int mlx5e_htb_init(struct mlx5e_htb *htb, struct tc_htb_qopt_offload *htb_qopt,
0706            struct net_device *netdev, struct mlx5_core_dev *mdev,
0707            struct mlx5e_selq *selq, struct mlx5e_priv *priv)
0708 {
0709     htb->mdev = mdev;
0710     htb->netdev = netdev;
0711     htb->selq = selq;
0712     htb->priv = priv;
0713     hash_init(htb->qos_tc2node);
0714     return mlx5e_htb_root_add(htb, htb_qopt->parent_classid, htb_qopt->classid,
0715                   htb_qopt->extack);
0716 }
0717
0718 void mlx5e_htb_cleanup(struct mlx5e_htb *htb)
0719 {
0720     mlx5e_htb_root_del(htb);
0721 }
0722