[RFC PATCH ethtool v2 09/23] netlink: add netlink handler for sset (-s)

[Michal Kubecek]

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@xxxxxxx>
---
 ethtool.c          |   9 +-
 netlink/extapi.h   |   1 +
 netlink/netlink.c  |   9 ++
 netlink/netlink.h  |   1 +
 netlink/parser.c   |  34 ++++++-
 netlink/parser.h   |   4 +
 netlink/settings.c | 216 +++++++++++++++++++++++++++++++++++++++++++++
 7 files changed, 269 insertions(+), 5 deletions(-)

diff --git a/ethtool.c b/ethtool.c
index bc7969f829f6..0b40b96cf9bf 100644
--- a/ethtool.c
+++ b/ethtool.c
@@ -4923,6 +4923,7 @@ static int show_usage(struct cmd_context *ctx);
  */
 #define nl_gdrv		NULL
 #define nl_gset		NULL
+#define nl_sset		NULL
 #endif
 
 static const struct option {
@@ -4933,19 +4934,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|d... ]\n"
+	  "		[ wol %d[/%d] |  p|u|m|b|a|g|s|d...[/p|u|m|b|a|g|s|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 20c9b03b2d3c..66573f0b4304 100644
--- a/netlink/extapi.h
+++ b/netlink/extapi.h
@@ -15,6 +15,7 @@ int netlink_done(struct cmd_context *ctx);
 
 int nl_gdrv(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 fc87577191f0..8b3dce970fd1 100644
--- a/netlink/netlink.c
+++ b/netlink/netlink.c
@@ -573,6 +573,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)
diff --git a/netlink/netlink.h b/netlink/netlink.h
index 32c2f9f33ba1..9c23526da627 100644
--- a/netlink/netlink.h
+++ b/netlink/netlink.h
@@ -72,6 +72,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 589004d1f9f9..d1536ecb1d52 100644
--- a/netlink/parser.c
+++ b/netlink/parser.c
@@ -69,7 +69,7 @@ static int parse_x32(const char *arg, u32 *result)
 	return __parse_u32(arg, result, 0, 0xffffffff, 16);
 }
 
-static int parse_u32(const char *arg, u32 *result)
+int parse_u32(const char *arg, u32 *result)
 {
 	if (!arg)
 		return -EINVAL;
@@ -485,6 +485,38 @@ int nl_parse_bitset(struct nl_context *nlctx, uint16_t type, const void *data)
 		return nl_parse_bitset_list(nlctx, type, data);
 }
 
+int nl_parse_mac_addr(struct nl_context *nlctx, uint16_t type, const void *data)
+{
+	const char *arg = *nlctx->argp;
+	u8 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++] = (u8) byte;
+		p = endp + (*endp ? 1 : 0);
+	}
+	if (i < ETH_ALEN)
+		goto err;
+
+	return ethnla_put(nlctx, type, ETH_ALEN, val);
+
+err:
+	fprintf(stderr, "ethtool (%s): invalid value '%s' of parameter '%s'\n",
+		nlctx->cmd, arg, nlctx->param);
+	return -EINVAL;
+}
+
 int nl_parser(struct cmd_context *ctx, const struct param_parser *params,
 	      unsigned int max_type)
 {
diff --git a/netlink/parser.h b/netlink/parser.h
index 7308da25f093..2fd9a8bb23d9 100644
--- a/netlink/parser.h
+++ b/netlink/parser.h
@@ -28,6 +28,8 @@ struct param_parser {
 	unsigned int		min_argc;
 };
 
+int parse_u32(const char *arg, u32 *result);
+
 int nl_parse_direct_u32(struct nl_context *nlctx, uint16_t type,
 			const void *data);
 int nl_parse_direct_u8(struct nl_context *nlctx, uint16_t type,
@@ -38,6 +40,8 @@ int nl_parse_lookup_u8(struct nl_context *nlctx, uint16_t type,
 int nl_parse_bitfield32(struct nl_context *nlctx, uint16_t type,
 			const void *data);
 int nl_parse_bitset(struct nl_context *nlctx, uint16_t type, const void *data);
+int nl_parse_mac_addr(struct nl_context *nlctx, uint16_t type,
+		      const void *data);
 
 int nl_parser(struct cmd_context *ctx, const struct param_parser *params,
 	      unsigned int max_type);
diff --git a/netlink/settings.c b/netlink/settings.c
index 120fc95aad19..b077a96325e5 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 */
 
@@ -299,3 +300,218 @@ 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,
+};
+
+#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',
+};
+
+static int __nl_parse_wol_modes(struct nl_context *nlctx, const char *str,
+				u32 *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;
+}
+
+static int nl_parse_wol_modes(struct nl_context *nlctx, uint16_t type,
+			      const void *data)
+{
+	const char *arg = *nlctx->argp;
+	char *mask = strchr(arg, '/');
+	u32 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 = ~(u32)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(ctx, sset_params, ETHA_SETTINGS_MAX);
+	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.18.0