17f59fb32SMichal Kubecek // SPDX-License-Identifier: GPL-2.0-only 27f59fb32SMichal Kubecek 37f59fb32SMichal Kubecek #include "netlink.h" 47f59fb32SMichal Kubecek #include "common.h" 57f59fb32SMichal Kubecek 67f59fb32SMichal Kubecek struct pause_req_info { 77f59fb32SMichal Kubecek struct ethnl_req_info base; 87f59fb32SMichal Kubecek }; 97f59fb32SMichal Kubecek 107f59fb32SMichal Kubecek struct pause_reply_data { 117f59fb32SMichal Kubecek struct ethnl_reply_data base; 127f59fb32SMichal Kubecek struct ethtool_pauseparam pauseparam; 139a27a330SJakub Kicinski struct ethtool_pause_stats pausestat; 147f59fb32SMichal Kubecek }; 157f59fb32SMichal Kubecek 167f59fb32SMichal Kubecek #define PAUSE_REPDATA(__reply_base) \ 177f59fb32SMichal Kubecek container_of(__reply_base, struct pause_reply_data, base) 187f59fb32SMichal Kubecek 197f59fb32SMichal Kubecek static const struct nla_policy 207f59fb32SMichal Kubecek pause_get_policy[ETHTOOL_A_PAUSE_MAX + 1] = { 217f59fb32SMichal Kubecek [ETHTOOL_A_PAUSE_UNSPEC] = { .type = NLA_REJECT }, 227f59fb32SMichal Kubecek [ETHTOOL_A_PAUSE_HEADER] = { .type = NLA_NESTED }, 237f59fb32SMichal Kubecek [ETHTOOL_A_PAUSE_AUTONEG] = { .type = NLA_REJECT }, 247f59fb32SMichal Kubecek [ETHTOOL_A_PAUSE_RX] = { .type = NLA_REJECT }, 257f59fb32SMichal Kubecek [ETHTOOL_A_PAUSE_TX] = { .type = NLA_REJECT }, 269a27a330SJakub Kicinski [ETHTOOL_A_PAUSE_STATS] = { .type = NLA_REJECT }, 277f59fb32SMichal Kubecek }; 287f59fb32SMichal Kubecek 299a27a330SJakub Kicinski static void ethtool_stats_init(u64 *stats, unsigned int n) 309a27a330SJakub Kicinski { 319a27a330SJakub Kicinski while (n--) 329a27a330SJakub Kicinski stats[n] = ETHTOOL_STAT_NOT_SET; 339a27a330SJakub Kicinski } 349a27a330SJakub Kicinski 357f59fb32SMichal Kubecek static int pause_prepare_data(const struct ethnl_req_info *req_base, 367f59fb32SMichal Kubecek struct ethnl_reply_data *reply_base, 377f59fb32SMichal Kubecek struct genl_info *info) 387f59fb32SMichal Kubecek { 397f59fb32SMichal Kubecek struct pause_reply_data *data = PAUSE_REPDATA(reply_base); 407f59fb32SMichal Kubecek struct net_device *dev = reply_base->dev; 417f59fb32SMichal Kubecek int ret; 427f59fb32SMichal Kubecek 437f59fb32SMichal Kubecek if (!dev->ethtool_ops->get_pauseparam) 447f59fb32SMichal Kubecek return -EOPNOTSUPP; 459a27a330SJakub Kicinski 467f59fb32SMichal Kubecek ret = ethnl_ops_begin(dev); 477f59fb32SMichal Kubecek if (ret < 0) 487f59fb32SMichal Kubecek return ret; 497f59fb32SMichal Kubecek dev->ethtool_ops->get_pauseparam(dev, &data->pauseparam); 509a27a330SJakub Kicinski if (req_base->flags & ETHTOOL_FLAG_STATS && 519a27a330SJakub Kicinski dev->ethtool_ops->get_pause_stats) { 529a27a330SJakub Kicinski ethtool_stats_init((u64 *)&data->pausestat, 539a27a330SJakub Kicinski sizeof(data->pausestat) / 8); 549a27a330SJakub Kicinski dev->ethtool_ops->get_pause_stats(dev, &data->pausestat); 559a27a330SJakub Kicinski } 567f59fb32SMichal Kubecek ethnl_ops_complete(dev); 577f59fb32SMichal Kubecek 587f59fb32SMichal Kubecek return 0; 597f59fb32SMichal Kubecek } 607f59fb32SMichal Kubecek 617f59fb32SMichal Kubecek static int pause_reply_size(const struct ethnl_req_info *req_base, 627f59fb32SMichal Kubecek const struct ethnl_reply_data *reply_base) 637f59fb32SMichal Kubecek { 649a27a330SJakub Kicinski int n = nla_total_size(sizeof(u8)) + /* _PAUSE_AUTONEG */ 657f59fb32SMichal Kubecek nla_total_size(sizeof(u8)) + /* _PAUSE_RX */ 667f59fb32SMichal Kubecek nla_total_size(sizeof(u8)); /* _PAUSE_TX */ 679a27a330SJakub Kicinski 689a27a330SJakub Kicinski if (req_base->flags & ETHTOOL_FLAG_STATS) 699a27a330SJakub Kicinski n += nla_total_size(0) + /* _PAUSE_STATS */ 709a27a330SJakub Kicinski nla_total_size_64bit(sizeof(u64)) * 719a27a330SJakub Kicinski (ETHTOOL_A_PAUSE_STAT_MAX - 2); 729a27a330SJakub Kicinski return n; 739a27a330SJakub Kicinski } 749a27a330SJakub Kicinski 759a27a330SJakub Kicinski static int ethtool_put_stat(struct sk_buff *skb, u64 val, u16 attrtype, 769a27a330SJakub Kicinski u16 padtype) 779a27a330SJakub Kicinski { 789a27a330SJakub Kicinski if (val == ETHTOOL_STAT_NOT_SET) 799a27a330SJakub Kicinski return 0; 809a27a330SJakub Kicinski if (nla_put_u64_64bit(skb, attrtype, val, padtype)) 819a27a330SJakub Kicinski return -EMSGSIZE; 829a27a330SJakub Kicinski 839a27a330SJakub Kicinski return 0; 849a27a330SJakub Kicinski } 859a27a330SJakub Kicinski 869a27a330SJakub Kicinski static int pause_put_stats(struct sk_buff *skb, 879a27a330SJakub Kicinski const struct ethtool_pause_stats *pause_stats) 889a27a330SJakub Kicinski { 899a27a330SJakub Kicinski const u16 pad = ETHTOOL_A_PAUSE_STAT_PAD; 909a27a330SJakub Kicinski struct nlattr *nest; 919a27a330SJakub Kicinski 929a27a330SJakub Kicinski nest = nla_nest_start(skb, ETHTOOL_A_PAUSE_STATS); 939a27a330SJakub Kicinski if (!nest) 949a27a330SJakub Kicinski return -EMSGSIZE; 959a27a330SJakub Kicinski 969a27a330SJakub Kicinski if (ethtool_put_stat(skb, pause_stats->tx_pause_frames, 979a27a330SJakub Kicinski ETHTOOL_A_PAUSE_STAT_TX_FRAMES, pad) || 989a27a330SJakub Kicinski ethtool_put_stat(skb, pause_stats->rx_pause_frames, 999a27a330SJakub Kicinski ETHTOOL_A_PAUSE_STAT_RX_FRAMES, pad)) 1009a27a330SJakub Kicinski goto err_cancel; 1019a27a330SJakub Kicinski 1029a27a330SJakub Kicinski nla_nest_end(skb, nest); 1039a27a330SJakub Kicinski return 0; 1049a27a330SJakub Kicinski 1059a27a330SJakub Kicinski err_cancel: 1069a27a330SJakub Kicinski nla_nest_cancel(skb, nest); 1079a27a330SJakub Kicinski return -EMSGSIZE; 1087f59fb32SMichal Kubecek } 1097f59fb32SMichal Kubecek 1107f59fb32SMichal Kubecek static int pause_fill_reply(struct sk_buff *skb, 1117f59fb32SMichal Kubecek const struct ethnl_req_info *req_base, 1127f59fb32SMichal Kubecek const struct ethnl_reply_data *reply_base) 1137f59fb32SMichal Kubecek { 1147f59fb32SMichal Kubecek const struct pause_reply_data *data = PAUSE_REPDATA(reply_base); 1157f59fb32SMichal Kubecek const struct ethtool_pauseparam *pauseparam = &data->pauseparam; 1167f59fb32SMichal Kubecek 1177f59fb32SMichal Kubecek if (nla_put_u8(skb, ETHTOOL_A_PAUSE_AUTONEG, !!pauseparam->autoneg) || 1187f59fb32SMichal Kubecek nla_put_u8(skb, ETHTOOL_A_PAUSE_RX, !!pauseparam->rx_pause) || 1197f59fb32SMichal Kubecek nla_put_u8(skb, ETHTOOL_A_PAUSE_TX, !!pauseparam->tx_pause)) 1207f59fb32SMichal Kubecek return -EMSGSIZE; 1217f59fb32SMichal Kubecek 1229a27a330SJakub Kicinski if (req_base->flags & ETHTOOL_FLAG_STATS && 1239a27a330SJakub Kicinski pause_put_stats(skb, &data->pausestat)) 1249a27a330SJakub Kicinski return -EMSGSIZE; 1259a27a330SJakub Kicinski 1267f59fb32SMichal Kubecek return 0; 1277f59fb32SMichal Kubecek } 1287f59fb32SMichal Kubecek 1297f59fb32SMichal Kubecek const struct ethnl_request_ops ethnl_pause_request_ops = { 1307f59fb32SMichal Kubecek .request_cmd = ETHTOOL_MSG_PAUSE_GET, 1317f59fb32SMichal Kubecek .reply_cmd = ETHTOOL_MSG_PAUSE_GET_REPLY, 1327f59fb32SMichal Kubecek .hdr_attr = ETHTOOL_A_PAUSE_HEADER, 1337f59fb32SMichal Kubecek .max_attr = ETHTOOL_A_PAUSE_MAX, 1347f59fb32SMichal Kubecek .req_info_size = sizeof(struct pause_req_info), 1357f59fb32SMichal Kubecek .reply_data_size = sizeof(struct pause_reply_data), 1367f59fb32SMichal Kubecek .request_policy = pause_get_policy, 1377f59fb32SMichal Kubecek 1387f59fb32SMichal Kubecek .prepare_data = pause_prepare_data, 1397f59fb32SMichal Kubecek .reply_size = pause_reply_size, 1407f59fb32SMichal Kubecek .fill_reply = pause_fill_reply, 1417f59fb32SMichal Kubecek }; 1423ab87993SMichal Kubecek 1433ab87993SMichal Kubecek /* PAUSE_SET */ 1443ab87993SMichal Kubecek 1453ab87993SMichal Kubecek static const struct nla_policy 1463ab87993SMichal Kubecek pause_set_policy[ETHTOOL_A_PAUSE_MAX + 1] = { 1473ab87993SMichal Kubecek [ETHTOOL_A_PAUSE_UNSPEC] = { .type = NLA_REJECT }, 1483ab87993SMichal Kubecek [ETHTOOL_A_PAUSE_HEADER] = { .type = NLA_NESTED }, 1493ab87993SMichal Kubecek [ETHTOOL_A_PAUSE_AUTONEG] = { .type = NLA_U8 }, 1503ab87993SMichal Kubecek [ETHTOOL_A_PAUSE_RX] = { .type = NLA_U8 }, 1513ab87993SMichal Kubecek [ETHTOOL_A_PAUSE_TX] = { .type = NLA_U8 }, 1529a27a330SJakub Kicinski [ETHTOOL_A_PAUSE_STATS] = { .type = NLA_REJECT }, 1533ab87993SMichal Kubecek }; 1543ab87993SMichal Kubecek 1553ab87993SMichal Kubecek int ethnl_set_pause(struct sk_buff *skb, struct genl_info *info) 1563ab87993SMichal Kubecek { 1573ab87993SMichal Kubecek struct nlattr *tb[ETHTOOL_A_PAUSE_MAX + 1]; 1583ab87993SMichal Kubecek struct ethtool_pauseparam params = {}; 1593ab87993SMichal Kubecek struct ethnl_req_info req_info = {}; 1603ab87993SMichal Kubecek const struct ethtool_ops *ops; 1613ab87993SMichal Kubecek struct net_device *dev; 1623ab87993SMichal Kubecek bool mod = false; 1633ab87993SMichal Kubecek int ret; 1643ab87993SMichal Kubecek 1653ab87993SMichal Kubecek ret = nlmsg_parse(info->nlhdr, GENL_HDRLEN, tb, ETHTOOL_A_PAUSE_MAX, 1663ab87993SMichal Kubecek pause_set_policy, info->extack); 1673ab87993SMichal Kubecek if (ret < 0) 1683ab87993SMichal Kubecek return ret; 1693ab87993SMichal Kubecek ret = ethnl_parse_header_dev_get(&req_info, 1703ab87993SMichal Kubecek tb[ETHTOOL_A_PAUSE_HEADER], 1713ab87993SMichal Kubecek genl_info_net(info), info->extack, 1723ab87993SMichal Kubecek true); 1733ab87993SMichal Kubecek if (ret < 0) 1743ab87993SMichal Kubecek return ret; 1753ab87993SMichal Kubecek dev = req_info.dev; 1763ab87993SMichal Kubecek ops = dev->ethtool_ops; 1773ab87993SMichal Kubecek ret = -EOPNOTSUPP; 1783ab87993SMichal Kubecek if (!ops->get_pauseparam || !ops->set_pauseparam) 1793ab87993SMichal Kubecek goto out_dev; 1803ab87993SMichal Kubecek 1813ab87993SMichal Kubecek rtnl_lock(); 1823ab87993SMichal Kubecek ret = ethnl_ops_begin(dev); 1833ab87993SMichal Kubecek if (ret < 0) 1843ab87993SMichal Kubecek goto out_rtnl; 1853ab87993SMichal Kubecek ops->get_pauseparam(dev, ¶ms); 1863ab87993SMichal Kubecek 1873ab87993SMichal Kubecek ethnl_update_bool32(¶ms.autoneg, tb[ETHTOOL_A_PAUSE_AUTONEG], &mod); 1883ab87993SMichal Kubecek ethnl_update_bool32(¶ms.rx_pause, tb[ETHTOOL_A_PAUSE_RX], &mod); 1893ab87993SMichal Kubecek ethnl_update_bool32(¶ms.tx_pause, tb[ETHTOOL_A_PAUSE_TX], &mod); 1903ab87993SMichal Kubecek ret = 0; 1913ab87993SMichal Kubecek if (!mod) 1923ab87993SMichal Kubecek goto out_ops; 1933ab87993SMichal Kubecek 1943ab87993SMichal Kubecek ret = dev->ethtool_ops->set_pauseparam(dev, ¶ms); 195bf37faa3SMichal Kubecek if (ret < 0) 196bf37faa3SMichal Kubecek goto out_ops; 197bf37faa3SMichal Kubecek ethtool_notify(dev, ETHTOOL_MSG_PAUSE_NTF, NULL); 1983ab87993SMichal Kubecek 1993ab87993SMichal Kubecek out_ops: 2003ab87993SMichal Kubecek ethnl_ops_complete(dev); 2013ab87993SMichal Kubecek out_rtnl: 2023ab87993SMichal Kubecek rtnl_unlock(); 2033ab87993SMichal Kubecek out_dev: 2043ab87993SMichal Kubecek dev_put(dev); 2053ab87993SMichal Kubecek return ret; 2063ab87993SMichal Kubecek } 207