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 /* One TDR sample occupies 20 bytes. For a 150 meter cable, 104 * with four pairs, around 12K is needed. 105 */ 106 phydev->skb = genlmsg_new(SZ_16K, GFP_KERNEL); 107 if (!phydev->skb) 108 goto out; 109 110 phydev->ehdr = ethnl_bcastmsg_put(phydev->skb, cmd); 111 if (!phydev->ehdr) { 112 err = -EMSGSIZE; 113 goto out; 114 } 115 116 err = ethnl_fill_reply_header(phydev->skb, phydev->attached_dev, 117 ETHTOOL_A_CABLE_TEST_NTF_HEADER); 118 if (err) 119 goto out; 120 121 err = nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_TEST_NTF_STATUS, 122 ETHTOOL_A_CABLE_TEST_NTF_STATUS_COMPLETED); 123 if (err) 124 goto out; 125 126 phydev->nest = nla_nest_start(phydev->skb, 127 ETHTOOL_A_CABLE_TEST_NTF_NEST); 128 if (!phydev->nest) { 129 err = -EMSGSIZE; 130 goto out; 131 } 132 133 return 0; 134 135 out: 136 nlmsg_free(phydev->skb); 137 phydev->skb = NULL; 138 return err; 139 } 140 EXPORT_SYMBOL_GPL(ethnl_cable_test_alloc); 141 142 void ethnl_cable_test_free(struct phy_device *phydev) 143 { 144 nlmsg_free(phydev->skb); 145 phydev->skb = NULL; 146 } 147 EXPORT_SYMBOL_GPL(ethnl_cable_test_free); 148 149 void ethnl_cable_test_finished(struct phy_device *phydev) 150 { 151 nla_nest_end(phydev->skb, phydev->nest); 152 153 genlmsg_end(phydev->skb, phydev->ehdr); 154 155 ethnl_multicast(phydev->skb, phydev->attached_dev); 156 } 157 EXPORT_SYMBOL_GPL(ethnl_cable_test_finished); 158 159 int ethnl_cable_test_result(struct phy_device *phydev, u8 pair, u8 result) 160 { 161 struct nlattr *nest; 162 int ret = -EMSGSIZE; 163 164 nest = nla_nest_start(phydev->skb, ETHTOOL_A_CABLE_NEST_RESULT); 165 if (!nest) 166 return -EMSGSIZE; 167 168 if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_RESULT_PAIR, pair)) 169 goto err; 170 if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_RESULT_CODE, result)) 171 goto err; 172 173 nla_nest_end(phydev->skb, nest); 174 return 0; 175 176 err: 177 nla_nest_cancel(phydev->skb, nest); 178 return ret; 179 } 180 EXPORT_SYMBOL_GPL(ethnl_cable_test_result); 181 182 int ethnl_cable_test_fault_length(struct phy_device *phydev, u8 pair, u32 cm) 183 { 184 struct nlattr *nest; 185 int ret = -EMSGSIZE; 186 187 nest = nla_nest_start(phydev->skb, 188 ETHTOOL_A_CABLE_NEST_FAULT_LENGTH); 189 if (!nest) 190 return -EMSGSIZE; 191 192 if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_FAULT_LENGTH_PAIR, pair)) 193 goto err; 194 if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_FAULT_LENGTH_CM, cm)) 195 goto err; 196 197 nla_nest_end(phydev->skb, nest); 198 return 0; 199 200 err: 201 nla_nest_cancel(phydev->skb, nest); 202 return ret; 203 } 204 EXPORT_SYMBOL_GPL(ethnl_cable_test_fault_length); 205 206 static const struct nla_policy 207 cable_test_tdr_act_policy[ETHTOOL_A_CABLE_TEST_TDR_MAX + 1] = { 208 [ETHTOOL_A_CABLE_TEST_TDR_UNSPEC] = { .type = NLA_REJECT }, 209 [ETHTOOL_A_CABLE_TEST_TDR_HEADER] = { .type = NLA_NESTED }, 210 }; 211 212 int ethnl_act_cable_test_tdr(struct sk_buff *skb, struct genl_info *info) 213 { 214 struct nlattr *tb[ETHTOOL_A_CABLE_TEST_TDR_MAX + 1]; 215 struct ethnl_req_info req_info = {}; 216 struct net_device *dev; 217 int ret; 218 219 ret = nlmsg_parse(info->nlhdr, GENL_HDRLEN, tb, 220 ETHTOOL_A_CABLE_TEST_TDR_MAX, 221 cable_test_tdr_act_policy, info->extack); 222 if (ret < 0) 223 return ret; 224 225 ret = ethnl_parse_header_dev_get(&req_info, 226 tb[ETHTOOL_A_CABLE_TEST_TDR_HEADER], 227 genl_info_net(info), info->extack, 228 true); 229 if (ret < 0) 230 return ret; 231 232 dev = req_info.dev; 233 if (!dev->phydev) { 234 ret = -EOPNOTSUPP; 235 goto out_dev_put; 236 } 237 238 rtnl_lock(); 239 ret = ethnl_ops_begin(dev); 240 if (ret < 0) 241 goto out_rtnl; 242 243 ret = phy_start_cable_test_tdr(dev->phydev, info->extack); 244 245 ethnl_ops_complete(dev); 246 247 if (!ret) 248 ethnl_cable_test_started(dev->phydev, 249 ETHTOOL_MSG_CABLE_TEST_TDR_NTF); 250 251 out_rtnl: 252 rtnl_unlock(); 253 out_dev_put: 254 dev_put(dev); 255 return ret; 256 } 257 258 int ethnl_cable_test_amplitude(struct phy_device *phydev, 259 u8 pair, s16 mV) 260 { 261 struct nlattr *nest; 262 int ret = -EMSGSIZE; 263 264 nest = nla_nest_start(phydev->skb, 265 ETHTOOL_A_CABLE_TDR_NEST_AMPLITUDE); 266 if (!nest) 267 return -EMSGSIZE; 268 269 if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_AMPLITUDE_PAIR, pair)) 270 goto err; 271 if (nla_put_u16(phydev->skb, ETHTOOL_A_CABLE_AMPLITUDE_mV, mV)) 272 goto err; 273 274 nla_nest_end(phydev->skb, nest); 275 return 0; 276 277 err: 278 nla_nest_cancel(phydev->skb, nest); 279 return ret; 280 } 281 EXPORT_SYMBOL_GPL(ethnl_cable_test_amplitude); 282 283 int ethnl_cable_test_pulse(struct phy_device *phydev, u16 mV) 284 { 285 struct nlattr *nest; 286 int ret = -EMSGSIZE; 287 288 nest = nla_nest_start(phydev->skb, ETHTOOL_A_CABLE_TDR_NEST_PULSE); 289 if (!nest) 290 return -EMSGSIZE; 291 292 if (nla_put_u16(phydev->skb, ETHTOOL_A_CABLE_PULSE_mV, mV)) 293 goto err; 294 295 nla_nest_end(phydev->skb, nest); 296 return 0; 297 298 err: 299 nla_nest_cancel(phydev->skb, nest); 300 return ret; 301 } 302 EXPORT_SYMBOL_GPL(ethnl_cable_test_pulse); 303 304 int ethnl_cable_test_step(struct phy_device *phydev, u32 first, u32 last, 305 u32 step) 306 { 307 struct nlattr *nest; 308 int ret = -EMSGSIZE; 309 310 nest = nla_nest_start(phydev->skb, ETHTOOL_A_CABLE_TDR_NEST_STEP); 311 if (!nest) 312 return -EMSGSIZE; 313 314 if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_STEP_FIRST_DISTANCE, 315 first)) 316 goto err; 317 318 if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_STEP_LAST_DISTANCE, last)) 319 goto err; 320 321 if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_STEP_STEP_DISTANCE, step)) 322 goto err; 323 324 nla_nest_end(phydev->skb, nest); 325 return 0; 326 327 err: 328 nla_nest_cancel(phydev->skb, nest); 329 return ret; 330 } 331 EXPORT_SYMBOL_GPL(ethnl_cable_test_step); 332