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 161a644de2SAndrew Lunn static int ethnl_cable_test_started(struct phy_device *phydev, u8 cmd) 179896a457SAndrew Lunn { 189896a457SAndrew Lunn struct sk_buff *skb; 199896a457SAndrew Lunn int err = -ENOMEM; 209896a457SAndrew Lunn void *ehdr; 219896a457SAndrew Lunn 229896a457SAndrew Lunn skb = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); 239896a457SAndrew Lunn if (!skb) 249896a457SAndrew Lunn goto out; 259896a457SAndrew Lunn 261a644de2SAndrew Lunn ehdr = ethnl_bcastmsg_put(skb, cmd); 279896a457SAndrew Lunn if (!ehdr) { 289896a457SAndrew Lunn err = -EMSGSIZE; 299896a457SAndrew Lunn goto out; 309896a457SAndrew Lunn } 319896a457SAndrew Lunn 329896a457SAndrew Lunn err = ethnl_fill_reply_header(skb, phydev->attached_dev, 339896a457SAndrew Lunn ETHTOOL_A_CABLE_TEST_NTF_HEADER); 349896a457SAndrew Lunn if (err) 359896a457SAndrew Lunn goto out; 369896a457SAndrew Lunn 379896a457SAndrew Lunn err = nla_put_u8(skb, ETHTOOL_A_CABLE_TEST_NTF_STATUS, 389896a457SAndrew Lunn ETHTOOL_A_CABLE_TEST_NTF_STATUS_STARTED); 399896a457SAndrew Lunn if (err) 409896a457SAndrew Lunn goto out; 419896a457SAndrew Lunn 429896a457SAndrew Lunn genlmsg_end(skb, ehdr); 439896a457SAndrew Lunn 449896a457SAndrew Lunn return ethnl_multicast(skb, phydev->attached_dev); 459896a457SAndrew Lunn 469896a457SAndrew Lunn out: 479896a457SAndrew Lunn nlmsg_free(skb); 489896a457SAndrew Lunn phydev_err(phydev, "%s: Error %pe\n", __func__, ERR_PTR(err)); 499896a457SAndrew Lunn 509896a457SAndrew Lunn return err; 519896a457SAndrew Lunn } 529896a457SAndrew Lunn 5311ca3c42SAndrew Lunn int ethnl_act_cable_test(struct sk_buff *skb, struct genl_info *info) 5411ca3c42SAndrew Lunn { 5511ca3c42SAndrew Lunn struct nlattr *tb[ETHTOOL_A_CABLE_TEST_MAX + 1]; 5611ca3c42SAndrew Lunn struct ethnl_req_info req_info = {}; 5711ca3c42SAndrew Lunn struct net_device *dev; 5811ca3c42SAndrew Lunn int ret; 5911ca3c42SAndrew Lunn 6011ca3c42SAndrew Lunn ret = nlmsg_parse(info->nlhdr, GENL_HDRLEN, tb, 6111ca3c42SAndrew Lunn ETHTOOL_A_CABLE_TEST_MAX, 6211ca3c42SAndrew Lunn cable_test_act_policy, info->extack); 6311ca3c42SAndrew Lunn if (ret < 0) 6411ca3c42SAndrew Lunn return ret; 6511ca3c42SAndrew Lunn 6611ca3c42SAndrew Lunn ret = ethnl_parse_header_dev_get(&req_info, 6711ca3c42SAndrew Lunn tb[ETHTOOL_A_CABLE_TEST_HEADER], 6811ca3c42SAndrew Lunn genl_info_net(info), info->extack, 6911ca3c42SAndrew Lunn true); 7011ca3c42SAndrew Lunn if (ret < 0) 7111ca3c42SAndrew Lunn return ret; 7211ca3c42SAndrew Lunn 7311ca3c42SAndrew Lunn dev = req_info.dev; 7411ca3c42SAndrew Lunn if (!dev->phydev) { 7511ca3c42SAndrew Lunn ret = -EOPNOTSUPP; 7611ca3c42SAndrew Lunn goto out_dev_put; 7711ca3c42SAndrew Lunn } 7811ca3c42SAndrew Lunn 7911ca3c42SAndrew Lunn rtnl_lock(); 8011ca3c42SAndrew Lunn ret = ethnl_ops_begin(dev); 8111ca3c42SAndrew Lunn if (ret < 0) 8211ca3c42SAndrew Lunn goto out_rtnl; 8311ca3c42SAndrew Lunn 8411ca3c42SAndrew Lunn ret = phy_start_cable_test(dev->phydev, info->extack); 8511ca3c42SAndrew Lunn 8611ca3c42SAndrew Lunn ethnl_ops_complete(dev); 879896a457SAndrew Lunn 889896a457SAndrew Lunn if (!ret) 891a644de2SAndrew Lunn ethnl_cable_test_started(dev->phydev, 901a644de2SAndrew Lunn ETHTOOL_MSG_CABLE_TEST_NTF); 919896a457SAndrew Lunn 9211ca3c42SAndrew Lunn out_rtnl: 9311ca3c42SAndrew Lunn rtnl_unlock(); 9411ca3c42SAndrew Lunn out_dev_put: 9511ca3c42SAndrew Lunn dev_put(dev); 9611ca3c42SAndrew Lunn return ret; 9711ca3c42SAndrew Lunn } 981dd3f212SAndrew Lunn 991a644de2SAndrew Lunn int ethnl_cable_test_alloc(struct phy_device *phydev, u8 cmd) 1001dd3f212SAndrew Lunn { 1011dd3f212SAndrew Lunn int err = -ENOMEM; 1021dd3f212SAndrew Lunn 1036b4a0fc1SAndrew Lunn /* One TDR sample occupies 20 bytes. For a 150 meter cable, 1046b4a0fc1SAndrew Lunn * with four pairs, around 12K is needed. 1056b4a0fc1SAndrew Lunn */ 1066b4a0fc1SAndrew Lunn phydev->skb = genlmsg_new(SZ_16K, GFP_KERNEL); 1071dd3f212SAndrew Lunn if (!phydev->skb) 1081dd3f212SAndrew Lunn goto out; 1091dd3f212SAndrew Lunn 1101a644de2SAndrew Lunn phydev->ehdr = ethnl_bcastmsg_put(phydev->skb, cmd); 1111dd3f212SAndrew Lunn if (!phydev->ehdr) { 1121dd3f212SAndrew Lunn err = -EMSGSIZE; 1131dd3f212SAndrew Lunn goto out; 1141dd3f212SAndrew Lunn } 1151dd3f212SAndrew Lunn 1161dd3f212SAndrew Lunn err = ethnl_fill_reply_header(phydev->skb, phydev->attached_dev, 1171dd3f212SAndrew Lunn ETHTOOL_A_CABLE_TEST_NTF_HEADER); 1181dd3f212SAndrew Lunn if (err) 1191dd3f212SAndrew Lunn goto out; 1201dd3f212SAndrew Lunn 1211dd3f212SAndrew Lunn err = nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_TEST_NTF_STATUS, 1221dd3f212SAndrew Lunn ETHTOOL_A_CABLE_TEST_NTF_STATUS_COMPLETED); 1231dd3f212SAndrew Lunn if (err) 1241dd3f212SAndrew Lunn goto out; 1251dd3f212SAndrew Lunn 1261dd3f212SAndrew Lunn phydev->nest = nla_nest_start(phydev->skb, 1271dd3f212SAndrew Lunn ETHTOOL_A_CABLE_TEST_NTF_NEST); 1281e2dc145SAndrew Lunn if (!phydev->nest) { 1291e2dc145SAndrew Lunn err = -EMSGSIZE; 1301dd3f212SAndrew Lunn goto out; 1311e2dc145SAndrew Lunn } 1321dd3f212SAndrew Lunn 1331dd3f212SAndrew Lunn return 0; 1341dd3f212SAndrew Lunn 1351dd3f212SAndrew Lunn out: 1361dd3f212SAndrew Lunn nlmsg_free(phydev->skb); 1371e2dc145SAndrew Lunn phydev->skb = NULL; 1381dd3f212SAndrew Lunn return err; 1391dd3f212SAndrew Lunn } 1401dd3f212SAndrew Lunn EXPORT_SYMBOL_GPL(ethnl_cable_test_alloc); 1411dd3f212SAndrew Lunn 1421dd3f212SAndrew Lunn void ethnl_cable_test_free(struct phy_device *phydev) 1431dd3f212SAndrew Lunn { 1441dd3f212SAndrew Lunn nlmsg_free(phydev->skb); 1451e2dc145SAndrew Lunn phydev->skb = NULL; 1461dd3f212SAndrew Lunn } 1471dd3f212SAndrew Lunn EXPORT_SYMBOL_GPL(ethnl_cable_test_free); 1481dd3f212SAndrew Lunn 1491dd3f212SAndrew Lunn void ethnl_cable_test_finished(struct phy_device *phydev) 1501dd3f212SAndrew Lunn { 1511dd3f212SAndrew Lunn nla_nest_end(phydev->skb, phydev->nest); 1521dd3f212SAndrew Lunn 1531dd3f212SAndrew Lunn genlmsg_end(phydev->skb, phydev->ehdr); 1541dd3f212SAndrew Lunn 1551dd3f212SAndrew Lunn ethnl_multicast(phydev->skb, phydev->attached_dev); 1561dd3f212SAndrew Lunn } 1571dd3f212SAndrew Lunn EXPORT_SYMBOL_GPL(ethnl_cable_test_finished); 1581e2dc145SAndrew Lunn 1591e2dc145SAndrew Lunn int ethnl_cable_test_result(struct phy_device *phydev, u8 pair, u8 result) 1601e2dc145SAndrew Lunn { 1611e2dc145SAndrew Lunn struct nlattr *nest; 1621e2dc145SAndrew Lunn int ret = -EMSGSIZE; 1631e2dc145SAndrew Lunn 1641e2dc145SAndrew Lunn nest = nla_nest_start(phydev->skb, ETHTOOL_A_CABLE_NEST_RESULT); 1651e2dc145SAndrew Lunn if (!nest) 1661e2dc145SAndrew Lunn return -EMSGSIZE; 1671e2dc145SAndrew Lunn 1681e2dc145SAndrew Lunn if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_RESULT_PAIR, pair)) 1691e2dc145SAndrew Lunn goto err; 1701e2dc145SAndrew Lunn if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_RESULT_CODE, result)) 1711e2dc145SAndrew Lunn goto err; 1721e2dc145SAndrew Lunn 1731e2dc145SAndrew Lunn nla_nest_end(phydev->skb, nest); 1741e2dc145SAndrew Lunn return 0; 1751e2dc145SAndrew Lunn 1761e2dc145SAndrew Lunn err: 1771e2dc145SAndrew Lunn nla_nest_cancel(phydev->skb, nest); 1781e2dc145SAndrew Lunn return ret; 1791e2dc145SAndrew Lunn } 1801e2dc145SAndrew Lunn EXPORT_SYMBOL_GPL(ethnl_cable_test_result); 1811e2dc145SAndrew Lunn 1821e2dc145SAndrew Lunn int ethnl_cable_test_fault_length(struct phy_device *phydev, u8 pair, u32 cm) 1831e2dc145SAndrew Lunn { 1841e2dc145SAndrew Lunn struct nlattr *nest; 1851e2dc145SAndrew Lunn int ret = -EMSGSIZE; 1861e2dc145SAndrew Lunn 1871e2dc145SAndrew Lunn nest = nla_nest_start(phydev->skb, 1881e2dc145SAndrew Lunn ETHTOOL_A_CABLE_NEST_FAULT_LENGTH); 1891e2dc145SAndrew Lunn if (!nest) 1901e2dc145SAndrew Lunn return -EMSGSIZE; 1911e2dc145SAndrew Lunn 1921e2dc145SAndrew Lunn if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_FAULT_LENGTH_PAIR, pair)) 1931e2dc145SAndrew Lunn goto err; 1941e2dc145SAndrew Lunn if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_FAULT_LENGTH_CM, cm)) 1951e2dc145SAndrew Lunn goto err; 1961e2dc145SAndrew Lunn 1971e2dc145SAndrew Lunn nla_nest_end(phydev->skb, nest); 1981e2dc145SAndrew Lunn return 0; 1991e2dc145SAndrew Lunn 2001e2dc145SAndrew Lunn err: 2011e2dc145SAndrew Lunn nla_nest_cancel(phydev->skb, nest); 2021e2dc145SAndrew Lunn return ret; 2031e2dc145SAndrew Lunn } 2041e2dc145SAndrew Lunn EXPORT_SYMBOL_GPL(ethnl_cable_test_fault_length); 2051a644de2SAndrew Lunn 2061a644de2SAndrew Lunn static const struct nla_policy 2071a644de2SAndrew Lunn cable_test_tdr_act_policy[ETHTOOL_A_CABLE_TEST_TDR_MAX + 1] = { 2081a644de2SAndrew Lunn [ETHTOOL_A_CABLE_TEST_TDR_UNSPEC] = { .type = NLA_REJECT }, 2091a644de2SAndrew Lunn [ETHTOOL_A_CABLE_TEST_TDR_HEADER] = { .type = NLA_NESTED }, 2101a644de2SAndrew Lunn }; 2111a644de2SAndrew Lunn 2121a644de2SAndrew Lunn int ethnl_act_cable_test_tdr(struct sk_buff *skb, struct genl_info *info) 2131a644de2SAndrew Lunn { 2141a644de2SAndrew Lunn struct nlattr *tb[ETHTOOL_A_CABLE_TEST_TDR_MAX + 1]; 2151a644de2SAndrew Lunn struct ethnl_req_info req_info = {}; 2161a644de2SAndrew Lunn struct net_device *dev; 2171a644de2SAndrew Lunn int ret; 2181a644de2SAndrew Lunn 2191a644de2SAndrew Lunn ret = nlmsg_parse(info->nlhdr, GENL_HDRLEN, tb, 2201a644de2SAndrew Lunn ETHTOOL_A_CABLE_TEST_TDR_MAX, 2211a644de2SAndrew Lunn cable_test_tdr_act_policy, info->extack); 2221a644de2SAndrew Lunn if (ret < 0) 2231a644de2SAndrew Lunn return ret; 2241a644de2SAndrew Lunn 2251a644de2SAndrew Lunn ret = ethnl_parse_header_dev_get(&req_info, 2261a644de2SAndrew Lunn tb[ETHTOOL_A_CABLE_TEST_TDR_HEADER], 2271a644de2SAndrew Lunn genl_info_net(info), info->extack, 2281a644de2SAndrew Lunn true); 2291a644de2SAndrew Lunn if (ret < 0) 2301a644de2SAndrew Lunn return ret; 2311a644de2SAndrew Lunn 2321a644de2SAndrew Lunn dev = req_info.dev; 2331a644de2SAndrew Lunn if (!dev->phydev) { 2341a644de2SAndrew Lunn ret = -EOPNOTSUPP; 2351a644de2SAndrew Lunn goto out_dev_put; 2361a644de2SAndrew Lunn } 2371a644de2SAndrew Lunn 2381a644de2SAndrew Lunn rtnl_lock(); 2391a644de2SAndrew Lunn ret = ethnl_ops_begin(dev); 2401a644de2SAndrew Lunn if (ret < 0) 2411a644de2SAndrew Lunn goto out_rtnl; 2421a644de2SAndrew Lunn 2431a644de2SAndrew Lunn ret = phy_start_cable_test_tdr(dev->phydev, info->extack); 2441a644de2SAndrew Lunn 2451a644de2SAndrew Lunn ethnl_ops_complete(dev); 2461a644de2SAndrew Lunn 2471a644de2SAndrew Lunn if (!ret) 2481a644de2SAndrew Lunn ethnl_cable_test_started(dev->phydev, 2491a644de2SAndrew Lunn ETHTOOL_MSG_CABLE_TEST_TDR_NTF); 2501a644de2SAndrew Lunn 2511a644de2SAndrew Lunn out_rtnl: 2521a644de2SAndrew Lunn rtnl_unlock(); 2531a644de2SAndrew Lunn out_dev_put: 2541a644de2SAndrew Lunn dev_put(dev); 2551a644de2SAndrew Lunn return ret; 2561a644de2SAndrew Lunn } 2576b4a0fc1SAndrew Lunn 2586b4a0fc1SAndrew Lunn int ethnl_cable_test_amplitude(struct phy_device *phydev, 2596b4a0fc1SAndrew Lunn u8 pair, s16 mV) 2606b4a0fc1SAndrew Lunn { 2616b4a0fc1SAndrew Lunn struct nlattr *nest; 2626b4a0fc1SAndrew Lunn int ret = -EMSGSIZE; 2636b4a0fc1SAndrew Lunn 2646b4a0fc1SAndrew Lunn nest = nla_nest_start(phydev->skb, 2656b4a0fc1SAndrew Lunn ETHTOOL_A_CABLE_TDR_NEST_AMPLITUDE); 2666b4a0fc1SAndrew Lunn if (!nest) 2676b4a0fc1SAndrew Lunn return -EMSGSIZE; 2686b4a0fc1SAndrew Lunn 2696b4a0fc1SAndrew Lunn if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_AMPLITUDE_PAIR, pair)) 2706b4a0fc1SAndrew Lunn goto err; 2716b4a0fc1SAndrew Lunn if (nla_put_u16(phydev->skb, ETHTOOL_A_CABLE_AMPLITUDE_mV, mV)) 2726b4a0fc1SAndrew Lunn goto err; 2736b4a0fc1SAndrew Lunn 2746b4a0fc1SAndrew Lunn nla_nest_end(phydev->skb, nest); 2756b4a0fc1SAndrew Lunn return 0; 2766b4a0fc1SAndrew Lunn 2776b4a0fc1SAndrew Lunn err: 2786b4a0fc1SAndrew Lunn nla_nest_cancel(phydev->skb, nest); 2796b4a0fc1SAndrew Lunn return ret; 2806b4a0fc1SAndrew Lunn } 2816b4a0fc1SAndrew Lunn EXPORT_SYMBOL_GPL(ethnl_cable_test_amplitude); 2826b4a0fc1SAndrew Lunn 2836b4a0fc1SAndrew Lunn int ethnl_cable_test_pulse(struct phy_device *phydev, u16 mV) 2846b4a0fc1SAndrew Lunn { 2856b4a0fc1SAndrew Lunn struct nlattr *nest; 2866b4a0fc1SAndrew Lunn int ret = -EMSGSIZE; 2876b4a0fc1SAndrew Lunn 2886b4a0fc1SAndrew Lunn nest = nla_nest_start(phydev->skb, ETHTOOL_A_CABLE_TDR_NEST_PULSE); 2896b4a0fc1SAndrew Lunn if (!nest) 2906b4a0fc1SAndrew Lunn return -EMSGSIZE; 2916b4a0fc1SAndrew Lunn 2926b4a0fc1SAndrew Lunn if (nla_put_u16(phydev->skb, ETHTOOL_A_CABLE_PULSE_mV, mV)) 2936b4a0fc1SAndrew Lunn goto err; 2946b4a0fc1SAndrew Lunn 2956b4a0fc1SAndrew Lunn nla_nest_end(phydev->skb, nest); 2966b4a0fc1SAndrew Lunn return 0; 2976b4a0fc1SAndrew Lunn 2986b4a0fc1SAndrew Lunn err: 2996b4a0fc1SAndrew Lunn nla_nest_cancel(phydev->skb, nest); 3006b4a0fc1SAndrew Lunn return ret; 3016b4a0fc1SAndrew Lunn } 3026b4a0fc1SAndrew Lunn EXPORT_SYMBOL_GPL(ethnl_cable_test_pulse); 3036b4a0fc1SAndrew Lunn 3046b4a0fc1SAndrew Lunn int ethnl_cable_test_step(struct phy_device *phydev, u32 first, u32 last, 3056b4a0fc1SAndrew Lunn u32 step) 3066b4a0fc1SAndrew Lunn { 3076b4a0fc1SAndrew Lunn struct nlattr *nest; 3086b4a0fc1SAndrew Lunn int ret = -EMSGSIZE; 3096b4a0fc1SAndrew Lunn 3106b4a0fc1SAndrew Lunn nest = nla_nest_start(phydev->skb, ETHTOOL_A_CABLE_TDR_NEST_STEP); 3116b4a0fc1SAndrew Lunn if (!nest) 3126b4a0fc1SAndrew Lunn return -EMSGSIZE; 3136b4a0fc1SAndrew Lunn 3146b4a0fc1SAndrew Lunn if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_STEP_FIRST_DISTANCE, 3156b4a0fc1SAndrew Lunn first)) 3166b4a0fc1SAndrew Lunn goto err; 3176b4a0fc1SAndrew Lunn 3186b4a0fc1SAndrew Lunn if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_STEP_LAST_DISTANCE, last)) 3196b4a0fc1SAndrew Lunn goto err; 3206b4a0fc1SAndrew Lunn 3216b4a0fc1SAndrew Lunn if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_STEP_STEP_DISTANCE, step)) 3226b4a0fc1SAndrew Lunn goto err; 3236b4a0fc1SAndrew Lunn 3246b4a0fc1SAndrew Lunn nla_nest_end(phydev->skb, nest); 3256b4a0fc1SAndrew Lunn return 0; 3266b4a0fc1SAndrew Lunn 3276b4a0fc1SAndrew Lunn err: 3286b4a0fc1SAndrew Lunn nla_nest_cancel(phydev->skb, nest); 3296b4a0fc1SAndrew Lunn return ret; 3306b4a0fc1SAndrew Lunn } 3316b4a0fc1SAndrew Lunn EXPORT_SYMBOL_GPL(ethnl_cable_test_step); 332