net/ethtool/pause.c

0001 // SPDX-License-Identifier: GPL-2.0-only
0002
0003 #include "netlink.h"
0004 #include "common.h"
0005
0006 struct pause_req_info {
0007     struct ethnl_req_info       base;
0008 };
0009
0010 struct pause_reply_data {
0011     struct ethnl_reply_data     base;
0012     struct ethtool_pauseparam   pauseparam;
0013     struct ethtool_pause_stats  pausestat;
0014 };
0015
0016 #define PAUSE_REPDATA(__reply_base) \
0017     container_of(__reply_base, struct pause_reply_data, base)
0018
0019 const struct nla_policy ethnl_pause_get_policy[] = {
0020     [ETHTOOL_A_PAUSE_HEADER]        =
0021         NLA_POLICY_NESTED(ethnl_header_policy_stats),
0022 };
0023
0024 static int pause_prepare_data(const struct ethnl_req_info *req_base,
0025                   struct ethnl_reply_data *reply_base,
0026                   struct genl_info *info)
0027 {
0028     struct pause_reply_data *data = PAUSE_REPDATA(reply_base);
0029     struct net_device *dev = reply_base->dev;
0030     int ret;
0031
0032     if (!dev->ethtool_ops->get_pauseparam)
0033         return -EOPNOTSUPP;
0034
0035     ethtool_stats_init((u64 *)&data->pausestat,
0036                sizeof(data->pausestat) / 8);
0037
0038     ret = ethnl_ops_begin(dev);
0039     if (ret < 0)
0040         return ret;
0041     dev->ethtool_ops->get_pauseparam(dev, &data->pauseparam);
0042     if (req_base->flags & ETHTOOL_FLAG_STATS &&
0043         dev->ethtool_ops->get_pause_stats)
0044         dev->ethtool_ops->get_pause_stats(dev, &data->pausestat);
0045     ethnl_ops_complete(dev);
0046
0047     return 0;
0048 }
0049
0050 static int pause_reply_size(const struct ethnl_req_info *req_base,
0051                 const struct ethnl_reply_data *reply_base)
0052 {
0053     int n = nla_total_size(sizeof(u8)) +    /* _PAUSE_AUTONEG */
0054         nla_total_size(sizeof(u8)) +    /* _PAUSE_RX */
0055         nla_total_size(sizeof(u8)); /* _PAUSE_TX */
0056
0057     if (req_base->flags & ETHTOOL_FLAG_STATS)
0058         n += nla_total_size(0) +    /* _PAUSE_STATS */
0059              nla_total_size_64bit(sizeof(u64)) * ETHTOOL_PAUSE_STAT_CNT;
0060     return n;
0061 }
0062
0063 static int ethtool_put_stat(struct sk_buff *skb, u64 val, u16 attrtype,
0064                 u16 padtype)
0065 {
0066     if (val == ETHTOOL_STAT_NOT_SET)
0067         return 0;
0068     if (nla_put_u64_64bit(skb, attrtype, val, padtype))
0069         return -EMSGSIZE;
0070
0071     return 0;
0072 }
0073
0074 static int pause_put_stats(struct sk_buff *skb,
0075                const struct ethtool_pause_stats *pause_stats)
0076 {
0077     const u16 pad = ETHTOOL_A_PAUSE_STAT_PAD;
0078     struct nlattr *nest;
0079
0080     nest = nla_nest_start(skb, ETHTOOL_A_PAUSE_STATS);
0081     if (!nest)
0082         return -EMSGSIZE;
0083
0084     if (ethtool_put_stat(skb, pause_stats->tx_pause_frames,
0085                  ETHTOOL_A_PAUSE_STAT_TX_FRAMES, pad) ||
0086         ethtool_put_stat(skb, pause_stats->rx_pause_frames,
0087                  ETHTOOL_A_PAUSE_STAT_RX_FRAMES, pad))
0088         goto err_cancel;
0089
0090     nla_nest_end(skb, nest);
0091     return 0;
0092
0093 err_cancel:
0094     nla_nest_cancel(skb, nest);
0095     return -EMSGSIZE;
0096 }
0097
0098 static int pause_fill_reply(struct sk_buff *skb,
0099                 const struct ethnl_req_info *req_base,
0100                 const struct ethnl_reply_data *reply_base)
0101 {
0102     const struct pause_reply_data *data = PAUSE_REPDATA(reply_base);
0103     const struct ethtool_pauseparam *pauseparam = &data->pauseparam;
0104
0105     if (nla_put_u8(skb, ETHTOOL_A_PAUSE_AUTONEG, !!pauseparam->autoneg) ||
0106         nla_put_u8(skb, ETHTOOL_A_PAUSE_RX, !!pauseparam->rx_pause) ||
0107         nla_put_u8(skb, ETHTOOL_A_PAUSE_TX, !!pauseparam->tx_pause))
0108         return -EMSGSIZE;
0109
0110     if (req_base->flags & ETHTOOL_FLAG_STATS &&
0111         pause_put_stats(skb, &data->pausestat))
0112         return -EMSGSIZE;
0113
0114     return 0;
0115 }
0116
0117 const struct ethnl_request_ops ethnl_pause_request_ops = {
0118     .request_cmd        = ETHTOOL_MSG_PAUSE_GET,
0119     .reply_cmd      = ETHTOOL_MSG_PAUSE_GET_REPLY,
0120     .hdr_attr       = ETHTOOL_A_PAUSE_HEADER,
0121     .req_info_size      = sizeof(struct pause_req_info),
0122     .reply_data_size    = sizeof(struct pause_reply_data),
0123
0124     .prepare_data       = pause_prepare_data,
0125     .reply_size     = pause_reply_size,
0126     .fill_reply     = pause_fill_reply,
0127 };
0128
0129 /* PAUSE_SET */
0130
0131 const struct nla_policy ethnl_pause_set_policy[] = {
0132     [ETHTOOL_A_PAUSE_HEADER]        =
0133         NLA_POLICY_NESTED(ethnl_header_policy),
0134     [ETHTOOL_A_PAUSE_AUTONEG]       = { .type = NLA_U8 },
0135     [ETHTOOL_A_PAUSE_RX]            = { .type = NLA_U8 },
0136     [ETHTOOL_A_PAUSE_TX]            = { .type = NLA_U8 },
0137 };
0138
0139 int ethnl_set_pause(struct sk_buff *skb, struct genl_info *info)
0140 {
0141     struct ethtool_pauseparam params = {};
0142     struct ethnl_req_info req_info = {};
0143     struct nlattr **tb = info->attrs;
0144     const struct ethtool_ops *ops;
0145     struct net_device *dev;
0146     bool mod = false;
0147     int ret;
0148
0149     ret = ethnl_parse_header_dev_get(&req_info,
0150                      tb[ETHTOOL_A_PAUSE_HEADER],
0151                      genl_info_net(info), info->extack,
0152                      true);
0153     if (ret < 0)
0154         return ret;
0155     dev = req_info.dev;
0156     ops = dev->ethtool_ops;
0157     ret = -EOPNOTSUPP;
0158     if (!ops->get_pauseparam || !ops->set_pauseparam)
0159         goto out_dev;
0160
0161     rtnl_lock();
0162     ret = ethnl_ops_begin(dev);
0163     if (ret < 0)
0164         goto out_rtnl;
0165     ops->get_pauseparam(dev, &params);
0166
0167     ethnl_update_bool32(&params.autoneg, tb[ETHTOOL_A_PAUSE_AUTONEG], &mod);
0168     ethnl_update_bool32(&params.rx_pause, tb[ETHTOOL_A_PAUSE_RX], &mod);
0169     ethnl_update_bool32(&params.tx_pause, tb[ETHTOOL_A_PAUSE_TX], &mod);
0170     ret = 0;
0171     if (!mod)
0172         goto out_ops;
0173
0174     ret = dev->ethtool_ops->set_pauseparam(dev, &params);
0175     if (ret < 0)
0176         goto out_ops;
0177     ethtool_notify(dev, ETHTOOL_MSG_PAUSE_NTF, NULL);
0178
0179 out_ops:
0180     ethnl_ops_complete(dev);
0181 out_rtnl:
0182     rtnl_unlock();
0183 out_dev:
0184     ethnl_parse_header_dev_put(&req_info);
0185     return ret;
0186 }