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; 804692c90SVladimir Oltean enum ethtool_mac_stats_src src; 97f59fb32SMichal Kubecek }; 107f59fb32SMichal Kubecek 1104692c90SVladimir Oltean #define PAUSE_REQINFO(__req_base) \ 1204692c90SVladimir Oltean container_of(__req_base, struct pause_req_info, base) 1304692c90SVladimir 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), 2604692c90SVladimir Oltean [ETHTOOL_A_PAUSE_STATS_SRC] = 2704692c90SVladimir Oltean NLA_POLICY_MAX(NLA_U32, ETHTOOL_MAC_STATS_SRC_PMAC), 287f59fb32SMichal Kubecek }; 297f59fb32SMichal Kubecek 3004692c90SVladimir Oltean static int pause_parse_request(struct ethnl_req_info *req_base, 3104692c90SVladimir Oltean struct nlattr **tb, 3204692c90SVladimir Oltean struct netlink_ext_ack *extack) 3304692c90SVladimir Oltean { 3404692c90SVladimir Oltean enum ethtool_mac_stats_src src = ETHTOOL_MAC_STATS_SRC_AGGREGATE; 3504692c90SVladimir Oltean struct pause_req_info *req_info = PAUSE_REQINFO(req_base); 3604692c90SVladimir Oltean 3704692c90SVladimir Oltean if (tb[ETHTOOL_A_PAUSE_STATS_SRC]) { 3804692c90SVladimir Oltean if (!(req_base->flags & ETHTOOL_FLAG_STATS)) { 3904692c90SVladimir Oltean NL_SET_ERR_MSG_MOD(extack, 4004692c90SVladimir Oltean "ETHTOOL_FLAG_STATS must be set when requesting a source of stats"); 4104692c90SVladimir Oltean return -EINVAL; 4204692c90SVladimir Oltean } 4304692c90SVladimir Oltean 4404692c90SVladimir Oltean src = nla_get_u32(tb[ETHTOOL_A_PAUSE_STATS_SRC]); 4504692c90SVladimir Oltean } 4604692c90SVladimir Oltean 4704692c90SVladimir Oltean req_info->src = src; 4804692c90SVladimir Oltean 4904692c90SVladimir Oltean return 0; 5004692c90SVladimir Oltean } 5104692c90SVladimir 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 { 5604692c90SVladimir Oltean const struct pause_req_info *req_info = PAUSE_REQINFO(req_base); 57f5be9cafSVladimir Oltean struct netlink_ext_ack *extack = info ? info->extack : NULL; 587f59fb32SMichal Kubecek struct pause_reply_data *data = PAUSE_REPDATA(reply_base); 5904692c90SVladimir Oltean enum ethtool_mac_stats_src src = req_info->src; 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); 6804692c90SVladimir Oltean data->pausestat.src = src; 6916756d3eSJakub Kicinski 707f59fb32SMichal Kubecek ret = ethnl_ops_begin(dev); 717f59fb32SMichal Kubecek if (ret < 0) 727f59fb32SMichal Kubecek return ret; 7304692c90SVladimir Oltean 7404692c90SVladimir Oltean if ((src == ETHTOOL_MAC_STATS_SRC_EMAC || 7504692c90SVladimir Oltean src == ETHTOOL_MAC_STATS_SRC_PMAC) && 7604692c90SVladimir Oltean !__ethtool_dev_mm_supported(dev)) { 7704692c90SVladimir Oltean NL_SET_ERR_MSG_MOD(extack, 7804692c90SVladimir Oltean "Device does not support MAC merge layer"); 7904692c90SVladimir Oltean ethnl_ops_complete(dev); 8004692c90SVladimir Oltean return -EOPNOTSUPP; 8104692c90SVladimir Oltean } 8204692c90SVladimir 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); 8704692c90SVladimir 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 */ 10204692c90SVladimir 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 12404692c90SVladimir Oltean if (nla_put_u32(skb, ETHTOOL_A_PAUSE_STATS_SRC, pause_stats->src)) 12504692c90SVladimir Oltean return -EMSGSIZE; 12604692c90SVladimir 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 164*99132b6eSJakub Kicinski /* PAUSE_SET */ 165*99132b6eSJakub Kicinski 166*99132b6eSJakub Kicinski const struct nla_policy ethnl_pause_set_policy[] = { 167*99132b6eSJakub Kicinski [ETHTOOL_A_PAUSE_HEADER] = 168*99132b6eSJakub Kicinski NLA_POLICY_NESTED(ethnl_header_policy), 169*99132b6eSJakub Kicinski [ETHTOOL_A_PAUSE_AUTONEG] = { .type = NLA_U8 }, 170*99132b6eSJakub Kicinski [ETHTOOL_A_PAUSE_RX] = { .type = NLA_U8 }, 171*99132b6eSJakub Kicinski [ETHTOOL_A_PAUSE_TX] = { .type = NLA_U8 }, 172*99132b6eSJakub Kicinski }; 173*99132b6eSJakub Kicinski 174*99132b6eSJakub Kicinski static int 175*99132b6eSJakub Kicinski ethnl_set_pause_validate(struct ethnl_req_info *req_info, 176*99132b6eSJakub Kicinski struct genl_info *info) 177*99132b6eSJakub Kicinski { 178*99132b6eSJakub Kicinski const struct ethtool_ops *ops = req_info->dev->ethtool_ops; 179*99132b6eSJakub Kicinski 180*99132b6eSJakub Kicinski return ops->get_pauseparam && ops->set_pauseparam ? 1 : -EOPNOTSUPP; 181*99132b6eSJakub Kicinski } 182*99132b6eSJakub Kicinski 183*99132b6eSJakub Kicinski static int 184*99132b6eSJakub Kicinski ethnl_set_pause(struct ethnl_req_info *req_info, struct genl_info *info) 185*99132b6eSJakub Kicinski { 186*99132b6eSJakub Kicinski struct net_device *dev = req_info->dev; 187*99132b6eSJakub Kicinski struct ethtool_pauseparam params = {}; 188*99132b6eSJakub Kicinski struct nlattr **tb = info->attrs; 189*99132b6eSJakub Kicinski bool mod = false; 190*99132b6eSJakub Kicinski int ret; 191*99132b6eSJakub Kicinski 192*99132b6eSJakub Kicinski dev->ethtool_ops->get_pauseparam(dev, ¶ms); 193*99132b6eSJakub Kicinski 194*99132b6eSJakub Kicinski ethnl_update_bool32(¶ms.autoneg, tb[ETHTOOL_A_PAUSE_AUTONEG], &mod); 195*99132b6eSJakub Kicinski ethnl_update_bool32(¶ms.rx_pause, tb[ETHTOOL_A_PAUSE_RX], &mod); 196*99132b6eSJakub Kicinski ethnl_update_bool32(¶ms.tx_pause, tb[ETHTOOL_A_PAUSE_TX], &mod); 197*99132b6eSJakub Kicinski if (!mod) 198*99132b6eSJakub Kicinski return 0; 199*99132b6eSJakub Kicinski 200*99132b6eSJakub Kicinski ret = dev->ethtool_ops->set_pauseparam(dev, ¶ms); 201*99132b6eSJakub Kicinski return ret < 0 ? ret : 1; 202*99132b6eSJakub Kicinski } 203*99132b6eSJakub Kicinski 2047f59fb32SMichal Kubecek const struct ethnl_request_ops ethnl_pause_request_ops = { 2057f59fb32SMichal Kubecek .request_cmd = ETHTOOL_MSG_PAUSE_GET, 2067f59fb32SMichal Kubecek .reply_cmd = ETHTOOL_MSG_PAUSE_GET_REPLY, 2077f59fb32SMichal Kubecek .hdr_attr = ETHTOOL_A_PAUSE_HEADER, 2087f59fb32SMichal Kubecek .req_info_size = sizeof(struct pause_req_info), 2097f59fb32SMichal Kubecek .reply_data_size = sizeof(struct pause_reply_data), 2107f59fb32SMichal Kubecek 21104692c90SVladimir Oltean .parse_request = pause_parse_request, 2127f59fb32SMichal Kubecek .prepare_data = pause_prepare_data, 2137f59fb32SMichal Kubecek .reply_size = pause_reply_size, 2147f59fb32SMichal Kubecek .fill_reply = pause_fill_reply, 215*99132b6eSJakub Kicinski 216*99132b6eSJakub Kicinski .set_validate = ethnl_set_pause_validate, 217*99132b6eSJakub Kicinski .set = ethnl_set_pause, 218*99132b6eSJakub Kicinski .set_ntf_cmd = ETHTOOL_MSG_PAUSE_NTF, 2197f59fb32SMichal Kubecek }; 220