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(¶m_sq, 0, sizeof(param_sq)); 250 memset(¶m_cq, 0, sizeof(param_cq)); 251 mlx5e_build_sq_param(priv->mdev, params, ¶m_sq); 252 mlx5e_build_tx_cq_param(priv->mdev, params, ¶m_cq); 253 err = mlx5e_open_cq(priv, params->tx_cq_moderation, ¶m_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 ¶m_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 } 1083