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         *max_average_bw = div_u64(ceil, BYTES_IN_MBIT);
 574
 575         qos_dbg(priv->mdev, "Convert: ceil %llu -> max_average_bw %u\n",
 576                 ceil, *max_average_bw);
 577 }
 578
 579 int mlx5e_htb_leaf_alloc_queue(struct mlx5e_priv *priv, u16 classid,
 580                                u32 parent_classid, u64 rate, u64 ceil,
 581                                struct netlink_ext_ack *extack)
 582 {
 583         struct mlx5e_qos_node *node, *parent;
 584         int qid;
 585         int err;
 586
 587         qos_dbg(priv->mdev, "TC_HTB_LEAF_ALLOC_QUEUE classid %04x, parent %04x, rate %llu, ceil %llu\n",
 588                 classid, parent_classid, rate, ceil);
 589
 590         qid = mlx5e_find_unused_qos_qid(priv);
 591         if (qid < 0) {
 592                 NL_SET_ERR_MSG_MOD(extack, "Maximum amount of leaf classes is reached.");
 593                 return qid;
 594         }
 595
 596         parent = mlx5e_sw_node_find(priv, parent_classid);
 597         if (!parent)
 598                 return -EINVAL;
 599
 600         node = mlx5e_sw_node_create_leaf(priv, classid, qid, parent);
 601         if (IS_ERR(node))
 602                 return PTR_ERR(node);
 603
 604         node->rate = rate;
 605         mlx5e_htb_convert_rate(priv, rate, node->parent, &node->bw_share);
 606         mlx5e_htb_convert_ceil(priv, ceil, &node->max_average_bw);
 607
 608         err = mlx5_qos_create_leaf_node(priv->mdev, node->parent->hw_id,
 609                                         node->bw_share, node->max_average_bw,
 610                                         &node->hw_id);
 611         if (err) {
 612                 NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating a leaf node.");
 613                 qos_err(priv->mdev, "Failed to create a leaf node (class %04x), err = %d\n",
 614                         classid, err);
 615                 mlx5e_sw_node_delete(priv, node);
 616                 return err;
 617         }
 618
 619         if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
 620                 err = mlx5e_open_qos_sq(priv, &priv->channels, node);
 621                 if (err) {
 622                         NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
 623                         qos_warn(priv->mdev, "Failed to create a QoS SQ (class %04x), err = %d\n",
 624                                  classid, err);
 625                 } else {
 626                         mlx5e_activate_qos_sq(priv, node);
 627                 }
 628         }
 629
 630         return mlx5e_qid_from_qos(&priv->channels, node->qid);
 631 }
 632
 633 int mlx5e_htb_leaf_to_inner(struct mlx5e_priv *priv, u16 classid, u16 child_classid,
 634                             u64 rate, u64 ceil, struct netlink_ext_ack *extack)
 635 {
 636         struct mlx5e_qos_node *node, *child;
 637         int err, tmp_err;
 638         u32 new_hw_id;
 639         u16 qid;
 640
 641         qos_dbg(priv->mdev, "TC_HTB_LEAF_TO_INNER classid %04x, upcoming child %04x, rate %llu, ceil %llu\n",
 642                 classid, child_classid, rate, ceil);
 643
 644         node = mlx5e_sw_node_find(priv, classid);
 645         if (!node)
 646                 return -ENOENT;
 647
 648         err = mlx5_qos_create_inner_node(priv->mdev, node->parent->hw_id,
 649                                          node->bw_share, node->max_average_bw,
 650                                          &new_hw_id);
 651         if (err) {
 652                 NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating an inner node.");
 653                 qos_err(priv->mdev, "Failed to create an inner node (class %04x), err = %d\n",
 654                         classid, err);
 655                 return err;
 656         }
 657
 658         /* Intentionally reuse the qid for the upcoming first child. */
 659         child = mlx5e_sw_node_create_leaf(priv, child_classid, node->qid, node);
 660         if (IS_ERR(child)) {
 661                 err = PTR_ERR(child);
 662                 goto err_destroy_hw_node;
 663         }
 664
 665         child->rate = rate;
 666         mlx5e_htb_convert_rate(priv, rate, node, &child->bw_share);
 667         mlx5e_htb_convert_ceil(priv, ceil, &child->max_average_bw);
 668
 669         err = mlx5_qos_create_leaf_node(priv->mdev, new_hw_id, child->bw_share,
 670                                         child->max_average_bw, &child->hw_id);
 671         if (err) {
 672                 NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating a leaf node.");
 673                 qos_err(priv->mdev, "Failed to create a leaf node (class %04x), err = %d\n",
 674                         classid, err);
 675                 goto err_delete_sw_node;
 676         }
 677
 678         /* No fail point. */
 679
 680         qid = node->qid;
 681         /* Pairs with mlx5e_get_txq_by_classid. */
 682         WRITE_ONCE(node->qid, MLX5E_QOS_QID_INNER);
 683
 684         if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
 685                 mlx5e_deactivate_qos_sq(priv, qid);
 686                 mlx5e_close_qos_sq(priv, qid);
 687         }
 688
 689         err = mlx5_qos_destroy_node(priv->mdev, node->hw_id);
 690         if (err) /* Not fatal. */
 691                 qos_warn(priv->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
 692                          node->hw_id, classid, err);
 693
 694         node->hw_id = new_hw_id;
 695
 696         if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
 697                 err = mlx5e_open_qos_sq(priv, &priv->channels, child);
 698                 if (err) {
 699                         NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
 700                         qos_warn(priv->mdev, "Failed to create a QoS SQ (class %04x), err = %d\n",
 701                                  classid, err);
 702                 } else {
 703                         mlx5e_activate_qos_sq(priv, child);
 704                 }
 705         }
 706
 707         return 0;
 708
 709 err_delete_sw_node:
 710         child->qid = MLX5E_QOS_QID_INNER;
 711         mlx5e_sw_node_delete(priv, child);
 712
 713 err_destroy_hw_node:
 714         tmp_err = mlx5_qos_destroy_node(priv->mdev, new_hw_id);
 715         if (tmp_err) /* Not fatal. */
 716                 qos_warn(priv->mdev, "Failed to roll back creation of an inner node %u (class %04x), err = %d\n",
 717                          new_hw_id, classid, tmp_err);
 718         return err;
 719 }
 720
 721 static struct mlx5e_qos_node *mlx5e_sw_node_find_by_qid(struct mlx5e_priv *priv, u16 qid)
 722 {
 723         struct mlx5e_qos_node *node = NULL;
 724         int bkt;
 725
 726         hash_for_each(priv->htb.qos_tc2node, bkt, node, hnode)
 727                 if (node->qid == qid)
 728                         break;
 729
 730         return node;
 731 }
 732
 733 static void mlx5e_reactivate_qos_sq(struct mlx5e_priv *priv, u16 qid, struct netdev_queue *txq)
 734 {
 735         qos_dbg(priv->mdev, "Reactivate QoS SQ qid %u\n", qid);
 736         netdev_tx_reset_queue(txq);
 737         netif_tx_start_queue(txq);
 738 }
 739
 740 static void mlx5e_reset_qdisc(struct net_device *dev, u16 qid)
 741 {
 742         struct netdev_queue *dev_queue = netdev_get_tx_queue(dev, qid);
 743         struct Qdisc *qdisc = dev_queue->qdisc_sleeping;
 744
 745         if (!qdisc)
 746                 return;
 747
 748         spin_lock_bh(qdisc_lock(qdisc));
 749         qdisc_reset(qdisc);
 750         spin_unlock_bh(qdisc_lock(qdisc));
 751 }
 752
 753 int mlx5e_htb_leaf_del(struct mlx5e_priv *priv, u16 *classid,
 754                        struct netlink_ext_ack *extack)
 755 {
 756         struct mlx5e_qos_node *node;
 757         struct netdev_queue *txq;
 758         u16 qid, moved_qid;
 759         bool opened;
 760         int err;
 761
 762         qos_dbg(priv->mdev, "TC_HTB_LEAF_DEL classid %04x\n", *classid);
 763
 764         node = mlx5e_sw_node_find(priv, *classid);
 765         if (!node)
 766                 return -ENOENT;
 767
 768         /* Store qid for reuse. */
 769         qid = node->qid;
 770
 771         opened = test_bit(MLX5E_STATE_OPENED, &priv->state);
 772         if (opened) {
 773                 txq = netdev_get_tx_queue(priv->netdev,
 774                                           mlx5e_qid_from_qos(&priv->channels, qid));
 775                 mlx5e_deactivate_qos_sq(priv, qid);
 776                 mlx5e_close_qos_sq(priv, qid);
 777         }
 778
 779         err = mlx5_qos_destroy_node(priv->mdev, node->hw_id);
 780         if (err) /* Not fatal. */
 781                 qos_warn(priv->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
 782                          node->hw_id, *classid, err);
 783
 784         mlx5e_sw_node_delete(priv, node);
 785
 786         moved_qid = mlx5e_qos_cur_leaf_nodes(priv);
 787
 788         if (moved_qid == 0) {
 789                 /* The last QoS SQ was just destroyed. */
 790                 if (opened)
 791                         mlx5e_reactivate_qos_sq(priv, qid, txq);
 792                 return 0;
 793         }
 794         moved_qid--;
 795
 796         if (moved_qid < qid) {
 797                 /* The highest QoS SQ was just destroyed. */
 798                 WARN(moved_qid != qid - 1, "Gaps in queue numeration: destroyed queue %u, the highest queue is %u",
 799                      qid, moved_qid);
 800                 if (opened)
 801                         mlx5e_reactivate_qos_sq(priv, qid, txq);
 802                 return 0;
 803         }
 804
 805         WARN(moved_qid == qid, "Can't move node with qid %u to itself", qid);
 806         qos_dbg(priv->mdev, "Moving QoS SQ %u to %u\n", moved_qid, qid);
 807
 808         node = mlx5e_sw_node_find_by_qid(priv, moved_qid);
 809         WARN(!node, "Could not find a node with qid %u to move to queue %u",
 810              moved_qid, qid);
 811
 812         /* Stop traffic to the old queue. */
 813         WRITE_ONCE(node->qid, MLX5E_QOS_QID_INNER);
 814         __clear_bit(moved_qid, priv->htb.qos_used_qids);
 815
 816         if (opened) {
 817                 txq = netdev_get_tx_queue(priv->netdev,
 818                                           mlx5e_qid_from_qos(&priv->channels, moved_qid));
 819                 mlx5e_deactivate_qos_sq(priv, moved_qid);
 820                 mlx5e_close_qos_sq(priv, moved_qid);
 821         }
 822
 823         /* Prevent packets from the old class from getting into the new one. */
 824         mlx5e_reset_qdisc(priv->netdev, moved_qid);
 825
 826         __set_bit(qid, priv->htb.qos_used_qids);
 827         WRITE_ONCE(node->qid, qid);
 828
 829         if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
 830                 err = mlx5e_open_qos_sq(priv, &priv->channels, node);
 831                 if (err) {
 832                         NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
 833                         qos_warn(priv->mdev, "Failed to create a QoS SQ (class %04x) while moving qid %u to %u, err = %d\n",
 834                                  node->classid, moved_qid, qid, err);
 835                 } else {
 836                         mlx5e_activate_qos_sq(priv, node);
 837                 }
 838         }
 839
 840         mlx5e_update_tx_netdev_queues(priv);
 841         if (opened)
 842                 mlx5e_reactivate_qos_sq(priv, moved_qid, txq);
 843
 844         *classid = node->classid;
 845         return 0;
 846 }
 847
 848 int mlx5e_htb_leaf_del_last(struct mlx5e_priv *priv, u16 classid, bool force,
 849                             struct netlink_ext_ack *extack)
 850 {
 851         struct mlx5e_qos_node *node, *parent;
 852         u32 old_hw_id, new_hw_id;
 853         int err, saved_err = 0;
 854         u16 qid;
 855
 856         qos_dbg(priv->mdev, "TC_HTB_LEAF_DEL_LAST%s classid %04x\n",
 857                 force ? "_FORCE" : "", classid);
 858
 859         node = mlx5e_sw_node_find(priv, classid);
 860         if (!node)
 861                 return -ENOENT;
 862
 863         err = mlx5_qos_create_leaf_node(priv->mdev, node->parent->parent->hw_id,
 864                                         node->parent->bw_share,
 865                                         node->parent->max_average_bw,
 866                                         &new_hw_id);
 867         if (err) {
 868                 NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating a leaf node.");
 869                 qos_err(priv->mdev, "Failed to create a leaf node (class %04x), err = %d\n",
 870                         classid, err);
 871                 if (!force)
 872                         return err;
 873                 saved_err = err;
 874         }
 875
 876         /* Store qid for reuse and prevent clearing the bit. */
 877         qid = node->qid;
 878         /* Pairs with mlx5e_get_txq_by_classid. */
 879         WRITE_ONCE(node->qid, MLX5E_QOS_QID_INNER);
 880
 881         if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
 882                 mlx5e_deactivate_qos_sq(priv, qid);
 883                 mlx5e_close_qos_sq(priv, qid);
 884         }
 885
 886         /* Prevent packets from the old class from getting into the new one. */
 887         mlx5e_reset_qdisc(priv->netdev, qid);
 888
 889         err = mlx5_qos_destroy_node(priv->mdev, node->hw_id);
 890         if (err) /* Not fatal. */
 891                 qos_warn(priv->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
 892                          node->hw_id, classid, err);
 893
 894         parent = node->parent;
 895         mlx5e_sw_node_delete(priv, node);
 896
 897         node = parent;
 898         WRITE_ONCE(node->qid, qid);
 899
 900         /* Early return on error in force mode. Parent will still be an inner
 901          * node to be deleted by a following delete operation.
 902          */
 903         if (saved_err)
 904                 return saved_err;
 905
 906         old_hw_id = node->hw_id;
 907         node->hw_id = new_hw_id;
 908
 909         if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
 910                 err = mlx5e_open_qos_sq(priv, &priv->channels, node);
 911                 if (err) {
 912                         NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
 913                         qos_warn(priv->mdev, "Failed to create a QoS SQ (class %04x), err = %d\n",
 914                                  classid, err);
 915                 } else {
 916                         mlx5e_activate_qos_sq(priv, node);
 917                 }
 918         }
 919
 920         err = mlx5_qos_destroy_node(priv->mdev, old_hw_id);
 921         if (err) /* Not fatal. */
 922                 qos_warn(priv->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
 923                          node->hw_id, classid, err);
 924
 925         return 0;
 926 }
 927
 928 static int mlx5e_qos_update_children(struct mlx5e_priv *priv, struct mlx5e_qos_node *node,
 929                                      struct netlink_ext_ack *extack)
 930 {
 931         struct mlx5e_qos_node *child;
 932         int err = 0;
 933         int bkt;
 934
 935         hash_for_each(priv->htb.qos_tc2node, bkt, child, hnode) {
 936                 u32 old_bw_share = child->bw_share;
 937                 int err_one;
 938
 939                 if (child->parent != node)
 940                         continue;
 941
 942                 mlx5e_htb_convert_rate(priv, child->rate, node, &child->bw_share);
 943                 if (child->bw_share == old_bw_share)
 944                         continue;
 945
 946                 err_one = mlx5_qos_update_node(priv->mdev, child->hw_id, child->bw_share,
 947                                                child->max_average_bw, child->hw_id);
 948                 if (!err && err_one) {
 949                         err = err_one;
 950
 951                         NL_SET_ERR_MSG_MOD(extack, "Firmware error when modifying a child node.");
 952                         qos_err(priv->mdev, "Failed to modify a child node (class %04x), err = %d\n",
 953                                 node->classid, err);
 954                 }
 955         }
 956
 957         return err;
 958 }
 959
 960 int mlx5e_htb_node_modify(struct mlx5e_priv *priv, u16 classid, u64 rate, u64 ceil,
 961                           struct netlink_ext_ack *extack)
 962 {
 963         u32 bw_share, max_average_bw;
 964         struct mlx5e_qos_node *node;
 965         bool ceil_changed = false;
 966         int err;
 967
 968         qos_dbg(priv->mdev, "TC_HTB_LEAF_MODIFY classid %04x, rate %llu, ceil %llu\n",
 969                 classid, rate, ceil);
 970
 971         node = mlx5e_sw_node_find(priv, classid);
 972         if (!node)
 973                 return -ENOENT;
 974
 975         node->rate = rate;
 976         mlx5e_htb_convert_rate(priv, rate, node->parent, &bw_share);
 977         mlx5e_htb_convert_ceil(priv, ceil, &max_average_bw);
 978
 979         err = mlx5_qos_update_node(priv->mdev, node->parent->hw_id, bw_share,
 980                                    max_average_bw, node->hw_id);
 981         if (err) {
 982                 NL_SET_ERR_MSG_MOD(extack, "Firmware error when modifying a node.");
 983                 qos_err(priv->mdev, "Failed to modify a node (class %04x), err = %d\n",
 984                         classid, err);
 985                 return err;
 986         }
 987
 988         if (max_average_bw != node->max_average_bw)
 989                 ceil_changed = true;
 990
 991         node->bw_share = bw_share;
 992         node->max_average_bw = max_average_bw;
 993
 994         if (ceil_changed)
 995                 err = mlx5e_qos_update_children(priv, node, extack);
 996
 997         return err;
 998 }
 999
1000 struct mlx5e_mqprio_rl {
1001         struct mlx5_core_dev *mdev;
1002         u32 root_id;
1003         u32 *leaves_id;
1004         u8 num_tc;
1005 };
1006
1007 struct mlx5e_mqprio_rl *mlx5e_mqprio_rl_alloc(void)
1008 {
1009         return kvzalloc(sizeof(struct mlx5e_mqprio_rl), GFP_KERNEL);
1010 }
1011
1012 void mlx5e_mqprio_rl_free(struct mlx5e_mqprio_rl *rl)
1013 {
1014         kvfree(rl);
1015 }
1016
1017 int mlx5e_mqprio_rl_init(struct mlx5e_mqprio_rl *rl, struct mlx5_core_dev *mdev, u8 num_tc,
1018                          u64 max_rate[])
1019 {
1020         int err;
1021         int tc;
1022
1023         if (!mlx5_qos_is_supported(mdev)) {
1024                 qos_warn(mdev, "Missing QoS capabilities. Try disabling SRIOV or use a supported device.");
1025                 return -EOPNOTSUPP;
1026         }
1027         if (num_tc > mlx5e_qos_max_leaf_nodes(mdev))
1028                 return -EINVAL;
1029
1030         rl->mdev = mdev;
1031         rl->num_tc = num_tc;
1032         rl->leaves_id = kvcalloc(num_tc, sizeof(*rl->leaves_id), GFP_KERNEL);
1033         if (!rl->leaves_id)
1034                 return -ENOMEM;
1035
1036         err = mlx5_qos_create_root_node(mdev, &rl->root_id);
1037         if (err)
1038                 goto err_free_leaves;
1039
1040         qos_dbg(mdev, "Root created, id %#x\n", rl->root_id);
1041
1042         for (tc = 0; tc < num_tc; tc++) {
1043                 u32 max_average_bw;
1044
1045                 max_average_bw = mlx5e_qos_bytes2mbits(mdev, max_rate[tc]);
1046                 err = mlx5_qos_create_leaf_node(mdev, rl->root_id, 0, max_average_bw,
1047                                                 &rl->leaves_id[tc]);
1048                 if (err)
1049                         goto err_destroy_leaves;
1050
1051                 qos_dbg(mdev, "Leaf[%d] created, id %#x, max average bw %u Mbits/sec\n",
1052                         tc, rl->leaves_id[tc], max_average_bw);
1053         }
1054         return 0;
1055
1056 err_destroy_leaves:
1057         while (--tc >= 0)
1058                 mlx5_qos_destroy_node(mdev, rl->leaves_id[tc]);
1059         mlx5_qos_destroy_node(mdev, rl->root_id);
1060 err_free_leaves:
1061         kvfree(rl->leaves_id);
1062         return err;
1063 }
1064
1065 void mlx5e_mqprio_rl_cleanup(struct mlx5e_mqprio_rl *rl)
1066 {
1067         int tc;
1068
1069         for (tc = 0; tc < rl->num_tc; tc++)
1070                 mlx5_qos_destroy_node(rl->mdev, rl->leaves_id[tc]);
1071         mlx5_qos_destroy_node(rl->mdev, rl->root_id);
1072         kvfree(rl->leaves_id);
1073 }
1074
1075 int mlx5e_mqprio_rl_get_node_hw_id(struct mlx5e_mqprio_rl *rl, int tc, u32 *hw_id)
1076 {
1077         if (tc >= rl->num_tc)
1078                 return -EINVAL;
1079
1080         *hw_id = rl->leaves_id[tc];
1081         return 0;
1082 }