1 // SPDX-License-Identifier: GPL-2.0-only 2 3 #include <net/netdev_lock.h> 4 5 #include "netlink.h" 6 #include "common.h" 7 #include "bitset.h" 8 9 struct features_req_info { 10 struct ethnl_req_info base; 11 }; 12 13 struct features_reply_data { 14 struct ethnl_reply_data base; 15 u32 hw[ETHTOOL_DEV_FEATURE_WORDS]; 16 u32 wanted[ETHTOOL_DEV_FEATURE_WORDS]; 17 u32 active[ETHTOOL_DEV_FEATURE_WORDS]; 18 u32 nochange[ETHTOOL_DEV_FEATURE_WORDS]; 19 u32 all[ETHTOOL_DEV_FEATURE_WORDS]; 20 }; 21 22 #define FEATURES_REPDATA(__reply_base) \ 23 container_of(__reply_base, struct features_reply_data, base) 24 25 const struct nla_policy ethnl_features_get_policy[] = { 26 [ETHTOOL_A_FEATURES_HEADER] = 27 NLA_POLICY_NESTED(ethnl_header_policy), 28 }; 29 30 static void ethnl_features_to_bitmap32(u32 *dest, netdev_features_t src) 31 { 32 unsigned int i; 33 34 for (i = 0; i < ETHTOOL_DEV_FEATURE_WORDS; i++) 35 dest[i] = src >> (32 * i); 36 } 37 38 static int features_prepare_data(const struct ethnl_req_info *req_base, 39 struct ethnl_reply_data *reply_base, 40 const struct genl_info *info) 41 { 42 struct features_reply_data *data = FEATURES_REPDATA(reply_base); 43 struct net_device *dev = reply_base->dev; 44 netdev_features_t all_features; 45 46 ethnl_features_to_bitmap32(data->hw, dev->hw_features); 47 ethnl_features_to_bitmap32(data->wanted, dev->wanted_features); 48 ethnl_features_to_bitmap32(data->active, dev->features); 49 ethnl_features_to_bitmap32(data->nochange, NETIF_F_NEVER_CHANGE); 50 all_features = GENMASK_ULL(NETDEV_FEATURE_COUNT - 1, 0); 51 ethnl_features_to_bitmap32(data->all, all_features); 52 53 return 0; 54 } 55 56 static int features_reply_size(const struct ethnl_req_info *req_base, 57 const struct ethnl_reply_data *reply_base) 58 { 59 const struct features_reply_data *data = FEATURES_REPDATA(reply_base); 60 bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS; 61 unsigned int len = 0; 62 int ret; 63 64 ret = ethnl_bitset32_size(data->hw, data->all, NETDEV_FEATURE_COUNT, 65 netdev_features_strings, compact); 66 if (ret < 0) 67 return ret; 68 len += ret; 69 ret = ethnl_bitset32_size(data->wanted, NULL, NETDEV_FEATURE_COUNT, 70 netdev_features_strings, compact); 71 if (ret < 0) 72 return ret; 73 len += ret; 74 ret = ethnl_bitset32_size(data->active, NULL, NETDEV_FEATURE_COUNT, 75 netdev_features_strings, compact); 76 if (ret < 0) 77 return ret; 78 len += ret; 79 ret = ethnl_bitset32_size(data->nochange, NULL, NETDEV_FEATURE_COUNT, 80 netdev_features_strings, compact); 81 if (ret < 0) 82 return ret; 83 len += ret; 84 85 return len; 86 } 87 88 static int features_fill_reply(struct sk_buff *skb, 89 const struct ethnl_req_info *req_base, 90 const struct ethnl_reply_data *reply_base) 91 { 92 const struct features_reply_data *data = FEATURES_REPDATA(reply_base); 93 bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS; 94 int ret; 95 96 ret = ethnl_put_bitset32(skb, ETHTOOL_A_FEATURES_HW, data->hw, 97 data->all, NETDEV_FEATURE_COUNT, 98 netdev_features_strings, compact); 99 if (ret < 0) 100 return ret; 101 ret = ethnl_put_bitset32(skb, ETHTOOL_A_FEATURES_WANTED, data->wanted, 102 NULL, NETDEV_FEATURE_COUNT, 103 netdev_features_strings, compact); 104 if (ret < 0) 105 return ret; 106 ret = ethnl_put_bitset32(skb, ETHTOOL_A_FEATURES_ACTIVE, data->active, 107 NULL, NETDEV_FEATURE_COUNT, 108 netdev_features_strings, compact); 109 if (ret < 0) 110 return ret; 111 return ethnl_put_bitset32(skb, ETHTOOL_A_FEATURES_NOCHANGE, 112 data->nochange, NULL, NETDEV_FEATURE_COUNT, 113 netdev_features_strings, compact); 114 } 115 116 const struct ethnl_request_ops ethnl_features_request_ops = { 117 .request_cmd = ETHTOOL_MSG_FEATURES_GET, 118 .reply_cmd = ETHTOOL_MSG_FEATURES_GET_REPLY, 119 .hdr_attr = ETHTOOL_A_FEATURES_HEADER, 120 .req_info_size = sizeof(struct features_req_info), 121 .reply_data_size = sizeof(struct features_reply_data), 122 123 .prepare_data = features_prepare_data, 124 .reply_size = features_reply_size, 125 .fill_reply = features_fill_reply, 126 }; 127 128 /* FEATURES_SET */ 129 130 const struct nla_policy ethnl_features_set_policy[] = { 131 [ETHTOOL_A_FEATURES_HEADER] = 132 NLA_POLICY_NESTED(ethnl_header_policy), 133 [ETHTOOL_A_FEATURES_WANTED] = { .type = NLA_NESTED }, 134 }; 135 136 static void ethnl_features_to_bitmap(unsigned long *dest, netdev_features_t val) 137 { 138 const unsigned int words = BITS_TO_LONGS(NETDEV_FEATURE_COUNT); 139 unsigned int i; 140 141 for (i = 0; i < words; i++) 142 dest[i] = (unsigned long)(val >> (i * BITS_PER_LONG)); 143 } 144 145 static netdev_features_t ethnl_bitmap_to_features(unsigned long *src) 146 { 147 const unsigned int nft_bits = sizeof(netdev_features_t) * BITS_PER_BYTE; 148 const unsigned int words = BITS_TO_LONGS(NETDEV_FEATURE_COUNT); 149 netdev_features_t ret = 0; 150 unsigned int i; 151 152 for (i = 0; i < words; i++) 153 ret |= (netdev_features_t)(src[i]) << (i * BITS_PER_LONG); 154 ret &= ~(netdev_features_t)0 >> (nft_bits - NETDEV_FEATURE_COUNT); 155 return ret; 156 } 157 158 static int features_send_reply(struct net_device *dev, struct genl_info *info, 159 const unsigned long *wanted, 160 const unsigned long *wanted_mask, 161 const unsigned long *active, 162 const unsigned long *active_mask, bool compact) 163 { 164 struct sk_buff *rskb; 165 void *reply_payload; 166 int reply_len = 0; 167 int ret; 168 169 reply_len = ethnl_reply_header_size(); 170 ret = ethnl_bitset_size(wanted, wanted_mask, NETDEV_FEATURE_COUNT, 171 netdev_features_strings, compact); 172 if (ret < 0) 173 goto err; 174 reply_len += ret; 175 ret = ethnl_bitset_size(active, active_mask, NETDEV_FEATURE_COUNT, 176 netdev_features_strings, compact); 177 if (ret < 0) 178 goto err; 179 reply_len += ret; 180 181 ret = -ENOMEM; 182 rskb = ethnl_reply_init(reply_len, dev, ETHTOOL_MSG_FEATURES_SET_REPLY, 183 ETHTOOL_A_FEATURES_HEADER, info, 184 &reply_payload); 185 if (!rskb) 186 goto err; 187 188 ret = ethnl_put_bitset(rskb, ETHTOOL_A_FEATURES_WANTED, wanted, 189 wanted_mask, NETDEV_FEATURE_COUNT, 190 netdev_features_strings, compact); 191 if (ret < 0) 192 goto nla_put_failure; 193 ret = ethnl_put_bitset(rskb, ETHTOOL_A_FEATURES_ACTIVE, active, 194 active_mask, NETDEV_FEATURE_COUNT, 195 netdev_features_strings, compact); 196 if (ret < 0) 197 goto nla_put_failure; 198 199 genlmsg_end(rskb, reply_payload); 200 ret = genlmsg_reply(rskb, info); 201 return ret; 202 203 nla_put_failure: 204 nlmsg_free(rskb); 205 WARN_ONCE(1, "calculated message payload length (%d) not sufficient\n", 206 reply_len); 207 err: 208 GENL_SET_ERR_MSG(info, "failed to send reply message"); 209 return ret; 210 } 211 212 int ethnl_set_features(struct sk_buff *skb, struct genl_info *info) 213 { 214 DECLARE_BITMAP(wanted_diff_mask, NETDEV_FEATURE_COUNT); 215 DECLARE_BITMAP(active_diff_mask, NETDEV_FEATURE_COUNT); 216 DECLARE_BITMAP(old_active, NETDEV_FEATURE_COUNT); 217 DECLARE_BITMAP(old_wanted, NETDEV_FEATURE_COUNT); 218 DECLARE_BITMAP(new_active, NETDEV_FEATURE_COUNT); 219 DECLARE_BITMAP(new_wanted, NETDEV_FEATURE_COUNT); 220 DECLARE_BITMAP(req_wanted, NETDEV_FEATURE_COUNT); 221 DECLARE_BITMAP(req_mask, NETDEV_FEATURE_COUNT); 222 struct ethnl_req_info req_info = {}; 223 struct nlattr **tb = info->attrs; 224 struct net_device *dev; 225 bool mod; 226 int ret; 227 228 if (!tb[ETHTOOL_A_FEATURES_WANTED]) 229 return -EINVAL; 230 ret = ethnl_parse_header_dev_get(&req_info, 231 tb[ETHTOOL_A_FEATURES_HEADER], 232 genl_info_net(info), info->extack, 233 true); 234 if (ret < 0) 235 return ret; 236 dev = req_info.dev; 237 238 rtnl_lock(); 239 netdev_lock_ops(dev); 240 ret = ethnl_ops_begin(dev); 241 if (ret < 0) 242 goto out_unlock; 243 ethnl_features_to_bitmap(old_active, dev->features); 244 ethnl_features_to_bitmap(old_wanted, dev->wanted_features); 245 ret = ethnl_parse_bitset(req_wanted, req_mask, NETDEV_FEATURE_COUNT, 246 tb[ETHTOOL_A_FEATURES_WANTED], 247 netdev_features_strings, info->extack); 248 if (ret < 0) 249 goto out_ops; 250 if (ethnl_bitmap_to_features(req_mask) & ~NETIF_F_ETHTOOL_BITS) { 251 GENL_SET_ERR_MSG(info, "attempt to change non-ethtool features"); 252 ret = -EINVAL; 253 goto out_ops; 254 } 255 256 /* set req_wanted bits not in req_mask from old_wanted */ 257 bitmap_and(req_wanted, req_wanted, req_mask, NETDEV_FEATURE_COUNT); 258 bitmap_andnot(new_wanted, old_wanted, req_mask, NETDEV_FEATURE_COUNT); 259 bitmap_or(req_wanted, new_wanted, req_wanted, NETDEV_FEATURE_COUNT); 260 if (!bitmap_equal(req_wanted, old_wanted, NETDEV_FEATURE_COUNT)) { 261 dev->wanted_features &= ~dev->hw_features; 262 dev->wanted_features |= ethnl_bitmap_to_features(req_wanted) & dev->hw_features; 263 __netdev_update_features(dev); 264 } 265 ethnl_features_to_bitmap(new_active, dev->features); 266 mod = !bitmap_equal(old_active, new_active, NETDEV_FEATURE_COUNT); 267 268 ret = 0; 269 if (!(req_info.flags & ETHTOOL_FLAG_OMIT_REPLY)) { 270 bool compact = req_info.flags & ETHTOOL_FLAG_COMPACT_BITSETS; 271 272 bitmap_xor(wanted_diff_mask, req_wanted, new_active, 273 NETDEV_FEATURE_COUNT); 274 bitmap_xor(active_diff_mask, old_active, new_active, 275 NETDEV_FEATURE_COUNT); 276 bitmap_and(wanted_diff_mask, wanted_diff_mask, req_mask, 277 NETDEV_FEATURE_COUNT); 278 bitmap_and(req_wanted, req_wanted, wanted_diff_mask, 279 NETDEV_FEATURE_COUNT); 280 bitmap_and(new_active, new_active, active_diff_mask, 281 NETDEV_FEATURE_COUNT); 282 283 ret = features_send_reply(dev, info, req_wanted, 284 wanted_diff_mask, new_active, 285 active_diff_mask, compact); 286 } 287 if (mod) 288 netdev_features_change(dev); 289 290 out_ops: 291 ethnl_ops_complete(dev); 292 out_unlock: 293 netdev_unlock_ops(dev); 294 rtnl_unlock(); 295 ethnl_parse_header_dev_put(&req_info); 296 return ret; 297 } 298