File netlink-add-netlink-handler-for-reset-reset.patch of Package ethtool

From: Michal Kubecek <mkubecek@suse.cz>
Date: Tue, 4 Sep 2018 08:43:44 +0200
Subject: netlink: add netlink handler for reset (--reset)
Patch-mainline: Not yet, work in progress
References: none

Implement "ethtool --reset <dev>" using netlink interface command
ETHNL_CMD_ACT_RESET.

Signed-off-by: Michal Kubecek <mkubecek@suse.cz>
---
 ethtool.c         |   3 +-
 netlink/actions.c | 243 ++++++++++++++++++++++++++++++++++++++++++++++
 netlink/extapi.h  |   1 +
 netlink/monitor.c |   9 ++
 4 files changed, 255 insertions(+), 1 deletion(-)

diff --git a/ethtool.c b/ethtool.c
index a9c408545dab..029deb187268 100644
--- a/ethtool.c
+++ b/ethtool.c
@@ -4857,6 +4857,7 @@ static int show_usage(struct cmd_context *ctx);
 #define nl_sfec		NULL
 #define nl_nway_rst	NULL
 #define nl_phys_id	NULL
+#define nl_reset	NULL
 #endif
 
 static const struct option {
@@ -5041,7 +5042,7 @@ static const struct option {
 	{ "--get-phy-tunable", 1, do_get_phy_tunable, NULL,
 	  "Get PHY tunable",
 	  "		[ downshift ]\n"},
-	{ "--reset", 1, do_reset, NULL,
+	{ "--reset", 1, do_reset, nl_reset,
 	  "Reset components",
 	  "		[ flags %x ]\n"
 	  "		[ mgmt ]\n"
diff --git a/netlink/actions.c b/netlink/actions.c
index 160352ea8df3..9ca69e288ff8 100644
--- a/netlink/actions.c
+++ b/netlink/actions.c
@@ -6,6 +6,7 @@
 #include "../common.h"
 #include "netlink.h"
 #include "parser.h"
+#include "strset.h"
 
 /* ACT_NWAY_RST */
 
@@ -130,3 +131,245 @@ int nl_phys_id(struct cmd_context *ctx)
 
 	return ret;
 }
+
+/* ACT_RESET */
+
+struct reset_walk_data {
+	uint32_t flags_t;
+	uint32_t flags_f;
+	bool shared;
+};
+
+void reset_walk_cb(unsigned int idx, const char *name, bool val, void *__data)
+{
+	struct reset_walk_data *data = __data;
+	u32 mask = 1U << (idx + (data->shared ? ETH_RESET_SHARED_SHIFT : 0));
+
+	if (val)
+		data->flags_t |= mask;
+	else
+		data->flags_f |= mask;
+}
+
+int reset_reply_cb(const struct nlmsghdr *nlhdr, void *data)
+{
+	const struct nlattr *tb[ETHA_RESET_MAX + 1] = {};
+	DECLARE_ATTR_TB_INFO(tb);
+	struct nl_context *nlctx = data;
+	const struct stringset *labels = global_stringset(ETH_SS_RESET_FLAGS);
+	struct reset_walk_data walk_data = {};
+	int ret;
+
+	ret = mnl_attr_parse(nlhdr, GENL_HDRLEN, attr_cb, &tb_info);
+	if (ret < 0)
+		return ret;
+	nlctx->devname = get_dev_name(tb[ETHA_RESET_DEV]);
+	if (!dev_ok(nlctx))
+		return MNL_CB_OK;
+
+	if (tb[ETHA_RESET_ALL]) {
+		walk_data.flags_t = ETH_RESET_ALL;
+		goto show;
+	}
+	walk_data.shared = true;
+	if (tb[ETHA_RESET_SHARED])
+		walk_bitset(tb[ETHA_RESET_SHARED], labels, reset_walk_cb,
+			    &walk_data);
+
+	if (tb[ETHA_RESET_ALL_DEDICATED]) {
+		walk_data.flags_t |= ETH_RESET_DEDICATED;
+		goto show;
+	}
+	walk_data.shared = false;
+	if (tb[ETHA_RESET_DEDICATED])
+		walk_bitset(tb[ETHA_RESET_DEDICATED], labels, reset_walk_cb,
+			    &walk_data);
+
+show:
+	if (nlctx->is_monitor)
+		putchar('\n');
+	printf("Reset performed on %s\n", nlctx->devname);
+	printf("Components reset:     0x%x\n", walk_data.flags_t);
+	if (walk_data.flags_f)
+		printf("Components not reset: 0x%x\n", walk_data.flags_f);
+
+	return MNL_CB_OK;
+}
+
+static int fill_bitset_from_u32(struct nl_context *nlctx, uint16_t type,
+				uint32_t value)
+{
+	struct nlattr *nest = ethnla_nest_start(nlctx, type);
+
+	if (!nest)
+		return -EMSGSIZE;
+	if (ethnla_put_u32(nlctx, ETHA_BITSET_SIZE, nsb(value)) ||
+	    ethnla_put_u32(nlctx, ETHA_BITSET_VALUE, value) ||
+	    ethnla_put_u32(nlctx, ETHA_BITSET_MASK, value))
+		return -EMSGSIZE;
+	mnl_attr_nest_end(nlctx->nlhdr, nest);
+
+	return 0;
+}
+
+
+static int fill_reset(struct nl_context *nlctx, uint32_t flags)
+{
+	int ret;
+
+	if (flags == ETH_RESET_ALL)
+		return ethnla_put_flag(nlctx, ETHA_RESET_ALL, true) ?
+		       -EMSGSIZE : 0;
+
+	if ((flags & ETH_RESET_DEDICATED) == ETH_RESET_DEDICATED) {
+		if (ethnla_put_flag(nlctx, ETHA_RESET_ALL_DEDICATED, true))
+			return -EMSGSIZE;
+	} else if (flags & ETH_RESET_DEDICATED) {
+		ret = fill_bitset_from_u32(nlctx, ETHA_RESET_DEDICATED,
+					   flags & ETH_RESET_DEDICATED);
+		if (ret < 0)
+			return ret;
+	}
+
+	if (flags >> ETH_RESET_SHARED_SHIFT) {
+		ret = fill_bitset_from_u32(nlctx, ETHA_RESET_SHARED,
+					   flags >> ETH_RESET_SHARED_SHIFT);
+		if (ret < 0)
+			return ret;
+	}
+	return 0;
+}
+
+#define SHARED_SUFFIX "-shared"
+#define SHARED_SUFFIX_LEN 7
+static int parse_reset(struct nl_context *nlctx)
+{
+	bool all_dedicated = false;
+	struct nlmsghdr *nlhdr = nlctx->nlhdr;
+	struct nlattr *bitset;
+	struct nlattr *bits;
+	unsigned int i;
+	uint32_t flags;
+	int ret;
+
+	if (nlctx->argc < 1)
+		return -EINVAL;
+	ret = parse_u32(nlctx->argp[0], &flags);
+	if (ret == 0)
+		return fill_reset(nlctx, flags);
+
+	for (i = 0; i < nlctx->argc; i++) {
+		if (!strcmp(nlctx->argp[i], "all")) {
+			if (ethnla_put_flag(nlctx, ETHA_RESET_ALL, true))
+				return -EMSGSIZE;
+			else
+				return 0;
+		}
+		if (!strcmp(nlctx->argp[i], "dedicated"))
+			all_dedicated = true;
+	}
+	if (all_dedicated) {
+	       if (ethnla_put_flag(nlctx, ETHA_RESET_ALL_DEDICATED, true))
+		       return -EMSGSIZE;
+	       goto check_shared;
+	}
+
+	bitset = bits = NULL;
+	for (i = 0; i < nlctx->argc; i++) {
+		const char *arg = nlctx->argp[i];
+		int len = strlen(arg) - SHARED_SUFFIX_LEN;
+		struct nlattr *bit;
+
+		if (len >= 0 && !strcmp(arg + len, SHARED_SUFFIX))
+			continue;
+		if (!bitset) {
+			bitset = ethnla_nest_start(nlctx, ETHA_RESET_DEDICATED);
+			if (!bitset)
+				return -EMSGSIZE;
+			bits = ethnla_nest_start(nlctx, ETHA_BITSET_BITS);
+			if (!bits)
+				return -EMSGSIZE;
+		}
+
+		bit = ethnla_nest_start(nlctx, ETHA_BITS_BIT);
+		if (!bit)
+			return -EMSGSIZE;
+		if (ethnla_put_strz(nlctx, ETHA_BIT_NAME, arg))
+			return -EMSGSIZE;
+		if (ethnla_put_flag(nlctx, ETHA_BIT_VALUE, true))
+			return -EMSGSIZE;
+		mnl_attr_nest_end(nlhdr, bit);
+	}
+	if (bitset) {
+		mnl_attr_nest_end(nlhdr, bits);
+		mnl_attr_nest_end(nlhdr, bitset);
+	}
+
+check_shared:
+	bitset = bits = NULL;
+	for (i = 0; i < nlctx->argc; i++) {
+		const char *arg = nlctx->argp[i];
+		int len = strlen(arg) - SHARED_SUFFIX_LEN;
+		struct nlattr *bit;
+		char *tail;
+
+		if (!strcmp(arg, "dedicated"))
+			continue;
+		if (len < 0 || strcmp(arg + len, SHARED_SUFFIX))
+			continue;
+		if (!bitset) {
+			bitset = ethnla_nest_start(nlctx, ETHA_RESET_SHARED);
+			if (!bitset)
+				return -EMSGSIZE;
+			bits = ethnla_nest_start(nlctx, ETHA_BITSET_BITS);
+			if (!bits)
+				return -EMSGSIZE;
+		}
+
+		bit = ethnla_nest_start(nlctx, ETHA_BITS_BIT);
+		if (!bit)
+			return -EMSGSIZE;
+		tail = mnl_nlmsg_get_payload_tail(nlhdr);
+		if (ethnla_put(nlctx, ETHA_BIT_NAME, len + 1, arg))
+			return -EMSGSIZE;
+		tail[MNL_ATTR_HDRLEN + len] = '\0';
+		if (ethnla_put_flag(nlctx, ETHA_BIT_VALUE, true))
+			return -EMSGSIZE;
+		mnl_attr_nest_end(nlhdr, bit);
+	}
+	if (bitset) {
+		mnl_attr_nest_end(nlhdr, bits);
+		mnl_attr_nest_end(nlhdr, bitset);
+	}
+
+	return 0;
+}
+
+int nl_reset(struct cmd_context *ctx)
+{
+	struct nl_context *nlctx = ctx->nlctx;
+	int ret;
+
+	nlctx->cmd = "-p";
+	nlctx->argp = ctx->argp;
+	nlctx->argc = ctx->argc;
+
+	ret = msg_init(nlctx, ETHNL_CMD_ACT_RESET, NLM_F_REQUEST | NLM_F_ACK);
+	if (ret < 0)
+		return -EFAULT;
+	if (ethnla_put_dev(nlctx, ETHA_RESET_DEV, ctx->devname) ||
+	    ethnla_put_flag(nlctx, ETHA_RESET_COMPACT, true))
+		return -EMSGSIZE;
+	ret = parse_reset(nlctx);
+	if (ret < 0)
+		return ret;
+
+	ret = ethnl_sendmsg(nlctx);
+	if (ret < 0)
+		return -EFAULT;
+	ret = ethnl_process_reply(nlctx, reset_reply_cb);
+	if (ret < 0)
+		fprintf(stderr, "Cannot identify NIC\n");
+
+	return ret;
+}
diff --git a/netlink/extapi.h b/netlink/extapi.h
index 87ac57955bab..af2a5042f637 100644
--- a/netlink/extapi.h
+++ b/netlink/extapi.h
@@ -34,6 +34,7 @@ int nl_seee(struct cmd_context *ctx);
 int nl_sfec(struct cmd_context *ctx);
 int nl_nway_rst(struct cmd_context *ctx);
 int nl_phys_id(struct cmd_context *ctx);
+int nl_reset(struct cmd_context *ctx);
 int nl_monitor(struct cmd_context *ctx);
 
 void monitor_usage();
diff --git a/netlink/monitor.c b/netlink/monitor.c
index 281aa1f5da16..522ad654ad72 100644
--- a/netlink/monitor.c
+++ b/netlink/monitor.c
@@ -71,6 +71,7 @@ int settings_reply_cb(const struct nlmsghdr *nlhdr, void *data);
 int params_reply_cb(const struct nlmsghdr *nlhdr, void *data);
 int nwayrst_reply_cb(const struct nlmsghdr *nlhdr, void *data);
 int physid_reply_cb(const struct nlmsghdr *nlhdr, void *data);
+int reset_reply_cb(const struct nlmsghdr *nlhdr, void *data);
 
 static struct {
 	uint8_t		cmd;
@@ -100,6 +101,10 @@ static struct {
 		.cmd	= ETHNL_CMD_ACT_PHYS_ID,
 		.cb	= physid_reply_cb,
 	},
+	{
+		.cmd	= ETHNL_CMD_ACT_RESET,
+		.cb	= reset_reply_cb,
+	},
 };
 
 static int monitor_any_cb(const struct nlmsghdr *nlhdr, void *data)
@@ -186,6 +191,10 @@ static struct monitor_option monitor_opts[] = {
 		.pattern	= "-p|--identify",
 		.cmd		= ETHNL_CMD_ACT_PHYS_ID,
 	},
+	{
+		.pattern	= "--reset",
+		.cmd		= ETHNL_CMD_ACT_RESET,
+	},
 };
 
 static bool pattern_match(const char *s, const char *pattern)
-- 
2.19.0