1 // SPDX-License-Identifier: GPL-2.0-only 2 3 #include "netlink.h" 4 #include "common.h" 5 #include "bitset.h" 6 7 struct linkmodes_req_info { 8 struct ethnl_req_info base; 9 }; 10 11 struct linkmodes_reply_data { 12 struct ethnl_reply_data base; 13 struct ethtool_link_ksettings ksettings; 14 struct ethtool_link_settings *lsettings; 15 bool peer_empty; 16 }; 17 18 #define LINKMODES_REPDATA(__reply_base) \ 19 container_of(__reply_base, struct linkmodes_reply_data, base) 20 21 static const struct nla_policy 22 linkmodes_get_policy[ETHTOOL_A_LINKMODES_MAX + 1] = { 23 [ETHTOOL_A_LINKMODES_UNSPEC] = { .type = NLA_REJECT }, 24 [ETHTOOL_A_LINKMODES_HEADER] = { .type = NLA_NESTED }, 25 [ETHTOOL_A_LINKMODES_AUTONEG] = { .type = NLA_REJECT }, 26 [ETHTOOL_A_LINKMODES_OURS] = { .type = NLA_REJECT }, 27 [ETHTOOL_A_LINKMODES_PEER] = { .type = NLA_REJECT }, 28 [ETHTOOL_A_LINKMODES_SPEED] = { .type = NLA_REJECT }, 29 [ETHTOOL_A_LINKMODES_DUPLEX] = { .type = NLA_REJECT }, 30 [ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG] = { .type = NLA_REJECT }, 31 [ETHTOOL_A_LINKMODES_MASTER_SLAVE_STATE] = { .type = NLA_REJECT }, 32 }; 33 34 static int linkmodes_prepare_data(const struct ethnl_req_info *req_base, 35 struct ethnl_reply_data *reply_base, 36 struct genl_info *info) 37 { 38 struct linkmodes_reply_data *data = LINKMODES_REPDATA(reply_base); 39 struct net_device *dev = reply_base->dev; 40 int ret; 41 42 data->lsettings = &data->ksettings.base; 43 44 ret = ethnl_ops_begin(dev); 45 if (ret < 0) 46 return ret; 47 48 ret = __ethtool_get_link_ksettings(dev, &data->ksettings); 49 if (ret < 0 && info) { 50 GENL_SET_ERR_MSG(info, "failed to retrieve link settings"); 51 goto out; 52 } 53 54 data->peer_empty = 55 bitmap_empty(data->ksettings.link_modes.lp_advertising, 56 __ETHTOOL_LINK_MODE_MASK_NBITS); 57 58 out: 59 ethnl_ops_complete(dev); 60 return ret; 61 } 62 63 static int linkmodes_reply_size(const struct ethnl_req_info *req_base, 64 const struct ethnl_reply_data *reply_base) 65 { 66 const struct linkmodes_reply_data *data = LINKMODES_REPDATA(reply_base); 67 const struct ethtool_link_ksettings *ksettings = &data->ksettings; 68 const struct ethtool_link_settings *lsettings = &ksettings->base; 69 bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS; 70 int len, ret; 71 72 len = nla_total_size(sizeof(u8)) /* LINKMODES_AUTONEG */ 73 + nla_total_size(sizeof(u32)) /* LINKMODES_SPEED */ 74 + nla_total_size(sizeof(u8)) /* LINKMODES_DUPLEX */ 75 + 0; 76 ret = ethnl_bitset_size(ksettings->link_modes.advertising, 77 ksettings->link_modes.supported, 78 __ETHTOOL_LINK_MODE_MASK_NBITS, 79 link_mode_names, compact); 80 if (ret < 0) 81 return ret; 82 len += ret; 83 if (!data->peer_empty) { 84 ret = ethnl_bitset_size(ksettings->link_modes.lp_advertising, 85 NULL, __ETHTOOL_LINK_MODE_MASK_NBITS, 86 link_mode_names, compact); 87 if (ret < 0) 88 return ret; 89 len += ret; 90 } 91 92 if (lsettings->master_slave_cfg != MASTER_SLAVE_CFG_UNSUPPORTED) 93 len += nla_total_size(sizeof(u8)); 94 95 if (lsettings->master_slave_state != MASTER_SLAVE_STATE_UNSUPPORTED) 96 len += nla_total_size(sizeof(u8)); 97 98 return len; 99 } 100 101 static int linkmodes_fill_reply(struct sk_buff *skb, 102 const struct ethnl_req_info *req_base, 103 const struct ethnl_reply_data *reply_base) 104 { 105 const struct linkmodes_reply_data *data = LINKMODES_REPDATA(reply_base); 106 const struct ethtool_link_ksettings *ksettings = &data->ksettings; 107 const struct ethtool_link_settings *lsettings = &ksettings->base; 108 bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS; 109 int ret; 110 111 if (nla_put_u8(skb, ETHTOOL_A_LINKMODES_AUTONEG, lsettings->autoneg)) 112 return -EMSGSIZE; 113 114 ret = ethnl_put_bitset(skb, ETHTOOL_A_LINKMODES_OURS, 115 ksettings->link_modes.advertising, 116 ksettings->link_modes.supported, 117 __ETHTOOL_LINK_MODE_MASK_NBITS, link_mode_names, 118 compact); 119 if (ret < 0) 120 return -EMSGSIZE; 121 if (!data->peer_empty) { 122 ret = ethnl_put_bitset(skb, ETHTOOL_A_LINKMODES_PEER, 123 ksettings->link_modes.lp_advertising, 124 NULL, __ETHTOOL_LINK_MODE_MASK_NBITS, 125 link_mode_names, compact); 126 if (ret < 0) 127 return -EMSGSIZE; 128 } 129 130 if (nla_put_u32(skb, ETHTOOL_A_LINKMODES_SPEED, lsettings->speed) || 131 nla_put_u8(skb, ETHTOOL_A_LINKMODES_DUPLEX, lsettings->duplex)) 132 return -EMSGSIZE; 133 134 if (lsettings->master_slave_cfg != MASTER_SLAVE_CFG_UNSUPPORTED && 135 nla_put_u8(skb, ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG, 136 lsettings->master_slave_cfg)) 137 return -EMSGSIZE; 138 139 if (lsettings->master_slave_state != MASTER_SLAVE_STATE_UNSUPPORTED && 140 nla_put_u8(skb, ETHTOOL_A_LINKMODES_MASTER_SLAVE_STATE, 141 lsettings->master_slave_state)) 142 return -EMSGSIZE; 143 144 return 0; 145 } 146 147 const struct ethnl_request_ops ethnl_linkmodes_request_ops = { 148 .request_cmd = ETHTOOL_MSG_LINKMODES_GET, 149 .reply_cmd = ETHTOOL_MSG_LINKMODES_GET_REPLY, 150 .hdr_attr = ETHTOOL_A_LINKMODES_HEADER, 151 .max_attr = ETHTOOL_A_LINKMODES_MAX, 152 .req_info_size = sizeof(struct linkmodes_req_info), 153 .reply_data_size = sizeof(struct linkmodes_reply_data), 154 .request_policy = linkmodes_get_policy, 155 156 .prepare_data = linkmodes_prepare_data, 157 .reply_size = linkmodes_reply_size, 158 .fill_reply = linkmodes_fill_reply, 159 }; 160 161 /* LINKMODES_SET */ 162 163 struct link_mode_info { 164 int speed; 165 u8 duplex; 166 }; 167 168 #define __DEFINE_LINK_MODE_PARAMS(_speed, _type, _duplex) \ 169 [ETHTOOL_LINK_MODE(_speed, _type, _duplex)] = { \ 170 .speed = SPEED_ ## _speed, \ 171 .duplex = __DUPLEX_ ## _duplex \ 172 } 173 #define __DUPLEX_Half DUPLEX_HALF 174 #define __DUPLEX_Full DUPLEX_FULL 175 #define __DEFINE_SPECIAL_MODE_PARAMS(_mode) \ 176 [ETHTOOL_LINK_MODE_ ## _mode ## _BIT] = { \ 177 .speed = SPEED_UNKNOWN, \ 178 .duplex = DUPLEX_UNKNOWN, \ 179 } 180 181 static const struct link_mode_info link_mode_params[] = { 182 __DEFINE_LINK_MODE_PARAMS(10, T, Half), 183 __DEFINE_LINK_MODE_PARAMS(10, T, Full), 184 __DEFINE_LINK_MODE_PARAMS(100, T, Half), 185 __DEFINE_LINK_MODE_PARAMS(100, T, Full), 186 __DEFINE_LINK_MODE_PARAMS(1000, T, Half), 187 __DEFINE_LINK_MODE_PARAMS(1000, T, Full), 188 __DEFINE_SPECIAL_MODE_PARAMS(Autoneg), 189 __DEFINE_SPECIAL_MODE_PARAMS(TP), 190 __DEFINE_SPECIAL_MODE_PARAMS(AUI), 191 __DEFINE_SPECIAL_MODE_PARAMS(MII), 192 __DEFINE_SPECIAL_MODE_PARAMS(FIBRE), 193 __DEFINE_SPECIAL_MODE_PARAMS(BNC), 194 __DEFINE_LINK_MODE_PARAMS(10000, T, Full), 195 __DEFINE_SPECIAL_MODE_PARAMS(Pause), 196 __DEFINE_SPECIAL_MODE_PARAMS(Asym_Pause), 197 __DEFINE_LINK_MODE_PARAMS(2500, X, Full), 198 __DEFINE_SPECIAL_MODE_PARAMS(Backplane), 199 __DEFINE_LINK_MODE_PARAMS(1000, KX, Full), 200 __DEFINE_LINK_MODE_PARAMS(10000, KX4, Full), 201 __DEFINE_LINK_MODE_PARAMS(10000, KR, Full), 202 [ETHTOOL_LINK_MODE_10000baseR_FEC_BIT] = { 203 .speed = SPEED_10000, 204 .duplex = DUPLEX_FULL, 205 }, 206 __DEFINE_LINK_MODE_PARAMS(20000, MLD2, Full), 207 __DEFINE_LINK_MODE_PARAMS(20000, KR2, Full), 208 __DEFINE_LINK_MODE_PARAMS(40000, KR4, Full), 209 __DEFINE_LINK_MODE_PARAMS(40000, CR4, Full), 210 __DEFINE_LINK_MODE_PARAMS(40000, SR4, Full), 211 __DEFINE_LINK_MODE_PARAMS(40000, LR4, Full), 212 __DEFINE_LINK_MODE_PARAMS(56000, KR4, Full), 213 __DEFINE_LINK_MODE_PARAMS(56000, CR4, Full), 214 __DEFINE_LINK_MODE_PARAMS(56000, SR4, Full), 215 __DEFINE_LINK_MODE_PARAMS(56000, LR4, Full), 216 __DEFINE_LINK_MODE_PARAMS(25000, CR, Full), 217 __DEFINE_LINK_MODE_PARAMS(25000, KR, Full), 218 __DEFINE_LINK_MODE_PARAMS(25000, SR, Full), 219 __DEFINE_LINK_MODE_PARAMS(50000, CR2, Full), 220 __DEFINE_LINK_MODE_PARAMS(50000, KR2, Full), 221 __DEFINE_LINK_MODE_PARAMS(100000, KR4, Full), 222 __DEFINE_LINK_MODE_PARAMS(100000, SR4, Full), 223 __DEFINE_LINK_MODE_PARAMS(100000, CR4, Full), 224 __DEFINE_LINK_MODE_PARAMS(100000, LR4_ER4, Full), 225 __DEFINE_LINK_MODE_PARAMS(50000, SR2, Full), 226 __DEFINE_LINK_MODE_PARAMS(1000, X, Full), 227 __DEFINE_LINK_MODE_PARAMS(10000, CR, Full), 228 __DEFINE_LINK_MODE_PARAMS(10000, SR, Full), 229 __DEFINE_LINK_MODE_PARAMS(10000, LR, Full), 230 __DEFINE_LINK_MODE_PARAMS(10000, LRM, Full), 231 __DEFINE_LINK_MODE_PARAMS(10000, ER, Full), 232 __DEFINE_LINK_MODE_PARAMS(2500, T, Full), 233 __DEFINE_LINK_MODE_PARAMS(5000, T, Full), 234 __DEFINE_SPECIAL_MODE_PARAMS(FEC_NONE), 235 __DEFINE_SPECIAL_MODE_PARAMS(FEC_RS), 236 __DEFINE_SPECIAL_MODE_PARAMS(FEC_BASER), 237 __DEFINE_LINK_MODE_PARAMS(50000, KR, Full), 238 __DEFINE_LINK_MODE_PARAMS(50000, SR, Full), 239 __DEFINE_LINK_MODE_PARAMS(50000, CR, Full), 240 __DEFINE_LINK_MODE_PARAMS(50000, LR_ER_FR, Full), 241 __DEFINE_LINK_MODE_PARAMS(50000, DR, Full), 242 __DEFINE_LINK_MODE_PARAMS(100000, KR2, Full), 243 __DEFINE_LINK_MODE_PARAMS(100000, SR2, Full), 244 __DEFINE_LINK_MODE_PARAMS(100000, CR2, Full), 245 __DEFINE_LINK_MODE_PARAMS(100000, LR2_ER2_FR2, Full), 246 __DEFINE_LINK_MODE_PARAMS(100000, DR2, Full), 247 __DEFINE_LINK_MODE_PARAMS(200000, KR4, Full), 248 __DEFINE_LINK_MODE_PARAMS(200000, SR4, Full), 249 __DEFINE_LINK_MODE_PARAMS(200000, LR4_ER4_FR4, Full), 250 __DEFINE_LINK_MODE_PARAMS(200000, DR4, Full), 251 __DEFINE_LINK_MODE_PARAMS(200000, CR4, Full), 252 __DEFINE_LINK_MODE_PARAMS(100, T1, Full), 253 __DEFINE_LINK_MODE_PARAMS(1000, T1, Full), 254 __DEFINE_LINK_MODE_PARAMS(400000, KR8, Full), 255 __DEFINE_LINK_MODE_PARAMS(400000, SR8, Full), 256 __DEFINE_LINK_MODE_PARAMS(400000, LR8_ER8_FR8, Full), 257 __DEFINE_LINK_MODE_PARAMS(400000, DR8, Full), 258 __DEFINE_LINK_MODE_PARAMS(400000, CR8, Full), 259 __DEFINE_SPECIAL_MODE_PARAMS(FEC_LLRS), 260 __DEFINE_LINK_MODE_PARAMS(100000, KR, Full), 261 __DEFINE_LINK_MODE_PARAMS(100000, SR, Full), 262 __DEFINE_LINK_MODE_PARAMS(100000, LR_ER_FR, Full), 263 __DEFINE_LINK_MODE_PARAMS(100000, DR, Full), 264 __DEFINE_LINK_MODE_PARAMS(100000, CR, Full), 265 __DEFINE_LINK_MODE_PARAMS(200000, KR2, Full), 266 __DEFINE_LINK_MODE_PARAMS(200000, SR2, Full), 267 __DEFINE_LINK_MODE_PARAMS(200000, LR2_ER2_FR2, Full), 268 __DEFINE_LINK_MODE_PARAMS(200000, DR2, Full), 269 __DEFINE_LINK_MODE_PARAMS(200000, CR2, Full), 270 __DEFINE_LINK_MODE_PARAMS(400000, KR4, Full), 271 __DEFINE_LINK_MODE_PARAMS(400000, SR4, Full), 272 __DEFINE_LINK_MODE_PARAMS(400000, LR4_ER4_FR4, Full), 273 __DEFINE_LINK_MODE_PARAMS(400000, DR4, Full), 274 __DEFINE_LINK_MODE_PARAMS(400000, CR4, Full), 275 }; 276 277 static const struct nla_policy 278 linkmodes_set_policy[ETHTOOL_A_LINKMODES_MAX + 1] = { 279 [ETHTOOL_A_LINKMODES_UNSPEC] = { .type = NLA_REJECT }, 280 [ETHTOOL_A_LINKMODES_HEADER] = { .type = NLA_NESTED }, 281 [ETHTOOL_A_LINKMODES_AUTONEG] = { .type = NLA_U8 }, 282 [ETHTOOL_A_LINKMODES_OURS] = { .type = NLA_NESTED }, 283 [ETHTOOL_A_LINKMODES_PEER] = { .type = NLA_REJECT }, 284 [ETHTOOL_A_LINKMODES_SPEED] = { .type = NLA_U32 }, 285 [ETHTOOL_A_LINKMODES_DUPLEX] = { .type = NLA_U8 }, 286 [ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG] = { .type = NLA_U8 }, 287 [ETHTOOL_A_LINKMODES_MASTER_SLAVE_STATE] = { .type = NLA_REJECT }, 288 }; 289 290 /* Set advertised link modes to all supported modes matching requested speed 291 * and duplex values. Called when autonegotiation is on, speed or duplex is 292 * requested but no link mode change. This is done in userspace with ioctl() 293 * interface, move it into kernel for netlink. 294 * Returns true if advertised modes bitmap was modified. 295 */ 296 static bool ethnl_auto_linkmodes(struct ethtool_link_ksettings *ksettings, 297 bool req_speed, bool req_duplex) 298 { 299 unsigned long *advertising = ksettings->link_modes.advertising; 300 unsigned long *supported = ksettings->link_modes.supported; 301 DECLARE_BITMAP(old_adv, __ETHTOOL_LINK_MODE_MASK_NBITS); 302 unsigned int i; 303 304 BUILD_BUG_ON(ARRAY_SIZE(link_mode_params) != 305 __ETHTOOL_LINK_MODE_MASK_NBITS); 306 307 bitmap_copy(old_adv, advertising, __ETHTOOL_LINK_MODE_MASK_NBITS); 308 309 for (i = 0; i < __ETHTOOL_LINK_MODE_MASK_NBITS; i++) { 310 const struct link_mode_info *info = &link_mode_params[i]; 311 312 if (info->speed == SPEED_UNKNOWN) 313 continue; 314 if (test_bit(i, supported) && 315 (!req_speed || info->speed == ksettings->base.speed) && 316 (!req_duplex || info->duplex == ksettings->base.duplex)) 317 set_bit(i, advertising); 318 else 319 clear_bit(i, advertising); 320 } 321 322 return !bitmap_equal(old_adv, advertising, 323 __ETHTOOL_LINK_MODE_MASK_NBITS); 324 } 325 326 static bool ethnl_validate_master_slave_cfg(u8 cfg) 327 { 328 switch (cfg) { 329 case MASTER_SLAVE_CFG_MASTER_PREFERRED: 330 case MASTER_SLAVE_CFG_SLAVE_PREFERRED: 331 case MASTER_SLAVE_CFG_MASTER_FORCE: 332 case MASTER_SLAVE_CFG_SLAVE_FORCE: 333 return true; 334 } 335 336 return false; 337 } 338 339 static int ethnl_update_linkmodes(struct genl_info *info, struct nlattr **tb, 340 struct ethtool_link_ksettings *ksettings, 341 bool *mod) 342 { 343 struct ethtool_link_settings *lsettings = &ksettings->base; 344 bool req_speed, req_duplex; 345 const struct nlattr *master_slave_cfg; 346 int ret; 347 348 master_slave_cfg = tb[ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG]; 349 if (master_slave_cfg) { 350 u8 cfg = nla_get_u8(master_slave_cfg); 351 352 if (lsettings->master_slave_cfg == MASTER_SLAVE_CFG_UNSUPPORTED) { 353 NL_SET_ERR_MSG_ATTR(info->extack, master_slave_cfg, 354 "master/slave configuration not supported by device"); 355 return -EOPNOTSUPP; 356 } 357 358 if (!ethnl_validate_master_slave_cfg(cfg)) { 359 NL_SET_ERR_MSG_ATTR(info->extack, master_slave_cfg, 360 "master/slave value is invalid"); 361 return -EOPNOTSUPP; 362 } 363 } 364 365 *mod = false; 366 req_speed = tb[ETHTOOL_A_LINKMODES_SPEED]; 367 req_duplex = tb[ETHTOOL_A_LINKMODES_DUPLEX]; 368 369 ethnl_update_u8(&lsettings->autoneg, tb[ETHTOOL_A_LINKMODES_AUTONEG], 370 mod); 371 ret = ethnl_update_bitset(ksettings->link_modes.advertising, 372 __ETHTOOL_LINK_MODE_MASK_NBITS, 373 tb[ETHTOOL_A_LINKMODES_OURS], link_mode_names, 374 info->extack, mod); 375 if (ret < 0) 376 return ret; 377 ethnl_update_u32(&lsettings->speed, tb[ETHTOOL_A_LINKMODES_SPEED], 378 mod); 379 ethnl_update_u8(&lsettings->duplex, tb[ETHTOOL_A_LINKMODES_DUPLEX], 380 mod); 381 ethnl_update_u8(&lsettings->master_slave_cfg, master_slave_cfg, mod); 382 383 if (!tb[ETHTOOL_A_LINKMODES_OURS] && lsettings->autoneg && 384 (req_speed || req_duplex) && 385 ethnl_auto_linkmodes(ksettings, req_speed, req_duplex)) 386 *mod = true; 387 388 return 0; 389 } 390 391 int ethnl_set_linkmodes(struct sk_buff *skb, struct genl_info *info) 392 { 393 struct nlattr *tb[ETHTOOL_A_LINKMODES_MAX + 1]; 394 struct ethtool_link_ksettings ksettings = {}; 395 struct ethnl_req_info req_info = {}; 396 struct net_device *dev; 397 bool mod = false; 398 int ret; 399 400 ret = nlmsg_parse(info->nlhdr, GENL_HDRLEN, tb, 401 ETHTOOL_A_LINKMODES_MAX, linkmodes_set_policy, 402 info->extack); 403 if (ret < 0) 404 return ret; 405 ret = ethnl_parse_header_dev_get(&req_info, 406 tb[ETHTOOL_A_LINKMODES_HEADER], 407 genl_info_net(info), info->extack, 408 true); 409 if (ret < 0) 410 return ret; 411 dev = req_info.dev; 412 ret = -EOPNOTSUPP; 413 if (!dev->ethtool_ops->get_link_ksettings || 414 !dev->ethtool_ops->set_link_ksettings) 415 goto out_dev; 416 417 rtnl_lock(); 418 ret = ethnl_ops_begin(dev); 419 if (ret < 0) 420 goto out_rtnl; 421 422 ret = __ethtool_get_link_ksettings(dev, &ksettings); 423 if (ret < 0) { 424 GENL_SET_ERR_MSG(info, "failed to retrieve link settings"); 425 goto out_ops; 426 } 427 428 ret = ethnl_update_linkmodes(info, tb, &ksettings, &mod); 429 if (ret < 0) 430 goto out_ops; 431 432 if (mod) { 433 ret = dev->ethtool_ops->set_link_ksettings(dev, &ksettings); 434 if (ret < 0) 435 GENL_SET_ERR_MSG(info, "link settings update failed"); 436 else 437 ethtool_notify(dev, ETHTOOL_MSG_LINKMODES_NTF, NULL); 438 } 439 440 out_ops: 441 ethnl_ops_complete(dev); 442 out_rtnl: 443 rtnl_unlock(); 444 out_dev: 445 dev_put(dev); 446 return ret; 447 } 448