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