xref: /openbmc/linux/drivers/net/ethernet/mellanox/mlx5/core/en/htb.c (revision 9a87ffc99ec8eb8d35eed7c4f816d75f5cc9662e)
1  // SPDX-License-Identifier: GPL-2.0 OR Linux-OpenIB
2  /* Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES. All rights reserved. */
3  
4  #include <net/pkt_cls.h>
5  #include "htb.h"
6  #include "en.h"
7  #include "../qos.h"
8  
9  struct mlx5e_qos_node {
10  	struct hlist_node hnode;
11  	struct mlx5e_qos_node *parent;
12  	u64 rate;
13  	u32 bw_share;
14  	u32 max_average_bw;
15  	u32 hw_id;
16  	u32 classid; /* 16-bit, except root. */
17  	u16 qid;
18  };
19  
20  struct mlx5e_htb {
21  	DECLARE_HASHTABLE(qos_tc2node, order_base_2(MLX5E_QOS_MAX_LEAF_NODES));
22  	DECLARE_BITMAP(qos_used_qids, MLX5E_QOS_MAX_LEAF_NODES);
23  	struct mlx5_core_dev *mdev;
24  	struct net_device *netdev;
25  	struct mlx5e_priv *priv;
26  	struct mlx5e_selq *selq;
27  };
28  
29  #define MLX5E_QOS_QID_INNER 0xffff
30  #define MLX5E_HTB_CLASSID_ROOT 0xffffffff
31  
32  /* Software representation of the QoS tree */
33  
mlx5e_htb_enumerate_leaves(struct mlx5e_htb * htb,mlx5e_fp_htb_enumerate callback,void * data)34  int mlx5e_htb_enumerate_leaves(struct mlx5e_htb *htb, mlx5e_fp_htb_enumerate callback, void *data)
35  {
36  	struct mlx5e_qos_node *node = NULL;
37  	int bkt, err;
38  
39  	hash_for_each(htb->qos_tc2node, bkt, node, hnode) {
40  		if (node->qid == MLX5E_QOS_QID_INNER)
41  			continue;
42  		err = callback(data, node->qid, node->hw_id);
43  		if (err)
44  			return err;
45  	}
46  	return 0;
47  }
48  
mlx5e_htb_cur_leaf_nodes(struct mlx5e_htb * htb)49  int mlx5e_htb_cur_leaf_nodes(struct mlx5e_htb *htb)
50  {
51  	int last;
52  
53  	last = find_last_bit(htb->qos_used_qids, mlx5e_qos_max_leaf_nodes(htb->mdev));
54  	return last == mlx5e_qos_max_leaf_nodes(htb->mdev) ? 0 : last + 1;
55  }
56  
mlx5e_htb_find_unused_qos_qid(struct mlx5e_htb * htb)57  static int mlx5e_htb_find_unused_qos_qid(struct mlx5e_htb *htb)
58  {
59  	int size = mlx5e_qos_max_leaf_nodes(htb->mdev);
60  	struct mlx5e_priv *priv = htb->priv;
61  	int res;
62  
63  	WARN_ONCE(!mutex_is_locked(&priv->state_lock), "%s: state_lock is not held\n", __func__);
64  	res = find_first_zero_bit(htb->qos_used_qids, size);
65  
66  	return res == size ? -ENOSPC : res;
67  }
68  
69  static struct mlx5e_qos_node *
mlx5e_htb_node_create_leaf(struct mlx5e_htb * htb,u16 classid,u16 qid,struct mlx5e_qos_node * parent)70  mlx5e_htb_node_create_leaf(struct mlx5e_htb *htb, u16 classid, u16 qid,
71  			   struct mlx5e_qos_node *parent)
72  {
73  	struct mlx5e_qos_node *node;
74  
75  	node = kzalloc(sizeof(*node), GFP_KERNEL);
76  	if (!node)
77  		return ERR_PTR(-ENOMEM);
78  
79  	node->parent = parent;
80  
81  	node->qid = qid;
82  	__set_bit(qid, htb->qos_used_qids);
83  
84  	node->classid = classid;
85  	hash_add_rcu(htb->qos_tc2node, &node->hnode, classid);
86  
87  	mlx5e_update_tx_netdev_queues(htb->priv);
88  
89  	return node;
90  }
91  
mlx5e_htb_node_create_root(struct mlx5e_htb * htb)92  static struct mlx5e_qos_node *mlx5e_htb_node_create_root(struct mlx5e_htb *htb)
93  {
94  	struct mlx5e_qos_node *node;
95  
96  	node = kzalloc(sizeof(*node), GFP_KERNEL);
97  	if (!node)
98  		return ERR_PTR(-ENOMEM);
99  
100  	node->qid = MLX5E_QOS_QID_INNER;
101  	node->classid = MLX5E_HTB_CLASSID_ROOT;
102  	hash_add_rcu(htb->qos_tc2node, &node->hnode, node->classid);
103  
104  	return node;
105  }
106  
mlx5e_htb_node_find(struct mlx5e_htb * htb,u32 classid)107  static struct mlx5e_qos_node *mlx5e_htb_node_find(struct mlx5e_htb *htb, u32 classid)
108  {
109  	struct mlx5e_qos_node *node = NULL;
110  
111  	hash_for_each_possible(htb->qos_tc2node, node, hnode, classid) {
112  		if (node->classid == classid)
113  			break;
114  	}
115  
116  	return node;
117  }
118  
mlx5e_htb_node_find_rcu(struct mlx5e_htb * htb,u32 classid)119  static struct mlx5e_qos_node *mlx5e_htb_node_find_rcu(struct mlx5e_htb *htb, u32 classid)
120  {
121  	struct mlx5e_qos_node *node = NULL;
122  
123  	hash_for_each_possible_rcu(htb->qos_tc2node, node, hnode, classid) {
124  		if (node->classid == classid)
125  			break;
126  	}
127  
128  	return node;
129  }
130  
mlx5e_htb_node_delete(struct mlx5e_htb * htb,struct mlx5e_qos_node * node)131  static void mlx5e_htb_node_delete(struct mlx5e_htb *htb, struct mlx5e_qos_node *node)
132  {
133  	hash_del_rcu(&node->hnode);
134  	if (node->qid != MLX5E_QOS_QID_INNER) {
135  		__clear_bit(node->qid, htb->qos_used_qids);
136  		mlx5e_update_tx_netdev_queues(htb->priv);
137  	}
138  	/* Make sure this qid is no longer selected by mlx5e_select_queue, so
139  	 * that mlx5e_reactivate_qos_sq can safely restart the netdev TX queue.
140  	 */
141  	synchronize_net();
142  	kfree(node);
143  }
144  
145  /* TX datapath API */
146  
mlx5e_htb_get_txq_by_classid(struct mlx5e_htb * htb,u16 classid)147  int mlx5e_htb_get_txq_by_classid(struct mlx5e_htb *htb, u16 classid)
148  {
149  	struct mlx5e_qos_node *node;
150  	u16 qid;
151  	int res;
152  
153  	rcu_read_lock();
154  
155  	node = mlx5e_htb_node_find_rcu(htb, classid);
156  	if (!node) {
157  		res = -ENOENT;
158  		goto out;
159  	}
160  	qid = READ_ONCE(node->qid);
161  	if (qid == MLX5E_QOS_QID_INNER) {
162  		res = -EINVAL;
163  		goto out;
164  	}
165  	res = mlx5e_qid_from_qos(&htb->priv->channels, qid);
166  
167  out:
168  	rcu_read_unlock();
169  	return res;
170  }
171  
172  /* HTB TC handlers */
173  
174  static int
mlx5e_htb_root_add(struct mlx5e_htb * htb,u16 htb_maj_id,u16 htb_defcls,struct netlink_ext_ack * extack)175  mlx5e_htb_root_add(struct mlx5e_htb *htb, u16 htb_maj_id, u16 htb_defcls,
176  		   struct netlink_ext_ack *extack)
177  {
178  	struct mlx5e_priv *priv = htb->priv;
179  	struct mlx5e_qos_node *root;
180  	bool opened;
181  	int err;
182  
183  	qos_dbg(htb->mdev, "TC_HTB_CREATE handle %04x:, default :%04x\n", htb_maj_id, htb_defcls);
184  
185  	mlx5e_selq_prepare_htb(htb->selq, htb_maj_id, htb_defcls);
186  
187  	opened = test_bit(MLX5E_STATE_OPENED, &priv->state);
188  	if (opened) {
189  		err = mlx5e_qos_alloc_queues(priv, &priv->channels);
190  		if (err)
191  			goto err_cancel_selq;
192  	}
193  
194  	root = mlx5e_htb_node_create_root(htb);
195  	if (IS_ERR(root)) {
196  		err = PTR_ERR(root);
197  		goto err_free_queues;
198  	}
199  
200  	err = mlx5_qos_create_root_node(htb->mdev, &root->hw_id);
201  	if (err) {
202  		NL_SET_ERR_MSG_MOD(extack, "Firmware error. Try upgrading firmware.");
203  		goto err_sw_node_delete;
204  	}
205  
206  	mlx5e_selq_apply(htb->selq);
207  
208  	return 0;
209  
210  err_sw_node_delete:
211  	mlx5e_htb_node_delete(htb, root);
212  
213  err_free_queues:
214  	if (opened)
215  		mlx5e_qos_close_all_queues(&priv->channels);
216  err_cancel_selq:
217  	mlx5e_selq_cancel(htb->selq);
218  	return err;
219  }
220  
mlx5e_htb_root_del(struct mlx5e_htb * htb)221  static int mlx5e_htb_root_del(struct mlx5e_htb *htb)
222  {
223  	struct mlx5e_priv *priv = htb->priv;
224  	struct mlx5e_qos_node *root;
225  	int err;
226  
227  	qos_dbg(htb->mdev, "TC_HTB_DESTROY\n");
228  
229  	/* Wait until real_num_tx_queues is updated for mlx5e_select_queue,
230  	 * so that we can safely switch to its non-HTB non-PTP fastpath.
231  	 */
232  	synchronize_net();
233  
234  	mlx5e_selq_prepare_htb(htb->selq, 0, 0);
235  	mlx5e_selq_apply(htb->selq);
236  
237  	root = mlx5e_htb_node_find(htb, MLX5E_HTB_CLASSID_ROOT);
238  	if (!root) {
239  		qos_err(htb->mdev, "Failed to find the root node in the QoS tree\n");
240  		return -ENOENT;
241  	}
242  	err = mlx5_qos_destroy_node(htb->mdev, root->hw_id);
243  	if (err)
244  		qos_err(htb->mdev, "Failed to destroy root node %u, err = %d\n",
245  			root->hw_id, err);
246  	mlx5e_htb_node_delete(htb, root);
247  
248  	mlx5e_qos_deactivate_all_queues(&priv->channels);
249  	mlx5e_qos_close_all_queues(&priv->channels);
250  
251  	return err;
252  }
253  
mlx5e_htb_convert_rate(struct mlx5e_htb * htb,u64 rate,struct mlx5e_qos_node * parent,u32 * bw_share)254  static int mlx5e_htb_convert_rate(struct mlx5e_htb *htb, u64 rate,
255  				  struct mlx5e_qos_node *parent, u32 *bw_share)
256  {
257  	u64 share = 0;
258  
259  	while (parent->classid != MLX5E_HTB_CLASSID_ROOT && !parent->max_average_bw)
260  		parent = parent->parent;
261  
262  	if (parent->max_average_bw)
263  		share = div64_u64(div_u64(rate * 100, BYTES_IN_MBIT),
264  				  parent->max_average_bw);
265  	else
266  		share = 101;
267  
268  	*bw_share = share == 0 ? 1 : share > 100 ? 0 : share;
269  
270  	qos_dbg(htb->mdev, "Convert: rate %llu, parent ceil %llu -> bw_share %u\n",
271  		rate, (u64)parent->max_average_bw * BYTES_IN_MBIT, *bw_share);
272  
273  	return 0;
274  }
275  
mlx5e_htb_convert_ceil(struct mlx5e_htb * htb,u64 ceil,u32 * max_average_bw)276  static void mlx5e_htb_convert_ceil(struct mlx5e_htb *htb, u64 ceil, u32 *max_average_bw)
277  {
278  	/* Hardware treats 0 as "unlimited", set at least 1. */
279  	*max_average_bw = max_t(u32, div_u64(ceil, BYTES_IN_MBIT), 1);
280  
281  	qos_dbg(htb->mdev, "Convert: ceil %llu -> max_average_bw %u\n",
282  		ceil, *max_average_bw);
283  }
284  
285  int
mlx5e_htb_leaf_alloc_queue(struct mlx5e_htb * htb,u16 classid,u32 parent_classid,u64 rate,u64 ceil,struct netlink_ext_ack * extack)286  mlx5e_htb_leaf_alloc_queue(struct mlx5e_htb *htb, u16 classid,
287  			   u32 parent_classid, u64 rate, u64 ceil,
288  			   struct netlink_ext_ack *extack)
289  {
290  	struct mlx5e_qos_node *node, *parent;
291  	struct mlx5e_priv *priv = htb->priv;
292  	int qid;
293  	int err;
294  
295  	qos_dbg(htb->mdev, "TC_HTB_LEAF_ALLOC_QUEUE classid %04x, parent %04x, rate %llu, ceil %llu\n",
296  		classid, parent_classid, rate, ceil);
297  
298  	qid = mlx5e_htb_find_unused_qos_qid(htb);
299  	if (qid < 0) {
300  		NL_SET_ERR_MSG_MOD(extack, "Maximum amount of leaf classes is reached.");
301  		return qid;
302  	}
303  
304  	parent = mlx5e_htb_node_find(htb, parent_classid);
305  	if (!parent)
306  		return -EINVAL;
307  
308  	node = mlx5e_htb_node_create_leaf(htb, classid, qid, parent);
309  	if (IS_ERR(node))
310  		return PTR_ERR(node);
311  
312  	node->rate = rate;
313  	mlx5e_htb_convert_rate(htb, rate, node->parent, &node->bw_share);
314  	mlx5e_htb_convert_ceil(htb, ceil, &node->max_average_bw);
315  
316  	err = mlx5_qos_create_leaf_node(htb->mdev, node->parent->hw_id,
317  					node->bw_share, node->max_average_bw,
318  					&node->hw_id);
319  	if (err) {
320  		NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating a leaf node.");
321  		qos_err(htb->mdev, "Failed to create a leaf node (class %04x), err = %d\n",
322  			classid, err);
323  		mlx5e_htb_node_delete(htb, node);
324  		return err;
325  	}
326  
327  	if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
328  		err = mlx5e_open_qos_sq(priv, &priv->channels, node->qid, node->hw_id);
329  		if (err) {
330  			NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
331  			qos_warn(htb->mdev, "Failed to create a QoS SQ (class %04x), err = %d\n",
332  				 classid, err);
333  		} else {
334  			mlx5e_activate_qos_sq(priv, node->qid, node->hw_id);
335  		}
336  	}
337  
338  	return mlx5e_qid_from_qos(&priv->channels, node->qid);
339  }
340  
341  int
mlx5e_htb_leaf_to_inner(struct mlx5e_htb * htb,u16 classid,u16 child_classid,u64 rate,u64 ceil,struct netlink_ext_ack * extack)342  mlx5e_htb_leaf_to_inner(struct mlx5e_htb *htb, u16 classid, u16 child_classid,
343  			u64 rate, u64 ceil, struct netlink_ext_ack *extack)
344  {
345  	struct mlx5e_qos_node *node, *child;
346  	struct mlx5e_priv *priv = htb->priv;
347  	int err, tmp_err;
348  	u32 new_hw_id;
349  	u16 qid;
350  
351  	qos_dbg(htb->mdev, "TC_HTB_LEAF_TO_INNER classid %04x, upcoming child %04x, rate %llu, ceil %llu\n",
352  		classid, child_classid, rate, ceil);
353  
354  	node = mlx5e_htb_node_find(htb, classid);
355  	if (!node)
356  		return -ENOENT;
357  
358  	err = mlx5_qos_create_inner_node(htb->mdev, node->parent->hw_id,
359  					 node->bw_share, node->max_average_bw,
360  					 &new_hw_id);
361  	if (err) {
362  		NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating an inner node.");
363  		qos_err(htb->mdev, "Failed to create an inner node (class %04x), err = %d\n",
364  			classid, err);
365  		return err;
366  	}
367  
368  	/* Intentionally reuse the qid for the upcoming first child. */
369  	child = mlx5e_htb_node_create_leaf(htb, child_classid, node->qid, node);
370  	if (IS_ERR(child)) {
371  		err = PTR_ERR(child);
372  		goto err_destroy_hw_node;
373  	}
374  
375  	child->rate = rate;
376  	mlx5e_htb_convert_rate(htb, rate, node, &child->bw_share);
377  	mlx5e_htb_convert_ceil(htb, ceil, &child->max_average_bw);
378  
379  	err = mlx5_qos_create_leaf_node(htb->mdev, new_hw_id, child->bw_share,
380  					child->max_average_bw, &child->hw_id);
381  	if (err) {
382  		NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating a leaf node.");
383  		qos_err(htb->mdev, "Failed to create a leaf node (class %04x), err = %d\n",
384  			classid, err);
385  		goto err_delete_sw_node;
386  	}
387  
388  	/* No fail point. */
389  
390  	qid = node->qid;
391  	/* Pairs with mlx5e_htb_get_txq_by_classid. */
392  	WRITE_ONCE(node->qid, MLX5E_QOS_QID_INNER);
393  
394  	if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
395  		mlx5e_deactivate_qos_sq(priv, qid);
396  		mlx5e_close_qos_sq(priv, qid);
397  	}
398  
399  	err = mlx5_qos_destroy_node(htb->mdev, node->hw_id);
400  	if (err) /* Not fatal. */
401  		qos_warn(htb->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
402  			 node->hw_id, classid, err);
403  
404  	node->hw_id = new_hw_id;
405  
406  	if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
407  		err = mlx5e_open_qos_sq(priv, &priv->channels, child->qid, child->hw_id);
408  		if (err) {
409  			NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
410  			qos_warn(htb->mdev, "Failed to create a QoS SQ (class %04x), err = %d\n",
411  				 classid, err);
412  		} else {
413  			mlx5e_activate_qos_sq(priv, child->qid, child->hw_id);
414  		}
415  	}
416  
417  	return 0;
418  
419  err_delete_sw_node:
420  	child->qid = MLX5E_QOS_QID_INNER;
421  	mlx5e_htb_node_delete(htb, child);
422  
423  err_destroy_hw_node:
424  	tmp_err = mlx5_qos_destroy_node(htb->mdev, new_hw_id);
425  	if (tmp_err) /* Not fatal. */
426  		qos_warn(htb->mdev, "Failed to roll back creation of an inner node %u (class %04x), err = %d\n",
427  			 new_hw_id, classid, tmp_err);
428  	return err;
429  }
430  
mlx5e_htb_node_find_by_qid(struct mlx5e_htb * htb,u16 qid)431  static struct mlx5e_qos_node *mlx5e_htb_node_find_by_qid(struct mlx5e_htb *htb, u16 qid)
432  {
433  	struct mlx5e_qos_node *node = NULL;
434  	int bkt;
435  
436  	hash_for_each(htb->qos_tc2node, bkt, node, hnode)
437  		if (node->qid == qid)
438  			break;
439  
440  	return node;
441  }
442  
mlx5e_htb_leaf_del(struct mlx5e_htb * htb,u16 * classid,struct netlink_ext_ack * extack)443  int mlx5e_htb_leaf_del(struct mlx5e_htb *htb, u16 *classid,
444  		       struct netlink_ext_ack *extack)
445  {
446  	struct mlx5e_priv *priv = htb->priv;
447  	struct mlx5e_qos_node *node;
448  	struct netdev_queue *txq;
449  	u16 qid, moved_qid;
450  	bool opened;
451  	int err;
452  
453  	qos_dbg(htb->mdev, "TC_HTB_LEAF_DEL classid %04x\n", *classid);
454  
455  	node = mlx5e_htb_node_find(htb, *classid);
456  	if (!node)
457  		return -ENOENT;
458  
459  	/* Store qid for reuse. */
460  	qid = node->qid;
461  
462  	opened = test_bit(MLX5E_STATE_OPENED, &priv->state);
463  	if (opened) {
464  		txq = netdev_get_tx_queue(htb->netdev,
465  					  mlx5e_qid_from_qos(&priv->channels, qid));
466  		mlx5e_deactivate_qos_sq(priv, qid);
467  		mlx5e_close_qos_sq(priv, qid);
468  	}
469  
470  	err = mlx5_qos_destroy_node(htb->mdev, node->hw_id);
471  	if (err) /* Not fatal. */
472  		qos_warn(htb->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
473  			 node->hw_id, *classid, err);
474  
475  	mlx5e_htb_node_delete(htb, node);
476  
477  	moved_qid = mlx5e_htb_cur_leaf_nodes(htb);
478  
479  	if (moved_qid == 0) {
480  		/* The last QoS SQ was just destroyed. */
481  		if (opened)
482  			mlx5e_reactivate_qos_sq(priv, qid, txq);
483  		return 0;
484  	}
485  	moved_qid--;
486  
487  	if (moved_qid < qid) {
488  		/* The highest QoS SQ was just destroyed. */
489  		WARN(moved_qid != qid - 1, "Gaps in queue numeration: destroyed queue %u, the highest queue is %u",
490  		     qid, moved_qid);
491  		if (opened)
492  			mlx5e_reactivate_qos_sq(priv, qid, txq);
493  		return 0;
494  	}
495  
496  	WARN(moved_qid == qid, "Can't move node with qid %u to itself", qid);
497  	qos_dbg(htb->mdev, "Moving QoS SQ %u to %u\n", moved_qid, qid);
498  
499  	node = mlx5e_htb_node_find_by_qid(htb, moved_qid);
500  	WARN(!node, "Could not find a node with qid %u to move to queue %u",
501  	     moved_qid, qid);
502  
503  	/* Stop traffic to the old queue. */
504  	WRITE_ONCE(node->qid, MLX5E_QOS_QID_INNER);
505  	__clear_bit(moved_qid, priv->htb->qos_used_qids);
506  
507  	if (opened) {
508  		txq = netdev_get_tx_queue(htb->netdev,
509  					  mlx5e_qid_from_qos(&priv->channels, moved_qid));
510  		mlx5e_deactivate_qos_sq(priv, moved_qid);
511  		mlx5e_close_qos_sq(priv, moved_qid);
512  	}
513  
514  	/* Prevent packets from the old class from getting into the new one. */
515  	mlx5e_reset_qdisc(htb->netdev, moved_qid);
516  
517  	__set_bit(qid, htb->qos_used_qids);
518  	WRITE_ONCE(node->qid, qid);
519  
520  	if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
521  		err = mlx5e_open_qos_sq(priv, &priv->channels, node->qid, node->hw_id);
522  		if (err) {
523  			NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
524  			qos_warn(htb->mdev, "Failed to create a QoS SQ (class %04x) while moving qid %u to %u, err = %d\n",
525  				 node->classid, moved_qid, qid, err);
526  		} else {
527  			mlx5e_activate_qos_sq(priv, node->qid, node->hw_id);
528  		}
529  	}
530  
531  	mlx5e_update_tx_netdev_queues(priv);
532  	if (opened)
533  		mlx5e_reactivate_qos_sq(priv, moved_qid, txq);
534  
535  	*classid = node->classid;
536  	return 0;
537  }
538  
539  int
mlx5e_htb_leaf_del_last(struct mlx5e_htb * htb,u16 classid,bool force,struct netlink_ext_ack * extack)540  mlx5e_htb_leaf_del_last(struct mlx5e_htb *htb, u16 classid, bool force,
541  			struct netlink_ext_ack *extack)
542  {
543  	struct mlx5e_qos_node *node, *parent;
544  	struct mlx5e_priv *priv = htb->priv;
545  	u32 old_hw_id, new_hw_id;
546  	int err, saved_err = 0;
547  	u16 qid;
548  
549  	qos_dbg(htb->mdev, "TC_HTB_LEAF_DEL_LAST%s classid %04x\n",
550  		force ? "_FORCE" : "", classid);
551  
552  	node = mlx5e_htb_node_find(htb, classid);
553  	if (!node)
554  		return -ENOENT;
555  
556  	err = mlx5_qos_create_leaf_node(htb->mdev, node->parent->parent->hw_id,
557  					node->parent->bw_share,
558  					node->parent->max_average_bw,
559  					&new_hw_id);
560  	if (err) {
561  		NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating a leaf node.");
562  		qos_err(htb->mdev, "Failed to create a leaf node (class %04x), err = %d\n",
563  			classid, err);
564  		if (!force)
565  			return err;
566  		saved_err = err;
567  	}
568  
569  	/* Store qid for reuse and prevent clearing the bit. */
570  	qid = node->qid;
571  	/* Pairs with mlx5e_htb_get_txq_by_classid. */
572  	WRITE_ONCE(node->qid, MLX5E_QOS_QID_INNER);
573  
574  	if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
575  		mlx5e_deactivate_qos_sq(priv, qid);
576  		mlx5e_close_qos_sq(priv, qid);
577  	}
578  
579  	/* Prevent packets from the old class from getting into the new one. */
580  	mlx5e_reset_qdisc(htb->netdev, qid);
581  
582  	err = mlx5_qos_destroy_node(htb->mdev, node->hw_id);
583  	if (err) /* Not fatal. */
584  		qos_warn(htb->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
585  			 node->hw_id, classid, err);
586  
587  	parent = node->parent;
588  	mlx5e_htb_node_delete(htb, node);
589  
590  	node = parent;
591  	WRITE_ONCE(node->qid, qid);
592  
593  	/* Early return on error in force mode. Parent will still be an inner
594  	 * node to be deleted by a following delete operation.
595  	 */
596  	if (saved_err)
597  		return saved_err;
598  
599  	old_hw_id = node->hw_id;
600  	node->hw_id = new_hw_id;
601  
602  	if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
603  		err = mlx5e_open_qos_sq(priv, &priv->channels, node->qid, node->hw_id);
604  		if (err) {
605  			NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
606  			qos_warn(htb->mdev, "Failed to create a QoS SQ (class %04x), err = %d\n",
607  				 classid, err);
608  		} else {
609  			mlx5e_activate_qos_sq(priv, node->qid, node->hw_id);
610  		}
611  	}
612  
613  	err = mlx5_qos_destroy_node(htb->mdev, old_hw_id);
614  	if (err) /* Not fatal. */
615  		qos_warn(htb->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
616  			 node->hw_id, classid, err);
617  
618  	return 0;
619  }
620  
621  static int
mlx5e_htb_update_children(struct mlx5e_htb * htb,struct mlx5e_qos_node * node,struct netlink_ext_ack * extack)622  mlx5e_htb_update_children(struct mlx5e_htb *htb, struct mlx5e_qos_node *node,
623  			  struct netlink_ext_ack *extack)
624  {
625  	struct mlx5e_qos_node *child;
626  	int err = 0;
627  	int bkt;
628  
629  	hash_for_each(htb->qos_tc2node, bkt, child, hnode) {
630  		u32 old_bw_share = child->bw_share;
631  		int err_one;
632  
633  		if (child->parent != node)
634  			continue;
635  
636  		mlx5e_htb_convert_rate(htb, child->rate, node, &child->bw_share);
637  		if (child->bw_share == old_bw_share)
638  			continue;
639  
640  		err_one = mlx5_qos_update_node(htb->mdev, child->bw_share,
641  					       child->max_average_bw, child->hw_id);
642  		if (!err && err_one) {
643  			err = err_one;
644  
645  			NL_SET_ERR_MSG_MOD(extack, "Firmware error when modifying a child node.");
646  			qos_err(htb->mdev, "Failed to modify a child node (class %04x), err = %d\n",
647  				node->classid, err);
648  		}
649  	}
650  
651  	return err;
652  }
653  
654  int
mlx5e_htb_node_modify(struct mlx5e_htb * htb,u16 classid,u64 rate,u64 ceil,struct netlink_ext_ack * extack)655  mlx5e_htb_node_modify(struct mlx5e_htb *htb, u16 classid, u64 rate, u64 ceil,
656  		      struct netlink_ext_ack *extack)
657  {
658  	u32 bw_share, max_average_bw;
659  	struct mlx5e_qos_node *node;
660  	bool ceil_changed = false;
661  	int err;
662  
663  	qos_dbg(htb->mdev, "TC_HTB_LEAF_MODIFY classid %04x, rate %llu, ceil %llu\n",
664  		classid, rate, ceil);
665  
666  	node = mlx5e_htb_node_find(htb, classid);
667  	if (!node)
668  		return -ENOENT;
669  
670  	node->rate = rate;
671  	mlx5e_htb_convert_rate(htb, rate, node->parent, &bw_share);
672  	mlx5e_htb_convert_ceil(htb, ceil, &max_average_bw);
673  
674  	err = mlx5_qos_update_node(htb->mdev, bw_share,
675  				   max_average_bw, node->hw_id);
676  	if (err) {
677  		NL_SET_ERR_MSG_MOD(extack, "Firmware error when modifying a node.");
678  		qos_err(htb->mdev, "Failed to modify a node (class %04x), err = %d\n",
679  			classid, err);
680  		return err;
681  	}
682  
683  	if (max_average_bw != node->max_average_bw)
684  		ceil_changed = true;
685  
686  	node->bw_share = bw_share;
687  	node->max_average_bw = max_average_bw;
688  
689  	if (ceil_changed)
690  		err = mlx5e_htb_update_children(htb, node, extack);
691  
692  	return err;
693  }
694  
mlx5e_htb_alloc(void)695  struct mlx5e_htb *mlx5e_htb_alloc(void)
696  {
697  	return kvzalloc(sizeof(struct mlx5e_htb), GFP_KERNEL);
698  }
699  
mlx5e_htb_free(struct mlx5e_htb * htb)700  void mlx5e_htb_free(struct mlx5e_htb *htb)
701  {
702  	kvfree(htb);
703  }
704  
mlx5e_htb_init(struct mlx5e_htb * htb,struct tc_htb_qopt_offload * htb_qopt,struct net_device * netdev,struct mlx5_core_dev * mdev,struct mlx5e_selq * selq,struct mlx5e_priv * priv)705  int mlx5e_htb_init(struct mlx5e_htb *htb, struct tc_htb_qopt_offload *htb_qopt,
706  		   struct net_device *netdev, struct mlx5_core_dev *mdev,
707  		   struct mlx5e_selq *selq, struct mlx5e_priv *priv)
708  {
709  	htb->mdev = mdev;
710  	htb->netdev = netdev;
711  	htb->selq = selq;
712  	htb->priv = priv;
713  	hash_init(htb->qos_tc2node);
714  	return mlx5e_htb_root_add(htb, htb_qopt->parent_classid, htb_qopt->classid,
715  				  htb_qopt->extack);
716  }
717  
mlx5e_htb_cleanup(struct mlx5e_htb * htb)718  void mlx5e_htb_cleanup(struct mlx5e_htb *htb)
719  {
720  	mlx5e_htb_root_del(htb);
721  }
722  
723