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(¶m_sq, 0, sizeof(param_sq)); 234 memset(¶m_cq, 0, sizeof(param_cq)); 235 mlx5e_build_sq_param(priv, params, ¶m_sq); 236 mlx5e_build_tx_cq_param(priv, params, ¶m_cq); 237 err = mlx5e_open_cq(priv, params->tx_cq_moderation, ¶m_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 ¶m_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 } 985