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; 8*04692c90SVladimir Oltean enum ethtool_mac_stats_src src; 97f59fb32SMichal Kubecek }; 107f59fb32SMichal Kubecek 11*04692c90SVladimir Oltean #define PAUSE_REQINFO(__req_base) \ 12*04692c90SVladimir Oltean container_of(__req_base, struct pause_req_info, base) 13*04692c90SVladimir Oltean 147f59fb32SMichal Kubecek struct pause_reply_data { 157f59fb32SMichal Kubecek struct ethnl_reply_data base; 167f59fb32SMichal Kubecek struct ethtool_pauseparam pauseparam; 179a27a330SJakub Kicinski struct ethtool_pause_stats pausestat; 187f59fb32SMichal Kubecek }; 197f59fb32SMichal Kubecek 207f59fb32SMichal Kubecek #define PAUSE_REPDATA(__reply_base) \ 217f59fb32SMichal Kubecek container_of(__reply_base, struct pause_reply_data, base) 227f59fb32SMichal Kubecek 23ff419afaSJakub Kicinski const struct nla_policy ethnl_pause_get_policy[] = { 24329d9c33SJakub Kicinski [ETHTOOL_A_PAUSE_HEADER] = 25a0de1cd3SJakub Kicinski NLA_POLICY_NESTED(ethnl_header_policy_stats), 26*04692c90SVladimir Oltean [ETHTOOL_A_PAUSE_STATS_SRC] = 27*04692c90SVladimir Oltean NLA_POLICY_MAX(NLA_U32, ETHTOOL_MAC_STATS_SRC_PMAC), 287f59fb32SMichal Kubecek }; 297f59fb32SMichal Kubecek 30*04692c90SVladimir Oltean static int pause_parse_request(struct ethnl_req_info *req_base, 31*04692c90SVladimir Oltean struct nlattr **tb, 32*04692c90SVladimir Oltean struct netlink_ext_ack *extack) 33*04692c90SVladimir Oltean { 34*04692c90SVladimir Oltean enum ethtool_mac_stats_src src = ETHTOOL_MAC_STATS_SRC_AGGREGATE; 35*04692c90SVladimir Oltean struct pause_req_info *req_info = PAUSE_REQINFO(req_base); 36*04692c90SVladimir Oltean 37*04692c90SVladimir Oltean if (tb[ETHTOOL_A_PAUSE_STATS_SRC]) { 38*04692c90SVladimir Oltean if (!(req_base->flags & ETHTOOL_FLAG_STATS)) { 39*04692c90SVladimir Oltean NL_SET_ERR_MSG_MOD(extack, 40*04692c90SVladimir Oltean "ETHTOOL_FLAG_STATS must be set when requesting a source of stats"); 41*04692c90SVladimir Oltean return -EINVAL; 42*04692c90SVladimir Oltean } 43*04692c90SVladimir Oltean 44*04692c90SVladimir Oltean src = nla_get_u32(tb[ETHTOOL_A_PAUSE_STATS_SRC]); 45*04692c90SVladimir Oltean } 46*04692c90SVladimir Oltean 47*04692c90SVladimir Oltean req_info->src = src; 48*04692c90SVladimir Oltean 49*04692c90SVladimir Oltean return 0; 50*04692c90SVladimir Oltean } 51*04692c90SVladimir Oltean 527f59fb32SMichal Kubecek static int pause_prepare_data(const struct ethnl_req_info *req_base, 537f59fb32SMichal Kubecek struct ethnl_reply_data *reply_base, 547f59fb32SMichal Kubecek struct genl_info *info) 557f59fb32SMichal Kubecek { 56*04692c90SVladimir Oltean const struct pause_req_info *req_info = PAUSE_REQINFO(req_base); 577f59fb32SMichal Kubecek struct pause_reply_data *data = PAUSE_REPDATA(reply_base); 58*04692c90SVladimir Oltean enum ethtool_mac_stats_src src = req_info->src; 59*04692c90SVladimir Oltean struct netlink_ext_ack *extack = info->extack; 607f59fb32SMichal Kubecek struct net_device *dev = reply_base->dev; 617f59fb32SMichal Kubecek int ret; 627f59fb32SMichal Kubecek 637f59fb32SMichal Kubecek if (!dev->ethtool_ops->get_pauseparam) 647f59fb32SMichal Kubecek return -EOPNOTSUPP; 659a27a330SJakub Kicinski 6616756d3eSJakub Kicinski ethtool_stats_init((u64 *)&data->pausestat, 6716756d3eSJakub Kicinski sizeof(data->pausestat) / 8); 68*04692c90SVladimir Oltean data->pausestat.src = src; 6916756d3eSJakub Kicinski 707f59fb32SMichal Kubecek ret = ethnl_ops_begin(dev); 717f59fb32SMichal Kubecek if (ret < 0) 727f59fb32SMichal Kubecek return ret; 73*04692c90SVladimir Oltean 74*04692c90SVladimir Oltean if ((src == ETHTOOL_MAC_STATS_SRC_EMAC || 75*04692c90SVladimir Oltean src == ETHTOOL_MAC_STATS_SRC_PMAC) && 76*04692c90SVladimir Oltean !__ethtool_dev_mm_supported(dev)) { 77*04692c90SVladimir Oltean NL_SET_ERR_MSG_MOD(extack, 78*04692c90SVladimir Oltean "Device does not support MAC merge layer"); 79*04692c90SVladimir Oltean ethnl_ops_complete(dev); 80*04692c90SVladimir Oltean return -EOPNOTSUPP; 81*04692c90SVladimir Oltean } 82*04692c90SVladimir Oltean 837f59fb32SMichal Kubecek dev->ethtool_ops->get_pauseparam(dev, &data->pauseparam); 849a27a330SJakub Kicinski if (req_base->flags & ETHTOOL_FLAG_STATS && 8516756d3eSJakub Kicinski dev->ethtool_ops->get_pause_stats) 869a27a330SJakub Kicinski dev->ethtool_ops->get_pause_stats(dev, &data->pausestat); 87*04692c90SVladimir Oltean 887f59fb32SMichal Kubecek ethnl_ops_complete(dev); 897f59fb32SMichal Kubecek 907f59fb32SMichal Kubecek return 0; 917f59fb32SMichal Kubecek } 927f59fb32SMichal Kubecek 937f59fb32SMichal Kubecek static int pause_reply_size(const struct ethnl_req_info *req_base, 947f59fb32SMichal Kubecek const struct ethnl_reply_data *reply_base) 957f59fb32SMichal Kubecek { 969a27a330SJakub Kicinski int n = nla_total_size(sizeof(u8)) + /* _PAUSE_AUTONEG */ 977f59fb32SMichal Kubecek nla_total_size(sizeof(u8)) + /* _PAUSE_RX */ 987f59fb32SMichal Kubecek nla_total_size(sizeof(u8)); /* _PAUSE_TX */ 999a27a330SJakub Kicinski 1009a27a330SJakub Kicinski if (req_base->flags & ETHTOOL_FLAG_STATS) 1019a27a330SJakub Kicinski n += nla_total_size(0) + /* _PAUSE_STATS */ 102*04692c90SVladimir Oltean nla_total_size(sizeof(u32)) + /* _PAUSE_STATS_SRC */ 1031aabe578SJakub Kicinski nla_total_size_64bit(sizeof(u64)) * ETHTOOL_PAUSE_STAT_CNT; 1049a27a330SJakub Kicinski return n; 1059a27a330SJakub Kicinski } 1069a27a330SJakub Kicinski 1079a27a330SJakub Kicinski static int ethtool_put_stat(struct sk_buff *skb, u64 val, u16 attrtype, 1089a27a330SJakub Kicinski u16 padtype) 1099a27a330SJakub Kicinski { 1109a27a330SJakub Kicinski if (val == ETHTOOL_STAT_NOT_SET) 1119a27a330SJakub Kicinski return 0; 1129a27a330SJakub Kicinski if (nla_put_u64_64bit(skb, attrtype, val, padtype)) 1139a27a330SJakub Kicinski return -EMSGSIZE; 1149a27a330SJakub Kicinski 1159a27a330SJakub Kicinski return 0; 1169a27a330SJakub Kicinski } 1179a27a330SJakub Kicinski 1189a27a330SJakub Kicinski static int pause_put_stats(struct sk_buff *skb, 1199a27a330SJakub Kicinski const struct ethtool_pause_stats *pause_stats) 1209a27a330SJakub Kicinski { 1219a27a330SJakub Kicinski const u16 pad = ETHTOOL_A_PAUSE_STAT_PAD; 1229a27a330SJakub Kicinski struct nlattr *nest; 1239a27a330SJakub Kicinski 124*04692c90SVladimir Oltean if (nla_put_u32(skb, ETHTOOL_A_PAUSE_STATS_SRC, pause_stats->src)) 125*04692c90SVladimir Oltean return -EMSGSIZE; 126*04692c90SVladimir Oltean 1279a27a330SJakub Kicinski nest = nla_nest_start(skb, ETHTOOL_A_PAUSE_STATS); 1289a27a330SJakub Kicinski if (!nest) 1299a27a330SJakub Kicinski return -EMSGSIZE; 1309a27a330SJakub Kicinski 1319a27a330SJakub Kicinski if (ethtool_put_stat(skb, pause_stats->tx_pause_frames, 1329a27a330SJakub Kicinski ETHTOOL_A_PAUSE_STAT_TX_FRAMES, pad) || 1339a27a330SJakub Kicinski ethtool_put_stat(skb, pause_stats->rx_pause_frames, 1349a27a330SJakub Kicinski ETHTOOL_A_PAUSE_STAT_RX_FRAMES, pad)) 1359a27a330SJakub Kicinski goto err_cancel; 1369a27a330SJakub Kicinski 1379a27a330SJakub Kicinski nla_nest_end(skb, nest); 1389a27a330SJakub Kicinski return 0; 1399a27a330SJakub Kicinski 1409a27a330SJakub Kicinski err_cancel: 1419a27a330SJakub Kicinski nla_nest_cancel(skb, nest); 1429a27a330SJakub Kicinski return -EMSGSIZE; 1437f59fb32SMichal Kubecek } 1447f59fb32SMichal Kubecek 1457f59fb32SMichal Kubecek static int pause_fill_reply(struct sk_buff *skb, 1467f59fb32SMichal Kubecek const struct ethnl_req_info *req_base, 1477f59fb32SMichal Kubecek const struct ethnl_reply_data *reply_base) 1487f59fb32SMichal Kubecek { 1497f59fb32SMichal Kubecek const struct pause_reply_data *data = PAUSE_REPDATA(reply_base); 1507f59fb32SMichal Kubecek const struct ethtool_pauseparam *pauseparam = &data->pauseparam; 1517f59fb32SMichal Kubecek 1527f59fb32SMichal Kubecek if (nla_put_u8(skb, ETHTOOL_A_PAUSE_AUTONEG, !!pauseparam->autoneg) || 1537f59fb32SMichal Kubecek nla_put_u8(skb, ETHTOOL_A_PAUSE_RX, !!pauseparam->rx_pause) || 1547f59fb32SMichal Kubecek nla_put_u8(skb, ETHTOOL_A_PAUSE_TX, !!pauseparam->tx_pause)) 1557f59fb32SMichal Kubecek return -EMSGSIZE; 1567f59fb32SMichal Kubecek 1579a27a330SJakub Kicinski if (req_base->flags & ETHTOOL_FLAG_STATS && 1589a27a330SJakub Kicinski pause_put_stats(skb, &data->pausestat)) 1599a27a330SJakub Kicinski return -EMSGSIZE; 1609a27a330SJakub Kicinski 1617f59fb32SMichal Kubecek return 0; 1627f59fb32SMichal Kubecek } 1637f59fb32SMichal Kubecek 1647f59fb32SMichal Kubecek const struct ethnl_request_ops ethnl_pause_request_ops = { 1657f59fb32SMichal Kubecek .request_cmd = ETHTOOL_MSG_PAUSE_GET, 1667f59fb32SMichal Kubecek .reply_cmd = ETHTOOL_MSG_PAUSE_GET_REPLY, 1677f59fb32SMichal Kubecek .hdr_attr = ETHTOOL_A_PAUSE_HEADER, 1687f59fb32SMichal Kubecek .req_info_size = sizeof(struct pause_req_info), 1697f59fb32SMichal Kubecek .reply_data_size = sizeof(struct pause_reply_data), 1707f59fb32SMichal Kubecek 171*04692c90SVladimir Oltean .parse_request = pause_parse_request, 1727f59fb32SMichal Kubecek .prepare_data = pause_prepare_data, 1737f59fb32SMichal Kubecek .reply_size = pause_reply_size, 1747f59fb32SMichal Kubecek .fill_reply = pause_fill_reply, 1757f59fb32SMichal Kubecek }; 1763ab87993SMichal Kubecek 1773ab87993SMichal Kubecek /* PAUSE_SET */ 1783ab87993SMichal Kubecek 179ff419afaSJakub Kicinski const struct nla_policy ethnl_pause_set_policy[] = { 180329d9c33SJakub Kicinski [ETHTOOL_A_PAUSE_HEADER] = 181329d9c33SJakub Kicinski NLA_POLICY_NESTED(ethnl_header_policy), 1823ab87993SMichal Kubecek [ETHTOOL_A_PAUSE_AUTONEG] = { .type = NLA_U8 }, 1833ab87993SMichal Kubecek [ETHTOOL_A_PAUSE_RX] = { .type = NLA_U8 }, 1843ab87993SMichal Kubecek [ETHTOOL_A_PAUSE_TX] = { .type = NLA_U8 }, 1853ab87993SMichal Kubecek }; 1863ab87993SMichal Kubecek 1873ab87993SMichal Kubecek int ethnl_set_pause(struct sk_buff *skb, struct genl_info *info) 1883ab87993SMichal Kubecek { 1893ab87993SMichal Kubecek struct ethtool_pauseparam params = {}; 1903ab87993SMichal Kubecek struct ethnl_req_info req_info = {}; 1915028588bSJakub Kicinski struct nlattr **tb = info->attrs; 1923ab87993SMichal Kubecek const struct ethtool_ops *ops; 1933ab87993SMichal Kubecek struct net_device *dev; 1943ab87993SMichal Kubecek bool mod = false; 1953ab87993SMichal Kubecek int ret; 1963ab87993SMichal Kubecek 1973ab87993SMichal Kubecek ret = ethnl_parse_header_dev_get(&req_info, 1983ab87993SMichal Kubecek tb[ETHTOOL_A_PAUSE_HEADER], 1993ab87993SMichal Kubecek genl_info_net(info), info->extack, 2003ab87993SMichal Kubecek true); 2013ab87993SMichal Kubecek if (ret < 0) 2023ab87993SMichal Kubecek return ret; 2033ab87993SMichal Kubecek dev = req_info.dev; 2043ab87993SMichal Kubecek ops = dev->ethtool_ops; 2053ab87993SMichal Kubecek ret = -EOPNOTSUPP; 2063ab87993SMichal Kubecek if (!ops->get_pauseparam || !ops->set_pauseparam) 2073ab87993SMichal Kubecek goto out_dev; 2083ab87993SMichal Kubecek 2093ab87993SMichal Kubecek rtnl_lock(); 2103ab87993SMichal Kubecek ret = ethnl_ops_begin(dev); 2113ab87993SMichal Kubecek if (ret < 0) 2123ab87993SMichal Kubecek goto out_rtnl; 2133ab87993SMichal Kubecek ops->get_pauseparam(dev, ¶ms); 2143ab87993SMichal Kubecek 2153ab87993SMichal Kubecek ethnl_update_bool32(¶ms.autoneg, tb[ETHTOOL_A_PAUSE_AUTONEG], &mod); 2163ab87993SMichal Kubecek ethnl_update_bool32(¶ms.rx_pause, tb[ETHTOOL_A_PAUSE_RX], &mod); 2173ab87993SMichal Kubecek ethnl_update_bool32(¶ms.tx_pause, tb[ETHTOOL_A_PAUSE_TX], &mod); 2183ab87993SMichal Kubecek ret = 0; 2193ab87993SMichal Kubecek if (!mod) 2203ab87993SMichal Kubecek goto out_ops; 2213ab87993SMichal Kubecek 2223ab87993SMichal Kubecek ret = dev->ethtool_ops->set_pauseparam(dev, ¶ms); 223bf37faa3SMichal Kubecek if (ret < 0) 224bf37faa3SMichal Kubecek goto out_ops; 225bf37faa3SMichal Kubecek ethtool_notify(dev, ETHTOOL_MSG_PAUSE_NTF, NULL); 2263ab87993SMichal Kubecek 2273ab87993SMichal Kubecek out_ops: 2283ab87993SMichal Kubecek ethnl_ops_complete(dev); 2293ab87993SMichal Kubecek out_rtnl: 2303ab87993SMichal Kubecek rtnl_unlock(); 2313ab87993SMichal Kubecek out_dev: 23234ac17ecSEric Dumazet ethnl_parse_header_dev_put(&req_info); 2333ab87993SMichal Kubecek return ret; 2343ab87993SMichal Kubecek } 235