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 struct fec_req_info {
0008     struct ethnl_req_info       base;
0009 };
0010 
0011 struct fec_reply_data {
0012     struct ethnl_reply_data     base;
0013     __ETHTOOL_DECLARE_LINK_MODE_MASK(fec_link_modes);
0014     u32 active_fec;
0015     u8 fec_auto;
0016     struct fec_stat_grp {
0017         u64 stats[1 + ETHTOOL_MAX_LANES];
0018         u8 cnt;
0019     } corr, uncorr, corr_bits;
0020 };
0021 
0022 #define FEC_REPDATA(__reply_base) \
0023     container_of(__reply_base, struct fec_reply_data, base)
0024 
0025 #define ETHTOOL_FEC_MASK    ((ETHTOOL_FEC_LLRS << 1) - 1)
0026 
0027 const struct nla_policy ethnl_fec_get_policy[ETHTOOL_A_FEC_HEADER + 1] = {
0028     [ETHTOOL_A_FEC_HEADER]  = NLA_POLICY_NESTED(ethnl_header_policy_stats),
0029 };
0030 
0031 static void
0032 ethtool_fec_to_link_modes(u32 fec, unsigned long *link_modes, u8 *fec_auto)
0033 {
0034     if (fec_auto)
0035         *fec_auto = !!(fec & ETHTOOL_FEC_AUTO);
0036 
0037     if (fec & ETHTOOL_FEC_OFF)
0038         __set_bit(ETHTOOL_LINK_MODE_FEC_NONE_BIT, link_modes);
0039     if (fec & ETHTOOL_FEC_RS)
0040         __set_bit(ETHTOOL_LINK_MODE_FEC_RS_BIT, link_modes);
0041     if (fec & ETHTOOL_FEC_BASER)
0042         __set_bit(ETHTOOL_LINK_MODE_FEC_BASER_BIT, link_modes);
0043     if (fec & ETHTOOL_FEC_LLRS)
0044         __set_bit(ETHTOOL_LINK_MODE_FEC_LLRS_BIT, link_modes);
0045 }
0046 
0047 static int
0048 ethtool_link_modes_to_fecparam(struct ethtool_fecparam *fec,
0049                    unsigned long *link_modes, u8 fec_auto)
0050 {
0051     memset(fec, 0, sizeof(*fec));
0052 
0053     if (fec_auto)
0054         fec->fec |= ETHTOOL_FEC_AUTO;
0055 
0056     if (__test_and_clear_bit(ETHTOOL_LINK_MODE_FEC_NONE_BIT, link_modes))
0057         fec->fec |= ETHTOOL_FEC_OFF;
0058     if (__test_and_clear_bit(ETHTOOL_LINK_MODE_FEC_RS_BIT, link_modes))
0059         fec->fec |= ETHTOOL_FEC_RS;
0060     if (__test_and_clear_bit(ETHTOOL_LINK_MODE_FEC_BASER_BIT, link_modes))
0061         fec->fec |= ETHTOOL_FEC_BASER;
0062     if (__test_and_clear_bit(ETHTOOL_LINK_MODE_FEC_LLRS_BIT, link_modes))
0063         fec->fec |= ETHTOOL_FEC_LLRS;
0064 
0065     if (!bitmap_empty(link_modes, __ETHTOOL_LINK_MODE_MASK_NBITS))
0066         return -EINVAL;
0067 
0068     return 0;
0069 }
0070 
0071 static void
0072 fec_stats_recalc(struct fec_stat_grp *grp, struct ethtool_fec_stat *stats)
0073 {
0074     int i;
0075 
0076     if (stats->lanes[0] == ETHTOOL_STAT_NOT_SET) {
0077         grp->stats[0] = stats->total;
0078         grp->cnt = stats->total != ETHTOOL_STAT_NOT_SET;
0079         return;
0080     }
0081 
0082     grp->cnt = 1;
0083     grp->stats[0] = 0;
0084     for (i = 0; i < ETHTOOL_MAX_LANES; i++) {
0085         if (stats->lanes[i] == ETHTOOL_STAT_NOT_SET)
0086             break;
0087 
0088         grp->stats[0] += stats->lanes[i];
0089         grp->stats[grp->cnt++] = stats->lanes[i];
0090     }
0091 }
0092 
0093 static int fec_prepare_data(const struct ethnl_req_info *req_base,
0094                 struct ethnl_reply_data *reply_base,
0095                 struct genl_info *info)
0096 {
0097     __ETHTOOL_DECLARE_LINK_MODE_MASK(active_fec_modes) = {};
0098     struct fec_reply_data *data = FEC_REPDATA(reply_base);
0099     struct net_device *dev = reply_base->dev;
0100     struct ethtool_fecparam fec = {};
0101     int ret;
0102 
0103     if (!dev->ethtool_ops->get_fecparam)
0104         return -EOPNOTSUPP;
0105     ret = ethnl_ops_begin(dev);
0106     if (ret < 0)
0107         return ret;
0108     ret = dev->ethtool_ops->get_fecparam(dev, &fec);
0109     if (ret)
0110         goto out_complete;
0111     if (req_base->flags & ETHTOOL_FLAG_STATS &&
0112         dev->ethtool_ops->get_fec_stats) {
0113         struct ethtool_fec_stats stats;
0114 
0115         ethtool_stats_init((u64 *)&stats, sizeof(stats) / 8);
0116         dev->ethtool_ops->get_fec_stats(dev, &stats);
0117 
0118         fec_stats_recalc(&data->corr, &stats.corrected_blocks);
0119         fec_stats_recalc(&data->uncorr, &stats.uncorrectable_blocks);
0120         fec_stats_recalc(&data->corr_bits, &stats.corrected_bits);
0121     }
0122 
0123     WARN_ON_ONCE(fec.reserved);
0124 
0125     ethtool_fec_to_link_modes(fec.fec, data->fec_link_modes,
0126                   &data->fec_auto);
0127 
0128     ethtool_fec_to_link_modes(fec.active_fec, active_fec_modes, NULL);
0129     data->active_fec = find_first_bit(active_fec_modes,
0130                       __ETHTOOL_LINK_MODE_MASK_NBITS);
0131     /* Don't report attr if no FEC mode set. Note that
0132      * ethtool_fecparam_to_link_modes() ignores NONE and AUTO.
0133      */
0134     if (data->active_fec == __ETHTOOL_LINK_MODE_MASK_NBITS)
0135         data->active_fec = 0;
0136 
0137 out_complete:
0138     ethnl_ops_complete(dev);
0139     return ret;
0140 }
0141 
0142 static int fec_reply_size(const struct ethnl_req_info *req_base,
0143               const struct ethnl_reply_data *reply_base)
0144 {
0145     bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
0146     const struct fec_reply_data *data = FEC_REPDATA(reply_base);
0147     int len = 0;
0148     int ret;
0149 
0150     ret = ethnl_bitset_size(data->fec_link_modes, NULL,
0151                 __ETHTOOL_LINK_MODE_MASK_NBITS,
0152                 link_mode_names, compact);
0153     if (ret < 0)
0154         return ret;
0155     len += ret;
0156 
0157     len += nla_total_size(sizeof(u8)) + /* _FEC_AUTO */
0158            nla_total_size(sizeof(u32)); /* _FEC_ACTIVE */
0159 
0160     if (req_base->flags & ETHTOOL_FLAG_STATS)
0161         len += 3 * nla_total_size_64bit(sizeof(u64) *
0162                         (1 + ETHTOOL_MAX_LANES));
0163 
0164     return len;
0165 }
0166 
0167 static int fec_put_stats(struct sk_buff *skb, const struct fec_reply_data *data)
0168 {
0169     struct nlattr *nest;
0170 
0171     nest = nla_nest_start(skb, ETHTOOL_A_FEC_STATS);
0172     if (!nest)
0173         return -EMSGSIZE;
0174 
0175     if (nla_put_64bit(skb, ETHTOOL_A_FEC_STAT_CORRECTED,
0176               sizeof(u64) * data->corr.cnt,
0177               data->corr.stats, ETHTOOL_A_FEC_STAT_PAD) ||
0178         nla_put_64bit(skb, ETHTOOL_A_FEC_STAT_UNCORR,
0179               sizeof(u64) * data->uncorr.cnt,
0180               data->uncorr.stats, ETHTOOL_A_FEC_STAT_PAD) ||
0181         nla_put_64bit(skb, ETHTOOL_A_FEC_STAT_CORR_BITS,
0182               sizeof(u64) * data->corr_bits.cnt,
0183               data->corr_bits.stats, ETHTOOL_A_FEC_STAT_PAD))
0184         goto err_cancel;
0185 
0186     nla_nest_end(skb, nest);
0187     return 0;
0188 
0189 err_cancel:
0190     nla_nest_cancel(skb, nest);
0191     return -EMSGSIZE;
0192 }
0193 
0194 static int fec_fill_reply(struct sk_buff *skb,
0195               const struct ethnl_req_info *req_base,
0196               const struct ethnl_reply_data *reply_base)
0197 {
0198     bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
0199     const struct fec_reply_data *data = FEC_REPDATA(reply_base);
0200     int ret;
0201 
0202     ret = ethnl_put_bitset(skb, ETHTOOL_A_FEC_MODES,
0203                    data->fec_link_modes, NULL,
0204                    __ETHTOOL_LINK_MODE_MASK_NBITS,
0205                    link_mode_names, compact);
0206     if (ret < 0)
0207         return ret;
0208 
0209     if (nla_put_u8(skb, ETHTOOL_A_FEC_AUTO, data->fec_auto) ||
0210         (data->active_fec &&
0211          nla_put_u32(skb, ETHTOOL_A_FEC_ACTIVE, data->active_fec)))
0212         return -EMSGSIZE;
0213 
0214     if (req_base->flags & ETHTOOL_FLAG_STATS && fec_put_stats(skb, data))
0215         return -EMSGSIZE;
0216 
0217     return 0;
0218 }
0219 
0220 const struct ethnl_request_ops ethnl_fec_request_ops = {
0221     .request_cmd        = ETHTOOL_MSG_FEC_GET,
0222     .reply_cmd      = ETHTOOL_MSG_FEC_GET_REPLY,
0223     .hdr_attr       = ETHTOOL_A_FEC_HEADER,
0224     .req_info_size      = sizeof(struct fec_req_info),
0225     .reply_data_size    = sizeof(struct fec_reply_data),
0226 
0227     .prepare_data       = fec_prepare_data,
0228     .reply_size     = fec_reply_size,
0229     .fill_reply     = fec_fill_reply,
0230 };
0231 
0232 /* FEC_SET */
0233 
0234 const struct nla_policy ethnl_fec_set_policy[ETHTOOL_A_FEC_AUTO + 1] = {
0235     [ETHTOOL_A_FEC_HEADER]  = NLA_POLICY_NESTED(ethnl_header_policy),
0236     [ETHTOOL_A_FEC_MODES]   = { .type = NLA_NESTED },
0237     [ETHTOOL_A_FEC_AUTO]    = NLA_POLICY_MAX(NLA_U8, 1),
0238 };
0239 
0240 int ethnl_set_fec(struct sk_buff *skb, struct genl_info *info)
0241 {
0242     __ETHTOOL_DECLARE_LINK_MODE_MASK(fec_link_modes) = {};
0243     struct ethnl_req_info req_info = {};
0244     struct nlattr **tb = info->attrs;
0245     struct ethtool_fecparam fec = {};
0246     const struct ethtool_ops *ops;
0247     struct net_device *dev;
0248     bool mod = false;
0249     u8 fec_auto;
0250     int ret;
0251 
0252     ret = ethnl_parse_header_dev_get(&req_info, tb[ETHTOOL_A_FEC_HEADER],
0253                      genl_info_net(info), info->extack,
0254                      true);
0255     if (ret < 0)
0256         return ret;
0257     dev = req_info.dev;
0258     ops = dev->ethtool_ops;
0259     ret = -EOPNOTSUPP;
0260     if (!ops->get_fecparam || !ops->set_fecparam)
0261         goto out_dev;
0262 
0263     rtnl_lock();
0264     ret = ethnl_ops_begin(dev);
0265     if (ret < 0)
0266         goto out_rtnl;
0267     ret = ops->get_fecparam(dev, &fec);
0268     if (ret < 0)
0269         goto out_ops;
0270 
0271     ethtool_fec_to_link_modes(fec.fec, fec_link_modes, &fec_auto);
0272 
0273     ret = ethnl_update_bitset(fec_link_modes,
0274                   __ETHTOOL_LINK_MODE_MASK_NBITS,
0275                   tb[ETHTOOL_A_FEC_MODES],
0276                   link_mode_names, info->extack, &mod);
0277     if (ret < 0)
0278         goto out_ops;
0279     ethnl_update_u8(&fec_auto, tb[ETHTOOL_A_FEC_AUTO], &mod);
0280 
0281     ret = 0;
0282     if (!mod)
0283         goto out_ops;
0284 
0285     ret = ethtool_link_modes_to_fecparam(&fec, fec_link_modes, fec_auto);
0286     if (ret) {
0287         NL_SET_ERR_MSG_ATTR(info->extack, tb[ETHTOOL_A_FEC_MODES],
0288                     "invalid FEC modes requested");
0289         goto out_ops;
0290     }
0291     if (!fec.fec) {
0292         ret = -EINVAL;
0293         NL_SET_ERR_MSG_ATTR(info->extack, tb[ETHTOOL_A_FEC_MODES],
0294                     "no FEC modes set");
0295         goto out_ops;
0296     }
0297 
0298     ret = dev->ethtool_ops->set_fecparam(dev, &fec);
0299     if (ret < 0)
0300         goto out_ops;
0301     ethtool_notify(dev, ETHTOOL_MSG_FEC_NTF, NULL);
0302 
0303 out_ops:
0304     ethnl_ops_complete(dev);
0305 out_rtnl:
0306     rtnl_unlock();
0307 out_dev:
0308     ethnl_parse_header_dev_put(&req_info);
0309     return ret;
0310 }
[ 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!