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 "en.h"
   8 #include "en/ptp.h"
   9
  10 struct mlx5e_selq_params {
  11         unsigned int num_regular_queues;
  12         unsigned int num_channels;
  13         unsigned int num_tcs;
  14         bool is_htb;
  15         bool is_ptp;
  16 };
  17
  18 int mlx5e_selq_init(struct mlx5e_selq *selq, struct mutex *state_lock)
  19 {
  20         struct mlx5e_selq_params *init_params;
  21
  22         selq->state_lock = state_lock;
  23
  24         selq->standby = kvzalloc(sizeof(*selq->standby), GFP_KERNEL);
  25         if (!selq->standby)
  26                 return -ENOMEM;
  27
  28         init_params = kvzalloc(sizeof(*selq->active), GFP_KERNEL);
  29         if (!init_params) {
  30                 kvfree(selq->standby);
  31                 selq->standby = NULL;
  32                 return -ENOMEM;
  33         }
  34         /* Assign dummy values, so that mlx5e_select_queue won't crash. */
  35         *init_params = (struct mlx5e_selq_params) {
  36                 .num_regular_queues = 1,
  37                 .num_channels = 1,
  38                 .num_tcs = 1,
  39                 .is_htb = false,
  40                 .is_ptp = false,
  41         };
  42         rcu_assign_pointer(selq->active, init_params);
  43
  44         return 0;
  45 }
  46
  47 void mlx5e_selq_cleanup(struct mlx5e_selq *selq)
  48 {
  49         WARN_ON_ONCE(selq->is_prepared);
  50
  51         kvfree(selq->standby);
  52         selq->standby = NULL;
  53         selq->is_prepared = true;
  54
  55         mlx5e_selq_apply(selq);
  56
  57         kvfree(selq->standby);
  58         selq->standby = NULL;
  59 }
  60
  61 void mlx5e_selq_prepare(struct mlx5e_selq *selq, struct mlx5e_params *params, bool htb)
  62 {
  63         lockdep_assert_held(selq->state_lock);
  64         WARN_ON_ONCE(selq->is_prepared);
  65
  66         selq->is_prepared = true;
  67
  68         selq->standby->num_channels = params->num_channels;
  69         selq->standby->num_tcs = mlx5e_get_dcb_num_tc(params);
  70         selq->standby->num_regular_queues =
  71                 selq->standby->num_channels * selq->standby->num_tcs;
  72         selq->standby->is_htb = htb;
  73         selq->standby->is_ptp = MLX5E_GET_PFLAG(params, MLX5E_PFLAG_TX_PORT_TS);
  74 }
  75
  76 void mlx5e_selq_apply(struct mlx5e_selq *selq)
  77 {
  78         struct mlx5e_selq_params *old_params;
  79
  80         WARN_ON_ONCE(!selq->is_prepared);
  81
  82         selq->is_prepared = false;
  83
  84         old_params = rcu_replace_pointer(selq->active, selq->standby,
  85                                          lockdep_is_held(selq->state_lock));
  86         synchronize_net(); /* Wait until ndo_select_queue starts emitting correct values. */
  87         selq->standby = old_params;
  88 }
  89
  90 void mlx5e_selq_cancel(struct mlx5e_selq *selq)
  91 {
  92         lockdep_assert_held(selq->state_lock);
  93         WARN_ON_ONCE(!selq->is_prepared);
  94
  95         selq->is_prepared = false;
  96 }
  97
  98 #ifdef CONFIG_MLX5_CORE_EN_DCB
  99 static int mlx5e_get_dscp_up(struct mlx5e_priv *priv, struct sk_buff *skb)
 100 {
 101         int dscp_cp = 0;
 102
 103         if (skb->protocol == htons(ETH_P_IP))
 104                 dscp_cp = ipv4_get_dsfield(ip_hdr(skb)) >> 2;
 105         else if (skb->protocol == htons(ETH_P_IPV6))
 106                 dscp_cp = ipv6_get_dsfield(ipv6_hdr(skb)) >> 2;
 107
 108         return priv->dcbx_dp.dscp2prio[dscp_cp];
 109 }
 110 #endif
 111
 112 static u16 mlx5e_select_ptpsq(struct net_device *dev, struct sk_buff *skb)
 113 {
 114         struct mlx5e_priv *priv = netdev_priv(dev);
 115         int up = 0;
 116
 117         if (!netdev_get_num_tc(dev))
 118                 goto return_txq;
 119
 120 #ifdef CONFIG_MLX5_CORE_EN_DCB
 121         if (priv->dcbx_dp.trust_state == MLX5_QPTS_TRUST_DSCP)
 122                 up = mlx5e_get_dscp_up(priv, skb);
 123         else
 124 #endif
 125                 if (skb_vlan_tag_present(skb))
 126                         up = skb_vlan_tag_get_prio(skb);
 127
 128 return_txq:
 129         return priv->port_ptp_tc2realtxq[up];
 130 }
 131
 132 static int mlx5e_select_htb_queue(struct mlx5e_priv *priv, struct sk_buff *skb,
 133                                   u16 htb_maj_id)
 134 {
 135         u16 classid;
 136
 137         if ((TC_H_MAJ(skb->priority) >> 16) == htb_maj_id)
 138                 classid = TC_H_MIN(skb->priority);
 139         else
 140                 classid = READ_ONCE(priv->htb.defcls);
 141
 142         if (!classid)
 143                 return 0;
 144
 145         return mlx5e_get_txq_by_classid(priv, classid);
 146 }
 147
 148 u16 mlx5e_select_queue(struct net_device *dev, struct sk_buff *skb,
 149                        struct net_device *sb_dev)
 150 {
 151         struct mlx5e_priv *priv = netdev_priv(dev);
 152         int num_tc_x_num_ch;
 153         int txq_ix;
 154         int up = 0;
 155         int ch_ix;
 156
 157         /* Sync with mlx5e_update_num_tc_x_num_ch - avoid refetching. */
 158         num_tc_x_num_ch = READ_ONCE(priv->num_tc_x_num_ch);
 159         if (unlikely(dev->real_num_tx_queues > num_tc_x_num_ch)) {
 160                 struct mlx5e_ptp *ptp_channel;
 161
 162                 /* Order maj_id before defcls - pairs with mlx5e_htb_root_add. */
 163                 u16 htb_maj_id = smp_load_acquire(&priv->htb.maj_id);
 164
 165                 if (unlikely(htb_maj_id)) {
 166                         txq_ix = mlx5e_select_htb_queue(priv, skb, htb_maj_id);
 167                         if (txq_ix > 0)
 168                                 return txq_ix;
 169                 }
 170
 171                 ptp_channel = READ_ONCE(priv->channels.ptp);
 172                 if (unlikely(ptp_channel &&
 173                              test_bit(MLX5E_PTP_STATE_TX, ptp_channel->state) &&
 174                              mlx5e_use_ptpsq(skb)))
 175                         return mlx5e_select_ptpsq(dev, skb);
 176
 177                 txq_ix = netdev_pick_tx(dev, skb, NULL);
 178                 /* Fix netdev_pick_tx() not to choose ptp_channel and HTB txqs.
 179                  * If they are selected, switch to regular queues.
 180                  * Driver to select these queues only at mlx5e_select_ptpsq()
 181                  * and mlx5e_select_htb_queue().
 182                  */
 183                 if (unlikely(txq_ix >= num_tc_x_num_ch))
 184                         txq_ix %= num_tc_x_num_ch;
 185         } else {
 186                 txq_ix = netdev_pick_tx(dev, skb, NULL);
 187         }
 188
 189         if (!netdev_get_num_tc(dev))
 190                 return txq_ix;
 191
 192 #ifdef CONFIG_MLX5_CORE_EN_DCB
 193         if (priv->dcbx_dp.trust_state == MLX5_QPTS_TRUST_DSCP)
 194                 up = mlx5e_get_dscp_up(priv, skb);
 195         else
 196 #endif
 197                 if (skb_vlan_tag_present(skb))
 198                         up = skb_vlan_tag_get_prio(skb);
 199
 200         /* Normalize any picked txq_ix to [0, num_channels),
 201          * So we can return a txq_ix that matches the channel and
 202          * packet UP.
 203          */
 204         ch_ix = priv->txq2sq[txq_ix]->ch_ix;
 205
 206         return priv->channel_tc2realtxq[ch_ix][up];
 207 }