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 | |
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(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 | |
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(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 | |
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 | |
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 | |
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 | |
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 | |
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 |
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(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 | |
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 | |
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 | |
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(htb: priv->htb, classid); |
191 | } |
192 | |
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: 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 | |