1 // SPDX-License-Identifier: GPL-2.0-only 2 3 #include <linux/phy.h> 4 #include <linux/ethtool_netlink.h> 5 #include "netlink.h" 6 #include "common.h" 7 8 /* CABLE_TEST_ACT */ 9 10 static const struct nla_policy 11 cable_test_act_policy[ETHTOOL_A_CABLE_TEST_MAX + 1] = { 12 [ETHTOOL_A_CABLE_TEST_UNSPEC] = { .type = NLA_REJECT }, 13 [ETHTOOL_A_CABLE_TEST_HEADER] = { .type = NLA_NESTED }, 14 }; 15 16 static int ethnl_cable_test_started(struct phy_device *phydev, u8 cmd) 17 { 18 struct sk_buff *skb; 19 int err = -ENOMEM; 20 void *ehdr; 21 22 skb = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); 23 if (!skb) 24 goto out; 25 26 ehdr = ethnl_bcastmsg_put(skb, cmd); 27 if (!ehdr) { 28 err = -EMSGSIZE; 29 goto out; 30 } 31 32 err = ethnl_fill_reply_header(skb, phydev->attached_dev, 33 ETHTOOL_A_CABLE_TEST_NTF_HEADER); 34 if (err) 35 goto out; 36 37 err = nla_put_u8(skb, ETHTOOL_A_CABLE_TEST_NTF_STATUS, 38 ETHTOOL_A_CABLE_TEST_NTF_STATUS_STARTED); 39 if (err) 40 goto out; 41 42 genlmsg_end(skb, ehdr); 43 44 return ethnl_multicast(skb, phydev->attached_dev); 45 46 out: 47 nlmsg_free(skb); 48 phydev_err(phydev, "%s: Error %pe\n", __func__, ERR_PTR(err)); 49 50 return err; 51 } 52 53 int ethnl_act_cable_test(struct sk_buff *skb, struct genl_info *info) 54 { 55 struct nlattr *tb[ETHTOOL_A_CABLE_TEST_MAX + 1]; 56 struct ethnl_req_info req_info = {}; 57 struct net_device *dev; 58 int ret; 59 60 ret = nlmsg_parse(info->nlhdr, GENL_HDRLEN, tb, 61 ETHTOOL_A_CABLE_TEST_MAX, 62 cable_test_act_policy, info->extack); 63 if (ret < 0) 64 return ret; 65 66 ret = ethnl_parse_header_dev_get(&req_info, 67 tb[ETHTOOL_A_CABLE_TEST_HEADER], 68 genl_info_net(info), info->extack, 69 true); 70 if (ret < 0) 71 return ret; 72 73 dev = req_info.dev; 74 if (!dev->phydev) { 75 ret = -EOPNOTSUPP; 76 goto out_dev_put; 77 } 78 79 rtnl_lock(); 80 ret = ethnl_ops_begin(dev); 81 if (ret < 0) 82 goto out_rtnl; 83 84 ret = phy_start_cable_test(dev->phydev, info->extack); 85 86 ethnl_ops_complete(dev); 87 88 if (!ret) 89 ethnl_cable_test_started(dev->phydev, 90 ETHTOOL_MSG_CABLE_TEST_NTF); 91 92 out_rtnl: 93 rtnl_unlock(); 94 out_dev_put: 95 dev_put(dev); 96 return ret; 97 } 98 99 int ethnl_cable_test_alloc(struct phy_device *phydev, u8 cmd) 100 { 101 int err = -ENOMEM; 102 103 phydev->skb = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); 104 if (!phydev->skb) 105 goto out; 106 107 phydev->ehdr = ethnl_bcastmsg_put(phydev->skb, cmd); 108 if (!phydev->ehdr) { 109 err = -EMSGSIZE; 110 goto out; 111 } 112 113 err = ethnl_fill_reply_header(phydev->skb, phydev->attached_dev, 114 ETHTOOL_A_CABLE_TEST_NTF_HEADER); 115 if (err) 116 goto out; 117 118 err = nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_TEST_NTF_STATUS, 119 ETHTOOL_A_CABLE_TEST_NTF_STATUS_COMPLETED); 120 if (err) 121 goto out; 122 123 phydev->nest = nla_nest_start(phydev->skb, 124 ETHTOOL_A_CABLE_TEST_NTF_NEST); 125 if (!phydev->nest) { 126 err = -EMSGSIZE; 127 goto out; 128 } 129 130 return 0; 131 132 out: 133 nlmsg_free(phydev->skb); 134 phydev->skb = NULL; 135 return err; 136 } 137 EXPORT_SYMBOL_GPL(ethnl_cable_test_alloc); 138 139 void ethnl_cable_test_free(struct phy_device *phydev) 140 { 141 nlmsg_free(phydev->skb); 142 phydev->skb = NULL; 143 } 144 EXPORT_SYMBOL_GPL(ethnl_cable_test_free); 145 146 void ethnl_cable_test_finished(struct phy_device *phydev) 147 { 148 nla_nest_end(phydev->skb, phydev->nest); 149 150 genlmsg_end(phydev->skb, phydev->ehdr); 151 152 ethnl_multicast(phydev->skb, phydev->attached_dev); 153 } 154 EXPORT_SYMBOL_GPL(ethnl_cable_test_finished); 155 156 int ethnl_cable_test_result(struct phy_device *phydev, u8 pair, u8 result) 157 { 158 struct nlattr *nest; 159 int ret = -EMSGSIZE; 160 161 nest = nla_nest_start(phydev->skb, ETHTOOL_A_CABLE_NEST_RESULT); 162 if (!nest) 163 return -EMSGSIZE; 164 165 if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_RESULT_PAIR, pair)) 166 goto err; 167 if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_RESULT_CODE, result)) 168 goto err; 169 170 nla_nest_end(phydev->skb, nest); 171 return 0; 172 173 err: 174 nla_nest_cancel(phydev->skb, nest); 175 return ret; 176 } 177 EXPORT_SYMBOL_GPL(ethnl_cable_test_result); 178 179 int ethnl_cable_test_fault_length(struct phy_device *phydev, u8 pair, u32 cm) 180 { 181 struct nlattr *nest; 182 int ret = -EMSGSIZE; 183 184 nest = nla_nest_start(phydev->skb, 185 ETHTOOL_A_CABLE_NEST_FAULT_LENGTH); 186 if (!nest) 187 return -EMSGSIZE; 188 189 if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_FAULT_LENGTH_PAIR, pair)) 190 goto err; 191 if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_FAULT_LENGTH_CM, cm)) 192 goto err; 193 194 nla_nest_end(phydev->skb, nest); 195 return 0; 196 197 err: 198 nla_nest_cancel(phydev->skb, nest); 199 return ret; 200 } 201 EXPORT_SYMBOL_GPL(ethnl_cable_test_fault_length); 202 203 static const struct nla_policy 204 cable_test_tdr_act_policy[ETHTOOL_A_CABLE_TEST_TDR_MAX + 1] = { 205 [ETHTOOL_A_CABLE_TEST_TDR_UNSPEC] = { .type = NLA_REJECT }, 206 [ETHTOOL_A_CABLE_TEST_TDR_HEADER] = { .type = NLA_NESTED }, 207 }; 208 209 int ethnl_act_cable_test_tdr(struct sk_buff *skb, struct genl_info *info) 210 { 211 struct nlattr *tb[ETHTOOL_A_CABLE_TEST_TDR_MAX + 1]; 212 struct ethnl_req_info req_info = {}; 213 struct net_device *dev; 214 int ret; 215 216 ret = nlmsg_parse(info->nlhdr, GENL_HDRLEN, tb, 217 ETHTOOL_A_CABLE_TEST_TDR_MAX, 218 cable_test_tdr_act_policy, info->extack); 219 if (ret < 0) 220 return ret; 221 222 ret = ethnl_parse_header_dev_get(&req_info, 223 tb[ETHTOOL_A_CABLE_TEST_TDR_HEADER], 224 genl_info_net(info), info->extack, 225 true); 226 if (ret < 0) 227 return ret; 228 229 dev = req_info.dev; 230 if (!dev->phydev) { 231 ret = -EOPNOTSUPP; 232 goto out_dev_put; 233 } 234 235 rtnl_lock(); 236 ret = ethnl_ops_begin(dev); 237 if (ret < 0) 238 goto out_rtnl; 239 240 ret = phy_start_cable_test_tdr(dev->phydev, info->extack); 241 242 ethnl_ops_complete(dev); 243 244 if (!ret) 245 ethnl_cable_test_started(dev->phydev, 246 ETHTOOL_MSG_CABLE_TEST_TDR_NTF); 247 248 out_rtnl: 249 rtnl_unlock(); 250 out_dev_put: 251 dev_put(dev); 252 return ret; 253 } 254