[linux-2.6-block.git] / net / ncsi / ncsi-netlink.c

/*
 * Copyright Samuel Mendoza-Jonas, IBM Corporation 2018.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 */

#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/if_arp.h>
#include <linux/rtnetlink.h>
#include <linux/etherdevice.h>
#include <net/genetlink.h>
#include <net/ncsi.h>
#include <linux/skbuff.h>
#include <net/sock.h>
#include <uapi/linux/ncsi.h>

#include "internal.h"
#include "ncsi-pkt.h"
#include "ncsi-netlink.h"

static struct genl_family ncsi_genl_family;

static const struct nla_policy ncsi_genl_policy[NCSI_ATTR_MAX + 1] = {
	[NCSI_ATTR_IFINDEX] =		{ .type = NLA_U32 },
	[NCSI_ATTR_PACKAGE_LIST] =	{ .type = NLA_NESTED },
	[NCSI_ATTR_PACKAGE_ID] =	{ .type = NLA_U32 },
	[NCSI_ATTR_CHANNEL_ID] =	{ .type = NLA_U32 },
	[NCSI_ATTR_DATA] =		{ .type = NLA_BINARY, .len = 2048 },
};

static struct ncsi_dev_priv *ndp_from_ifindex(struct net *net, u32 ifindex)
{
	struct ncsi_dev_priv *ndp;
	struct net_device *dev;
	struct ncsi_dev *nd;
	struct ncsi_dev;

	if (!net)
		return NULL;

	dev = dev_get_by_index(net, ifindex);
	if (!dev) {
		pr_err("NCSI netlink: No device for ifindex %u\n", ifindex);
		return NULL;
	}

	nd = ncsi_find_dev(dev);
	ndp = nd ? TO_NCSI_DEV_PRIV(nd) : NULL;

	dev_put(dev);
	return ndp;
}

static int ncsi_write_channel_info(struct sk_buff *skb,
				   struct ncsi_dev_priv *ndp,
				   struct ncsi_channel *nc)
{
	struct ncsi_channel_vlan_filter *ncf;
	struct ncsi_channel_mode *m;
	struct nlattr *vid_nest;
	int i;

	nla_put_u32(skb, NCSI_CHANNEL_ATTR_ID, nc->id);
	m = &nc->modes[NCSI_MODE_LINK];
	nla_put_u32(skb, NCSI_CHANNEL_ATTR_LINK_STATE, m->data[2]);
	if (nc->state == NCSI_CHANNEL_ACTIVE)
		nla_put_flag(skb, NCSI_CHANNEL_ATTR_ACTIVE);
	if (ndp->force_channel == nc)
		nla_put_flag(skb, NCSI_CHANNEL_ATTR_FORCED);

	nla_put_u32(skb, NCSI_CHANNEL_ATTR_VERSION_MAJOR, nc->version.version);
	nla_put_u32(skb, NCSI_CHANNEL_ATTR_VERSION_MINOR, nc->version.alpha2);
	nla_put_string(skb, NCSI_CHANNEL_ATTR_VERSION_STR, nc->version.fw_name);

	vid_nest = nla_nest_start(skb, NCSI_CHANNEL_ATTR_VLAN_LIST);
	if (!vid_nest)
		return -ENOMEM;
	ncf = &nc->vlan_filter;
	i = -1;
	while ((i = find_next_bit((void *)&ncf->bitmap, ncf->n_vids,
				  i + 1)) < ncf->n_vids) {
		if (ncf->vids[i])
			nla_put_u16(skb, NCSI_CHANNEL_ATTR_VLAN_ID,
				    ncf->vids[i]);
	}
	nla_nest_end(skb, vid_nest);

	return 0;
}

static int ncsi_write_package_info(struct sk_buff *skb,
				   struct ncsi_dev_priv *ndp, unsigned int id)
{
	struct nlattr *pnest, *cnest, *nest;
	struct ncsi_package *np;
	struct ncsi_channel *nc;
	bool found;
	int rc;

	if (id > ndp->package_num - 1) {
		netdev_info(ndp->ndev.dev, "NCSI: No package with id %u\n", id);
		return -ENODEV;
	}

	found = false;
	NCSI_FOR_EACH_PACKAGE(ndp, np) {
		if (np->id != id)
			continue;
		pnest = nla_nest_start(skb, NCSI_PKG_ATTR);
		if (!pnest)
			return -ENOMEM;
		nla_put_u32(skb, NCSI_PKG_ATTR_ID, np->id);
		if (ndp->force_package == np)
			nla_put_flag(skb, NCSI_PKG_ATTR_FORCED);
		cnest = nla_nest_start(skb, NCSI_PKG_ATTR_CHANNEL_LIST);
		if (!cnest) {
			nla_nest_cancel(skb, pnest);
			return -ENOMEM;
		}
		NCSI_FOR_EACH_CHANNEL(np, nc) {
			nest = nla_nest_start(skb, NCSI_CHANNEL_ATTR);
			if (!nest) {
				nla_nest_cancel(skb, cnest);
				nla_nest_cancel(skb, pnest);
				return -ENOMEM;
			}
			rc = ncsi_write_channel_info(skb, ndp, nc);
			if (rc) {
				nla_nest_cancel(skb, nest);
				nla_nest_cancel(skb, cnest);
				nla_nest_cancel(skb, pnest);
				return rc;
			}
			nla_nest_end(skb, nest);
		}
		nla_nest_end(skb, cnest);
		nla_nest_end(skb, pnest);
		found = true;
	}

	if (!found)
		return -ENODEV;

	return 0;
}

static int ncsi_pkg_info_nl(struct sk_buff *msg, struct genl_info *info)
{
	struct ncsi_dev_priv *ndp;
	unsigned int package_id;
	struct sk_buff *skb;
	struct nlattr *attr;
	void *hdr;
	int rc;

	if (!info || !info->attrs)
		return -EINVAL;

	if (!info->attrs[NCSI_ATTR_IFINDEX])
		return -EINVAL;

	if (!info->attrs[NCSI_ATTR_PACKAGE_ID])
		return -EINVAL;

	ndp = ndp_from_ifindex(genl_info_net(info),
			       nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
	if (!ndp)
		return -ENODEV;

	skb = genlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
	if (!skb)
		return -ENOMEM;

	hdr = genlmsg_put(skb, info->snd_portid, info->snd_seq,
			  &ncsi_genl_family, 0, NCSI_CMD_PKG_INFO);
	if (!hdr) {
		kfree_skb(skb);
		return -EMSGSIZE;
	}

	package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]);

	attr = nla_nest_start(skb, NCSI_ATTR_PACKAGE_LIST);
	if (!attr) {
		kfree_skb(skb);
		return -EMSGSIZE;
	}
	rc = ncsi_write_package_info(skb, ndp, package_id);

	if (rc) {
		nla_nest_cancel(skb, attr);
		goto err;
	}

	nla_nest_end(skb, attr);

	genlmsg_end(skb, hdr);
	return genlmsg_reply(skb, info);

err:
	kfree_skb(skb);
	return rc;
}

static int ncsi_pkg_info_all_nl(struct sk_buff *skb,
				struct netlink_callback *cb)
{
	struct nlattr *attrs[NCSI_ATTR_MAX + 1];
	struct ncsi_package *np, *package;
	struct ncsi_dev_priv *ndp;
	unsigned int package_id;
	struct nlattr *attr;
	void *hdr;
	int rc;

	rc = genlmsg_parse(cb->nlh, &ncsi_genl_family, attrs, NCSI_ATTR_MAX,
			   ncsi_genl_policy, NULL);
	if (rc)
		return rc;

	if (!attrs[NCSI_ATTR_IFINDEX])
		return -EINVAL;

	ndp = ndp_from_ifindex(get_net(sock_net(skb->sk)),
			       nla_get_u32(attrs[NCSI_ATTR_IFINDEX]));

	if (!ndp)
		return -ENODEV;

	package_id = cb->args[0];
	package = NULL;
	NCSI_FOR_EACH_PACKAGE(ndp, np)
		if (np->id == package_id)
			package = np;

	if (!package)
		return 0; /* done */

	hdr = genlmsg_put(skb, NETLINK_CB(cb->skb).portid, cb->nlh->nlmsg_seq,
			  &ncsi_genl_family, NLM_F_MULTI,  NCSI_CMD_PKG_INFO);
	if (!hdr) {
		rc = -EMSGSIZE;
		goto err;
	}

	attr = nla_nest_start(skb, NCSI_ATTR_PACKAGE_LIST);
	rc = ncsi_write_package_info(skb, ndp, package->id);
	if (rc) {
		nla_nest_cancel(skb, attr);
		goto err;
	}

	nla_nest_end(skb, attr);
	genlmsg_end(skb, hdr);

	cb->args[0] = package_id + 1;

	return skb->len;
err:
	genlmsg_cancel(skb, hdr);
	return rc;
}

static int ncsi_set_interface_nl(struct sk_buff *msg, struct genl_info *info)
{
	struct ncsi_package *np, *package;
	struct ncsi_channel *nc, *channel;
	u32 package_id, channel_id;
	struct ncsi_dev_priv *ndp;
	unsigned long flags;

	if (!info || !info->attrs)
		return -EINVAL;

	if (!info->attrs[NCSI_ATTR_IFINDEX])
		return -EINVAL;

	if (!info->attrs[NCSI_ATTR_PACKAGE_ID])
		return -EINVAL;

	ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
			       nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
	if (!ndp)
		return -ENODEV;

	package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]);
	package = NULL;

	spin_lock_irqsave(&ndp->lock, flags);

	NCSI_FOR_EACH_PACKAGE(ndp, np)
		if (np->id == package_id)
			package = np;
	if (!package) {
		/* The user has set a package that does not exist */
		spin_unlock_irqrestore(&ndp->lock, flags);
		return -ERANGE;
	}

	channel = NULL;
	if (!info->attrs[NCSI_ATTR_CHANNEL_ID]) {
		/* Allow any channel */
		channel_id = NCSI_RESERVED_CHANNEL;
	} else {
		channel_id = nla_get_u32(info->attrs[NCSI_ATTR_CHANNEL_ID]);
		NCSI_FOR_EACH_CHANNEL(package, nc)
			if (nc->id == channel_id)
				channel = nc;
	}

	if (channel_id != NCSI_RESERVED_CHANNEL && !channel) {
		/* The user has set a channel that does not exist on this
		 * package
		 */
		spin_unlock_irqrestore(&ndp->lock, flags);
		netdev_info(ndp->ndev.dev, "NCSI: Channel %u does not exist!\n",
			    channel_id);
		return -ERANGE;
	}

	ndp->force_package = package;
	ndp->force_channel = channel;
	spin_unlock_irqrestore(&ndp->lock, flags);

	netdev_info(ndp->ndev.dev, "Set package 0x%x, channel 0x%x%s as preferred\n",
		    package_id, channel_id,
		    channel_id == NCSI_RESERVED_CHANNEL ? " (any)" : "");

	/* Bounce the NCSI channel to set changes */
	ncsi_stop_dev(&ndp->ndev);
	ncsi_start_dev(&ndp->ndev);

	return 0;
}

static int ncsi_clear_interface_nl(struct sk_buff *msg, struct genl_info *info)
{
	struct ncsi_dev_priv *ndp;
	unsigned long flags;

	if (!info || !info->attrs)
		return -EINVAL;

	if (!info->attrs[NCSI_ATTR_IFINDEX])
		return -EINVAL;

	ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
			       nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
	if (!ndp)
		return -ENODEV;

	/* Clear any override */
	spin_lock_irqsave(&ndp->lock, flags);
	ndp->force_package = NULL;
	ndp->force_channel = NULL;
	spin_unlock_irqrestore(&ndp->lock, flags);
	netdev_info(ndp->ndev.dev, "NCSI: Cleared preferred package/channel\n");

	/* Bounce the NCSI channel to set changes */
	ncsi_stop_dev(&ndp->ndev);
	ncsi_start_dev(&ndp->ndev);

	return 0;
}

static int ncsi_send_cmd_nl(struct sk_buff *msg, struct genl_info *info)
{
	struct ncsi_dev_priv *ndp;
	struct ncsi_pkt_hdr *hdr;
	struct ncsi_cmd_arg nca;
	unsigned char *data;
	u32 package_id;
	u32 channel_id;
	int len, ret;

	if (!info || !info->attrs) {
		ret = -EINVAL;
		goto out;
	}

	if (!info->attrs[NCSI_ATTR_IFINDEX]) {
		ret = -EINVAL;
		goto out;
	}

	if (!info->attrs[NCSI_ATTR_PACKAGE_ID]) {
		ret = -EINVAL;
		goto out;
	}

	if (!info->attrs[NCSI_ATTR_CHANNEL_ID]) {
		ret = -EINVAL;
		goto out;
	}

	if (!info->attrs[NCSI_ATTR_DATA]) {
		ret = -EINVAL;
		goto out;
	}

	ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
			       nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
	if (!ndp) {
		ret = -ENODEV;
		goto out;
	}

	package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]);
	channel_id = nla_get_u32(info->attrs[NCSI_ATTR_CHANNEL_ID]);

	if (package_id >= NCSI_MAX_PACKAGE || channel_id >= NCSI_MAX_CHANNEL) {
		ret = -ERANGE;
		goto out_netlink;
	}

	len = nla_len(info->attrs[NCSI_ATTR_DATA]);
	if (len < sizeof(struct ncsi_pkt_hdr)) {
		netdev_info(ndp->ndev.dev, "NCSI: no command to send %u\n",
			    package_id);
		ret = -EINVAL;
		goto out_netlink;
	} else {
		data = (unsigned char *)nla_data(info->attrs[NCSI_ATTR_DATA]);
	}

	hdr = (struct ncsi_pkt_hdr *)data;

	nca.ndp = ndp;
	nca.package = (unsigned char)package_id;
	nca.channel = (unsigned char)channel_id;
	nca.type = hdr->type;
	nca.req_flags = NCSI_REQ_FLAG_NETLINK_DRIVEN;
	nca.info = info;
	nca.payload = ntohs(hdr->length);
	nca.data = data + sizeof(*hdr);

	ret = ncsi_xmit_cmd(&nca);
out_netlink:
	if (ret != 0) {
		netdev_err(ndp->ndev.dev,
			   "NCSI: Error %d sending command\n",
			   ret);
		ncsi_send_netlink_err(ndp->ndev.dev,
				      info->snd_seq,
				      info->snd_portid,
				      info->nlhdr,
				      ret);
	}
out:
	return ret;
}

int ncsi_send_netlink_rsp(struct ncsi_request *nr,
			  struct ncsi_package *np,
			  struct ncsi_channel *nc)
{
	struct sk_buff *skb;
	struct net *net;
	void *hdr;
	int rc;

	net = dev_net(nr->rsp->dev);

	skb = genlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
	if (!skb)
		return -ENOMEM;

	hdr = genlmsg_put(skb, nr->snd_portid, nr->snd_seq,
			  &ncsi_genl_family, 0, NCSI_CMD_SEND_CMD);
	if (!hdr) {
		kfree_skb(skb);
		return -EMSGSIZE;
	}

	nla_put_u32(skb, NCSI_ATTR_IFINDEX, nr->rsp->dev->ifindex);
	if (np)
		nla_put_u32(skb, NCSI_ATTR_PACKAGE_ID, np->id);
	if (nc)
		nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, nc->id);
	else
		nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, NCSI_RESERVED_CHANNEL);

	rc = nla_put(skb, NCSI_ATTR_DATA, nr->rsp->len, (void *)nr->rsp->data);
	if (rc)
		goto err;

	genlmsg_end(skb, hdr);
	return genlmsg_unicast(net, skb, nr->snd_portid);

err:
	kfree_skb(skb);
	return rc;
}

int ncsi_send_netlink_timeout(struct ncsi_request *nr,
			      struct ncsi_package *np,
			      struct ncsi_channel *nc)
{
	struct sk_buff *skb;
	struct net *net;
	void *hdr;

	skb = genlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
	if (!skb)
		return -ENOMEM;

	hdr = genlmsg_put(skb, nr->snd_portid, nr->snd_seq,
			  &ncsi_genl_family, 0, NCSI_CMD_SEND_CMD);
	if (!hdr) {
		kfree_skb(skb);
		return -EMSGSIZE;
	}

	net = dev_net(nr->cmd->dev);

	nla_put_u32(skb, NCSI_ATTR_IFINDEX, nr->cmd->dev->ifindex);

	if (np)
		nla_put_u32(skb, NCSI_ATTR_PACKAGE_ID, np->id);
	else
		nla_put_u32(skb, NCSI_ATTR_PACKAGE_ID,
			    NCSI_PACKAGE_INDEX((((struct ncsi_pkt_hdr *)
						 nr->cmd->data)->channel)));

	if (nc)
		nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, nc->id);
	else
		nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, NCSI_RESERVED_CHANNEL);

	genlmsg_end(skb, hdr);
	return genlmsg_unicast(net, skb, nr->snd_portid);
}

int ncsi_send_netlink_err(struct net_device *dev,
			  u32 snd_seq,
			  u32 snd_portid,
			  struct nlmsghdr *nlhdr,
			  int err)
{
	struct nlmsghdr *nlh;
	struct nlmsgerr *nle;
	struct sk_buff *skb;
	struct net *net;

	skb = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
	if (!skb)
		return -ENOMEM;

	net = dev_net(dev);

	nlh = nlmsg_put(skb, snd_portid, snd_seq,
			NLMSG_ERROR, sizeof(*nle), 0);
	nle = (struct nlmsgerr *)nlmsg_data(nlh);
	nle->error = err;
	memcpy(&nle->msg, nlhdr, sizeof(*nlh));

	nlmsg_end(skb, nlh);

	return nlmsg_unicast(net->genl_sock, skb, snd_portid);
}

static const struct genl_ops ncsi_ops[] = {
	{
		.cmd = NCSI_CMD_PKG_INFO,
		.policy = ncsi_genl_policy,
		.doit = ncsi_pkg_info_nl,
		.dumpit = ncsi_pkg_info_all_nl,
		.flags = 0,
	},
	{
		.cmd = NCSI_CMD_SET_INTERFACE,
		.policy = ncsi_genl_policy,
		.doit = ncsi_set_interface_nl,
		.flags = GENL_ADMIN_PERM,
	},
	{
		.cmd = NCSI_CMD_CLEAR_INTERFACE,
		.policy = ncsi_genl_policy,
		.doit = ncsi_clear_interface_nl,
		.flags = GENL_ADMIN_PERM,
	},
	{
		.cmd = NCSI_CMD_SEND_CMD,
		.policy = ncsi_genl_policy,
		.doit = ncsi_send_cmd_nl,
		.flags = GENL_ADMIN_PERM,
	},
};

static struct genl_family ncsi_genl_family __ro_after_init = {
	.name = "NCSI",
	.version = 0,
	.maxattr = NCSI_ATTR_MAX,
	.module = THIS_MODULE,
	.ops = ncsi_ops,
	.n_ops = ARRAY_SIZE(ncsi_ops),
};

int ncsi_init_netlink(struct net_device *dev)
{
	int rc;

	rc = genl_register_family(&ncsi_genl_family);
	if (rc)
		netdev_err(dev, "ncsi: failed to register netlink family\n");

	return rc;
}

int ncsi_unregister_netlink(struct net_device *dev)
{
	int rc;

	rc = genl_unregister_family(&ncsi_genl_family);
	if (rc)
		netdev_err(dev, "ncsi: failed to unregister netlink family\n");

	return rc;
}
Commit	Line	Data
955dc68c SMJ	1	/*
	2	* Copyright Samuel Mendoza-Jonas, IBM Corporation 2018.
	3	*
	4	* This program is free software; you can redistribute it and/or modify
	5	* it under the terms of the GNU General Public License as published by
	6	* the Free Software Foundation; either version 2 of the License, or
	7	* (at your option) any later version.
	8	*/
	9
	10	#include <linux/module.h>
	11	#include <linux/kernel.h>
	12	#include <linux/if_arp.h>
	13	#include <linux/rtnetlink.h>
	14	#include <linux/etherdevice.h>
955dc68c SMJ	15	#include <net/genetlink.h>
	16	#include <net/ncsi.h>
	17	#include <linux/skbuff.h>
	18	#include <net/sock.h>
	19	#include <uapi/linux/ncsi.h>
	20
	21	#include "internal.h"
9771b8cc	22	#include "ncsi-pkt.h"
955dc68c SMJ	23	#include "ncsi-netlink.h"
	24
	25	static struct genl_family ncsi_genl_family;
	26
	27	static const struct nla_policy ncsi_genl_policy[NCSI_ATTR_MAX + 1] = {
	28	[NCSI_ATTR_IFINDEX] = { .type = NLA_U32 },
	29	[NCSI_ATTR_PACKAGE_LIST] = { .type = NLA_NESTED },
	30	[NCSI_ATTR_PACKAGE_ID] = { .type = NLA_U32 },
	31	[NCSI_ATTR_CHANNEL_ID] = { .type = NLA_U32 },
9771b8cc	32	[NCSI_ATTR_DATA] = { .type = NLA_BINARY, .len = 2048 },
955dc68c SMJ	33	};
	34
	35	static struct ncsi_dev_priv ndp_from_ifindex(struct net net, u32 ifindex)
	36	{
	37	struct ncsi_dev_priv *ndp;
	38	struct net_device *dev;
	39	struct ncsi_dev *nd;
	40	struct ncsi_dev;
	41
	42	if (!net)
	43	return NULL;
	44
	45	dev = dev_get_by_index(net, ifindex);
	46	if (!dev) {
	47	pr_err("NCSI netlink: No device for ifindex %u\n", ifindex);
	48	return NULL;
	49	}
	50
	51	nd = ncsi_find_dev(dev);
	52	ndp = nd ? TO_NCSI_DEV_PRIV(nd) : NULL;
	53
	54	dev_put(dev);
	55	return ndp;
	56	}
	57
	58	static int ncsi_write_channel_info(struct sk_buff *skb,
	59	struct ncsi_dev_priv *ndp,
	60	struct ncsi_channel *nc)
	61	{
062b3e1b	62	struct ncsi_channel_vlan_filter *ncf;
955dc68c	63	struct ncsi_channel_mode *m;
062b3e1b	64	struct nlattr *vid_nest;
955dc68c SMJ	65	int i;
	66
	67	nla_put_u32(skb, NCSI_CHANNEL_ATTR_ID, nc->id);
	68	m = &nc->modes[NCSI_MODE_LINK];
	69	nla_put_u32(skb, NCSI_CHANNEL_ATTR_LINK_STATE, m->data[2]);
	70	if (nc->state == NCSI_CHANNEL_ACTIVE)
	71	nla_put_flag(skb, NCSI_CHANNEL_ATTR_ACTIVE);
	72	if (ndp->force_channel == nc)
	73	nla_put_flag(skb, NCSI_CHANNEL_ATTR_FORCED);
	74
	75	nla_put_u32(skb, NCSI_CHANNEL_ATTR_VERSION_MAJOR, nc->version.version);
	76	nla_put_u32(skb, NCSI_CHANNEL_ATTR_VERSION_MINOR, nc->version.alpha2);
	77	nla_put_string(skb, NCSI_CHANNEL_ATTR_VERSION_STR, nc->version.fw_name);
	78
	79	vid_nest = nla_nest_start(skb, NCSI_CHANNEL_ATTR_VLAN_LIST);
	80	if (!vid_nest)
	81	return -ENOMEM;
062b3e1b	82	ncf = &nc->vlan_filter;
955dc68c	83	i = -1;
062b3e1b SMJ	84	while ((i = find_next_bit((void *)&ncf->bitmap, ncf->n_vids,
	85	i + 1)) < ncf->n_vids) {
	86	if (ncf->vids[i])
955dc68c	87	nla_put_u16(skb, NCSI_CHANNEL_ATTR_VLAN_ID,
062b3e1b	88	ncf->vids[i]);
955dc68c SMJ	89	}
	90	nla_nest_end(skb, vid_nest);
	91
	92	return 0;
	93	}
	94
	95	static int ncsi_write_package_info(struct sk_buff *skb,
	96	struct ncsi_dev_priv *ndp, unsigned int id)
	97	{
	98	struct nlattr pnest, cnest, *nest;
	99	struct ncsi_package *np;
	100	struct ncsi_channel *nc;
	101	bool found;
	102	int rc;
	103
3d0371b3	104	if (id > ndp->package_num - 1) {
955dc68c SMJ	105	netdev_info(ndp->ndev.dev, "NCSI: No package with id %u\n", id);
	106	return -ENODEV;
	107	}
	108
	109	found = false;
	110	NCSI_FOR_EACH_PACKAGE(ndp, np) {
	111	if (np->id != id)
	112	continue;
	113	pnest = nla_nest_start(skb, NCSI_PKG_ATTR);
	114	if (!pnest)
	115	return -ENOMEM;
	116	nla_put_u32(skb, NCSI_PKG_ATTR_ID, np->id);
	117	if (ndp->force_package == np)
	118	nla_put_flag(skb, NCSI_PKG_ATTR_FORCED);
	119	cnest = nla_nest_start(skb, NCSI_PKG_ATTR_CHANNEL_LIST);
	120	if (!cnest) {
	121	nla_nest_cancel(skb, pnest);
	122	return -ENOMEM;
	123	}
	124	NCSI_FOR_EACH_CHANNEL(np, nc) {
	125	nest = nla_nest_start(skb, NCSI_CHANNEL_ATTR);
	126	if (!nest) {
	127	nla_nest_cancel(skb, cnest);
	128	nla_nest_cancel(skb, pnest);
	129	return -ENOMEM;
	130	}
	131	rc = ncsi_write_channel_info(skb, ndp, nc);
	132	if (rc) {
	133	nla_nest_cancel(skb, nest);
	134	nla_nest_cancel(skb, cnest);
	135	nla_nest_cancel(skb, pnest);
	136	return rc;
	137	}
	138	nla_nest_end(skb, nest);
	139	}
	140	nla_nest_end(skb, cnest);
	141	nla_nest_end(skb, pnest);
	142	found = true;
	143	}
	144
	145	if (!found)
	146	return -ENODEV;
	147
	148	return 0;
	149	}
	150
	151	static int ncsi_pkg_info_nl(struct sk_buff msg, struct genl_info info)
	152	{
	153	struct ncsi_dev_priv *ndp;
	154	unsigned int package_id;
	155	struct sk_buff *skb;
	156	struct nlattr *attr;
	157	void *hdr;
	158	int rc;
	159
	160	if (!info \|\| !info->attrs)
	161	return -EINVAL;
	162
	163	if (!info->attrs[NCSI_ATTR_IFINDEX])
	164	return -EINVAL;
	165
	166	if (!info->attrs[NCSI_ATTR_PACKAGE_ID])
	167	return -EINVAL;
	168
169	ndp = ndp_from_ifindex(genl_info_net(info),
170	nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
171	if (!ndp)
172	return -ENODEV;
173
174	skb = genlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
175	if (!skb)
176	return -ENOMEM;
177
178	hdr = genlmsg_put(skb, info->snd_portid, info->snd_seq,
179	&ncsi_genl_family, 0, NCSI_CMD_PKG_INFO);
180	if (!hdr) {
50db64b0	181	kfree_skb(skb);
955dc68c SMJ	182	return -EMSGSIZE;
	183	}
	184
	185	package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]);
	186
	187	attr = nla_nest_start(skb, NCSI_ATTR_PACKAGE_LIST);
8daf1a2d CIK	188	if (!attr) {
	189	kfree_skb(skb);
	190	return -EMSGSIZE;
	191	}
955dc68c SMJ	192	rc = ncsi_write_package_info(skb, ndp, package_id);
	193
	194	if (rc) {
	195	nla_nest_cancel(skb, attr);
	196	goto err;
	197	}
	198
	199	nla_nest_end(skb, attr);
	200
	201	genlmsg_end(skb, hdr);
	202	return genlmsg_reply(skb, info);
	203
	204	err:
50db64b0	205	kfree_skb(skb);
955dc68c SMJ	206	return rc;
	207	}
	208
	209	static int ncsi_pkg_info_all_nl(struct sk_buff *skb,
	210	struct netlink_callback *cb)
	211	{
0f51f358	212	struct nlattr *attrs[NCSI_ATTR_MAX + 1];
955dc68c SMJ	213	struct ncsi_package np, package;
	214	struct ncsi_dev_priv *ndp;
	215	unsigned int package_id;
	216	struct nlattr *attr;
	217	void *hdr;
	218	int rc;
	219
	220	rc = genlmsg_parse(cb->nlh, &ncsi_genl_family, attrs, NCSI_ATTR_MAX,
	221	ncsi_genl_policy, NULL);
	222	if (rc)
	223	return rc;
	224
	225	if (!attrs[NCSI_ATTR_IFINDEX])
	226	return -EINVAL;
	227
	228	ndp = ndp_from_ifindex(get_net(sock_net(skb->sk)),
	229	nla_get_u32(attrs[NCSI_ATTR_IFINDEX]));
	230
	231	if (!ndp)
	232	return -ENODEV;
	233
	234	package_id = cb->args[0];
	235	package = NULL;
	236	NCSI_FOR_EACH_PACKAGE(ndp, np)
	237	if (np->id == package_id)
	238	package = np;
	239
	240	if (!package)
	241	return 0; /* done */
	242
	243	hdr = genlmsg_put(skb, NETLINK_CB(cb->skb).portid, cb->nlh->nlmsg_seq,
3d0371b3	244	&ncsi_genl_family, NLM_F_MULTI, NCSI_CMD_PKG_INFO);
955dc68c SMJ	245	if (!hdr) {
	246	rc = -EMSGSIZE;
	247	goto err;
	248	}
	249
	250	attr = nla_nest_start(skb, NCSI_ATTR_PACKAGE_LIST);
	251	rc = ncsi_write_package_info(skb, ndp, package->id);
	252	if (rc) {
	253	nla_nest_cancel(skb, attr);
	254	goto err;
	255	}
	256
	257	nla_nest_end(skb, attr);
	258	genlmsg_end(skb, hdr);
	259
	260	cb->args[0] = package_id + 1;
	261
	262	return skb->len;
	263	err:
	264	genlmsg_cancel(skb, hdr);
	265	return rc;
	266	}
	267
	268	static int ncsi_set_interface_nl(struct sk_buff msg, struct genl_info info)
	269	{
	270	struct ncsi_package np, package;
	271	struct ncsi_channel nc, channel;
	272	u32 package_id, channel_id;
	273	struct ncsi_dev_priv *ndp;
	274	unsigned long flags;
	275
	276	if (!info \|\| !info->attrs)
	277	return -EINVAL;
	278
	279	if (!info->attrs[NCSI_ATTR_IFINDEX])
	280	return -EINVAL;
	281
	282	if (!info->attrs[NCSI_ATTR_PACKAGE_ID])
	283	return -EINVAL;
	284
	285	ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
	286	nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
	287	if (!ndp)
	288	return -ENODEV;
	289
	290	package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]);
	291	package = NULL;
	292
	293	spin_lock_irqsave(&ndp->lock, flags);
	294
	295	NCSI_FOR_EACH_PACKAGE(ndp, np)
	296	if (np->id == package_id)
	297	package = np;
	298	if (!package) {
	299	/* The user has set a package that does not exist */
054f34da	300	spin_unlock_irqrestore(&ndp->lock, flags);
955dc68c SMJ	301	return -ERANGE;
	302	}
	303
	304	channel = NULL;
	305	if (!info->attrs[NCSI_ATTR_CHANNEL_ID]) {
	306	/* Allow any channel */
	307	channel_id = NCSI_RESERVED_CHANNEL;
	308	} else {
	309	channel_id = nla_get_u32(info->attrs[NCSI_ATTR_CHANNEL_ID]);
	310	NCSI_FOR_EACH_CHANNEL(package, nc)
	311	if (nc->id == channel_id)
	312	channel = nc;
	313	}
	314
	315	if (channel_id != NCSI_RESERVED_CHANNEL && !channel) {
	316	/* The user has set a channel that does not exist on this
	317	* package
	318	*/
054f34da	319	spin_unlock_irqrestore(&ndp->lock, flags);
955dc68c SMJ	320	netdev_info(ndp->ndev.dev, "NCSI: Channel %u does not exist!\n",
	321	channel_id);
	322	return -ERANGE;
	323	}
	324
	325	ndp->force_package = package;
	326	ndp->force_channel = channel;
	327	spin_unlock_irqrestore(&ndp->lock, flags);
	328
	329	netdev_info(ndp->ndev.dev, "Set package 0x%x, channel 0x%x%s as preferred\n",
	330	package_id, channel_id,
	331	channel_id == NCSI_RESERVED_CHANNEL ? " (any)" : "");
	332
	333	/* Bounce the NCSI channel to set changes */
	334	ncsi_stop_dev(&ndp->ndev);
	335	ncsi_start_dev(&ndp->ndev);
	336
	337	return 0;
	338	}
	339
	340	static int ncsi_clear_interface_nl(struct sk_buff msg, struct genl_info info)
	341	{
	342	struct ncsi_dev_priv *ndp;
	343	unsigned long flags;
	344
	345	if (!info \|\| !info->attrs)
	346	return -EINVAL;
	347
	348	if (!info->attrs[NCSI_ATTR_IFINDEX])
	349	return -EINVAL;
	350
	351	ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
	352	nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
	353	if (!ndp)
	354	return -ENODEV;
	355
	356	/* Clear any override */
	357	spin_lock_irqsave(&ndp->lock, flags);
	358	ndp->force_package = NULL;
	359	ndp->force_channel = NULL;
	360	spin_unlock_irqrestore(&ndp->lock, flags);
	361	netdev_info(ndp->ndev.dev, "NCSI: Cleared preferred package/channel\n");
	362
	363	/* Bounce the NCSI channel to set changes */
	364	ncsi_stop_dev(&ndp->ndev);
	365	ncsi_start_dev(&ndp->ndev);
	366
	367	return 0;
	368	}
	369
9771b8cc JLD	370	static int ncsi_send_cmd_nl(struct sk_buff msg, struct genl_info info)
	371	{
	372	struct ncsi_dev_priv *ndp;
	373	struct ncsi_pkt_hdr *hdr;
	374	struct ncsi_cmd_arg nca;
	375	unsigned char *data;
	376	u32 package_id;
	377	u32 channel_id;
	378	int len, ret;
	379
	380	if (!info \|\| !info->attrs) {
	381	ret = -EINVAL;
	382	goto out;
	383	}
	384
	385	if (!info->attrs[NCSI_ATTR_IFINDEX]) {
	386	ret = -EINVAL;
	387	goto out;
	388	}
	389
	390	if (!info->attrs[NCSI_ATTR_PACKAGE_ID]) {
	391	ret = -EINVAL;
	392	goto out;
	393	}
	394
	395	if (!info->attrs[NCSI_ATTR_CHANNEL_ID]) {
	396	ret = -EINVAL;
	397	goto out;
	398	}
	399
	400	if (!info->attrs[NCSI_ATTR_DATA]) {
	401	ret = -EINVAL;
	402	goto out;
	403	}
	404
	405	ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
	406	nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
	407	if (!ndp) {
	408	ret = -ENODEV;
	409	goto out;
	410	}
	411
	412	package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]);
	413	channel_id = nla_get_u32(info->attrs[NCSI_ATTR_CHANNEL_ID]);
	414
	415	if (package_id >= NCSI_MAX_PACKAGE \|\| channel_id >= NCSI_MAX_CHANNEL) {
	416	ret = -ERANGE;
	417	goto out_netlink;
	418	}
	419
	420	len = nla_len(info->attrs[NCSI_ATTR_DATA]);
	421	if (len < sizeof(struct ncsi_pkt_hdr)) {
	422	netdev_info(ndp->ndev.dev, "NCSI: no command to send %u\n",
	423	package_id);
	424	ret = -EINVAL;
	425	goto out_netlink;
	426	} else {
	427	data = (unsigned char *)nla_data(info->attrs[NCSI_ATTR_DATA]);
	428	}
	429
	430	hdr = (struct ncsi_pkt_hdr *)data;
	431
	432	nca.ndp = ndp;
	433	nca.package = (unsigned char)package_id;
434	nca.channel = (unsigned char)channel_id;
435	nca.type = hdr->type;
436	nca.req_flags = NCSI_REQ_FLAG_NETLINK_DRIVEN;
437	nca.info = info;
438	nca.payload = ntohs(hdr->length);
439	nca.data = data + sizeof(*hdr);
440
441	ret = ncsi_xmit_cmd(&nca);
442	out_netlink:
443	if (ret != 0) {
444	netdev_err(ndp->ndev.dev,
445	"NCSI: Error %d sending command\n",
446	ret);
447	ncsi_send_netlink_err(ndp->ndev.dev,
448	info->snd_seq,
449	info->snd_portid,
450	info->nlhdr,
451	ret);
452	}
453	out:
454	return ret;
455	}
456
457	int ncsi_send_netlink_rsp(struct ncsi_request *nr,
458	struct ncsi_package *np,
459	struct ncsi_channel *nc)
460	{
461	struct sk_buff *skb;
462	struct net *net;
463	void *hdr;
464	int rc;
465
466	net = dev_net(nr->rsp->dev);
467
468	skb = genlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
469	if (!skb)
470	return -ENOMEM;
471
472	hdr = genlmsg_put(skb, nr->snd_portid, nr->snd_seq,
473	&ncsi_genl_family, 0, NCSI_CMD_SEND_CMD);
474	if (!hdr) {
475	kfree_skb(skb);
476	return -EMSGSIZE;
477	}
478
479	nla_put_u32(skb, NCSI_ATTR_IFINDEX, nr->rsp->dev->ifindex);
480	if (np)
481	nla_put_u32(skb, NCSI_ATTR_PACKAGE_ID, np->id);
482	if (nc)
483	nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, nc->id);
484	else
485	nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, NCSI_RESERVED_CHANNEL);
486
487	rc = nla_put(skb, NCSI_ATTR_DATA, nr->rsp->len, (void *)nr->rsp->data);
488	if (rc)
489	goto err;
490
491	genlmsg_end(skb, hdr);
492	return genlmsg_unicast(net, skb, nr->snd_portid);
493
494	err:
495	kfree_skb(skb);
496	return rc;
497	}
498
499	int ncsi_send_netlink_timeout(struct ncsi_request *nr,
500	struct ncsi_package *np,
501	struct ncsi_channel *nc)
502	{
503	struct sk_buff *skb;
504	struct net *net;
505	void *hdr;
506
507	skb = genlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
508	if (!skb)
509	return -ENOMEM;
510
511	hdr = genlmsg_put(skb, nr->snd_portid, nr->snd_seq,
512	&ncsi_genl_family, 0, NCSI_CMD_SEND_CMD);
513	if (!hdr) {
514	kfree_skb(skb);
515	return -EMSGSIZE;
516	}
517
518	net = dev_net(nr->cmd->dev);
519
520	nla_put_u32(skb, NCSI_ATTR_IFINDEX, nr->cmd->dev->ifindex);
521
522	if (np)
523	nla_put_u32(skb, NCSI_ATTR_PACKAGE_ID, np->id);
524	else
525	nla_put_u32(skb, NCSI_ATTR_PACKAGE_ID,
526	NCSI_PACKAGE_INDEX((((struct ncsi_pkt_hdr *)
527	nr->cmd->data)->channel)));
528
529	if (nc)
530	nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, nc->id);
531	else
532	nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, NCSI_RESERVED_CHANNEL);
533
534	genlmsg_end(skb, hdr);
535	return genlmsg_unicast(net, skb, nr->snd_portid);
536	}
537
538	int ncsi_send_netlink_err(struct net_device *dev,
539	u32 snd_seq,
540	u32 snd_portid,
541	struct nlmsghdr *nlhdr,
542	int err)
543	{
544	struct nlmsghdr *nlh;
545	struct nlmsgerr *nle;
546	struct sk_buff *skb;
547	struct net *net;
548
549	skb = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
550	if (!skb)
551	return -ENOMEM;
552
553	net = dev_net(dev);
554
555	nlh = nlmsg_put(skb, snd_portid, snd_seq,
556	NLMSG_ERROR, sizeof(*nle), 0);
557	nle = (struct nlmsgerr *)nlmsg_data(nlh);
558	nle->error = err;
559	memcpy(&nle->msg, nlhdr, sizeof(*nlh));
560
561	nlmsg_end(skb, nlh);
562
563	return nlmsg_unicast(net->genl_sock, skb, snd_portid);
564	}
565
955dc68c SMJ	566	static const struct genl_ops ncsi_ops[] = {
	567	{
	568	.cmd = NCSI_CMD_PKG_INFO,
	569	.policy = ncsi_genl_policy,
	570	.doit = ncsi_pkg_info_nl,
	571	.dumpit = ncsi_pkg_info_all_nl,
	572	.flags = 0,
	573	},
	574	{
	575	.cmd = NCSI_CMD_SET_INTERFACE,
	576	.policy = ncsi_genl_policy,
	577	.doit = ncsi_set_interface_nl,
	578	.flags = GENL_ADMIN_PERM,
	579	},
	580	{
	581	.cmd = NCSI_CMD_CLEAR_INTERFACE,
	582	.policy = ncsi_genl_policy,
	583	.doit = ncsi_clear_interface_nl,
	584	.flags = GENL_ADMIN_PERM,
	585	},
9771b8cc JLD	586	{
	587	.cmd = NCSI_CMD_SEND_CMD,
	588	.policy = ncsi_genl_policy,
	589	.doit = ncsi_send_cmd_nl,
	590	.flags = GENL_ADMIN_PERM,
	591	},
955dc68c SMJ	592	};
	593
	594	static struct genl_family ncsi_genl_family __ro_after_init = {
	595	.name = "NCSI",
	596	.version = 0,
	597	.maxattr = NCSI_ATTR_MAX,
	598	.module = THIS_MODULE,
	599	.ops = ncsi_ops,
	600	.n_ops = ARRAY_SIZE(ncsi_ops),
	601	};
	602
	603	int ncsi_init_netlink(struct net_device *dev)
	604	{
	605	int rc;
	606
	607	rc = genl_register_family(&ncsi_genl_family);
	608	if (rc)
	609	netdev_err(dev, "ncsi: failed to register netlink family\n");
	610
	611	return rc;
	612	}
	613
	614	int ncsi_unregister_netlink(struct net_device *dev)
	615	{
	616	int rc;
	617
	618	rc = genl_unregister_family(&ncsi_genl_family);
	619	if (rc)
	620	netdev_err(dev, "ncsi: failed to unregister netlink family\n");
	621
	622	return rc;
	623	}