drivers/net/ethernet/mellanox/mlx5/core/en/selq.c

   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         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
  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(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
 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(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_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 }