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