pause.c 4.9 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192
  1. // SPDX-License-Identifier: GPL-2.0-only
  2. #include "netlink.h"
  3. #include "common.h"
  4. struct pause_req_info {
  5. struct ethnl_req_info base;
  6. };
  7. struct pause_reply_data {
  8. struct ethnl_reply_data base;
  9. struct ethtool_pauseparam pauseparam;
  10. struct ethtool_pause_stats pausestat;
  11. };
  12. #define PAUSE_REPDATA(__reply_base) \
  13. container_of(__reply_base, struct pause_reply_data, base)
  14. const struct nla_policy ethnl_pause_get_policy[] = {
  15. [ETHTOOL_A_PAUSE_HEADER] =
  16. NLA_POLICY_NESTED(ethnl_header_policy_stats),
  17. };
  18. static void ethtool_stats_init(u64 *stats, unsigned int n)
  19. {
  20. while (n--)
  21. stats[n] = ETHTOOL_STAT_NOT_SET;
  22. }
  23. static int pause_prepare_data(const struct ethnl_req_info *req_base,
  24. struct ethnl_reply_data *reply_base,
  25. struct genl_info *info)
  26. {
  27. struct pause_reply_data *data = PAUSE_REPDATA(reply_base);
  28. struct net_device *dev = reply_base->dev;
  29. int ret;
  30. if (!dev->ethtool_ops->get_pauseparam)
  31. return -EOPNOTSUPP;
  32. ethtool_stats_init((u64 *)&data->pausestat,
  33. sizeof(data->pausestat) / 8);
  34. ret = ethnl_ops_begin(dev);
  35. if (ret < 0)
  36. return ret;
  37. dev->ethtool_ops->get_pauseparam(dev, &data->pauseparam);
  38. if (req_base->flags & ETHTOOL_FLAG_STATS &&
  39. dev->ethtool_ops->get_pause_stats)
  40. dev->ethtool_ops->get_pause_stats(dev, &data->pausestat);
  41. ethnl_ops_complete(dev);
  42. return 0;
  43. }
  44. static int pause_reply_size(const struct ethnl_req_info *req_base,
  45. const struct ethnl_reply_data *reply_base)
  46. {
  47. int n = nla_total_size(sizeof(u8)) + /* _PAUSE_AUTONEG */
  48. nla_total_size(sizeof(u8)) + /* _PAUSE_RX */
  49. nla_total_size(sizeof(u8)); /* _PAUSE_TX */
  50. if (req_base->flags & ETHTOOL_FLAG_STATS)
  51. n += nla_total_size(0) + /* _PAUSE_STATS */
  52. nla_total_size_64bit(sizeof(u64)) * ETHTOOL_PAUSE_STAT_CNT;
  53. return n;
  54. }
  55. static int ethtool_put_stat(struct sk_buff *skb, u64 val, u16 attrtype,
  56. u16 padtype)
  57. {
  58. if (val == ETHTOOL_STAT_NOT_SET)
  59. return 0;
  60. if (nla_put_u64_64bit(skb, attrtype, val, padtype))
  61. return -EMSGSIZE;
  62. return 0;
  63. }
  64. static int pause_put_stats(struct sk_buff *skb,
  65. const struct ethtool_pause_stats *pause_stats)
  66. {
  67. const u16 pad = ETHTOOL_A_PAUSE_STAT_PAD;
  68. struct nlattr *nest;
  69. nest = nla_nest_start(skb, ETHTOOL_A_PAUSE_STATS);
  70. if (!nest)
  71. return -EMSGSIZE;
  72. if (ethtool_put_stat(skb, pause_stats->tx_pause_frames,
  73. ETHTOOL_A_PAUSE_STAT_TX_FRAMES, pad) ||
  74. ethtool_put_stat(skb, pause_stats->rx_pause_frames,
  75. ETHTOOL_A_PAUSE_STAT_RX_FRAMES, pad))
  76. goto err_cancel;
  77. nla_nest_end(skb, nest);
  78. return 0;
  79. err_cancel:
  80. nla_nest_cancel(skb, nest);
  81. return -EMSGSIZE;
  82. }
  83. static int pause_fill_reply(struct sk_buff *skb,
  84. const struct ethnl_req_info *req_base,
  85. const struct ethnl_reply_data *reply_base)
  86. {
  87. const struct pause_reply_data *data = PAUSE_REPDATA(reply_base);
  88. const struct ethtool_pauseparam *pauseparam = &data->pauseparam;
  89. if (nla_put_u8(skb, ETHTOOL_A_PAUSE_AUTONEG, !!pauseparam->autoneg) ||
  90. nla_put_u8(skb, ETHTOOL_A_PAUSE_RX, !!pauseparam->rx_pause) ||
  91. nla_put_u8(skb, ETHTOOL_A_PAUSE_TX, !!pauseparam->tx_pause))
  92. return -EMSGSIZE;
  93. if (req_base->flags & ETHTOOL_FLAG_STATS &&
  94. pause_put_stats(skb, &data->pausestat))
  95. return -EMSGSIZE;
  96. return 0;
  97. }
  98. const struct ethnl_request_ops ethnl_pause_request_ops = {
  99. .request_cmd = ETHTOOL_MSG_PAUSE_GET,
  100. .reply_cmd = ETHTOOL_MSG_PAUSE_GET_REPLY,
  101. .hdr_attr = ETHTOOL_A_PAUSE_HEADER,
  102. .req_info_size = sizeof(struct pause_req_info),
  103. .reply_data_size = sizeof(struct pause_reply_data),
  104. .prepare_data = pause_prepare_data,
  105. .reply_size = pause_reply_size,
  106. .fill_reply = pause_fill_reply,
  107. };
  108. /* PAUSE_SET */
  109. const struct nla_policy ethnl_pause_set_policy[] = {
  110. [ETHTOOL_A_PAUSE_HEADER] =
  111. NLA_POLICY_NESTED(ethnl_header_policy),
  112. [ETHTOOL_A_PAUSE_AUTONEG] = { .type = NLA_U8 },
  113. [ETHTOOL_A_PAUSE_RX] = { .type = NLA_U8 },
  114. [ETHTOOL_A_PAUSE_TX] = { .type = NLA_U8 },
  115. };
  116. int ethnl_set_pause(struct sk_buff *skb, struct genl_info *info)
  117. {
  118. struct ethtool_pauseparam params = {};
  119. struct ethnl_req_info req_info = {};
  120. struct nlattr **tb = info->attrs;
  121. const struct ethtool_ops *ops;
  122. struct net_device *dev;
  123. bool mod = false;
  124. int ret;
  125. ret = ethnl_parse_header_dev_get(&req_info,
  126. tb[ETHTOOL_A_PAUSE_HEADER],
  127. genl_info_net(info), info->extack,
  128. true);
  129. if (ret < 0)
  130. return ret;
  131. dev = req_info.dev;
  132. ops = dev->ethtool_ops;
  133. ret = -EOPNOTSUPP;
  134. if (!ops->get_pauseparam || !ops->set_pauseparam)
  135. goto out_dev;
  136. rtnl_lock();
  137. ret = ethnl_ops_begin(dev);
  138. if (ret < 0)
  139. goto out_rtnl;
  140. ops->get_pauseparam(dev, &params);
  141. ethnl_update_bool32(&params.autoneg, tb[ETHTOOL_A_PAUSE_AUTONEG], &mod);
  142. ethnl_update_bool32(&params.rx_pause, tb[ETHTOOL_A_PAUSE_RX], &mod);
  143. ethnl_update_bool32(&params.tx_pause, tb[ETHTOOL_A_PAUSE_TX], &mod);
  144. ret = 0;
  145. if (!mod)
  146. goto out_ops;
  147. ret = dev->ethtool_ops->set_pauseparam(dev, &params);
  148. if (ret < 0)
  149. goto out_ops;
  150. ethtool_notify(dev, ETHTOOL_MSG_PAUSE_NTF, NULL);
  151. out_ops:
  152. ethnl_ops_complete(dev);
  153. out_rtnl:
  154. rtnl_unlock();
  155. out_dev:
  156. dev_put(dev);
  157. return ret;
  158. }