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
12struct 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
27int 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(size: sizeof(*selq->standby), GFP_KERNEL);
34 if (!selq->standby)
35 return -ENOMEM;
36
37 init_params = kvzalloc(size: sizeof(*selq->active), GFP_KERNEL);
38 if (!init_params) {
39 kvfree(addr: 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
58void mlx5e_selq_cleanup(struct mlx5e_selq *selq)
59{
60 mutex_lock(selq->state_lock);
61 WARN_ON_ONCE(selq->is_prepared);
62
63 kvfree(addr: selq->standby);
64 selq->standby = NULL;
65 selq->is_prepared = true;
66
67 mlx5e_selq_apply(selq);
68
69 kvfree(addr: selq->standby);
70 selq->standby = NULL;
71 mutex_unlock(lock: selq->state_lock);
72}
73
74void 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
93bool 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
101void 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
118void 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
132void 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
141static 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(iph: ip_hdr(skb)) >> 2;
147 else if (skb->protocol == htons(ETH_P_IPV6))
148 dscp_cp = ipv6_get_dsfield(ipv6h: ipv6_hdr(skb)) >> 2;
149
150 return priv->dcbx_dp.dscp2prio[dscp_cp];
151}
152#endif
153
154static 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
165static 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
176static 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(htb: priv->htb, classid);
191}
192
193u16 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: txq_ix, num_channels: 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: txq_ix, num_channels: 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: txq_ix, num_channels: 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

source code of linux/drivers/net/ethernet/mellanox/mlx5/core/en/selq.c