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