[linux-block.git] / net / ethtool / cabletest.c

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

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

/* CABLE_TEST_ACT */

static const struct nla_policy
cable_test_act_policy[ETHTOOL_A_CABLE_TEST_MAX + 1] = {
	[ETHTOOL_A_CABLE_TEST_UNSPEC]		= { .type = NLA_REJECT },
	[ETHTOOL_A_CABLE_TEST_HEADER]		= { .type = NLA_NESTED },
};

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 nlattr *tb[ETHTOOL_A_CABLE_TEST_MAX + 1];
	struct ethnl_req_info req_info = {};
	struct net_device *dev;
	int ret;

	ret = nlmsg_parse(info->nlhdr, GENL_HDRLEN, tb,
			  ETHTOOL_A_CABLE_TEST_MAX,
			  cable_test_act_policy, info->extack);
	if (ret < 0)
		return 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;
	if (!dev->phydev) {
		ret = -EOPNOTSUPP;
		goto out_dev_put;
	}

	rtnl_lock();
	ret = ethnl_ops_begin(dev);
	if (ret < 0)
		goto out_rtnl;

	ret = phy_start_cable_test(dev->phydev, info->extack);

	ethnl_ops_complete(dev);

	if (!ret)
		ethnl_cable_test_started(dev->phydev,
					 ETHTOOL_MSG_CABLE_TEST_NTF);

out_rtnl:
	rtnl_unlock();
out_dev_put:
	dev_put(dev);
	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(struct phy_device *phydev, u8 pair, u8 result)
{
	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;

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

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

int ethnl_cable_test_fault_length(struct phy_device *phydev, u8 pair, u32 cm)
{
	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;

	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);

static const struct nla_policy
cable_test_tdr_act_policy[ETHTOOL_A_CABLE_TEST_TDR_MAX + 1] = {
	[ETHTOOL_A_CABLE_TEST_TDR_UNSPEC]	= { .type = NLA_REJECT },
	[ETHTOOL_A_CABLE_TEST_TDR_HEADER]	= { .type = NLA_NESTED },
};

int ethnl_act_cable_test_tdr(struct sk_buff *skb, struct genl_info *info)
{
	struct nlattr *tb[ETHTOOL_A_CABLE_TEST_TDR_MAX + 1];
	struct ethnl_req_info req_info = {};
	struct net_device *dev;
	int ret;

	ret = nlmsg_parse(info->nlhdr, GENL_HDRLEN, tb,
			  ETHTOOL_A_CABLE_TEST_TDR_MAX,
			  cable_test_tdr_act_policy, info->extack);
	if (ret < 0)
		return 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;
	if (!dev->phydev) {
		ret = -EOPNOTSUPP;
		goto out_dev_put;
	}

	rtnl_lock();
	ret = ethnl_ops_begin(dev);
	if (ret < 0)
		goto out_rtnl;

	ret = phy_start_cable_test_tdr(dev->phydev, info->extack);

	ethnl_ops_complete(dev);

	if (!ret)
		ethnl_cable_test_started(dev->phydev,
					 ETHTOOL_MSG_CABLE_TEST_TDR_NTF);

out_rtnl:
	rtnl_unlock();
out_dev_put:
	dev_put(dev);
	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);
Commit	Line	Data
11ca3c42 AL	1	// SPDX-License-Identifier: GPL-2.0-only
	2
	3	#include <linux/phy.h>
1dd3f212	4	#include <linux/ethtool_netlink.h>
11ca3c42 AL	5	#include "netlink.h"
	6	#include "common.h"
	7
	8	/* CABLE_TEST_ACT */
	9
	10	static const struct nla_policy
	11	cable_test_act_policy[ETHTOOL_A_CABLE_TEST_MAX + 1] = {
	12	[ETHTOOL_A_CABLE_TEST_UNSPEC] = { .type = NLA_REJECT },
	13	[ETHTOOL_A_CABLE_TEST_HEADER] = { .type = NLA_NESTED },
	14	};
	15
1a644de2	16	static int ethnl_cable_test_started(struct phy_device *phydev, u8 cmd)
9896a457 AL	17	{
	18	struct sk_buff *skb;
	19	int err = -ENOMEM;
	20	void *ehdr;
	21
	22	skb = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
	23	if (!skb)
	24	goto out;
	25
1a644de2	26	ehdr = ethnl_bcastmsg_put(skb, cmd);
9896a457 AL	27	if (!ehdr) {
	28	err = -EMSGSIZE;
	29	goto out;
	30	}
	31
	32	err = ethnl_fill_reply_header(skb, phydev->attached_dev,
	33	ETHTOOL_A_CABLE_TEST_NTF_HEADER);
	34	if (err)
	35	goto out;
	36
	37	err = nla_put_u8(skb, ETHTOOL_A_CABLE_TEST_NTF_STATUS,
	38	ETHTOOL_A_CABLE_TEST_NTF_STATUS_STARTED);
	39	if (err)
	40	goto out;
	41
	42	genlmsg_end(skb, ehdr);
	43
	44	return ethnl_multicast(skb, phydev->attached_dev);
	45
	46	out:
	47	nlmsg_free(skb);
	48	phydev_err(phydev, "%s: Error %pe\n", __func__, ERR_PTR(err));
	49
	50	return err;
	51	}
	52
11ca3c42 AL	53	int ethnl_act_cable_test(struct sk_buff skb, struct genl_info info)
	54	{
	55	struct nlattr *tb[ETHTOOL_A_CABLE_TEST_MAX + 1];
	56	struct ethnl_req_info req_info = {};
	57	struct net_device *dev;
	58	int ret;
	59
	60	ret = nlmsg_parse(info->nlhdr, GENL_HDRLEN, tb,
	61	ETHTOOL_A_CABLE_TEST_MAX,
	62	cable_test_act_policy, info->extack);
	63	if (ret < 0)
	64	return ret;
	65
	66	ret = ethnl_parse_header_dev_get(&req_info,
	67	tb[ETHTOOL_A_CABLE_TEST_HEADER],
	68	genl_info_net(info), info->extack,
	69	true);
	70	if (ret < 0)
	71	return ret;
	72
	73	dev = req_info.dev;
	74	if (!dev->phydev) {
	75	ret = -EOPNOTSUPP;
	76	goto out_dev_put;
	77	}
	78
	79	rtnl_lock();
	80	ret = ethnl_ops_begin(dev);
	81	if (ret < 0)
	82	goto out_rtnl;
	83
	84	ret = phy_start_cable_test(dev->phydev, info->extack);
	85
	86	ethnl_ops_complete(dev);
9896a457 AL	87
9896a457 AL	88	if (!ret)
1a644de2 AL	89	ethnl_cable_test_started(dev->phydev,
1a644de2 AL	90	ETHTOOL_MSG_CABLE_TEST_NTF);
9896a457	91
11ca3c42 AL	92	out_rtnl:
	93	rtnl_unlock();
	94	out_dev_put:
	95	dev_put(dev);
	96	return ret;
	97	}
1dd3f212	98
1a644de2	99	int ethnl_cable_test_alloc(struct phy_device *phydev, u8 cmd)
1dd3f212 AL	100	{
	101	int err = -ENOMEM;
	102
6b4a0fc1 AL	103	/* One TDR sample occupies 20 bytes. For a 150 meter cable,
	104	* with four pairs, around 12K is needed.
	105	*/
	106	phydev->skb = genlmsg_new(SZ_16K, GFP_KERNEL);
1dd3f212 AL	107	if (!phydev->skb)
	108	goto out;
	109
1a644de2	110	phydev->ehdr = ethnl_bcastmsg_put(phydev->skb, cmd);
1dd3f212 AL	111	if (!phydev->ehdr) {
	112	err = -EMSGSIZE;
	113	goto out;
	114	}
	115
	116	err = ethnl_fill_reply_header(phydev->skb, phydev->attached_dev,
	117	ETHTOOL_A_CABLE_TEST_NTF_HEADER);
	118	if (err)
	119	goto out;
	120
	121	err = nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_TEST_NTF_STATUS,
	122	ETHTOOL_A_CABLE_TEST_NTF_STATUS_COMPLETED);
	123	if (err)
	124	goto out;
	125
	126	phydev->nest = nla_nest_start(phydev->skb,
	127	ETHTOOL_A_CABLE_TEST_NTF_NEST);
1e2dc145 AL	128	if (!phydev->nest) {
1e2dc145 AL	129	err = -EMSGSIZE;
1dd3f212	130	goto out;
1e2dc145	131	}
1dd3f212 AL	132
	133	return 0;
	134
	135	out:
	136	nlmsg_free(phydev->skb);
1e2dc145	137	phydev->skb = NULL;
1dd3f212 AL	138	return err;
	139	}
	140	EXPORT_SYMBOL_GPL(ethnl_cable_test_alloc);
	141
	142	void ethnl_cable_test_free(struct phy_device *phydev)
	143	{
	144	nlmsg_free(phydev->skb);
1e2dc145	145	phydev->skb = NULL;
1dd3f212 AL	146	}
	147	EXPORT_SYMBOL_GPL(ethnl_cable_test_free);
	148
	149	void ethnl_cable_test_finished(struct phy_device *phydev)
	150	{
	151	nla_nest_end(phydev->skb, phydev->nest);
	152
	153	genlmsg_end(phydev->skb, phydev->ehdr);
	154
	155	ethnl_multicast(phydev->skb, phydev->attached_dev);
	156	}
	157	EXPORT_SYMBOL_GPL(ethnl_cable_test_finished);
1e2dc145 AL	158
	159	int ethnl_cable_test_result(struct phy_device *phydev, u8 pair, u8 result)
	160	{
	161	struct nlattr *nest;
	162	int ret = -EMSGSIZE;
	163
	164	nest = nla_nest_start(phydev->skb, ETHTOOL_A_CABLE_NEST_RESULT);
	165	if (!nest)
	166	return -EMSGSIZE;
	167
	168	if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_RESULT_PAIR, pair))
	169	goto err;
	170	if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_RESULT_CODE, result))
	171	goto err;
	172
	173	nla_nest_end(phydev->skb, nest);
	174	return 0;
	175
	176	err:
	177	nla_nest_cancel(phydev->skb, nest);
	178	return ret;
	179	}
	180	EXPORT_SYMBOL_GPL(ethnl_cable_test_result);
	181
	182	int ethnl_cable_test_fault_length(struct phy_device *phydev, u8 pair, u32 cm)
	183	{
	184	struct nlattr *nest;
	185	int ret = -EMSGSIZE;
	186
	187	nest = nla_nest_start(phydev->skb,
	188	ETHTOOL_A_CABLE_NEST_FAULT_LENGTH);
	189	if (!nest)
	190	return -EMSGSIZE;
	191
	192	if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_FAULT_LENGTH_PAIR, pair))
	193	goto err;
	194	if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_FAULT_LENGTH_CM, cm))
	195	goto err;
	196
	197	nla_nest_end(phydev->skb, nest);
	198	return 0;
	199
	200	err:
	201	nla_nest_cancel(phydev->skb, nest);
	202	return ret;
	203	}
	204	EXPORT_SYMBOL_GPL(ethnl_cable_test_fault_length);
1a644de2 AL	205
	206	static const struct nla_policy
	207	cable_test_tdr_act_policy[ETHTOOL_A_CABLE_TEST_TDR_MAX + 1] = {
	208	[ETHTOOL_A_CABLE_TEST_TDR_UNSPEC] = { .type = NLA_REJECT },
	209	[ETHTOOL_A_CABLE_TEST_TDR_HEADER] = { .type = NLA_NESTED },
	210	};
	211
	212	int ethnl_act_cable_test_tdr(struct sk_buff skb, struct genl_info info)
	213	{
	214	struct nlattr *tb[ETHTOOL_A_CABLE_TEST_TDR_MAX + 1];
	215	struct ethnl_req_info req_info = {};
	216	struct net_device *dev;
	217	int ret;
	218
	219	ret = nlmsg_parse(info->nlhdr, GENL_HDRLEN, tb,
	220	ETHTOOL_A_CABLE_TEST_TDR_MAX,
	221	cable_test_tdr_act_policy, info->extack);
	222	if (ret < 0)
	223	return ret;
	224
	225	ret = ethnl_parse_header_dev_get(&req_info,
	226	tb[ETHTOOL_A_CABLE_TEST_TDR_HEADER],
	227	genl_info_net(info), info->extack,
	228	true);
	229	if (ret < 0)
	230	return ret;
	231
	232	dev = req_info.dev;
	233	if (!dev->phydev) {
	234	ret = -EOPNOTSUPP;
	235	goto out_dev_put;
	236	}
	237
	238	rtnl_lock();
	239	ret = ethnl_ops_begin(dev);
	240	if (ret < 0)
	241	goto out_rtnl;
	242
	243	ret = phy_start_cable_test_tdr(dev->phydev, info->extack);
	244
	245	ethnl_ops_complete(dev);
	246
	247	if (!ret)
	248	ethnl_cable_test_started(dev->phydev,
	249	ETHTOOL_MSG_CABLE_TEST_TDR_NTF);
	250
	251	out_rtnl:
	252	rtnl_unlock();
	253	out_dev_put:
	254	dev_put(dev);
	255	return ret;
	256	}
6b4a0fc1 AL	257
	258	int ethnl_cable_test_amplitude(struct phy_device *phydev,
	259	u8 pair, s16 mV)
	260	{
	261	struct nlattr *nest;
	262	int ret = -EMSGSIZE;
	263
	264	nest = nla_nest_start(phydev->skb,
	265	ETHTOOL_A_CABLE_TDR_NEST_AMPLITUDE);
	266	if (!nest)
	267	return -EMSGSIZE;
	268
	269	if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_AMPLITUDE_PAIR, pair))
	270	goto err;
	271	if (nla_put_u16(phydev->skb, ETHTOOL_A_CABLE_AMPLITUDE_mV, mV))
	272	goto err;
	273
	274	nla_nest_end(phydev->skb, nest);
	275	return 0;
	276
	277	err:
	278	nla_nest_cancel(phydev->skb, nest);
	279	return ret;
	280	}
	281	EXPORT_SYMBOL_GPL(ethnl_cable_test_amplitude);
	282
	283	int ethnl_cable_test_pulse(struct phy_device *phydev, u16 mV)
	284	{
	285	struct nlattr *nest;
	286	int ret = -EMSGSIZE;
	287
	288	nest = nla_nest_start(phydev->skb, ETHTOOL_A_CABLE_TDR_NEST_PULSE);
	289	if (!nest)
	290	return -EMSGSIZE;
	291
	292	if (nla_put_u16(phydev->skb, ETHTOOL_A_CABLE_PULSE_mV, mV))
	293	goto err;
	294
	295	nla_nest_end(phydev->skb, nest);
	296	return 0;
	297
	298	err:
	299	nla_nest_cancel(phydev->skb, nest);
	300	return ret;
	301	}
	302	EXPORT_SYMBOL_GPL(ethnl_cable_test_pulse);
	303
	304	int ethnl_cable_test_step(struct phy_device *phydev, u32 first, u32 last,
	305	u32 step)
	306	{
	307	struct nlattr *nest;
	308	int ret = -EMSGSIZE;
	309
	310	nest = nla_nest_start(phydev->skb, ETHTOOL_A_CABLE_TDR_NEST_STEP);
	311	if (!nest)
	312	return -EMSGSIZE;
	313
	314	if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_STEP_FIRST_DISTANCE,
	315	first))
	316	goto err;
	317
	318	if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_STEP_LAST_DISTANCE, last))
	319	goto err;
	320
321	if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_STEP_STEP_DISTANCE, step))
322	goto err;
323
324	nla_nest_end(phydev->skb, nest);
325	return 0;
326
327	err:
328	nla_nest_cancel(phydev->skb, nest);
329	return ret;
330	}
331	EXPORT_SYMBOL_GPL(ethnl_cable_test_step);