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