Quelle  cabletest.c   Sprache: C

// SPDX-License-Identifier: GPL-2.0-only

#include <linux/phy.h>
#include <linux/ethtool_netlink.h>
#include <net/netdev_lock.h>
#include "netlink.h"
#include "common.h"

/* 802.3 standard allows 100 meters for BaseT cables. However longer
 * cables might work, depending on the quality of the cables and the
 * PHY. So allow testing for up to 150 meters.
 */
#define MAX_CABLE_LENGTH_CM (150 * 100)

const struct nla_policy ethnl_cable_test_act_policy[] = {
 [ETHTOOL_A_CABLE_TEST_HEADER]  =
  NLA_POLICY_NESTED(ethnl_header_policy_phy),
};

static int ethnl_cable_test_started(struct phy_device *phydev, u8 cmd)
{
 struct sk_buff *skb;
 int err = -ENOMEM;
 void *ehdr;

 skb = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
 if (!skb)
  goto out;

 ehdr = ethnl_bcastmsg_put(skb, cmd);
 if (!ehdr) {
  err = -EMSGSIZE;
  goto out;
 }

 err = ethnl_fill_reply_header(skb, phydev->attached_dev,
          ETHTOOL_A_CABLE_TEST_NTF_HEADER);
 if (err)
  goto out;

 err = nla_put_u8(skb, ETHTOOL_A_CABLE_TEST_NTF_STATUS,
    ETHTOOL_A_CABLE_TEST_NTF_STATUS_STARTED);
 if (err)
  goto out;

 genlmsg_end(skb, ehdr);

 return ethnl_multicast(skb, phydev->attached_dev);

out:
 nlmsg_free(skb);
 phydev_err(phydev, "%s: Error %pe\n", __func__, ERR_PTR(err));

 return err;
}

int ethnl_act_cable_test(struct sk_buff *skb, struct genl_info *info)
{
 struct ethnl_req_info req_info = {};
 const struct ethtool_phy_ops *ops;
 struct nlattr **tb = info->attrs;
 struct phy_device *phydev;
 struct net_device *dev;
 int ret;

 ret = ethnl_parse_header_dev_get(&req_info,
      tb[ETHTOOL_A_CABLE_TEST_HEADER],
      genl_info_net(info), info->extack,
      true);
 if (ret < 0)
  return ret;

 dev = req_info.dev;

 rtnl_lock();
 netdev_lock_ops(dev);
 phydev = ethnl_req_get_phydev(&req_info, tb,
          ETHTOOL_A_CABLE_TEST_HEADER,
          info->extack);
 if (IS_ERR_OR_NULL(phydev)) {
  ret = -EOPNOTSUPP;
  goto out_unlock;
 }

 ops = ethtool_phy_ops;
 if (!ops || !ops->start_cable_test) {
  ret = -EOPNOTSUPP;
  goto out_unlock;
 }

 ret = ethnl_ops_begin(dev);
 if (ret < 0)
  goto out_unlock;

 ret = ops->start_cable_test(phydev, info->extack);

 ethnl_ops_complete(dev);

 if (!ret)
  ethnl_cable_test_started(phydev, ETHTOOL_MSG_CABLE_TEST_NTF);

out_unlock:
 netdev_unlock_ops(dev);
 rtnl_unlock();
 ethnl_parse_header_dev_put(&req_info);
 return ret;
}

int ethnl_cable_test_alloc(struct phy_device *phydev, u8 cmd)
{
 int err = -ENOMEM;

 /* One TDR sample occupies 20 bytes. For a 150 meter cable,
 * with four pairs, around 12K is needed.
 */
 phydev->skb = genlmsg_new(SZ_16K, GFP_KERNEL);
 if (!phydev->skb)
  goto out;

 phydev->ehdr = ethnl_bcastmsg_put(phydev->skb, cmd);
 if (!phydev->ehdr) {
  err = -EMSGSIZE;
  goto out;
 }

 err = ethnl_fill_reply_header(phydev->skb, phydev->attached_dev,
          ETHTOOL_A_CABLE_TEST_NTF_HEADER);
 if (err)
  goto out;

 err = nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_TEST_NTF_STATUS,
    ETHTOOL_A_CABLE_TEST_NTF_STATUS_COMPLETED);
 if (err)
  goto out;

 phydev->nest = nla_nest_start(phydev->skb,
          ETHTOOL_A_CABLE_TEST_NTF_NEST);
 if (!phydev->nest) {
  err = -EMSGSIZE;
  goto out;
 }

 return 0;

out:
 nlmsg_free(phydev->skb);
 phydev->skb = NULL;
 return err;
}
EXPORT_SYMBOL_GPL(ethnl_cable_test_alloc);

void ethnl_cable_test_free(struct phy_device *phydev)
{
 nlmsg_free(phydev->skb);
 phydev->skb = NULL;
}
EXPORT_SYMBOL_GPL(ethnl_cable_test_free);

void ethnl_cable_test_finished(struct phy_device *phydev)
{
 nla_nest_end(phydev->skb, phydev->nest);

 genlmsg_end(phydev->skb, phydev->ehdr);

 ethnl_multicast(phydev->skb, phydev->attached_dev);
}
EXPORT_SYMBOL_GPL(ethnl_cable_test_finished);

int ethnl_cable_test_result_with_src(struct phy_device *phydev, u8 pair,
         u8 result, u32 src)
{
 struct nlattr *nest;
 int ret = -EMSGSIZE;

 nest = nla_nest_start(phydev->skb, ETHTOOL_A_CABLE_NEST_RESULT);
 if (!nest)
  return -EMSGSIZE;

 if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_RESULT_PAIR, pair))
  goto err;
 if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_RESULT_CODE, result))
  goto err;
 if (src != ETHTOOL_A_CABLE_INF_SRC_UNSPEC) {
  if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_RESULT_SRC, src))
   goto err;
 }

 nla_nest_end(phydev->skb, nest);
 return 0;

err:
 nla_nest_cancel(phydev->skb, nest);
 return ret;
}
EXPORT_SYMBOL_GPL(ethnl_cable_test_result_with_src);

int ethnl_cable_test_fault_length_with_src(struct phy_device *phydev, u8 pair,
        u32 cm, u32 src)
{
 struct nlattr *nest;
 int ret = -EMSGSIZE;

 nest = nla_nest_start(phydev->skb,
         ETHTOOL_A_CABLE_NEST_FAULT_LENGTH);
 if (!nest)
  return -EMSGSIZE;

 if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_FAULT_LENGTH_PAIR, pair))
  goto err;
 if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_FAULT_LENGTH_CM, cm))
  goto err;
 if (src != ETHTOOL_A_CABLE_INF_SRC_UNSPEC) {
  if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_FAULT_LENGTH_SRC,
    src))
   goto err;
 }

 nla_nest_end(phydev->skb, nest);
 return 0;

err:
 nla_nest_cancel(phydev->skb, nest);
 return ret;
}
EXPORT_SYMBOL_GPL(ethnl_cable_test_fault_length_with_src);

static const struct nla_policy cable_test_tdr_act_cfg_policy[] = {
 [ETHTOOL_A_CABLE_TEST_TDR_CFG_FIRST] = { .type = NLA_U32 },
 [ETHTOOL_A_CABLE_TEST_TDR_CFG_LAST] = { .type = NLA_U32 },
 [ETHTOOL_A_CABLE_TEST_TDR_CFG_STEP] = { .type = NLA_U32 },
 [ETHTOOL_A_CABLE_TEST_TDR_CFG_PAIR] = { .type = NLA_U8 },
};

const struct nla_policy ethnl_cable_test_tdr_act_policy[] = {
 [ETHTOOL_A_CABLE_TEST_TDR_HEADER] =
  NLA_POLICY_NESTED(ethnl_header_policy_phy),
 [ETHTOOL_A_CABLE_TEST_TDR_CFG]  = { .type = NLA_NESTED },
};

/* CABLE_TEST_TDR_ACT */
static int ethnl_act_cable_test_tdr_cfg(const struct nlattr *nest,
     struct genl_info *info,
     struct phy_tdr_config *cfg)
{
 struct nlattr *tb[ARRAY_SIZE(cable_test_tdr_act_cfg_policy)];
 int ret;

 cfg->first = 100;
 cfg->step = 100;
 cfg->last = MAX_CABLE_LENGTH_CM;
 cfg->pair = PHY_PAIR_ALL;

 if (!nest)
  return 0;

 ret = nla_parse_nested(tb,
          ARRAY_SIZE(cable_test_tdr_act_cfg_policy) - 1,
          nest, cable_test_tdr_act_cfg_policy,
          info->extack);
 if (ret < 0)
  return ret;

 if (tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_FIRST])
  cfg->first = nla_get_u32(
   tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_FIRST]);

 if (tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_LAST])
  cfg->last = nla_get_u32(tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_LAST]);

 if (tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_STEP])
  cfg->step = nla_get_u32(tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_STEP]);

 if (tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_PAIR]) {
  cfg->pair = nla_get_u8(tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_PAIR]);
  if (cfg->pair > ETHTOOL_A_CABLE_PAIR_D) {
   NL_SET_ERR_MSG_ATTR(
    info->extack,
    tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_PAIR],
    "invalid pair parameter");
   return -EINVAL;
  }
 }

 if (cfg->first > MAX_CABLE_LENGTH_CM) {
  NL_SET_ERR_MSG_ATTR(info->extack,
        tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_FIRST],
        "invalid first parameter");
  return -EINVAL;
 }

 if (cfg->last > MAX_CABLE_LENGTH_CM) {
  NL_SET_ERR_MSG_ATTR(info->extack,
        tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_LAST],
        "invalid last parameter");
  return -EINVAL;
 }

 if (cfg->first > cfg->last) {
  NL_SET_ERR_MSG(info->extack, "invalid first/last parameter");
  return -EINVAL;
 }

 if (!cfg->step) {
  NL_SET_ERR_MSG_ATTR(info->extack,
        tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_STEP],
        "invalid step parameter");
  return -EINVAL;
 }

 if (cfg->step > (cfg->last - cfg->first)) {
  NL_SET_ERR_MSG_ATTR(info->extack,
        tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_STEP],
        "step parameter too big");
  return -EINVAL;
 }

 return 0;
}

int ethnl_act_cable_test_tdr(struct sk_buff *skb, struct genl_info *info)
{
 struct ethnl_req_info req_info = {};
 const struct ethtool_phy_ops *ops;
 struct nlattr **tb = info->attrs;
 struct phy_device *phydev;
 struct phy_tdr_config cfg;
 struct net_device *dev;
 int ret;

 ret = ethnl_parse_header_dev_get(&req_info,
      tb[ETHTOOL_A_CABLE_TEST_TDR_HEADER],
      genl_info_net(info), info->extack,
      true);
 if (ret < 0)
  return ret;

 dev = req_info.dev;

 ret = ethnl_act_cable_test_tdr_cfg(tb[ETHTOOL_A_CABLE_TEST_TDR_CFG],
        info, &cfg);
 if (ret)
  goto out_dev_put;

 rtnl_lock();
 netdev_lock_ops(dev);
 phydev = ethnl_req_get_phydev(&req_info, tb,
          ETHTOOL_A_CABLE_TEST_TDR_HEADER,
          info->extack);
 if (IS_ERR_OR_NULL(phydev)) {
  ret = -EOPNOTSUPP;
  goto out_unlock;
 }

 ops = ethtool_phy_ops;
 if (!ops || !ops->start_cable_test_tdr) {
  ret = -EOPNOTSUPP;
  goto out_unlock;
 }

 ret = ethnl_ops_begin(dev);
 if (ret < 0)
  goto out_unlock;

 ret = ops->start_cable_test_tdr(phydev, info->extack, &cfg);

 ethnl_ops_complete(dev);

 if (!ret)
  ethnl_cable_test_started(phydev,
      ETHTOOL_MSG_CABLE_TEST_TDR_NTF);

out_unlock:
 netdev_unlock_ops(dev);
 rtnl_unlock();
out_dev_put:
 ethnl_parse_header_dev_put(&req_info);
 return ret;
}

int ethnl_cable_test_amplitude(struct phy_device *phydev,
          u8 pair, s16 mV)
{
 struct nlattr *nest;
 int ret = -EMSGSIZE;

 nest = nla_nest_start(phydev->skb,
         ETHTOOL_A_CABLE_TDR_NEST_AMPLITUDE);
 if (!nest)
  return -EMSGSIZE;

 if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_AMPLITUDE_PAIR, pair))
  goto err;
 if (nla_put_u16(phydev->skb, ETHTOOL_A_CABLE_AMPLITUDE_mV, mV))
  goto err;

 nla_nest_end(phydev->skb, nest);
 return 0;

err:
 nla_nest_cancel(phydev->skb, nest);
 return ret;
}
EXPORT_SYMBOL_GPL(ethnl_cable_test_amplitude);

int ethnl_cable_test_pulse(struct phy_device *phydev, u16 mV)
{
 struct nlattr *nest;
 int ret = -EMSGSIZE;

 nest = nla_nest_start(phydev->skb, ETHTOOL_A_CABLE_TDR_NEST_PULSE);
 if (!nest)
  return -EMSGSIZE;

 if (nla_put_u16(phydev->skb, ETHTOOL_A_CABLE_PULSE_mV, mV))
  goto err;

 nla_nest_end(phydev->skb, nest);
 return 0;

err:
 nla_nest_cancel(phydev->skb, nest);
 return ret;
}
EXPORT_SYMBOL_GPL(ethnl_cable_test_pulse);

int ethnl_cable_test_step(struct phy_device *phydev, u32 first, u32 last,
     u32 step)
{
 struct nlattr *nest;
 int ret = -EMSGSIZE;

 nest = nla_nest_start(phydev->skb, ETHTOOL_A_CABLE_TDR_NEST_STEP);
 if (!nest)
  return -EMSGSIZE;

 if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_STEP_FIRST_DISTANCE,
   first))
  goto err;

 if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_STEP_LAST_DISTANCE, last))
  goto err;

 if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_STEP_STEP_DISTANCE, step))
  goto err;

 nla_nest_end(phydev->skb, nest);
 return 0;

err:
 nla_nest_cancel(phydev->skb, nest);
 return ret;
}
EXPORT_SYMBOL_GPL(ethnl_cable_test_step);