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 1031dd3f212SAndrew Lunn phydev->skb = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); 1041dd3f212SAndrew Lunn if (!phydev->skb) 1051dd3f212SAndrew Lunn goto out; 1061dd3f212SAndrew Lunn 1071a644de2SAndrew Lunn phydev->ehdr = ethnl_bcastmsg_put(phydev->skb, cmd); 1081dd3f212SAndrew Lunn if (!phydev->ehdr) { 1091dd3f212SAndrew Lunn err = -EMSGSIZE; 1101dd3f212SAndrew Lunn goto out; 1111dd3f212SAndrew Lunn } 1121dd3f212SAndrew Lunn 1131dd3f212SAndrew Lunn err = ethnl_fill_reply_header(phydev->skb, phydev->attached_dev, 1141dd3f212SAndrew Lunn ETHTOOL_A_CABLE_TEST_NTF_HEADER); 1151dd3f212SAndrew Lunn if (err) 1161dd3f212SAndrew Lunn goto out; 1171dd3f212SAndrew Lunn 1181dd3f212SAndrew Lunn err = nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_TEST_NTF_STATUS, 1191dd3f212SAndrew Lunn ETHTOOL_A_CABLE_TEST_NTF_STATUS_COMPLETED); 1201dd3f212SAndrew Lunn if (err) 1211dd3f212SAndrew Lunn goto out; 1221dd3f212SAndrew Lunn 1231dd3f212SAndrew Lunn phydev->nest = nla_nest_start(phydev->skb, 1241dd3f212SAndrew Lunn ETHTOOL_A_CABLE_TEST_NTF_NEST); 1251e2dc145SAndrew Lunn if (!phydev->nest) { 1261e2dc145SAndrew Lunn err = -EMSGSIZE; 1271dd3f212SAndrew Lunn goto out; 1281e2dc145SAndrew Lunn } 1291dd3f212SAndrew Lunn 1301dd3f212SAndrew Lunn return 0; 1311dd3f212SAndrew Lunn 1321dd3f212SAndrew Lunn out: 1331dd3f212SAndrew Lunn nlmsg_free(phydev->skb); 1341e2dc145SAndrew Lunn phydev->skb = NULL; 1351dd3f212SAndrew Lunn return err; 1361dd3f212SAndrew Lunn } 1371dd3f212SAndrew Lunn EXPORT_SYMBOL_GPL(ethnl_cable_test_alloc); 1381dd3f212SAndrew Lunn 1391dd3f212SAndrew Lunn void ethnl_cable_test_free(struct phy_device *phydev) 1401dd3f212SAndrew Lunn { 1411dd3f212SAndrew Lunn nlmsg_free(phydev->skb); 1421e2dc145SAndrew Lunn phydev->skb = NULL; 1431dd3f212SAndrew Lunn } 1441dd3f212SAndrew Lunn EXPORT_SYMBOL_GPL(ethnl_cable_test_free); 1451dd3f212SAndrew Lunn 1461dd3f212SAndrew Lunn void ethnl_cable_test_finished(struct phy_device *phydev) 1471dd3f212SAndrew Lunn { 1481dd3f212SAndrew Lunn nla_nest_end(phydev->skb, phydev->nest); 1491dd3f212SAndrew Lunn 1501dd3f212SAndrew Lunn genlmsg_end(phydev->skb, phydev->ehdr); 1511dd3f212SAndrew Lunn 1521dd3f212SAndrew Lunn ethnl_multicast(phydev->skb, phydev->attached_dev); 1531dd3f212SAndrew Lunn } 1541dd3f212SAndrew Lunn EXPORT_SYMBOL_GPL(ethnl_cable_test_finished); 1551e2dc145SAndrew Lunn 1561e2dc145SAndrew Lunn int ethnl_cable_test_result(struct phy_device *phydev, u8 pair, u8 result) 1571e2dc145SAndrew Lunn { 1581e2dc145SAndrew Lunn struct nlattr *nest; 1591e2dc145SAndrew Lunn int ret = -EMSGSIZE; 1601e2dc145SAndrew Lunn 1611e2dc145SAndrew Lunn nest = nla_nest_start(phydev->skb, ETHTOOL_A_CABLE_NEST_RESULT); 1621e2dc145SAndrew Lunn if (!nest) 1631e2dc145SAndrew Lunn return -EMSGSIZE; 1641e2dc145SAndrew Lunn 1651e2dc145SAndrew Lunn if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_RESULT_PAIR, pair)) 1661e2dc145SAndrew Lunn goto err; 1671e2dc145SAndrew Lunn if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_RESULT_CODE, result)) 1681e2dc145SAndrew Lunn goto err; 1691e2dc145SAndrew Lunn 1701e2dc145SAndrew Lunn nla_nest_end(phydev->skb, nest); 1711e2dc145SAndrew Lunn return 0; 1721e2dc145SAndrew Lunn 1731e2dc145SAndrew Lunn err: 1741e2dc145SAndrew Lunn nla_nest_cancel(phydev->skb, nest); 1751e2dc145SAndrew Lunn return ret; 1761e2dc145SAndrew Lunn } 1771e2dc145SAndrew Lunn EXPORT_SYMBOL_GPL(ethnl_cable_test_result); 1781e2dc145SAndrew Lunn 1791e2dc145SAndrew Lunn int ethnl_cable_test_fault_length(struct phy_device *phydev, u8 pair, u32 cm) 1801e2dc145SAndrew Lunn { 1811e2dc145SAndrew Lunn struct nlattr *nest; 1821e2dc145SAndrew Lunn int ret = -EMSGSIZE; 1831e2dc145SAndrew Lunn 1841e2dc145SAndrew Lunn nest = nla_nest_start(phydev->skb, 1851e2dc145SAndrew Lunn ETHTOOL_A_CABLE_NEST_FAULT_LENGTH); 1861e2dc145SAndrew Lunn if (!nest) 1871e2dc145SAndrew Lunn return -EMSGSIZE; 1881e2dc145SAndrew Lunn 1891e2dc145SAndrew Lunn if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_FAULT_LENGTH_PAIR, pair)) 1901e2dc145SAndrew Lunn goto err; 1911e2dc145SAndrew Lunn if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_FAULT_LENGTH_CM, cm)) 1921e2dc145SAndrew Lunn goto err; 1931e2dc145SAndrew Lunn 1941e2dc145SAndrew Lunn nla_nest_end(phydev->skb, nest); 1951e2dc145SAndrew Lunn return 0; 1961e2dc145SAndrew Lunn 1971e2dc145SAndrew Lunn err: 1981e2dc145SAndrew Lunn nla_nest_cancel(phydev->skb, nest); 1991e2dc145SAndrew Lunn return ret; 2001e2dc145SAndrew Lunn } 2011e2dc145SAndrew Lunn EXPORT_SYMBOL_GPL(ethnl_cable_test_fault_length); 2021a644de2SAndrew Lunn 2031a644de2SAndrew Lunn static const struct nla_policy 2041a644de2SAndrew Lunn cable_test_tdr_act_policy[ETHTOOL_A_CABLE_TEST_TDR_MAX + 1] = { 2051a644de2SAndrew Lunn [ETHTOOL_A_CABLE_TEST_TDR_UNSPEC] = { .type = NLA_REJECT }, 2061a644de2SAndrew Lunn [ETHTOOL_A_CABLE_TEST_TDR_HEADER] = { .type = NLA_NESTED }, 2071a644de2SAndrew Lunn }; 2081a644de2SAndrew Lunn 2091a644de2SAndrew Lunn int ethnl_act_cable_test_tdr(struct sk_buff *skb, struct genl_info *info) 2101a644de2SAndrew Lunn { 2111a644de2SAndrew Lunn struct nlattr *tb[ETHTOOL_A_CABLE_TEST_TDR_MAX + 1]; 2121a644de2SAndrew Lunn struct ethnl_req_info req_info = {}; 2131a644de2SAndrew Lunn struct net_device *dev; 2141a644de2SAndrew Lunn int ret; 2151a644de2SAndrew Lunn 2161a644de2SAndrew Lunn ret = nlmsg_parse(info->nlhdr, GENL_HDRLEN, tb, 2171a644de2SAndrew Lunn ETHTOOL_A_CABLE_TEST_TDR_MAX, 2181a644de2SAndrew Lunn cable_test_tdr_act_policy, info->extack); 2191a644de2SAndrew Lunn if (ret < 0) 2201a644de2SAndrew Lunn return ret; 2211a644de2SAndrew Lunn 2221a644de2SAndrew Lunn ret = ethnl_parse_header_dev_get(&req_info, 2231a644de2SAndrew Lunn tb[ETHTOOL_A_CABLE_TEST_TDR_HEADER], 2241a644de2SAndrew Lunn genl_info_net(info), info->extack, 2251a644de2SAndrew Lunn true); 2261a644de2SAndrew Lunn if (ret < 0) 2271a644de2SAndrew Lunn return ret; 2281a644de2SAndrew Lunn 2291a644de2SAndrew Lunn dev = req_info.dev; 2301a644de2SAndrew Lunn if (!dev->phydev) { 2311a644de2SAndrew Lunn ret = -EOPNOTSUPP; 2321a644de2SAndrew Lunn goto out_dev_put; 2331a644de2SAndrew Lunn } 2341a644de2SAndrew Lunn 2351a644de2SAndrew Lunn rtnl_lock(); 2361a644de2SAndrew Lunn ret = ethnl_ops_begin(dev); 2371a644de2SAndrew Lunn if (ret < 0) 2381a644de2SAndrew Lunn goto out_rtnl; 2391a644de2SAndrew Lunn 2401a644de2SAndrew Lunn ret = phy_start_cable_test_tdr(dev->phydev, info->extack); 2411a644de2SAndrew Lunn 2421a644de2SAndrew Lunn ethnl_ops_complete(dev); 2431a644de2SAndrew Lunn 2441a644de2SAndrew Lunn if (!ret) 2451a644de2SAndrew Lunn ethnl_cable_test_started(dev->phydev, 2461a644de2SAndrew Lunn ETHTOOL_MSG_CABLE_TEST_TDR_NTF); 2471a644de2SAndrew Lunn 2481a644de2SAndrew Lunn out_rtnl: 2491a644de2SAndrew Lunn rtnl_unlock(); 2501a644de2SAndrew Lunn out_dev_put: 2511a644de2SAndrew Lunn dev_put(dev); 2521a644de2SAndrew Lunn return ret; 2531a644de2SAndrew Lunn } 254