1 // SPDX-License-Identifier: GPL-2.0 OR Linux-OpenIB
2 /* Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES. All rights reserved. */
3 
4 #include "selq.h"
5 #include <linux/slab.h>
6 #include <linux/netdevice.h>
7 #include <linux/rcupdate.h>
8 #include "en.h"
9 #include "en/ptp.h"
10 #include "en/htb.h"
11 
12 struct mlx5e_selq_params {
13 	unsigned int num_regular_queues;
14 	unsigned int num_channels;
15 	unsigned int num_tcs;
16 	union {
17 		u8 is_special_queues;
18 		struct {
19 			bool is_htb : 1;
20 			bool is_ptp : 1;
21 		};
22 	};
23 	u16 htb_maj_id;
24 	u16 htb_defcls;
25 };
26 
mlx5e_selq_init(struct mlx5e_selq * selq,struct mutex * state_lock)27 int mlx5e_selq_init(struct mlx5e_selq *selq, struct mutex *state_lock)
28 {
29 	struct mlx5e_selq_params *init_params;
30 
31 	selq->state_lock = state_lock;
32 
33 	selq->standby = kvzalloc(sizeof(*selq->standby), GFP_KERNEL);
34 	if (!selq->standby)
35 		return -ENOMEM;
36 
37 	init_params = kvzalloc(sizeof(*selq->active), GFP_KERNEL);
38 	if (!init_params) {
39 		kvfree(selq->standby);
40 		selq->standby = NULL;
41 		return -ENOMEM;
42 	}
43 	/* Assign dummy values, so that mlx5e_select_queue won't crash. */
44 	*init_params = (struct mlx5e_selq_params) {
45 		.num_regular_queues = 1,
46 		.num_channels = 1,
47 		.num_tcs = 1,
48 		.is_htb = false,
49 		.is_ptp = false,
50 		.htb_maj_id = 0,
51 		.htb_defcls = 0,
52 	};
53 	rcu_assign_pointer(selq->active, init_params);
54 
55 	return 0;
56 }
57 
mlx5e_selq_cleanup(struct mlx5e_selq * selq)58 void mlx5e_selq_cleanup(struct mlx5e_selq *selq)
59 {
60 	mutex_lock(selq->state_lock);
61 	WARN_ON_ONCE(selq->is_prepared);
62 
63 	kvfree(selq->standby);
64 	selq->standby = NULL;
65 	selq->is_prepared = true;
66 
67 	mlx5e_selq_apply(selq);
68 
69 	kvfree(selq->standby);
70 	selq->standby = NULL;
71 	mutex_unlock(selq->state_lock);
72 }
73 
mlx5e_selq_prepare_params(struct mlx5e_selq * selq,struct mlx5e_params * params)74 void mlx5e_selq_prepare_params(struct mlx5e_selq *selq, struct mlx5e_params *params)
75 {
76 	struct mlx5e_selq_params *selq_active;
77 
78 	lockdep_assert_held(selq->state_lock);
79 	WARN_ON_ONCE(selq->is_prepared);
80 
81 	selq->is_prepared = true;
82 
83 	selq_active = rcu_dereference_protected(selq->active,
84 						lockdep_is_held(selq->state_lock));
85 	*selq->standby = *selq_active;
86 	selq->standby->num_channels = params->num_channels;
87 	selq->standby->num_tcs = mlx5e_get_dcb_num_tc(params);
88 	selq->standby->num_regular_queues =
89 		selq->standby->num_channels * selq->standby->num_tcs;
90 	selq->standby->is_ptp = MLX5E_GET_PFLAG(params, MLX5E_PFLAG_TX_PORT_TS);
91 }
92 
mlx5e_selq_is_htb_enabled(struct mlx5e_selq * selq)93 bool mlx5e_selq_is_htb_enabled(struct mlx5e_selq *selq)
94 {
95 	struct mlx5e_selq_params *selq_active =
96 		rcu_dereference_protected(selq->active, lockdep_is_held(selq->state_lock));
97 
98 	return selq_active->htb_maj_id;
99 }
100 
mlx5e_selq_prepare_htb(struct mlx5e_selq * selq,u16 htb_maj_id,u16 htb_defcls)101 void mlx5e_selq_prepare_htb(struct mlx5e_selq *selq, u16 htb_maj_id, u16 htb_defcls)
102 {
103 	struct mlx5e_selq_params *selq_active;
104 
105 	lockdep_assert_held(selq->state_lock);
106 	WARN_ON_ONCE(selq->is_prepared);
107 
108 	selq->is_prepared = true;
109 
110 	selq_active = rcu_dereference_protected(selq->active,
111 						lockdep_is_held(selq->state_lock));
112 	*selq->standby = *selq_active;
113 	selq->standby->is_htb = htb_maj_id;
114 	selq->standby->htb_maj_id = htb_maj_id;
115 	selq->standby->htb_defcls = htb_defcls;
116 }
117 
mlx5e_selq_apply(struct mlx5e_selq * selq)118 void mlx5e_selq_apply(struct mlx5e_selq *selq)
119 {
120 	struct mlx5e_selq_params *old_params;
121 
122 	WARN_ON_ONCE(!selq->is_prepared);
123 
124 	selq->is_prepared = false;
125 
126 	old_params = rcu_replace_pointer(selq->active, selq->standby,
127 					 lockdep_is_held(selq->state_lock));
128 	synchronize_net(); /* Wait until ndo_select_queue starts emitting correct values. */
129 	selq->standby = old_params;
130 }
131 
mlx5e_selq_cancel(struct mlx5e_selq * selq)132 void mlx5e_selq_cancel(struct mlx5e_selq *selq)
133 {
134 	lockdep_assert_held(selq->state_lock);
135 	WARN_ON_ONCE(!selq->is_prepared);
136 
137 	selq->is_prepared = false;
138 }
139 
140 #ifdef CONFIG_MLX5_CORE_EN_DCB
mlx5e_get_dscp_up(struct mlx5e_priv * priv,struct sk_buff * skb)141 static int mlx5e_get_dscp_up(struct mlx5e_priv *priv, struct sk_buff *skb)
142 {
143 	int dscp_cp = 0;
144 
145 	if (skb->protocol == htons(ETH_P_IP))
146 		dscp_cp = ipv4_get_dsfield(ip_hdr(skb)) >> 2;
147 	else if (skb->protocol == htons(ETH_P_IPV6))
148 		dscp_cp = ipv6_get_dsfield(ipv6_hdr(skb)) >> 2;
149 
150 	return priv->dcbx_dp.dscp2prio[dscp_cp];
151 }
152 #endif
153 
mlx5e_get_up(struct mlx5e_priv * priv,struct sk_buff * skb)154 static int mlx5e_get_up(struct mlx5e_priv *priv, struct sk_buff *skb)
155 {
156 #ifdef CONFIG_MLX5_CORE_EN_DCB
157 	if (READ_ONCE(priv->dcbx_dp.trust_state) == MLX5_QPTS_TRUST_DSCP)
158 		return mlx5e_get_dscp_up(priv, skb);
159 #endif
160 	if (skb_vlan_tag_present(skb))
161 		return skb_vlan_tag_get_prio(skb);
162 	return 0;
163 }
164 
mlx5e_select_ptpsq(struct net_device * dev,struct sk_buff * skb,struct mlx5e_selq_params * selq)165 static u16 mlx5e_select_ptpsq(struct net_device *dev, struct sk_buff *skb,
166 			      struct mlx5e_selq_params *selq)
167 {
168 	struct mlx5e_priv *priv = netdev_priv(dev);
169 	int up;
170 
171 	up = selq->num_tcs > 1 ? mlx5e_get_up(priv, skb) : 0;
172 
173 	return selq->num_regular_queues + up;
174 }
175 
mlx5e_select_htb_queue(struct mlx5e_priv * priv,struct sk_buff * skb,struct mlx5e_selq_params * selq)176 static int mlx5e_select_htb_queue(struct mlx5e_priv *priv, struct sk_buff *skb,
177 				  struct mlx5e_selq_params *selq)
178 {
179 	u16 classid;
180 
181 	/* Order maj_id before defcls - pairs with mlx5e_htb_root_add. */
182 	if ((TC_H_MAJ(skb->priority) >> 16) == selq->htb_maj_id)
183 		classid = TC_H_MIN(skb->priority);
184 	else
185 		classid = selq->htb_defcls;
186 
187 	if (!classid)
188 		return 0;
189 
190 	return mlx5e_htb_get_txq_by_classid(priv->htb, classid);
191 }
192 
mlx5e_select_queue(struct net_device * dev,struct sk_buff * skb,struct net_device * sb_dev)193 u16 mlx5e_select_queue(struct net_device *dev, struct sk_buff *skb,
194 		       struct net_device *sb_dev)
195 {
196 	struct mlx5e_priv *priv = netdev_priv(dev);
197 	struct mlx5e_selq_params *selq;
198 	int txq_ix, up;
199 
200 	selq = rcu_dereference_bh(priv->selq.active);
201 
202 	/* This is a workaround needed only for the mlx5e_netdev_change_profile
203 	 * flow that zeroes out the whole priv without unregistering the netdev
204 	 * and without preventing ndo_select_queue from being called.
205 	 */
206 	if (unlikely(!selq))
207 		return 0;
208 
209 	if (likely(!selq->is_special_queues)) {
210 		/* No special queues, netdev_pick_tx returns one of the regular ones. */
211 
212 		txq_ix = netdev_pick_tx(dev, skb, NULL);
213 
214 		if (selq->num_tcs <= 1)
215 			return txq_ix;
216 
217 		up = mlx5e_get_up(priv, skb);
218 
219 		/* Normalize any picked txq_ix to [0, num_channels),
220 		 * So we can return a txq_ix that matches the channel and
221 		 * packet UP.
222 		 */
223 		return mlx5e_txq_to_ch_ix(txq_ix, selq->num_channels) +
224 			up * selq->num_channels;
225 	}
226 
227 	if (unlikely(selq->htb_maj_id)) {
228 		/* num_tcs == 1, shortcut for PTP */
229 
230 		txq_ix = mlx5e_select_htb_queue(priv, skb, selq);
231 		if (txq_ix > 0)
232 			return txq_ix;
233 
234 		if (unlikely(selq->is_ptp && mlx5e_use_ptpsq(skb)))
235 			return selq->num_channels;
236 
237 		txq_ix = netdev_pick_tx(dev, skb, NULL);
238 
239 		/* Fix netdev_pick_tx() not to choose ptp_channel and HTB txqs.
240 		 * If they are selected, switch to regular queues.
241 		 * Driver to select these queues only at mlx5e_select_ptpsq()
242 		 * and mlx5e_select_htb_queue().
243 		 */
244 		return mlx5e_txq_to_ch_ix_htb(txq_ix, selq->num_channels);
245 	}
246 
247 	/* PTP is enabled */
248 
249 	if (mlx5e_use_ptpsq(skb))
250 		return mlx5e_select_ptpsq(dev, skb, selq);
251 
252 	txq_ix = netdev_pick_tx(dev, skb, NULL);
253 
254 	/* Normalize any picked txq_ix to [0, num_channels). Queues in range
255 	 * [0, num_regular_queues) will be mapped to the corresponding channel
256 	 * index, so that we can apply the packet's UP (if num_tcs > 1).
257 	 * If netdev_pick_tx() picks ptp_channel, switch to a regular queue,
258 	 * because driver should select the PTP only at mlx5e_select_ptpsq().
259 	 */
260 	txq_ix = mlx5e_txq_to_ch_ix(txq_ix, selq->num_channels);
261 
262 	if (selq->num_tcs <= 1)
263 		return txq_ix;
264 
265 	up = mlx5e_get_up(priv, skb);
266 
267 	return txq_ix + up * selq->num_channels;
268 }
269