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