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 }; 261 262 static const struct nla_policy 263 linkmodes_set_policy[ETHTOOL_A_LINKMODES_MAX + 1] = { 264 [ETHTOOL_A_LINKMODES_UNSPEC] = { .type = NLA_REJECT }, 265 [ETHTOOL_A_LINKMODES_HEADER] = { .type = NLA_NESTED }, 266 [ETHTOOL_A_LINKMODES_AUTONEG] = { .type = NLA_U8 }, 267 [ETHTOOL_A_LINKMODES_OURS] = { .type = NLA_NESTED }, 268 [ETHTOOL_A_LINKMODES_PEER] = { .type = NLA_REJECT }, 269 [ETHTOOL_A_LINKMODES_SPEED] = { .type = NLA_U32 }, 270 [ETHTOOL_A_LINKMODES_DUPLEX] = { .type = NLA_U8 }, 271 [ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG] = { .type = NLA_U8 }, 272 [ETHTOOL_A_LINKMODES_MASTER_SLAVE_STATE] = { .type = NLA_REJECT }, 273 }; 274 275 /* Set advertised link modes to all supported modes matching requested speed 276 * and duplex values. Called when autonegotiation is on, speed or duplex is 277 * requested but no link mode change. This is done in userspace with ioctl() 278 * interface, move it into kernel for netlink. 279 * Returns true if advertised modes bitmap was modified. 280 */ 281 static bool ethnl_auto_linkmodes(struct ethtool_link_ksettings *ksettings, 282 bool req_speed, bool req_duplex) 283 { 284 unsigned long *advertising = ksettings->link_modes.advertising; 285 unsigned long *supported = ksettings->link_modes.supported; 286 DECLARE_BITMAP(old_adv, __ETHTOOL_LINK_MODE_MASK_NBITS); 287 unsigned int i; 288 289 BUILD_BUG_ON(ARRAY_SIZE(link_mode_params) != 290 __ETHTOOL_LINK_MODE_MASK_NBITS); 291 292 bitmap_copy(old_adv, advertising, __ETHTOOL_LINK_MODE_MASK_NBITS); 293 294 for (i = 0; i < __ETHTOOL_LINK_MODE_MASK_NBITS; i++) { 295 const struct link_mode_info *info = &link_mode_params[i]; 296 297 if (info->speed == SPEED_UNKNOWN) 298 continue; 299 if (test_bit(i, supported) && 300 (!req_speed || info->speed == ksettings->base.speed) && 301 (!req_duplex || info->duplex == ksettings->base.duplex)) 302 set_bit(i, advertising); 303 else 304 clear_bit(i, advertising); 305 } 306 307 return !bitmap_equal(old_adv, advertising, 308 __ETHTOOL_LINK_MODE_MASK_NBITS); 309 } 310 311 static bool ethnl_validate_master_slave_cfg(u8 cfg) 312 { 313 switch (cfg) { 314 case MASTER_SLAVE_CFG_MASTER_PREFERRED: 315 case MASTER_SLAVE_CFG_SLAVE_PREFERRED: 316 case MASTER_SLAVE_CFG_MASTER_FORCE: 317 case MASTER_SLAVE_CFG_SLAVE_FORCE: 318 return true; 319 } 320 321 return false; 322 } 323 324 static int ethnl_update_linkmodes(struct genl_info *info, struct nlattr **tb, 325 struct ethtool_link_ksettings *ksettings, 326 bool *mod) 327 { 328 struct ethtool_link_settings *lsettings = &ksettings->base; 329 bool req_speed, req_duplex; 330 const struct nlattr *master_slave_cfg; 331 int ret; 332 333 master_slave_cfg = tb[ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG]; 334 if (master_slave_cfg) { 335 u8 cfg = nla_get_u8(master_slave_cfg); 336 337 if (lsettings->master_slave_cfg == MASTER_SLAVE_CFG_UNSUPPORTED) { 338 NL_SET_ERR_MSG_ATTR(info->extack, master_slave_cfg, 339 "master/slave configuration not supported by device"); 340 return -EOPNOTSUPP; 341 } 342 343 if (!ethnl_validate_master_slave_cfg(cfg)) { 344 NL_SET_ERR_MSG_ATTR(info->extack, master_slave_cfg, 345 "master/slave value is invalid"); 346 return -EOPNOTSUPP; 347 } 348 } 349 350 *mod = false; 351 req_speed = tb[ETHTOOL_A_LINKMODES_SPEED]; 352 req_duplex = tb[ETHTOOL_A_LINKMODES_DUPLEX]; 353 354 ethnl_update_u8(&lsettings->autoneg, tb[ETHTOOL_A_LINKMODES_AUTONEG], 355 mod); 356 ret = ethnl_update_bitset(ksettings->link_modes.advertising, 357 __ETHTOOL_LINK_MODE_MASK_NBITS, 358 tb[ETHTOOL_A_LINKMODES_OURS], link_mode_names, 359 info->extack, mod); 360 if (ret < 0) 361 return ret; 362 ethnl_update_u32(&lsettings->speed, tb[ETHTOOL_A_LINKMODES_SPEED], 363 mod); 364 ethnl_update_u8(&lsettings->duplex, tb[ETHTOOL_A_LINKMODES_DUPLEX], 365 mod); 366 ethnl_update_u8(&lsettings->master_slave_cfg, master_slave_cfg, mod); 367 368 if (!tb[ETHTOOL_A_LINKMODES_OURS] && lsettings->autoneg && 369 (req_speed || req_duplex) && 370 ethnl_auto_linkmodes(ksettings, req_speed, req_duplex)) 371 *mod = true; 372 373 return 0; 374 } 375 376 int ethnl_set_linkmodes(struct sk_buff *skb, struct genl_info *info) 377 { 378 struct nlattr *tb[ETHTOOL_A_LINKMODES_MAX + 1]; 379 struct ethtool_link_ksettings ksettings = {}; 380 struct ethnl_req_info req_info = {}; 381 struct net_device *dev; 382 bool mod = false; 383 int ret; 384 385 ret = nlmsg_parse(info->nlhdr, GENL_HDRLEN, tb, 386 ETHTOOL_A_LINKMODES_MAX, linkmodes_set_policy, 387 info->extack); 388 if (ret < 0) 389 return ret; 390 ret = ethnl_parse_header_dev_get(&req_info, 391 tb[ETHTOOL_A_LINKMODES_HEADER], 392 genl_info_net(info), info->extack, 393 true); 394 if (ret < 0) 395 return ret; 396 dev = req_info.dev; 397 ret = -EOPNOTSUPP; 398 if (!dev->ethtool_ops->get_link_ksettings || 399 !dev->ethtool_ops->set_link_ksettings) 400 goto out_dev; 401 402 rtnl_lock(); 403 ret = ethnl_ops_begin(dev); 404 if (ret < 0) 405 goto out_rtnl; 406 407 ret = __ethtool_get_link_ksettings(dev, &ksettings); 408 if (ret < 0) { 409 if (info) 410 GENL_SET_ERR_MSG(info, "failed to retrieve link settings"); 411 goto out_ops; 412 } 413 414 ret = ethnl_update_linkmodes(info, tb, &ksettings, &mod); 415 if (ret < 0) 416 goto out_ops; 417 418 if (mod) { 419 ret = dev->ethtool_ops->set_link_ksettings(dev, &ksettings); 420 if (ret < 0) 421 GENL_SET_ERR_MSG(info, "link settings update failed"); 422 else 423 ethtool_notify(dev, ETHTOOL_MSG_LINKMODES_NTF, NULL); 424 } 425 426 out_ops: 427 ethnl_ops_complete(dev); 428 out_rtnl: 429 rtnl_unlock(); 430 out_dev: 431 dev_put(dev); 432 return ret; 433 } 434