0001
0002
0003 #include "netlink.h"
0004 #include "common.h"
0005 #include "bitset.h"
0006
0007 struct features_req_info {
0008 struct ethnl_req_info base;
0009 };
0010
0011 struct features_reply_data {
0012 struct ethnl_reply_data base;
0013 u32 hw[ETHTOOL_DEV_FEATURE_WORDS];
0014 u32 wanted[ETHTOOL_DEV_FEATURE_WORDS];
0015 u32 active[ETHTOOL_DEV_FEATURE_WORDS];
0016 u32 nochange[ETHTOOL_DEV_FEATURE_WORDS];
0017 u32 all[ETHTOOL_DEV_FEATURE_WORDS];
0018 };
0019
0020 #define FEATURES_REPDATA(__reply_base) \
0021 container_of(__reply_base, struct features_reply_data, base)
0022
0023 const struct nla_policy ethnl_features_get_policy[] = {
0024 [ETHTOOL_A_FEATURES_HEADER] =
0025 NLA_POLICY_NESTED(ethnl_header_policy),
0026 };
0027
0028 static void ethnl_features_to_bitmap32(u32 *dest, netdev_features_t src)
0029 {
0030 unsigned int i;
0031
0032 for (i = 0; i < ETHTOOL_DEV_FEATURE_WORDS; i++)
0033 dest[i] = src >> (32 * i);
0034 }
0035
0036 static int features_prepare_data(const struct ethnl_req_info *req_base,
0037 struct ethnl_reply_data *reply_base,
0038 struct genl_info *info)
0039 {
0040 struct features_reply_data *data = FEATURES_REPDATA(reply_base);
0041 struct net_device *dev = reply_base->dev;
0042 netdev_features_t all_features;
0043
0044 ethnl_features_to_bitmap32(data->hw, dev->hw_features);
0045 ethnl_features_to_bitmap32(data->wanted, dev->wanted_features);
0046 ethnl_features_to_bitmap32(data->active, dev->features);
0047 ethnl_features_to_bitmap32(data->nochange, NETIF_F_NEVER_CHANGE);
0048 all_features = GENMASK_ULL(NETDEV_FEATURE_COUNT - 1, 0);
0049 ethnl_features_to_bitmap32(data->all, all_features);
0050
0051 return 0;
0052 }
0053
0054 static int features_reply_size(const struct ethnl_req_info *req_base,
0055 const struct ethnl_reply_data *reply_base)
0056 {
0057 const struct features_reply_data *data = FEATURES_REPDATA(reply_base);
0058 bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
0059 unsigned int len = 0;
0060 int ret;
0061
0062 ret = ethnl_bitset32_size(data->hw, data->all, NETDEV_FEATURE_COUNT,
0063 netdev_features_strings, compact);
0064 if (ret < 0)
0065 return ret;
0066 len += ret;
0067 ret = ethnl_bitset32_size(data->wanted, NULL, NETDEV_FEATURE_COUNT,
0068 netdev_features_strings, compact);
0069 if (ret < 0)
0070 return ret;
0071 len += ret;
0072 ret = ethnl_bitset32_size(data->active, NULL, NETDEV_FEATURE_COUNT,
0073 netdev_features_strings, compact);
0074 if (ret < 0)
0075 return ret;
0076 len += ret;
0077 ret = ethnl_bitset32_size(data->nochange, NULL, NETDEV_FEATURE_COUNT,
0078 netdev_features_strings, compact);
0079 if (ret < 0)
0080 return ret;
0081 len += ret;
0082
0083 return len;
0084 }
0085
0086 static int features_fill_reply(struct sk_buff *skb,
0087 const struct ethnl_req_info *req_base,
0088 const struct ethnl_reply_data *reply_base)
0089 {
0090 const struct features_reply_data *data = FEATURES_REPDATA(reply_base);
0091 bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
0092 int ret;
0093
0094 ret = ethnl_put_bitset32(skb, ETHTOOL_A_FEATURES_HW, data->hw,
0095 data->all, NETDEV_FEATURE_COUNT,
0096 netdev_features_strings, compact);
0097 if (ret < 0)
0098 return ret;
0099 ret = ethnl_put_bitset32(skb, ETHTOOL_A_FEATURES_WANTED, data->wanted,
0100 NULL, NETDEV_FEATURE_COUNT,
0101 netdev_features_strings, compact);
0102 if (ret < 0)
0103 return ret;
0104 ret = ethnl_put_bitset32(skb, ETHTOOL_A_FEATURES_ACTIVE, data->active,
0105 NULL, NETDEV_FEATURE_COUNT,
0106 netdev_features_strings, compact);
0107 if (ret < 0)
0108 return ret;
0109 return ethnl_put_bitset32(skb, ETHTOOL_A_FEATURES_NOCHANGE,
0110 data->nochange, NULL, NETDEV_FEATURE_COUNT,
0111 netdev_features_strings, compact);
0112 }
0113
0114 const struct ethnl_request_ops ethnl_features_request_ops = {
0115 .request_cmd = ETHTOOL_MSG_FEATURES_GET,
0116 .reply_cmd = ETHTOOL_MSG_FEATURES_GET_REPLY,
0117 .hdr_attr = ETHTOOL_A_FEATURES_HEADER,
0118 .req_info_size = sizeof(struct features_req_info),
0119 .reply_data_size = sizeof(struct features_reply_data),
0120
0121 .prepare_data = features_prepare_data,
0122 .reply_size = features_reply_size,
0123 .fill_reply = features_fill_reply,
0124 };
0125
0126
0127
0128 const struct nla_policy ethnl_features_set_policy[] = {
0129 [ETHTOOL_A_FEATURES_HEADER] =
0130 NLA_POLICY_NESTED(ethnl_header_policy),
0131 [ETHTOOL_A_FEATURES_WANTED] = { .type = NLA_NESTED },
0132 };
0133
0134 static void ethnl_features_to_bitmap(unsigned long *dest, netdev_features_t val)
0135 {
0136 const unsigned int words = BITS_TO_LONGS(NETDEV_FEATURE_COUNT);
0137 unsigned int i;
0138
0139 for (i = 0; i < words; i++)
0140 dest[i] = (unsigned long)(val >> (i * BITS_PER_LONG));
0141 }
0142
0143 static netdev_features_t ethnl_bitmap_to_features(unsigned long *src)
0144 {
0145 const unsigned int nft_bits = sizeof(netdev_features_t) * BITS_PER_BYTE;
0146 const unsigned int words = BITS_TO_LONGS(NETDEV_FEATURE_COUNT);
0147 netdev_features_t ret = 0;
0148 unsigned int i;
0149
0150 for (i = 0; i < words; i++)
0151 ret |= (netdev_features_t)(src[i]) << (i * BITS_PER_LONG);
0152 ret &= ~(netdev_features_t)0 >> (nft_bits - NETDEV_FEATURE_COUNT);
0153 return ret;
0154 }
0155
0156 static int features_send_reply(struct net_device *dev, struct genl_info *info,
0157 const unsigned long *wanted,
0158 const unsigned long *wanted_mask,
0159 const unsigned long *active,
0160 const unsigned long *active_mask, bool compact)
0161 {
0162 struct sk_buff *rskb;
0163 void *reply_payload;
0164 int reply_len = 0;
0165 int ret;
0166
0167 reply_len = ethnl_reply_header_size();
0168 ret = ethnl_bitset_size(wanted, wanted_mask, NETDEV_FEATURE_COUNT,
0169 netdev_features_strings, compact);
0170 if (ret < 0)
0171 goto err;
0172 reply_len += ret;
0173 ret = ethnl_bitset_size(active, active_mask, NETDEV_FEATURE_COUNT,
0174 netdev_features_strings, compact);
0175 if (ret < 0)
0176 goto err;
0177 reply_len += ret;
0178
0179 ret = -ENOMEM;
0180 rskb = ethnl_reply_init(reply_len, dev, ETHTOOL_MSG_FEATURES_SET_REPLY,
0181 ETHTOOL_A_FEATURES_HEADER, info,
0182 &reply_payload);
0183 if (!rskb)
0184 goto err;
0185
0186 ret = ethnl_put_bitset(rskb, ETHTOOL_A_FEATURES_WANTED, wanted,
0187 wanted_mask, NETDEV_FEATURE_COUNT,
0188 netdev_features_strings, compact);
0189 if (ret < 0)
0190 goto nla_put_failure;
0191 ret = ethnl_put_bitset(rskb, ETHTOOL_A_FEATURES_ACTIVE, active,
0192 active_mask, NETDEV_FEATURE_COUNT,
0193 netdev_features_strings, compact);
0194 if (ret < 0)
0195 goto nla_put_failure;
0196
0197 genlmsg_end(rskb, reply_payload);
0198 ret = genlmsg_reply(rskb, info);
0199 return ret;
0200
0201 nla_put_failure:
0202 nlmsg_free(rskb);
0203 WARN_ONCE(1, "calculated message payload length (%d) not sufficient\n",
0204 reply_len);
0205 err:
0206 GENL_SET_ERR_MSG(info, "failed to send reply message");
0207 return ret;
0208 }
0209
0210 int ethnl_set_features(struct sk_buff *skb, struct genl_info *info)
0211 {
0212 DECLARE_BITMAP(wanted_diff_mask, NETDEV_FEATURE_COUNT);
0213 DECLARE_BITMAP(active_diff_mask, NETDEV_FEATURE_COUNT);
0214 DECLARE_BITMAP(old_active, NETDEV_FEATURE_COUNT);
0215 DECLARE_BITMAP(old_wanted, NETDEV_FEATURE_COUNT);
0216 DECLARE_BITMAP(new_active, NETDEV_FEATURE_COUNT);
0217 DECLARE_BITMAP(new_wanted, NETDEV_FEATURE_COUNT);
0218 DECLARE_BITMAP(req_wanted, NETDEV_FEATURE_COUNT);
0219 DECLARE_BITMAP(req_mask, NETDEV_FEATURE_COUNT);
0220 struct ethnl_req_info req_info = {};
0221 struct nlattr **tb = info->attrs;
0222 struct net_device *dev;
0223 bool mod;
0224 int ret;
0225
0226 if (!tb[ETHTOOL_A_FEATURES_WANTED])
0227 return -EINVAL;
0228 ret = ethnl_parse_header_dev_get(&req_info,
0229 tb[ETHTOOL_A_FEATURES_HEADER],
0230 genl_info_net(info), info->extack,
0231 true);
0232 if (ret < 0)
0233 return ret;
0234 dev = req_info.dev;
0235
0236 rtnl_lock();
0237 ethnl_features_to_bitmap(old_active, dev->features);
0238 ethnl_features_to_bitmap(old_wanted, dev->wanted_features);
0239 ret = ethnl_parse_bitset(req_wanted, req_mask, NETDEV_FEATURE_COUNT,
0240 tb[ETHTOOL_A_FEATURES_WANTED],
0241 netdev_features_strings, info->extack);
0242 if (ret < 0)
0243 goto out_rtnl;
0244 if (ethnl_bitmap_to_features(req_mask) & ~NETIF_F_ETHTOOL_BITS) {
0245 GENL_SET_ERR_MSG(info, "attempt to change non-ethtool features");
0246 ret = -EINVAL;
0247 goto out_rtnl;
0248 }
0249
0250
0251 bitmap_and(req_wanted, req_wanted, req_mask, NETDEV_FEATURE_COUNT);
0252 bitmap_andnot(new_wanted, old_wanted, req_mask, NETDEV_FEATURE_COUNT);
0253 bitmap_or(req_wanted, new_wanted, req_wanted, NETDEV_FEATURE_COUNT);
0254 if (!bitmap_equal(req_wanted, old_wanted, NETDEV_FEATURE_COUNT)) {
0255 dev->wanted_features &= ~dev->hw_features;
0256 dev->wanted_features |= ethnl_bitmap_to_features(req_wanted) & dev->hw_features;
0257 __netdev_update_features(dev);
0258 }
0259 ethnl_features_to_bitmap(new_active, dev->features);
0260 mod = !bitmap_equal(old_active, new_active, NETDEV_FEATURE_COUNT);
0261
0262 ret = 0;
0263 if (!(req_info.flags & ETHTOOL_FLAG_OMIT_REPLY)) {
0264 bool compact = req_info.flags & ETHTOOL_FLAG_COMPACT_BITSETS;
0265
0266 bitmap_xor(wanted_diff_mask, req_wanted, new_active,
0267 NETDEV_FEATURE_COUNT);
0268 bitmap_xor(active_diff_mask, old_active, new_active,
0269 NETDEV_FEATURE_COUNT);
0270 bitmap_and(wanted_diff_mask, wanted_diff_mask, req_mask,
0271 NETDEV_FEATURE_COUNT);
0272 bitmap_and(req_wanted, req_wanted, wanted_diff_mask,
0273 NETDEV_FEATURE_COUNT);
0274 bitmap_and(new_active, new_active, active_diff_mask,
0275 NETDEV_FEATURE_COUNT);
0276
0277 ret = features_send_reply(dev, info, req_wanted,
0278 wanted_diff_mask, new_active,
0279 active_diff_mask, compact);
0280 }
0281 if (mod)
0282 netdev_features_change(dev);
0283
0284 out_rtnl:
0285 rtnl_unlock();
0286 ethnl_parse_header_dev_put(&req_info);
0287 return ret;
0288 }
