net/ethtool/pause.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2
   3 #include "netlink.h"
   4 #include "common.h"
   5
   6 struct pause_req_info {
   7         struct ethnl_req_info           base;
   8 };
   9
  10 struct pause_reply_data {
  11         struct ethnl_reply_data         base;
  12         struct ethtool_pauseparam       pauseparam;
  13         struct ethtool_pause_stats      pausestat;
  14 };
  15
  16 #define PAUSE_REPDATA(__reply_base) \
  17         container_of(__reply_base, struct pause_reply_data, base)
  18
  19 static const struct nla_policy
  20 pause_get_policy[ETHTOOL_A_PAUSE_MAX + 1] = {
  21         [ETHTOOL_A_PAUSE_UNSPEC]                = { .type = NLA_REJECT },
  22         [ETHTOOL_A_PAUSE_HEADER]                = { .type = NLA_NESTED },
  23         [ETHTOOL_A_PAUSE_AUTONEG]               = { .type = NLA_REJECT },
  24         [ETHTOOL_A_PAUSE_RX]                    = { .type = NLA_REJECT },
  25         [ETHTOOL_A_PAUSE_TX]                    = { .type = NLA_REJECT },
  26         [ETHTOOL_A_PAUSE_STATS]                 = { .type = NLA_REJECT },
  27 };
  28
  29 static void ethtool_stats_init(u64 *stats, unsigned int n)
  30 {
  31         while (n--)
  32                 stats[n] = ETHTOOL_STAT_NOT_SET;
  33 }
  34
  35 static int pause_prepare_data(const struct ethnl_req_info *req_base,
  36                               struct ethnl_reply_data *reply_base,
  37                               struct genl_info *info)
  38 {
  39         struct pause_reply_data *data = PAUSE_REPDATA(reply_base);
  40         struct net_device *dev = reply_base->dev;
  41         int ret;
  42
  43         if (!dev->ethtool_ops->get_pauseparam)
  44                 return -EOPNOTSUPP;
  45
  46         ret = ethnl_ops_begin(dev);
  47         if (ret < 0)
  48                 return ret;
  49         dev->ethtool_ops->get_pauseparam(dev, &data->pauseparam);
  50         if (req_base->flags & ETHTOOL_FLAG_STATS &&
  51             dev->ethtool_ops->get_pause_stats) {
  52                 ethtool_stats_init((u64 *)&data->pausestat,
  53                                    sizeof(data->pausestat) / 8);
  54                 dev->ethtool_ops->get_pause_stats(dev, &data->pausestat);
  55         }
  56         ethnl_ops_complete(dev);
  57
  58         return 0;
  59 }
  60
  61 static int pause_reply_size(const struct ethnl_req_info *req_base,
  62                             const struct ethnl_reply_data *reply_base)
  63 {
  64         int n = nla_total_size(sizeof(u8)) +    /* _PAUSE_AUTONEG */
  65                 nla_total_size(sizeof(u8)) +    /* _PAUSE_RX */
  66                 nla_total_size(sizeof(u8));     /* _PAUSE_TX */
  67
  68         if (req_base->flags & ETHTOOL_FLAG_STATS)
  69                 n += nla_total_size(0) +        /* _PAUSE_STATS */
  70                         nla_total_size_64bit(sizeof(u64)) *
  71                                 (ETHTOOL_A_PAUSE_STAT_MAX - 2);
  72         return n;
  73 }
  74
  75 static int ethtool_put_stat(struct sk_buff *skb, u64 val, u16 attrtype,
  76                             u16 padtype)
  77 {
  78         if (val == ETHTOOL_STAT_NOT_SET)
  79                 return 0;
  80         if (nla_put_u64_64bit(skb, attrtype, val, padtype))
  81                 return -EMSGSIZE;
  82
  83         return 0;
  84 }
  85
  86 static int pause_put_stats(struct sk_buff *skb,
  87                            const struct ethtool_pause_stats *pause_stats)
  88 {
  89         const u16 pad = ETHTOOL_A_PAUSE_STAT_PAD;
  90         struct nlattr *nest;
  91
  92         nest = nla_nest_start(skb, ETHTOOL_A_PAUSE_STATS);
  93         if (!nest)
  94                 return -EMSGSIZE;
  95
  96         if (ethtool_put_stat(skb, pause_stats->tx_pause_frames,
  97                              ETHTOOL_A_PAUSE_STAT_TX_FRAMES, pad) ||
  98             ethtool_put_stat(skb, pause_stats->rx_pause_frames,
  99                              ETHTOOL_A_PAUSE_STAT_RX_FRAMES, pad))
 100                 goto err_cancel;
 101
 102         nla_nest_end(skb, nest);
 103         return 0;
 104
 105 err_cancel:
 106         nla_nest_cancel(skb, nest);
 107         return -EMSGSIZE;
 108 }
 109
 110 static int pause_fill_reply(struct sk_buff *skb,
 111                             const struct ethnl_req_info *req_base,
 112                             const struct ethnl_reply_data *reply_base)
 113 {
 114         const struct pause_reply_data *data = PAUSE_REPDATA(reply_base);
 115         const struct ethtool_pauseparam *pauseparam = &data->pauseparam;
 116
 117         if (nla_put_u8(skb, ETHTOOL_A_PAUSE_AUTONEG, !!pauseparam->autoneg) ||
 118             nla_put_u8(skb, ETHTOOL_A_PAUSE_RX, !!pauseparam->rx_pause) ||
 119             nla_put_u8(skb, ETHTOOL_A_PAUSE_TX, !!pauseparam->tx_pause))
 120                 return -EMSGSIZE;
 121
 122         if (req_base->flags & ETHTOOL_FLAG_STATS &&
 123             pause_put_stats(skb, &data->pausestat))
 124                 return -EMSGSIZE;
 125
 126         return 0;
 127 }
 128
 129 const struct ethnl_request_ops ethnl_pause_request_ops = {
 130         .request_cmd            = ETHTOOL_MSG_PAUSE_GET,
 131         .reply_cmd              = ETHTOOL_MSG_PAUSE_GET_REPLY,
 132         .hdr_attr               = ETHTOOL_A_PAUSE_HEADER,
 133         .max_attr               = ETHTOOL_A_PAUSE_MAX,
 134         .req_info_size          = sizeof(struct pause_req_info),
 135         .reply_data_size        = sizeof(struct pause_reply_data),
 136         .request_policy         = pause_get_policy,
 137
 138         .prepare_data           = pause_prepare_data,
 139         .reply_size             = pause_reply_size,
 140         .fill_reply             = pause_fill_reply,
 141 };
 142
 143 /* PAUSE_SET */
 144
 145 static const struct nla_policy
 146 pause_set_policy[ETHTOOL_A_PAUSE_MAX + 1] = {
 147         [ETHTOOL_A_PAUSE_UNSPEC]                = { .type = NLA_REJECT },
 148         [ETHTOOL_A_PAUSE_HEADER]                = { .type = NLA_NESTED },
 149         [ETHTOOL_A_PAUSE_AUTONEG]               = { .type = NLA_U8 },
 150         [ETHTOOL_A_PAUSE_RX]                    = { .type = NLA_U8 },
 151         [ETHTOOL_A_PAUSE_TX]                    = { .type = NLA_U8 },
 152         [ETHTOOL_A_PAUSE_STATS]                 = { .type = NLA_REJECT },
 153 };
 154
 155 int ethnl_set_pause(struct sk_buff *skb, struct genl_info *info)
 156 {
 157         struct nlattr *tb[ETHTOOL_A_PAUSE_MAX + 1];
 158         struct ethtool_pauseparam params = {};
 159         struct ethnl_req_info req_info = {};
 160         const struct ethtool_ops *ops;
 161         struct net_device *dev;
 162         bool mod = false;
 163         int ret;
 164
 165         ret = nlmsg_parse(info->nlhdr, GENL_HDRLEN, tb, ETHTOOL_A_PAUSE_MAX,
 166                           pause_set_policy, info->extack);
 167         if (ret < 0)
 168                 return ret;
 169         ret = ethnl_parse_header_dev_get(&req_info,
 170                                          tb[ETHTOOL_A_PAUSE_HEADER],
 171                                          genl_info_net(info), info->extack,
 172                                          true);
 173         if (ret < 0)
 174                 return ret;
 175         dev = req_info.dev;
 176         ops = dev->ethtool_ops;
 177         ret = -EOPNOTSUPP;
 178         if (!ops->get_pauseparam || !ops->set_pauseparam)
 179                 goto out_dev;
 180
 181         rtnl_lock();
 182         ret = ethnl_ops_begin(dev);
 183         if (ret < 0)
 184                 goto out_rtnl;
 185         ops->get_pauseparam(dev, &params);
 186
 187         ethnl_update_bool32(&params.autoneg, tb[ETHTOOL_A_PAUSE_AUTONEG], &mod);
 188         ethnl_update_bool32(&params.rx_pause, tb[ETHTOOL_A_PAUSE_RX], &mod);
 189         ethnl_update_bool32(&params.tx_pause, tb[ETHTOOL_A_PAUSE_TX], &mod);
 190         ret = 0;
 191         if (!mod)
 192                 goto out_ops;
 193
 194         ret = dev->ethtool_ops->set_pauseparam(dev, &params);
 195         if (ret < 0)
 196                 goto out_ops;
 197         ethtool_notify(dev, ETHTOOL_MSG_PAUSE_NTF, NULL);
 198
 199 out_ops:
 200         ethnl_ops_complete(dev);
 201 out_rtnl:
 202         rtnl_unlock();
 203 out_dev:
 204         dev_put(dev);
 205         return ret;
 206 }