linkmodes.c 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369
  1. // SPDX-License-Identifier: GPL-2.0-only
  2. #include "netlink.h"
  3. #include "common.h"
  4. #include "bitset.h"
  5. /* LINKMODES_GET */
  6. struct linkmodes_req_info {
  7. struct ethnl_req_info base;
  8. };
  9. struct linkmodes_reply_data {
  10. struct ethnl_reply_data base;
  11. struct ethtool_link_ksettings ksettings;
  12. struct ethtool_link_settings *lsettings;
  13. bool peer_empty;
  14. };
  15. #define LINKMODES_REPDATA(__reply_base) \
  16. container_of(__reply_base, struct linkmodes_reply_data, base)
  17. const struct nla_policy ethnl_linkmodes_get_policy[] = {
  18. [ETHTOOL_A_LINKMODES_HEADER] =
  19. NLA_POLICY_NESTED(ethnl_header_policy),
  20. };
  21. static int linkmodes_prepare_data(const struct ethnl_req_info *req_base,
  22. struct ethnl_reply_data *reply_base,
  23. struct genl_info *info)
  24. {
  25. struct linkmodes_reply_data *data = LINKMODES_REPDATA(reply_base);
  26. struct net_device *dev = reply_base->dev;
  27. int ret;
  28. data->lsettings = &data->ksettings.base;
  29. ret = ethnl_ops_begin(dev);
  30. if (ret < 0)
  31. return ret;
  32. ret = __ethtool_get_link_ksettings(dev, &data->ksettings);
  33. if (ret < 0 && info) {
  34. GENL_SET_ERR_MSG(info, "failed to retrieve link settings");
  35. goto out;
  36. }
  37. if (!dev->ethtool_ops->cap_link_lanes_supported)
  38. data->ksettings.lanes = 0;
  39. data->peer_empty =
  40. bitmap_empty(data->ksettings.link_modes.lp_advertising,
  41. __ETHTOOL_LINK_MODE_MASK_NBITS);
  42. out:
  43. ethnl_ops_complete(dev);
  44. return ret;
  45. }
  46. static int linkmodes_reply_size(const struct ethnl_req_info *req_base,
  47. const struct ethnl_reply_data *reply_base)
  48. {
  49. const struct linkmodes_reply_data *data = LINKMODES_REPDATA(reply_base);
  50. const struct ethtool_link_ksettings *ksettings = &data->ksettings;
  51. const struct ethtool_link_settings *lsettings = &ksettings->base;
  52. bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
  53. int len, ret;
  54. len = nla_total_size(sizeof(u8)) /* LINKMODES_AUTONEG */
  55. + nla_total_size(sizeof(u32)) /* LINKMODES_SPEED */
  56. + nla_total_size(sizeof(u32)) /* LINKMODES_LANES */
  57. + nla_total_size(sizeof(u8)) /* LINKMODES_DUPLEX */
  58. + nla_total_size(sizeof(u8)) /* LINKMODES_RATE_MATCHING */
  59. + 0;
  60. ret = ethnl_bitset_size(ksettings->link_modes.advertising,
  61. ksettings->link_modes.supported,
  62. __ETHTOOL_LINK_MODE_MASK_NBITS,
  63. link_mode_names, compact);
  64. if (ret < 0)
  65. return ret;
  66. len += ret;
  67. if (!data->peer_empty) {
  68. ret = ethnl_bitset_size(ksettings->link_modes.lp_advertising,
  69. NULL, __ETHTOOL_LINK_MODE_MASK_NBITS,
  70. link_mode_names, compact);
  71. if (ret < 0)
  72. return ret;
  73. len += ret;
  74. }
  75. if (lsettings->master_slave_cfg != MASTER_SLAVE_CFG_UNSUPPORTED)
  76. len += nla_total_size(sizeof(u8));
  77. if (lsettings->master_slave_state != MASTER_SLAVE_STATE_UNSUPPORTED)
  78. len += nla_total_size(sizeof(u8));
  79. return len;
  80. }
  81. static int linkmodes_fill_reply(struct sk_buff *skb,
  82. const struct ethnl_req_info *req_base,
  83. const struct ethnl_reply_data *reply_base)
  84. {
  85. const struct linkmodes_reply_data *data = LINKMODES_REPDATA(reply_base);
  86. const struct ethtool_link_ksettings *ksettings = &data->ksettings;
  87. const struct ethtool_link_settings *lsettings = &ksettings->base;
  88. bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
  89. int ret;
  90. if (nla_put_u8(skb, ETHTOOL_A_LINKMODES_AUTONEG, lsettings->autoneg))
  91. return -EMSGSIZE;
  92. ret = ethnl_put_bitset(skb, ETHTOOL_A_LINKMODES_OURS,
  93. ksettings->link_modes.advertising,
  94. ksettings->link_modes.supported,
  95. __ETHTOOL_LINK_MODE_MASK_NBITS, link_mode_names,
  96. compact);
  97. if (ret < 0)
  98. return -EMSGSIZE;
  99. if (!data->peer_empty) {
  100. ret = ethnl_put_bitset(skb, ETHTOOL_A_LINKMODES_PEER,
  101. ksettings->link_modes.lp_advertising,
  102. NULL, __ETHTOOL_LINK_MODE_MASK_NBITS,
  103. link_mode_names, compact);
  104. if (ret < 0)
  105. return -EMSGSIZE;
  106. }
  107. if (nla_put_u32(skb, ETHTOOL_A_LINKMODES_SPEED, lsettings->speed) ||
  108. nla_put_u8(skb, ETHTOOL_A_LINKMODES_DUPLEX, lsettings->duplex))
  109. return -EMSGSIZE;
  110. if (ksettings->lanes &&
  111. nla_put_u32(skb, ETHTOOL_A_LINKMODES_LANES, ksettings->lanes))
  112. return -EMSGSIZE;
  113. if (lsettings->master_slave_cfg != MASTER_SLAVE_CFG_UNSUPPORTED &&
  114. nla_put_u8(skb, ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG,
  115. lsettings->master_slave_cfg))
  116. return -EMSGSIZE;
  117. if (lsettings->master_slave_state != MASTER_SLAVE_STATE_UNSUPPORTED &&
  118. nla_put_u8(skb, ETHTOOL_A_LINKMODES_MASTER_SLAVE_STATE,
  119. lsettings->master_slave_state))
  120. return -EMSGSIZE;
  121. if (nla_put_u8(skb, ETHTOOL_A_LINKMODES_RATE_MATCHING,
  122. lsettings->rate_matching))
  123. return -EMSGSIZE;
  124. return 0;
  125. }
  126. const struct ethnl_request_ops ethnl_linkmodes_request_ops = {
  127. .request_cmd = ETHTOOL_MSG_LINKMODES_GET,
  128. .reply_cmd = ETHTOOL_MSG_LINKMODES_GET_REPLY,
  129. .hdr_attr = ETHTOOL_A_LINKMODES_HEADER,
  130. .req_info_size = sizeof(struct linkmodes_req_info),
  131. .reply_data_size = sizeof(struct linkmodes_reply_data),
  132. .prepare_data = linkmodes_prepare_data,
  133. .reply_size = linkmodes_reply_size,
  134. .fill_reply = linkmodes_fill_reply,
  135. };
  136. /* LINKMODES_SET */
  137. const struct nla_policy ethnl_linkmodes_set_policy[] = {
  138. [ETHTOOL_A_LINKMODES_HEADER] =
  139. NLA_POLICY_NESTED(ethnl_header_policy),
  140. [ETHTOOL_A_LINKMODES_AUTONEG] = { .type = NLA_U8 },
  141. [ETHTOOL_A_LINKMODES_OURS] = { .type = NLA_NESTED },
  142. [ETHTOOL_A_LINKMODES_SPEED] = { .type = NLA_U32 },
  143. [ETHTOOL_A_LINKMODES_DUPLEX] = { .type = NLA_U8 },
  144. [ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG] = { .type = NLA_U8 },
  145. [ETHTOOL_A_LINKMODES_LANES] = NLA_POLICY_RANGE(NLA_U32, 1, 8),
  146. };
  147. /* Set advertised link modes to all supported modes matching requested speed,
  148. * lanes and duplex values. Called when autonegotiation is on, speed, lanes or
  149. * duplex is requested but no link mode change. This is done in userspace with
  150. * ioctl() interface, move it into kernel for netlink.
  151. * Returns true if advertised modes bitmap was modified.
  152. */
  153. static bool ethnl_auto_linkmodes(struct ethtool_link_ksettings *ksettings,
  154. bool req_speed, bool req_lanes, bool req_duplex)
  155. {
  156. unsigned long *advertising = ksettings->link_modes.advertising;
  157. unsigned long *supported = ksettings->link_modes.supported;
  158. DECLARE_BITMAP(old_adv, __ETHTOOL_LINK_MODE_MASK_NBITS);
  159. unsigned int i;
  160. bitmap_copy(old_adv, advertising, __ETHTOOL_LINK_MODE_MASK_NBITS);
  161. for (i = 0; i < __ETHTOOL_LINK_MODE_MASK_NBITS; i++) {
  162. const struct link_mode_info *info = &link_mode_params[i];
  163. if (info->speed == SPEED_UNKNOWN)
  164. continue;
  165. if (test_bit(i, supported) &&
  166. (!req_speed || info->speed == ksettings->base.speed) &&
  167. (!req_lanes || info->lanes == ksettings->lanes) &&
  168. (!req_duplex || info->duplex == ksettings->base.duplex))
  169. set_bit(i, advertising);
  170. else
  171. clear_bit(i, advertising);
  172. }
  173. return !bitmap_equal(old_adv, advertising,
  174. __ETHTOOL_LINK_MODE_MASK_NBITS);
  175. }
  176. static bool ethnl_validate_master_slave_cfg(u8 cfg)
  177. {
  178. switch (cfg) {
  179. case MASTER_SLAVE_CFG_MASTER_PREFERRED:
  180. case MASTER_SLAVE_CFG_SLAVE_PREFERRED:
  181. case MASTER_SLAVE_CFG_MASTER_FORCE:
  182. case MASTER_SLAVE_CFG_SLAVE_FORCE:
  183. return true;
  184. }
  185. return false;
  186. }
  187. static int ethnl_check_linkmodes(struct genl_info *info, struct nlattr **tb)
  188. {
  189. const struct nlattr *master_slave_cfg, *lanes_cfg;
  190. master_slave_cfg = tb[ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG];
  191. if (master_slave_cfg &&
  192. !ethnl_validate_master_slave_cfg(nla_get_u8(master_slave_cfg))) {
  193. NL_SET_ERR_MSG_ATTR(info->extack, master_slave_cfg,
  194. "master/slave value is invalid");
  195. return -EOPNOTSUPP;
  196. }
  197. lanes_cfg = tb[ETHTOOL_A_LINKMODES_LANES];
  198. if (lanes_cfg && !is_power_of_2(nla_get_u32(lanes_cfg))) {
  199. NL_SET_ERR_MSG_ATTR(info->extack, lanes_cfg,
  200. "lanes value is invalid");
  201. return -EINVAL;
  202. }
  203. return 0;
  204. }
  205. static int ethnl_update_linkmodes(struct genl_info *info, struct nlattr **tb,
  206. struct ethtool_link_ksettings *ksettings,
  207. bool *mod, const struct net_device *dev)
  208. {
  209. struct ethtool_link_settings *lsettings = &ksettings->base;
  210. bool req_speed, req_lanes, req_duplex;
  211. const struct nlattr *master_slave_cfg, *lanes_cfg;
  212. int ret;
  213. master_slave_cfg = tb[ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG];
  214. if (master_slave_cfg) {
  215. if (lsettings->master_slave_cfg == MASTER_SLAVE_CFG_UNSUPPORTED) {
  216. NL_SET_ERR_MSG_ATTR(info->extack, master_slave_cfg,
  217. "master/slave configuration not supported by device");
  218. return -EOPNOTSUPP;
  219. }
  220. }
  221. *mod = false;
  222. req_speed = tb[ETHTOOL_A_LINKMODES_SPEED];
  223. req_lanes = tb[ETHTOOL_A_LINKMODES_LANES];
  224. req_duplex = tb[ETHTOOL_A_LINKMODES_DUPLEX];
  225. ethnl_update_u8(&lsettings->autoneg, tb[ETHTOOL_A_LINKMODES_AUTONEG],
  226. mod);
  227. lanes_cfg = tb[ETHTOOL_A_LINKMODES_LANES];
  228. if (lanes_cfg) {
  229. /* If autoneg is off and lanes parameter is not supported by the
  230. * driver, return an error.
  231. */
  232. if (!lsettings->autoneg &&
  233. !dev->ethtool_ops->cap_link_lanes_supported) {
  234. NL_SET_ERR_MSG_ATTR(info->extack, lanes_cfg,
  235. "lanes configuration not supported by device");
  236. return -EOPNOTSUPP;
  237. }
  238. } else if (!lsettings->autoneg && ksettings->lanes) {
  239. /* If autoneg is off and lanes parameter is not passed from user but
  240. * it was defined previously then set the lanes parameter to 0.
  241. */
  242. ksettings->lanes = 0;
  243. *mod = true;
  244. }
  245. ret = ethnl_update_bitset(ksettings->link_modes.advertising,
  246. __ETHTOOL_LINK_MODE_MASK_NBITS,
  247. tb[ETHTOOL_A_LINKMODES_OURS], link_mode_names,
  248. info->extack, mod);
  249. if (ret < 0)
  250. return ret;
  251. ethnl_update_u32(&lsettings->speed, tb[ETHTOOL_A_LINKMODES_SPEED],
  252. mod);
  253. ethnl_update_u32(&ksettings->lanes, lanes_cfg, mod);
  254. ethnl_update_u8(&lsettings->duplex, tb[ETHTOOL_A_LINKMODES_DUPLEX],
  255. mod);
  256. ethnl_update_u8(&lsettings->master_slave_cfg, master_slave_cfg, mod);
  257. if (!tb[ETHTOOL_A_LINKMODES_OURS] && lsettings->autoneg &&
  258. (req_speed || req_lanes || req_duplex) &&
  259. ethnl_auto_linkmodes(ksettings, req_speed, req_lanes, req_duplex))
  260. *mod = true;
  261. return 0;
  262. }
  263. int ethnl_set_linkmodes(struct sk_buff *skb, struct genl_info *info)
  264. {
  265. struct ethtool_link_ksettings ksettings = {};
  266. struct ethnl_req_info req_info = {};
  267. struct nlattr **tb = info->attrs;
  268. struct net_device *dev;
  269. bool mod = false;
  270. int ret;
  271. ret = ethnl_check_linkmodes(info, tb);
  272. if (ret < 0)
  273. return ret;
  274. ret = ethnl_parse_header_dev_get(&req_info,
  275. tb[ETHTOOL_A_LINKMODES_HEADER],
  276. genl_info_net(info), info->extack,
  277. true);
  278. if (ret < 0)
  279. return ret;
  280. dev = req_info.dev;
  281. ret = -EOPNOTSUPP;
  282. if (!dev->ethtool_ops->get_link_ksettings ||
  283. !dev->ethtool_ops->set_link_ksettings)
  284. goto out_dev;
  285. rtnl_lock();
  286. ret = ethnl_ops_begin(dev);
  287. if (ret < 0)
  288. goto out_rtnl;
  289. ret = __ethtool_get_link_ksettings(dev, &ksettings);
  290. if (ret < 0) {
  291. GENL_SET_ERR_MSG(info, "failed to retrieve link settings");
  292. goto out_ops;
  293. }
  294. ret = ethnl_update_linkmodes(info, tb, &ksettings, &mod, dev);
  295. if (ret < 0)
  296. goto out_ops;
  297. if (mod) {
  298. ret = dev->ethtool_ops->set_link_ksettings(dev, &ksettings);
  299. if (ret < 0)
  300. GENL_SET_ERR_MSG(info, "link settings update failed");
  301. else
  302. ethtool_notify(dev, ETHTOOL_MSG_LINKMODES_NTF, NULL);
  303. }
  304. out_ops:
  305. ethnl_ops_complete(dev);
  306. out_rtnl:
  307. rtnl_unlock();
  308. out_dev:
  309. ethnl_parse_header_dev_put(&req_info);
  310. return ret;
  311. }