[pull request][net-next V2 00/13] mlx5 updates 2022-07-17

[pull request][net-next V2 00/13] mlx5 updates 2022-07-17

[Saeed Mahameed]

From: Saeed Mahameed <saeedm@nvidia.com>

v1->v2:
 1) toss, "net/mlx5e: Expose rx_oversize_pkts_buffer counter", it might
    need some extra work

Misc updates for mlx5 driver.

For more information please see tag log below.

Please pull and let me know if there is any problem.

Thanks,
Saeed.


The following changes since commit 769e2695be4132fe0be4c964c9d8ea97c74d9a6a:

  net: dsa: microchip: fix the missing ksz8_r_mib_cnt (2022-07-19 15:33:51 +0200)

are available in the Git repository at:

  git://git.kernel.org/pub/scm/linux/kernel/git/saeed/linux.git tags/mlx5-updates-2022-07-17

for you to fetch changes up to 22df2e93622fdb1672c2ba8ff1d2fc90980b3358:

  net/mlx5: CT: Remove warning of ignore_flow_level support for non PF (2022-07-19 13:32:54 -0700)

----------------------------------------------------------------
mlx5-updates-2022-07-17

1) Add resiliency for lost completions for PTP TX port timestamp

2) Report Header-data split state via ethtool

3) Decouple HTB code from main regular TX code

----------------------------------------------------------------
Aya Levin (2):
      net/mlx5: Expose ts_cqe_metadata_size2wqe_counter
      net/mlx5e: Add resiliency for PTP TX port timestamp

Gal Pressman (1):
      net/mlx5e: Report header-data split state through ethtool

Moshe Tal (7):
      net/mlx5e: Fix mqprio_rl handling on devlink reload
      net/mlx5e: HTB, move ids to selq_params struct
      net/mlx5e: HTB, move section comment to the right place
      net/mlx5e: HTB, move stats and max_sqs to priv
      net/mlx5e: HTB, remove priv from htb function calls
      net/mlx5e: HTB, change functions name to follow convention
      net/mlx5e: HTB, move htb functions to a new file

Roi Dayan (1):
      net/mlx5: CT: Remove warning of ignore_flow_level support for non PF

Saeed Mahameed (2):
      net/mlx5e: HTB, reduce visibility of htb functions
      net/mlx5e: HTB, hide and dynamically allocate mlx5e_htb structure

 drivers/net/ethernet/mellanox/mlx5/core/Makefile   |   3 +-
 drivers/net/ethernet/mellanox/mlx5/core/en.h       |  20 +-
 drivers/net/ethernet/mellanox/mlx5/core/en/htb.c   | 722 ++++++++++++++++++
 drivers/net/ethernet/mellanox/mlx5/core/en/htb.h   |  46 ++
 drivers/net/ethernet/mellanox/mlx5/core/en/ptp.c   |  37 +-
 drivers/net/ethernet/mellanox/mlx5/core/en/ptp.h   |   1 +
 drivers/net/ethernet/mellanox/mlx5/core/en/qos.c   | 813 +++------------------
 drivers/net/ethernet/mellanox/mlx5/core/en/qos.h   |  37 +-
 drivers/net/ethernet/mellanox/mlx5/core/en/selq.c  |  51 +-
 drivers/net/ethernet/mellanox/mlx5/core/en/selq.h  |   4 +-
 .../ethernet/mellanox/mlx5/core/en/tc/post_act.c   |   2 +-
 drivers/net/ethernet/mellanox/mlx5/core/en/tc_ct.c |   2 +-
 .../net/ethernet/mellanox/mlx5/core/en_ethtool.c   |  16 +-
 drivers/net/ethernet/mellanox/mlx5/core/en_main.c  | 228 +++---
 drivers/net/ethernet/mellanox/mlx5/core/en_rep.c   |   2 +-
 drivers/net/ethernet/mellanox/mlx5/core/en_stats.c |  14 +-
 drivers/net/ethernet/mellanox/mlx5/core/en_stats.h |   2 +
 drivers/net/ethernet/mellanox/mlx5/core/en_tx.c    |  10 +
 .../ethernet/mellanox/mlx5/core/ipoib/ethtool.c    |   2 +-
 include/linux/mlx5/mlx5_ifc.h                      |   6 +-
 20 files changed, 1157 insertions(+), 861 deletions(-)
 create mode 100644 drivers/net/ethernet/mellanox/mlx5/core/en/htb.c
 create mode 100644 drivers/net/ethernet/mellanox/mlx5/core/en/htb.h

[net-next V2 01/13] net/mlx5e: Report header-data split state through ethtool

[Saeed Mahameed]

From: Gal Pressman <gal@nvidia.com>

HW-GRO (SHAMPO) packet merger scheme implies header-data split in the
driver, report it through the ethtool interface.

Signed-off-by: Gal Pressman <gal@nvidia.com>
Reviewed-by: Tariq Toukan <tariqt@nvidia.com>
Signed-off-by: Saeed Mahameed <saeedm@nvidia.com>
---
 drivers/net/ethernet/mellanox/mlx5/core/en.h         |  3 ++-
 drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c | 12 ++++++++++--
 drivers/net/ethernet/mellanox/mlx5/core/en_rep.c     |  2 +-
 .../net/ethernet/mellanox/mlx5/core/ipoib/ethtool.c  |  2 +-
 4 files changed, 14 insertions(+), 5 deletions(-)

diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en.h b/drivers/net/ethernet/mellanox/mlx5/core/en.h
index b6c15efe92ad..da10061d0c03 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en.h
@@ -1181,7 +1181,8 @@ int mlx5e_ethtool_get_sset_count(struct mlx5e_priv *priv, int sset);
 void mlx5e_ethtool_get_ethtool_stats(struct mlx5e_priv *priv,
 				     struct ethtool_stats *stats, u64 *data);
 void mlx5e_ethtool_get_ringparam(struct mlx5e_priv *priv,
-				 struct ethtool_ringparam *param);
+				 struct ethtool_ringparam *param,
+				 struct kernel_ethtool_ringparam *kernel_param);
 int mlx5e_ethtool_set_ringparam(struct mlx5e_priv *priv,
 				struct ethtool_ringparam *param);
 void mlx5e_ethtool_get_channels(struct mlx5e_priv *priv,
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c b/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c
index 6e80585d731f..820912eb7bcf 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c
@@ -30,6 +30,8 @@
  * SOFTWARE.
  */
 
+#include <linux/ethtool_netlink.h>
+
 #include "en.h"
 #include "en/port.h"
 #include "en/params.h"
@@ -305,12 +307,18 @@ static void mlx5e_get_ethtool_stats(struct net_device *dev,
 }
 
 void mlx5e_ethtool_get_ringparam(struct mlx5e_priv *priv,
-				 struct ethtool_ringparam *param)
+				 struct ethtool_ringparam *param,
+				 struct kernel_ethtool_ringparam *kernel_param)
 {
 	param->rx_max_pending = 1 << MLX5E_PARAMS_MAXIMUM_LOG_RQ_SIZE;
 	param->tx_max_pending = 1 << MLX5E_PARAMS_MAXIMUM_LOG_SQ_SIZE;
 	param->rx_pending     = 1 << priv->channels.params.log_rq_mtu_frames;
 	param->tx_pending     = 1 << priv->channels.params.log_sq_size;
+
+	kernel_param->tcp_data_split =
+		(priv->channels.params.packet_merge.type == MLX5E_PACKET_MERGE_SHAMPO) ?
+		ETHTOOL_TCP_DATA_SPLIT_ENABLED :
+		ETHTOOL_TCP_DATA_SPLIT_DISABLED;
 }
 
 static void mlx5e_get_ringparam(struct net_device *dev,
@@ -320,7 +328,7 @@ static void mlx5e_get_ringparam(struct net_device *dev,
 {
 	struct mlx5e_priv *priv = netdev_priv(dev);
 
-	mlx5e_ethtool_get_ringparam(priv, param);
+	mlx5e_ethtool_get_ringparam(priv, param, kernel_param);
 }
 
 int mlx5e_ethtool_set_ringparam(struct mlx5e_priv *priv,
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_rep.c b/drivers/net/ethernet/mellanox/mlx5/core/en_rep.c
index f797fd97d305..ae90b06d21e2 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_rep.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_rep.c
@@ -229,7 +229,7 @@ mlx5e_rep_get_ringparam(struct net_device *dev,
 {
 	struct mlx5e_priv *priv = netdev_priv(dev);
 
-	mlx5e_ethtool_get_ringparam(priv, param);
+	mlx5e_ethtool_get_ringparam(priv, param, kernel_param);
 }
 
 static int
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/ipoib/ethtool.c b/drivers/net/ethernet/mellanox/mlx5/core/ipoib/ethtool.c
index 8da73ef5680f..ac3757beaea2 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/ipoib/ethtool.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/ipoib/ethtool.c
@@ -83,7 +83,7 @@ static void mlx5i_get_ringparam(struct net_device *dev,
 {
 	struct mlx5e_priv *priv = mlx5i_epriv(dev);
 
-	mlx5e_ethtool_get_ringparam(priv, param);
+	mlx5e_ethtool_get_ringparam(priv, param, kernel_param);
 }
 
 static int mlx5i_set_channels(struct net_device *dev,
-- 
2.36.1

[net-next V2 02/13] net/mlx5e: Fix mqprio_rl handling on devlink reload

[Saeed Mahameed]

From: Moshe Tal <moshet@nvidia.com>

Keep mqprio_rl data to params and restore the configuration in case of
devlink reload.
Change the location of mqprio_rl resources cleanup so it will be done
also in reload flow.

Also, remove the rl pointer from the params, since this is dynamic object
and saved to priv.

Signed-off-by: Moshe Tal <moshet@nvidia.com>
Reviewed-by: Tariq Toukan <tariqt@nvidia.com>
Signed-off-by: Saeed Mahameed <saeedm@nvidia.com>
---
 drivers/net/ethernet/mellanox/mlx5/core/en.h  |   3 +-
 .../net/ethernet/mellanox/mlx5/core/en_main.c | 144 +++++++++++++-----
 2 files changed, 106 insertions(+), 41 deletions(-)

diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en.h b/drivers/net/ethernet/mellanox/mlx5/core/en.h
index da10061d0c03..5c88c3896b96 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en.h
@@ -321,7 +321,8 @@ struct mlx5e_params {
 		u8 num_tc;
 		struct netdev_tc_txq tc_to_txq[TC_MAX_QUEUE];
 		struct {
-			struct mlx5e_mqprio_rl *rl;
+			u64 max_rate[TC_MAX_QUEUE];
+			u32 hw_id[TC_MAX_QUEUE];
 		} channel;
 	} mqprio;
 	bool rx_cqe_compress_def;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_main.c b/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
index cac4022ba7f3..fe07180a957a 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
@@ -1912,8 +1912,7 @@ static int mlx5e_txq_get_qos_node_hw_id(struct mlx5e_params *params, int txq_ix,
 {
 	int tc;
 
-	if (params->mqprio.mode != TC_MQPRIO_MODE_CHANNEL ||
-	    !params->mqprio.channel.rl) {
+	if (params->mqprio.mode != TC_MQPRIO_MODE_CHANNEL) {
 		*hw_id = 0;
 		return 0;
 	}
@@ -1922,7 +1921,14 @@ static int mlx5e_txq_get_qos_node_hw_id(struct mlx5e_params *params, int txq_ix,
 	if (tc < 0)
 		return tc;
 
-	return mlx5e_mqprio_rl_get_node_hw_id(params->mqprio.channel.rl, tc, hw_id);
+	if (tc >= params->mqprio.num_tc) {
+		WARN(1, "Unexpected TCs configuration. tc %d is out of range of %u",
+		     tc, params->mqprio.num_tc);
+		return -EINVAL;
+	}
+
+	*hw_id = params->mqprio.channel.hw_id[tc];
+	return 0;
 }
 
 static int mlx5e_open_sqs(struct mlx5e_channel *c,
@@ -2615,13 +2621,6 @@ static int mlx5e_update_netdev_queues(struct mlx5e_priv *priv)
 		netdev_warn(netdev, "netif_set_real_num_rx_queues failed, %d\n", err);
 		goto err_txqs;
 	}
-	if (priv->mqprio_rl != priv->channels.params.mqprio.channel.rl) {
-		if (priv->mqprio_rl) {
-			mlx5e_mqprio_rl_cleanup(priv->mqprio_rl);
-			mlx5e_mqprio_rl_free(priv->mqprio_rl);
-		}
-		priv->mqprio_rl = priv->channels.params.mqprio.channel.rl;
-	}
 
 	return 0;
 
@@ -3135,6 +3134,11 @@ int mlx5e_create_tises(struct mlx5e_priv *priv)
 
 static void mlx5e_cleanup_nic_tx(struct mlx5e_priv *priv)
 {
+	if (priv->mqprio_rl) {
+		mlx5e_mqprio_rl_cleanup(priv->mqprio_rl);
+		mlx5e_mqprio_rl_free(priv->mqprio_rl);
+		priv->mqprio_rl = NULL;
+	}
 	mlx5e_destroy_tises(priv);
 }
 
@@ -3203,19 +3207,38 @@ static void mlx5e_params_mqprio_dcb_set(struct mlx5e_params *params, u8 num_tc)
 {
 	params->mqprio.mode = TC_MQPRIO_MODE_DCB;
 	params->mqprio.num_tc = num_tc;
-	params->mqprio.channel.rl = NULL;
 	mlx5e_mqprio_build_default_tc_to_txq(params->mqprio.tc_to_txq, num_tc,
 					     params->num_channels);
 }
 
+static void mlx5e_mqprio_rl_update_params(struct mlx5e_params *params,
+					  struct mlx5e_mqprio_rl *rl)
+{
+	int tc;
+
+	for (tc = 0; tc < TC_MAX_QUEUE; tc++) {
+		u32 hw_id = 0;
+
+		if (rl)
+			mlx5e_mqprio_rl_get_node_hw_id(rl, tc, &hw_id);
+		params->mqprio.channel.hw_id[tc] = hw_id;
+	}
+}
+
 static void mlx5e_params_mqprio_channel_set(struct mlx5e_params *params,
-					    struct tc_mqprio_qopt *qopt,
+					    struct tc_mqprio_qopt_offload *mqprio,
 					    struct mlx5e_mqprio_rl *rl)
 {
+	int tc;
+
 	params->mqprio.mode = TC_MQPRIO_MODE_CHANNEL;
-	params->mqprio.num_tc = qopt->num_tc;
-	params->mqprio.channel.rl = rl;
-	mlx5e_mqprio_build_tc_to_txq(params->mqprio.tc_to_txq, qopt);
+	params->mqprio.num_tc = mqprio->qopt.num_tc;
+
+	for (tc = 0; tc < TC_MAX_QUEUE; tc++)
+		params->mqprio.channel.max_rate[tc] = mqprio->max_rate[tc];
+
+	mlx5e_mqprio_rl_update_params(params, rl);
+	mlx5e_mqprio_build_tc_to_txq(params->mqprio.tc_to_txq, &mqprio->qopt);
 }
 
 static void mlx5e_params_mqprio_reset(struct mlx5e_params *params)
@@ -3241,6 +3264,12 @@ static int mlx5e_setup_tc_mqprio_dcb(struct mlx5e_priv *priv,
 	err = mlx5e_safe_switch_params(priv, &new_params,
 				       mlx5e_num_channels_changed_ctx, NULL, true);
 
+	if (!err && priv->mqprio_rl) {
+		mlx5e_mqprio_rl_cleanup(priv->mqprio_rl);
+		mlx5e_mqprio_rl_free(priv->mqprio_rl);
+		priv->mqprio_rl = NULL;
+	}
+
 	priv->max_opened_tc = max_t(u8, priv->max_opened_tc,
 				    mlx5e_get_dcb_num_tc(&priv->channels.params));
 	return err;
@@ -3299,16 +3328,38 @@ static int mlx5e_mqprio_channel_validate(struct mlx5e_priv *priv,
 	return 0;
 }
 
-static bool mlx5e_mqprio_rate_limit(struct tc_mqprio_qopt_offload *mqprio)
+static bool mlx5e_mqprio_rate_limit(u8 num_tc, u64 max_rate[])
 {
 	int tc;
 
-	for (tc = 0; tc < mqprio->qopt.num_tc; tc++)
-		if (mqprio->max_rate[tc])
+	for (tc = 0; tc < num_tc; tc++)
+		if (max_rate[tc])
 			return true;
 	return false;
 }
 
+static struct mlx5e_mqprio_rl *mlx5e_mqprio_rl_create(struct mlx5_core_dev *mdev,
+						      u8 num_tc, u64 max_rate[])
+{
+	struct mlx5e_mqprio_rl *rl;
+	int err;
+
+	if (!mlx5e_mqprio_rate_limit(num_tc, max_rate))
+		return NULL;
+
+	rl = mlx5e_mqprio_rl_alloc();
+	if (!rl)
+		return ERR_PTR(-ENOMEM);
+
+	err = mlx5e_mqprio_rl_init(rl, mdev, num_tc, max_rate);
+	if (err) {
+		mlx5e_mqprio_rl_free(rl);
+		return ERR_PTR(err);
+	}
+
+	return rl;
+}
+
 static int mlx5e_setup_tc_mqprio_channel(struct mlx5e_priv *priv,
 					 struct tc_mqprio_qopt_offload *mqprio)
 {
@@ -3322,32 +3373,32 @@ static int mlx5e_setup_tc_mqprio_channel(struct mlx5e_priv *priv,
 	if (err)
 		return err;
 
-	rl = NULL;
-	if (mlx5e_mqprio_rate_limit(mqprio)) {
-		rl = mlx5e_mqprio_rl_alloc();
-		if (!rl)
-			return -ENOMEM;
-		err = mlx5e_mqprio_rl_init(rl, priv->mdev, mqprio->qopt.num_tc,
-					   mqprio->max_rate);
-		if (err) {
-			mlx5e_mqprio_rl_free(rl);
-			return err;
-		}
-	}
+	rl = mlx5e_mqprio_rl_create(priv->mdev, mqprio->qopt.num_tc, mqprio->max_rate);
+	if (IS_ERR(rl))
+		return PTR_ERR(rl);
 
 	new_params = priv->channels.params;
-	mlx5e_params_mqprio_channel_set(&new_params, &mqprio->qopt, rl);
+	mlx5e_params_mqprio_channel_set(&new_params, mqprio, rl);
 
 	nch_changed = mlx5e_get_dcb_num_tc(&priv->channels.params) > 1;
 	preactivate = nch_changed ? mlx5e_num_channels_changed_ctx :
 		mlx5e_update_netdev_queues_ctx;
 	err = mlx5e_safe_switch_params(priv, &new_params, preactivate, NULL, true);
-	if (err && rl) {
-		mlx5e_mqprio_rl_cleanup(rl);
-		mlx5e_mqprio_rl_free(rl);
+	if (err) {
+		if (rl) {
+			mlx5e_mqprio_rl_cleanup(rl);
+			mlx5e_mqprio_rl_free(rl);
+		}
+		return err;
 	}
 
-	return err;
+	if (priv->mqprio_rl) {
+		mlx5e_mqprio_rl_cleanup(priv->mqprio_rl);
+		mlx5e_mqprio_rl_free(priv->mqprio_rl);
+	}
+	priv->mqprio_rl = rl;
+
+	return 0;
 }
 
 static int mlx5e_setup_tc_mqprio(struct mlx5e_priv *priv,
@@ -5102,6 +5153,23 @@ static void mlx5e_cleanup_nic_rx(struct mlx5e_priv *priv)
 	priv->rx_res = NULL;
 }
 
+static void mlx5e_set_mqprio_rl(struct mlx5e_priv *priv)
+{
+	struct mlx5e_params *params;
+	struct mlx5e_mqprio_rl *rl;
+
+	params = &priv->channels.params;
+	if (params->mqprio.mode != TC_MQPRIO_MODE_CHANNEL)
+		return;
+
+	rl = mlx5e_mqprio_rl_create(priv->mdev, params->mqprio.num_tc,
+				    params->mqprio.channel.max_rate);
+	if (IS_ERR(rl))
+		rl = NULL;
+	priv->mqprio_rl = rl;
+	mlx5e_mqprio_rl_update_params(params, rl);
+}
+
 static int mlx5e_init_nic_tx(struct mlx5e_priv *priv)
 {
 	int err;
@@ -5112,6 +5180,7 @@ static int mlx5e_init_nic_tx(struct mlx5e_priv *priv)
 		return err;
 	}
 
+	mlx5e_set_mqprio_rl(priv);
 	mlx5e_dcbnl_initialize(priv);
 	return 0;
 }
@@ -5346,11 +5415,6 @@ void mlx5e_priv_cleanup(struct mlx5e_priv *priv)
 		kfree(priv->htb.qos_sq_stats[i]);
 	kvfree(priv->htb.qos_sq_stats);
 
-	if (priv->mqprio_rl) {
-		mlx5e_mqprio_rl_cleanup(priv->mqprio_rl);
-		mlx5e_mqprio_rl_free(priv->mqprio_rl);
-	}
-
 	memset(priv, 0, sizeof(*priv));
 }
 
-- 
2.36.1

[net-next V2 03/13] net/mlx5e: HTB, reduce visibility of htb functions

[Saeed Mahameed]

From: Saeed Mahameed <saeedm@nvidia.com>

No need to expose all htb tc functions to the main driver file,
expose only the master htb tc function mlx5e_htb_setup_tc()
which selects the internal "now static" function to call.

Signed-off-by: Saeed Mahameed <saeedm@nvidia.com>
Signed-off-by: Moshe Tal <moshet@nvidia.com>
Reviewed-by: Tariq Toukan <tariqt@nvidia.com>
Reviewed-by: Maxim Mikityanskiy <maximmi@nvidia.com>
---
 .../net/ethernet/mellanox/mlx5/core/en/qos.c  | 84 +++++++++++++++----
 .../net/ethernet/mellanox/mlx5/core/en/qos.h  | 16 +---
 .../net/ethernet/mellanox/mlx5/core/en_main.c | 44 +---------
 3 files changed, 70 insertions(+), 74 deletions(-)

diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c b/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c
index 9db677e9ca9c..c37f346b5a3b 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c
@@ -2,6 +2,7 @@
 /* Copyright (c) 2020, Mellanox Technologies inc. All rights reserved. */
 #include <net/sch_generic.h>
 
+#include <net/pkt_cls.h>
 #include "en.h"
 #include "params.h"
 #include "../qos.h"
@@ -482,10 +483,11 @@ static void mlx5e_qos_deactivate_all_queues(struct mlx5e_channels *chs)
 		mlx5e_qos_deactivate_queues(chs->c[i]);
 }
 
-/* HTB API */
+/* HTB TC handlers */
 
-int mlx5e_htb_root_add(struct mlx5e_priv *priv, u16 htb_maj_id, u16 htb_defcls,
-		       struct netlink_ext_ack *extack)
+static int
+mlx5e_htb_root_add(struct mlx5e_priv *priv, u16 htb_maj_id, u16 htb_defcls,
+		   struct netlink_ext_ack *extack)
 {
 	struct mlx5e_qos_node *root;
 	bool opened;
@@ -542,7 +544,7 @@ int mlx5e_htb_root_add(struct mlx5e_priv *priv, u16 htb_maj_id, u16 htb_defcls,
 	return err;
 }
 
-int mlx5e_htb_root_del(struct mlx5e_priv *priv)
+static int mlx5e_htb_root_del(struct mlx5e_priv *priv)
 {
 	struct mlx5e_qos_node *root;
 	int err;
@@ -607,9 +609,10 @@ static void mlx5e_htb_convert_ceil(struct mlx5e_priv *priv, u64 ceil, u32 *max_a
 		ceil, *max_average_bw);
 }
 
-int mlx5e_htb_leaf_alloc_queue(struct mlx5e_priv *priv, u16 classid,
-			       u32 parent_classid, u64 rate, u64 ceil,
-			       struct netlink_ext_ack *extack)
+static int
+mlx5e_htb_leaf_alloc_queue(struct mlx5e_priv *priv, u16 classid,
+			   u32 parent_classid, u64 rate, u64 ceil,
+			   struct netlink_ext_ack *extack)
 {
 	struct mlx5e_qos_node *node, *parent;
 	int qid;
@@ -661,8 +664,9 @@ int mlx5e_htb_leaf_alloc_queue(struct mlx5e_priv *priv, u16 classid,
 	return mlx5e_qid_from_qos(&priv->channels, node->qid);
 }
 
-int mlx5e_htb_leaf_to_inner(struct mlx5e_priv *priv, u16 classid, u16 child_classid,
-			    u64 rate, u64 ceil, struct netlink_ext_ack *extack)
+static int
+mlx5e_htb_leaf_to_inner(struct mlx5e_priv *priv, u16 classid, u16 child_classid,
+			u64 rate, u64 ceil, struct netlink_ext_ack *extack)
 {
 	struct mlx5e_qos_node *node, *child;
 	int err, tmp_err;
@@ -781,8 +785,8 @@ static void mlx5e_reset_qdisc(struct net_device *dev, u16 qid)
 	spin_unlock_bh(qdisc_lock(qdisc));
 }
 
-int mlx5e_htb_leaf_del(struct mlx5e_priv *priv, u16 *classid,
-		       struct netlink_ext_ack *extack)
+static int mlx5e_htb_leaf_del(struct mlx5e_priv *priv, u16 *classid,
+			      struct netlink_ext_ack *extack)
 {
 	struct mlx5e_qos_node *node;
 	struct netdev_queue *txq;
@@ -876,8 +880,9 @@ int mlx5e_htb_leaf_del(struct mlx5e_priv *priv, u16 *classid,
 	return 0;
 }
 
-int mlx5e_htb_leaf_del_last(struct mlx5e_priv *priv, u16 classid, bool force,
-			    struct netlink_ext_ack *extack)
+static int
+mlx5e_htb_leaf_del_last(struct mlx5e_priv *priv, u16 classid, bool force,
+			struct netlink_ext_ack *extack)
 {
 	struct mlx5e_qos_node *node, *parent;
 	u32 old_hw_id, new_hw_id;
@@ -956,8 +961,9 @@ int mlx5e_htb_leaf_del_last(struct mlx5e_priv *priv, u16 classid, bool force,
 	return 0;
 }
 
-static int mlx5e_qos_update_children(struct mlx5e_priv *priv, struct mlx5e_qos_node *node,
-				     struct netlink_ext_ack *extack)
+static int
+mlx5e_qos_update_children(struct mlx5e_priv *priv, struct mlx5e_qos_node *node,
+			  struct netlink_ext_ack *extack)
 {
 	struct mlx5e_qos_node *child;
 	int err = 0;
@@ -988,8 +994,9 @@ static int mlx5e_qos_update_children(struct mlx5e_priv *priv, struct mlx5e_qos_n
 	return err;
 }
 
-int mlx5e_htb_node_modify(struct mlx5e_priv *priv, u16 classid, u64 rate, u64 ceil,
-			  struct netlink_ext_ack *extack)
+static int
+mlx5e_htb_node_modify(struct mlx5e_priv *priv, u16 classid, u64 rate, u64 ceil,
+		      struct netlink_ext_ack *extack)
 {
 	u32 bw_share, max_average_bw;
 	struct mlx5e_qos_node *node;
@@ -1028,6 +1035,48 @@ int mlx5e_htb_node_modify(struct mlx5e_priv *priv, u16 classid, u64 rate, u64 ce
 	return err;
 }
 
+/* HTB API */
+int mlx5e_htb_setup_tc(struct mlx5e_priv *priv, struct tc_htb_qopt_offload *htb)
+{
+	int res;
+
+	switch (htb->command) {
+	case TC_HTB_CREATE:
+		return mlx5e_htb_root_add(priv, htb->parent_classid, htb->classid,
+					  htb->extack);
+	case TC_HTB_DESTROY:
+		return mlx5e_htb_root_del(priv);
+	case TC_HTB_LEAF_ALLOC_QUEUE:
+		res = mlx5e_htb_leaf_alloc_queue(priv, htb->classid, htb->parent_classid,
+						 htb->rate, htb->ceil, htb->extack);
+		if (res < 0)
+			return res;
+		htb->qid = res;
+		return 0;
+	case TC_HTB_LEAF_TO_INNER:
+		return mlx5e_htb_leaf_to_inner(priv, htb->parent_classid, htb->classid,
+					       htb->rate, htb->ceil, htb->extack);
+	case TC_HTB_LEAF_DEL:
+		return mlx5e_htb_leaf_del(priv, &htb->classid, htb->extack);
+	case TC_HTB_LEAF_DEL_LAST:
+	case TC_HTB_LEAF_DEL_LAST_FORCE:
+		return mlx5e_htb_leaf_del_last(priv, htb->classid,
+					       htb->command == TC_HTB_LEAF_DEL_LAST_FORCE,
+					       htb->extack);
+	case TC_HTB_NODE_MODIFY:
+		return mlx5e_htb_node_modify(priv, htb->classid, htb->rate, htb->ceil,
+					     htb->extack);
+	case TC_HTB_LEAF_QUERY_QUEUE:
+		res = mlx5e_get_txq_by_classid(priv, htb->classid);
+		if (res < 0)
+			return res;
+		htb->qid = res;
+		return 0;
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
 struct mlx5e_mqprio_rl {
 	struct mlx5_core_dev *mdev;
 	u32 root_id;
@@ -1111,3 +1160,4 @@ int mlx5e_mqprio_rl_get_node_hw_id(struct mlx5e_mqprio_rl *rl, int tc, u32 *hw_i
 	*hw_id = rl->leaves_id[tc];
 	return 0;
 }
+
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en/qos.h b/drivers/net/ethernet/mellanox/mlx5/core/en/qos.h
index 5d9bd91d86c2..6fbddd586736 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en/qos.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en/qos.h
@@ -11,6 +11,7 @@
 struct mlx5e_priv;
 struct mlx5e_channels;
 struct mlx5e_channel;
+struct tc_htb_qopt_offload;
 
 int mlx5e_qos_bytes_rate_check(struct mlx5_core_dev *mdev, u64 nbytes);
 int mlx5e_qos_max_leaf_nodes(struct mlx5_core_dev *mdev);
@@ -26,20 +27,7 @@ void mlx5e_qos_deactivate_queues(struct mlx5e_channel *c);
 void mlx5e_qos_close_queues(struct mlx5e_channel *c);
 
 /* HTB API */
-int mlx5e_htb_root_add(struct mlx5e_priv *priv, u16 htb_maj_id, u16 htb_defcls,
-		       struct netlink_ext_ack *extack);
-int mlx5e_htb_root_del(struct mlx5e_priv *priv);
-int mlx5e_htb_leaf_alloc_queue(struct mlx5e_priv *priv, u16 classid,
-			       u32 parent_classid, u64 rate, u64 ceil,
-			       struct netlink_ext_ack *extack);
-int mlx5e_htb_leaf_to_inner(struct mlx5e_priv *priv, u16 classid, u16 child_classid,
-			    u64 rate, u64 ceil, struct netlink_ext_ack *extack);
-int mlx5e_htb_leaf_del(struct mlx5e_priv *priv, u16 *classid,
-		       struct netlink_ext_ack *extack);
-int mlx5e_htb_leaf_del_last(struct mlx5e_priv *priv, u16 classid, bool force,
-			    struct netlink_ext_ack *extack);
-int mlx5e_htb_node_modify(struct mlx5e_priv *priv, u16 classid, u64 rate, u64 ceil,
-			  struct netlink_ext_ack *extack);
+int mlx5e_htb_setup_tc(struct mlx5e_priv *priv, struct tc_htb_qopt_offload *htb);
 
 /* MQPRIO TX rate limit */
 struct mlx5e_mqprio_rl;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_main.c b/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
index fe07180a957a..d4b39351a223 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
@@ -31,7 +31,6 @@
  */
 
 #include <net/tc_act/tc_gact.h>
-#include <net/pkt_cls.h>
 #include <linux/mlx5/fs.h>
 #include <net/vxlan.h>
 #include <net/geneve.h>
@@ -3420,47 +3419,6 @@ static int mlx5e_setup_tc_mqprio(struct mlx5e_priv *priv,
 	}
 }
 
-static int mlx5e_setup_tc_htb(struct mlx5e_priv *priv, struct tc_htb_qopt_offload *htb)
-{
-	int res;
-
-	switch (htb->command) {
-	case TC_HTB_CREATE:
-		return mlx5e_htb_root_add(priv, htb->parent_classid, htb->classid,
-					  htb->extack);
-	case TC_HTB_DESTROY:
-		return mlx5e_htb_root_del(priv);
-	case TC_HTB_LEAF_ALLOC_QUEUE:
-		res = mlx5e_htb_leaf_alloc_queue(priv, htb->classid, htb->parent_classid,
-						 htb->rate, htb->ceil, htb->extack);
-		if (res < 0)
-			return res;
-		htb->qid = res;
-		return 0;
-	case TC_HTB_LEAF_TO_INNER:
-		return mlx5e_htb_leaf_to_inner(priv, htb->parent_classid, htb->classid,
-					       htb->rate, htb->ceil, htb->extack);
-	case TC_HTB_LEAF_DEL:
-		return mlx5e_htb_leaf_del(priv, &htb->classid, htb->extack);
-	case TC_HTB_LEAF_DEL_LAST:
-	case TC_HTB_LEAF_DEL_LAST_FORCE:
-		return mlx5e_htb_leaf_del_last(priv, htb->classid,
-					       htb->command == TC_HTB_LEAF_DEL_LAST_FORCE,
-					       htb->extack);
-	case TC_HTB_NODE_MODIFY:
-		return mlx5e_htb_node_modify(priv, htb->classid, htb->rate, htb->ceil,
-					     htb->extack);
-	case TC_HTB_LEAF_QUERY_QUEUE:
-		res = mlx5e_get_txq_by_classid(priv, htb->classid);
-		if (res < 0)
-			return res;
-		htb->qid = res;
-		return 0;
-	default:
-		return -EOPNOTSUPP;
-	}
-}
-
 static LIST_HEAD(mlx5e_block_cb_list);
 
 static int mlx5e_setup_tc(struct net_device *dev, enum tc_setup_type type,
@@ -3494,7 +3452,7 @@ static int mlx5e_setup_tc(struct net_device *dev, enum tc_setup_type type,
 		return err;
 	case TC_SETUP_QDISC_HTB:
 		mutex_lock(&priv->state_lock);
-		err = mlx5e_setup_tc_htb(priv, type_data);
+		err = mlx5e_htb_setup_tc(priv, type_data);
 		mutex_unlock(&priv->state_lock);
 		return err;
 	default:
-- 
2.36.1

[net-next V2 04/13] net/mlx5e: HTB, move ids to selq_params struct

[Saeed Mahameed]

From: Moshe Tal <moshet@nvidia.com>

HTB id fields are needed for selecting queue. Moving them to the
selq_params struct will simplify synchronization between control flow
and mlx5e_select_queues and will keep the IDs in the hot cacheline of
mlx5e_selq_params.

Replace mlx5e_selq_prepare() with separate functions that change subsets
of parameters, while keeping the rest.

This also will be useful to hide mlx5e_htb structure from the rest of the
driver in a later patch in this series.

Signed-off-by: Saeed Mahameed <saeedm@nvidia.com>
Signed-off-by: Moshe Tal <moshet@nvidia.com>
Reviewed-by: Tariq Toukan <tariqt@nvidia.com>
Reviewed-by: Maxim Mikityanskiy <maximmi@nvidia.com>
---
 drivers/net/ethernet/mellanox/mlx5/core/en.h  |  2 -
 .../net/ethernet/mellanox/mlx5/core/en/qos.c  | 23 +++------
 .../net/ethernet/mellanox/mlx5/core/en/selq.c | 48 ++++++++++++++++---
 .../net/ethernet/mellanox/mlx5/core/en/selq.h |  4 +-
 .../ethernet/mellanox/mlx5/core/en_ethtool.c  |  4 +-
 .../net/ethernet/mellanox/mlx5/core/en_main.c | 15 +++---
 6 files changed, 62 insertions(+), 34 deletions(-)

diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en.h b/drivers/net/ethernet/mellanox/mlx5/core/en.h
index 5c88c3896b96..1222156e222b 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en.h
@@ -904,8 +904,6 @@ struct mlx5e_htb {
 	DECLARE_BITMAP(qos_used_qids, MLX5E_QOS_MAX_LEAF_NODES);
 	struct mlx5e_sq_stats **qos_sq_stats;
 	u16 max_qos_sqs;
-	u16 maj_id;
-	u16 defcls;
 };
 
 struct mlx5e_trap;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c b/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c
index c37f346b5a3b..75df55850954 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c
@@ -144,9 +144,9 @@ static void mlx5e_sw_node_delete(struct mlx5e_priv *priv, struct mlx5e_qos_node
 static u16 mlx5e_qid_from_qos(struct mlx5e_channels *chs, u16 qid)
 {
 	/* These channel params are safe to access from the datapath, because:
-	 * 1. This function is called only after checking priv->htb.maj_id != 0,
+	 * 1. This function is called only after checking selq->htb_maj_id != 0,
 	 *    and the number of queues can't change while HTB offload is active.
-	 * 2. When priv->htb.maj_id becomes 0, synchronize_rcu waits for
+	 * 2. When selq->htb_maj_id becomes 0, synchronize_rcu waits for
 	 *    mlx5e_select_queue to finish while holding priv->state_lock,
 	 *    preventing other code from changing the number of queues.
 	 */
@@ -417,7 +417,7 @@ int mlx5e_qos_open_queues(struct mlx5e_priv *priv, struct mlx5e_channels *chs)
 	struct mlx5e_qos_node *node = NULL;
 	int bkt, err;
 
-	if (!priv->htb.maj_id)
+	if (!mlx5e_selq_is_htb_enabled(&priv->selq))
 		return 0;
 
 	err = mlx5e_qos_alloc_queues(priv, chs);
@@ -501,10 +501,10 @@ mlx5e_htb_root_add(struct mlx5e_priv *priv, u16 htb_maj_id, u16 htb_defcls,
 		return -EOPNOTSUPP;
 	}
 
+	mlx5e_selq_prepare_htb(&priv->selq, htb_maj_id, htb_defcls);
+
 	opened = test_bit(MLX5E_STATE_OPENED, &priv->state);
 	if (opened) {
-		mlx5e_selq_prepare(&priv->selq, &priv->channels.params, true);
-
 		err = mlx5e_qos_alloc_queues(priv, &priv->channels);
 		if (err)
 			goto err_cancel_selq;
@@ -522,14 +522,7 @@ mlx5e_htb_root_add(struct mlx5e_priv *priv, u16 htb_maj_id, u16 htb_defcls,
 		goto err_sw_node_delete;
 	}
 
-	WRITE_ONCE(priv->htb.defcls, htb_defcls);
-	/* Order maj_id after defcls - pairs with
-	 * mlx5e_select_queue/mlx5e_select_htb_queues.
-	 */
-	smp_store_release(&priv->htb.maj_id, htb_maj_id);
-
-	if (opened)
-		mlx5e_selq_apply(&priv->selq);
+	mlx5e_selq_apply(&priv->selq);
 
 	return 0;
 
@@ -556,11 +549,9 @@ static int mlx5e_htb_root_del(struct mlx5e_priv *priv)
 	 */
 	synchronize_net();
 
-	mlx5e_selq_prepare(&priv->selq, &priv->channels.params, false);
+	mlx5e_selq_prepare_htb(&priv->selq, 0, 0);
 	mlx5e_selq_apply(&priv->selq);
 
-	WRITE_ONCE(priv->htb.maj_id, 0);
-
 	root = mlx5e_sw_node_find(priv, MLX5E_HTB_CLASSID_ROOT);
 	if (!root) {
 		qos_err(priv->mdev, "Failed to find the root node in the QoS tree\n");
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en/selq.c b/drivers/net/ethernet/mellanox/mlx5/core/en/selq.c
index d98a277eb7f8..0e1d84ef82b6 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en/selq.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en/selq.c
@@ -19,6 +19,8 @@ struct mlx5e_selq_params {
 			bool is_ptp : 1;
 		};
 	};
+	u16 htb_maj_id;
+	u16 htb_defcls;
 };
 
 int mlx5e_selq_init(struct mlx5e_selq *selq, struct mutex *state_lock)
@@ -44,6 +46,8 @@ int mlx5e_selq_init(struct mlx5e_selq *selq, struct mutex *state_lock)
 		.num_tcs = 1,
 		.is_htb = false,
 		.is_ptp = false,
+		.htb_maj_id = 0,
+		.htb_defcls = 0,
 	};
 	rcu_assign_pointer(selq->active, init_params);
 
@@ -64,21 +68,50 @@ void mlx5e_selq_cleanup(struct mlx5e_selq *selq)
 	selq->standby = NULL;
 }
 
-void mlx5e_selq_prepare(struct mlx5e_selq *selq, struct mlx5e_params *params, bool htb)
+void mlx5e_selq_prepare_params(struct mlx5e_selq *selq, struct mlx5e_params *params)
 {
+	struct mlx5e_selq_params *selq_active;
+
 	lockdep_assert_held(selq->state_lock);
 	WARN_ON_ONCE(selq->is_prepared);
 
 	selq->is_prepared = true;
 
+	selq_active = rcu_dereference_protected(selq->active,
+						lockdep_is_held(selq->state_lock));
+	*selq->standby = *selq_active;
 	selq->standby->num_channels = params->num_channels;
 	selq->standby->num_tcs = mlx5e_get_dcb_num_tc(params);
 	selq->standby->num_regular_queues =
 		selq->standby->num_channels * selq->standby->num_tcs;
-	selq->standby->is_htb = htb;
 	selq->standby->is_ptp = MLX5E_GET_PFLAG(params, MLX5E_PFLAG_TX_PORT_TS);
 }
 
+bool mlx5e_selq_is_htb_enabled(struct mlx5e_selq *selq)
+{
+	struct mlx5e_selq_params *selq_active =
+		rcu_dereference_protected(selq->active, lockdep_is_held(selq->state_lock));
+
+	return selq_active->htb_maj_id;
+}
+
+void mlx5e_selq_prepare_htb(struct mlx5e_selq *selq, u16 htb_maj_id, u16 htb_defcls)
+{
+	struct mlx5e_selq_params *selq_active;
+
+	lockdep_assert_held(selq->state_lock);
+	WARN_ON_ONCE(selq->is_prepared);
+
+	selq->is_prepared = true;
+
+	selq_active = rcu_dereference_protected(selq->active,
+						lockdep_is_held(selq->state_lock));
+	*selq->standby = *selq_active;
+	selq->standby->is_htb = htb_maj_id;
+	selq->standby->htb_maj_id = htb_maj_id;
+	selq->standby->htb_defcls = htb_defcls;
+}
+
 void mlx5e_selq_apply(struct mlx5e_selq *selq)
 {
 	struct mlx5e_selq_params *old_params;
@@ -137,15 +170,16 @@ static u16 mlx5e_select_ptpsq(struct net_device *dev, struct sk_buff *skb,
 	return selq->num_regular_queues + up;
 }
 
-static int mlx5e_select_htb_queue(struct mlx5e_priv *priv, struct sk_buff *skb)
+static int mlx5e_select_htb_queue(struct mlx5e_priv *priv, struct sk_buff *skb,
+				  struct mlx5e_selq_params *selq)
 {
 	u16 classid;
 
 	/* Order maj_id before defcls - pairs with mlx5e_htb_root_add. */
-	if ((TC_H_MAJ(skb->priority) >> 16) == smp_load_acquire(&priv->htb.maj_id))
+	if ((TC_H_MAJ(skb->priority) >> 16) == selq->htb_maj_id)
 		classid = TC_H_MIN(skb->priority);
 	else
-		classid = READ_ONCE(priv->htb.defcls);
+		classid = selq->htb_defcls;
 
 	if (!classid)
 		return 0;
@@ -187,10 +221,10 @@ u16 mlx5e_select_queue(struct net_device *dev, struct sk_buff *skb,
 			up * selq->num_channels;
 	}
 
-	if (unlikely(selq->is_htb)) {
+	if (unlikely(selq->htb_maj_id)) {
 		/* num_tcs == 1, shortcut for PTP */
 
-		txq_ix = mlx5e_select_htb_queue(priv, skb);
+		txq_ix = mlx5e_select_htb_queue(priv, skb, selq);
 		if (txq_ix > 0)
 			return txq_ix;
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en/selq.h b/drivers/net/ethernet/mellanox/mlx5/core/en/selq.h
index 6c070141d8f1..fd590f80e4d1 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en/selq.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en/selq.h
@@ -21,7 +21,9 @@ struct sk_buff;
 
 int mlx5e_selq_init(struct mlx5e_selq *selq, struct mutex *state_lock);
 void mlx5e_selq_cleanup(struct mlx5e_selq *selq);
-void mlx5e_selq_prepare(struct mlx5e_selq *selq, struct mlx5e_params *params, bool htb);
+void mlx5e_selq_prepare_params(struct mlx5e_selq *selq, struct mlx5e_params *params);
+void mlx5e_selq_prepare_htb(struct mlx5e_selq *selq, u16 htb_maj_id, u16 htb_defcls);
+bool mlx5e_selq_is_htb_enabled(struct mlx5e_selq *selq);
 void mlx5e_selq_apply(struct mlx5e_selq *selq);
 void mlx5e_selq_cancel(struct mlx5e_selq *selq);
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c b/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c
index 820912eb7bcf..b811207fe5ed 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c
@@ -459,7 +459,7 @@ int mlx5e_ethtool_set_channels(struct mlx5e_priv *priv,
 	 * because the numeration of the QoS SQs will change, while per-queue
 	 * qdiscs are attached.
 	 */
-	if (priv->htb.maj_id) {
+	if (mlx5e_selq_is_htb_enabled(&priv->selq)) {
 		err = -EINVAL;
 		netdev_err(priv->netdev, "%s: HTB offload is active, cannot change the number of channels\n",
 			   __func__);
@@ -2075,7 +2075,7 @@ static int set_pflag_tx_port_ts(struct net_device *netdev, bool enable)
 	 * the numeration of the QoS SQs will change, while per-queue qdiscs are
 	 * attached.
 	 */
-	if (priv->htb.maj_id) {
+	if (mlx5e_selq_is_htb_enabled(&priv->selq)) {
 		netdev_err(priv->netdev, "%s: HTB offload is active, cannot change the PTP state\n",
 			   __func__);
 		return -EINVAL;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_main.c b/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
index d4b39351a223..700bca033769 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
@@ -2839,7 +2839,7 @@ int mlx5e_safe_switch_params(struct mlx5e_priv *priv,
 
 	new_chs.params = *params;
 
-	mlx5e_selq_prepare(&priv->selq, &new_chs.params, !!priv->htb.maj_id);
+	mlx5e_selq_prepare_params(&priv->selq, &new_chs.params);
 
 	err = mlx5e_open_channels(priv, &new_chs);
 	if (err)
@@ -2895,7 +2895,7 @@ int mlx5e_open_locked(struct net_device *netdev)
 	struct mlx5e_priv *priv = netdev_priv(netdev);
 	int err;
 
-	mlx5e_selq_prepare(&priv->selq, &priv->channels.params, !!priv->htb.maj_id);
+	mlx5e_selq_prepare_params(&priv->selq, &priv->channels.params);
 
 	set_bit(MLX5E_STATE_OPENED, &priv->state);
 
@@ -3406,7 +3406,7 @@ static int mlx5e_setup_tc_mqprio(struct mlx5e_priv *priv,
 	/* MQPRIO is another toplevel qdisc that can't be attached
 	 * simultaneously with the offloaded HTB.
 	 */
-	if (WARN_ON(priv->htb.maj_id))
+	if (WARN_ON(mlx5e_selq_is_htb_enabled(&priv->selq)))
 		return -EINVAL;
 
 	switch (mqprio->mode) {
@@ -3672,6 +3672,7 @@ static int set_feature_cvlan_filter(struct net_device *netdev, bool enable)
 static int set_feature_hw_tc(struct net_device *netdev, bool enable)
 {
 	struct mlx5e_priv *priv = netdev_priv(netdev);
+	int err = 0;
 
 #if IS_ENABLED(CONFIG_MLX5_CLS_ACT)
 	if (!enable && mlx5e_tc_num_filters(priv, MLX5_TC_FLAG(NIC_OFFLOAD))) {
@@ -3681,12 +3682,14 @@ static int set_feature_hw_tc(struct net_device *netdev, bool enable)
 	}
 #endif
 
-	if (!enable && priv->htb.maj_id) {
+	mutex_lock(&priv->state_lock);
+	if (!enable && mlx5e_selq_is_htb_enabled(&priv->selq)) {
 		netdev_err(netdev, "Active HTB offload, can't turn hw_tc_offload off\n");
-		return -EINVAL;
+		err = -EINVAL;
 	}
+	mutex_unlock(&priv->state_lock);
 
-	return 0;
+	return err;
 }
 
 static int set_feature_rx_all(struct net_device *netdev, bool enable)
-- 
2.36.1

[net-next V2 05/13] net/mlx5e: HTB, move section comment to the right place

[Saeed Mahameed]

From: Moshe Tal <moshet@nvidia.com>

mlx5e_get_qos_sq is a part of the SQ lifecycle, so need be under the
title.

Signed-off-by: Moshe Tal <moshet@nvidia.com>
Reviewed-by: Tariq Toukan <tariqt@nvidia.com>
Reviewed-by: Maxim Mikityanskiy <maximmi@nvidia.com>
Signed-off-by: Saeed Mahameed <saeedm@nvidia.com>
---
 drivers/net/ethernet/mellanox/mlx5/core/en/qos.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c b/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c
index 75df55850954..9a61c44e7f72 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c
@@ -180,6 +180,8 @@ int mlx5e_get_txq_by_classid(struct mlx5e_priv *priv, u16 classid)
 	return res;
 }
 
+/* SQ lifecycle */
+
 static struct mlx5e_txqsq *mlx5e_get_qos_sq(struct mlx5e_priv *priv, int qid)
 {
 	struct mlx5e_params *params = &priv->channels.params;
@@ -195,8 +197,6 @@ static struct mlx5e_txqsq *mlx5e_get_qos_sq(struct mlx5e_priv *priv, int qid)
 	return mlx5e_state_dereference(priv, qos_sqs[qid]);
 }
 
-/* SQ lifecycle */
-
 static int mlx5e_open_qos_sq(struct mlx5e_priv *priv, struct mlx5e_channels *chs,
 			     struct mlx5e_qos_node *node)
 {
-- 
2.36.1

[net-next V2 06/13] net/mlx5e: HTB, move stats and max_sqs to priv

[Saeed Mahameed]

From: Moshe Tal <moshet@nvidia.com>

Preparation for dynamic allocation of the HTB struct.
The statistics should be preserved even when the struct is de-allocated.

Signed-off-by: Moshe Tal <moshet@nvidia.com>
Reviewed-by: Tariq Toukan <tariqt@nvidia.com>
Reviewed-by: Maxim Mikityanskiy <maximmi@nvidia.com>
Signed-off-by: Saeed Mahameed <saeedm@nvidia.com>
---
 drivers/net/ethernet/mellanox/mlx5/core/en.h     |  4 ++--
 drivers/net/ethernet/mellanox/mlx5/core/en/qos.c | 16 ++++++++--------
 .../net/ethernet/mellanox/mlx5/core/en_main.c    |  6 +++---
 .../net/ethernet/mellanox/mlx5/core/en_stats.c   | 12 ++++++------
 4 files changed, 19 insertions(+), 19 deletions(-)

diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en.h b/drivers/net/ethernet/mellanox/mlx5/core/en.h
index 1222156e222b..d2ed27575097 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en.h
@@ -902,8 +902,6 @@ struct mlx5e_scratchpad {
 struct mlx5e_htb {
 	DECLARE_HASHTABLE(qos_tc2node, order_base_2(MLX5E_QOS_MAX_LEAF_NODES));
 	DECLARE_BITMAP(qos_used_qids, MLX5E_QOS_MAX_LEAF_NODES);
-	struct mlx5e_sq_stats **qos_sq_stats;
-	u16 max_qos_sqs;
 };
 
 struct mlx5e_trap;
@@ -944,6 +942,8 @@ struct mlx5e_priv {
 	struct mlx5e_channel_stats **channel_stats;
 	struct mlx5e_channel_stats trap_stats;
 	struct mlx5e_ptp_stats     ptp_stats;
+	struct mlx5e_sq_stats      **htb_qos_sq_stats;
+	u16                        htb_max_qos_sqs;
 	u16                        stats_nch;
 	u16                        max_nch;
 	u8                         max_opened_tc;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c b/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c
index 9a61c44e7f72..6136cad397dd 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c
@@ -213,11 +213,11 @@ static int mlx5e_open_qos_sq(struct mlx5e_priv *priv, struct mlx5e_channels *chs
 
 	txq_ix = mlx5e_qid_from_qos(chs, node->qid);
 
-	WARN_ON(node->qid > priv->htb.max_qos_sqs);
-	if (node->qid == priv->htb.max_qos_sqs) {
+	WARN_ON(node->qid > priv->htb_max_qos_sqs);
+	if (node->qid == priv->htb_max_qos_sqs) {
 		struct mlx5e_sq_stats *stats, **stats_list = NULL;
 
-		if (priv->htb.max_qos_sqs == 0) {
+		if (priv->htb_max_qos_sqs == 0) {
 			stats_list = kvcalloc(mlx5e_qos_max_leaf_nodes(priv->mdev),
 					      sizeof(*stats_list),
 					      GFP_KERNEL);
@@ -230,12 +230,12 @@ static int mlx5e_open_qos_sq(struct mlx5e_priv *priv, struct mlx5e_channels *chs
 			return -ENOMEM;
 		}
 		if (stats_list)
-			WRITE_ONCE(priv->htb.qos_sq_stats, stats_list);
-		WRITE_ONCE(priv->htb.qos_sq_stats[node->qid], stats);
-		/* Order max_qos_sqs increment after writing the array pointer.
+			WRITE_ONCE(priv->htb_qos_sq_stats, stats_list);
+		WRITE_ONCE(priv->htb_qos_sq_stats[node->qid], stats);
+		/* Order htb_max_qos_sqs increment after writing the array pointer.
 		 * Pairs with smp_load_acquire in en_stats.c.
 		 */
-		smp_store_release(&priv->htb.max_qos_sqs, priv->htb.max_qos_sqs + 1);
+		smp_store_release(&priv->htb_max_qos_sqs, priv->htb_max_qos_sqs + 1);
 	}
 
 	ix = node->qid % params->num_channels;
@@ -259,7 +259,7 @@ static int mlx5e_open_qos_sq(struct mlx5e_priv *priv, struct mlx5e_channels *chs
 		goto err_free_sq;
 	err = mlx5e_open_txqsq(c, priv->tisn[c->lag_port][0], txq_ix, params,
 			       &param_sq, sq, 0, node->hw_id,
-			       priv->htb.qos_sq_stats[node->qid]);
+			       priv->htb_qos_sq_stats[node->qid]);
 	if (err)
 		goto err_close_cq;
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_main.c b/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
index 700bca033769..fed24b5a0bae 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
@@ -5372,9 +5372,9 @@ void mlx5e_priv_cleanup(struct mlx5e_priv *priv)
 	mutex_unlock(&priv->state_lock);
 	free_cpumask_var(priv->scratchpad.cpumask);
 
-	for (i = 0; i < priv->htb.max_qos_sqs; i++)
-		kfree(priv->htb.qos_sq_stats[i]);
-	kvfree(priv->htb.qos_sq_stats);
+	for (i = 0; i < priv->htb_max_qos_sqs; i++)
+		kfree(priv->htb_qos_sq_stats[i]);
+	kvfree(priv->htb_qos_sq_stats);
 
 	memset(priv, 0, sizeof(*priv));
 }
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_stats.c b/drivers/net/ethernet/mellanox/mlx5/core/en_stats.c
index 1e87bb2b7541..1a88406ee6d2 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_stats.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_stats.c
@@ -474,8 +474,8 @@ static void mlx5e_stats_grp_sw_update_stats_qos(struct mlx5e_priv *priv,
 	int i;
 
 	/* Pairs with smp_store_release in mlx5e_open_qos_sq. */
-	max_qos_sqs = smp_load_acquire(&priv->htb.max_qos_sqs);
-	stats = READ_ONCE(priv->htb.qos_sq_stats);
+	max_qos_sqs = smp_load_acquire(&priv->htb_max_qos_sqs);
+	stats = READ_ONCE(priv->htb_qos_sq_stats);
 
 	for (i = 0; i < max_qos_sqs; i++) {
 		mlx5e_stats_grp_sw_update_stats_sq(s, READ_ONCE(stats[i]));
@@ -2184,13 +2184,13 @@ static const struct counter_desc qos_sq_stats_desc[] = {
 static MLX5E_DECLARE_STATS_GRP_OP_NUM_STATS(qos)
 {
 	/* Pairs with smp_store_release in mlx5e_open_qos_sq. */
-	return NUM_QOS_SQ_STATS * smp_load_acquire(&priv->htb.max_qos_sqs);
+	return NUM_QOS_SQ_STATS * smp_load_acquire(&priv->htb_max_qos_sqs);
 }
 
 static MLX5E_DECLARE_STATS_GRP_OP_FILL_STRS(qos)
 {
 	/* Pairs with smp_store_release in mlx5e_open_qos_sq. */
-	u16 max_qos_sqs = smp_load_acquire(&priv->htb.max_qos_sqs);
+	u16 max_qos_sqs = smp_load_acquire(&priv->htb_max_qos_sqs);
 	int i, qid;
 
 	for (qid = 0; qid < max_qos_sqs; qid++)
@@ -2208,8 +2208,8 @@ static MLX5E_DECLARE_STATS_GRP_OP_FILL_STATS(qos)
 	int i, qid;
 
 	/* Pairs with smp_store_release in mlx5e_open_qos_sq. */
-	max_qos_sqs = smp_load_acquire(&priv->htb.max_qos_sqs);
-	stats = READ_ONCE(priv->htb.qos_sq_stats);
+	max_qos_sqs = smp_load_acquire(&priv->htb_max_qos_sqs);
+	stats = READ_ONCE(priv->htb_qos_sq_stats);
 
 	for (qid = 0; qid < max_qos_sqs; qid++) {
 		struct mlx5e_sq_stats *s = READ_ONCE(stats[qid]);
-- 
2.36.1

[net-next V2 07/13] net/mlx5e: HTB, hide and dynamically allocate mlx5e_htb structure

[Saeed Mahameed]

From: Saeed Mahameed <saeedm@nvidia.com>

Move structure mlx5e_htb from the main driver include file "en.h" to be
hidden in qos.c where the qos functionality is implemented, forward
declare it for the rest of the driver and allocate it dynamically upon
user demand only.

Signed-off-by: Saeed Mahameed <saeedm@nvidia.com>
Signed-off-by: Moshe Tal <moshet@nvidia.com>
Reviewed-by: Tariq Toukan <tariqt@nvidia.com>
Reviewed-by: Maxim Mikityanskiy <maximmi@nvidia.com>
---
 drivers/net/ethernet/mellanox/mlx5/core/en.h  |  8 +-
 .../net/ethernet/mellanox/mlx5/core/en/qos.c  | 87 ++++++++++++++-----
 .../net/ethernet/mellanox/mlx5/core/en_main.c |  7 +-
 3 files changed, 70 insertions(+), 32 deletions(-)

diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en.h b/drivers/net/ethernet/mellanox/mlx5/core/en.h
index d2ed27575097..b07228f69b91 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en.h
@@ -899,12 +899,8 @@ struct mlx5e_scratchpad {
 	cpumask_var_t cpumask;
 };
 
-struct mlx5e_htb {
-	DECLARE_HASHTABLE(qos_tc2node, order_base_2(MLX5E_QOS_MAX_LEAF_NODES));
-	DECLARE_BITMAP(qos_used_qids, MLX5E_QOS_MAX_LEAF_NODES);
-};
-
 struct mlx5e_trap;
+struct mlx5e_htb;
 
 struct mlx5e_priv {
 	/* priv data path fields - start */
@@ -975,7 +971,7 @@ struct mlx5e_priv {
 	struct mlx5e_hv_vhca_stats_agent stats_agent;
 #endif
 	struct mlx5e_scratchpad    scratchpad;
-	struct mlx5e_htb           htb;
+	struct mlx5e_htb          *htb;
 	struct mlx5e_mqprio_rl    *mqprio_rl;
 };
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c b/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c
index 6136cad397dd..3848f06ac516 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c
@@ -9,6 +9,11 @@
 
 #define BYTES_IN_MBIT 125000
 
+struct mlx5e_htb {
+	DECLARE_HASHTABLE(qos_tc2node, order_base_2(MLX5E_QOS_MAX_LEAF_NODES));
+	DECLARE_BITMAP(qos_used_qids, MLX5E_QOS_MAX_LEAF_NODES);
+};
+
 int mlx5e_qos_bytes_rate_check(struct mlx5_core_dev *mdev, u64 nbytes)
 {
 	if (nbytes < BYTES_IN_MBIT) {
@@ -31,8 +36,9 @@ int mlx5e_qos_max_leaf_nodes(struct mlx5_core_dev *mdev)
 
 int mlx5e_qos_cur_leaf_nodes(struct mlx5e_priv *priv)
 {
-	int last = find_last_bit(priv->htb.qos_used_qids, mlx5e_qos_max_leaf_nodes(priv->mdev));
+	int last;
 
+	last = find_last_bit(priv->htb->qos_used_qids, mlx5e_qos_max_leaf_nodes(priv->mdev));
 	return last == mlx5e_qos_max_leaf_nodes(priv->mdev) ? 0 : last + 1;
 }
 
@@ -44,7 +50,7 @@ static int mlx5e_find_unused_qos_qid(struct mlx5e_priv *priv)
 	int res;
 
 	WARN_ONCE(!mutex_is_locked(&priv->state_lock), "%s: state_lock is not held\n", __func__);
-	res = find_first_zero_bit(priv->htb.qos_used_qids, size);
+	res = find_first_zero_bit(priv->htb->qos_used_qids, size);
 
 	return res == size ? -ENOSPC : res;
 }
@@ -76,10 +82,10 @@ mlx5e_sw_node_create_leaf(struct mlx5e_priv *priv, u16 classid, u16 qid,
 	node->parent = parent;
 
 	node->qid = qid;
-	__set_bit(qid, priv->htb.qos_used_qids);
+	__set_bit(qid, priv->htb->qos_used_qids);
 
 	node->classid = classid;
-	hash_add_rcu(priv->htb.qos_tc2node, &node->hnode, classid);
+	hash_add_rcu(priv->htb->qos_tc2node, &node->hnode, classid);
 
 	mlx5e_update_tx_netdev_queues(priv);
 
@@ -96,7 +102,7 @@ static struct mlx5e_qos_node *mlx5e_sw_node_create_root(struct mlx5e_priv *priv)
 
 	node->qid = MLX5E_QOS_QID_INNER;
 	node->classid = MLX5E_HTB_CLASSID_ROOT;
-	hash_add_rcu(priv->htb.qos_tc2node, &node->hnode, node->classid);
+	hash_add_rcu(priv->htb->qos_tc2node, &node->hnode, node->classid);
 
 	return node;
 }
@@ -105,7 +111,7 @@ static struct mlx5e_qos_node *mlx5e_sw_node_find(struct mlx5e_priv *priv, u32 cl
 {
 	struct mlx5e_qos_node *node = NULL;
 
-	hash_for_each_possible(priv->htb.qos_tc2node, node, hnode, classid) {
+	hash_for_each_possible(priv->htb->qos_tc2node, node, hnode, classid) {
 		if (node->classid == classid)
 			break;
 	}
@@ -117,7 +123,7 @@ static struct mlx5e_qos_node *mlx5e_sw_node_find_rcu(struct mlx5e_priv *priv, u3
 {
 	struct mlx5e_qos_node *node = NULL;
 
-	hash_for_each_possible_rcu(priv->htb.qos_tc2node, node, hnode, classid) {
+	hash_for_each_possible_rcu(priv->htb->qos_tc2node, node, hnode, classid) {
 		if (node->classid == classid)
 			break;
 	}
@@ -129,7 +135,7 @@ static void mlx5e_sw_node_delete(struct mlx5e_priv *priv, struct mlx5e_qos_node
 {
 	hash_del_rcu(&node->hnode);
 	if (node->qid != MLX5E_QOS_QID_INNER) {
-		__clear_bit(node->qid, priv->htb.qos_used_qids);
+		__clear_bit(node->qid, priv->htb->qos_used_qids);
 		mlx5e_update_tx_netdev_queues(priv);
 	}
 	/* Make sure this qid is no longer selected by mlx5e_select_queue, so
@@ -424,7 +430,7 @@ int mlx5e_qos_open_queues(struct mlx5e_priv *priv, struct mlx5e_channels *chs)
 	if (err)
 		return err;
 
-	hash_for_each(priv->htb.qos_tc2node, bkt, node, hnode) {
+	hash_for_each(priv->htb->qos_tc2node, bkt, node, hnode) {
 		if (node->qid == MLX5E_QOS_QID_INNER)
 			continue;
 		err = mlx5e_open_qos_sq(priv, chs, node);
@@ -442,7 +448,7 @@ void mlx5e_qos_activate_queues(struct mlx5e_priv *priv)
 	struct mlx5e_qos_node *node = NULL;
 	int bkt;
 
-	hash_for_each(priv->htb.qos_tc2node, bkt, node, hnode) {
+	hash_for_each(priv->htb->qos_tc2node, bkt, node, hnode) {
 		if (node->qid == MLX5E_QOS_QID_INNER)
 			continue;
 		mlx5e_activate_qos_sq(priv, node);
@@ -495,12 +501,6 @@ mlx5e_htb_root_add(struct mlx5e_priv *priv, u16 htb_maj_id, u16 htb_defcls,
 
 	qos_dbg(priv->mdev, "TC_HTB_CREATE handle %04x:, default :%04x\n", htb_maj_id, htb_defcls);
 
-	if (!mlx5_qos_is_supported(priv->mdev)) {
-		NL_SET_ERR_MSG_MOD(extack,
-				   "Missing QoS capabilities. Try disabling SRIOV or use a supported device.");
-		return -EOPNOTSUPP;
-	}
-
 	mlx5e_selq_prepare_htb(&priv->selq, htb_maj_id, htb_defcls);
 
 	opened = test_bit(MLX5E_STATE_OPENED, &priv->state);
@@ -749,7 +749,7 @@ static struct mlx5e_qos_node *mlx5e_sw_node_find_by_qid(struct mlx5e_priv *priv,
 	struct mlx5e_qos_node *node = NULL;
 	int bkt;
 
-	hash_for_each(priv->htb.qos_tc2node, bkt, node, hnode)
+	hash_for_each(priv->htb->qos_tc2node, bkt, node, hnode)
 		if (node->qid == qid)
 			break;
 
@@ -837,7 +837,7 @@ static int mlx5e_htb_leaf_del(struct mlx5e_priv *priv, u16 *classid,
 
 	/* Stop traffic to the old queue. */
 	WRITE_ONCE(node->qid, MLX5E_QOS_QID_INNER);
-	__clear_bit(moved_qid, priv->htb.qos_used_qids);
+	__clear_bit(moved_qid, priv->htb->qos_used_qids);
 
 	if (opened) {
 		txq = netdev_get_tx_queue(priv->netdev,
@@ -849,7 +849,7 @@ static int mlx5e_htb_leaf_del(struct mlx5e_priv *priv, u16 *classid,
 	/* Prevent packets from the old class from getting into the new one. */
 	mlx5e_reset_qdisc(priv->netdev, moved_qid);
 
-	__set_bit(qid, priv->htb.qos_used_qids);
+	__set_bit(qid, priv->htb->qos_used_qids);
 	WRITE_ONCE(node->qid, qid);
 
 	if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
@@ -960,7 +960,7 @@ mlx5e_qos_update_children(struct mlx5e_priv *priv, struct mlx5e_qos_node *node,
 	int err = 0;
 	int bkt;
 
-	hash_for_each(priv->htb.qos_tc2node, bkt, child, hnode) {
+	hash_for_each(priv->htb->qos_tc2node, bkt, child, hnode) {
 		u32 old_bw_share = child->bw_share;
 		int err_one;
 
@@ -1027,16 +1027,57 @@ mlx5e_htb_node_modify(struct mlx5e_priv *priv, u16 classid, u64 rate, u64 ceil,
 }
 
 /* HTB API */
+
+static struct mlx5e_htb *mlx5e_htb_alloc(void)
+{
+	return kvzalloc(sizeof(struct mlx5e_htb), GFP_KERNEL);
+}
+
+static void mlx5e_htb_free(struct mlx5e_htb *htb)
+{
+	kvfree(htb);
+}
+
+static int mlx5e_htb_init(struct mlx5e_priv *priv, struct tc_htb_qopt_offload *htb)
+{
+	hash_init(priv->htb->qos_tc2node);
+
+	return mlx5e_htb_root_add(priv, htb->parent_classid, htb->classid, htb->extack);
+}
+
+static void mlx5e_htb_cleanup(struct mlx5e_priv *priv)
+{
+	mlx5e_htb_root_del(priv);
+}
+
 int mlx5e_htb_setup_tc(struct mlx5e_priv *priv, struct tc_htb_qopt_offload *htb)
 {
 	int res;
 
+	if (!priv->htb && htb->command != TC_HTB_CREATE)
+		return -EINVAL;
+
 	switch (htb->command) {
 	case TC_HTB_CREATE:
-		return mlx5e_htb_root_add(priv, htb->parent_classid, htb->classid,
-					  htb->extack);
+		if (!mlx5_qos_is_supported(priv->mdev)) {
+			NL_SET_ERR_MSG_MOD(htb->extack,
+					   "Missing QoS capabilities. Try disabling SRIOV or use a supported device.");
+			return -EOPNOTSUPP;
+		}
+		priv->htb = mlx5e_htb_alloc();
+		if (!priv->htb)
+			return -ENOMEM;
+		res = mlx5e_htb_init(priv, htb);
+		if (res) {
+			mlx5e_htb_free(priv->htb);
+			priv->htb = NULL;
+		}
+		return res;
 	case TC_HTB_DESTROY:
-		return mlx5e_htb_root_del(priv);
+		mlx5e_htb_cleanup(priv);
+		mlx5e_htb_free(priv->htb);
+		priv->htb = NULL;
+		return 0;
 	case TC_HTB_LEAF_ALLOC_QUEUE:
 		res = mlx5e_htb_leaf_alloc_queue(priv, htb->classid, htb->parent_classid,
 						 htb->rate, htb->ceil, htb->extack);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_main.c b/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
index fed24b5a0bae..127aaa1c1d19 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
@@ -2572,9 +2572,11 @@ static int mlx5e_netdev_set_tcs(struct net_device *netdev, u16 nch, u8 ntc,
 
 int mlx5e_update_tx_netdev_queues(struct mlx5e_priv *priv)
 {
-	int qos_queues, nch, ntc, num_txqs, err;
+	int nch, ntc, num_txqs, err;
+	int qos_queues = 0;
 
-	qos_queues = mlx5e_qos_cur_leaf_nodes(priv);
+	if (priv->htb)
+		qos_queues = mlx5e_qos_cur_leaf_nodes(priv);
 
 	nch = priv->channels.params.num_channels;
 	ntc = mlx5e_get_dcb_num_tc(&priv->channels.params);
@@ -5315,7 +5317,6 @@ int mlx5e_priv_init(struct mlx5e_priv *priv,
 	if (err)
 		goto err_free_cpumask;
 
-	hash_init(priv->htb.qos_tc2node);
 	INIT_WORK(&priv->update_carrier_work, mlx5e_update_carrier_work);
 	INIT_WORK(&priv->set_rx_mode_work, mlx5e_set_rx_mode_work);
 	INIT_WORK(&priv->tx_timeout_work, mlx5e_tx_timeout_work);
-- 
2.36.1

[net-next V2 08/13] net/mlx5e: HTB, remove priv from htb function calls

[Saeed Mahameed]

From: Moshe Tal <moshet@nvidia.com>

As a step to make htb self-contained replace the passing of priv as a
parameter to htb function calls with members in the htb struct.

Full decoupling the htb from priv will require more work, so for now
leave the priv as one of the members in the htb struct, to be replaced
by channels in a future commit.

Signed-off-by: Moshe Tal <moshet@nvidia.com>
Reviewed-by: Tariq Toukan <tariqt@nvidia.com>
Reviewed-by: Maxim Mikityanskiy <maximmi@nvidia.com>
Signed-off-by: Saeed Mahameed <saeedm@nvidia.com>
---
 .../net/ethernet/mellanox/mlx5/core/en/qos.c  | 302 ++++++++++--------
 .../net/ethernet/mellanox/mlx5/core/en/qos.h  |   5 +-
 .../net/ethernet/mellanox/mlx5/core/en/selq.c |   2 +-
 .../net/ethernet/mellanox/mlx5/core/en_main.c |  13 +-
 4 files changed, 172 insertions(+), 150 deletions(-)

diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c b/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c
index 3848f06ac516..3eb4a741d75b 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c
@@ -12,6 +12,10 @@
 struct mlx5e_htb {
 	DECLARE_HASHTABLE(qos_tc2node, order_base_2(MLX5E_QOS_MAX_LEAF_NODES));
 	DECLARE_BITMAP(qos_used_qids, MLX5E_QOS_MAX_LEAF_NODES);
+	struct mlx5_core_dev *mdev;
+	struct net_device *netdev;
+	struct mlx5e_priv *priv;
+	struct mlx5e_selq *selq;
 };
 
 int mlx5e_qos_bytes_rate_check(struct mlx5_core_dev *mdev, u64 nbytes)
@@ -34,23 +38,24 @@ int mlx5e_qos_max_leaf_nodes(struct mlx5_core_dev *mdev)
 	return min(MLX5E_QOS_MAX_LEAF_NODES, mlx5_qos_max_leaf_nodes(mdev));
 }
 
-int mlx5e_qos_cur_leaf_nodes(struct mlx5e_priv *priv)
+int mlx5e_qos_cur_leaf_nodes(struct mlx5e_htb *htb)
 {
 	int last;
 
-	last = find_last_bit(priv->htb->qos_used_qids, mlx5e_qos_max_leaf_nodes(priv->mdev));
-	return last == mlx5e_qos_max_leaf_nodes(priv->mdev) ? 0 : last + 1;
+	last = find_last_bit(htb->qos_used_qids, mlx5e_qos_max_leaf_nodes(htb->mdev));
+	return last == mlx5e_qos_max_leaf_nodes(htb->mdev) ? 0 : last + 1;
 }
 
 /* Software representation of the QoS tree (internal to this file) */
 
-static int mlx5e_find_unused_qos_qid(struct mlx5e_priv *priv)
+static int mlx5e_find_unused_qos_qid(struct mlx5e_htb *htb)
 {
-	int size = mlx5e_qos_max_leaf_nodes(priv->mdev);
+	int size = mlx5e_qos_max_leaf_nodes(htb->mdev);
+	struct mlx5e_priv *priv = htb->priv;
 	int res;
 
 	WARN_ONCE(!mutex_is_locked(&priv->state_lock), "%s: state_lock is not held\n", __func__);
-	res = find_first_zero_bit(priv->htb->qos_used_qids, size);
+	res = find_first_zero_bit(htb->qos_used_qids, size);
 
 	return res == size ? -ENOSPC : res;
 }
@@ -70,7 +75,7 @@ struct mlx5e_qos_node {
 #define MLX5E_HTB_CLASSID_ROOT 0xffffffff
 
 static struct mlx5e_qos_node *
-mlx5e_sw_node_create_leaf(struct mlx5e_priv *priv, u16 classid, u16 qid,
+mlx5e_sw_node_create_leaf(struct mlx5e_htb *htb, u16 classid, u16 qid,
 			  struct mlx5e_qos_node *parent)
 {
 	struct mlx5e_qos_node *node;
@@ -82,17 +87,17 @@ mlx5e_sw_node_create_leaf(struct mlx5e_priv *priv, u16 classid, u16 qid,
 	node->parent = parent;
 
 	node->qid = qid;
-	__set_bit(qid, priv->htb->qos_used_qids);
+	__set_bit(qid, htb->qos_used_qids);
 
 	node->classid = classid;
-	hash_add_rcu(priv->htb->qos_tc2node, &node->hnode, classid);
+	hash_add_rcu(htb->qos_tc2node, &node->hnode, classid);
 
-	mlx5e_update_tx_netdev_queues(priv);
+	mlx5e_update_tx_netdev_queues(htb->priv);
 
 	return node;
 }
 
-static struct mlx5e_qos_node *mlx5e_sw_node_create_root(struct mlx5e_priv *priv)
+static struct mlx5e_qos_node *mlx5e_sw_node_create_root(struct mlx5e_htb *htb)
 {
 	struct mlx5e_qos_node *node;
 
@@ -102,16 +107,16 @@ static struct mlx5e_qos_node *mlx5e_sw_node_create_root(struct mlx5e_priv *priv)
 
 	node->qid = MLX5E_QOS_QID_INNER;
 	node->classid = MLX5E_HTB_CLASSID_ROOT;
-	hash_add_rcu(priv->htb->qos_tc2node, &node->hnode, node->classid);
+	hash_add_rcu(htb->qos_tc2node, &node->hnode, node->classid);
 
 	return node;
 }
 
-static struct mlx5e_qos_node *mlx5e_sw_node_find(struct mlx5e_priv *priv, u32 classid)
+static struct mlx5e_qos_node *mlx5e_sw_node_find(struct mlx5e_htb *htb, u32 classid)
 {
 	struct mlx5e_qos_node *node = NULL;
 
-	hash_for_each_possible(priv->htb->qos_tc2node, node, hnode, classid) {
+	hash_for_each_possible(htb->qos_tc2node, node, hnode, classid) {
 		if (node->classid == classid)
 			break;
 	}
@@ -119,11 +124,11 @@ static struct mlx5e_qos_node *mlx5e_sw_node_find(struct mlx5e_priv *priv, u32 cl
 	return node;
 }
 
-static struct mlx5e_qos_node *mlx5e_sw_node_find_rcu(struct mlx5e_priv *priv, u32 classid)
+static struct mlx5e_qos_node *mlx5e_sw_node_find_rcu(struct mlx5e_htb *htb, u32 classid)
 {
 	struct mlx5e_qos_node *node = NULL;
 
-	hash_for_each_possible_rcu(priv->htb->qos_tc2node, node, hnode, classid) {
+	hash_for_each_possible_rcu(htb->qos_tc2node, node, hnode, classid) {
 		if (node->classid == classid)
 			break;
 	}
@@ -131,12 +136,12 @@ static struct mlx5e_qos_node *mlx5e_sw_node_find_rcu(struct mlx5e_priv *priv, u3
 	return node;
 }
 
-static void mlx5e_sw_node_delete(struct mlx5e_priv *priv, struct mlx5e_qos_node *node)
+static void mlx5e_sw_node_delete(struct mlx5e_htb *htb, struct mlx5e_qos_node *node)
 {
 	hash_del_rcu(&node->hnode);
 	if (node->qid != MLX5E_QOS_QID_INNER) {
-		__clear_bit(node->qid, priv->htb->qos_used_qids);
-		mlx5e_update_tx_netdev_queues(priv);
+		__clear_bit(node->qid, htb->qos_used_qids);
+		mlx5e_update_tx_netdev_queues(htb->priv);
 	}
 	/* Make sure this qid is no longer selected by mlx5e_select_queue, so
 	 * that mlx5e_reactivate_qos_sq can safely restart the netdev TX queue.
@@ -161,7 +166,7 @@ static u16 mlx5e_qid_from_qos(struct mlx5e_channels *chs, u16 qid)
 	return (chs->params.num_channels + is_ptp) * mlx5e_get_dcb_num_tc(&chs->params) + qid;
 }
 
-int mlx5e_get_txq_by_classid(struct mlx5e_priv *priv, u16 classid)
+int mlx5e_get_txq_by_classid(struct mlx5e_htb *htb, u16 classid)
 {
 	struct mlx5e_qos_node *node;
 	u16 qid;
@@ -169,7 +174,7 @@ int mlx5e_get_txq_by_classid(struct mlx5e_priv *priv, u16 classid)
 
 	rcu_read_lock();
 
-	node = mlx5e_sw_node_find_rcu(priv, classid);
+	node = mlx5e_sw_node_find_rcu(htb, classid);
 	if (!node) {
 		res = -ENOENT;
 		goto out;
@@ -179,7 +184,7 @@ int mlx5e_get_txq_by_classid(struct mlx5e_priv *priv, u16 classid)
 		res = -EINVAL;
 		goto out;
 	}
-	res = mlx5e_qid_from_qos(&priv->channels, qid);
+	res = mlx5e_qid_from_qos(&htb->priv->channels, qid);
 
 out:
 	rcu_read_unlock();
@@ -423,9 +428,6 @@ int mlx5e_qos_open_queues(struct mlx5e_priv *priv, struct mlx5e_channels *chs)
 	struct mlx5e_qos_node *node = NULL;
 	int bkt, err;
 
-	if (!mlx5e_selq_is_htb_enabled(&priv->selq))
-		return 0;
-
 	err = mlx5e_qos_alloc_queues(priv, chs);
 	if (err)
 		return err;
@@ -492,16 +494,17 @@ static void mlx5e_qos_deactivate_all_queues(struct mlx5e_channels *chs)
 /* HTB TC handlers */
 
 static int
-mlx5e_htb_root_add(struct mlx5e_priv *priv, u16 htb_maj_id, u16 htb_defcls,
+mlx5e_htb_root_add(struct mlx5e_htb *htb, u16 htb_maj_id, u16 htb_defcls,
 		   struct netlink_ext_ack *extack)
 {
+	struct mlx5e_priv *priv = htb->priv;
 	struct mlx5e_qos_node *root;
 	bool opened;
 	int err;
 
-	qos_dbg(priv->mdev, "TC_HTB_CREATE handle %04x:, default :%04x\n", htb_maj_id, htb_defcls);
+	qos_dbg(htb->mdev, "TC_HTB_CREATE handle %04x:, default :%04x\n", htb_maj_id, htb_defcls);
 
-	mlx5e_selq_prepare_htb(&priv->selq, htb_maj_id, htb_defcls);
+	mlx5e_selq_prepare_htb(htb->selq, htb_maj_id, htb_defcls);
 
 	opened = test_bit(MLX5E_STATE_OPENED, &priv->state);
 	if (opened) {
@@ -510,58 +513,59 @@ mlx5e_htb_root_add(struct mlx5e_priv *priv, u16 htb_maj_id, u16 htb_defcls,
 			goto err_cancel_selq;
 	}
 
-	root = mlx5e_sw_node_create_root(priv);
+	root = mlx5e_sw_node_create_root(htb);
 	if (IS_ERR(root)) {
 		err = PTR_ERR(root);
 		goto err_free_queues;
 	}
 
-	err = mlx5_qos_create_root_node(priv->mdev, &root->hw_id);
+	err = mlx5_qos_create_root_node(htb->mdev, &root->hw_id);
 	if (err) {
 		NL_SET_ERR_MSG_MOD(extack, "Firmware error. Try upgrading firmware.");
 		goto err_sw_node_delete;
 	}
 
-	mlx5e_selq_apply(&priv->selq);
+	mlx5e_selq_apply(htb->selq);
 
 	return 0;
 
 err_sw_node_delete:
-	mlx5e_sw_node_delete(priv, root);
+	mlx5e_sw_node_delete(htb, root);
 
 err_free_queues:
 	if (opened)
 		mlx5e_qos_close_all_queues(&priv->channels);
 err_cancel_selq:
-	mlx5e_selq_cancel(&priv->selq);
+	mlx5e_selq_cancel(htb->selq);
 	return err;
 }
 
-static int mlx5e_htb_root_del(struct mlx5e_priv *priv)
+static int mlx5e_htb_root_del(struct mlx5e_htb *htb)
 {
+	struct mlx5e_priv *priv = htb->priv;
 	struct mlx5e_qos_node *root;
 	int err;
 
-	qos_dbg(priv->mdev, "TC_HTB_DESTROY\n");
+	qos_dbg(htb->mdev, "TC_HTB_DESTROY\n");
 
 	/* Wait until real_num_tx_queues is updated for mlx5e_select_queue,
 	 * so that we can safely switch to its non-HTB non-PTP fastpath.
 	 */
 	synchronize_net();
 
-	mlx5e_selq_prepare_htb(&priv->selq, 0, 0);
-	mlx5e_selq_apply(&priv->selq);
+	mlx5e_selq_prepare_htb(htb->selq, 0, 0);
+	mlx5e_selq_apply(htb->selq);
 
-	root = mlx5e_sw_node_find(priv, MLX5E_HTB_CLASSID_ROOT);
+	root = mlx5e_sw_node_find(htb, MLX5E_HTB_CLASSID_ROOT);
 	if (!root) {
-		qos_err(priv->mdev, "Failed to find the root node in the QoS tree\n");
+		qos_err(htb->mdev, "Failed to find the root node in the QoS tree\n");
 		return -ENOENT;
 	}
-	err = mlx5_qos_destroy_node(priv->mdev, root->hw_id);
+	err = mlx5_qos_destroy_node(htb->mdev, root->hw_id);
 	if (err)
-		qos_err(priv->mdev, "Failed to destroy root node %u, err = %d\n",
+		qos_err(htb->mdev, "Failed to destroy root node %u, err = %d\n",
 			root->hw_id, err);
-	mlx5e_sw_node_delete(priv, root);
+	mlx5e_sw_node_delete(htb, root);
 
 	mlx5e_qos_deactivate_all_queues(&priv->channels);
 	mlx5e_qos_close_all_queues(&priv->channels);
@@ -569,7 +573,7 @@ static int mlx5e_htb_root_del(struct mlx5e_priv *priv)
 	return err;
 }
 
-static int mlx5e_htb_convert_rate(struct mlx5e_priv *priv, u64 rate,
+static int mlx5e_htb_convert_rate(struct mlx5e_htb *htb, u64 rate,
 				  struct mlx5e_qos_node *parent, u32 *bw_share)
 {
 	u64 share = 0;
@@ -585,59 +589,60 @@ static int mlx5e_htb_convert_rate(struct mlx5e_priv *priv, u64 rate,
 
 	*bw_share = share == 0 ? 1 : share > 100 ? 0 : share;
 
-	qos_dbg(priv->mdev, "Convert: rate %llu, parent ceil %llu -> bw_share %u\n",
+	qos_dbg(htb->mdev, "Convert: rate %llu, parent ceil %llu -> bw_share %u\n",
 		rate, (u64)parent->max_average_bw * BYTES_IN_MBIT, *bw_share);
 
 	return 0;
 }
 
-static void mlx5e_htb_convert_ceil(struct mlx5e_priv *priv, u64 ceil, u32 *max_average_bw)
+static void mlx5e_htb_convert_ceil(struct mlx5e_htb *htb, u64 ceil, u32 *max_average_bw)
 {
 	/* Hardware treats 0 as "unlimited", set at least 1. */
 	*max_average_bw = max_t(u32, div_u64(ceil, BYTES_IN_MBIT), 1);
 
-	qos_dbg(priv->mdev, "Convert: ceil %llu -> max_average_bw %u\n",
+	qos_dbg(htb->mdev, "Convert: ceil %llu -> max_average_bw %u\n",
 		ceil, *max_average_bw);
 }
 
 static int
-mlx5e_htb_leaf_alloc_queue(struct mlx5e_priv *priv, u16 classid,
+mlx5e_htb_leaf_alloc_queue(struct mlx5e_htb *htb, u16 classid,
 			   u32 parent_classid, u64 rate, u64 ceil,
 			   struct netlink_ext_ack *extack)
 {
 	struct mlx5e_qos_node *node, *parent;
+	struct mlx5e_priv *priv = htb->priv;
 	int qid;
 	int err;
 
-	qos_dbg(priv->mdev, "TC_HTB_LEAF_ALLOC_QUEUE classid %04x, parent %04x, rate %llu, ceil %llu\n",
+	qos_dbg(htb->mdev, "TC_HTB_LEAF_ALLOC_QUEUE classid %04x, parent %04x, rate %llu, ceil %llu\n",
 		classid, parent_classid, rate, ceil);
 
-	qid = mlx5e_find_unused_qos_qid(priv);
+	qid = mlx5e_find_unused_qos_qid(htb);
 	if (qid < 0) {
 		NL_SET_ERR_MSG_MOD(extack, "Maximum amount of leaf classes is reached.");
 		return qid;
 	}
 
-	parent = mlx5e_sw_node_find(priv, parent_classid);
+	parent = mlx5e_sw_node_find(htb, parent_classid);
 	if (!parent)
 		return -EINVAL;
 
-	node = mlx5e_sw_node_create_leaf(priv, classid, qid, parent);
+	node = mlx5e_sw_node_create_leaf(htb, classid, qid, parent);
 	if (IS_ERR(node))
 		return PTR_ERR(node);
 
 	node->rate = rate;
-	mlx5e_htb_convert_rate(priv, rate, node->parent, &node->bw_share);
-	mlx5e_htb_convert_ceil(priv, ceil, &node->max_average_bw);
+	mlx5e_htb_convert_rate(htb, rate, node->parent, &node->bw_share);
+	mlx5e_htb_convert_ceil(htb, ceil, &node->max_average_bw);
 
-	err = mlx5_qos_create_leaf_node(priv->mdev, node->parent->hw_id,
+	err = mlx5_qos_create_leaf_node(htb->mdev, node->parent->hw_id,
 					node->bw_share, node->max_average_bw,
 					&node->hw_id);
 	if (err) {
 		NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating a leaf node.");
-		qos_err(priv->mdev, "Failed to create a leaf node (class %04x), err = %d\n",
+		qos_err(htb->mdev, "Failed to create a leaf node (class %04x), err = %d\n",
 			classid, err);
-		mlx5e_sw_node_delete(priv, node);
+		mlx5e_sw_node_delete(htb, node);
 		return err;
 	}
 
@@ -645,7 +650,7 @@ mlx5e_htb_leaf_alloc_queue(struct mlx5e_priv *priv, u16 classid,
 		err = mlx5e_open_qos_sq(priv, &priv->channels, node);
 		if (err) {
 			NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
-			qos_warn(priv->mdev, "Failed to create a QoS SQ (class %04x), err = %d\n",
+			qos_warn(htb->mdev, "Failed to create a QoS SQ (class %04x), err = %d\n",
 				 classid, err);
 		} else {
 			mlx5e_activate_qos_sq(priv, node);
@@ -656,47 +661,48 @@ mlx5e_htb_leaf_alloc_queue(struct mlx5e_priv *priv, u16 classid,
 }
 
 static int
-mlx5e_htb_leaf_to_inner(struct mlx5e_priv *priv, u16 classid, u16 child_classid,
+mlx5e_htb_leaf_to_inner(struct mlx5e_htb *htb, u16 classid, u16 child_classid,
 			u64 rate, u64 ceil, struct netlink_ext_ack *extack)
 {
 	struct mlx5e_qos_node *node, *child;
+	struct mlx5e_priv *priv = htb->priv;
 	int err, tmp_err;
 	u32 new_hw_id;
 	u16 qid;
 
-	qos_dbg(priv->mdev, "TC_HTB_LEAF_TO_INNER classid %04x, upcoming child %04x, rate %llu, ceil %llu\n",
+	qos_dbg(htb->mdev, "TC_HTB_LEAF_TO_INNER classid %04x, upcoming child %04x, rate %llu, ceil %llu\n",
 		classid, child_classid, rate, ceil);
 
-	node = mlx5e_sw_node_find(priv, classid);
+	node = mlx5e_sw_node_find(htb, classid);
 	if (!node)
 		return -ENOENT;
 
-	err = mlx5_qos_create_inner_node(priv->mdev, node->parent->hw_id,
+	err = mlx5_qos_create_inner_node(htb->mdev, node->parent->hw_id,
 					 node->bw_share, node->max_average_bw,
 					 &new_hw_id);
 	if (err) {
 		NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating an inner node.");
-		qos_err(priv->mdev, "Failed to create an inner node (class %04x), err = %d\n",
+		qos_err(htb->mdev, "Failed to create an inner node (class %04x), err = %d\n",
 			classid, err);
 		return err;
 	}
 
 	/* Intentionally reuse the qid for the upcoming first child. */
-	child = mlx5e_sw_node_create_leaf(priv, child_classid, node->qid, node);
+	child = mlx5e_sw_node_create_leaf(htb, child_classid, node->qid, node);
 	if (IS_ERR(child)) {
 		err = PTR_ERR(child);
 		goto err_destroy_hw_node;
 	}
 
 	child->rate = rate;
-	mlx5e_htb_convert_rate(priv, rate, node, &child->bw_share);
-	mlx5e_htb_convert_ceil(priv, ceil, &child->max_average_bw);
+	mlx5e_htb_convert_rate(htb, rate, node, &child->bw_share);
+	mlx5e_htb_convert_ceil(htb, ceil, &child->max_average_bw);
 
-	err = mlx5_qos_create_leaf_node(priv->mdev, new_hw_id, child->bw_share,
+	err = mlx5_qos_create_leaf_node(htb->mdev, new_hw_id, child->bw_share,
 					child->max_average_bw, &child->hw_id);
 	if (err) {
 		NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating a leaf node.");
-		qos_err(priv->mdev, "Failed to create a leaf node (class %04x), err = %d\n",
+		qos_err(htb->mdev, "Failed to create a leaf node (class %04x), err = %d\n",
 			classid, err);
 		goto err_delete_sw_node;
 	}
@@ -712,9 +718,9 @@ mlx5e_htb_leaf_to_inner(struct mlx5e_priv *priv, u16 classid, u16 child_classid,
 		mlx5e_close_qos_sq(priv, qid);
 	}
 
-	err = mlx5_qos_destroy_node(priv->mdev, node->hw_id);
+	err = mlx5_qos_destroy_node(htb->mdev, node->hw_id);
 	if (err) /* Not fatal. */
-		qos_warn(priv->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
+		qos_warn(htb->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
 			 node->hw_id, classid, err);
 
 	node->hw_id = new_hw_id;
@@ -723,7 +729,7 @@ mlx5e_htb_leaf_to_inner(struct mlx5e_priv *priv, u16 classid, u16 child_classid,
 		err = mlx5e_open_qos_sq(priv, &priv->channels, child);
 		if (err) {
 			NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
-			qos_warn(priv->mdev, "Failed to create a QoS SQ (class %04x), err = %d\n",
+			qos_warn(htb->mdev, "Failed to create a QoS SQ (class %04x), err = %d\n",
 				 classid, err);
 		} else {
 			mlx5e_activate_qos_sq(priv, child);
@@ -734,22 +740,22 @@ mlx5e_htb_leaf_to_inner(struct mlx5e_priv *priv, u16 classid, u16 child_classid,
 
 err_delete_sw_node:
 	child->qid = MLX5E_QOS_QID_INNER;
-	mlx5e_sw_node_delete(priv, child);
+	mlx5e_sw_node_delete(htb, child);
 
 err_destroy_hw_node:
-	tmp_err = mlx5_qos_destroy_node(priv->mdev, new_hw_id);
+	tmp_err = mlx5_qos_destroy_node(htb->mdev, new_hw_id);
 	if (tmp_err) /* Not fatal. */
-		qos_warn(priv->mdev, "Failed to roll back creation of an inner node %u (class %04x), err = %d\n",
+		qos_warn(htb->mdev, "Failed to roll back creation of an inner node %u (class %04x), err = %d\n",
 			 new_hw_id, classid, tmp_err);
 	return err;
 }
 
-static struct mlx5e_qos_node *mlx5e_sw_node_find_by_qid(struct mlx5e_priv *priv, u16 qid)
+static struct mlx5e_qos_node *mlx5e_sw_node_find_by_qid(struct mlx5e_htb *htb, u16 qid)
 {
 	struct mlx5e_qos_node *node = NULL;
 	int bkt;
 
-	hash_for_each(priv->htb->qos_tc2node, bkt, node, hnode)
+	hash_for_each(htb->qos_tc2node, bkt, node, hnode)
 		if (node->qid == qid)
 			break;
 
@@ -776,18 +782,19 @@ static void mlx5e_reset_qdisc(struct net_device *dev, u16 qid)
 	spin_unlock_bh(qdisc_lock(qdisc));
 }
 
-static int mlx5e_htb_leaf_del(struct mlx5e_priv *priv, u16 *classid,
+static int mlx5e_htb_leaf_del(struct mlx5e_htb *htb, u16 *classid,
 			      struct netlink_ext_ack *extack)
 {
+	struct mlx5e_priv *priv = htb->priv;
 	struct mlx5e_qos_node *node;
 	struct netdev_queue *txq;
 	u16 qid, moved_qid;
 	bool opened;
 	int err;
 
-	qos_dbg(priv->mdev, "TC_HTB_LEAF_DEL classid %04x\n", *classid);
+	qos_dbg(htb->mdev, "TC_HTB_LEAF_DEL classid %04x\n", *classid);
 
-	node = mlx5e_sw_node_find(priv, *classid);
+	node = mlx5e_sw_node_find(htb, *classid);
 	if (!node)
 		return -ENOENT;
 
@@ -796,20 +803,20 @@ static int mlx5e_htb_leaf_del(struct mlx5e_priv *priv, u16 *classid,
 
 	opened = test_bit(MLX5E_STATE_OPENED, &priv->state);
 	if (opened) {
-		txq = netdev_get_tx_queue(priv->netdev,
+		txq = netdev_get_tx_queue(htb->netdev,
 					  mlx5e_qid_from_qos(&priv->channels, qid));
 		mlx5e_deactivate_qos_sq(priv, qid);
 		mlx5e_close_qos_sq(priv, qid);
 	}
 
-	err = mlx5_qos_destroy_node(priv->mdev, node->hw_id);
+	err = mlx5_qos_destroy_node(htb->mdev, node->hw_id);
 	if (err) /* Not fatal. */
-		qos_warn(priv->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
+		qos_warn(htb->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
 			 node->hw_id, *classid, err);
 
-	mlx5e_sw_node_delete(priv, node);
+	mlx5e_sw_node_delete(htb, node);
 
-	moved_qid = mlx5e_qos_cur_leaf_nodes(priv);
+	moved_qid = mlx5e_qos_cur_leaf_nodes(htb);
 
 	if (moved_qid == 0) {
 		/* The last QoS SQ was just destroyed. */
@@ -829,9 +836,9 @@ static int mlx5e_htb_leaf_del(struct mlx5e_priv *priv, u16 *classid,
 	}
 
 	WARN(moved_qid == qid, "Can't move node with qid %u to itself", qid);
-	qos_dbg(priv->mdev, "Moving QoS SQ %u to %u\n", moved_qid, qid);
+	qos_dbg(htb->mdev, "Moving QoS SQ %u to %u\n", moved_qid, qid);
 
-	node = mlx5e_sw_node_find_by_qid(priv, moved_qid);
+	node = mlx5e_sw_node_find_by_qid(htb, moved_qid);
 	WARN(!node, "Could not find a node with qid %u to move to queue %u",
 	     moved_qid, qid);
 
@@ -840,23 +847,23 @@ static int mlx5e_htb_leaf_del(struct mlx5e_priv *priv, u16 *classid,
 	__clear_bit(moved_qid, priv->htb->qos_used_qids);
 
 	if (opened) {
-		txq = netdev_get_tx_queue(priv->netdev,
+		txq = netdev_get_tx_queue(htb->netdev,
 					  mlx5e_qid_from_qos(&priv->channels, moved_qid));
 		mlx5e_deactivate_qos_sq(priv, moved_qid);
 		mlx5e_close_qos_sq(priv, moved_qid);
 	}
 
 	/* Prevent packets from the old class from getting into the new one. */
-	mlx5e_reset_qdisc(priv->netdev, moved_qid);
+	mlx5e_reset_qdisc(htb->netdev, moved_qid);
 
-	__set_bit(qid, priv->htb->qos_used_qids);
+	__set_bit(qid, htb->qos_used_qids);
 	WRITE_ONCE(node->qid, qid);
 
 	if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
 		err = mlx5e_open_qos_sq(priv, &priv->channels, node);
 		if (err) {
 			NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
-			qos_warn(priv->mdev, "Failed to create a QoS SQ (class %04x) while moving qid %u to %u, err = %d\n",
+			qos_warn(htb->mdev, "Failed to create a QoS SQ (class %04x) while moving qid %u to %u, err = %d\n",
 				 node->classid, moved_qid, qid, err);
 		} else {
 			mlx5e_activate_qos_sq(priv, node);
@@ -872,28 +879,29 @@ static int mlx5e_htb_leaf_del(struct mlx5e_priv *priv, u16 *classid,
 }
 
 static int
-mlx5e_htb_leaf_del_last(struct mlx5e_priv *priv, u16 classid, bool force,
+mlx5e_htb_leaf_del_last(struct mlx5e_htb *htb, u16 classid, bool force,
 			struct netlink_ext_ack *extack)
 {
 	struct mlx5e_qos_node *node, *parent;
+	struct mlx5e_priv *priv = htb->priv;
 	u32 old_hw_id, new_hw_id;
 	int err, saved_err = 0;
 	u16 qid;
 
-	qos_dbg(priv->mdev, "TC_HTB_LEAF_DEL_LAST%s classid %04x\n",
+	qos_dbg(htb->mdev, "TC_HTB_LEAF_DEL_LAST%s classid %04x\n",
 		force ? "_FORCE" : "", classid);
 
-	node = mlx5e_sw_node_find(priv, classid);
+	node = mlx5e_sw_node_find(htb, classid);
 	if (!node)
 		return -ENOENT;
 
-	err = mlx5_qos_create_leaf_node(priv->mdev, node->parent->parent->hw_id,
+	err = mlx5_qos_create_leaf_node(htb->mdev, node->parent->parent->hw_id,
 					node->parent->bw_share,
 					node->parent->max_average_bw,
 					&new_hw_id);
 	if (err) {
 		NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating a leaf node.");
-		qos_err(priv->mdev, "Failed to create a leaf node (class %04x), err = %d\n",
+		qos_err(htb->mdev, "Failed to create a leaf node (class %04x), err = %d\n",
 			classid, err);
 		if (!force)
 			return err;
@@ -911,15 +919,15 @@ mlx5e_htb_leaf_del_last(struct mlx5e_priv *priv, u16 classid, bool force,
 	}
 
 	/* Prevent packets from the old class from getting into the new one. */
-	mlx5e_reset_qdisc(priv->netdev, qid);
+	mlx5e_reset_qdisc(htb->netdev, qid);
 
-	err = mlx5_qos_destroy_node(priv->mdev, node->hw_id);
+	err = mlx5_qos_destroy_node(htb->mdev, node->hw_id);
 	if (err) /* Not fatal. */
-		qos_warn(priv->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
+		qos_warn(htb->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
 			 node->hw_id, classid, err);
 
 	parent = node->parent;
-	mlx5e_sw_node_delete(priv, node);
+	mlx5e_sw_node_delete(htb, node);
 
 	node = parent;
 	WRITE_ONCE(node->qid, qid);
@@ -937,47 +945,47 @@ mlx5e_htb_leaf_del_last(struct mlx5e_priv *priv, u16 classid, bool force,
 		err = mlx5e_open_qos_sq(priv, &priv->channels, node);
 		if (err) {
 			NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
-			qos_warn(priv->mdev, "Failed to create a QoS SQ (class %04x), err = %d\n",
+			qos_warn(htb->mdev, "Failed to create a QoS SQ (class %04x), err = %d\n",
 				 classid, err);
 		} else {
 			mlx5e_activate_qos_sq(priv, node);
 		}
 	}
 
-	err = mlx5_qos_destroy_node(priv->mdev, old_hw_id);
+	err = mlx5_qos_destroy_node(htb->mdev, old_hw_id);
 	if (err) /* Not fatal. */
-		qos_warn(priv->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
+		qos_warn(htb->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
 			 node->hw_id, classid, err);
 
 	return 0;
 }
 
 static int
-mlx5e_qos_update_children(struct mlx5e_priv *priv, struct mlx5e_qos_node *node,
+mlx5e_qos_update_children(struct mlx5e_htb *htb, struct mlx5e_qos_node *node,
 			  struct netlink_ext_ack *extack)
 {
 	struct mlx5e_qos_node *child;
 	int err = 0;
 	int bkt;
 
-	hash_for_each(priv->htb->qos_tc2node, bkt, child, hnode) {
+	hash_for_each(htb->qos_tc2node, bkt, child, hnode) {
 		u32 old_bw_share = child->bw_share;
 		int err_one;
 
 		if (child->parent != node)
 			continue;
 
-		mlx5e_htb_convert_rate(priv, child->rate, node, &child->bw_share);
+		mlx5e_htb_convert_rate(htb, child->rate, node, &child->bw_share);
 		if (child->bw_share == old_bw_share)
 			continue;
 
-		err_one = mlx5_qos_update_node(priv->mdev, child->hw_id, child->bw_share,
+		err_one = mlx5_qos_update_node(htb->mdev, child->hw_id, child->bw_share,
 					       child->max_average_bw, child->hw_id);
 		if (!err && err_one) {
 			err = err_one;
 
 			NL_SET_ERR_MSG_MOD(extack, "Firmware error when modifying a child node.");
-			qos_err(priv->mdev, "Failed to modify a child node (class %04x), err = %d\n",
+			qos_err(htb->mdev, "Failed to modify a child node (class %04x), err = %d\n",
 				node->classid, err);
 		}
 	}
@@ -986,7 +994,7 @@ mlx5e_qos_update_children(struct mlx5e_priv *priv, struct mlx5e_qos_node *node,
 }
 
 static int
-mlx5e_htb_node_modify(struct mlx5e_priv *priv, u16 classid, u64 rate, u64 ceil,
+mlx5e_htb_node_modify(struct mlx5e_htb *htb, u16 classid, u64 rate, u64 ceil,
 		      struct netlink_ext_ack *extack)
 {
 	u32 bw_share, max_average_bw;
@@ -994,22 +1002,22 @@ mlx5e_htb_node_modify(struct mlx5e_priv *priv, u16 classid, u64 rate, u64 ceil,
 	bool ceil_changed = false;
 	int err;
 
-	qos_dbg(priv->mdev, "TC_HTB_LEAF_MODIFY classid %04x, rate %llu, ceil %llu\n",
+	qos_dbg(htb->mdev, "TC_HTB_LEAF_MODIFY classid %04x, rate %llu, ceil %llu\n",
 		classid, rate, ceil);
 
-	node = mlx5e_sw_node_find(priv, classid);
+	node = mlx5e_sw_node_find(htb, classid);
 	if (!node)
 		return -ENOENT;
 
 	node->rate = rate;
-	mlx5e_htb_convert_rate(priv, rate, node->parent, &bw_share);
-	mlx5e_htb_convert_ceil(priv, ceil, &max_average_bw);
+	mlx5e_htb_convert_rate(htb, rate, node->parent, &bw_share);
+	mlx5e_htb_convert_ceil(htb, ceil, &max_average_bw);
 
-	err = mlx5_qos_update_node(priv->mdev, node->parent->hw_id, bw_share,
+	err = mlx5_qos_update_node(htb->mdev, node->parent->hw_id, bw_share,
 				   max_average_bw, node->hw_id);
 	if (err) {
 		NL_SET_ERR_MSG_MOD(extack, "Firmware error when modifying a node.");
-		qos_err(priv->mdev, "Failed to modify a node (class %04x), err = %d\n",
+		qos_err(htb->mdev, "Failed to modify a node (class %04x), err = %d\n",
 			classid, err);
 		return err;
 	}
@@ -1021,7 +1029,7 @@ mlx5e_htb_node_modify(struct mlx5e_priv *priv, u16 classid, u64 rate, u64 ceil,
 	node->max_average_bw = max_average_bw;
 
 	if (ceil_changed)
-		err = mlx5e_qos_update_children(priv, node, extack);
+		err = mlx5e_qos_update_children(htb, node, extack);
 
 	return err;
 }
@@ -1038,71 +1046,81 @@ static void mlx5e_htb_free(struct mlx5e_htb *htb)
 	kvfree(htb);
 }
 
-static int mlx5e_htb_init(struct mlx5e_priv *priv, struct tc_htb_qopt_offload *htb)
-{
-	hash_init(priv->htb->qos_tc2node);
+/* HTB API */
 
-	return mlx5e_htb_root_add(priv, htb->parent_classid, htb->classid, htb->extack);
+static int 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)
+{
+	htb->mdev = mdev;
+	htb->netdev = netdev;
+	htb->selq = selq;
+	htb->priv = priv;
+	hash_init(htb->qos_tc2node);
+	return mlx5e_htb_root_add(htb, htb_qopt->parent_classid, htb_qopt->classid,
+				  htb_qopt->extack);
 }
 
-static void mlx5e_htb_cleanup(struct mlx5e_priv *priv)
+static void mlx5e_htb_cleanup(struct mlx5e_htb *htb)
 {
-	mlx5e_htb_root_del(priv);
+	mlx5e_htb_root_del(htb);
 }
 
-int mlx5e_htb_setup_tc(struct mlx5e_priv *priv, struct tc_htb_qopt_offload *htb)
+int mlx5e_htb_setup_tc(struct mlx5e_priv *priv, struct tc_htb_qopt_offload *htb_qopt)
 {
+	struct mlx5e_htb *htb = priv->htb;
 	int res;
 
-	if (!priv->htb && htb->command != TC_HTB_CREATE)
+	if (!htb && htb_qopt->command != TC_HTB_CREATE)
 		return -EINVAL;
 
-	switch (htb->command) {
+	switch (htb_qopt->command) {
 	case TC_HTB_CREATE:
 		if (!mlx5_qos_is_supported(priv->mdev)) {
-			NL_SET_ERR_MSG_MOD(htb->extack,
+			NL_SET_ERR_MSG_MOD(htb_qopt->extack,
 					   "Missing QoS capabilities. Try disabling SRIOV or use a supported device.");
 			return -EOPNOTSUPP;
 		}
 		priv->htb = mlx5e_htb_alloc();
-		if (!priv->htb)
+		htb = priv->htb;
+		if (!htb)
 			return -ENOMEM;
-		res = mlx5e_htb_init(priv, htb);
+		res = mlx5e_htb_init(htb, htb_qopt, priv->netdev, priv->mdev, &priv->selq, priv);
 		if (res) {
-			mlx5e_htb_free(priv->htb);
+			mlx5e_htb_free(htb);
 			priv->htb = NULL;
 		}
 		return res;
 	case TC_HTB_DESTROY:
-		mlx5e_htb_cleanup(priv);
-		mlx5e_htb_free(priv->htb);
+		mlx5e_htb_cleanup(htb);
+		mlx5e_htb_free(htb);
 		priv->htb = NULL;
 		return 0;
 	case TC_HTB_LEAF_ALLOC_QUEUE:
-		res = mlx5e_htb_leaf_alloc_queue(priv, htb->classid, htb->parent_classid,
-						 htb->rate, htb->ceil, htb->extack);
+		res = mlx5e_htb_leaf_alloc_queue(htb, htb_qopt->classid, htb_qopt->parent_classid,
+						 htb_qopt->rate, htb_qopt->ceil, htb_qopt->extack);
 		if (res < 0)
 			return res;
-		htb->qid = res;
+		htb_qopt->qid = res;
 		return 0;
 	case TC_HTB_LEAF_TO_INNER:
-		return mlx5e_htb_leaf_to_inner(priv, htb->parent_classid, htb->classid,
-					       htb->rate, htb->ceil, htb->extack);
+		return mlx5e_htb_leaf_to_inner(htb, htb_qopt->parent_classid, htb_qopt->classid,
+					       htb_qopt->rate, htb_qopt->ceil, htb_qopt->extack);
 	case TC_HTB_LEAF_DEL:
-		return mlx5e_htb_leaf_del(priv, &htb->classid, htb->extack);
+		return mlx5e_htb_leaf_del(htb, &htb_qopt->classid, htb_qopt->extack);
 	case TC_HTB_LEAF_DEL_LAST:
 	case TC_HTB_LEAF_DEL_LAST_FORCE:
-		return mlx5e_htb_leaf_del_last(priv, htb->classid,
-					       htb->command == TC_HTB_LEAF_DEL_LAST_FORCE,
-					       htb->extack);
+		return mlx5e_htb_leaf_del_last(htb, htb_qopt->classid,
+					       htb_qopt->command == TC_HTB_LEAF_DEL_LAST_FORCE,
+					       htb_qopt->extack);
 	case TC_HTB_NODE_MODIFY:
-		return mlx5e_htb_node_modify(priv, htb->classid, htb->rate, htb->ceil,
-					     htb->extack);
+		return mlx5e_htb_node_modify(htb, htb_qopt->classid, htb_qopt->rate, htb_qopt->ceil,
+					     htb_qopt->extack);
 	case TC_HTB_LEAF_QUERY_QUEUE:
-		res = mlx5e_get_txq_by_classid(priv, htb->classid);
+		res = mlx5e_get_txq_by_classid(htb, htb_qopt->classid);
 		if (res < 0)
 			return res;
-		htb->qid = res;
+		htb_qopt->qid = res;
 		return 0;
 	default:
 		return -EOPNOTSUPP;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en/qos.h b/drivers/net/ethernet/mellanox/mlx5/core/en/qos.h
index 6fbddd586736..f2c043dfaedd 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en/qos.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en/qos.h
@@ -9,16 +9,17 @@
 #define MLX5E_QOS_MAX_LEAF_NODES 256
 
 struct mlx5e_priv;
+struct mlx5e_htb;
 struct mlx5e_channels;
 struct mlx5e_channel;
 struct tc_htb_qopt_offload;
 
 int mlx5e_qos_bytes_rate_check(struct mlx5_core_dev *mdev, u64 nbytes);
 int mlx5e_qos_max_leaf_nodes(struct mlx5_core_dev *mdev);
-int mlx5e_qos_cur_leaf_nodes(struct mlx5e_priv *priv);
+int mlx5e_qos_cur_leaf_nodes(struct mlx5e_htb *htb);
 
 /* TX datapath API */
-int mlx5e_get_txq_by_classid(struct mlx5e_priv *priv, u16 classid);
+int mlx5e_get_txq_by_classid(struct mlx5e_htb *htb, u16 classid);
 
 /* SQ lifecycle */
 int mlx5e_qos_open_queues(struct mlx5e_priv *priv, struct mlx5e_channels *chs);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en/selq.c b/drivers/net/ethernet/mellanox/mlx5/core/en/selq.c
index 0e1d84ef82b6..b8e6236c7678 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en/selq.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en/selq.c
@@ -184,7 +184,7 @@ static int mlx5e_select_htb_queue(struct mlx5e_priv *priv, struct sk_buff *skb,
 	if (!classid)
 		return 0;
 
-	return mlx5e_get_txq_by_classid(priv, classid);
+	return mlx5e_get_txq_by_classid(priv->htb, classid);
 }
 
 u16 mlx5e_select_queue(struct net_device *dev, struct sk_buff *skb,
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_main.c b/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
index 127aaa1c1d19..baa71fbae973 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
@@ -2388,9 +2388,11 @@ int mlx5e_open_channels(struct mlx5e_priv *priv,
 			goto err_close_channels;
 	}
 
-	err = mlx5e_qos_open_queues(priv, chs);
-	if (err)
-		goto err_close_ptp;
+	if (priv->htb) {
+		err = mlx5e_qos_open_queues(priv, chs);
+		if (err)
+			goto err_close_ptp;
+	}
 
 	mlx5e_health_channels_update(priv);
 	kvfree(cparam);
@@ -2576,7 +2578,7 @@ int mlx5e_update_tx_netdev_queues(struct mlx5e_priv *priv)
 	int qos_queues = 0;
 
 	if (priv->htb)
-		qos_queues = mlx5e_qos_cur_leaf_nodes(priv);
+		qos_queues = mlx5e_qos_cur_leaf_nodes(priv->htb);
 
 	nch = priv->channels.params.num_channels;
 	ntc = mlx5e_get_dcb_num_tc(&priv->channels.params);
@@ -2724,7 +2726,8 @@ void mlx5e_activate_priv_channels(struct mlx5e_priv *priv)
 {
 	mlx5e_build_txq_maps(priv);
 	mlx5e_activate_channels(&priv->channels);
-	mlx5e_qos_activate_queues(priv);
+	if (priv->htb)
+		mlx5e_qos_activate_queues(priv);
 	mlx5e_xdp_tx_enable(priv);
 
 	/* dev_watchdog() wants all TX queues to be started when the carrier is
-- 
2.36.1

[net-next V2 09/13] net/mlx5e: HTB, change functions name to follow convention

[Saeed Mahameed]

From: Moshe Tal <moshet@nvidia.com>

Following the change of the functions to be object like, change also
the names.

Signed-off-by: Moshe Tal <moshet@nvidia.com>
Reviewed-by: Tariq Toukan <tariqt@nvidia.com>
Reviewed-by: Maxim Mikityanskiy <maximmi@nvidia.com>
Signed-off-by: Saeed Mahameed <saeedm@nvidia.com>
---
 .../net/ethernet/mellanox/mlx5/core/en/qos.c  | 64 +++++++++----------
 .../net/ethernet/mellanox/mlx5/core/en/qos.h  |  4 +-
 .../net/ethernet/mellanox/mlx5/core/en/selq.c |  2 +-
 .../net/ethernet/mellanox/mlx5/core/en_main.c |  2 +-
 4 files changed, 36 insertions(+), 36 deletions(-)

diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c b/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c
index 3eb4a741d75b..4428bbad3381 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c
@@ -38,7 +38,7 @@ int mlx5e_qos_max_leaf_nodes(struct mlx5_core_dev *mdev)
 	return min(MLX5E_QOS_MAX_LEAF_NODES, mlx5_qos_max_leaf_nodes(mdev));
 }
 
-int mlx5e_qos_cur_leaf_nodes(struct mlx5e_htb *htb)
+int mlx5e_htb_cur_leaf_nodes(struct mlx5e_htb *htb)
 {
 	int last;
 
@@ -75,8 +75,8 @@ struct mlx5e_qos_node {
 #define MLX5E_HTB_CLASSID_ROOT 0xffffffff
 
 static struct mlx5e_qos_node *
-mlx5e_sw_node_create_leaf(struct mlx5e_htb *htb, u16 classid, u16 qid,
-			  struct mlx5e_qos_node *parent)
+mlx5e_htb_node_create_leaf(struct mlx5e_htb *htb, u16 classid, u16 qid,
+			   struct mlx5e_qos_node *parent)
 {
 	struct mlx5e_qos_node *node;
 
@@ -97,7 +97,7 @@ mlx5e_sw_node_create_leaf(struct mlx5e_htb *htb, u16 classid, u16 qid,
 	return node;
 }
 
-static struct mlx5e_qos_node *mlx5e_sw_node_create_root(struct mlx5e_htb *htb)
+static struct mlx5e_qos_node *mlx5e_htb_node_create_root(struct mlx5e_htb *htb)
 {
 	struct mlx5e_qos_node *node;
 
@@ -112,7 +112,7 @@ static struct mlx5e_qos_node *mlx5e_sw_node_create_root(struct mlx5e_htb *htb)
 	return node;
 }
 
-static struct mlx5e_qos_node *mlx5e_sw_node_find(struct mlx5e_htb *htb, u32 classid)
+static struct mlx5e_qos_node *mlx5e_htb_node_find(struct mlx5e_htb *htb, u32 classid)
 {
 	struct mlx5e_qos_node *node = NULL;
 
@@ -124,7 +124,7 @@ static struct mlx5e_qos_node *mlx5e_sw_node_find(struct mlx5e_htb *htb, u32 clas
 	return node;
 }
 
-static struct mlx5e_qos_node *mlx5e_sw_node_find_rcu(struct mlx5e_htb *htb, u32 classid)
+static struct mlx5e_qos_node *mlx5e_htb_node_find_rcu(struct mlx5e_htb *htb, u32 classid)
 {
 	struct mlx5e_qos_node *node = NULL;
 
@@ -136,7 +136,7 @@ static struct mlx5e_qos_node *mlx5e_sw_node_find_rcu(struct mlx5e_htb *htb, u32
 	return node;
 }
 
-static void mlx5e_sw_node_delete(struct mlx5e_htb *htb, struct mlx5e_qos_node *node)
+static void mlx5e_htb_node_delete(struct mlx5e_htb *htb, struct mlx5e_qos_node *node)
 {
 	hash_del_rcu(&node->hnode);
 	if (node->qid != MLX5E_QOS_QID_INNER) {
@@ -166,7 +166,7 @@ static u16 mlx5e_qid_from_qos(struct mlx5e_channels *chs, u16 qid)
 	return (chs->params.num_channels + is_ptp) * mlx5e_get_dcb_num_tc(&chs->params) + qid;
 }
 
-int mlx5e_get_txq_by_classid(struct mlx5e_htb *htb, u16 classid)
+int mlx5e_htb_get_txq_by_classid(struct mlx5e_htb *htb, u16 classid)
 {
 	struct mlx5e_qos_node *node;
 	u16 qid;
@@ -174,7 +174,7 @@ int mlx5e_get_txq_by_classid(struct mlx5e_htb *htb, u16 classid)
 
 	rcu_read_lock();
 
-	node = mlx5e_sw_node_find_rcu(htb, classid);
+	node = mlx5e_htb_node_find_rcu(htb, classid);
 	if (!node) {
 		res = -ENOENT;
 		goto out;
@@ -513,7 +513,7 @@ mlx5e_htb_root_add(struct mlx5e_htb *htb, u16 htb_maj_id, u16 htb_defcls,
 			goto err_cancel_selq;
 	}
 
-	root = mlx5e_sw_node_create_root(htb);
+	root = mlx5e_htb_node_create_root(htb);
 	if (IS_ERR(root)) {
 		err = PTR_ERR(root);
 		goto err_free_queues;
@@ -530,7 +530,7 @@ mlx5e_htb_root_add(struct mlx5e_htb *htb, u16 htb_maj_id, u16 htb_defcls,
 	return 0;
 
 err_sw_node_delete:
-	mlx5e_sw_node_delete(htb, root);
+	mlx5e_htb_node_delete(htb, root);
 
 err_free_queues:
 	if (opened)
@@ -556,7 +556,7 @@ static int mlx5e_htb_root_del(struct mlx5e_htb *htb)
 	mlx5e_selq_prepare_htb(htb->selq, 0, 0);
 	mlx5e_selq_apply(htb->selq);
 
-	root = mlx5e_sw_node_find(htb, MLX5E_HTB_CLASSID_ROOT);
+	root = mlx5e_htb_node_find(htb, MLX5E_HTB_CLASSID_ROOT);
 	if (!root) {
 		qos_err(htb->mdev, "Failed to find the root node in the QoS tree\n");
 		return -ENOENT;
@@ -565,7 +565,7 @@ static int mlx5e_htb_root_del(struct mlx5e_htb *htb)
 	if (err)
 		qos_err(htb->mdev, "Failed to destroy root node %u, err = %d\n",
 			root->hw_id, err);
-	mlx5e_sw_node_delete(htb, root);
+	mlx5e_htb_node_delete(htb, root);
 
 	mlx5e_qos_deactivate_all_queues(&priv->channels);
 	mlx5e_qos_close_all_queues(&priv->channels);
@@ -623,11 +623,11 @@ mlx5e_htb_leaf_alloc_queue(struct mlx5e_htb *htb, u16 classid,
 		return qid;
 	}
 
-	parent = mlx5e_sw_node_find(htb, parent_classid);
+	parent = mlx5e_htb_node_find(htb, parent_classid);
 	if (!parent)
 		return -EINVAL;
 
-	node = mlx5e_sw_node_create_leaf(htb, classid, qid, parent);
+	node = mlx5e_htb_node_create_leaf(htb, classid, qid, parent);
 	if (IS_ERR(node))
 		return PTR_ERR(node);
 
@@ -642,7 +642,7 @@ mlx5e_htb_leaf_alloc_queue(struct mlx5e_htb *htb, u16 classid,
 		NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating a leaf node.");
 		qos_err(htb->mdev, "Failed to create a leaf node (class %04x), err = %d\n",
 			classid, err);
-		mlx5e_sw_node_delete(htb, node);
+		mlx5e_htb_node_delete(htb, node);
 		return err;
 	}
 
@@ -673,7 +673,7 @@ mlx5e_htb_leaf_to_inner(struct mlx5e_htb *htb, u16 classid, u16 child_classid,
 	qos_dbg(htb->mdev, "TC_HTB_LEAF_TO_INNER classid %04x, upcoming child %04x, rate %llu, ceil %llu\n",
 		classid, child_classid, rate, ceil);
 
-	node = mlx5e_sw_node_find(htb, classid);
+	node = mlx5e_htb_node_find(htb, classid);
 	if (!node)
 		return -ENOENT;
 
@@ -688,7 +688,7 @@ mlx5e_htb_leaf_to_inner(struct mlx5e_htb *htb, u16 classid, u16 child_classid,
 	}
 
 	/* Intentionally reuse the qid for the upcoming first child. */
-	child = mlx5e_sw_node_create_leaf(htb, child_classid, node->qid, node);
+	child = mlx5e_htb_node_create_leaf(htb, child_classid, node->qid, node);
 	if (IS_ERR(child)) {
 		err = PTR_ERR(child);
 		goto err_destroy_hw_node;
@@ -710,7 +710,7 @@ mlx5e_htb_leaf_to_inner(struct mlx5e_htb *htb, u16 classid, u16 child_classid,
 	/* No fail point. */
 
 	qid = node->qid;
-	/* Pairs with mlx5e_get_txq_by_classid. */
+	/* Pairs with mlx5e_htb_get_txq_by_classid. */
 	WRITE_ONCE(node->qid, MLX5E_QOS_QID_INNER);
 
 	if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
@@ -740,7 +740,7 @@ mlx5e_htb_leaf_to_inner(struct mlx5e_htb *htb, u16 classid, u16 child_classid,
 
 err_delete_sw_node:
 	child->qid = MLX5E_QOS_QID_INNER;
-	mlx5e_sw_node_delete(htb, child);
+	mlx5e_htb_node_delete(htb, child);
 
 err_destroy_hw_node:
 	tmp_err = mlx5_qos_destroy_node(htb->mdev, new_hw_id);
@@ -750,7 +750,7 @@ mlx5e_htb_leaf_to_inner(struct mlx5e_htb *htb, u16 classid, u16 child_classid,
 	return err;
 }
 
-static struct mlx5e_qos_node *mlx5e_sw_node_find_by_qid(struct mlx5e_htb *htb, u16 qid)
+static struct mlx5e_qos_node *mlx5e_htb_node_find_by_qid(struct mlx5e_htb *htb, u16 qid)
 {
 	struct mlx5e_qos_node *node = NULL;
 	int bkt;
@@ -794,7 +794,7 @@ static int mlx5e_htb_leaf_del(struct mlx5e_htb *htb, u16 *classid,
 
 	qos_dbg(htb->mdev, "TC_HTB_LEAF_DEL classid %04x\n", *classid);
 
-	node = mlx5e_sw_node_find(htb, *classid);
+	node = mlx5e_htb_node_find(htb, *classid);
 	if (!node)
 		return -ENOENT;
 
@@ -814,9 +814,9 @@ static int mlx5e_htb_leaf_del(struct mlx5e_htb *htb, u16 *classid,
 		qos_warn(htb->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
 			 node->hw_id, *classid, err);
 
-	mlx5e_sw_node_delete(htb, node);
+	mlx5e_htb_node_delete(htb, node);
 
-	moved_qid = mlx5e_qos_cur_leaf_nodes(htb);
+	moved_qid = mlx5e_htb_cur_leaf_nodes(htb);
 
 	if (moved_qid == 0) {
 		/* The last QoS SQ was just destroyed. */
@@ -838,7 +838,7 @@ static int mlx5e_htb_leaf_del(struct mlx5e_htb *htb, u16 *classid,
 	WARN(moved_qid == qid, "Can't move node with qid %u to itself", qid);
 	qos_dbg(htb->mdev, "Moving QoS SQ %u to %u\n", moved_qid, qid);
 
-	node = mlx5e_sw_node_find_by_qid(htb, moved_qid);
+	node = mlx5e_htb_node_find_by_qid(htb, moved_qid);
 	WARN(!node, "Could not find a node with qid %u to move to queue %u",
 	     moved_qid, qid);
 
@@ -891,7 +891,7 @@ mlx5e_htb_leaf_del_last(struct mlx5e_htb *htb, u16 classid, bool force,
 	qos_dbg(htb->mdev, "TC_HTB_LEAF_DEL_LAST%s classid %04x\n",
 		force ? "_FORCE" : "", classid);
 
-	node = mlx5e_sw_node_find(htb, classid);
+	node = mlx5e_htb_node_find(htb, classid);
 	if (!node)
 		return -ENOENT;
 
@@ -910,7 +910,7 @@ mlx5e_htb_leaf_del_last(struct mlx5e_htb *htb, u16 classid, bool force,
 
 	/* Store qid for reuse and prevent clearing the bit. */
 	qid = node->qid;
-	/* Pairs with mlx5e_get_txq_by_classid. */
+	/* Pairs with mlx5e_htb_get_txq_by_classid. */
 	WRITE_ONCE(node->qid, MLX5E_QOS_QID_INNER);
 
 	if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
@@ -927,7 +927,7 @@ mlx5e_htb_leaf_del_last(struct mlx5e_htb *htb, u16 classid, bool force,
 			 node->hw_id, classid, err);
 
 	parent = node->parent;
-	mlx5e_sw_node_delete(htb, node);
+	mlx5e_htb_node_delete(htb, node);
 
 	node = parent;
 	WRITE_ONCE(node->qid, qid);
@@ -961,7 +961,7 @@ mlx5e_htb_leaf_del_last(struct mlx5e_htb *htb, u16 classid, bool force,
 }
 
 static int
-mlx5e_qos_update_children(struct mlx5e_htb *htb, struct mlx5e_qos_node *node,
+mlx5e_htb_update_children(struct mlx5e_htb *htb, struct mlx5e_qos_node *node,
 			  struct netlink_ext_ack *extack)
 {
 	struct mlx5e_qos_node *child;
@@ -1005,7 +1005,7 @@ mlx5e_htb_node_modify(struct mlx5e_htb *htb, u16 classid, u64 rate, u64 ceil,
 	qos_dbg(htb->mdev, "TC_HTB_LEAF_MODIFY classid %04x, rate %llu, ceil %llu\n",
 		classid, rate, ceil);
 
-	node = mlx5e_sw_node_find(htb, classid);
+	node = mlx5e_htb_node_find(htb, classid);
 	if (!node)
 		return -ENOENT;
 
@@ -1029,7 +1029,7 @@ mlx5e_htb_node_modify(struct mlx5e_htb *htb, u16 classid, u64 rate, u64 ceil,
 	node->max_average_bw = max_average_bw;
 
 	if (ceil_changed)
-		err = mlx5e_qos_update_children(htb, node, extack);
+		err = mlx5e_htb_update_children(htb, node, extack);
 
 	return err;
 }
@@ -1117,7 +1117,7 @@ int mlx5e_htb_setup_tc(struct mlx5e_priv *priv, struct tc_htb_qopt_offload *htb_
 		return mlx5e_htb_node_modify(htb, htb_qopt->classid, htb_qopt->rate, htb_qopt->ceil,
 					     htb_qopt->extack);
 	case TC_HTB_LEAF_QUERY_QUEUE:
-		res = mlx5e_get_txq_by_classid(htb, htb_qopt->classid);
+		res = mlx5e_htb_get_txq_by_classid(htb, htb_qopt->classid);
 		if (res < 0)
 			return res;
 		htb_qopt->qid = res;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en/qos.h b/drivers/net/ethernet/mellanox/mlx5/core/en/qos.h
index f2c043dfaedd..c54eb6c50332 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en/qos.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en/qos.h
@@ -16,10 +16,9 @@ struct tc_htb_qopt_offload;
 
 int mlx5e_qos_bytes_rate_check(struct mlx5_core_dev *mdev, u64 nbytes);
 int mlx5e_qos_max_leaf_nodes(struct mlx5_core_dev *mdev);
-int mlx5e_qos_cur_leaf_nodes(struct mlx5e_htb *htb);
 
 /* TX datapath API */
-int mlx5e_get_txq_by_classid(struct mlx5e_htb *htb, u16 classid);
+int mlx5e_htb_get_txq_by_classid(struct mlx5e_htb *htb, u16 classid);
 
 /* SQ lifecycle */
 int mlx5e_qos_open_queues(struct mlx5e_priv *priv, struct mlx5e_channels *chs);
@@ -28,6 +27,7 @@ void mlx5e_qos_deactivate_queues(struct mlx5e_channel *c);
 void mlx5e_qos_close_queues(struct mlx5e_channel *c);
 
 /* HTB API */
+int mlx5e_htb_cur_leaf_nodes(struct mlx5e_htb *htb);
 int mlx5e_htb_setup_tc(struct mlx5e_priv *priv, struct tc_htb_qopt_offload *htb);
 
 /* MQPRIO TX rate limit */
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en/selq.c b/drivers/net/ethernet/mellanox/mlx5/core/en/selq.c
index b8e6236c7678..e721f59fd79b 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en/selq.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en/selq.c
@@ -184,7 +184,7 @@ static int mlx5e_select_htb_queue(struct mlx5e_priv *priv, struct sk_buff *skb,
 	if (!classid)
 		return 0;
 
-	return mlx5e_get_txq_by_classid(priv->htb, classid);
+	return mlx5e_htb_get_txq_by_classid(priv->htb, classid);
 }
 
 u16 mlx5e_select_queue(struct net_device *dev, struct sk_buff *skb,
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_main.c b/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
index baa71fbae973..992672fa53c0 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
@@ -2578,7 +2578,7 @@ int mlx5e_update_tx_netdev_queues(struct mlx5e_priv *priv)
 	int qos_queues = 0;
 
 	if (priv->htb)
-		qos_queues = mlx5e_qos_cur_leaf_nodes(priv->htb);
+		qos_queues = mlx5e_htb_cur_leaf_nodes(priv->htb);
 
 	nch = priv->channels.params.num_channels;
 	ntc = mlx5e_get_dcb_num_tc(&priv->channels.params);
-- 
2.36.1

[net-next V2 10/13] net/mlx5e: HTB, move htb functions to a new file

[Saeed Mahameed]

From: Moshe Tal <moshet@nvidia.com>

Move htb related functions and data to a separated file for better
encapsulation.

Signed-off-by: Moshe Tal <moshet@nvidia.com>
Reviewed-by: Tariq Toukan <tariqt@nvidia.com>
Reviewed-by: Maxim Mikityanskiy <maximmi@nvidia.com>
Signed-off-by: Saeed Mahameed <saeedm@nvidia.com>
---
 .../net/ethernet/mellanox/mlx5/core/Makefile  |   3 +-
 .../net/ethernet/mellanox/mlx5/core/en/htb.c  | 722 ++++++++++++++++
 .../net/ethernet/mellanox/mlx5/core/en/htb.h  |  46 +
 .../net/ethernet/mellanox/mlx5/core/en/qos.c  | 785 +-----------------
 .../net/ethernet/mellanox/mlx5/core/en/qos.h  |  20 +-
 .../net/ethernet/mellanox/mlx5/core/en/selq.c |   1 +
 .../net/ethernet/mellanox/mlx5/core/en_main.c |   1 +
 7 files changed, 832 insertions(+), 746 deletions(-)
 create mode 100644 drivers/net/ethernet/mellanox/mlx5/core/en/htb.c
 create mode 100644 drivers/net/ethernet/mellanox/mlx5/core/en/htb.h

diff --git a/drivers/net/ethernet/mellanox/mlx5/core/Makefile b/drivers/net/ethernet/mellanox/mlx5/core/Makefile
index 7ab432cc522f..a3773a8177ed 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/Makefile
+++ b/drivers/net/ethernet/mellanox/mlx5/core/Makefile
@@ -28,7 +28,8 @@ mlx5_core-$(CONFIG_MLX5_CORE_EN) += en/rqt.o en/tir.o en/rss.o en/rx_res.o \
 		en_selftest.o en/port.o en/monitor_stats.o en/health.o \
 		en/reporter_tx.o en/reporter_rx.o en/params.o en/xsk/pool.o \
 		en/xsk/setup.o en/xsk/rx.o en/xsk/tx.o en/devlink.o en/ptp.o \
-		en/qos.o en/trap.o en/fs_tt_redirect.o en/selq.o lib/crypto.o
+		en/qos.o en/htb.o en/trap.o en/fs_tt_redirect.o en/selq.o \
+		lib/crypto.o
 
 #
 # Netdev extra
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en/htb.c b/drivers/net/ethernet/mellanox/mlx5/core/en/htb.c
new file mode 100644
index 000000000000..6dac76fa58a3
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en/htb.c
@@ -0,0 +1,722 @@
+// SPDX-License-Identifier: GPL-2.0 OR Linux-OpenIB
+/* Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES. All rights reserved. */
+
+#include <net/pkt_cls.h>
+#include "htb.h"
+#include "en.h"
+#include "../qos.h"
+
+struct mlx5e_qos_node {
+	struct hlist_node hnode;
+	struct mlx5e_qos_node *parent;
+	u64 rate;
+	u32 bw_share;
+	u32 max_average_bw;
+	u32 hw_id;
+	u32 classid; /* 16-bit, except root. */
+	u16 qid;
+};
+
+struct mlx5e_htb {
+	DECLARE_HASHTABLE(qos_tc2node, order_base_2(MLX5E_QOS_MAX_LEAF_NODES));
+	DECLARE_BITMAP(qos_used_qids, MLX5E_QOS_MAX_LEAF_NODES);
+	struct mlx5_core_dev *mdev;
+	struct net_device *netdev;
+	struct mlx5e_priv *priv;
+	struct mlx5e_selq *selq;
+};
+
+#define MLX5E_QOS_QID_INNER 0xffff
+#define MLX5E_HTB_CLASSID_ROOT 0xffffffff
+
+/* Software representation of the QoS tree */
+
+int mlx5e_htb_enumerate_leaves(struct mlx5e_htb *htb, mlx5e_fp_htb_enumerate callback, void *data)
+{
+	struct mlx5e_qos_node *node = NULL;
+	int bkt, err;
+
+	hash_for_each(htb->qos_tc2node, bkt, node, hnode) {
+		if (node->qid == MLX5E_QOS_QID_INNER)
+			continue;
+		err = callback(data, node->qid, node->hw_id);
+		if (err)
+			return err;
+	}
+	return 0;
+}
+
+int mlx5e_htb_cur_leaf_nodes(struct mlx5e_htb *htb)
+{
+	int last;
+
+	last = find_last_bit(htb->qos_used_qids, mlx5e_qos_max_leaf_nodes(htb->mdev));
+	return last == mlx5e_qos_max_leaf_nodes(htb->mdev) ? 0 : last + 1;
+}
+
+static int mlx5e_htb_find_unused_qos_qid(struct mlx5e_htb *htb)
+{
+	int size = mlx5e_qos_max_leaf_nodes(htb->mdev);
+	struct mlx5e_priv *priv = htb->priv;
+	int res;
+
+	WARN_ONCE(!mutex_is_locked(&priv->state_lock), "%s: state_lock is not held\n", __func__);
+	res = find_first_zero_bit(htb->qos_used_qids, size);
+
+	return res == size ? -ENOSPC : res;
+}
+
+static struct mlx5e_qos_node *
+mlx5e_htb_node_create_leaf(struct mlx5e_htb *htb, u16 classid, u16 qid,
+			   struct mlx5e_qos_node *parent)
+{
+	struct mlx5e_qos_node *node;
+
+	node = kzalloc(sizeof(*node), GFP_KERNEL);
+	if (!node)
+		return ERR_PTR(-ENOMEM);
+
+	node->parent = parent;
+
+	node->qid = qid;
+	__set_bit(qid, htb->qos_used_qids);
+
+	node->classid = classid;
+	hash_add_rcu(htb->qos_tc2node, &node->hnode, classid);
+
+	mlx5e_update_tx_netdev_queues(htb->priv);
+
+	return node;
+}
+
+static struct mlx5e_qos_node *mlx5e_htb_node_create_root(struct mlx5e_htb *htb)
+{
+	struct mlx5e_qos_node *node;
+
+	node = kzalloc(sizeof(*node), GFP_KERNEL);
+	if (!node)
+		return ERR_PTR(-ENOMEM);
+
+	node->qid = MLX5E_QOS_QID_INNER;
+	node->classid = MLX5E_HTB_CLASSID_ROOT;
+	hash_add_rcu(htb->qos_tc2node, &node->hnode, node->classid);
+
+	return node;
+}
+
+static struct mlx5e_qos_node *mlx5e_htb_node_find(struct mlx5e_htb *htb, u32 classid)
+{
+	struct mlx5e_qos_node *node = NULL;
+
+	hash_for_each_possible(htb->qos_tc2node, node, hnode, classid) {
+		if (node->classid == classid)
+			break;
+	}
+
+	return node;
+}
+
+static struct mlx5e_qos_node *mlx5e_htb_node_find_rcu(struct mlx5e_htb *htb, u32 classid)
+{
+	struct mlx5e_qos_node *node = NULL;
+
+	hash_for_each_possible_rcu(htb->qos_tc2node, node, hnode, classid) {
+		if (node->classid == classid)
+			break;
+	}
+
+	return node;
+}
+
+static void mlx5e_htb_node_delete(struct mlx5e_htb *htb, struct mlx5e_qos_node *node)
+{
+	hash_del_rcu(&node->hnode);
+	if (node->qid != MLX5E_QOS_QID_INNER) {
+		__clear_bit(node->qid, htb->qos_used_qids);
+		mlx5e_update_tx_netdev_queues(htb->priv);
+	}
+	/* Make sure this qid is no longer selected by mlx5e_select_queue, so
+	 * that mlx5e_reactivate_qos_sq can safely restart the netdev TX queue.
+	 */
+	synchronize_net();
+	kfree(node);
+}
+
+/* TX datapath API */
+
+int mlx5e_htb_get_txq_by_classid(struct mlx5e_htb *htb, u16 classid)
+{
+	struct mlx5e_qos_node *node;
+	u16 qid;
+	int res;
+
+	rcu_read_lock();
+
+	node = mlx5e_htb_node_find_rcu(htb, classid);
+	if (!node) {
+		res = -ENOENT;
+		goto out;
+	}
+	qid = READ_ONCE(node->qid);
+	if (qid == MLX5E_QOS_QID_INNER) {
+		res = -EINVAL;
+		goto out;
+	}
+	res = mlx5e_qid_from_qos(&htb->priv->channels, qid);
+
+out:
+	rcu_read_unlock();
+	return res;
+}
+
+/* HTB TC handlers */
+
+static int
+mlx5e_htb_root_add(struct mlx5e_htb *htb, u16 htb_maj_id, u16 htb_defcls,
+		   struct netlink_ext_ack *extack)
+{
+	struct mlx5e_priv *priv = htb->priv;
+	struct mlx5e_qos_node *root;
+	bool opened;
+	int err;
+
+	qos_dbg(htb->mdev, "TC_HTB_CREATE handle %04x:, default :%04x\n", htb_maj_id, htb_defcls);
+
+	mlx5e_selq_prepare_htb(htb->selq, htb_maj_id, htb_defcls);
+
+	opened = test_bit(MLX5E_STATE_OPENED, &priv->state);
+	if (opened) {
+		err = mlx5e_qos_alloc_queues(priv, &priv->channels);
+		if (err)
+			goto err_cancel_selq;
+	}
+
+	root = mlx5e_htb_node_create_root(htb);
+	if (IS_ERR(root)) {
+		err = PTR_ERR(root);
+		goto err_free_queues;
+	}
+
+	err = mlx5_qos_create_root_node(htb->mdev, &root->hw_id);
+	if (err) {
+		NL_SET_ERR_MSG_MOD(extack, "Firmware error. Try upgrading firmware.");
+		goto err_sw_node_delete;
+	}
+
+	mlx5e_selq_apply(htb->selq);
+
+	return 0;
+
+err_sw_node_delete:
+	mlx5e_htb_node_delete(htb, root);
+
+err_free_queues:
+	if (opened)
+		mlx5e_qos_close_all_queues(&priv->channels);
+err_cancel_selq:
+	mlx5e_selq_cancel(htb->selq);
+	return err;
+}
+
+static int mlx5e_htb_root_del(struct mlx5e_htb *htb)
+{
+	struct mlx5e_priv *priv = htb->priv;
+	struct mlx5e_qos_node *root;
+	int err;
+
+	qos_dbg(htb->mdev, "TC_HTB_DESTROY\n");
+
+	/* Wait until real_num_tx_queues is updated for mlx5e_select_queue,
+	 * so that we can safely switch to its non-HTB non-PTP fastpath.
+	 */
+	synchronize_net();
+
+	mlx5e_selq_prepare_htb(htb->selq, 0, 0);
+	mlx5e_selq_apply(htb->selq);
+
+	root = mlx5e_htb_node_find(htb, MLX5E_HTB_CLASSID_ROOT);
+	if (!root) {
+		qos_err(htb->mdev, "Failed to find the root node in the QoS tree\n");
+		return -ENOENT;
+	}
+	err = mlx5_qos_destroy_node(htb->mdev, root->hw_id);
+	if (err)
+		qos_err(htb->mdev, "Failed to destroy root node %u, err = %d\n",
+			root->hw_id, err);
+	mlx5e_htb_node_delete(htb, root);
+
+	mlx5e_qos_deactivate_all_queues(&priv->channels);
+	mlx5e_qos_close_all_queues(&priv->channels);
+
+	return err;
+}
+
+static int mlx5e_htb_convert_rate(struct mlx5e_htb *htb, u64 rate,
+				  struct mlx5e_qos_node *parent, u32 *bw_share)
+{
+	u64 share = 0;
+
+	while (parent->classid != MLX5E_HTB_CLASSID_ROOT && !parent->max_average_bw)
+		parent = parent->parent;
+
+	if (parent->max_average_bw)
+		share = div64_u64(div_u64(rate * 100, BYTES_IN_MBIT),
+				  parent->max_average_bw);
+	else
+		share = 101;
+
+	*bw_share = share == 0 ? 1 : share > 100 ? 0 : share;
+
+	qos_dbg(htb->mdev, "Convert: rate %llu, parent ceil %llu -> bw_share %u\n",
+		rate, (u64)parent->max_average_bw * BYTES_IN_MBIT, *bw_share);
+
+	return 0;
+}
+
+static void mlx5e_htb_convert_ceil(struct mlx5e_htb *htb, u64 ceil, u32 *max_average_bw)
+{
+	/* Hardware treats 0 as "unlimited", set at least 1. */
+	*max_average_bw = max_t(u32, div_u64(ceil, BYTES_IN_MBIT), 1);
+
+	qos_dbg(htb->mdev, "Convert: ceil %llu -> max_average_bw %u\n",
+		ceil, *max_average_bw);
+}
+
+int
+mlx5e_htb_leaf_alloc_queue(struct mlx5e_htb *htb, u16 classid,
+			   u32 parent_classid, u64 rate, u64 ceil,
+			   struct netlink_ext_ack *extack)
+{
+	struct mlx5e_qos_node *node, *parent;
+	struct mlx5e_priv *priv = htb->priv;
+	int qid;
+	int err;
+
+	qos_dbg(htb->mdev, "TC_HTB_LEAF_ALLOC_QUEUE classid %04x, parent %04x, rate %llu, ceil %llu\n",
+		classid, parent_classid, rate, ceil);
+
+	qid = mlx5e_htb_find_unused_qos_qid(htb);
+	if (qid < 0) {
+		NL_SET_ERR_MSG_MOD(extack, "Maximum amount of leaf classes is reached.");
+		return qid;
+	}
+
+	parent = mlx5e_htb_node_find(htb, parent_classid);
+	if (!parent)
+		return -EINVAL;
+
+	node = mlx5e_htb_node_create_leaf(htb, classid, qid, parent);
+	if (IS_ERR(node))
+		return PTR_ERR(node);
+
+	node->rate = rate;
+	mlx5e_htb_convert_rate(htb, rate, node->parent, &node->bw_share);
+	mlx5e_htb_convert_ceil(htb, ceil, &node->max_average_bw);
+
+	err = mlx5_qos_create_leaf_node(htb->mdev, node->parent->hw_id,
+					node->bw_share, node->max_average_bw,
+					&node->hw_id);
+	if (err) {
+		NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating a leaf node.");
+		qos_err(htb->mdev, "Failed to create a leaf node (class %04x), err = %d\n",
+			classid, err);
+		mlx5e_htb_node_delete(htb, node);
+		return err;
+	}
+
+	if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
+		err = mlx5e_open_qos_sq(priv, &priv->channels, node->qid, node->hw_id);
+		if (err) {
+			NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
+			qos_warn(htb->mdev, "Failed to create a QoS SQ (class %04x), err = %d\n",
+				 classid, err);
+		} else {
+			mlx5e_activate_qos_sq(priv, node->qid, node->hw_id);
+		}
+	}
+
+	return mlx5e_qid_from_qos(&priv->channels, node->qid);
+}
+
+int
+mlx5e_htb_leaf_to_inner(struct mlx5e_htb *htb, u16 classid, u16 child_classid,
+			u64 rate, u64 ceil, struct netlink_ext_ack *extack)
+{
+	struct mlx5e_qos_node *node, *child;
+	struct mlx5e_priv *priv = htb->priv;
+	int err, tmp_err;
+	u32 new_hw_id;
+	u16 qid;
+
+	qos_dbg(htb->mdev, "TC_HTB_LEAF_TO_INNER classid %04x, upcoming child %04x, rate %llu, ceil %llu\n",
+		classid, child_classid, rate, ceil);
+
+	node = mlx5e_htb_node_find(htb, classid);
+	if (!node)
+		return -ENOENT;
+
+	err = mlx5_qos_create_inner_node(htb->mdev, node->parent->hw_id,
+					 node->bw_share, node->max_average_bw,
+					 &new_hw_id);
+	if (err) {
+		NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating an inner node.");
+		qos_err(htb->mdev, "Failed to create an inner node (class %04x), err = %d\n",
+			classid, err);
+		return err;
+	}
+
+	/* Intentionally reuse the qid for the upcoming first child. */
+	child = mlx5e_htb_node_create_leaf(htb, child_classid, node->qid, node);
+	if (IS_ERR(child)) {
+		err = PTR_ERR(child);
+		goto err_destroy_hw_node;
+	}
+
+	child->rate = rate;
+	mlx5e_htb_convert_rate(htb, rate, node, &child->bw_share);
+	mlx5e_htb_convert_ceil(htb, ceil, &child->max_average_bw);
+
+	err = mlx5_qos_create_leaf_node(htb->mdev, new_hw_id, child->bw_share,
+					child->max_average_bw, &child->hw_id);
+	if (err) {
+		NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating a leaf node.");
+		qos_err(htb->mdev, "Failed to create a leaf node (class %04x), err = %d\n",
+			classid, err);
+		goto err_delete_sw_node;
+	}
+
+	/* No fail point. */
+
+	qid = node->qid;
+	/* Pairs with mlx5e_htb_get_txq_by_classid. */
+	WRITE_ONCE(node->qid, MLX5E_QOS_QID_INNER);
+
+	if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
+		mlx5e_deactivate_qos_sq(priv, qid);
+		mlx5e_close_qos_sq(priv, qid);
+	}
+
+	err = mlx5_qos_destroy_node(htb->mdev, node->hw_id);
+	if (err) /* Not fatal. */
+		qos_warn(htb->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
+			 node->hw_id, classid, err);
+
+	node->hw_id = new_hw_id;
+
+	if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
+		err = mlx5e_open_qos_sq(priv, &priv->channels, child->qid, child->hw_id);
+		if (err) {
+			NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
+			qos_warn(htb->mdev, "Failed to create a QoS SQ (class %04x), err = %d\n",
+				 classid, err);
+		} else {
+			mlx5e_activate_qos_sq(priv, child->qid, child->hw_id);
+		}
+	}
+
+	return 0;
+
+err_delete_sw_node:
+	child->qid = MLX5E_QOS_QID_INNER;
+	mlx5e_htb_node_delete(htb, child);
+
+err_destroy_hw_node:
+	tmp_err = mlx5_qos_destroy_node(htb->mdev, new_hw_id);
+	if (tmp_err) /* Not fatal. */
+		qos_warn(htb->mdev, "Failed to roll back creation of an inner node %u (class %04x), err = %d\n",
+			 new_hw_id, classid, tmp_err);
+	return err;
+}
+
+static struct mlx5e_qos_node *mlx5e_htb_node_find_by_qid(struct mlx5e_htb *htb, u16 qid)
+{
+	struct mlx5e_qos_node *node = NULL;
+	int bkt;
+
+	hash_for_each(htb->qos_tc2node, bkt, node, hnode)
+		if (node->qid == qid)
+			break;
+
+	return node;
+}
+
+int mlx5e_htb_leaf_del(struct mlx5e_htb *htb, u16 *classid,
+		       struct netlink_ext_ack *extack)
+{
+	struct mlx5e_priv *priv = htb->priv;
+	struct mlx5e_qos_node *node;
+	struct netdev_queue *txq;
+	u16 qid, moved_qid;
+	bool opened;
+	int err;
+
+	qos_dbg(htb->mdev, "TC_HTB_LEAF_DEL classid %04x\n", *classid);
+
+	node = mlx5e_htb_node_find(htb, *classid);
+	if (!node)
+		return -ENOENT;
+
+	/* Store qid for reuse. */
+	qid = node->qid;
+
+	opened = test_bit(MLX5E_STATE_OPENED, &priv->state);
+	if (opened) {
+		txq = netdev_get_tx_queue(htb->netdev,
+					  mlx5e_qid_from_qos(&priv->channels, qid));
+		mlx5e_deactivate_qos_sq(priv, qid);
+		mlx5e_close_qos_sq(priv, qid);
+	}
+
+	err = mlx5_qos_destroy_node(htb->mdev, node->hw_id);
+	if (err) /* Not fatal. */
+		qos_warn(htb->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
+			 node->hw_id, *classid, err);
+
+	mlx5e_htb_node_delete(htb, node);
+
+	moved_qid = mlx5e_htb_cur_leaf_nodes(htb);
+
+	if (moved_qid == 0) {
+		/* The last QoS SQ was just destroyed. */
+		if (opened)
+			mlx5e_reactivate_qos_sq(priv, qid, txq);
+		return 0;
+	}
+	moved_qid--;
+
+	if (moved_qid < qid) {
+		/* The highest QoS SQ was just destroyed. */
+		WARN(moved_qid != qid - 1, "Gaps in queue numeration: destroyed queue %u, the highest queue is %u",
+		     qid, moved_qid);
+		if (opened)
+			mlx5e_reactivate_qos_sq(priv, qid, txq);
+		return 0;
+	}
+
+	WARN(moved_qid == qid, "Can't move node with qid %u to itself", qid);
+	qos_dbg(htb->mdev, "Moving QoS SQ %u to %u\n", moved_qid, qid);
+
+	node = mlx5e_htb_node_find_by_qid(htb, moved_qid);
+	WARN(!node, "Could not find a node with qid %u to move to queue %u",
+	     moved_qid, qid);
+
+	/* Stop traffic to the old queue. */
+	WRITE_ONCE(node->qid, MLX5E_QOS_QID_INNER);
+	__clear_bit(moved_qid, priv->htb->qos_used_qids);
+
+	if (opened) {
+		txq = netdev_get_tx_queue(htb->netdev,
+					  mlx5e_qid_from_qos(&priv->channels, moved_qid));
+		mlx5e_deactivate_qos_sq(priv, moved_qid);
+		mlx5e_close_qos_sq(priv, moved_qid);
+	}
+
+	/* Prevent packets from the old class from getting into the new one. */
+	mlx5e_reset_qdisc(htb->netdev, moved_qid);
+
+	__set_bit(qid, htb->qos_used_qids);
+	WRITE_ONCE(node->qid, qid);
+
+	if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
+		err = mlx5e_open_qos_sq(priv, &priv->channels, node->qid, node->hw_id);
+		if (err) {
+			NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
+			qos_warn(htb->mdev, "Failed to create a QoS SQ (class %04x) while moving qid %u to %u, err = %d\n",
+				 node->classid, moved_qid, qid, err);
+		} else {
+			mlx5e_activate_qos_sq(priv, node->qid, node->hw_id);
+		}
+	}
+
+	mlx5e_update_tx_netdev_queues(priv);
+	if (opened)
+		mlx5e_reactivate_qos_sq(priv, moved_qid, txq);
+
+	*classid = node->classid;
+	return 0;
+}
+
+int
+mlx5e_htb_leaf_del_last(struct mlx5e_htb *htb, u16 classid, bool force,
+			struct netlink_ext_ack *extack)
+{
+	struct mlx5e_qos_node *node, *parent;
+	struct mlx5e_priv *priv = htb->priv;
+	u32 old_hw_id, new_hw_id;
+	int err, saved_err = 0;
+	u16 qid;
+
+	qos_dbg(htb->mdev, "TC_HTB_LEAF_DEL_LAST%s classid %04x\n",
+		force ? "_FORCE" : "", classid);
+
+	node = mlx5e_htb_node_find(htb, classid);
+	if (!node)
+		return -ENOENT;
+
+	err = mlx5_qos_create_leaf_node(htb->mdev, node->parent->parent->hw_id,
+					node->parent->bw_share,
+					node->parent->max_average_bw,
+					&new_hw_id);
+	if (err) {
+		NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating a leaf node.");
+		qos_err(htb->mdev, "Failed to create a leaf node (class %04x), err = %d\n",
+			classid, err);
+		if (!force)
+			return err;
+		saved_err = err;
+	}
+
+	/* Store qid for reuse and prevent clearing the bit. */
+	qid = node->qid;
+	/* Pairs with mlx5e_htb_get_txq_by_classid. */
+	WRITE_ONCE(node->qid, MLX5E_QOS_QID_INNER);
+
+	if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
+		mlx5e_deactivate_qos_sq(priv, qid);
+		mlx5e_close_qos_sq(priv, qid);
+	}
+
+	/* Prevent packets from the old class from getting into the new one. */
+	mlx5e_reset_qdisc(htb->netdev, qid);
+
+	err = mlx5_qos_destroy_node(htb->mdev, node->hw_id);
+	if (err) /* Not fatal. */
+		qos_warn(htb->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
+			 node->hw_id, classid, err);
+
+	parent = node->parent;
+	mlx5e_htb_node_delete(htb, node);
+
+	node = parent;
+	WRITE_ONCE(node->qid, qid);
+
+	/* Early return on error in force mode. Parent will still be an inner
+	 * node to be deleted by a following delete operation.
+	 */
+	if (saved_err)
+		return saved_err;
+
+	old_hw_id = node->hw_id;
+	node->hw_id = new_hw_id;
+
+	if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
+		err = mlx5e_open_qos_sq(priv, &priv->channels, node->qid, node->hw_id);
+		if (err) {
+			NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
+			qos_warn(htb->mdev, "Failed to create a QoS SQ (class %04x), err = %d\n",
+				 classid, err);
+		} else {
+			mlx5e_activate_qos_sq(priv, node->qid, node->hw_id);
+		}
+	}
+
+	err = mlx5_qos_destroy_node(htb->mdev, old_hw_id);
+	if (err) /* Not fatal. */
+		qos_warn(htb->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
+			 node->hw_id, classid, err);
+
+	return 0;
+}
+
+static int
+mlx5e_htb_update_children(struct mlx5e_htb *htb, struct mlx5e_qos_node *node,
+			  struct netlink_ext_ack *extack)
+{
+	struct mlx5e_qos_node *child;
+	int err = 0;
+	int bkt;
+
+	hash_for_each(htb->qos_tc2node, bkt, child, hnode) {
+		u32 old_bw_share = child->bw_share;
+		int err_one;
+
+		if (child->parent != node)
+			continue;
+
+		mlx5e_htb_convert_rate(htb, child->rate, node, &child->bw_share);
+		if (child->bw_share == old_bw_share)
+			continue;
+
+		err_one = mlx5_qos_update_node(htb->mdev, child->hw_id, child->bw_share,
+					       child->max_average_bw, child->hw_id);
+		if (!err && err_one) {
+			err = err_one;
+
+			NL_SET_ERR_MSG_MOD(extack, "Firmware error when modifying a child node.");
+			qos_err(htb->mdev, "Failed to modify a child node (class %04x), err = %d\n",
+				node->classid, err);
+		}
+	}
+
+	return err;
+}
+
+int
+mlx5e_htb_node_modify(struct mlx5e_htb *htb, u16 classid, u64 rate, u64 ceil,
+		      struct netlink_ext_ack *extack)
+{
+	u32 bw_share, max_average_bw;
+	struct mlx5e_qos_node *node;
+	bool ceil_changed = false;
+	int err;
+
+	qos_dbg(htb->mdev, "TC_HTB_LEAF_MODIFY classid %04x, rate %llu, ceil %llu\n",
+		classid, rate, ceil);
+
+	node = mlx5e_htb_node_find(htb, classid);
+	if (!node)
+		return -ENOENT;
+
+	node->rate = rate;
+	mlx5e_htb_convert_rate(htb, rate, node->parent, &bw_share);
+	mlx5e_htb_convert_ceil(htb, ceil, &max_average_bw);
+
+	err = mlx5_qos_update_node(htb->mdev, node->parent->hw_id, bw_share,
+				   max_average_bw, node->hw_id);
+	if (err) {
+		NL_SET_ERR_MSG_MOD(extack, "Firmware error when modifying a node.");
+		qos_err(htb->mdev, "Failed to modify a node (class %04x), err = %d\n",
+			classid, err);
+		return err;
+	}
+
+	if (max_average_bw != node->max_average_bw)
+		ceil_changed = true;
+
+	node->bw_share = bw_share;
+	node->max_average_bw = max_average_bw;
+
+	if (ceil_changed)
+		err = mlx5e_htb_update_children(htb, node, extack);
+
+	return err;
+}
+
+struct mlx5e_htb *mlx5e_htb_alloc(void)
+{
+	return kvzalloc(sizeof(struct mlx5e_htb), GFP_KERNEL);
+}
+
+void mlx5e_htb_free(struct mlx5e_htb *htb)
+{
+	kvfree(htb);
+}
+
+int 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)
+{
+	htb->mdev = mdev;
+	htb->netdev = netdev;
+	htb->selq = selq;
+	htb->priv = priv;
+	hash_init(htb->qos_tc2node);
+	return mlx5e_htb_root_add(htb, htb_qopt->parent_classid, htb_qopt->classid,
+				  htb_qopt->extack);
+}
+
+void mlx5e_htb_cleanup(struct mlx5e_htb *htb)
+{
+	mlx5e_htb_root_del(htb);
+}
+
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en/htb.h b/drivers/net/ethernet/mellanox/mlx5/core/en/htb.h
new file mode 100644
index 000000000000..8386f1ea4559
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en/htb.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 OR Linux-OpenIB */
+/* Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES. All rights reserved. */
+
+#ifndef __MLX5E_EN_HTB_H_
+#define __MLX5E_EN_HTB_H_
+
+#include "qos.h"
+
+#define MLX5E_QOS_MAX_LEAF_NODES 256
+
+struct mlx5e_selq;
+struct mlx5e_htb;
+
+typedef int (*mlx5e_fp_htb_enumerate)(void *data, u16 qid, u32 hw_id);
+int mlx5e_htb_enumerate_leaves(struct mlx5e_htb *htb, mlx5e_fp_htb_enumerate callback, void *data);
+
+int mlx5e_htb_cur_leaf_nodes(struct mlx5e_htb *htb);
+
+/* TX datapath API */
+int mlx5e_htb_get_txq_by_classid(struct mlx5e_htb *htb, u16 classid);
+
+/* HTB TC handlers */
+
+int
+mlx5e_htb_leaf_alloc_queue(struct mlx5e_htb *htb, u16 classid,
+			   u32 parent_classid, u64 rate, u64 ceil,
+			   struct netlink_ext_ack *extack);
+int
+mlx5e_htb_leaf_to_inner(struct mlx5e_htb *htb, u16 classid, u16 child_classid,
+			u64 rate, u64 ceil, struct netlink_ext_ack *extack);
+int mlx5e_htb_leaf_del(struct mlx5e_htb *htb, u16 *classid,
+		       struct netlink_ext_ack *extack);
+int
+mlx5e_htb_leaf_del_last(struct mlx5e_htb *htb, u16 classid, bool force,
+			struct netlink_ext_ack *extack);
+int
+mlx5e_htb_node_modify(struct mlx5e_htb *htb, u16 classid, u64 rate, u64 ceil,
+		      struct netlink_ext_ack *extack);
+struct mlx5e_htb *mlx5e_htb_alloc(void);
+void mlx5e_htb_free(struct mlx5e_htb *htb);
+int 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);
+void mlx5e_htb_cleanup(struct mlx5e_htb *htb);
+#endif
+
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c b/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c
index 4428bbad3381..2842195ee548 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en/qos.c
@@ -6,16 +6,11 @@
 #include "en.h"
 #include "params.h"
 #include "../qos.h"
+#include "en/htb.h"
 
-#define BYTES_IN_MBIT 125000
-
-struct mlx5e_htb {
-	DECLARE_HASHTABLE(qos_tc2node, order_base_2(MLX5E_QOS_MAX_LEAF_NODES));
-	DECLARE_BITMAP(qos_used_qids, MLX5E_QOS_MAX_LEAF_NODES);
-	struct mlx5_core_dev *mdev;
-	struct net_device *netdev;
+struct qos_sq_callback_params {
 	struct mlx5e_priv *priv;
-	struct mlx5e_selq *selq;
+	struct mlx5e_channels *chs;
 };
 
 int mlx5e_qos_bytes_rate_check(struct mlx5_core_dev *mdev, u64 nbytes)
@@ -38,121 +33,9 @@ int mlx5e_qos_max_leaf_nodes(struct mlx5_core_dev *mdev)
 	return min(MLX5E_QOS_MAX_LEAF_NODES, mlx5_qos_max_leaf_nodes(mdev));
 }
 
-int mlx5e_htb_cur_leaf_nodes(struct mlx5e_htb *htb)
-{
-	int last;
-
-	last = find_last_bit(htb->qos_used_qids, mlx5e_qos_max_leaf_nodes(htb->mdev));
-	return last == mlx5e_qos_max_leaf_nodes(htb->mdev) ? 0 : last + 1;
-}
-
-/* Software representation of the QoS tree (internal to this file) */
-
-static int mlx5e_find_unused_qos_qid(struct mlx5e_htb *htb)
-{
-	int size = mlx5e_qos_max_leaf_nodes(htb->mdev);
-	struct mlx5e_priv *priv = htb->priv;
-	int res;
-
-	WARN_ONCE(!mutex_is_locked(&priv->state_lock), "%s: state_lock is not held\n", __func__);
-	res = find_first_zero_bit(htb->qos_used_qids, size);
-
-	return res == size ? -ENOSPC : res;
-}
-
-struct mlx5e_qos_node {
-	struct hlist_node hnode;
-	struct mlx5e_qos_node *parent;
-	u64 rate;
-	u32 bw_share;
-	u32 max_average_bw;
-	u32 hw_id;
-	u32 classid; /* 16-bit, except root. */
-	u16 qid;
-};
-
-#define MLX5E_QOS_QID_INNER 0xffff
-#define MLX5E_HTB_CLASSID_ROOT 0xffffffff
-
-static struct mlx5e_qos_node *
-mlx5e_htb_node_create_leaf(struct mlx5e_htb *htb, u16 classid, u16 qid,
-			   struct mlx5e_qos_node *parent)
-{
-	struct mlx5e_qos_node *node;
-
-	node = kzalloc(sizeof(*node), GFP_KERNEL);
-	if (!node)
-		return ERR_PTR(-ENOMEM);
-
-	node->parent = parent;
-
-	node->qid = qid;
-	__set_bit(qid, htb->qos_used_qids);
-
-	node->classid = classid;
-	hash_add_rcu(htb->qos_tc2node, &node->hnode, classid);
-
-	mlx5e_update_tx_netdev_queues(htb->priv);
-
-	return node;
-}
-
-static struct mlx5e_qos_node *mlx5e_htb_node_create_root(struct mlx5e_htb *htb)
-{
-	struct mlx5e_qos_node *node;
-
-	node = kzalloc(sizeof(*node), GFP_KERNEL);
-	if (!node)
-		return ERR_PTR(-ENOMEM);
-
-	node->qid = MLX5E_QOS_QID_INNER;
-	node->classid = MLX5E_HTB_CLASSID_ROOT;
-	hash_add_rcu(htb->qos_tc2node, &node->hnode, node->classid);
-
-	return node;
-}
-
-static struct mlx5e_qos_node *mlx5e_htb_node_find(struct mlx5e_htb *htb, u32 classid)
-{
-	struct mlx5e_qos_node *node = NULL;
-
-	hash_for_each_possible(htb->qos_tc2node, node, hnode, classid) {
-		if (node->classid == classid)
-			break;
-	}
-
-	return node;
-}
-
-static struct mlx5e_qos_node *mlx5e_htb_node_find_rcu(struct mlx5e_htb *htb, u32 classid)
-{
-	struct mlx5e_qos_node *node = NULL;
-
-	hash_for_each_possible_rcu(htb->qos_tc2node, node, hnode, classid) {
-		if (node->classid == classid)
-			break;
-	}
-
-	return node;
-}
-
-static void mlx5e_htb_node_delete(struct mlx5e_htb *htb, struct mlx5e_qos_node *node)
-{
-	hash_del_rcu(&node->hnode);
-	if (node->qid != MLX5E_QOS_QID_INNER) {
-		__clear_bit(node->qid, htb->qos_used_qids);
-		mlx5e_update_tx_netdev_queues(htb->priv);
-	}
-	/* Make sure this qid is no longer selected by mlx5e_select_queue, so
-	 * that mlx5e_reactivate_qos_sq can safely restart the netdev TX queue.
-	 */
-	synchronize_net();
-	kfree(node);
-}
-
 /* TX datapath API */
 
-static u16 mlx5e_qid_from_qos(struct mlx5e_channels *chs, u16 qid)
+u16 mlx5e_qid_from_qos(struct mlx5e_channels *chs, u16 qid)
 {
 	/* These channel params are safe to access from the datapath, because:
 	 * 1. This function is called only after checking selq->htb_maj_id != 0,
@@ -166,31 +49,6 @@ static u16 mlx5e_qid_from_qos(struct mlx5e_channels *chs, u16 qid)
 	return (chs->params.num_channels + is_ptp) * mlx5e_get_dcb_num_tc(&chs->params) + qid;
 }
 
-int mlx5e_htb_get_txq_by_classid(struct mlx5e_htb *htb, u16 classid)
-{
-	struct mlx5e_qos_node *node;
-	u16 qid;
-	int res;
-
-	rcu_read_lock();
-
-	node = mlx5e_htb_node_find_rcu(htb, classid);
-	if (!node) {
-		res = -ENOENT;
-		goto out;
-	}
-	qid = READ_ONCE(node->qid);
-	if (qid == MLX5E_QOS_QID_INNER) {
-		res = -EINVAL;
-		goto out;
-	}
-	res = mlx5e_qid_from_qos(&htb->priv->channels, qid);
-
-out:
-	rcu_read_unlock();
-	return res;
-}
-
 /* SQ lifecycle */
 
 static struct mlx5e_txqsq *mlx5e_get_qos_sq(struct mlx5e_priv *priv, int qid)
@@ -208,8 +66,8 @@ static struct mlx5e_txqsq *mlx5e_get_qos_sq(struct mlx5e_priv *priv, int qid)
 	return mlx5e_state_dereference(priv, qos_sqs[qid]);
 }
 
-static int mlx5e_open_qos_sq(struct mlx5e_priv *priv, struct mlx5e_channels *chs,
-			     struct mlx5e_qos_node *node)
+int mlx5e_open_qos_sq(struct mlx5e_priv *priv, struct mlx5e_channels *chs,
+		      u16 node_qid, u32 hw_id)
 {
 	struct mlx5e_create_cq_param ccp = {};
 	struct mlx5e_txqsq __rcu **qos_sqs;
@@ -222,10 +80,10 @@ static int mlx5e_open_qos_sq(struct mlx5e_priv *priv, struct mlx5e_channels *chs
 
 	params = &chs->params;
 
-	txq_ix = mlx5e_qid_from_qos(chs, node->qid);
+	txq_ix = mlx5e_qid_from_qos(chs, node_qid);
 
-	WARN_ON(node->qid > priv->htb_max_qos_sqs);
-	if (node->qid == priv->htb_max_qos_sqs) {
+	WARN_ON(node_qid > priv->htb_max_qos_sqs);
+	if (node_qid == priv->htb_max_qos_sqs) {
 		struct mlx5e_sq_stats *stats, **stats_list = NULL;
 
 		if (priv->htb_max_qos_sqs == 0) {
@@ -242,15 +100,15 @@ static int mlx5e_open_qos_sq(struct mlx5e_priv *priv, struct mlx5e_channels *chs
 		}
 		if (stats_list)
 			WRITE_ONCE(priv->htb_qos_sq_stats, stats_list);
-		WRITE_ONCE(priv->htb_qos_sq_stats[node->qid], stats);
+		WRITE_ONCE(priv->htb_qos_sq_stats[node_qid], stats);
 		/* Order htb_max_qos_sqs increment after writing the array pointer.
 		 * Pairs with smp_load_acquire in en_stats.c.
 		 */
 		smp_store_release(&priv->htb_max_qos_sqs, priv->htb_max_qos_sqs + 1);
 	}
 
-	ix = node->qid % params->num_channels;
-	qid = node->qid / params->num_channels;
+	ix = node_qid % params->num_channels;
+	qid = node_qid / params->num_channels;
 	c = chs->c[ix];
 
 	qos_sqs = mlx5e_state_dereference(priv, c->qos_sqs);
@@ -269,8 +127,8 @@ static int mlx5e_open_qos_sq(struct mlx5e_priv *priv, struct mlx5e_channels *chs
 	if (err)
 		goto err_free_sq;
 	err = mlx5e_open_txqsq(c, priv->tisn[c->lag_port][0], txq_ix, params,
-			       &param_sq, sq, 0, node->hw_id,
-			       priv->htb_qos_sq_stats[node->qid]);
+			       &param_sq, sq, 0, hw_id,
+			       priv->htb_qos_sq_stats[node_qid]);
 	if (err)
 		goto err_close_cq;
 
@@ -285,14 +143,22 @@ static int mlx5e_open_qos_sq(struct mlx5e_priv *priv, struct mlx5e_channels *chs
 	return err;
 }
 
-static void mlx5e_activate_qos_sq(struct mlx5e_priv *priv, struct mlx5e_qos_node *node)
+static int mlx5e_open_qos_sq_cb_wrapper(void *data, u16 node_qid, u32 hw_id)
 {
+	struct qos_sq_callback_params *cb_params = data;
+
+	return mlx5e_open_qos_sq(cb_params->priv, cb_params->chs, node_qid, hw_id);
+}
+
+int mlx5e_activate_qos_sq(void *data, u16 node_qid, u32 hw_id)
+{
+	struct mlx5e_priv *priv = data;
 	struct mlx5e_txqsq *sq;
 	u16 qid;
 
-	sq = mlx5e_get_qos_sq(priv, node->qid);
+	sq = mlx5e_get_qos_sq(priv, node_qid);
 
-	qid = mlx5e_qid_from_qos(&priv->channels, node->qid);
+	qid = mlx5e_qid_from_qos(&priv->channels, node_qid);
 
 	/* If it's a new queue, it will be marked as started at this point.
 	 * Stop it before updating txq2sq.
@@ -307,11 +173,13 @@ static void mlx5e_activate_qos_sq(struct mlx5e_priv *priv, struct mlx5e_qos_node
 	 */
 	smp_wmb();
 
-	qos_dbg(priv->mdev, "Activate QoS SQ qid %u\n", node->qid);
+	qos_dbg(priv->mdev, "Activate QoS SQ qid %u\n", node_qid);
 	mlx5e_activate_txqsq(sq);
+
+	return 0;
 }
 
-static void mlx5e_deactivate_qos_sq(struct mlx5e_priv *priv, u16 qid)
+void mlx5e_deactivate_qos_sq(struct mlx5e_priv *priv, u16 qid)
 {
 	struct mlx5e_txqsq *sq;
 
@@ -331,7 +199,7 @@ static void mlx5e_deactivate_qos_sq(struct mlx5e_priv *priv, u16 qid)
 	smp_wmb();
 }
 
-static void mlx5e_close_qos_sq(struct mlx5e_priv *priv, u16 qid)
+void mlx5e_close_qos_sq(struct mlx5e_priv *priv, u16 qid)
 {
 	struct mlx5e_txqsq __rcu **qos_sqs;
 	struct mlx5e_params *params;
@@ -381,7 +249,7 @@ void mlx5e_qos_close_queues(struct mlx5e_channel *c)
 	kvfree(qos_sqs);
 }
 
-static void mlx5e_qos_close_all_queues(struct mlx5e_channels *chs)
+void mlx5e_qos_close_all_queues(struct mlx5e_channels *chs)
 {
 	int i;
 
@@ -389,7 +257,7 @@ static void mlx5e_qos_close_all_queues(struct mlx5e_channels *chs)
 		mlx5e_qos_close_queues(chs->c[i]);
 }
 
-static int mlx5e_qos_alloc_queues(struct mlx5e_priv *priv, struct mlx5e_channels *chs)
+int mlx5e_qos_alloc_queues(struct mlx5e_priv *priv, struct mlx5e_channels *chs)
 {
 	u16 qos_sqs_size;
 	int i;
@@ -425,21 +293,20 @@ static int mlx5e_qos_alloc_queues(struct mlx5e_priv *priv, struct mlx5e_channels
 
 int mlx5e_qos_open_queues(struct mlx5e_priv *priv, struct mlx5e_channels *chs)
 {
-	struct mlx5e_qos_node *node = NULL;
-	int bkt, err;
+	struct qos_sq_callback_params callback_params;
+	int err;
 
 	err = mlx5e_qos_alloc_queues(priv, chs);
 	if (err)
 		return err;
 
-	hash_for_each(priv->htb->qos_tc2node, bkt, node, hnode) {
-		if (node->qid == MLX5E_QOS_QID_INNER)
-			continue;
-		err = mlx5e_open_qos_sq(priv, chs, node);
-		if (err) {
-			mlx5e_qos_close_all_queues(chs);
-			return err;
-		}
+	callback_params.priv = priv;
+	callback_params.chs = chs;
+
+	err = mlx5e_htb_enumerate_leaves(priv->htb, mlx5e_open_qos_sq_cb_wrapper, &callback_params);
+	if (err) {
+		mlx5e_qos_close_all_queues(chs);
+		return err;
 	}
 
 	return 0;
@@ -447,14 +314,7 @@ int mlx5e_qos_open_queues(struct mlx5e_priv *priv, struct mlx5e_channels *chs)
 
 void mlx5e_qos_activate_queues(struct mlx5e_priv *priv)
 {
-	struct mlx5e_qos_node *node = NULL;
-	int bkt;
-
-	hash_for_each(priv->htb->qos_tc2node, bkt, node, hnode) {
-		if (node->qid == MLX5E_QOS_QID_INNER)
-			continue;
-		mlx5e_activate_qos_sq(priv, node);
-	}
+	mlx5e_htb_enumerate_leaves(priv->htb, mlx5e_activate_qos_sq, priv);
 }
 
 void mlx5e_qos_deactivate_queues(struct mlx5e_channel *c)
@@ -483,7 +343,7 @@ void mlx5e_qos_deactivate_queues(struct mlx5e_channel *c)
 	}
 }
 
-static void mlx5e_qos_deactivate_all_queues(struct mlx5e_channels *chs)
+void mlx5e_qos_deactivate_all_queues(struct mlx5e_channels *chs)
 {
 	int i;
 
@@ -491,285 +351,14 @@ static void mlx5e_qos_deactivate_all_queues(struct mlx5e_channels *chs)
 		mlx5e_qos_deactivate_queues(chs->c[i]);
 }
 
-/* HTB TC handlers */
-
-static int
-mlx5e_htb_root_add(struct mlx5e_htb *htb, u16 htb_maj_id, u16 htb_defcls,
-		   struct netlink_ext_ack *extack)
-{
-	struct mlx5e_priv *priv = htb->priv;
-	struct mlx5e_qos_node *root;
-	bool opened;
-	int err;
-
-	qos_dbg(htb->mdev, "TC_HTB_CREATE handle %04x:, default :%04x\n", htb_maj_id, htb_defcls);
-
-	mlx5e_selq_prepare_htb(htb->selq, htb_maj_id, htb_defcls);
-
-	opened = test_bit(MLX5E_STATE_OPENED, &priv->state);
-	if (opened) {
-		err = mlx5e_qos_alloc_queues(priv, &priv->channels);
-		if (err)
-			goto err_cancel_selq;
-	}
-
-	root = mlx5e_htb_node_create_root(htb);
-	if (IS_ERR(root)) {
-		err = PTR_ERR(root);
-		goto err_free_queues;
-	}
-
-	err = mlx5_qos_create_root_node(htb->mdev, &root->hw_id);
-	if (err) {
-		NL_SET_ERR_MSG_MOD(extack, "Firmware error. Try upgrading firmware.");
-		goto err_sw_node_delete;
-	}
-
-	mlx5e_selq_apply(htb->selq);
-
-	return 0;
-
-err_sw_node_delete:
-	mlx5e_htb_node_delete(htb, root);
-
-err_free_queues:
-	if (opened)
-		mlx5e_qos_close_all_queues(&priv->channels);
-err_cancel_selq:
-	mlx5e_selq_cancel(htb->selq);
-	return err;
-}
-
-static int mlx5e_htb_root_del(struct mlx5e_htb *htb)
-{
-	struct mlx5e_priv *priv = htb->priv;
-	struct mlx5e_qos_node *root;
-	int err;
-
-	qos_dbg(htb->mdev, "TC_HTB_DESTROY\n");
-
-	/* Wait until real_num_tx_queues is updated for mlx5e_select_queue,
-	 * so that we can safely switch to its non-HTB non-PTP fastpath.
-	 */
-	synchronize_net();
-
-	mlx5e_selq_prepare_htb(htb->selq, 0, 0);
-	mlx5e_selq_apply(htb->selq);
-
-	root = mlx5e_htb_node_find(htb, MLX5E_HTB_CLASSID_ROOT);
-	if (!root) {
-		qos_err(htb->mdev, "Failed to find the root node in the QoS tree\n");
-		return -ENOENT;
-	}
-	err = mlx5_qos_destroy_node(htb->mdev, root->hw_id);
-	if (err)
-		qos_err(htb->mdev, "Failed to destroy root node %u, err = %d\n",
-			root->hw_id, err);
-	mlx5e_htb_node_delete(htb, root);
-
-	mlx5e_qos_deactivate_all_queues(&priv->channels);
-	mlx5e_qos_close_all_queues(&priv->channels);
-
-	return err;
-}
-
-static int mlx5e_htb_convert_rate(struct mlx5e_htb *htb, u64 rate,
-				  struct mlx5e_qos_node *parent, u32 *bw_share)
-{
-	u64 share = 0;
-
-	while (parent->classid != MLX5E_HTB_CLASSID_ROOT && !parent->max_average_bw)
-		parent = parent->parent;
-
-	if (parent->max_average_bw)
-		share = div64_u64(div_u64(rate * 100, BYTES_IN_MBIT),
-				  parent->max_average_bw);
-	else
-		share = 101;
-
-	*bw_share = share == 0 ? 1 : share > 100 ? 0 : share;
-
-	qos_dbg(htb->mdev, "Convert: rate %llu, parent ceil %llu -> bw_share %u\n",
-		rate, (u64)parent->max_average_bw * BYTES_IN_MBIT, *bw_share);
-
-	return 0;
-}
-
-static void mlx5e_htb_convert_ceil(struct mlx5e_htb *htb, u64 ceil, u32 *max_average_bw)
-{
-	/* Hardware treats 0 as "unlimited", set at least 1. */
-	*max_average_bw = max_t(u32, div_u64(ceil, BYTES_IN_MBIT), 1);
-
-	qos_dbg(htb->mdev, "Convert: ceil %llu -> max_average_bw %u\n",
-		ceil, *max_average_bw);
-}
-
-static int
-mlx5e_htb_leaf_alloc_queue(struct mlx5e_htb *htb, u16 classid,
-			   u32 parent_classid, u64 rate, u64 ceil,
-			   struct netlink_ext_ack *extack)
-{
-	struct mlx5e_qos_node *node, *parent;
-	struct mlx5e_priv *priv = htb->priv;
-	int qid;
-	int err;
-
-	qos_dbg(htb->mdev, "TC_HTB_LEAF_ALLOC_QUEUE classid %04x, parent %04x, rate %llu, ceil %llu\n",
-		classid, parent_classid, rate, ceil);
-
-	qid = mlx5e_find_unused_qos_qid(htb);
-	if (qid < 0) {
-		NL_SET_ERR_MSG_MOD(extack, "Maximum amount of leaf classes is reached.");
-		return qid;
-	}
-
-	parent = mlx5e_htb_node_find(htb, parent_classid);
-	if (!parent)
-		return -EINVAL;
-
-	node = mlx5e_htb_node_create_leaf(htb, classid, qid, parent);
-	if (IS_ERR(node))
-		return PTR_ERR(node);
-
-	node->rate = rate;
-	mlx5e_htb_convert_rate(htb, rate, node->parent, &node->bw_share);
-	mlx5e_htb_convert_ceil(htb, ceil, &node->max_average_bw);
-
-	err = mlx5_qos_create_leaf_node(htb->mdev, node->parent->hw_id,
-					node->bw_share, node->max_average_bw,
-					&node->hw_id);
-	if (err) {
-		NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating a leaf node.");
-		qos_err(htb->mdev, "Failed to create a leaf node (class %04x), err = %d\n",
-			classid, err);
-		mlx5e_htb_node_delete(htb, node);
-		return err;
-	}
-
-	if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
-		err = mlx5e_open_qos_sq(priv, &priv->channels, node);
-		if (err) {
-			NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
-			qos_warn(htb->mdev, "Failed to create a QoS SQ (class %04x), err = %d\n",
-				 classid, err);
-		} else {
-			mlx5e_activate_qos_sq(priv, node);
-		}
-	}
-
-	return mlx5e_qid_from_qos(&priv->channels, node->qid);
-}
-
-static int
-mlx5e_htb_leaf_to_inner(struct mlx5e_htb *htb, u16 classid, u16 child_classid,
-			u64 rate, u64 ceil, struct netlink_ext_ack *extack)
-{
-	struct mlx5e_qos_node *node, *child;
-	struct mlx5e_priv *priv = htb->priv;
-	int err, tmp_err;
-	u32 new_hw_id;
-	u16 qid;
-
-	qos_dbg(htb->mdev, "TC_HTB_LEAF_TO_INNER classid %04x, upcoming child %04x, rate %llu, ceil %llu\n",
-		classid, child_classid, rate, ceil);
-
-	node = mlx5e_htb_node_find(htb, classid);
-	if (!node)
-		return -ENOENT;
-
-	err = mlx5_qos_create_inner_node(htb->mdev, node->parent->hw_id,
-					 node->bw_share, node->max_average_bw,
-					 &new_hw_id);
-	if (err) {
-		NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating an inner node.");
-		qos_err(htb->mdev, "Failed to create an inner node (class %04x), err = %d\n",
-			classid, err);
-		return err;
-	}
-
-	/* Intentionally reuse the qid for the upcoming first child. */
-	child = mlx5e_htb_node_create_leaf(htb, child_classid, node->qid, node);
-	if (IS_ERR(child)) {
-		err = PTR_ERR(child);
-		goto err_destroy_hw_node;
-	}
-
-	child->rate = rate;
-	mlx5e_htb_convert_rate(htb, rate, node, &child->bw_share);
-	mlx5e_htb_convert_ceil(htb, ceil, &child->max_average_bw);
-
-	err = mlx5_qos_create_leaf_node(htb->mdev, new_hw_id, child->bw_share,
-					child->max_average_bw, &child->hw_id);
-	if (err) {
-		NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating a leaf node.");
-		qos_err(htb->mdev, "Failed to create a leaf node (class %04x), err = %d\n",
-			classid, err);
-		goto err_delete_sw_node;
-	}
-
-	/* No fail point. */
-
-	qid = node->qid;
-	/* Pairs with mlx5e_htb_get_txq_by_classid. */
-	WRITE_ONCE(node->qid, MLX5E_QOS_QID_INNER);
-
-	if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
-		mlx5e_deactivate_qos_sq(priv, qid);
-		mlx5e_close_qos_sq(priv, qid);
-	}
-
-	err = mlx5_qos_destroy_node(htb->mdev, node->hw_id);
-	if (err) /* Not fatal. */
-		qos_warn(htb->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
-			 node->hw_id, classid, err);
-
-	node->hw_id = new_hw_id;
-
-	if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
-		err = mlx5e_open_qos_sq(priv, &priv->channels, child);
-		if (err) {
-			NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
-			qos_warn(htb->mdev, "Failed to create a QoS SQ (class %04x), err = %d\n",
-				 classid, err);
-		} else {
-			mlx5e_activate_qos_sq(priv, child);
-		}
-	}
-
-	return 0;
-
-err_delete_sw_node:
-	child->qid = MLX5E_QOS_QID_INNER;
-	mlx5e_htb_node_delete(htb, child);
-
-err_destroy_hw_node:
-	tmp_err = mlx5_qos_destroy_node(htb->mdev, new_hw_id);
-	if (tmp_err) /* Not fatal. */
-		qos_warn(htb->mdev, "Failed to roll back creation of an inner node %u (class %04x), err = %d\n",
-			 new_hw_id, classid, tmp_err);
-	return err;
-}
-
-static struct mlx5e_qos_node *mlx5e_htb_node_find_by_qid(struct mlx5e_htb *htb, u16 qid)
-{
-	struct mlx5e_qos_node *node = NULL;
-	int bkt;
-
-	hash_for_each(htb->qos_tc2node, bkt, node, hnode)
-		if (node->qid == qid)
-			break;
-
-	return node;
-}
-
-static void mlx5e_reactivate_qos_sq(struct mlx5e_priv *priv, u16 qid, struct netdev_queue *txq)
+void mlx5e_reactivate_qos_sq(struct mlx5e_priv *priv, u16 qid, struct netdev_queue *txq)
 {
 	qos_dbg(priv->mdev, "Reactivate QoS SQ qid %u\n", qid);
 	netdev_tx_reset_queue(txq);
 	netif_tx_start_queue(txq);
 }
 
-static void mlx5e_reset_qdisc(struct net_device *dev, u16 qid)
+void mlx5e_reset_qdisc(struct net_device *dev, u16 qid)
 {
 	struct netdev_queue *dev_queue = netdev_get_tx_queue(dev, qid);
 	struct Qdisc *qdisc = dev_queue->qdisc_sleeping;
@@ -782,290 +371,6 @@ static void mlx5e_reset_qdisc(struct net_device *dev, u16 qid)
 	spin_unlock_bh(qdisc_lock(qdisc));
 }
 
-static int mlx5e_htb_leaf_del(struct mlx5e_htb *htb, u16 *classid,
-			      struct netlink_ext_ack *extack)
-{
-	struct mlx5e_priv *priv = htb->priv;
-	struct mlx5e_qos_node *node;
-	struct netdev_queue *txq;
-	u16 qid, moved_qid;
-	bool opened;
-	int err;
-
-	qos_dbg(htb->mdev, "TC_HTB_LEAF_DEL classid %04x\n", *classid);
-
-	node = mlx5e_htb_node_find(htb, *classid);
-	if (!node)
-		return -ENOENT;
-
-	/* Store qid for reuse. */
-	qid = node->qid;
-
-	opened = test_bit(MLX5E_STATE_OPENED, &priv->state);
-	if (opened) {
-		txq = netdev_get_tx_queue(htb->netdev,
-					  mlx5e_qid_from_qos(&priv->channels, qid));
-		mlx5e_deactivate_qos_sq(priv, qid);
-		mlx5e_close_qos_sq(priv, qid);
-	}
-
-	err = mlx5_qos_destroy_node(htb->mdev, node->hw_id);
-	if (err) /* Not fatal. */
-		qos_warn(htb->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
-			 node->hw_id, *classid, err);
-
-	mlx5e_htb_node_delete(htb, node);
-
-	moved_qid = mlx5e_htb_cur_leaf_nodes(htb);
-
-	if (moved_qid == 0) {
-		/* The last QoS SQ was just destroyed. */
-		if (opened)
-			mlx5e_reactivate_qos_sq(priv, qid, txq);
-		return 0;
-	}
-	moved_qid--;
-
-	if (moved_qid < qid) {
-		/* The highest QoS SQ was just destroyed. */
-		WARN(moved_qid != qid - 1, "Gaps in queue numeration: destroyed queue %u, the highest queue is %u",
-		     qid, moved_qid);
-		if (opened)
-			mlx5e_reactivate_qos_sq(priv, qid, txq);
-		return 0;
-	}
-
-	WARN(moved_qid == qid, "Can't move node with qid %u to itself", qid);
-	qos_dbg(htb->mdev, "Moving QoS SQ %u to %u\n", moved_qid, qid);
-
-	node = mlx5e_htb_node_find_by_qid(htb, moved_qid);
-	WARN(!node, "Could not find a node with qid %u to move to queue %u",
-	     moved_qid, qid);
-
-	/* Stop traffic to the old queue. */
-	WRITE_ONCE(node->qid, MLX5E_QOS_QID_INNER);
-	__clear_bit(moved_qid, priv->htb->qos_used_qids);
-
-	if (opened) {
-		txq = netdev_get_tx_queue(htb->netdev,
-					  mlx5e_qid_from_qos(&priv->channels, moved_qid));
-		mlx5e_deactivate_qos_sq(priv, moved_qid);
-		mlx5e_close_qos_sq(priv, moved_qid);
-	}
-
-	/* Prevent packets from the old class from getting into the new one. */
-	mlx5e_reset_qdisc(htb->netdev, moved_qid);
-
-	__set_bit(qid, htb->qos_used_qids);
-	WRITE_ONCE(node->qid, qid);
-
-	if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
-		err = mlx5e_open_qos_sq(priv, &priv->channels, node);
-		if (err) {
-			NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
-			qos_warn(htb->mdev, "Failed to create a QoS SQ (class %04x) while moving qid %u to %u, err = %d\n",
-				 node->classid, moved_qid, qid, err);
-		} else {
-			mlx5e_activate_qos_sq(priv, node);
-		}
-	}
-
-	mlx5e_update_tx_netdev_queues(priv);
-	if (opened)
-		mlx5e_reactivate_qos_sq(priv, moved_qid, txq);
-
-	*classid = node->classid;
-	return 0;
-}
-
-static int
-mlx5e_htb_leaf_del_last(struct mlx5e_htb *htb, u16 classid, bool force,
-			struct netlink_ext_ack *extack)
-{
-	struct mlx5e_qos_node *node, *parent;
-	struct mlx5e_priv *priv = htb->priv;
-	u32 old_hw_id, new_hw_id;
-	int err, saved_err = 0;
-	u16 qid;
-
-	qos_dbg(htb->mdev, "TC_HTB_LEAF_DEL_LAST%s classid %04x\n",
-		force ? "_FORCE" : "", classid);
-
-	node = mlx5e_htb_node_find(htb, classid);
-	if (!node)
-		return -ENOENT;
-
-	err = mlx5_qos_create_leaf_node(htb->mdev, node->parent->parent->hw_id,
-					node->parent->bw_share,
-					node->parent->max_average_bw,
-					&new_hw_id);
-	if (err) {
-		NL_SET_ERR_MSG_MOD(extack, "Firmware error when creating a leaf node.");
-		qos_err(htb->mdev, "Failed to create a leaf node (class %04x), err = %d\n",
-			classid, err);
-		if (!force)
-			return err;
-		saved_err = err;
-	}
-
-	/* Store qid for reuse and prevent clearing the bit. */
-	qid = node->qid;
-	/* Pairs with mlx5e_htb_get_txq_by_classid. */
-	WRITE_ONCE(node->qid, MLX5E_QOS_QID_INNER);
-
-	if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
-		mlx5e_deactivate_qos_sq(priv, qid);
-		mlx5e_close_qos_sq(priv, qid);
-	}
-
-	/* Prevent packets from the old class from getting into the new one. */
-	mlx5e_reset_qdisc(htb->netdev, qid);
-
-	err = mlx5_qos_destroy_node(htb->mdev, node->hw_id);
-	if (err) /* Not fatal. */
-		qos_warn(htb->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
-			 node->hw_id, classid, err);
-
-	parent = node->parent;
-	mlx5e_htb_node_delete(htb, node);
-
-	node = parent;
-	WRITE_ONCE(node->qid, qid);
-
-	/* Early return on error in force mode. Parent will still be an inner
-	 * node to be deleted by a following delete operation.
-	 */
-	if (saved_err)
-		return saved_err;
-
-	old_hw_id = node->hw_id;
-	node->hw_id = new_hw_id;
-
-	if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
-		err = mlx5e_open_qos_sq(priv, &priv->channels, node);
-		if (err) {
-			NL_SET_ERR_MSG_MOD(extack, "Error creating an SQ.");
-			qos_warn(htb->mdev, "Failed to create a QoS SQ (class %04x), err = %d\n",
-				 classid, err);
-		} else {
-			mlx5e_activate_qos_sq(priv, node);
-		}
-	}
-
-	err = mlx5_qos_destroy_node(htb->mdev, old_hw_id);
-	if (err) /* Not fatal. */
-		qos_warn(htb->mdev, "Failed to destroy leaf node %u (class %04x), err = %d\n",
-			 node->hw_id, classid, err);
-
-	return 0;
-}
-
-static int
-mlx5e_htb_update_children(struct mlx5e_htb *htb, struct mlx5e_qos_node *node,
-			  struct netlink_ext_ack *extack)
-{
-	struct mlx5e_qos_node *child;
-	int err = 0;
-	int bkt;
-
-	hash_for_each(htb->qos_tc2node, bkt, child, hnode) {
-		u32 old_bw_share = child->bw_share;
-		int err_one;
-
-		if (child->parent != node)
-			continue;
-
-		mlx5e_htb_convert_rate(htb, child->rate, node, &child->bw_share);
-		if (child->bw_share == old_bw_share)
-			continue;
-
-		err_one = mlx5_qos_update_node(htb->mdev, child->hw_id, child->bw_share,
-					       child->max_average_bw, child->hw_id);
-		if (!err && err_one) {
-			err = err_one;
-
-			NL_SET_ERR_MSG_MOD(extack, "Firmware error when modifying a child node.");
-			qos_err(htb->mdev, "Failed to modify a child node (class %04x), err = %d\n",
-				node->classid, err);
-		}
-	}
-
-	return err;
-}
-
-static int
-mlx5e_htb_node_modify(struct mlx5e_htb *htb, u16 classid, u64 rate, u64 ceil,
-		      struct netlink_ext_ack *extack)
-{
-	u32 bw_share, max_average_bw;
-	struct mlx5e_qos_node *node;
-	bool ceil_changed = false;
-	int err;
-
-	qos_dbg(htb->mdev, "TC_HTB_LEAF_MODIFY classid %04x, rate %llu, ceil %llu\n",
-		classid, rate, ceil);
-
-	node = mlx5e_htb_node_find(htb, classid);
-	if (!node)
-		return -ENOENT;
-
-	node->rate = rate;
-	mlx5e_htb_convert_rate(htb, rate, node->parent, &bw_share);
-	mlx5e_htb_convert_ceil(htb, ceil, &max_average_bw);
-
-	err = mlx5_qos_update_node(htb->mdev, node->parent->hw_id, bw_share,
-				   max_average_bw, node->hw_id);
-	if (err) {
-		NL_SET_ERR_MSG_MOD(extack, "Firmware error when modifying a node.");
-		qos_err(htb->mdev, "Failed to modify a node (class %04x), err = %d\n",
-			classid, err);
-		return err;
-	}
-
-	if (max_average_bw != node->max_average_bw)
-		ceil_changed = true;
-
-	node->bw_share = bw_share;
-	node->max_average_bw = max_average_bw;
-
-	if (ceil_changed)
-		err = mlx5e_htb_update_children(htb, node, extack);
-
-	return err;
-}
-
-/* HTB API */
-
-static struct mlx5e_htb *mlx5e_htb_alloc(void)
-{
-	return kvzalloc(sizeof(struct mlx5e_htb), GFP_KERNEL);
-}
-
-static void mlx5e_htb_free(struct mlx5e_htb *htb)
-{
-	kvfree(htb);
-}
-
-/* HTB API */
-
-static int 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)
-{
-	htb->mdev = mdev;
-	htb->netdev = netdev;
-	htb->selq = selq;
-	htb->priv = priv;
-	hash_init(htb->qos_tc2node);
-	return mlx5e_htb_root_add(htb, htb_qopt->parent_classid, htb_qopt->classid,
-				  htb_qopt->extack);
-}
-
-static void mlx5e_htb_cleanup(struct mlx5e_htb *htb)
-{
-	mlx5e_htb_root_del(htb);
-}
-
 int mlx5e_htb_setup_tc(struct mlx5e_priv *priv, struct tc_htb_qopt_offload *htb_qopt)
 {
 	struct mlx5e_htb *htb = priv->htb;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en/qos.h b/drivers/net/ethernet/mellanox/mlx5/core/en/qos.h
index c54eb6c50332..4947afa23b73 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en/qos.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en/qos.h
@@ -6,7 +6,7 @@
 
 #include <linux/mlx5/driver.h>
 
-#define MLX5E_QOS_MAX_LEAF_NODES 256
+#define BYTES_IN_MBIT 125000
 
 struct mlx5e_priv;
 struct mlx5e_htb;
@@ -17,17 +17,27 @@ struct tc_htb_qopt_offload;
 int mlx5e_qos_bytes_rate_check(struct mlx5_core_dev *mdev, u64 nbytes);
 int mlx5e_qos_max_leaf_nodes(struct mlx5_core_dev *mdev);
 
-/* TX datapath API */
-int mlx5e_htb_get_txq_by_classid(struct mlx5e_htb *htb, u16 classid);
-
 /* SQ lifecycle */
+int mlx5e_open_qos_sq(struct mlx5e_priv *priv, struct mlx5e_channels *chs,
+		      u16 node_qid, u32 hw_id);
+int mlx5e_activate_qos_sq(void *data, u16 node_qid, u32 hw_id);
+void mlx5e_deactivate_qos_sq(struct mlx5e_priv *priv, u16 qid);
+void mlx5e_close_qos_sq(struct mlx5e_priv *priv, u16 qid);
+void mlx5e_reactivate_qos_sq(struct mlx5e_priv *priv, u16 qid, struct netdev_queue *txq);
+void mlx5e_reset_qdisc(struct net_device *dev, u16 qid);
+
 int mlx5e_qos_open_queues(struct mlx5e_priv *priv, struct mlx5e_channels *chs);
 void mlx5e_qos_activate_queues(struct mlx5e_priv *priv);
 void mlx5e_qos_deactivate_queues(struct mlx5e_channel *c);
+void mlx5e_qos_deactivate_all_queues(struct mlx5e_channels *chs);
 void mlx5e_qos_close_queues(struct mlx5e_channel *c);
+void mlx5e_qos_close_all_queues(struct mlx5e_channels *chs);
+int mlx5e_qos_alloc_queues(struct mlx5e_priv *priv, struct mlx5e_channels *chs);
+
+/* TX datapath API */
+u16 mlx5e_qid_from_qos(struct mlx5e_channels *chs, u16 qid);
 
 /* HTB API */
-int mlx5e_htb_cur_leaf_nodes(struct mlx5e_htb *htb);
 int mlx5e_htb_setup_tc(struct mlx5e_priv *priv, struct tc_htb_qopt_offload *htb);
 
 /* MQPRIO TX rate limit */
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en/selq.c b/drivers/net/ethernet/mellanox/mlx5/core/en/selq.c
index e721f59fd79b..f675b1926340 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en/selq.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en/selq.c
@@ -7,6 +7,7 @@
 #include <linux/rcupdate.h>
 #include "en.h"
 #include "en/ptp.h"
+#include "en/htb.h"
 
 struct mlx5e_selq_params {
 	unsigned int num_regular_queues;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_main.c b/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
index 992672fa53c0..180b2f418339 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
@@ -63,6 +63,7 @@
 #include "en/devlink.h"
 #include "lib/mlx5.h"
 #include "en/ptp.h"
+#include "en/htb.h"
 #include "qos.h"
 #include "en/trap.h"
 
-- 
2.36.1

[net-next V2 11/13] net/mlx5: Expose ts_cqe_metadata_size2wqe_counter

[Saeed Mahameed]

From: Aya Levin <ayal@nvidia.com>

Add capability field which indicates the mask for wqe_counter which
connects between loopback CQE and the original WQE. With this connection
the driver can identify lost of the loopback CQE and reply PTP
synchronization with timestamp given in the original CQE.

Signed-off-by: Aya Levin <ayal@nvidia.com>
Reviewed-by: Tariq Toukan <tariqt@nvidia.com>
Signed-off-by: Saeed Mahameed <saeedm@nvidia.com>
---
 include/linux/mlx5/mlx5_ifc.h | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/include/linux/mlx5/mlx5_ifc.h b/include/linux/mlx5/mlx5_ifc.h
index 254cc22f5eec..51b4e71017ee 100644
--- a/include/linux/mlx5/mlx5_ifc.h
+++ b/include/linux/mlx5/mlx5_ifc.h
@@ -1833,7 +1833,11 @@ struct mlx5_ifc_cmd_hca_cap_2_bits {
 	u8	   sw_vhca_id[0xe];
 	u8	   reserved_at_230[0x10];
 
-	u8	   reserved_at_240[0x5c0];
+	u8	   reserved_at_240[0xb];
+	u8	   ts_cqe_metadata_size2wqe_counter[0x5];
+	u8	   reserved_at_250[0x10];
+
+	u8	   reserved_at_260[0x5a0];
 };
 
 enum mlx5_ifc_flow_destination_type {
-- 
2.36.1

[net-next V2 12/13] net/mlx5e: Add resiliency for PTP TX port timestamp

[Saeed Mahameed]

From: Aya Levin <ayal@nvidia.com>

PTP TX port timestamp relies on receiving 2 CQEs for each outgoing
packet (WQE). The regular CQE has a less accurate timestamp than the
wire CQE. On link change, the wire CQE may get lost. Let the driver
detect and restore the relation between the CQEs, and re-sync after
timeout.

Add resiliency for this as follows: add id (producer counter)
into the WQE's metadata. This id will be received in the wire
CQE (in wqe_counter field). On handling the wire CQE, if there is no
match, replay the PTP application with the time-stamp from the regular
CQE and restore the sync between the CQEs and their SKBs. This patch
adds 2 ptp counters:
1) ptp_cq0_resync_event: number of times a mismatch was detected between
   the regular CQE and the wire CQE.
2) ptp_cq0_resync_cqe: total amount of missing wire CQEs.

Signed-off-by: Aya Levin <ayal@nvidia.com>
Reviewed-by: Tariq Toukan <tariqt@nvidia.com>
Signed-off-by: Saeed Mahameed <saeedm@nvidia.com>
---
 .../net/ethernet/mellanox/mlx5/core/en/ptp.c  | 37 ++++++++++++++++++-
 .../net/ethernet/mellanox/mlx5/core/en/ptp.h  |  1 +
 .../ethernet/mellanox/mlx5/core/en_stats.c    |  2 +
 .../ethernet/mellanox/mlx5/core/en_stats.h    |  2 +
 .../net/ethernet/mellanox/mlx5/core/en_tx.c   | 10 +++++
 5 files changed, 50 insertions(+), 2 deletions(-)

diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en/ptp.c b/drivers/net/ethernet/mellanox/mlx5/core/en/ptp.c
index 047f88f09203..78ad96cf4222 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en/ptp.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en/ptp.c
@@ -79,19 +79,49 @@ void mlx5e_skb_cb_hwtstamp_handler(struct sk_buff *skb, int hwtstamp_type,
 	memset(skb->cb, 0, sizeof(struct mlx5e_skb_cb_hwtstamp));
 }
 
+#define PTP_WQE_CTR2IDX(val) ((val) & ptpsq->ts_cqe_ctr_mask)
+
+static bool mlx5e_ptp_ts_cqe_drop(struct mlx5e_ptpsq *ptpsq, u16 skb_cc, u16 skb_id)
+{
+	return (ptpsq->ts_cqe_ctr_mask && (skb_cc != skb_id));
+}
+
+static void mlx5e_ptp_skb_fifo_ts_cqe_resync(struct mlx5e_ptpsq *ptpsq, u16 skb_cc, u16 skb_id)
+{
+	struct skb_shared_hwtstamps hwts = {};
+	struct sk_buff *skb;
+
+	ptpsq->cq_stats->resync_event++;
+
+	while (skb_cc != skb_id) {
+		skb = mlx5e_skb_fifo_pop(&ptpsq->skb_fifo);
+		hwts.hwtstamp = mlx5e_skb_cb_get_hwts(skb)->cqe_hwtstamp;
+		skb_tstamp_tx(skb, &hwts);
+		ptpsq->cq_stats->resync_cqe++;
+		skb_cc = PTP_WQE_CTR2IDX(ptpsq->skb_fifo_cc);
+	}
+}
+
 static void mlx5e_ptp_handle_ts_cqe(struct mlx5e_ptpsq *ptpsq,
 				    struct mlx5_cqe64 *cqe,
 				    int budget)
 {
-	struct sk_buff *skb = mlx5e_skb_fifo_pop(&ptpsq->skb_fifo);
+	u16 skb_id = PTP_WQE_CTR2IDX(be16_to_cpu(cqe->wqe_counter));
+	u16 skb_cc = PTP_WQE_CTR2IDX(ptpsq->skb_fifo_cc);
 	struct mlx5e_txqsq *sq = &ptpsq->txqsq;
+	struct sk_buff *skb;
 	ktime_t hwtstamp;
 
 	if (unlikely(MLX5E_RX_ERR_CQE(cqe))) {
+		skb = mlx5e_skb_fifo_pop(&ptpsq->skb_fifo);
 		ptpsq->cq_stats->err_cqe++;
 		goto out;
 	}
 
+	if (mlx5e_ptp_ts_cqe_drop(ptpsq, skb_cc, skb_id))
+		mlx5e_ptp_skb_fifo_ts_cqe_resync(ptpsq, skb_cc, skb_id);
+
+	skb = mlx5e_skb_fifo_pop(&ptpsq->skb_fifo);
 	hwtstamp = mlx5e_cqe_ts_to_ns(sq->ptp_cyc2time, sq->clock, get_cqe_ts(cqe));
 	mlx5e_skb_cb_hwtstamp_handler(skb, MLX5E_SKB_CB_PORT_HWTSTAMP,
 				      hwtstamp, ptpsq->cq_stats);
@@ -241,6 +271,7 @@ static void mlx5e_ptp_destroy_sq(struct mlx5_core_dev *mdev, u32 sqn)
 static int mlx5e_ptp_alloc_traffic_db(struct mlx5e_ptpsq *ptpsq, int numa)
 {
 	int wq_sz = mlx5_wq_cyc_get_size(&ptpsq->txqsq.wq);
+	struct mlx5_core_dev *mdev = ptpsq->txqsq.mdev;
 
 	ptpsq->skb_fifo.fifo = kvzalloc_node(array_size(wq_sz, sizeof(*ptpsq->skb_fifo.fifo)),
 					     GFP_KERNEL, numa);
@@ -250,7 +281,9 @@ static int mlx5e_ptp_alloc_traffic_db(struct mlx5e_ptpsq *ptpsq, int numa)
 	ptpsq->skb_fifo.pc   = &ptpsq->skb_fifo_pc;
 	ptpsq->skb_fifo.cc   = &ptpsq->skb_fifo_cc;
 	ptpsq->skb_fifo.mask = wq_sz - 1;
-
+	if (MLX5_CAP_GEN_2(mdev, ts_cqe_metadata_size2wqe_counter))
+		ptpsq->ts_cqe_ctr_mask =
+			(1 << MLX5_CAP_GEN_2(mdev, ts_cqe_metadata_size2wqe_counter)) - 1;
 	return 0;
 }
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en/ptp.h b/drivers/net/ethernet/mellanox/mlx5/core/en/ptp.h
index a71a32e00ebb..92dbbec472ec 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en/ptp.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en/ptp.h
@@ -17,6 +17,7 @@ struct mlx5e_ptpsq {
 	u16                      skb_fifo_pc;
 	struct mlx5e_skb_fifo    skb_fifo;
 	struct mlx5e_ptp_cq_stats *cq_stats;
+	u16                      ts_cqe_ctr_mask;
 };
 
 enum {
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_stats.c b/drivers/net/ethernet/mellanox/mlx5/core/en_stats.c
index 1a88406ee6d2..7409829d1201 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_stats.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_stats.c
@@ -2100,6 +2100,8 @@ static const struct counter_desc ptp_cq_stats_desc[] = {
 	{ MLX5E_DECLARE_PTP_CQ_STAT(struct mlx5e_ptp_cq_stats, err_cqe) },
 	{ MLX5E_DECLARE_PTP_CQ_STAT(struct mlx5e_ptp_cq_stats, abort) },
 	{ MLX5E_DECLARE_PTP_CQ_STAT(struct mlx5e_ptp_cq_stats, abort_abs_diff_ns) },
+	{ MLX5E_DECLARE_PTP_CQ_STAT(struct mlx5e_ptp_cq_stats, resync_cqe) },
+	{ MLX5E_DECLARE_PTP_CQ_STAT(struct mlx5e_ptp_cq_stats, resync_event) },
 };
 
 static const struct counter_desc ptp_rq_stats_desc[] = {
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_stats.h b/drivers/net/ethernet/mellanox/mlx5/core/en_stats.h
index e48b15b55b6f..ed4fc940e4ef 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_stats.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_stats.h
@@ -453,6 +453,8 @@ struct mlx5e_ptp_cq_stats {
 	u64 err_cqe;
 	u64 abort;
 	u64 abort_abs_diff_ns;
+	u64 resync_cqe;
+	u64 resync_event;
 };
 
 struct mlx5e_stats {
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_tx.c b/drivers/net/ethernet/mellanox/mlx5/core/en_tx.c
index 699d3a9886bd..dc1e01e93d5a 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_tx.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_tx.c
@@ -631,12 +631,22 @@ void mlx5e_tx_mpwqe_ensure_complete(struct mlx5e_txqsq *sq)
 		mlx5e_tx_mpwqe_session_complete(sq);
 }
 
+static void mlx5e_cqe_ts_id_eseg(struct mlx5e_ptpsq *ptpsq, struct sk_buff *skb,
+				 struct mlx5_wqe_eth_seg *eseg)
+{
+	if (ptpsq->ts_cqe_ctr_mask && unlikely(skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP))
+		eseg->flow_table_metadata = cpu_to_be32(ptpsq->skb_fifo_pc &
+							ptpsq->ts_cqe_ctr_mask);
+}
+
 static void mlx5e_txwqe_build_eseg(struct mlx5e_priv *priv, struct mlx5e_txqsq *sq,
 				   struct sk_buff *skb, struct mlx5e_accel_tx_state *accel,
 				   struct mlx5_wqe_eth_seg *eseg, u16 ihs)
 {
 	mlx5e_accel_tx_eseg(priv, skb, eseg, ihs);
 	mlx5e_txwqe_build_eseg_csum(sq, skb, accel, eseg);
+	if (unlikely(sq->ptpsq))
+		mlx5e_cqe_ts_id_eseg(sq->ptpsq, skb, eseg);
 }
 
 netdev_tx_t mlx5e_xmit(struct sk_buff *skb, struct net_device *dev)
-- 
2.36.1

[net-next V2 13/13] net/mlx5: CT: Remove warning of ignore_flow_level support for non PF

[Saeed Mahameed]

From: Roi Dayan <roid@nvidia.com>

ignore_flow_level isn't supported for SFs, and so it causes
post_act and ct to warn about it per SF.
Apply the warning only for PF.

Signed-off-by: Roi Dayan <roid@nvidia.com>
Signed-off-by: Saeed Mahameed <saeedm@nvidia.com>
---
 drivers/net/ethernet/mellanox/mlx5/core/en/tc/post_act.c | 2 +-
 drivers/net/ethernet/mellanox/mlx5/core/en/tc_ct.c       | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en/tc/post_act.c b/drivers/net/ethernet/mellanox/mlx5/core/en/tc/post_act.c
index 2093cc2b0d48..33c1411ed8db 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en/tc/post_act.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en/tc/post_act.c
@@ -36,7 +36,7 @@ mlx5e_tc_post_act_init(struct mlx5e_priv *priv, struct mlx5_fs_chains *chains,
 	int err;
 
 	if (!MLX5_CAP_FLOWTABLE_TYPE(priv->mdev, ignore_flow_level, table_type)) {
-		if (priv->mdev->coredev_type != MLX5_COREDEV_VF)
+		if (priv->mdev->coredev_type == MLX5_COREDEV_PF)
 			mlx5_core_warn(priv->mdev, "firmware level support is missing\n");
 		err = -EOPNOTSUPP;
 		goto err_check;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en/tc_ct.c b/drivers/net/ethernet/mellanox/mlx5/core/en/tc_ct.c
index a6d84ff3a0e7..864ce0c393e6 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en/tc_ct.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en/tc_ct.c
@@ -2062,7 +2062,7 @@ mlx5_tc_ct_init_check_support(struct mlx5e_priv *priv,
 		/* Ignore_flow_level support isn't supported by default for VFs and so post_act
 		 * won't be supported. Skip showing error msg.
 		 */
-		if (priv->mdev->coredev_type != MLX5_COREDEV_VF)
+		if (priv->mdev->coredev_type == MLX5_COREDEV_PF)
 			err_msg = "post action is missing";
 		err = -EOPNOTSUPP;
 		goto out_err;
-- 
2.36.1

Re: [net-next V2 01/13] net/mlx5e: Report header-data split state through ethtool

[patchwork-bot+netdevbpf]

Hello:

This series was applied to netdev/net-next.git (master)
by Saeed Mahameed <saeedm@nvidia.com>:

On Tue, 19 Jul 2022 13:35:17 -0700 you wrote:
> From: Gal Pressman <gal@nvidia.com>
> 
> HW-GRO (SHAMPO) packet merger scheme implies header-data split in the
> driver, report it through the ethtool interface.
> 
> Signed-off-by: Gal Pressman <gal@nvidia.com>
> Reviewed-by: Tariq Toukan <tariqt@nvidia.com>
> Signed-off-by: Saeed Mahameed <saeedm@nvidia.com>
> 
> [...]

Here is the summary with links:
  - [net-next,V2,01/13] net/mlx5e: Report header-data split state through ethtool
    https://git.kernel.org/netdev/net-next/c/07071e47da44
  - [net-next,V2,02/13] net/mlx5e: Fix mqprio_rl handling on devlink reload
    https://git.kernel.org/netdev/net-next/c/0bb7228f7096
  - [net-next,V2,03/13] net/mlx5e: HTB, reduce visibility of htb functions
    https://git.kernel.org/netdev/net-next/c/efe317997ec9
  - [net-next,V2,04/13] net/mlx5e: HTB, move ids to selq_params struct
    https://git.kernel.org/netdev/net-next/c/4f8d1d3adc8d
  - [net-next,V2,05/13] net/mlx5e: HTB, move section comment to the right place
    https://git.kernel.org/netdev/net-next/c/66d95936488c
  - [net-next,V2,06/13] net/mlx5e: HTB, move stats and max_sqs to priv
    https://git.kernel.org/netdev/net-next/c/db83f24d89e6
  - [net-next,V2,07/13] net/mlx5e: HTB, hide and dynamically allocate mlx5e_htb structure
    https://git.kernel.org/netdev/net-next/c/aaffda6b3668
  - [net-next,V2,08/13] net/mlx5e: HTB, remove priv from htb function calls
    https://git.kernel.org/netdev/net-next/c/28df4a0117e2
  - [net-next,V2,09/13] net/mlx5e: HTB, change functions name to follow convention
    https://git.kernel.org/netdev/net-next/c/3685eed56f81
  - [net-next,V2,10/13] net/mlx5e: HTB, move htb functions to a new file
    https://git.kernel.org/netdev/net-next/c/462b00599936
  - [net-next,V2,11/13] net/mlx5: Expose ts_cqe_metadata_size2wqe_counter
    https://git.kernel.org/netdev/net-next/c/2e5e4185ff89
  - [net-next,V2,12/13] net/mlx5e: Add resiliency for PTP TX port timestamp
    https://git.kernel.org/netdev/net-next/c/58a518948f60
  - [net-next,V2,13/13] net/mlx5: CT: Remove warning of ignore_flow_level support for non PF
    https://git.kernel.org/netdev/net-next/c/22df2e93622f

You are awesome, thank you!
-- 
Deet-doot-dot, I am a bot.
https://korg.docs.kernel.org/patchwork/pwbot.html