diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c b/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c index c61f6302cde2c68e57489a4d044da65a618bdaf9..3e770abfd80212d70e3b7952fa05ba57ae1490e1 100644 --- a/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c +++ b/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c @@ -547,6 +547,70 @@ static void ptys2ethtool_adver_link(unsigned long *advertising_modes, __ETHTOOL_LINK_MODE_MASK_NBITS); } +static const u32 pplm_fec_2_ethtool[] = { + [MLX5E_FEC_NOFEC] = ETHTOOL_FEC_OFF, + [MLX5E_FEC_FIRECODE] = ETHTOOL_FEC_BASER, + [MLX5E_FEC_RS_528_514] = ETHTOOL_FEC_RS, +}; + +static u32 pplm2ethtool_fec(u_long fec_mode, unsigned long size) +{ + int mode = 0; + + if (!fec_mode) + return ETHTOOL_FEC_AUTO; + + mode = find_first_bit(&fec_mode, size); + + if (mode < ARRAY_SIZE(pplm_fec_2_ethtool)) + return pplm_fec_2_ethtool[mode]; + + return 0; +} + +/* we use ETHTOOL_FEC_* offset and apply it to ETHTOOL_LINK_MODE_FEC_*_BIT */ +static u32 ethtool_fec2ethtool_caps(u_long ethtool_fec_code) +{ + u32 offset; + + offset = find_first_bit(ðtool_fec_code, sizeof(u32)); + offset -= ETHTOOL_FEC_OFF_BIT; + offset += ETHTOOL_LINK_MODE_FEC_NONE_BIT; + + return offset; +} + +static int get_fec_supported_advertised(struct mlx5_core_dev *dev, + struct ethtool_link_ksettings *link_ksettings) +{ + u_long fec_caps = 0; + u32 active_fec = 0; + u32 offset; + u32 bitn; + int err; + + err = mlx5e_get_fec_caps(dev, (u8 *)&fec_caps); + if (err) + return (err == -EOPNOTSUPP) ? 0 : err; + + err = mlx5e_get_fec_mode(dev, &active_fec, NULL); + if (err) + return err; + + for_each_set_bit(bitn, &fec_caps, ARRAY_SIZE(pplm_fec_2_ethtool)) { + u_long ethtool_bitmask = pplm_fec_2_ethtool[bitn]; + + offset = ethtool_fec2ethtool_caps(ethtool_bitmask); + __set_bit(offset, link_ksettings->link_modes.supported); + } + + active_fec = pplm2ethtool_fec(active_fec, sizeof(u32) * BITS_PER_BYTE); + offset = ethtool_fec2ethtool_caps(active_fec); + __set_bit(offset, link_ksettings->link_modes.advertising); + + return 0; +} + static void ptys2ethtool_supported_advertised_port(struct ethtool_link_ksettings *link_ksettings, u32 eth_proto_cap, u8 connector_type) @@ -742,7 +806,7 @@ static int mlx5e_get_link_ksettings(struct net_device *netdev, if (err) { netdev_err(netdev, "%s: query port ptys failed: %d\n", __func__, err); - goto err_query_ptys; + goto err_query_regs; } eth_proto_cap = MLX5_GET(ptys_reg, out, eth_proto_capability); @@ -778,11 +842,17 @@ static int mlx5e_get_link_ksettings(struct net_device *netdev, AUTONEG_ENABLE; ethtool_link_ksettings_add_link_mode(link_ksettings, supported, Autoneg); + + err = get_fec_supported_advertised(mdev, link_ksettings); + if (err) + netdev_dbg(netdev, "%s: FEC caps query failed: %d\n", + __func__, err); + if (!an_disable_admin) ethtool_link_ksettings_add_link_mode(link_ksettings, advertising, Autoneg); -err_query_ptys: +err_query_regs: return err; } @@ -1277,6 +1347,58 @@ static int mlx5e_set_wol(struct net_device *netdev, struct ethtool_wolinfo *wol) return mlx5_set_port_wol(mdev, mlx5_wol_mode); } +static int mlx5e_get_fecparam(struct net_device *netdev, + struct ethtool_fecparam *fecparam) +{ + struct mlx5e_priv *priv = netdev_priv(netdev); + struct mlx5_core_dev *mdev = priv->mdev; + u8 fec_configured = 0; + u32 fec_active = 0; + int err; + + err = mlx5e_get_fec_mode(mdev, &fec_active, &fec_configured); + + if (err) + return err; + + fecparam->active_fec = pplm2ethtool_fec((u_long)fec_active, + sizeof(u32) * BITS_PER_BYTE); + + if (!fecparam->active_fec) + return -EOPNOTSUPP; + + fecparam->fec = pplm2ethtool_fec((u_long)fec_configured, + sizeof(u8) * BITS_PER_BYTE); + + return 0; +} + +static int mlx5e_set_fecparam(struct net_device *netdev, + struct ethtool_fecparam *fecparam) +{ + struct mlx5e_priv *priv = netdev_priv(netdev); + struct mlx5_core_dev *mdev = priv->mdev; + u8 fec_policy = 0; + int mode; + int err; + + for (mode = 0; mode < ARRAY_SIZE(pplm_fec_2_ethtool); mode++) { + if (!(pplm_fec_2_ethtool[mode] & fecparam->fec)) + continue; + fec_policy |= (1 << mode); + break; + } + + err = mlx5e_set_fec_mode(mdev, fec_policy); + + if (err) + return err; + + mlx5_toggle_port_link(mdev); + + return 0; +} + static u32 mlx5e_get_msglevel(struct net_device *dev) { return ((struct mlx5e_priv *)netdev_priv(dev))->msglevel; @@ -1699,4 +1821,6 @@ const struct ethtool_ops mlx5e_ethtool_ops = { .self_test = mlx5e_self_test, .get_msglevel = mlx5e_get_msglevel, .set_msglevel = mlx5e_set_msglevel, + .get_fecparam = mlx5e_get_fecparam, + .set_fecparam = mlx5e_set_fecparam, };