File netlink-add-netlink-handler-for-sset-s.patch of Package ethtool

From: Michal Kubecek <mkubecek@suse.cz>
Date: Sun, 26 Nov 2017 22:45:28 +0100
Subject: netlink: add netlink handler for sset (-s)
Patch-mainline: Not yet, work in progress
References: none

Implement "ethtool -s <dev>" subcommand using netlink interface command
ETHNL_CMD_SET_SETTINGS.

Add specific parsers for wol modes and MAC address (used for wake-on-lan
password here).

Signed-off-by: Michal Kubecek <mkubecek@suse.cz>
---
 ethtool.c          |   9 +-
 netlink/extapi.h   |   1 +
 netlink/netlink.c  |   9 ++
 netlink/netlink.h  |   1 +
 netlink/parser.c   |  36 ++++++++
 netlink/parser.h   |   2 +
 netlink/settings.c | 219 +++++++++++++++++++++++++++++++++++++++++++++
 7 files changed, 273 insertions(+), 4 deletions(-)

diff --git a/ethtool.c b/ethtool.c
index 0d4e0abe8179..1a50958ea0d6 100644
--- a/ethtool.c
+++ b/ethtool.c
@@ -4885,6 +4885,7 @@ static int show_usage(struct cmd_context *ctx);
 #define nl_permaddr	NULL
 #define nl_tsinfo	NULL
 #define nl_gset		NULL
+#define nl_sset		NULL
 #endif
 
 static const struct option {
@@ -4895,19 +4896,19 @@ static const struct option {
 	char *help;
 	char *opthelp;
 } args[] = {
-	{ "-s|--change", 1, do_sset, NULL,
+	{ "-s|--change", 1, do_sset, nl_sset,
 	  "Change generic options",
 	  "		[ speed %d ]\n"
 	  "		[ duplex half|full ]\n"
 	  "		[ port tp|aui|bnc|mii|fibre ]\n"
 	  "		[ mdix auto|on|off ]\n"
 	  "		[ autoneg on|off ]\n"
-	  "		[ advertise %x ]\n"
+	  "		[ advertise %x[/%x] | mode on|off ... [--] ]\n"
 	  "		[ phyad %d ]\n"
 	  "		[ xcvr internal|external ]\n"
-	  "		[ wol p|u|m|b|a|g|s|f|d... ]\n"
+	  "		[ wol %d[/%d] |  p|u|m|b|a|g|s|f|d...[/p|u|m|b|a|g|s|f|d...] ]\n"
 	  "		[ sopass %x:%x:%x:%x:%x:%x ]\n"
-	  "		[ msglvl %d | msglvl type on|off ... ]\n" },
+	  "		[ msglvl %d[/%d] | type on|off ... [--] ]\n" },
 	{ "-a|--show-pause", 1, do_gpause, NULL,
 	  "Show pause options" },
 	{ "-A|--pause", 1, do_spause, NULL,
diff --git a/netlink/extapi.h b/netlink/extapi.h
index 938e71700f97..e3b8c6949f36 100644
--- a/netlink/extapi.h
+++ b/netlink/extapi.h
@@ -17,6 +17,7 @@ int nl_gdrv(struct cmd_context *ctx);
 int nl_permaddr(struct cmd_context *ctx);
 int nl_tsinfo(struct cmd_context *ctx);
 int nl_gset(struct cmd_context *ctx);
+int nl_sset(struct cmd_context *ctx);
 int nl_monitor(struct cmd_context *ctx);
 
 void monitor_usage();
diff --git a/netlink/netlink.c b/netlink/netlink.c
index db5c26be52a9..4c089a0af6eb 100644
--- a/netlink/netlink.c
+++ b/netlink/netlink.c
@@ -723,6 +723,15 @@ static int get_ethnl_family(struct nl_context *nlctx)
 	return (nlctx->ethnl_fam ? 0 : -EADDRNOTAVAIL);
 }
 
+int nomsg_reply_cb(const struct nlmsghdr *nlhdr, void *data)
+{
+	const struct genlmsghdr *ghdr = (const struct genlmsghdr *)(nlhdr + 1);
+
+	fprintf(stderr, "received unexpected message: len=%u type=%u cmd=%u\n",
+	       nlhdr->nlmsg_len, nlhdr->nlmsg_type, ghdr->cmd);
+	return MNL_CB_OK;
+}
+
 /* initialization */
 
 static int nlctx_init(struct nl_context *nlctx, unsigned long debug)
diff --git a/netlink/netlink.h b/netlink/netlink.h
index 052a91355cf8..7d7aad8ef8e3 100644
--- a/netlink/netlink.h
+++ b/netlink/netlink.h
@@ -78,6 +78,7 @@ int attr_cb(const struct nlattr *attr, void *data);
 int ethnl_prep_get_request(struct cmd_context *ctx, unsigned int nlcmd,
 			   uint16_t dev_attrtype);
 int ethnl_send_get_request(struct nl_context *nlctx, mnl_cb_t cb);
+int nomsg_reply_cb(const struct nlmsghdr *nlhdr, void *data);
 int __init_aux_nlctx(struct nl_context *nlctx);
 
 /* put data wrappers */
diff --git a/netlink/parser.c b/netlink/parser.c
index 07c2bc0d9cb2..acbb5c355959 100644
--- a/netlink/parser.c
+++ b/netlink/parser.c
@@ -662,6 +662,42 @@ err:
 	return -EINVAL;
 }
 
+/* parser handler for (6-byte) MAC address in the usual format */
+int nl_parse_mac_addr(struct nl_context *nlctx, uint16_t type, const void *data,
+		      void *dest)
+{
+	const char *arg = *nlctx->argp;
+	uint8_t val[ETH_ALEN];
+	unsigned int i;
+	const char *p;
+
+	nlctx->argp++;
+	nlctx->argc--;
+
+	p = arg;
+	i = 0;
+	while (i < ETH_ALEN && *p) {
+		char *endp;
+		unsigned long byte = strtoul(p, &endp, 16);
+
+		if ((endp - p > 2) || (*endp && *endp != ':'))
+			goto err;
+		val[i++] = (uint8_t) byte;
+		p = endp + (*endp ? 1 : 0);
+	}
+	if (i < ETH_ALEN)
+		goto err;
+
+	if (dest)
+		memcpy(dest, val, ETH_ALEN);
+	return type ? ethnla_put(nlctx, type, ETH_ALEN, val) : 0;
+
+err:
+	fprintf(stderr, "ethtool (%s): invalid value '%s' of parameter '%s'\n",
+		nlctx->cmd, arg, nlctx->param);
+	return -EINVAL;
+}
+
 /* Main entry point of parser implementation.
  * @nlctx: netlink context
  * @params: array of struct param_parser describing expected arguments and
diff --git a/netlink/parser.h b/netlink/parser.h
index fa2c103dc8e7..c7daa8b81d03 100644
--- a/netlink/parser.h
+++ b/netlink/parser.h
@@ -66,6 +66,8 @@ int nl_parse_bitlist(struct nl_context *nlctx, uint16_t type, const void *data,
 		     void *dest);
 int nl_parse_byte_str(struct nl_context *nlctx, uint16_t type,
 		      const void *data, void *dest);
+int nl_parse_mac_addr(struct nl_context *nlctx, uint16_t type,
+		      const void *data, void *dest);
 
 int nl_parser(struct nl_context *nlctx, const struct param_parser *params,
 	      void *dest);
diff --git a/netlink/settings.c b/netlink/settings.c
index b29d699a36d1..04a49ff92a81 100644
--- a/netlink/settings.c
+++ b/netlink/settings.c
@@ -5,6 +5,7 @@
 #include "../internal.h"
 #include "../common.h"
 #include "netlink.h"
+#include "parser.h"
 
 /* GET_SETTINGS */
 
@@ -300,3 +301,221 @@ int nl_gset(struct cmd_context *ctx)
 }
 
 }
+
+/* SET_SETTINGS */
+
+enum {
+	WAKE_PHY_BIT		= 0,
+	WAKE_UCAST_BIT		= 1,
+	WAKE_MCAST_BIT		= 2,
+	WAKE_BCAST_BIT		= 3,
+	WAKE_ARP_BIT		= 4,
+	WAKE_MAGIC_BIT		= 5,
+	WAKE_MAGICSECURE_BIT	= 6,
+	WAKE_FILTER_BIT		= 7,
+};
+
+#define WAKE_ALL (WAKE_PHY | WAKE_UCAST | WAKE_MCAST | WAKE_BCAST | WAKE_ARP | \
+		  WAKE_MAGIC | WAKE_MAGICSECURE)
+
+char wol_modes[32] = {
+	[WAKE_PHY_BIT]		= 'p',
+	[WAKE_UCAST_BIT]	= 'u',
+	[WAKE_MCAST_BIT]	= 'm',
+	[WAKE_BCAST_BIT]	= 'b',
+	[WAKE_ARP_BIT]		= 'a',
+	[WAKE_MAGIC_BIT]	= 'g',
+	[WAKE_MAGICSECURE_BIT]	= 's',
+	[WAKE_FILTER_BIT]	= 'f',
+};
+
+static int __nl_parse_wol_modes(struct nl_context *nlctx, const char *str,
+				uint32_t *result)
+{
+	unsigned int i;
+	const char *p;
+	int ret;
+
+	ret = parse_u32(str, result);
+	if (ret == 0)
+		return 0;
+
+	*result = 0;
+	if (str[0] == 'd' && !str[1])
+		return 0;
+
+	p = str;
+	while (*p) {
+		for (i = 0; i < 32; i++) {
+			if (wol_modes[i] == *p) {
+				*result |= (1U << i);
+				break;
+			}
+		}
+		if (i == 32) {
+			fprintf(stderr,
+				"ethtool (%s): invalid wol modes '%s'\n",
+				nlctx->cmd, str);
+			return -EINVAL;
+		}
+		p++;
+	}
+
+	return 0;
+}
+
+/* dest is ignored */
+static int nl_parse_wol_modes(struct nl_context *nlctx, uint16_t type,
+			      const void *data, void *dest)
+{
+	const char *arg = *nlctx->argp;
+	char *mask = strchr(arg, '/');
+	uint32_t value, selector;
+	int ret;
+
+	nlctx->argp++;
+	nlctx->argc--;
+	if (mask) {
+		*mask = '\0';
+		mask++;
+	}
+
+	ret = __nl_parse_wol_modes(nlctx, arg, &value);
+	if (ret < 0)
+		return ret;
+
+	if (!mask) {
+		selector = ~(uint32_t)0;
+	} else {
+		ret = __nl_parse_wol_modes(nlctx, mask, &selector);
+		if (ret < 0)
+			return ret;
+	}
+
+	return ethnla_put_bitfield32(nlctx, type, value, selector);
+}
+
+static const struct lookup_entry_u32 duplex_values[] = {
+	{ .arg = "half",	.val = DUPLEX_HALF },
+	{ .arg = "full",	.val = DUPLEX_FULL },
+	{}
+};
+
+static const struct lookup_entry_u8 port_values[] = {
+	{ .arg = "tp",		.val = PORT_TP },
+	{ .arg = "aui",		.val = PORT_AUI },
+	{ .arg = "bnc",		.val = PORT_BNC },
+	{ .arg = "mii",		.val = PORT_MII },
+	{ .arg = "fibre",	.val = PORT_FIBRE },
+	{}
+};
+
+static const struct lookup_entry_u8 mdix_values[] = {
+	{ .arg = "auto",	.val = ETH_TP_MDI_AUTO },
+	{ .arg = "on",		.val = ETH_TP_MDI_X },
+	{ .arg = "off",		.val = ETH_TP_MDI },
+	{}
+};
+
+static const struct lookup_entry_u8 autoneg_values[] = {
+	{ .arg = "off",		.val = AUTONEG_DISABLE },
+	{ .arg = "on",		.val = AUTONEG_ENABLE },
+	{}
+};
+
+static const struct param_parser sset_params[] = {
+	{
+		.arg		= "speed",
+		.type		= ETHA_SETTINGS_SPEED,
+		.handler	= nl_parse_direct_u32,
+		.min_argc	= 1,
+	},
+	{
+		.arg		= "duplex",
+		.type		= ETHA_SETTINGS_DUPLEX,
+		.handler	= nl_parse_lookup_u8,
+		.handler_data	= duplex_values,
+		.min_argc	= 1,
+	},
+	{
+		.arg		= "port",
+		.type		= ETHA_SETTINGS_PORT,
+		.handler	= nl_parse_lookup_u8,
+		.handler_data	= port_values,
+		.min_argc	= 1,
+	},
+	{
+		.arg		= "mdix",
+		.type		= ETHA_SETTINGS_TP_MDIX_CTRL,
+		.handler	= nl_parse_lookup_u8,
+		.handler_data	= mdix_values,
+		.min_argc	= 1,
+	},
+	{
+		.arg		= "autoneg",
+		.type		= ETHA_SETTINGS_AUTONEG,
+		.handler	= nl_parse_lookup_u8,
+		.handler_data	= autoneg_values,
+		.min_argc	= 1,
+	},
+	{
+		.arg		= "advertise",
+		.type		= ETHA_SETTINGS_LINK_MODES,
+		.handler	= nl_parse_bitset,
+		.min_argc	= 1,
+	},
+	{
+		.arg		= "phyad",
+		.type		= ETHA_SETTINGS_PHYADDR,
+		.handler	= nl_parse_direct_u8,
+		.min_argc	= 1,
+	},
+	{
+		.arg		= "wol",
+		.type		= ETHA_SETTINGS_WOL_MODES,
+		.handler	= nl_parse_wol_modes,
+		.min_argc	= 1,
+	},
+	{
+		.arg		= "sopass",
+		.type		= ETHA_SETTINGS_SOPASS,
+		.handler	= nl_parse_mac_addr,
+		.min_argc	= 1,
+	},
+	{
+		.arg		= "msglvl",
+		.type		= ETHA_SETTINGS_MSGLVL,
+		.handler	= nl_parse_bitfield32,
+		.handler_data	= flags_msglvl,
+		.min_argc	= 1,
+	},
+	{}
+};
+
+int nl_sset(struct cmd_context *ctx)
+{
+	struct nl_context *nlctx = ctx->nlctx;
+	int ret;
+
+	nlctx->cmd = "-s";
+	nlctx->argp = ctx->argp;
+	nlctx->argc = ctx->argc;
+	ret = msg_init(nlctx, ETHNL_CMD_SET_SETTINGS,
+		       NLM_F_REQUEST | NLM_F_ACK);
+	if (ret < 0)
+		return 2;
+	if (ethnla_put_dev(nlctx, ETHA_SETTINGS_DEV, ctx->devname))
+		return -EMSGSIZE;
+
+	ret = nl_parser(nlctx, sset_params, NULL);
+	if (ret < 0)
+		return 2;
+
+	ret = ethnl_sendmsg(nlctx);
+	if (ret < 0)
+		return 75;
+	ret = ethnl_process_reply(nlctx, nomsg_reply_cb);
+	if (ret == 0)
+		return 0;
+	return nlctx->exit_code ?: 75;
+}
-- 
2.19.0