02180fb525
PLCA is a PHY-specific command. Instead of targeting the command towards dev->phydev, use the request to pick the targeted PHY. Signed-off-by: Maxime Chevallier <maxime.chevallier@bootlin.com> Reviewed-by: Christophe Leroy <christophe.leroy@csgroup.eu> Tested-by: Christophe Leroy <christophe.leroy@csgroup.eu> Signed-off-by: David S. Miller <davem@davemloft.net>
272 lines
7.8 KiB
C
272 lines
7.8 KiB
C
// SPDX-License-Identifier: GPL-2.0-only
|
|
|
|
#include <linux/phy.h>
|
|
#include <linux/ethtool_netlink.h>
|
|
|
|
#include "netlink.h"
|
|
#include "common.h"
|
|
|
|
struct plca_req_info {
|
|
struct ethnl_req_info base;
|
|
};
|
|
|
|
struct plca_reply_data {
|
|
struct ethnl_reply_data base;
|
|
struct phy_plca_cfg plca_cfg;
|
|
struct phy_plca_status plca_st;
|
|
};
|
|
|
|
// Helpers ------------------------------------------------------------------ //
|
|
|
|
#define PLCA_REPDATA(__reply_base) \
|
|
container_of(__reply_base, struct plca_reply_data, base)
|
|
|
|
// PLCA get configuration message ------------------------------------------- //
|
|
|
|
const struct nla_policy ethnl_plca_get_cfg_policy[] = {
|
|
[ETHTOOL_A_PLCA_HEADER] =
|
|
NLA_POLICY_NESTED(ethnl_header_policy_phy),
|
|
};
|
|
|
|
static void plca_update_sint(int *dst, struct nlattr **tb, u32 attrid,
|
|
bool *mod)
|
|
{
|
|
const struct nlattr *attr = tb[attrid];
|
|
|
|
if (!attr ||
|
|
WARN_ON_ONCE(attrid >= ARRAY_SIZE(ethnl_plca_set_cfg_policy)))
|
|
return;
|
|
|
|
switch (ethnl_plca_set_cfg_policy[attrid].type) {
|
|
case NLA_U8:
|
|
*dst = nla_get_u8(attr);
|
|
break;
|
|
case NLA_U32:
|
|
*dst = nla_get_u32(attr);
|
|
break;
|
|
default:
|
|
WARN_ON_ONCE(1);
|
|
}
|
|
|
|
*mod = true;
|
|
}
|
|
|
|
static int plca_get_cfg_prepare_data(const struct ethnl_req_info *req_base,
|
|
struct ethnl_reply_data *reply_base,
|
|
const struct genl_info *info)
|
|
{
|
|
struct plca_reply_data *data = PLCA_REPDATA(reply_base);
|
|
struct net_device *dev = reply_base->dev;
|
|
const struct ethtool_phy_ops *ops;
|
|
struct nlattr **tb = info->attrs;
|
|
struct phy_device *phydev;
|
|
int ret;
|
|
|
|
phydev = ethnl_req_get_phydev(req_base, tb[ETHTOOL_A_PLCA_HEADER],
|
|
info->extack);
|
|
// check that the PHY device is available and connected
|
|
if (IS_ERR_OR_NULL(phydev)) {
|
|
ret = -EOPNOTSUPP;
|
|
goto out;
|
|
}
|
|
|
|
// note: rtnl_lock is held already by ethnl_default_doit
|
|
ops = ethtool_phy_ops;
|
|
if (!ops || !ops->get_plca_cfg) {
|
|
ret = -EOPNOTSUPP;
|
|
goto out;
|
|
}
|
|
|
|
ret = ethnl_ops_begin(dev);
|
|
if (ret < 0)
|
|
goto out;
|
|
|
|
memset(&data->plca_cfg, 0xff,
|
|
sizeof_field(struct plca_reply_data, plca_cfg));
|
|
|
|
ret = ops->get_plca_cfg(phydev, &data->plca_cfg);
|
|
ethnl_ops_complete(dev);
|
|
|
|
out:
|
|
return ret;
|
|
}
|
|
|
|
static int plca_get_cfg_reply_size(const struct ethnl_req_info *req_base,
|
|
const struct ethnl_reply_data *reply_base)
|
|
{
|
|
return nla_total_size(sizeof(u16)) + /* _VERSION */
|
|
nla_total_size(sizeof(u8)) + /* _ENABLED */
|
|
nla_total_size(sizeof(u32)) + /* _NODE_CNT */
|
|
nla_total_size(sizeof(u32)) + /* _NODE_ID */
|
|
nla_total_size(sizeof(u32)) + /* _TO_TIMER */
|
|
nla_total_size(sizeof(u32)) + /* _BURST_COUNT */
|
|
nla_total_size(sizeof(u32)); /* _BURST_TIMER */
|
|
}
|
|
|
|
static int plca_get_cfg_fill_reply(struct sk_buff *skb,
|
|
const struct ethnl_req_info *req_base,
|
|
const struct ethnl_reply_data *reply_base)
|
|
{
|
|
const struct plca_reply_data *data = PLCA_REPDATA(reply_base);
|
|
const struct phy_plca_cfg *plca = &data->plca_cfg;
|
|
|
|
if ((plca->version >= 0 &&
|
|
nla_put_u16(skb, ETHTOOL_A_PLCA_VERSION, plca->version)) ||
|
|
(plca->enabled >= 0 &&
|
|
nla_put_u8(skb, ETHTOOL_A_PLCA_ENABLED, !!plca->enabled)) ||
|
|
(plca->node_id >= 0 &&
|
|
nla_put_u32(skb, ETHTOOL_A_PLCA_NODE_ID, plca->node_id)) ||
|
|
(plca->node_cnt >= 0 &&
|
|
nla_put_u32(skb, ETHTOOL_A_PLCA_NODE_CNT, plca->node_cnt)) ||
|
|
(plca->to_tmr >= 0 &&
|
|
nla_put_u32(skb, ETHTOOL_A_PLCA_TO_TMR, plca->to_tmr)) ||
|
|
(plca->burst_cnt >= 0 &&
|
|
nla_put_u32(skb, ETHTOOL_A_PLCA_BURST_CNT, plca->burst_cnt)) ||
|
|
(plca->burst_tmr >= 0 &&
|
|
nla_put_u32(skb, ETHTOOL_A_PLCA_BURST_TMR, plca->burst_tmr)))
|
|
return -EMSGSIZE;
|
|
|
|
return 0;
|
|
};
|
|
|
|
// PLCA set configuration message ------------------------------------------- //
|
|
|
|
const struct nla_policy ethnl_plca_set_cfg_policy[] = {
|
|
[ETHTOOL_A_PLCA_HEADER] =
|
|
NLA_POLICY_NESTED(ethnl_header_policy_phy),
|
|
[ETHTOOL_A_PLCA_ENABLED] = NLA_POLICY_MAX(NLA_U8, 1),
|
|
[ETHTOOL_A_PLCA_NODE_ID] = NLA_POLICY_MAX(NLA_U32, 255),
|
|
[ETHTOOL_A_PLCA_NODE_CNT] = NLA_POLICY_RANGE(NLA_U32, 1, 255),
|
|
[ETHTOOL_A_PLCA_TO_TMR] = NLA_POLICY_MAX(NLA_U32, 255),
|
|
[ETHTOOL_A_PLCA_BURST_CNT] = NLA_POLICY_MAX(NLA_U32, 255),
|
|
[ETHTOOL_A_PLCA_BURST_TMR] = NLA_POLICY_MAX(NLA_U32, 255),
|
|
};
|
|
|
|
static int
|
|
ethnl_set_plca(struct ethnl_req_info *req_info, struct genl_info *info)
|
|
{
|
|
const struct ethtool_phy_ops *ops;
|
|
struct nlattr **tb = info->attrs;
|
|
struct phy_plca_cfg plca_cfg;
|
|
struct phy_device *phydev;
|
|
bool mod = false;
|
|
int ret;
|
|
|
|
phydev = ethnl_req_get_phydev(req_info, tb[ETHTOOL_A_PLCA_HEADER],
|
|
info->extack);
|
|
// check that the PHY device is available and connected
|
|
if (IS_ERR_OR_NULL(phydev))
|
|
return -EOPNOTSUPP;
|
|
|
|
ops = ethtool_phy_ops;
|
|
if (!ops || !ops->set_plca_cfg)
|
|
return -EOPNOTSUPP;
|
|
|
|
memset(&plca_cfg, 0xff, sizeof(plca_cfg));
|
|
plca_update_sint(&plca_cfg.enabled, tb, ETHTOOL_A_PLCA_ENABLED, &mod);
|
|
plca_update_sint(&plca_cfg.node_id, tb, ETHTOOL_A_PLCA_NODE_ID, &mod);
|
|
plca_update_sint(&plca_cfg.node_cnt, tb, ETHTOOL_A_PLCA_NODE_CNT, &mod);
|
|
plca_update_sint(&plca_cfg.to_tmr, tb, ETHTOOL_A_PLCA_TO_TMR, &mod);
|
|
plca_update_sint(&plca_cfg.burst_cnt, tb, ETHTOOL_A_PLCA_BURST_CNT,
|
|
&mod);
|
|
plca_update_sint(&plca_cfg.burst_tmr, tb, ETHTOOL_A_PLCA_BURST_TMR,
|
|
&mod);
|
|
if (!mod)
|
|
return 0;
|
|
|
|
ret = ops->set_plca_cfg(phydev, &plca_cfg, info->extack);
|
|
return ret < 0 ? ret : 1;
|
|
}
|
|
|
|
const struct ethnl_request_ops ethnl_plca_cfg_request_ops = {
|
|
.request_cmd = ETHTOOL_MSG_PLCA_GET_CFG,
|
|
.reply_cmd = ETHTOOL_MSG_PLCA_GET_CFG_REPLY,
|
|
.hdr_attr = ETHTOOL_A_PLCA_HEADER,
|
|
.req_info_size = sizeof(struct plca_req_info),
|
|
.reply_data_size = sizeof(struct plca_reply_data),
|
|
|
|
.prepare_data = plca_get_cfg_prepare_data,
|
|
.reply_size = plca_get_cfg_reply_size,
|
|
.fill_reply = plca_get_cfg_fill_reply,
|
|
|
|
.set = ethnl_set_plca,
|
|
.set_ntf_cmd = ETHTOOL_MSG_PLCA_NTF,
|
|
};
|
|
|
|
// PLCA get status message -------------------------------------------------- //
|
|
|
|
const struct nla_policy ethnl_plca_get_status_policy[] = {
|
|
[ETHTOOL_A_PLCA_HEADER] =
|
|
NLA_POLICY_NESTED(ethnl_header_policy_phy),
|
|
};
|
|
|
|
static int plca_get_status_prepare_data(const struct ethnl_req_info *req_base,
|
|
struct ethnl_reply_data *reply_base,
|
|
const struct genl_info *info)
|
|
{
|
|
struct plca_reply_data *data = PLCA_REPDATA(reply_base);
|
|
struct net_device *dev = reply_base->dev;
|
|
const struct ethtool_phy_ops *ops;
|
|
struct nlattr **tb = info->attrs;
|
|
struct phy_device *phydev;
|
|
int ret;
|
|
|
|
phydev = ethnl_req_get_phydev(req_base, tb[ETHTOOL_A_PLCA_HEADER],
|
|
info->extack);
|
|
// check that the PHY device is available and connected
|
|
if (IS_ERR_OR_NULL(phydev)) {
|
|
ret = -EOPNOTSUPP;
|
|
goto out;
|
|
}
|
|
|
|
// note: rtnl_lock is held already by ethnl_default_doit
|
|
ops = ethtool_phy_ops;
|
|
if (!ops || !ops->get_plca_status) {
|
|
ret = -EOPNOTSUPP;
|
|
goto out;
|
|
}
|
|
|
|
ret = ethnl_ops_begin(dev);
|
|
if (ret < 0)
|
|
goto out;
|
|
|
|
memset(&data->plca_st, 0xff,
|
|
sizeof_field(struct plca_reply_data, plca_st));
|
|
|
|
ret = ops->get_plca_status(phydev, &data->plca_st);
|
|
ethnl_ops_complete(dev);
|
|
out:
|
|
return ret;
|
|
}
|
|
|
|
static int plca_get_status_reply_size(const struct ethnl_req_info *req_base,
|
|
const struct ethnl_reply_data *reply_base)
|
|
{
|
|
return nla_total_size(sizeof(u8)); /* _STATUS */
|
|
}
|
|
|
|
static int plca_get_status_fill_reply(struct sk_buff *skb,
|
|
const struct ethnl_req_info *req_base,
|
|
const struct ethnl_reply_data *reply_base)
|
|
{
|
|
const struct plca_reply_data *data = PLCA_REPDATA(reply_base);
|
|
const u8 status = data->plca_st.pst;
|
|
|
|
if (nla_put_u8(skb, ETHTOOL_A_PLCA_STATUS, !!status))
|
|
return -EMSGSIZE;
|
|
|
|
return 0;
|
|
};
|
|
|
|
const struct ethnl_request_ops ethnl_plca_status_request_ops = {
|
|
.request_cmd = ETHTOOL_MSG_PLCA_GET_STATUS,
|
|
.reply_cmd = ETHTOOL_MSG_PLCA_GET_STATUS_REPLY,
|
|
.hdr_attr = ETHTOOL_A_PLCA_HEADER,
|
|
.req_info_size = sizeof(struct plca_req_info),
|
|
.reply_data_size = sizeof(struct plca_reply_data),
|
|
|
|
.prepare_data = plca_get_status_prepare_data,
|
|
.reply_size = plca_get_status_reply_size,
|
|
.fill_reply = plca_get_status_fill_reply,
|
|
};
|