drivers/net/ethernet/mellanox/mlx5/core/en/qos.c

   1 // SPDX-License-Identifier: GPL-2.0 OR Linux-OpenIB
   2 /* Copyright (c) 2020, Mellanox Technologies inc. All rights reserved. */
   3 #include <net/sch_generic.h>
   4
   5 #include "en.h"
   6 #include "params.h"
   7 #include "../qos.h"
   8
   9 #define BYTES_IN_MBIT 125000
  10
  11 int mlx5e_qos_bytes_rate_check(struct mlx5_core_dev *mdev, u64 nbytes)
  12 {
  13         if (nbytes < BYTES_IN_MBIT) {
  14                 qos_warn(mdev, "Input rate (%llu Bytes/sec) below minimum supported (%u Bytes/sec)\n",
  15                          nbytes, BYTES_IN_MBIT);
  16                 return -EINVAL;
  17         }
  18         return 0;
  19 }
  20
  21 static u32 mlx5e_qos_bytes2mbits(struct mlx5_core_dev *mdev, u64 nbytes)
  22 {
  23         return div_u64(nbytes, BYTES_IN_MBIT);
  24 }
  25
  26 int mlx5e_qos_max_leaf_nodes(struct mlx5_core_dev *mdev)
  27 {
  28         return min(MLX5E_QOS_MAX_LEAF_NODES, mlx5_qos_max_leaf_nodes(mdev));
  29 }
  30
  31 int mlx5e_qos_cur_leaf_nodes(struct mlx5e_priv *priv)
  32 {
  33         int last = find_last_bit(priv->htb.qos_used_qids, mlx5e_qos_max_leaf_nodes(priv->mdev));
  34
  35         return last == mlx5e_qos_max_leaf_nodes(priv->mdev) ? 0 : last + 1;
  36 }
  37
  38 /* Software representation of the QoS tree (internal to this file) */
  39
  40 static int mlx5e_find_unused_qos_qid(struct mlx5e_priv *priv)
  41 {
  42         int size = mlx5e_qos_max_leaf_nodes(priv->mdev);
  43         int res;
  44
  45         WARN_ONCE(!mutex_is_locked(&priv->state_lock), "%s: state_lock is not held\n", __func__);
  46         res = find_first_zero_bit(priv->htb.qos_used_qids, size);
  47
  48         return res == size ? -ENOSPC : res;
  49 }
  50
  51 struct mlx5e_qos_node {
  52         struct hlist_node hnode;
  53         struct rcu_head rcu;
  54         struct mlx5e_qos_node *parent;
  55         u64 rate;
  56         u32 bw_share;
  57         u32 max_average_bw;
  58         u32 hw_id;
  59         u32 classid; /* 16-bit, except root. */
  60         u16 qid;
  61 };
  62
  63 #define MLX5E_QOS_QID_INNER 0xffff
  64 #define MLX5E_HTB_CLASSID_ROOT 0xffffffff
  65
  66 static struct mlx5e_qos_node *
  67 mlx5e_sw_node_create_leaf(struct mlx5e_priv *priv, u16 classid, u16 qid,
  68                           struct mlx5e_qos_node *parent)
  69 {
  70         struct mlx5e_qos_node *node;
  71
  72         node = kzalloc(sizeof(*node), GFP_KERNEL);
  73         if (!node)
  74                 return ERR_PTR(-ENOMEM);
  75
  76         node->parent = parent;
  77
  78         node->qid = qid;
  79         __set_bit(qid, priv->htb.qos_used_qids);
  80
  81         node->classid = classid;
  82         hash_add_rcu(priv->htb.qos_tc2node, &node->hnode, classid);
  83
  84         mlx5e_update_tx_netdev_queues(priv);
  85
  86         return node;
  87 }
  88
  89 static struct mlx5e_qos_node *mlx5e_sw_node_create_root(struct mlx5e_priv *priv)
  90 {
  91         struct mlx5e_qos_node *node;
  92
  93         node = kzalloc(sizeof(*node), GFP_KERNEL);
  94         if (!node)
  95                 return ERR_PTR(-ENOMEM);
  96
  97         node->qid = MLX5E_QOS_QID_INNER;
  98         node->classid = MLX5E_HTB_CLASSID_ROOT;
  99         hash_add_rcu(priv->htb.qos_tc2node, &node->hnode, node->classid);
 100
 101         return node;
 102 }
 103
 104 static struct mlx5e_qos_node *mlx5e_sw_node_find(struct mlx5e_priv *priv, u32 classid)
 105 {
 106         struct mlx5e_qos_node *node = NULL;
 107
 108         hash_for_each_possible(priv->htb.qos_tc2node, node, hnode, classid) {
 109                 if (node->classid == classid)
 110                         break;
 111         }
 112
 113         return node;
 114 }
 115
 116 static struct mlx5e_qos_node *mlx5e_sw_node_find_rcu(struct mlx5e_priv *priv, u32 classid)
 117 {
 118         struct mlx5e_qos_node *node = NULL;
 119
 120         hash_for_each_possible_rcu(priv->htb.qos_tc2node, node, hnode, classid) {
 121                 if (node->classid == classid)
 122                         break;
 123         }
 124
 125         return node;
 126 }
 127
 128 static void mlx5e_sw_node_delete(struct mlx5e_priv *priv, struct mlx5e_qos_node *node)
 129 {
 130         hash_del_rcu(&node->hnode);
 131         if (node->qid != MLX5E_QOS_QID_INNER) {
 132                 __clear_bit(node->qid, priv->htb.qos_used_qids);
 133                 mlx5e_update_tx_netdev_queues(priv);
 134         }
 135         kfree_rcu(node, rcu);
 136 }
 137
 138 /* TX datapath API */
 139
 140 static u16 mlx5e_qid_from_qos(struct mlx5e_channels *chs, u16 qid)
 141 {
 142         /* These channel params are safe to access from the datapath, because:
 143          * 1. This function is called only after checking priv->htb.maj_id != 0,
 144          *    and the number of queues can't change while HTB offload is active.
 145          * 2. When priv->htb.maj_id becomes 0, synchronize_rcu waits for
 146          *    mlx5e_select_queue to finish while holding priv->state_lock,
 147          *    preventing other code from changing the number of queues.
 148          */
 149         bool is_ptp = MLX5E_GET_PFLAG(&chs->params, MLX5E_PFLAG_TX_PORT_TS);
 150
 151         return (chs->params.num_channels + is_ptp) * mlx5e_get_dcb_num_tc(&chs->params) + qid;
 152 }
 153
 154 int mlx5e_get_txq_by_classid(struct mlx5e_priv *priv, u16 classid)
 155 {
 156         struct mlx5e_qos_node *node;
 157         u16 qid;
 158         int res;
 159
 160         rcu_read_lock();
 161
 162         node = mlx5e_sw_node_find_rcu(priv, classid);
 163         if (!node) {
 164                 res = -ENOENT;
 165                 goto out;
 166         }
 167         qid = READ_ONCE(node->qid);
 168         if (qid == MLX5E_QOS_QID_INNER) {
 169                 res = -EINVAL;
 170                 goto out;
 171         }
 172         res = mlx5e_qid_from_qos(&priv->channels, qid);
 173
 174 out:
 175         rcu_read_unlock();
 176         return res;
 177 }
 178
 179 static struct mlx5e_txqsq *mlx5e_get_qos_sq(struct mlx5e_priv *priv, int qid)
 180 {
 181         struct mlx5e_params *params = &priv->channels.params;
 182         struct mlx5e_txqsq __rcu **qos_sqs;
 183         struct mlx5e_channel *c;
 184         int ix;
 185
 186         ix = qid % params->num_channels;
 187         qid /= params->num_channels;
 188         c = priv->channels.c[ix];
 189
 190         qos_sqs = mlx5e_state_dereference(priv, c->qos_sqs);
 191         return mlx5e_state_dereference(priv, qos_sqs[qid]);
 192 }
 193
 194 /* SQ lifecycle */
 195
 196 static int mlx5e_open_qos_sq(struct mlx5e_priv *priv, struct mlx5e_channels *chs,
 197                              struct mlx5e_qos_node *node)
 198 {
 199         struct mlx5e_create_cq_param ccp = {};
 200         struct mlx5e_txqsq __rcu **qos_sqs;
 201         struct mlx5e_sq_param param_sq;
 202         struct mlx5e_cq_param param_cq;
 203         int txq_ix, ix, qid, err = 0;
 204         struct mlx5e_params *params;
 205         struct mlx5e_channel *c;
 206         struct mlx5e_txqsq *sq;
 207
 208         params = &chs->params;
 209
 210         txq_ix = mlx5e_qid_from_qos(chs, node->qid);
 211
 212         WARN_ON(node->qid > priv->htb.max_qos_sqs);
 213         if (node->qid == priv->htb.max_qos_sqs) {
 214                 struct mlx5e_sq_stats *stats, **stats_list = NULL;
 215
 216                 if (priv->htb.max_qos_sqs == 0) {
 217                         stats_list = kvcalloc(mlx5e_qos_max_leaf_nodes(priv->mdev),
 218                                               sizeof(*stats_list),
 219                                               GFP_KERNEL);
 220                         if (!stats_list)
 221                                 return -ENOMEM;
 222                 }
 223                 stats = kzalloc(sizeof(*stats), GFP_KERNEL);
 224                 if (!stats) {
 225                         kvfree(stats_list);
 226                         return -ENOMEM;
 227                 }
 228                 if (stats_list)
 229                         WRITE_ONCE(priv->htb.qos_sq_stats, stats_list);
 230                 WRITE_ONCE(priv->htb.qos_sq_stats[node->qid], stats);
 231                 /* Order max_qos_sqs increment after writing the array pointer.
 232                  * Pairs with smp_load_acquire in en_stats.c.
 233                  */
 234                 smp_store_release(&priv->htb.max_qos_sqs, priv->htb.max_qos_sqs + 1);
 235         }
 236
 237         ix = node->qid % params->num_channels;
 238         qid = node->qid / params->num_channels;
 239         c = chs->c[ix];
 240
 241         qos_sqs = mlx5e_state_dereference(priv, c->qos_sqs);
 242         sq = kzalloc(sizeof(*sq), GFP_KERNEL);
 243
 244         if (!sq)
 245                 return -ENOMEM;
 246
 247         mlx5e_build_create_cq_param(&ccp, c);
 248
 249         memset(&param_sq, 0, sizeof(param_sq));
 250         memset(&param_cq, 0, sizeof(param_cq));
 251         mlx5e_build_sq_param(priv->mdev, params, &param_sq);
 252         mlx5e_build_tx_cq_param(priv->mdev, params, &param_cq);
 253         err = mlx5e_open_cq(priv, params->tx_cq_moderation, &param_cq, &ccp, &sq->cq);
 254         if (err)
 255                 goto err_free_sq;
 256         err = mlx5e_open_txqsq(c, priv->tisn[c->lag_port][0], txq_ix, params,
 257                                &param_sq, sq, 0, node->hw_id,
 258                                priv->htb.qos_sq_stats[node->qid]);
 259         if (err)
 260                 goto err_close_cq;
 261
 262         rcu_assign_pointer(qos_sqs[qid], sq);
 263
 264         return 0;
 265
 266 err_close_cq:
 267         mlx5e_close_cq(&sq->cq);
 268 err_free_sq:
 269         kfree(sq);
 270         return err;
 271 }
 272
 273 static void mlx5e_activate_qos_sq(struct mlx5e_priv *priv, struct mlx5e_qos_node *node)
 274 {
 275         struct mlx5e_txqsq *sq;
 276
 277         sq = mlx5e_get_qos_sq(priv, node->qid);
 278
 279         WRITE_ONCE(priv->txq2sq[mlx5e_qid_from_qos(&priv->channels, node->qid)], sq);
 280
 281         /* Make the change to txq2sq visible before the queue is started.
 282          * As mlx5e_xmit runs under a spinlock, there is an implicit ACQUIRE,
 283          * which pairs with this barrier.
 284          */
 285         smp_wmb();
 286
 287         qos_dbg(priv->mdev, "Activate QoS SQ qid %u\n", node->qid);
 288         mlx5e_activate_txqsq(sq);
 289 }
 290
 291 static void mlx5e_deactivate_qos_sq(struct mlx5e_priv *priv, u16 qid)
 292 {
 293         struct mlx5e_txqsq *sq;
 294
 295         sq = mlx5e_get_qos_sq(priv, qid);
 296         if (!sq) /* Handle the case when the SQ failed to open. */
 297                 return;
 298
 299         qos_dbg(priv->mdev, "Deactivate QoS SQ qid %u\n", qid);
 300         mlx5e_deactivate_txqsq(sq);
 301
 302         /* The queue is disabled, no synchronization with datapath is needed. */
 303         priv->txq2sq[mlx5e_qid_from_qos(&priv->channels, qid)] = NULL;
 304 }
 305
 306 static void mlx5e_close_qos_sq(struct mlx5e_priv *priv, u16 qid)
 307 {
 308         struct mlx5e_txqsq __rcu **qos_sqs;
 309         struct mlx5e_params *params;
 310         struct mlx5e_channel *c;
 311         struct mlx5e_txqsq *sq;
 312         int ix;
 313
 314         params = &priv->channels.params;
 315
 316         ix = qid % params->num_channels;
 317         qid /= params->num_channels;
 318         c = priv->channels.c[ix];
 319         qos_sqs = mlx5e_state_dereference(priv, c->qos_sqs);
 320         sq = rcu_replace_pointer(qos_sqs[qid], NULL, lockdep_is_held(&priv->state_lock));
 321         if (!sq) /* Handle the case when the SQ failed to open. */
 322                 return;
 323
 324         synchronize_rcu(); /* Sync with NAPI. */
 325
 326         mlx5e_close_txqsq(sq);
 327         mlx5e_close_cq(&sq->cq);
 328         kfree(sq);
 329 }
 330
 331 void mlx5e_qos_close_queues(struct mlx5e_channel *c)
 332 {
 333         struct mlx5e_txqsq __rcu **qos_sqs;
 334         int i;
 335
 336         qos_sqs = rcu_replace_pointer(c->qos_sqs, NULL, lockdep_is_held(&c->priv->state_lock));
 337         if (!qos_sqs)
 338                 return;
 339         synchronize_rcu(); /* Sync with NAPI. */
 340
 341         for (i = 0; i < c->qos_sqs_size; i++) {
 342                 struct mlx5e_txqsq *sq;
 343
 344                 sq = mlx5e_state_dereference(c->priv, qos_sqs[i]);
 345                 if (!sq) /* Handle the case when the SQ failed to open. */
 346                         continue;
 347
 348                 mlx5e_close_txqsq(sq);
 349                 mlx5e_close_cq(&sq->cq);
 350                 kfree(sq);
 351         }
 352
 353         kvfree(qos_sqs);
 354 }
 355
 356 static void mlx5e_qos_close_all_queues(struct mlx5e_channels *chs)
 357 {
 358         int i;
 359
 360         for (i = 0; i < chs->num; i++)
 361                 mlx5e_qos_close_queues(chs->c[i]);
 362 }
 363
 364 static int mlx5e_qos_alloc_queues(struct mlx5e_priv *priv, struct mlx5e_channels *chs)
 365 {
 366         u16 qos_sqs_size;
 367         int i;
 368
 369         qos_sqs_size = DIV_ROUND_UP(mlx5e_qos_max_leaf_nodes(priv->mdev), chs->num);
 370
 371         for (i = 0; i < chs->num; i++) {
 372                 struct mlx5e_txqsq **sqs;
 373
 374                 sqs = kvcalloc(qos_sqs_size, sizeof(struct mlx5e_txqsq *), GFP_KERNEL);
 375                 if (!sqs)
 376                         goto err_free;
 377
 378                 WRITE_ONCE(chs->c[i]->qos_sqs_size, qos_sqs_size);
 379                 smp_wmb(); /* Pairs with mlx5e_napi_poll. */
 380                 rcu_assign_pointer(chs->c[i]->qos_sqs, sqs);
 381         }
 382
 383         return 0;
 384
 385 err_free:
 386         while (--i >= 0) {
 387                 struct mlx5e_txqsq **sqs;
 388
 389                 sqs = rcu_replace_pointer(chs->c[i]->qos_sqs, NULL,
 390                                           lockdep_is_held(&priv->state_lock));
 391
 392                 synchronize_rcu(); /* Sync with NAPI. */
 393                 kvfree(sqs);
 394         }
 395         return -ENOMEM;
 396 }
 397
 398 int mlx5e_qos_open_queues(struct mlx5e_priv *priv, struct mlx5e_channels *chs)
 399 {
 400         struct mlx5e_qos_node *node = NULL;
 401         int bkt, err;
 402
 403         if (!priv->htb.maj_id)
 404                 return 0;
 405
 406         err = mlx5e_qos_alloc_queues(priv, chs);
 407         if (err)
 408                 return err;
 409
 410         hash_for_each(priv->htb.qos_tc2node, bkt, node, hnode) {
 411                 if (node->qid == MLX5E_QOS_QID_INNER)
 412                         continue;
 413                 err = mlx5e_open_qos_sq(priv, chs, node);
 414                 if (err) {
 415                         mlx5e_qos_close_all_queues(chs);
 416                         return err;
 417                 }
 418         }
 419
 420         return 0;
 421 }
 422
 423 void mlx5e_qos_activate_queues(struct mlx5e_priv *priv)
 424 {
 425         struct mlx5e_qos_node *node = NULL;
 426         int bkt;
 427
 428         hash_for_each(priv->htb.qos_tc2node, bkt, node, hnode) {
 429                 if (node->qid == MLX5E_QOS_QID_INNER)
 430                         continue;
 431                 mlx5e_activate_qos_sq(priv, node);
 432         }
 433 }
 434
 435 void mlx5e_qos_deactivate_queues(struct mlx5e_channel *c)
 436 {
 437         struct mlx5e_params *params = &c->priv->channels.params;
 438         struct mlx5e_txqsq __rcu **qos_sqs;
 439         int i;
 440
 441         qos_sqs = mlx5e_state_dereference(c->priv, c->qos_sqs);
 442         if (!qos_sqs)
 443                 return;
 444
 445         for (i = 0; i < c->qos_sqs_size; i++) {
 446                 u16 qid = params->num_channels * i + c->ix;
 447                 struct mlx5e_txqsq *sq;
 448
 449                 sq = mlx5e_state_dereference(c->priv, qos_sqs[i]);
 450                 if (!sq) /* Handle the case when the SQ failed to open. */
 451                         continue;
 452
 453                 qos_dbg(c->mdev, "Deactivate QoS SQ qid %u\n", qid);
 454                 mlx5e_deactivate_txqsq(sq);
 455
 456                 /* The queue is disabled, no synchronization with datapath is needed. */
 457                 c->priv->txq2sq[mlx5e_qid_from_qos(&c->priv->channels, qid)] = NULL;
 458         }
 459 }
 460
 461 static void mlx5e_qos_deactivate_all_queues(struct mlx5e_channels *chs)
 462 {
 463         int i;
 464
 465         for (i = 0; i < chs->num; i++)
 466                 mlx5e_qos_deactivate_queues(chs->c[i]);
 467 }
 468
 469 /* HTB API */
 470
 471 int mlx5e_htb_root_add(struct mlx5e_priv *priv, u16 htb_maj_id, u16 htb_defcls,
 472                        struct netlink_ext_ack *extack)
 473 {
 474         struct mlx5e_qos_node *root;
 475         bool opened;
 476         int err;
 477
 478         qos_dbg(priv->mdev, "TC_HTB_CREATE handle %04x:, default :%04x\n", htb_maj_id, htb_defcls);
 479
 480         if (!mlx5_qos_is_supported(priv->mdev)) {
 481                 NL_SET_ERR_MSG_MOD(extack,
 482                                    "Missing QoS capabilities. Try disabling SRIOV or use a supported device.");
 483                 return -EOPNOTSUPP;
 484         }
 485
 486         opened = test_bit(MLX5E_STATE_OPENED, &priv->state);
 487         if (opened) {
 488                 err = mlx5e_qos_alloc_queues(priv, &priv->channels);
 489                 if (err)
 490                         return err;
 491         }
 492
 493         root = mlx5e_sw_node_create_root(priv);
 494         if (IS_ERR(root)) {
 495                 err = PTR_ERR(root);
 496                 goto err_free_queues;
 497         }
 498
 499         err = mlx5_qos_create_root_node(priv->mdev, &root->hw_id);
 500         if (err) {
 501                 NL_SET_ERR_MSG_MOD(extack, "Firmware error. Try upgrading firmware.");
 502                 goto err_sw_node_delete;
 503         }
 504
 505         WRITE_ONCE(priv->htb.defcls, htb_defcls);
 506         /* Order maj_id after defcls - pairs with
 507          * mlx5e_select_queue/mlx5e_select_htb_queues.
 508          */
 509         smp_store_release(&priv->htb.maj_id, htb_maj_id);
 510
 511         return 0;
 512
 513 err_sw_node_delete:
 514         mlx5e_sw_node_delete(priv, root);
 515
 516 err_free_queues:
 517         if (opened)
 518                 mlx5e_qos_close_all_queues(&priv->channels);
 519         return err;
 520 }
 521
 522 int mlx5e_htb_root_del(struct mlx5e_priv *priv)
 523 {
 524         struct mlx5e_qos_node *root;
 525         int err;
 526
 527         qos_dbg(priv->mdev, "TC_HTB_DESTROY\n");
 528
 529         WRITE_ONCE(priv->htb.maj_id, 0);
 530         synchronize_rcu(); /* Sync with mlx5e_select_htb_queue and TX data path. */
 531
 532         root = mlx5e_sw_node_find(priv, MLX5E_HTB_CLASSID_ROOT);
 533         if (!root) {
 534                 qos_err(priv->mdev, "Failed to find the root node in the QoS tree\n");
 535                 return -ENOENT;
 536         }
 537         err = mlx5_qos_destroy_node(priv->mdev, root->hw_id);
 538         if (err)
 539                 qos_err(priv->mdev, "Failed to destroy root node %u, err = %d\n",
 540                         root->hw_id, err);
 541         mlx5e_sw_node_delete(priv, root);
 542
 543         mlx5e_qos_deactivate_all_queues(&priv->channels);
 544         mlx5e_qos_close_all_queues(&priv->channels);
 545
 546         return err;
 547 }
 548
 549 static int mlx5e_htb_convert_rate(struct mlx5e_priv *priv, u64 rate,
 550                                   struct mlx5e_qos_node *parent, u32 *bw_share)
 551 {
 552         u64 share = 0;
 553
 554         while (parent->classid != MLX5E_HTB_CLASSID_ROOT && !parent->max_average_bw)
 555                 parent = parent->parent;
 556
 557         if (parent->max_average_bw)
 558                 share = div64_u64(div_u64(rate * 100, BYTES_IN_MBIT),
 559                                   parent->max_average_bw);
 560         else
 561                 share = 101;
 562
 563         *bw_share = share == 0 ? 1 : share > 100 ? 0 : share;
 564
 565         qos_dbg(priv->mdev, "Convert: rate %llu, parent ceil %llu -> bw_share %u\n",
 566                 rate, (u64)parent->max_average_bw * BYTES_IN_MBIT, *bw_share);
 567
 568         return 0;
 569 }
 570
 571 static void mlx5e_htb_convert_ceil(struct mlx5e_priv *priv, u64 ceil, u32 *max_average_bw)
 572 {
 573         /* Hardware treats 0 as "unlimited", set at least 1. */
 574         *max_average_bw = max_t(u32, div_u64(ceil, BYTES_IN_MBIT), 1);
 575
 576         qos_dbg(priv->mdev, "Convert: ceil %llu -> max_average_bw %u\n",
 577                 ceil, *max_average_bw);
 578 }
 579
 580 int mlx5e_htb_leaf_alloc_queue(struct mlx5e_priv *priv, u16 classid,
 581                                u32 parent_classid, u64 rate, u64 ceil,
 582                                struct netlink_ext_ack *extack)
 583 {
 584         struct mlx5e_qos_node *node, *parent;
 585         int qid;
 586         int err;
 587
 588         qos_dbg(priv->mdev, "TC_HTB_LEAF_ALLOC_QUEUE classid %04x, parent %04x, rate %llu, ceil %llu\n",
 589                 classid, parent_classid, rate, ceil);
 590
 591         qid = mlx5e_find_unused_qos_qid(priv);
 592         if (qid < 0) {
 593                 NL_SET_ERR_MSG_MOD(extack, "Maximum amount of leaf classes is reached.");
 594                 return qid;
 595         }
 596
 597         parent = mlx5e_sw_node_find(priv, parent_classid);
 598         if (!parent)
 599                 return -EINVAL;
 600
 601         node = mlx5e_sw_node_create_leaf(priv, classid, qid, parent);
 602         if (IS_ERR(node))
 603                 return PTR_ERR(node);
 604
 605         node->rate = rate;
 606         mlx5e_htb_convert_rate(priv, rate, node->parent, &node->bw_share);
 607         mlx5e_htb_convert_ceil(priv, ceil, &node->max_average_bw);
 608
 609         err = mlx5_qos_create_leaf_node(priv->mdev, node->parent->hw_id,
 610                                         node->bw_share, node->max_average_bw,
 611                                         &node->hw_id);
 612         if (err) {
 613                 NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating a leaf node.");
 614                 qos_err(priv->mdev, "Failed to create a leaf node (class %04x), err = %d\n",
 615                         classid, err);
 616                 mlx5e_sw_node_delete(priv, node);
 617                 return err;
 618         }
 619
 620         if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
 621                 err = mlx5e_open_qos_sq(priv, &priv->channels, node);
 622                 if (err) {
 623                         NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
 624                         qos_warn(priv->mdev, "Failed to create a QoS SQ (class %04x), err = %d\n",
 625                                  classid, err);
 626                 } else {
 627                         mlx5e_activate_qos_sq(priv, node);
 628                 }
 629         }
 630
 631         return mlx5e_qid_from_qos(&priv->channels, node->qid);
 632 }
 633
 634 int mlx5e_htb_leaf_to_inner(struct mlx5e_priv *priv, u16 classid, u16 child_classid,
 635                             u64 rate, u64 ceil, struct netlink_ext_ack *extack)
 636 {
 637         struct mlx5e_qos_node *node, *child;
 638         int err, tmp_err;
 639         u32 new_hw_id;
 640         u16 qid;
 641
 642         qos_dbg(priv->mdev, "TC_HTB_LEAF_TO_INNER classid %04x, upcoming child %04x, rate %llu, ceil %llu\n",
 643                 classid, child_classid, rate, ceil);
 644
 645         node = mlx5e_sw_node_find(priv, classid);
 646         if (!node)
 647                 return -ENOENT;
 648
 649         err = mlx5_qos_create_inner_node(priv->mdev, node->parent->hw_id,
 650                                          node->bw_share, node->max_average_bw,
 651                                          &new_hw_id);
 652         if (err) {
 653                 NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating an inner node.");
 654                 qos_err(priv->mdev, "Failed to create an inner node (class %04x), err = %d\n",
 655                         classid, err);
 656                 return err;
 657         }
 658
 659         /* Intentionally reuse the qid for the upcoming first child. */
 660         child = mlx5e_sw_node_create_leaf(priv, child_classid, node->qid, node);
 661         if (IS_ERR(child)) {
 662                 err = PTR_ERR(child);
 663                 goto err_destroy_hw_node;
 664         }
 665
 666         child->rate = rate;
 667         mlx5e_htb_convert_rate(priv, rate, node, &child->bw_share);
 668         mlx5e_htb_convert_ceil(priv, ceil, &child->max_average_bw);
 669
 670         err = mlx5_qos_create_leaf_node(priv->mdev, new_hw_id, child->bw_share,
 671                                         child->max_average_bw, &child->hw_id);
 672         if (err) {
 673                 NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating a leaf node.");
 674                 qos_err(priv->mdev, "Failed to create a leaf node (class %04x), err = %d\n",
 675                         classid, err);
 676                 goto err_delete_sw_node;
 677         }
 678
 679         /* No fail point. */
 680
 681         qid = node->qid;
 682         /* Pairs with mlx5e_get_txq_by_classid. */
 683         WRITE_ONCE(node->qid, MLX5E_QOS_QID_INNER);
 684
 685         if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
 686                 mlx5e_deactivate_qos_sq(priv, qid);
 687                 mlx5e_close_qos_sq(priv, qid);
 688         }
 689
 690         err = mlx5_qos_destroy_node(priv->mdev, node->hw_id);
 691         if (err) /* Not fatal. */
 692                 qos_warn(priv->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
 693                          node->hw_id, classid, err);
 694
 695         node->hw_id = new_hw_id;
 696
 697         if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
 698                 err = mlx5e_open_qos_sq(priv, &priv->channels, child);
 699                 if (err) {
 700                         NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
 701                         qos_warn(priv->mdev, "Failed to create a QoS SQ (class %04x), err = %d\n",
 702                                  classid, err);
 703                 } else {
 704                         mlx5e_activate_qos_sq(priv, child);
 705                 }
 706         }
 707
 708         return 0;
 709
 710 err_delete_sw_node:
 711         child->qid = MLX5E_QOS_QID_INNER;
 712         mlx5e_sw_node_delete(priv, child);
 713
 714 err_destroy_hw_node:
 715         tmp_err = mlx5_qos_destroy_node(priv->mdev, new_hw_id);
 716         if (tmp_err) /* Not fatal. */
 717                 qos_warn(priv->mdev, "Failed to roll back creation of an inner node %u (class %04x), err = %d\n",
 718                          new_hw_id, classid, tmp_err);
 719         return err;
 720 }
 721
 722 static struct mlx5e_qos_node *mlx5e_sw_node_find_by_qid(struct mlx5e_priv *priv, u16 qid)
 723 {
 724         struct mlx5e_qos_node *node = NULL;
 725         int bkt;
 726
 727         hash_for_each(priv->htb.qos_tc2node, bkt, node, hnode)
 728                 if (node->qid == qid)
 729                         break;
 730
 731         return node;
 732 }
 733
 734 static void mlx5e_reactivate_qos_sq(struct mlx5e_priv *priv, u16 qid, struct netdev_queue *txq)
 735 {
 736         qos_dbg(priv->mdev, "Reactivate QoS SQ qid %u\n", qid);
 737         netdev_tx_reset_queue(txq);
 738         netif_tx_start_queue(txq);
 739 }
 740
 741 static void mlx5e_reset_qdisc(struct net_device *dev, u16 qid)
 742 {
 743         struct netdev_queue *dev_queue = netdev_get_tx_queue(dev, qid);
 744         struct Qdisc *qdisc = dev_queue->qdisc_sleeping;
 745
 746         if (!qdisc)
 747                 return;
 748
 749         spin_lock_bh(qdisc_lock(qdisc));
 750         qdisc_reset(qdisc);
 751         spin_unlock_bh(qdisc_lock(qdisc));
 752 }
 753
 754 int mlx5e_htb_leaf_del(struct mlx5e_priv *priv, u16 *classid,
 755                        struct netlink_ext_ack *extack)
 756 {
 757         struct mlx5e_qos_node *node;
 758         struct netdev_queue *txq;
 759         u16 qid, moved_qid;
 760         bool opened;
 761         int err;
 762
 763         qos_dbg(priv->mdev, "TC_HTB_LEAF_DEL classid %04x\n", *classid);
 764
 765         node = mlx5e_sw_node_find(priv, *classid);
 766         if (!node)
 767                 return -ENOENT;
 768
 769         /* Store qid for reuse. */
 770         qid = node->qid;
 771
 772         opened = test_bit(MLX5E_STATE_OPENED, &priv->state);
 773         if (opened) {
 774                 txq = netdev_get_tx_queue(priv->netdev,
 775                                           mlx5e_qid_from_qos(&priv->channels, qid));
 776                 mlx5e_deactivate_qos_sq(priv, qid);
 777                 mlx5e_close_qos_sq(priv, qid);
 778         }
 779
 780         err = mlx5_qos_destroy_node(priv->mdev, node->hw_id);
 781         if (err) /* Not fatal. */
 782                 qos_warn(priv->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
 783                          node->hw_id, *classid, err);
 784
 785         mlx5e_sw_node_delete(priv, node);
 786
 787         moved_qid = mlx5e_qos_cur_leaf_nodes(priv);
 788
 789         if (moved_qid == 0) {
 790                 /* The last QoS SQ was just destroyed. */
 791                 if (opened)
 792                         mlx5e_reactivate_qos_sq(priv, qid, txq);
 793                 return 0;
 794         }
 795         moved_qid--;
 796
 797         if (moved_qid < qid) {
 798                 /* The highest QoS SQ was just destroyed. */
 799                 WARN(moved_qid != qid - 1, "Gaps in queue numeration: destroyed queue %u, the highest queue is %u",
 800                      qid, moved_qid);
 801                 if (opened)
 802                         mlx5e_reactivate_qos_sq(priv, qid, txq);
 803                 return 0;
 804         }
 805
 806         WARN(moved_qid == qid, "Can't move node with qid %u to itself", qid);
 807         qos_dbg(priv->mdev, "Moving QoS SQ %u to %u\n", moved_qid, qid);
 808
 809         node = mlx5e_sw_node_find_by_qid(priv, moved_qid);
 810         WARN(!node, "Could not find a node with qid %u to move to queue %u",
 811              moved_qid, qid);
 812
 813         /* Stop traffic to the old queue. */
 814         WRITE_ONCE(node->qid, MLX5E_QOS_QID_INNER);
 815         __clear_bit(moved_qid, priv->htb.qos_used_qids);
 816
 817         if (opened) {
 818                 txq = netdev_get_tx_queue(priv->netdev,
 819                                           mlx5e_qid_from_qos(&priv->channels, moved_qid));
 820                 mlx5e_deactivate_qos_sq(priv, moved_qid);
 821                 mlx5e_close_qos_sq(priv, moved_qid);
 822         }
 823
 824         /* Prevent packets from the old class from getting into the new one. */
 825         mlx5e_reset_qdisc(priv->netdev, moved_qid);
 826
 827         __set_bit(qid, priv->htb.qos_used_qids);
 828         WRITE_ONCE(node->qid, qid);
 829
 830         if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
 831                 err = mlx5e_open_qos_sq(priv, &priv->channels, node);
 832                 if (err) {
 833                         NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
 834                         qos_warn(priv->mdev, "Failed to create a QoS SQ (class %04x) while moving qid %u to %u, err = %d\n",
 835                                  node->classid, moved_qid, qid, err);
 836                 } else {
 837                         mlx5e_activate_qos_sq(priv, node);
 838                 }
 839         }
 840
 841         mlx5e_update_tx_netdev_queues(priv);
 842         if (opened)
 843                 mlx5e_reactivate_qos_sq(priv, moved_qid, txq);
 844
 845         *classid = node->classid;
 846         return 0;
 847 }
 848
 849 int mlx5e_htb_leaf_del_last(struct mlx5e_priv *priv, u16 classid, bool force,
 850                             struct netlink_ext_ack *extack)
 851 {
 852         struct mlx5e_qos_node *node, *parent;
 853         u32 old_hw_id, new_hw_id;
 854         int err, saved_err = 0;
 855         u16 qid;
 856
 857         qos_dbg(priv->mdev, "TC_HTB_LEAF_DEL_LAST%s classid %04x\n",
 858                 force ? "_FORCE" : "", classid);
 859
 860         node = mlx5e_sw_node_find(priv, classid);
 861         if (!node)
 862                 return -ENOENT;
 863
 864         err = mlx5_qos_create_leaf_node(priv->mdev, node->parent->parent->hw_id,
 865                                         node->parent->bw_share,
 866                                         node->parent->max_average_bw,
 867                                         &new_hw_id);
 868         if (err) {
 869                 NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating a leaf node.");
 870                 qos_err(priv->mdev, "Failed to create a leaf node (class %04x), err = %d\n",
 871                         classid, err);
 872                 if (!force)
 873                         return err;
 874                 saved_err = err;
 875         }
 876
 877         /* Store qid for reuse and prevent clearing the bit. */
 878         qid = node->qid;
 879         /* Pairs with mlx5e_get_txq_by_classid. */
 880         WRITE_ONCE(node->qid, MLX5E_QOS_QID_INNER);
 881
 882         if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
 883                 mlx5e_deactivate_qos_sq(priv, qid);
 884                 mlx5e_close_qos_sq(priv, qid);
 885         }
 886
 887         /* Prevent packets from the old class from getting into the new one. */
 888         mlx5e_reset_qdisc(priv->netdev, qid);
 889
 890         err = mlx5_qos_destroy_node(priv->mdev, node->hw_id);
 891         if (err) /* Not fatal. */
 892                 qos_warn(priv->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
 893                          node->hw_id, classid, err);
 894
 895         parent = node->parent;
 896         mlx5e_sw_node_delete(priv, node);
 897
 898         node = parent;
 899         WRITE_ONCE(node->qid, qid);
 900
 901         /* Early return on error in force mode. Parent will still be an inner
 902          * node to be deleted by a following delete operation.
 903          */
 904         if (saved_err)
 905                 return saved_err;
 906
 907         old_hw_id = node->hw_id;
 908         node->hw_id = new_hw_id;
 909
 910         if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
 911                 err = mlx5e_open_qos_sq(priv, &priv->channels, node);
 912                 if (err) {
 913                         NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
 914                         qos_warn(priv->mdev, "Failed to create a QoS SQ (class %04x), err = %d\n",
 915                                  classid, err);
 916                 } else {
 917                         mlx5e_activate_qos_sq(priv, node);
 918                 }
 919         }
 920
 921         err = mlx5_qos_destroy_node(priv->mdev, old_hw_id);
 922         if (err) /* Not fatal. */
 923                 qos_warn(priv->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
 924                          node->hw_id, classid, err);
 925
 926         return 0;
 927 }
 928
 929 static int mlx5e_qos_update_children(struct mlx5e_priv *priv, struct mlx5e_qos_node *node,
 930                                      struct netlink_ext_ack *extack)
 931 {
 932         struct mlx5e_qos_node *child;
 933         int err = 0;
 934         int bkt;
 935
 936         hash_for_each(priv->htb.qos_tc2node, bkt, child, hnode) {
 937                 u32 old_bw_share = child->bw_share;
 938                 int err_one;
 939
 940                 if (child->parent != node)
 941                         continue;
 942
 943                 mlx5e_htb_convert_rate(priv, child->rate, node, &child->bw_share);
 944                 if (child->bw_share == old_bw_share)
 945                         continue;
 946
 947                 err_one = mlx5_qos_update_node(priv->mdev, child->hw_id, child->bw_share,
 948                                                child->max_average_bw, child->hw_id);
 949                 if (!err && err_one) {
 950                         err = err_one;
 951
 952                         NL_SET_ERR_MSG_MOD(extack, "Firmware error when modifying a child node.");
 953                         qos_err(priv->mdev, "Failed to modify a child node (class %04x), err = %d\n",
 954                                 node->classid, err);
 955                 }
 956         }
 957
 958         return err;
 959 }
 960
 961 int mlx5e_htb_node_modify(struct mlx5e_priv *priv, u16 classid, u64 rate, u64 ceil,
 962                           struct netlink_ext_ack *extack)
 963 {
 964         u32 bw_share, max_average_bw;
 965         struct mlx5e_qos_node *node;
 966         bool ceil_changed = false;
 967         int err;
 968
 969         qos_dbg(priv->mdev, "TC_HTB_LEAF_MODIFY classid %04x, rate %llu, ceil %llu\n",
 970                 classid, rate, ceil);
 971
 972         node = mlx5e_sw_node_find(priv, classid);
 973         if (!node)
 974                 return -ENOENT;
 975
 976         node->rate = rate;
 977         mlx5e_htb_convert_rate(priv, rate, node->parent, &bw_share);
 978         mlx5e_htb_convert_ceil(priv, ceil, &max_average_bw);
 979
 980         err = mlx5_qos_update_node(priv->mdev, node->parent->hw_id, bw_share,
 981                                    max_average_bw, node->hw_id);
 982         if (err) {
 983                 NL_SET_ERR_MSG_MOD(extack, "Firmware error when modifying a node.");
 984                 qos_err(priv->mdev, "Failed to modify a node (class %04x), err = %d\n",
 985                         classid, err);
 986                 return err;
 987         }
 988
 989         if (max_average_bw != node->max_average_bw)
 990                 ceil_changed = true;
 991
 992         node->bw_share = bw_share;
 993         node->max_average_bw = max_average_bw;
 994
 995         if (ceil_changed)
 996                 err = mlx5e_qos_update_children(priv, node, extack);
 997
 998         return err;
 999 }
1000
1001 struct mlx5e_mqprio_rl {
1002         struct mlx5_core_dev *mdev;
1003         u32 root_id;
1004         u32 *leaves_id;
1005         u8 num_tc;
1006 };
1007
1008 struct mlx5e_mqprio_rl *mlx5e_mqprio_rl_alloc(void)
1009 {
1010         return kvzalloc(sizeof(struct mlx5e_mqprio_rl), GFP_KERNEL);
1011 }
1012
1013 void mlx5e_mqprio_rl_free(struct mlx5e_mqprio_rl *rl)
1014 {
1015         kvfree(rl);
1016 }
1017
1018 int mlx5e_mqprio_rl_init(struct mlx5e_mqprio_rl *rl, struct mlx5_core_dev *mdev, u8 num_tc,
1019                          u64 max_rate[])
1020 {
1021         int err;
1022         int tc;
1023
1024         if (!mlx5_qos_is_supported(mdev)) {
1025                 qos_warn(mdev, "Missing QoS capabilities. Try disabling SRIOV or use a supported device.");
1026                 return -EOPNOTSUPP;
1027         }
1028         if (num_tc > mlx5e_qos_max_leaf_nodes(mdev))
1029                 return -EINVAL;
1030
1031         rl->mdev = mdev;
1032         rl->num_tc = num_tc;
1033         rl->leaves_id = kvcalloc(num_tc, sizeof(*rl->leaves_id), GFP_KERNEL);
1034         if (!rl->leaves_id)
1035                 return -ENOMEM;
1036
1037         err = mlx5_qos_create_root_node(mdev, &rl->root_id);
1038         if (err)
1039                 goto err_free_leaves;
1040
1041         qos_dbg(mdev, "Root created, id %#x\n", rl->root_id);
1042
1043         for (tc = 0; tc < num_tc; tc++) {
1044                 u32 max_average_bw;
1045
1046                 max_average_bw = mlx5e_qos_bytes2mbits(mdev, max_rate[tc]);
1047                 err = mlx5_qos_create_leaf_node(mdev, rl->root_id, 0, max_average_bw,
1048                                                 &rl->leaves_id[tc]);
1049                 if (err)
1050                         goto err_destroy_leaves;
1051
1052                 qos_dbg(mdev, "Leaf[%d] created, id %#x, max average bw %u Mbits/sec\n",
1053                         tc, rl->leaves_id[tc], max_average_bw);
1054         }
1055         return 0;
1056
1057 err_destroy_leaves:
1058         while (--tc >= 0)
1059                 mlx5_qos_destroy_node(mdev, rl->leaves_id[tc]);
1060         mlx5_qos_destroy_node(mdev, rl->root_id);
1061 err_free_leaves:
1062         kvfree(rl->leaves_id);
1063         return err;
1064 }
1065
1066 void mlx5e_mqprio_rl_cleanup(struct mlx5e_mqprio_rl *rl)
1067 {
1068         int tc;
1069
1070         for (tc = 0; tc < rl->num_tc; tc++)
1071                 mlx5_qos_destroy_node(rl->mdev, rl->leaves_id[tc]);
1072         mlx5_qos_destroy_node(rl->mdev, rl->root_id);
1073         kvfree(rl->leaves_id);
1074 }
1075
1076 int mlx5e_mqprio_rl_get_node_hw_id(struct mlx5e_mqprio_rl *rl, int tc, u32 *hw_id)
1077 {
1078         if (tc >= rl->num_tc)
1079                 return -EINVAL;
1080
1081         *hw_id = rl->leaves_id[tc];
1082         return 0;
1083 }