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