111ca3c42SAndrew Lunn // SPDX-License-Identifier: GPL-2.0-only 211ca3c42SAndrew Lunn 311ca3c42SAndrew Lunn #include <linux/phy.h> 41dd3f212SAndrew Lunn #include <linux/ethtool_netlink.h> 511ca3c42SAndrew Lunn #include "netlink.h" 611ca3c42SAndrew Lunn #include "common.h" 711ca3c42SAndrew Lunn 811ca3c42SAndrew Lunn /* CABLE_TEST_ACT */ 911ca3c42SAndrew Lunn 1011ca3c42SAndrew Lunn static const struct nla_policy 1111ca3c42SAndrew Lunn cable_test_act_policy[ETHTOOL_A_CABLE_TEST_MAX + 1] = { 1211ca3c42SAndrew Lunn [ETHTOOL_A_CABLE_TEST_UNSPEC] = { .type = NLA_REJECT }, 1311ca3c42SAndrew Lunn [ETHTOOL_A_CABLE_TEST_HEADER] = { .type = NLA_NESTED }, 1411ca3c42SAndrew Lunn }; 1511ca3c42SAndrew Lunn 1611ca3c42SAndrew Lunn int ethnl_act_cable_test(struct sk_buff *skb, struct genl_info *info) 1711ca3c42SAndrew Lunn { 1811ca3c42SAndrew Lunn struct nlattr *tb[ETHTOOL_A_CABLE_TEST_MAX + 1]; 1911ca3c42SAndrew Lunn struct ethnl_req_info req_info = {}; 2011ca3c42SAndrew Lunn struct net_device *dev; 2111ca3c42SAndrew Lunn int ret; 2211ca3c42SAndrew Lunn 2311ca3c42SAndrew Lunn ret = nlmsg_parse(info->nlhdr, GENL_HDRLEN, tb, 2411ca3c42SAndrew Lunn ETHTOOL_A_CABLE_TEST_MAX, 2511ca3c42SAndrew Lunn cable_test_act_policy, info->extack); 2611ca3c42SAndrew Lunn if (ret < 0) 2711ca3c42SAndrew Lunn return ret; 2811ca3c42SAndrew Lunn 2911ca3c42SAndrew Lunn ret = ethnl_parse_header_dev_get(&req_info, 3011ca3c42SAndrew Lunn tb[ETHTOOL_A_CABLE_TEST_HEADER], 3111ca3c42SAndrew Lunn genl_info_net(info), info->extack, 3211ca3c42SAndrew Lunn true); 3311ca3c42SAndrew Lunn if (ret < 0) 3411ca3c42SAndrew Lunn return ret; 3511ca3c42SAndrew Lunn 3611ca3c42SAndrew Lunn dev = req_info.dev; 3711ca3c42SAndrew Lunn if (!dev->phydev) { 3811ca3c42SAndrew Lunn ret = -EOPNOTSUPP; 3911ca3c42SAndrew Lunn goto out_dev_put; 4011ca3c42SAndrew Lunn } 4111ca3c42SAndrew Lunn 4211ca3c42SAndrew Lunn rtnl_lock(); 4311ca3c42SAndrew Lunn ret = ethnl_ops_begin(dev); 4411ca3c42SAndrew Lunn if (ret < 0) 4511ca3c42SAndrew Lunn goto out_rtnl; 4611ca3c42SAndrew Lunn 4711ca3c42SAndrew Lunn ret = phy_start_cable_test(dev->phydev, info->extack); 4811ca3c42SAndrew Lunn 4911ca3c42SAndrew Lunn ethnl_ops_complete(dev); 5011ca3c42SAndrew Lunn out_rtnl: 5111ca3c42SAndrew Lunn rtnl_unlock(); 5211ca3c42SAndrew Lunn out_dev_put: 5311ca3c42SAndrew Lunn dev_put(dev); 5411ca3c42SAndrew Lunn return ret; 5511ca3c42SAndrew Lunn } 561dd3f212SAndrew Lunn 571dd3f212SAndrew Lunn int ethnl_cable_test_alloc(struct phy_device *phydev) 581dd3f212SAndrew Lunn { 591dd3f212SAndrew Lunn int err = -ENOMEM; 601dd3f212SAndrew Lunn 611dd3f212SAndrew Lunn phydev->skb = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); 621dd3f212SAndrew Lunn if (!phydev->skb) 631dd3f212SAndrew Lunn goto out; 641dd3f212SAndrew Lunn 651dd3f212SAndrew Lunn phydev->ehdr = ethnl_bcastmsg_put(phydev->skb, 661dd3f212SAndrew Lunn ETHTOOL_MSG_CABLE_TEST_NTF); 671dd3f212SAndrew Lunn if (!phydev->ehdr) { 681dd3f212SAndrew Lunn err = -EMSGSIZE; 691dd3f212SAndrew Lunn goto out; 701dd3f212SAndrew Lunn } 711dd3f212SAndrew Lunn 721dd3f212SAndrew Lunn err = ethnl_fill_reply_header(phydev->skb, phydev->attached_dev, 731dd3f212SAndrew Lunn ETHTOOL_A_CABLE_TEST_NTF_HEADER); 741dd3f212SAndrew Lunn if (err) 751dd3f212SAndrew Lunn goto out; 761dd3f212SAndrew Lunn 771dd3f212SAndrew Lunn err = nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_TEST_NTF_STATUS, 781dd3f212SAndrew Lunn ETHTOOL_A_CABLE_TEST_NTF_STATUS_COMPLETED); 791dd3f212SAndrew Lunn if (err) 801dd3f212SAndrew Lunn goto out; 811dd3f212SAndrew Lunn 821dd3f212SAndrew Lunn phydev->nest = nla_nest_start(phydev->skb, 831dd3f212SAndrew Lunn ETHTOOL_A_CABLE_TEST_NTF_NEST); 841e2dc145SAndrew Lunn if (!phydev->nest) { 851e2dc145SAndrew Lunn err = -EMSGSIZE; 861dd3f212SAndrew Lunn goto out; 871e2dc145SAndrew Lunn } 881dd3f212SAndrew Lunn 891dd3f212SAndrew Lunn return 0; 901dd3f212SAndrew Lunn 911dd3f212SAndrew Lunn out: 921dd3f212SAndrew Lunn nlmsg_free(phydev->skb); 931e2dc145SAndrew Lunn phydev->skb = NULL; 941dd3f212SAndrew Lunn return err; 951dd3f212SAndrew Lunn } 961dd3f212SAndrew Lunn EXPORT_SYMBOL_GPL(ethnl_cable_test_alloc); 971dd3f212SAndrew Lunn 981dd3f212SAndrew Lunn void ethnl_cable_test_free(struct phy_device *phydev) 991dd3f212SAndrew Lunn { 1001dd3f212SAndrew Lunn nlmsg_free(phydev->skb); 1011e2dc145SAndrew Lunn phydev->skb = NULL; 1021dd3f212SAndrew Lunn } 1031dd3f212SAndrew Lunn EXPORT_SYMBOL_GPL(ethnl_cable_test_free); 1041dd3f212SAndrew Lunn 1051dd3f212SAndrew Lunn void ethnl_cable_test_finished(struct phy_device *phydev) 1061dd3f212SAndrew Lunn { 1071dd3f212SAndrew Lunn nla_nest_end(phydev->skb, phydev->nest); 1081dd3f212SAndrew Lunn 1091dd3f212SAndrew Lunn genlmsg_end(phydev->skb, phydev->ehdr); 1101dd3f212SAndrew Lunn 1111dd3f212SAndrew Lunn ethnl_multicast(phydev->skb, phydev->attached_dev); 1121dd3f212SAndrew Lunn } 1131dd3f212SAndrew Lunn EXPORT_SYMBOL_GPL(ethnl_cable_test_finished); 1141e2dc145SAndrew Lunn 1151e2dc145SAndrew Lunn int ethnl_cable_test_result(struct phy_device *phydev, u8 pair, u8 result) 1161e2dc145SAndrew Lunn { 1171e2dc145SAndrew Lunn struct nlattr *nest; 1181e2dc145SAndrew Lunn int ret = -EMSGSIZE; 1191e2dc145SAndrew Lunn 1201e2dc145SAndrew Lunn nest = nla_nest_start(phydev->skb, ETHTOOL_A_CABLE_NEST_RESULT); 1211e2dc145SAndrew Lunn if (!nest) 1221e2dc145SAndrew Lunn return -EMSGSIZE; 1231e2dc145SAndrew Lunn 1241e2dc145SAndrew Lunn if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_RESULT_PAIR, pair)) 1251e2dc145SAndrew Lunn goto err; 1261e2dc145SAndrew Lunn if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_RESULT_CODE, result)) 1271e2dc145SAndrew Lunn goto err; 1281e2dc145SAndrew Lunn 1291e2dc145SAndrew Lunn nla_nest_end(phydev->skb, nest); 1301e2dc145SAndrew Lunn return 0; 1311e2dc145SAndrew Lunn 1321e2dc145SAndrew Lunn err: 1331e2dc145SAndrew Lunn nla_nest_cancel(phydev->skb, nest); 1341e2dc145SAndrew Lunn return ret; 1351e2dc145SAndrew Lunn } 1361e2dc145SAndrew Lunn EXPORT_SYMBOL_GPL(ethnl_cable_test_result); 1371e2dc145SAndrew Lunn 1381e2dc145SAndrew Lunn int ethnl_cable_test_fault_length(struct phy_device *phydev, u8 pair, u32 cm) 1391e2dc145SAndrew Lunn { 1401e2dc145SAndrew Lunn struct nlattr *nest; 1411e2dc145SAndrew Lunn int ret = -EMSGSIZE; 1421e2dc145SAndrew Lunn 1431e2dc145SAndrew Lunn nest = nla_nest_start(phydev->skb, 1441e2dc145SAndrew Lunn ETHTOOL_A_CABLE_NEST_FAULT_LENGTH); 1451e2dc145SAndrew Lunn if (!nest) 1461e2dc145SAndrew Lunn return -EMSGSIZE; 1471e2dc145SAndrew Lunn 1481e2dc145SAndrew Lunn if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_FAULT_LENGTH_PAIR, pair)) 1491e2dc145SAndrew Lunn goto err; 1501e2dc145SAndrew Lunn if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_FAULT_LENGTH_CM, cm)) 1511e2dc145SAndrew Lunn goto err; 1521e2dc145SAndrew Lunn 1531e2dc145SAndrew Lunn nla_nest_end(phydev->skb, nest); 1541e2dc145SAndrew Lunn return 0; 1551e2dc145SAndrew Lunn 1561e2dc145SAndrew Lunn err: 1571e2dc145SAndrew Lunn nla_nest_cancel(phydev->skb, nest); 1581e2dc145SAndrew Lunn return ret; 1591e2dc145SAndrew Lunn } 1601e2dc145SAndrew Lunn EXPORT_SYMBOL_GPL(ethnl_cable_test_fault_length); 161