[linux-2.6-block.git] / net / ethtool / plca.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)

static void plca_update_sint(int *dst, const struct nlattr *attr,
			     bool *mod)
{
	if (!attr)
		return;

	*dst = nla_get_u32(attr);
	*mod = true;
}

// PLCA get configuration message ------------------------------------------- //

const struct nla_policy ethnl_plca_get_cfg_policy[] = {
	[ETHTOOL_A_PLCA_HEADER]		=
		NLA_POLICY_NESTED(ethnl_header_policy),
};

static int plca_get_cfg_prepare_data(const struct ethnl_req_info *req_base,
				     struct ethnl_reply_data *reply_base,
				     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;
	int ret;

	// check that the PHY device is available and connected
	if (!dev->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(dev->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),
	[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)
{
	struct net_device *dev = req_info->dev;
	const struct ethtool_phy_ops *ops;
	struct nlattr **tb = info->attrs;
	struct phy_plca_cfg plca_cfg;
	bool mod = false;
	int ret;

	// check that the PHY device is available and connected
	if (!dev->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(dev->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),
};

static int plca_get_status_prepare_data(const struct ethnl_req_info *req_base,
					struct ethnl_reply_data *reply_base,
					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;
	int ret;

	// check that the PHY device is available and connected
	if (!dev->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(dev->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,
};
Commit	Line	Data
8580e16c PB	1	// SPDX-License-Identifier: GPL-2.0-only
	2
	3	#include <linux/phy.h>
	4	#include <linux/ethtool_netlink.h>
	5
	6	#include "netlink.h"
	7	#include "common.h"
	8
	9	struct plca_req_info {
	10	struct ethnl_req_info base;
	11	};
	12
	13	struct plca_reply_data {
	14	struct ethnl_reply_data base;
	15	struct phy_plca_cfg plca_cfg;
	16	struct phy_plca_status plca_st;
	17	};
	18
	19	// Helpers ------------------------------------------------------------------ //
	20
	21	#define PLCA_REPDATA(__reply_base) \
	22	container_of(__reply_base, struct plca_reply_data, base)
	23
	24	static void plca_update_sint(int dst, const struct nlattr attr,
	25	bool *mod)
	26	{
	27	if (!attr)
	28	return;
	29
	30	*dst = nla_get_u32(attr);
	31	*mod = true;
	32	}
	33
	34	// PLCA get configuration message ------------------------------------------- //
	35
	36	const struct nla_policy ethnl_plca_get_cfg_policy[] = {
	37	[ETHTOOL_A_PLCA_HEADER] =
	38	NLA_POLICY_NESTED(ethnl_header_policy),
	39	};
	40
	41	static int plca_get_cfg_prepare_data(const struct ethnl_req_info *req_base,
	42	struct ethnl_reply_data *reply_base,
	43	struct genl_info *info)
	44	{
	45	struct plca_reply_data *data = PLCA_REPDATA(reply_base);
	46	struct net_device *dev = reply_base->dev;
	47	const struct ethtool_phy_ops *ops;
	48	int ret;
	49
	50	// check that the PHY device is available and connected
	51	if (!dev->phydev) {
	52	ret = -EOPNOTSUPP;
	53	goto out;
	54	}
	55
	56	// note: rtnl_lock is held already by ethnl_default_doit
	57	ops = ethtool_phy_ops;
	58	if (!ops \|\| !ops->get_plca_cfg) {
	59	ret = -EOPNOTSUPP;
	60	goto out;
	61	}
	62
	63	ret = ethnl_ops_begin(dev);
28dbf774	64	if (ret < 0)
8580e16c PB	65	goto out;
	66
	67	memset(&data->plca_cfg, 0xff,
	68	sizeof_field(struct plca_reply_data, plca_cfg));
	69
	70	ret = ops->get_plca_cfg(dev->phydev, &data->plca_cfg);
	71	ethnl_ops_complete(dev);
	72
	73	out:
	74	return ret;
	75	}
	76
	77	static int plca_get_cfg_reply_size(const struct ethnl_req_info *req_base,
	78	const struct ethnl_reply_data *reply_base)
	79	{
	80	return nla_total_size(sizeof(u16)) + /* _VERSION */
	81	nla_total_size(sizeof(u8)) + /* _ENABLED */
	82	nla_total_size(sizeof(u32)) + /* _NODE_CNT */
	83	nla_total_size(sizeof(u32)) + /* _NODE_ID */
	84	nla_total_size(sizeof(u32)) + /* _TO_TIMER */
	85	nla_total_size(sizeof(u32)) + /* _BURST_COUNT */
	86	nla_total_size(sizeof(u32)); /* _BURST_TIMER */
	87	}
	88
	89	static int plca_get_cfg_fill_reply(struct sk_buff *skb,
	90	const struct ethnl_req_info *req_base,
	91	const struct ethnl_reply_data *reply_base)
	92	{
	93	const struct plca_reply_data *data = PLCA_REPDATA(reply_base);
	94	const struct phy_plca_cfg *plca = &data->plca_cfg;
	95
	96	if ((plca->version >= 0 &&
	97	nla_put_u16(skb, ETHTOOL_A_PLCA_VERSION, plca->version)) \|\|
	98	(plca->enabled >= 0 &&
	99	nla_put_u8(skb, ETHTOOL_A_PLCA_ENABLED, !!plca->enabled)) \|\|
	100	(plca->node_id >= 0 &&
	101	nla_put_u32(skb, ETHTOOL_A_PLCA_NODE_ID, plca->node_id)) \|\|
	102	(plca->node_cnt >= 0 &&
	103	nla_put_u32(skb, ETHTOOL_A_PLCA_NODE_CNT, plca->node_cnt)) \|\|
	104	(plca->to_tmr >= 0 &&
	105	nla_put_u32(skb, ETHTOOL_A_PLCA_TO_TMR, plca->to_tmr)) \|\|
	106	(plca->burst_cnt >= 0 &&
	107	nla_put_u32(skb, ETHTOOL_A_PLCA_BURST_CNT, plca->burst_cnt)) \|\|
	108	(plca->burst_tmr >= 0 &&
	109	nla_put_u32(skb, ETHTOOL_A_PLCA_BURST_TMR, plca->burst_tmr)))
	110	return -EMSGSIZE;
	111
	112	return 0;
	113	};
	114
8580e16c PB	115	// PLCA set configuration message ------------------------------------------- //
	116
	117	const struct nla_policy ethnl_plca_set_cfg_policy[] = {
	118	[ETHTOOL_A_PLCA_HEADER] =
	119	NLA_POLICY_NESTED(ethnl_header_policy),
	120	[ETHTOOL_A_PLCA_ENABLED] = NLA_POLICY_MAX(NLA_U8, 1),
	121	[ETHTOOL_A_PLCA_NODE_ID] = NLA_POLICY_MAX(NLA_U32, 255),
	122	[ETHTOOL_A_PLCA_NODE_CNT] = NLA_POLICY_RANGE(NLA_U32, 1, 255),
	123	[ETHTOOL_A_PLCA_TO_TMR] = NLA_POLICY_MAX(NLA_U32, 255),
	124	[ETHTOOL_A_PLCA_BURST_CNT] = NLA_POLICY_MAX(NLA_U32, 255),
	125	[ETHTOOL_A_PLCA_BURST_TMR] = NLA_POLICY_MAX(NLA_U32, 255),
	126	};
	127
04007961 JK	128	static int
04007961 JK	129	ethnl_set_plca(struct ethnl_req_info req_info, struct genl_info info)
8580e16c	130	{
04007961	131	struct net_device *dev = req_info->dev;
8580e16c	132	const struct ethtool_phy_ops *ops;
04007961	133	struct nlattr **tb = info->attrs;
8580e16c	134	struct phy_plca_cfg plca_cfg;
8580e16c PB	135	bool mod = false;
	136	int ret;
	137
8580e16c	138	// check that the PHY device is available and connected
04007961 JK	139	if (!dev->phydev)
04007961 JK	140	return -EOPNOTSUPP;
8580e16c PB	141
8580e16c PB	142	ops = ethtool_phy_ops;
04007961 JK	143	if (!ops \|\| !ops->set_plca_cfg)
04007961 JK	144	return -EOPNOTSUPP;
8580e16c PB	145
	146	memset(&plca_cfg, 0xff, sizeof(plca_cfg));
	147	plca_update_sint(&plca_cfg.enabled, tb[ETHTOOL_A_PLCA_ENABLED], &mod);
	148	plca_update_sint(&plca_cfg.node_id, tb[ETHTOOL_A_PLCA_NODE_ID], &mod);
	149	plca_update_sint(&plca_cfg.node_cnt, tb[ETHTOOL_A_PLCA_NODE_CNT], &mod);
	150	plca_update_sint(&plca_cfg.to_tmr, tb[ETHTOOL_A_PLCA_TO_TMR], &mod);
	151	plca_update_sint(&plca_cfg.burst_cnt, tb[ETHTOOL_A_PLCA_BURST_CNT],
	152	&mod);
	153	plca_update_sint(&plca_cfg.burst_tmr, tb[ETHTOOL_A_PLCA_BURST_TMR],
	154	&mod);
8580e16c	155	if (!mod)
04007961	156	return 0;
8580e16c PB	157
8580e16c PB	158	ret = ops->set_plca_cfg(dev->phydev, &plca_cfg, info->extack);
04007961 JK	159	return ret < 0 ? ret : 1;
04007961 JK	160	}
8580e16c	161
04007961 JK	162	const struct ethnl_request_ops ethnl_plca_cfg_request_ops = {
	163	.request_cmd = ETHTOOL_MSG_PLCA_GET_CFG,
	164	.reply_cmd = ETHTOOL_MSG_PLCA_GET_CFG_REPLY,
	165	.hdr_attr = ETHTOOL_A_PLCA_HEADER,
	166	.req_info_size = sizeof(struct plca_req_info),
	167	.reply_data_size = sizeof(struct plca_reply_data),
8580e16c	168
04007961 JK	169	.prepare_data = plca_get_cfg_prepare_data,
	170	.reply_size = plca_get_cfg_reply_size,
	171	.fill_reply = plca_get_cfg_fill_reply,
8580e16c	172
04007961 JK	173	.set = ethnl_set_plca,
	174	.set_ntf_cmd = ETHTOOL_MSG_PLCA_NTF,
	175	};
8580e16c PB	176
	177	// PLCA get status message -------------------------------------------------- //
	178
	179	const struct nla_policy ethnl_plca_get_status_policy[] = {
	180	[ETHTOOL_A_PLCA_HEADER] =
	181	NLA_POLICY_NESTED(ethnl_header_policy),
	182	};
	183
	184	static int plca_get_status_prepare_data(const struct ethnl_req_info *req_base,
	185	struct ethnl_reply_data *reply_base,
	186	struct genl_info *info)
	187	{
	188	struct plca_reply_data *data = PLCA_REPDATA(reply_base);
	189	struct net_device *dev = reply_base->dev;
	190	const struct ethtool_phy_ops *ops;
	191	int ret;
	192
	193	// check that the PHY device is available and connected
	194	if (!dev->phydev) {
	195	ret = -EOPNOTSUPP;
	196	goto out;
	197	}
	198
	199	// note: rtnl_lock is held already by ethnl_default_doit
	200	ops = ethtool_phy_ops;
	201	if (!ops \|\| !ops->get_plca_status) {
	202	ret = -EOPNOTSUPP;
	203	goto out;
	204	}
	205
	206	ret = ethnl_ops_begin(dev);
28dbf774	207	if (ret < 0)
8580e16c PB	208	goto out;
	209
	210	memset(&data->plca_st, 0xff,
	211	sizeof_field(struct plca_reply_data, plca_st));
	212
	213	ret = ops->get_plca_status(dev->phydev, &data->plca_st);
	214	ethnl_ops_complete(dev);
	215	out:
	216	return ret;
	217	}
	218
	219	static int plca_get_status_reply_size(const struct ethnl_req_info *req_base,
	220	const struct ethnl_reply_data *reply_base)
	221	{
	222	return nla_total_size(sizeof(u8)); /* _STATUS */
	223	}
	224
	225	static int plca_get_status_fill_reply(struct sk_buff *skb,
	226	const struct ethnl_req_info *req_base,
	227	const struct ethnl_reply_data *reply_base)
	228	{
	229	const struct plca_reply_data *data = PLCA_REPDATA(reply_base);
	230	const u8 status = data->plca_st.pst;
	231
	232	if (nla_put_u8(skb, ETHTOOL_A_PLCA_STATUS, !!status))
	233	return -EMSGSIZE;
	234
	235	return 0;
	236	};
	237
	238	const struct ethnl_request_ops ethnl_plca_status_request_ops = {
	239	.request_cmd = ETHTOOL_MSG_PLCA_GET_STATUS,
	240	.reply_cmd = ETHTOOL_MSG_PLCA_GET_STATUS_REPLY,
	241	.hdr_attr = ETHTOOL_A_PLCA_HEADER,
	242	.req_info_size = sizeof(struct plca_req_info),
	243	.reply_data_size = sizeof(struct plca_reply_data),
	244
	245	.prepare_data = plca_get_status_prepare_data,
	246	.reply_size = plca_get_status_reply_size,
	247	.fill_reply = plca_get_status_fill_reply,
	248	};