phc_vclocks.c 2.4 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394
  1. // SPDX-License-Identifier: GPL-2.0-only
  2. /*
  3. * Copyright 2021 NXP
  4. */
  5. #include "netlink.h"
  6. #include "common.h"
  7. struct phc_vclocks_req_info {
  8. struct ethnl_req_info base;
  9. };
  10. struct phc_vclocks_reply_data {
  11. struct ethnl_reply_data base;
  12. int num;
  13. int *index;
  14. };
  15. #define PHC_VCLOCKS_REPDATA(__reply_base) \
  16. container_of(__reply_base, struct phc_vclocks_reply_data, base)
  17. const struct nla_policy ethnl_phc_vclocks_get_policy[] = {
  18. [ETHTOOL_A_PHC_VCLOCKS_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy),
  19. };
  20. static int phc_vclocks_prepare_data(const struct ethnl_req_info *req_base,
  21. struct ethnl_reply_data *reply_base,
  22. struct genl_info *info)
  23. {
  24. struct phc_vclocks_reply_data *data = PHC_VCLOCKS_REPDATA(reply_base);
  25. struct net_device *dev = reply_base->dev;
  26. int ret;
  27. ret = ethnl_ops_begin(dev);
  28. if (ret < 0)
  29. return ret;
  30. data->num = ethtool_get_phc_vclocks(dev, &data->index);
  31. ethnl_ops_complete(dev);
  32. return ret;
  33. }
  34. static int phc_vclocks_reply_size(const struct ethnl_req_info *req_base,
  35. const struct ethnl_reply_data *reply_base)
  36. {
  37. const struct phc_vclocks_reply_data *data =
  38. PHC_VCLOCKS_REPDATA(reply_base);
  39. int len = 0;
  40. if (data->num > 0) {
  41. len += nla_total_size(sizeof(u32));
  42. len += nla_total_size(sizeof(s32) * data->num);
  43. }
  44. return len;
  45. }
  46. static int phc_vclocks_fill_reply(struct sk_buff *skb,
  47. const struct ethnl_req_info *req_base,
  48. const struct ethnl_reply_data *reply_base)
  49. {
  50. const struct phc_vclocks_reply_data *data =
  51. PHC_VCLOCKS_REPDATA(reply_base);
  52. if (data->num <= 0)
  53. return 0;
  54. if (nla_put_u32(skb, ETHTOOL_A_PHC_VCLOCKS_NUM, data->num) ||
  55. nla_put(skb, ETHTOOL_A_PHC_VCLOCKS_INDEX,
  56. sizeof(s32) * data->num, data->index))
  57. return -EMSGSIZE;
  58. return 0;
  59. }
  60. static void phc_vclocks_cleanup_data(struct ethnl_reply_data *reply_base)
  61. {
  62. const struct phc_vclocks_reply_data *data =
  63. PHC_VCLOCKS_REPDATA(reply_base);
  64. kfree(data->index);
  65. }
  66. const struct ethnl_request_ops ethnl_phc_vclocks_request_ops = {
  67. .request_cmd = ETHTOOL_MSG_PHC_VCLOCKS_GET,
  68. .reply_cmd = ETHTOOL_MSG_PHC_VCLOCKS_GET_REPLY,
  69. .hdr_attr = ETHTOOL_A_PHC_VCLOCKS_HEADER,
  70. .req_info_size = sizeof(struct phc_vclocks_req_info),
  71. .reply_data_size = sizeof(struct phc_vclocks_reply_data),
  72. .prepare_data = phc_vclocks_prepare_data,
  73. .reply_size = phc_vclocks_reply_size,
  74. .fill_reply = phc_vclocks_fill_reply,
  75. .cleanup_data = phc_vclocks_cleanup_data,
  76. };