Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 
0003 #include "netlink.h"
0004 #include "common.h"
0005 #include "bitset.h"
0006 
0007 /* LINKMODES_GET */
0008 
0009 struct linkmodes_req_info {
0010     struct ethnl_req_info       base;
0011 };
0012 
0013 struct linkmodes_reply_data {
0014     struct ethnl_reply_data     base;
0015     struct ethtool_link_ksettings   ksettings;
0016     struct ethtool_link_settings    *lsettings;
0017     bool                peer_empty;
0018 };
0019 
0020 #define LINKMODES_REPDATA(__reply_base) \
0021     container_of(__reply_base, struct linkmodes_reply_data, base)
0022 
0023 const struct nla_policy ethnl_linkmodes_get_policy[] = {
0024     [ETHTOOL_A_LINKMODES_HEADER]        =
0025         NLA_POLICY_NESTED(ethnl_header_policy),
0026 };
0027 
0028 static int linkmodes_prepare_data(const struct ethnl_req_info *req_base,
0029                   struct ethnl_reply_data *reply_base,
0030                   struct genl_info *info)
0031 {
0032     struct linkmodes_reply_data *data = LINKMODES_REPDATA(reply_base);
0033     struct net_device *dev = reply_base->dev;
0034     int ret;
0035 
0036     data->lsettings = &data->ksettings.base;
0037 
0038     ret = ethnl_ops_begin(dev);
0039     if (ret < 0)
0040         return ret;
0041 
0042     ret = __ethtool_get_link_ksettings(dev, &data->ksettings);
0043     if (ret < 0 && info) {
0044         GENL_SET_ERR_MSG(info, "failed to retrieve link settings");
0045         goto out;
0046     }
0047 
0048     if (!dev->ethtool_ops->cap_link_lanes_supported)
0049         data->ksettings.lanes = 0;
0050 
0051     data->peer_empty =
0052         bitmap_empty(data->ksettings.link_modes.lp_advertising,
0053                  __ETHTOOL_LINK_MODE_MASK_NBITS);
0054 
0055 out:
0056     ethnl_ops_complete(dev);
0057     return ret;
0058 }
0059 
0060 static int linkmodes_reply_size(const struct ethnl_req_info *req_base,
0061                 const struct ethnl_reply_data *reply_base)
0062 {
0063     const struct linkmodes_reply_data *data = LINKMODES_REPDATA(reply_base);
0064     const struct ethtool_link_ksettings *ksettings = &data->ksettings;
0065     const struct ethtool_link_settings *lsettings = &ksettings->base;
0066     bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
0067     int len, ret;
0068 
0069     len = nla_total_size(sizeof(u8)) /* LINKMODES_AUTONEG */
0070         + nla_total_size(sizeof(u32)) /* LINKMODES_SPEED */
0071         + nla_total_size(sizeof(u32)) /* LINKMODES_LANES */
0072         + nla_total_size(sizeof(u8)) /* LINKMODES_DUPLEX */
0073         + 0;
0074     ret = ethnl_bitset_size(ksettings->link_modes.advertising,
0075                 ksettings->link_modes.supported,
0076                 __ETHTOOL_LINK_MODE_MASK_NBITS,
0077                 link_mode_names, compact);
0078     if (ret < 0)
0079         return ret;
0080     len += ret;
0081     if (!data->peer_empty) {
0082         ret = ethnl_bitset_size(ksettings->link_modes.lp_advertising,
0083                     NULL, __ETHTOOL_LINK_MODE_MASK_NBITS,
0084                     link_mode_names, compact);
0085         if (ret < 0)
0086             return ret;
0087         len += ret;
0088     }
0089 
0090     if (lsettings->master_slave_cfg != MASTER_SLAVE_CFG_UNSUPPORTED)
0091         len += nla_total_size(sizeof(u8));
0092 
0093     if (lsettings->master_slave_state != MASTER_SLAVE_STATE_UNSUPPORTED)
0094         len += nla_total_size(sizeof(u8));
0095 
0096     return len;
0097 }
0098 
0099 static int linkmodes_fill_reply(struct sk_buff *skb,
0100                 const struct ethnl_req_info *req_base,
0101                 const struct ethnl_reply_data *reply_base)
0102 {
0103     const struct linkmodes_reply_data *data = LINKMODES_REPDATA(reply_base);
0104     const struct ethtool_link_ksettings *ksettings = &data->ksettings;
0105     const struct ethtool_link_settings *lsettings = &ksettings->base;
0106     bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
0107     int ret;
0108 
0109     if (nla_put_u8(skb, ETHTOOL_A_LINKMODES_AUTONEG, lsettings->autoneg))
0110         return -EMSGSIZE;
0111 
0112     ret = ethnl_put_bitset(skb, ETHTOOL_A_LINKMODES_OURS,
0113                    ksettings->link_modes.advertising,
0114                    ksettings->link_modes.supported,
0115                    __ETHTOOL_LINK_MODE_MASK_NBITS, link_mode_names,
0116                    compact);
0117     if (ret < 0)
0118         return -EMSGSIZE;
0119     if (!data->peer_empty) {
0120         ret = ethnl_put_bitset(skb, ETHTOOL_A_LINKMODES_PEER,
0121                        ksettings->link_modes.lp_advertising,
0122                        NULL, __ETHTOOL_LINK_MODE_MASK_NBITS,
0123                        link_mode_names, compact);
0124         if (ret < 0)
0125             return -EMSGSIZE;
0126     }
0127 
0128     if (nla_put_u32(skb, ETHTOOL_A_LINKMODES_SPEED, lsettings->speed) ||
0129         nla_put_u8(skb, ETHTOOL_A_LINKMODES_DUPLEX, lsettings->duplex))
0130         return -EMSGSIZE;
0131 
0132     if (ksettings->lanes &&
0133         nla_put_u32(skb, ETHTOOL_A_LINKMODES_LANES, ksettings->lanes))
0134         return -EMSGSIZE;
0135 
0136     if (lsettings->master_slave_cfg != MASTER_SLAVE_CFG_UNSUPPORTED &&
0137         nla_put_u8(skb, ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG,
0138                lsettings->master_slave_cfg))
0139         return -EMSGSIZE;
0140 
0141     if (lsettings->master_slave_state != MASTER_SLAVE_STATE_UNSUPPORTED &&
0142         nla_put_u8(skb, ETHTOOL_A_LINKMODES_MASTER_SLAVE_STATE,
0143                lsettings->master_slave_state))
0144         return -EMSGSIZE;
0145 
0146     return 0;
0147 }
0148 
0149 const struct ethnl_request_ops ethnl_linkmodes_request_ops = {
0150     .request_cmd        = ETHTOOL_MSG_LINKMODES_GET,
0151     .reply_cmd      = ETHTOOL_MSG_LINKMODES_GET_REPLY,
0152     .hdr_attr       = ETHTOOL_A_LINKMODES_HEADER,
0153     .req_info_size      = sizeof(struct linkmodes_req_info),
0154     .reply_data_size    = sizeof(struct linkmodes_reply_data),
0155 
0156     .prepare_data       = linkmodes_prepare_data,
0157     .reply_size     = linkmodes_reply_size,
0158     .fill_reply     = linkmodes_fill_reply,
0159 };
0160 
0161 /* LINKMODES_SET */
0162 
0163 const struct nla_policy ethnl_linkmodes_set_policy[] = {
0164     [ETHTOOL_A_LINKMODES_HEADER]        =
0165         NLA_POLICY_NESTED(ethnl_header_policy),
0166     [ETHTOOL_A_LINKMODES_AUTONEG]       = { .type = NLA_U8 },
0167     [ETHTOOL_A_LINKMODES_OURS]      = { .type = NLA_NESTED },
0168     [ETHTOOL_A_LINKMODES_SPEED]     = { .type = NLA_U32 },
0169     [ETHTOOL_A_LINKMODES_DUPLEX]        = { .type = NLA_U8 },
0170     [ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG]  = { .type = NLA_U8 },
0171     [ETHTOOL_A_LINKMODES_LANES]     = NLA_POLICY_RANGE(NLA_U32, 1, 8),
0172 };
0173 
0174 /* Set advertised link modes to all supported modes matching requested speed,
0175  * lanes and duplex values. Called when autonegotiation is on, speed, lanes or
0176  * duplex is requested but no link mode change. This is done in userspace with
0177  * ioctl() interface, move it into kernel for netlink.
0178  * Returns true if advertised modes bitmap was modified.
0179  */
0180 static bool ethnl_auto_linkmodes(struct ethtool_link_ksettings *ksettings,
0181                  bool req_speed, bool req_lanes, bool req_duplex)
0182 {
0183     unsigned long *advertising = ksettings->link_modes.advertising;
0184     unsigned long *supported = ksettings->link_modes.supported;
0185     DECLARE_BITMAP(old_adv, __ETHTOOL_LINK_MODE_MASK_NBITS);
0186     unsigned int i;
0187 
0188     bitmap_copy(old_adv, advertising, __ETHTOOL_LINK_MODE_MASK_NBITS);
0189 
0190     for (i = 0; i < __ETHTOOL_LINK_MODE_MASK_NBITS; i++) {
0191         const struct link_mode_info *info = &link_mode_params[i];
0192 
0193         if (info->speed == SPEED_UNKNOWN)
0194             continue;
0195         if (test_bit(i, supported) &&
0196             (!req_speed || info->speed == ksettings->base.speed) &&
0197             (!req_lanes || info->lanes == ksettings->lanes) &&
0198             (!req_duplex || info->duplex == ksettings->base.duplex))
0199             set_bit(i, advertising);
0200         else
0201             clear_bit(i, advertising);
0202     }
0203 
0204     return !bitmap_equal(old_adv, advertising,
0205                  __ETHTOOL_LINK_MODE_MASK_NBITS);
0206 }
0207 
0208 static bool ethnl_validate_master_slave_cfg(u8 cfg)
0209 {
0210     switch (cfg) {
0211     case MASTER_SLAVE_CFG_MASTER_PREFERRED:
0212     case MASTER_SLAVE_CFG_SLAVE_PREFERRED:
0213     case MASTER_SLAVE_CFG_MASTER_FORCE:
0214     case MASTER_SLAVE_CFG_SLAVE_FORCE:
0215         return true;
0216     }
0217 
0218     return false;
0219 }
0220 
0221 static int ethnl_check_linkmodes(struct genl_info *info, struct nlattr **tb)
0222 {
0223     const struct nlattr *master_slave_cfg, *lanes_cfg;
0224 
0225     master_slave_cfg = tb[ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG];
0226     if (master_slave_cfg &&
0227         !ethnl_validate_master_slave_cfg(nla_get_u8(master_slave_cfg))) {
0228         NL_SET_ERR_MSG_ATTR(info->extack, master_slave_cfg,
0229                     "master/slave value is invalid");
0230         return -EOPNOTSUPP;
0231     }
0232 
0233     lanes_cfg = tb[ETHTOOL_A_LINKMODES_LANES];
0234     if (lanes_cfg && !is_power_of_2(nla_get_u32(lanes_cfg))) {
0235         NL_SET_ERR_MSG_ATTR(info->extack, lanes_cfg,
0236                     "lanes value is invalid");
0237         return -EINVAL;
0238     }
0239 
0240     return 0;
0241 }
0242 
0243 static int ethnl_update_linkmodes(struct genl_info *info, struct nlattr **tb,
0244                   struct ethtool_link_ksettings *ksettings,
0245                   bool *mod, const struct net_device *dev)
0246 {
0247     struct ethtool_link_settings *lsettings = &ksettings->base;
0248     bool req_speed, req_lanes, req_duplex;
0249     const struct nlattr *master_slave_cfg, *lanes_cfg;
0250     int ret;
0251 
0252     master_slave_cfg = tb[ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG];
0253     if (master_slave_cfg) {
0254         if (lsettings->master_slave_cfg == MASTER_SLAVE_CFG_UNSUPPORTED) {
0255             NL_SET_ERR_MSG_ATTR(info->extack, master_slave_cfg,
0256                         "master/slave configuration not supported by device");
0257             return -EOPNOTSUPP;
0258         }
0259     }
0260 
0261     *mod = false;
0262     req_speed = tb[ETHTOOL_A_LINKMODES_SPEED];
0263     req_lanes = tb[ETHTOOL_A_LINKMODES_LANES];
0264     req_duplex = tb[ETHTOOL_A_LINKMODES_DUPLEX];
0265 
0266     ethnl_update_u8(&lsettings->autoneg, tb[ETHTOOL_A_LINKMODES_AUTONEG],
0267             mod);
0268 
0269     lanes_cfg = tb[ETHTOOL_A_LINKMODES_LANES];
0270     if (lanes_cfg) {
0271         /* If autoneg is off and lanes parameter is not supported by the
0272          * driver, return an error.
0273          */
0274         if (!lsettings->autoneg &&
0275             !dev->ethtool_ops->cap_link_lanes_supported) {
0276             NL_SET_ERR_MSG_ATTR(info->extack, lanes_cfg,
0277                         "lanes configuration not supported by device");
0278             return -EOPNOTSUPP;
0279         }
0280     } else if (!lsettings->autoneg) {
0281         /* If autoneg is off and lanes parameter is not passed from user,
0282          * set the lanes parameter to 0.
0283          */
0284         ksettings->lanes = 0;
0285     }
0286 
0287     ret = ethnl_update_bitset(ksettings->link_modes.advertising,
0288                   __ETHTOOL_LINK_MODE_MASK_NBITS,
0289                   tb[ETHTOOL_A_LINKMODES_OURS], link_mode_names,
0290                   info->extack, mod);
0291     if (ret < 0)
0292         return ret;
0293     ethnl_update_u32(&lsettings->speed, tb[ETHTOOL_A_LINKMODES_SPEED],
0294              mod);
0295     ethnl_update_u32(&ksettings->lanes, lanes_cfg, mod);
0296     ethnl_update_u8(&lsettings->duplex, tb[ETHTOOL_A_LINKMODES_DUPLEX],
0297             mod);
0298     ethnl_update_u8(&lsettings->master_slave_cfg, master_slave_cfg, mod);
0299 
0300     if (!tb[ETHTOOL_A_LINKMODES_OURS] && lsettings->autoneg &&
0301         (req_speed || req_lanes || req_duplex) &&
0302         ethnl_auto_linkmodes(ksettings, req_speed, req_lanes, req_duplex))
0303         *mod = true;
0304 
0305     return 0;
0306 }
0307 
0308 int ethnl_set_linkmodes(struct sk_buff *skb, struct genl_info *info)
0309 {
0310     struct ethtool_link_ksettings ksettings = {};
0311     struct ethnl_req_info req_info = {};
0312     struct nlattr **tb = info->attrs;
0313     struct net_device *dev;
0314     bool mod = false;
0315     int ret;
0316 
0317     ret = ethnl_check_linkmodes(info, tb);
0318     if (ret < 0)
0319         return ret;
0320 
0321     ret = ethnl_parse_header_dev_get(&req_info,
0322                      tb[ETHTOOL_A_LINKMODES_HEADER],
0323                      genl_info_net(info), info->extack,
0324                      true);
0325     if (ret < 0)
0326         return ret;
0327     dev = req_info.dev;
0328     ret = -EOPNOTSUPP;
0329     if (!dev->ethtool_ops->get_link_ksettings ||
0330         !dev->ethtool_ops->set_link_ksettings)
0331         goto out_dev;
0332 
0333     rtnl_lock();
0334     ret = ethnl_ops_begin(dev);
0335     if (ret < 0)
0336         goto out_rtnl;
0337 
0338     ret = __ethtool_get_link_ksettings(dev, &ksettings);
0339     if (ret < 0) {
0340         GENL_SET_ERR_MSG(info, "failed to retrieve link settings");
0341         goto out_ops;
0342     }
0343 
0344     ret = ethnl_update_linkmodes(info, tb, &ksettings, &mod, dev);
0345     if (ret < 0)
0346         goto out_ops;
0347 
0348     if (mod) {
0349         ret = dev->ethtool_ops->set_link_ksettings(dev, &ksettings);
0350         if (ret < 0)
0351             GENL_SET_ERR_MSG(info, "link settings update failed");
0352         else
0353             ethtool_notify(dev, ETHTOOL_MSG_LINKMODES_NTF, NULL);
0354     }
0355 
0356 out_ops:
0357     ethnl_ops_complete(dev);
0358 out_rtnl:
0359     rtnl_unlock();
0360 out_dev:
0361     ethnl_parse_header_dev_put(&req_info);
0362     return ret;
0363 }
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.1.0 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!