// SPDX-License-Identifier: GPL-2.0-only #include <linux/phy.h>#include <linux/ethtool_netlink.h> #include "common.h"#include "netlink.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 intethnl_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,};