module.c 4.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180
  1. // SPDX-License-Identifier: GPL-2.0-only
  2. #include <linux/ethtool.h>
  3. #include "netlink.h"
  4. #include "common.h"
  5. #include "bitset.h"
  6. struct module_req_info {
  7. struct ethnl_req_info base;
  8. };
  9. struct module_reply_data {
  10. struct ethnl_reply_data base;
  11. struct ethtool_module_power_mode_params power;
  12. };
  13. #define MODULE_REPDATA(__reply_base) \
  14. container_of(__reply_base, struct module_reply_data, base)
  15. /* MODULE_GET */
  16. const struct nla_policy ethnl_module_get_policy[ETHTOOL_A_MODULE_HEADER + 1] = {
  17. [ETHTOOL_A_MODULE_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy),
  18. };
  19. static int module_get_power_mode(struct net_device *dev,
  20. struct module_reply_data *data,
  21. struct netlink_ext_ack *extack)
  22. {
  23. const struct ethtool_ops *ops = dev->ethtool_ops;
  24. if (!ops->get_module_power_mode)
  25. return 0;
  26. return ops->get_module_power_mode(dev, &data->power, extack);
  27. }
  28. static int module_prepare_data(const struct ethnl_req_info *req_base,
  29. struct ethnl_reply_data *reply_base,
  30. struct genl_info *info)
  31. {
  32. struct module_reply_data *data = MODULE_REPDATA(reply_base);
  33. struct netlink_ext_ack *extack = info ? info->extack : NULL;
  34. struct net_device *dev = reply_base->dev;
  35. int ret;
  36. ret = ethnl_ops_begin(dev);
  37. if (ret < 0)
  38. return ret;
  39. ret = module_get_power_mode(dev, data, extack);
  40. if (ret < 0)
  41. goto out_complete;
  42. out_complete:
  43. ethnl_ops_complete(dev);
  44. return ret;
  45. }
  46. static int module_reply_size(const struct ethnl_req_info *req_base,
  47. const struct ethnl_reply_data *reply_base)
  48. {
  49. struct module_reply_data *data = MODULE_REPDATA(reply_base);
  50. int len = 0;
  51. if (data->power.policy)
  52. len += nla_total_size(sizeof(u8)); /* _MODULE_POWER_MODE_POLICY */
  53. if (data->power.mode)
  54. len += nla_total_size(sizeof(u8)); /* _MODULE_POWER_MODE */
  55. return len;
  56. }
  57. static int module_fill_reply(struct sk_buff *skb,
  58. const struct ethnl_req_info *req_base,
  59. const struct ethnl_reply_data *reply_base)
  60. {
  61. const struct module_reply_data *data = MODULE_REPDATA(reply_base);
  62. if (data->power.policy &&
  63. nla_put_u8(skb, ETHTOOL_A_MODULE_POWER_MODE_POLICY,
  64. data->power.policy))
  65. return -EMSGSIZE;
  66. if (data->power.mode &&
  67. nla_put_u8(skb, ETHTOOL_A_MODULE_POWER_MODE, data->power.mode))
  68. return -EMSGSIZE;
  69. return 0;
  70. }
  71. const struct ethnl_request_ops ethnl_module_request_ops = {
  72. .request_cmd = ETHTOOL_MSG_MODULE_GET,
  73. .reply_cmd = ETHTOOL_MSG_MODULE_GET_REPLY,
  74. .hdr_attr = ETHTOOL_A_MODULE_HEADER,
  75. .req_info_size = sizeof(struct module_req_info),
  76. .reply_data_size = sizeof(struct module_reply_data),
  77. .prepare_data = module_prepare_data,
  78. .reply_size = module_reply_size,
  79. .fill_reply = module_fill_reply,
  80. };
  81. /* MODULE_SET */
  82. const struct nla_policy ethnl_module_set_policy[ETHTOOL_A_MODULE_POWER_MODE_POLICY + 1] = {
  83. [ETHTOOL_A_MODULE_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy),
  84. [ETHTOOL_A_MODULE_POWER_MODE_POLICY] =
  85. NLA_POLICY_RANGE(NLA_U8, ETHTOOL_MODULE_POWER_MODE_POLICY_HIGH,
  86. ETHTOOL_MODULE_POWER_MODE_POLICY_AUTO),
  87. };
  88. static int module_set_power_mode(struct net_device *dev, struct nlattr **tb,
  89. bool *p_mod, struct netlink_ext_ack *extack)
  90. {
  91. struct ethtool_module_power_mode_params power = {};
  92. struct ethtool_module_power_mode_params power_new;
  93. const struct ethtool_ops *ops = dev->ethtool_ops;
  94. int ret;
  95. if (!tb[ETHTOOL_A_MODULE_POWER_MODE_POLICY])
  96. return 0;
  97. if (!ops->get_module_power_mode || !ops->set_module_power_mode) {
  98. NL_SET_ERR_MSG_ATTR(extack,
  99. tb[ETHTOOL_A_MODULE_POWER_MODE_POLICY],
  100. "Setting power mode policy is not supported by this device");
  101. return -EOPNOTSUPP;
  102. }
  103. power_new.policy = nla_get_u8(tb[ETHTOOL_A_MODULE_POWER_MODE_POLICY]);
  104. ret = ops->get_module_power_mode(dev, &power, extack);
  105. if (ret < 0)
  106. return ret;
  107. if (power_new.policy == power.policy)
  108. return 0;
  109. *p_mod = true;
  110. return ops->set_module_power_mode(dev, &power_new, extack);
  111. }
  112. int ethnl_set_module(struct sk_buff *skb, struct genl_info *info)
  113. {
  114. struct ethnl_req_info req_info = {};
  115. struct nlattr **tb = info->attrs;
  116. struct net_device *dev;
  117. bool mod = false;
  118. int ret;
  119. ret = ethnl_parse_header_dev_get(&req_info, tb[ETHTOOL_A_MODULE_HEADER],
  120. genl_info_net(info), info->extack,
  121. true);
  122. if (ret < 0)
  123. return ret;
  124. dev = req_info.dev;
  125. rtnl_lock();
  126. ret = ethnl_ops_begin(dev);
  127. if (ret < 0)
  128. goto out_rtnl;
  129. ret = module_set_power_mode(dev, tb, &mod, info->extack);
  130. if (ret < 0)
  131. goto out_ops;
  132. if (!mod)
  133. goto out_ops;
  134. ethtool_notify(dev, ETHTOOL_MSG_MODULE_NTF, NULL);
  135. out_ops:
  136. ethnl_ops_complete(dev);
  137. out_rtnl:
  138. rtnl_unlock();
  139. ethnl_parse_header_dev_put(&req_info);
  140. return ret;
  141. }