[PATCH 6.1 02/15] genetlink: add TEST_GENL family for netlink testing

[Yana Bashlykova <yana2bsh@gmail.com>]

Implement basic Generic Netlink family with:
- 4 commands (ECHO, SET/GET_VALUE, NO_ATTRS)
- 1 small command (genl_small_ops)
- Multicast group support
- Attribute validation
- Mutex-protected operations
- Error handling with netlink_ext_ack

Signed-off-by: Yana Bashlykova <yana2bsh@gmail.com>
---
 .../net-pf-16-proto-16-family-PARALLEL_GENL.c | 438 +++++++++++++++++-
 1 file changed, 431 insertions(+), 7 deletions(-)

diff --git a/drivers/net/genetlink/net-pf-16-proto-16-family-PARALLEL_GENL.c b/drivers/net/genetlink/net-pf-16-proto-16-family-PARALLEL_GENL.c
index c50c0daae392..69bcad98babc 100644
--- a/drivers/net/genetlink/net-pf-16-proto-16-family-PARALLEL_GENL.c
+++ b/drivers/net/genetlink/net-pf-16-proto-16-family-PARALLEL_GENL.c
@@ -71,17 +71,16 @@ static ssize_t show_genl_test_message(struct kobject *kobj,
 
 static ssize_t store_genl_test_message(struct kobject *kobj,
 				       struct kobj_attribute *attr,
-				       const char *buf, size_t count)
+					   const char *buf, size_t count)
 {
-	size_t len = min(count, sizeof(sysfs_data.genl_test_message) - 1);
+		size_t len = min(count, sizeof(sysfs_data.genl_test_message) - 1);
 
-	strncpy(sysfs_data.genl_test_message, buf, len);
-	sysfs_data.genl_test_message[len] = '\0';
-	return count;
+		strncpy(sysfs_data.genl_test_message, buf, len);
+		sysfs_data.genl_test_message[len] = '\0';
+		return count;
 }
 
-static ssize_t show_genl_test_value(struct kobject *kobj,
-				    struct kobj_attribute *attr, char *buf)
+static ssize_t show_genl_test_value(struct kobject *kobj, struct kobj_attribute *attr, char *buf)
 {
 	return sprintf(buf, "%d", sysfs_data.genl_test_value);
 }
@@ -146,6 +145,424 @@ static struct kobj_attribute my_attr_str_parallel_genl =
 static struct kobj_attribute my_attr_str_third_genl =
 	__ATTR(message, 0664, show_third_genl_message, store_third_genl_message);
 
+static DEFINE_MUTEX(genl_mutex);
+
+#define MY_GENL_FAMILY_NAME "TEST_GENL"
+#define MY_GENL_VERSION 1
+
+#define PATH_GENL_TEST_NUM "/sys/kernel/genl_test/value"
+#define PATH_GENL_TEST_MES "/sys/kernel/genl_test/message"
+#define PATH_GENL_TEST_DEV "/sys/kernel/genl_test/some_info"
+
+// TEST_GENL
+enum {
+	MY_GENL_ATTR_UNSPEC,
+	MY_GENL_ATTR_DATA,
+	MY_GENL_ATTR_VALUE,
+	MY_GENL_ATTR_PATH,
+	MY_GENL_ATTR_NESTED,
+	__MY_GENL_ATTR_MAX,
+};
+
+#define MY_GENL_ATTR_MAX (__MY_GENL_ATTR_MAX - 1)
+
+enum {
+	MY_GENL_CMD_UNSPEC,
+	MY_GENL_CMD_ECHO,
+	MY_GENL_CMD_SET_VALUE,
+	MY_GENL_CMD_GET_VALUE,
+	MY_GENL_CMD_EVENT,
+	MY_GENL_CMD_NO_ATTRS,
+	__MY_GENL_CMD_MAX,
+};
+
+#define MY_GENL_CMD_MAX (__MY_GENL_CMD_MAX - 1)
+
+enum {
+	MY_GENL_SMALL_CMD_GET,
+	MY_GENL_SMALL_CMD_ERROR,
+	__MY_GENL_SMALL_CMD_MAX,
+};
+
+#define MY_GENL_SMALL_CMD_MAX (__MY_GENL_SMALL_CMD_MAX - 1)
+
+static const struct nla_policy my_genl_policy[MY_GENL_ATTR_MAX + 1] = {
+	[MY_GENL_ATTR_UNSPEC] = { .type = NLA_UNSPEC },
+	[MY_GENL_ATTR_DATA] = { .type = NLA_STRING },
+	[MY_GENL_ATTR_VALUE] = { .type = NLA_U32,
+				 .validation_type = NLA_VALIDATE_RANGE,
+				 .min = 0,
+				 .max = 100 },
+	[MY_GENL_ATTR_PATH] = { .type = NLA_STRING },
+	[MY_GENL_ATTR_NESTED] = { .type = NLA_NESTED },
+};
+
+/* netlink families */
+static struct genl_family my_genl_family;
+
+enum my_multicast_groups {
+	MY_MCGRP_GENL,
+};
+
+static const struct genl_multicast_group genl_mcgrps[] = {
+	[MY_MCGRP_GENL] = { .name = "MY_MCGRP_GENL", },
+};
+
+static int my_genl_pre_doit(const struct genl_ops *ops, struct sk_buff *skb,
+			    struct genl_info *info)
+{
+	mutex_lock(&genl_mutex);
+	return 0;
+}
+
+static void my_genl_post_doit(const struct genl_ops *ops, struct sk_buff *skb,
+			      struct genl_info *info)
+{
+	mutex_unlock(&genl_mutex);
+}
+
+static void my_genl_mcast_msg(struct sk_buff *mcast_skb, struct genl_info *info)
+{
+	if (info) {
+		genl_notify(&my_genl_family, mcast_skb, info, MY_MCGRP_GENL,
+			    GFP_KERNEL);
+	} else {
+		genlmsg_multicast(&my_genl_family, mcast_skb, 0, MY_MCGRP_GENL,
+				  GFP_KERNEL);
+	}
+}
+
+// Functions for Generic Netlink TEST_GENL family
+static int my_genl_echo(struct sk_buff *skb, struct genl_info *info)
+{
+	struct sk_buff *msg;
+	void *data;
+	int ret;
+	char *str;
+
+	if (info->nlhdr->nlmsg_flags & NLM_F_ECHO) {
+		msg = genlmsg_new(GENLMSG_DEFAULT_SIZE, GFP_KERNEL);
+		if (!msg)
+			return -ENOMEM;
+
+		data = genlmsg_put_reply(msg, info, &my_genl_family, 0,
+					 MY_GENL_CMD_ECHO);
+		if (!data)
+			goto error;
+
+		str = "Hello to mcast groups!";
+
+		strcpy(sysfs_data.genl_test_message, str);
+
+		ret = nla_put_string(msg, MY_GENL_ATTR_DATA, str);
+		if (ret < 0)
+			goto error;
+
+		genlmsg_end(msg, data);
+
+		my_genl_mcast_msg(msg, info);
+	}
+
+	return 0;
+
+error:
+	nlmsg_free(msg);
+	return -EMSGSIZE;
+}
+
+static int my_genl_set_value(struct sk_buff *skb, struct genl_info *info)
+{
+	struct sk_buff *msg;
+	void *msg_head;
+	struct nlattr *na_path;
+	struct nlattr *na_value;
+	char *sysfs_path;
+	u32 new_value;
+	int err;
+	int code;
+	struct netlink_ext_ack *extack;
+	struct nlattr *attr;
+	struct nlmsghdr *nlh;
+	char cookie[NETLINK_MAX_COOKIE_LEN];
+
+	if (!info->attrs[MY_GENL_ATTR_VALUE]) {
+		pr_info("%s: Missing MY_GENL_ATTR_VALUE\n", __func__);
+		return -EINVAL;
+	}
+
+	na_value = info->attrs[MY_GENL_ATTR_VALUE];
+	new_value = nla_get_u32(na_value);
+
+	if (new_value != 0 && new_value != 1) {
+		pr_err("%s: New value is incorrect\n", __func__);
+		goto error;
+	}
+
+	na_path = info->attrs[MY_GENL_ATTR_PATH];
+	if (!na_path) {
+		pr_info("%s: Missing MY_GENL_ATTR_PATH\n", __func__);
+		return -EINVAL;
+	}
+	sysfs_path = nla_data(na_path);
+
+	sysfs_data.genl_test_value = new_value;
+
+	msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
+	if (!msg)
+		return -ENOMEM;
+
+	msg_head = genlmsg_put(msg, info->snd_portid, info->snd_seq,
+			       &my_genl_family, 0, MY_GENL_CMD_SET_VALUE);
+	if (!msg_head) {
+		nlmsg_free(msg);
+		return -ENOMEM;
+	}
+
+	if (nla_put_u32(msg, MY_GENL_ATTR_VALUE, new_value)) {
+		genlmsg_cancel(msg, msg_head);
+		nlmsg_free(msg);
+		return -EMSGSIZE;
+	}
+
+	genlmsg_end(msg, msg_head);
+
+	err = netlink_unicast(skb->sk, msg, info->snd_portid, 0);
+	if (err < 0) {
+		pr_err("%s: Error in netlink_unicast, err=%d\n", __func__, err);
+		nlmsg_free(msg);
+		return err;
+	}
+
+	return 0;
+
+error:
+	// sending error ACK
+	code = -EINVAL;
+
+	extack = kmalloc(sizeof(*extack), GFP_KERNEL);
+	if (!extack)
+		return -ENOMEM;
+
+	strcpy(cookie, "000001");
+	extack->_msg = "Incorrect value from userspace";
+	extack->bad_attr = na_value;
+	extack->policy = my_genl_policy;
+	extack->cookie_len = strlen(cookie);
+	extack->miss_type = MY_GENL_ATTR_VALUE;
+	extack->miss_nest = attr;
+
+	nlh = nlmsg_hdr(skb);
+	netlink_ack(skb, nlh, code, extack);
+	pr_info("%s: Message with TLV was sent\n", __func__);
+	return -EINVAL;
+}
+
+static int my_genl_get_value(struct sk_buff *skb, struct genl_info *info)
+{
+	struct sk_buff *msg;
+	void *msg_head;
+	struct nlattr *na_path;
+	char *sysfs_path;
+	u32 value;
+	int err;
+	int code;
+
+	msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
+	if (!msg)
+		return -ENOMEM;
+
+	msg_head = genlmsg_put(msg, info->snd_portid, info->snd_seq,
+			       &my_genl_family, 0, MY_GENL_CMD_GET_VALUE);
+	if (!msg_head) {
+		nlmsg_free(msg);
+		return -ENOMEM;
+	}
+
+	if (!info->attrs[MY_GENL_ATTR_PATH]) {
+		nlmsg_free(msg);
+		return -EINVAL;
+	}
+	genl_unlock();
+	na_path = info->attrs[MY_GENL_ATTR_PATH];
+	sysfs_path = nla_data(na_path);
+	genl_lock();
+
+	if (strcmp(sysfs_path, PATH_GENL_TEST_NUM) != 0) {
+		pr_err("%s: Incorrect path: %s\n", __func__, sysfs_path);
+		goto error;
+	}
+
+	value = sysfs_data.genl_test_value;
+
+	if (nla_put_u32(msg, MY_GENL_ATTR_VALUE, value)) {
+		genlmsg_cancel(msg, msg_head);
+		nlmsg_free(msg);
+		return -EMSGSIZE;
+	}
+
+	genlmsg_end(msg, msg_head);
+
+	if (info) {
+		err = genlmsg_reply(msg, info);
+		if (err != 0) {
+			pr_err("%s: Error in genlmsg_reply, err=%d\n", __func__, err);
+			nlmsg_free(msg);
+			return err;
+		}
+	}
+
+	return 0;
+
+error:
+	code = -EINVAL;
+	netlink_set_err(skb->sk, 0, MY_MCGRP_GENL, code);
+	return -EINVAL;
+}
+
+static int my_genl_no_attrs(struct sk_buff *skb, struct genl_info *info)
+{
+	struct sk_buff *msg;
+	void *msg_head;
+	int ret;
+	char *str;
+
+	msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+	if (!msg)
+		return -ENOMEM;
+
+	msg_head = genlmsg_put_reply(msg, info, &my_genl_family, 0,
+				     info->genlhdr->cmd);
+	if (!msg_head)
+		goto error;
+
+	str = "Reply from GENL_TEST family function with no attrs";
+
+	strcpy(sysfs_data.genl_test_message, str);
+
+	if (nla_put_string(msg, MY_GENL_ATTR_DATA, str)) {
+		pr_err("%s: Error with putting value to MY_GENL_ATTR_DATA\n", __func__);
+		goto error;
+	}
+
+	genlmsg_end(msg, msg_head);
+	return genlmsg_reply(msg, info);
+
+error:
+	ret = -EMSGSIZE;
+	nlmsg_free(msg);
+	return ret;
+}
+
+// Generic Netlink operations for TEST_GENL family
+static const struct genl_ops my_genl_ops[] = {
+	{
+		.cmd = MY_GENL_CMD_ECHO,
+		.flags = 0,
+		.policy = my_genl_policy,
+		.doit = my_genl_echo,
+		.dumpit = NULL,
+	},
+	{
+		.cmd = MY_GENL_CMD_SET_VALUE,
+		.policy = my_genl_policy,
+		.doit = my_genl_set_value,
+		.flags = GENL_ADMIN_PERM,
+	},
+	{
+		.cmd = MY_GENL_CMD_GET_VALUE,
+		.flags = 0,
+		.policy = my_genl_policy,
+		.doit = my_genl_get_value,
+		.dumpit = NULL,
+	},
+	{
+		.cmd = MY_GENL_CMD_NO_ATTRS,
+		.flags = 0,
+		.policy = NULL,
+		.doit = my_genl_no_attrs,
+		.dumpit = NULL,
+	},
+};
+
+static int my_genl_small_cmd_get(struct sk_buff *skb, struct genl_info *info)
+{
+	struct sk_buff *msg;
+	void *reply;
+	int ret;
+	char *str;
+
+	msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+	if (!msg)
+		return -ENOMEM;
+
+	reply = genlmsg_put_reply(msg, info, &my_genl_family, 0,
+				  info->genlhdr->cmd);
+	if (!reply)
+		goto error;
+
+	str = "IT'S ME from kernel";
+
+	strcpy(sysfs_data.genl_test_message, str);
+
+	if (nla_put_string(msg, MY_GENL_ATTR_DATA, str)) {
+		nlmsg_free(msg);
+		pr_err("%s: Error with putting value to MY_GENL_ATTR_DATA\n", __func__);
+		return -EMSGSIZE;
+	}
+
+	genlmsg_end(msg, reply);
+	return genlmsg_reply(msg, info);
+
+error:
+	ret = -EMSGSIZE;
+	nlmsg_free(msg);
+	return ret;
+}
+
+static const struct genl_small_ops my_genl_small_ops[] = {
+	{
+		.cmd = MY_GENL_SMALL_CMD_GET,
+		.doit = my_genl_small_cmd_get,
+	},
+};
+
+// genl_family struct for TEST_GENL family
+static struct genl_family my_genl_family = {
+	.hdrsize = 0,
+	.name = MY_GENL_FAMILY_NAME,
+	.version = MY_GENL_VERSION,
+	.maxattr = MY_GENL_ATTR_MAX,
+	.netnsok = true,
+	.pre_doit = my_genl_pre_doit,
+	.post_doit = my_genl_post_doit,
+	.ops = my_genl_ops,
+	.n_ops = ARRAY_SIZE(my_genl_ops),
+	.small_ops = my_genl_small_ops,
+	.n_small_ops = ARRAY_SIZE(my_genl_small_ops),
+	.policy = my_genl_policy,
+	.mcgrps = genl_mcgrps,
+	.n_mcgrps = ARRAY_SIZE(genl_mcgrps),
+};
+
+static int __init init_netlink(void)
+{
+	int rc;
+
+	pr_info("%s: My module. Initializing Netlink\n", __func__);
+
+	rc = genl_register_family(&my_genl_family);
+	if (rc) {
+		pr_err("%s: Failed to register Generic Netlink family\n", __func__);
+		goto failure_1;
+	}
+
+	return 0;
+
+failure_1:
+	pr_debug("%s: My module. Error occurred in %s\n", __func__, __func__);
+	return rc;
+}
+
 static int __init init_sysfs_third_genl(void)
 {
 	int ret;
@@ -253,6 +670,11 @@ static int __init module_netlink_init(void)
 	if (ret)
 		goto err_sysfs;
 
+	ret = init_netlink();
+	if (ret)
+		goto err_sysfs;
+	pr_info("%s: New families are registered\n", __func__);
+
 	return 0;
 
 err_sysfs:
@@ -271,6 +693,8 @@ static int __init module_netlink_init(void)
 
 static void __exit module_netlink_exit(void)
 {
+	genl_unregister_family(&my_genl_family);
+
 	sysfs_remove_file(kobj_genl_test, &my_attr_u32_genl_test.attr);
 	sysfs_remove_file(kobj_genl_test, &my_attr_str_genl_test.attr);
 	device_remove_file(dev_genl_test, &dev_attr_info_genl_test);
-- 
2.34.1