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

// SPDX-License-Identifier: GPL-2.0-or-later
/*
 * Copyright Samuel Mendoza-Jonas, IBM Corporation 2018.
 */

#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 },
	[NCSI_ATTR_MULTI_FLAG] =	{ .type = NLA_FLAG },
	[NCSI_ATTR_PACKAGE_MASK] =	{ .type = NLA_U32 },
	[NCSI_ATTR_CHANNEL_MASK] =	{ .type = NLA_U32 },
};

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 (nc == nc->package->preferred_channel)
		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_noflag(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_noflag(skb, NCSI_PKG_ATTR);
		if (!pnest)
			return -ENOMEM;
		nla_put_u32(skb, NCSI_PKG_ATTR_ID, np->id);
		if ((0x1 << np->id) == ndp->package_whitelist)
			nla_put_flag(skb, NCSI_PKG_ATTR_FORCED);
		cnest = nla_nest_start_noflag(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_noflag(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_noflag(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_deprecated(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_noflag(skb, NCSI_ATTR_PACKAGE_LIST);
	if (!attr) {
		rc = -EMSGSIZE;
		goto err;
	}
	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;

	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 */
		return -ERANGE;
	}

	channel = NULL;
	if (info->attrs[NCSI_ATTR_CHANNEL_ID]) {
		channel_id = nla_get_u32(info->attrs[NCSI_ATTR_CHANNEL_ID]);
		NCSI_FOR_EACH_CHANNEL(package, nc)
			if (nc->id == channel_id) {
				channel = nc;
				break;
			}
		if (!channel) {
			netdev_info(ndp->ndev.dev,
				    "NCSI: Channel %u does not exist!\n",
				    channel_id);
			return -ERANGE;
		}
	}

	spin_lock_irqsave(&ndp->lock, flags);
	ndp->package_whitelist = 0x1 << package->id;
	ndp->multi_package = false;
	spin_unlock_irqrestore(&ndp->lock, flags);

	spin_lock_irqsave(&package->lock, flags);
	package->multi_channel = false;
	if (channel) {
		package->channel_whitelist = 0x1 << channel->id;
		package->preferred_channel = channel;
	} else {
		/* Allow any channel */
		package->channel_whitelist = UINT_MAX;
		package->preferred_channel = NULL;
	}
	spin_unlock_irqrestore(&package->lock, flags);

	if (channel)
		netdev_info(ndp->ndev.dev,
			    "Set package 0x%x, channel 0x%x as preferred\n",
			    package_id, channel_id);
	else
		netdev_info(ndp->ndev.dev, "Set package 0x%x as preferred\n",
			    package_id);

	/* Update channel configuration */
	if (!(ndp->flags & NCSI_DEV_RESET))
		ncsi_reset_dev(&ndp->ndev);

	return 0;
}

static int ncsi_clear_interface_nl(struct sk_buff *msg, struct genl_info *info)
{
	struct ncsi_dev_priv *ndp;
	struct ncsi_package *np;
	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;

	/* Reset any whitelists and disable multi mode */
	spin_lock_irqsave(&ndp->lock, flags);
	ndp->package_whitelist = UINT_MAX;
	ndp->multi_package = false;
	spin_unlock_irqrestore(&ndp->lock, flags);

	NCSI_FOR_EACH_PACKAGE(ndp, np) {
		spin_lock_irqsave(&np->lock, flags);
		np->multi_channel = false;
		np->channel_whitelist = UINT_MAX;
		np->preferred_channel = NULL;
		spin_unlock_irqrestore(&np->lock, flags);
	}
	netdev_info(ndp->ndev.dev, "NCSI: Cleared preferred package/channel\n");

	/* Update channel configuration */
	if (!(ndp->flags & NCSI_DEV_RESET))
		ncsi_reset_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 int ncsi_set_package_mask_nl(struct sk_buff *msg,
				    struct genl_info *info)
{
	struct ncsi_dev_priv *ndp;
	unsigned long flags;
	int rc;

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

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

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

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

	spin_lock_irqsave(&ndp->lock, flags);
	if (nla_get_flag(info->attrs[NCSI_ATTR_MULTI_FLAG])) {
		if (ndp->flags & NCSI_DEV_HWA) {
			ndp->multi_package = true;
			rc = 0;
		} else {
			netdev_err(ndp->ndev.dev,
				   "NCSI: Can't use multiple packages without HWA\n");
			rc = -EPERM;
		}
	} else {
		ndp->multi_package = false;
		rc = 0;
	}

	if (!rc)
		ndp->package_whitelist =
			nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_MASK]);
	spin_unlock_irqrestore(&ndp->lock, flags);

	if (!rc) {
		/* Update channel configuration */
		if (!(ndp->flags & NCSI_DEV_RESET))
			ncsi_reset_dev(&ndp->ndev);
	}

	return rc;
}

static int ncsi_set_channel_mask_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;

	if (!info->attrs[NCSI_ATTR_CHANNEL_MASK])
		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;
	NCSI_FOR_EACH_PACKAGE(ndp, np)
		if (np->id == package_id) {
			package = np;
			break;
		}
	if (!package)
		return -ERANGE;

	spin_lock_irqsave(&package->lock, flags);

	channel = NULL;
	if (info->attrs[NCSI_ATTR_CHANNEL_ID]) {
		channel_id = nla_get_u32(info->attrs[NCSI_ATTR_CHANNEL_ID]);
		NCSI_FOR_EACH_CHANNEL(np, nc)
			if (nc->id == channel_id) {
				channel = nc;
				break;
			}
		if (!channel) {
			spin_unlock_irqrestore(&package->lock, flags);
			return -ERANGE;
		}
		netdev_dbg(ndp->ndev.dev,
			   "NCSI: Channel %u set as preferred channel\n",
			   channel->id);
	}

	package->channel_whitelist =
		nla_get_u32(info->attrs[NCSI_ATTR_CHANNEL_MASK]);
	if (package->channel_whitelist == 0)
		netdev_dbg(ndp->ndev.dev,
			   "NCSI: Package %u set to all channels disabled\n",
			   package->id);

	package->preferred_channel = channel;

	if (nla_get_flag(info->attrs[NCSI_ATTR_MULTI_FLAG])) {
		package->multi_channel = true;
		netdev_info(ndp->ndev.dev,
			    "NCSI: Multi-channel enabled on package %u\n",
			    package_id);
	} else {
		package->multi_channel = false;
	}

	spin_unlock_irqrestore(&package->lock, flags);

	/* Update channel configuration */
	if (!(ndp->flags & NCSI_DEV_RESET))
		ncsi_reset_dev(&ndp->ndev);

	return 0;
}

static const struct genl_ops ncsi_ops[] = {
	{
		.cmd = NCSI_CMD_PKG_INFO,
		.validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
		.doit = ncsi_pkg_info_nl,
		.dumpit = ncsi_pkg_info_all_nl,
		.flags = 0,
	},
	{
		.cmd = NCSI_CMD_SET_INTERFACE,
		.validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
		.doit = ncsi_set_interface_nl,
		.flags = GENL_ADMIN_PERM,
	},
	{
		.cmd = NCSI_CMD_CLEAR_INTERFACE,
		.validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
		.doit = ncsi_clear_interface_nl,
		.flags = GENL_ADMIN_PERM,
	},
	{
		.cmd = NCSI_CMD_SEND_CMD,
		.validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
		.doit = ncsi_send_cmd_nl,
		.flags = GENL_ADMIN_PERM,
	},
	{
		.cmd = NCSI_CMD_SET_PACKAGE_MASK,
		.validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
		.doit = ncsi_set_package_mask_nl,
		.flags = GENL_ADMIN_PERM,
	},
	{
		.cmd = NCSI_CMD_SET_CHANNEL_MASK,
		.validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
		.doit = ncsi_set_channel_mask_nl,
		.flags = GENL_ADMIN_PERM,
	},
};

static struct genl_family ncsi_genl_family __ro_after_init = {
	.name = "NCSI",
	.version = 0,
	.maxattr = NCSI_ATTR_MAX,
	.policy = ncsi_genl_policy,
	.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
2874c5fd	1	// SPDX-License-Identifier: GPL-2.0-or-later
955dc68c SMJ	2	/*
955dc68c SMJ	3	* Copyright Samuel Mendoza-Jonas, IBM Corporation 2018.
955dc68c SMJ	4	*/
	5
	6	#include <linux/module.h>
	7	#include <linux/kernel.h>
	8	#include <linux/if_arp.h>
	9	#include <linux/rtnetlink.h>
	10	#include <linux/etherdevice.h>
955dc68c SMJ	11	#include <net/genetlink.h>
	12	#include <net/ncsi.h>
	13	#include <linux/skbuff.h>
	14	#include <net/sock.h>
	15	#include <uapi/linux/ncsi.h>
	16
	17	#include "internal.h"
9771b8cc	18	#include "ncsi-pkt.h"
955dc68c SMJ	19	#include "ncsi-netlink.h"
	20
	21	static struct genl_family ncsi_genl_family;
	22
	23	static const struct nla_policy ncsi_genl_policy[NCSI_ATTR_MAX + 1] = {
	24	[NCSI_ATTR_IFINDEX] = { .type = NLA_U32 },
	25	[NCSI_ATTR_PACKAGE_LIST] = { .type = NLA_NESTED },
	26	[NCSI_ATTR_PACKAGE_ID] = { .type = NLA_U32 },
	27	[NCSI_ATTR_CHANNEL_ID] = { .type = NLA_U32 },
9771b8cc	28	[NCSI_ATTR_DATA] = { .type = NLA_BINARY, .len = 2048 },
8d951a75 SMJ	29	[NCSI_ATTR_MULTI_FLAG] = { .type = NLA_FLAG },
	30	[NCSI_ATTR_PACKAGE_MASK] = { .type = NLA_U32 },
	31	[NCSI_ATTR_CHANNEL_MASK] = { .type = NLA_U32 },
955dc68c SMJ	32	};
	33
	34	static struct ncsi_dev_priv ndp_from_ifindex(struct net net, u32 ifindex)
	35	{
	36	struct ncsi_dev_priv *ndp;
	37	struct net_device *dev;
	38	struct ncsi_dev *nd;
	39	struct ncsi_dev;
	40
	41	if (!net)
	42	return NULL;
	43
	44	dev = dev_get_by_index(net, ifindex);
	45	if (!dev) {
	46	pr_err("NCSI netlink: No device for ifindex %u\n", ifindex);
	47	return NULL;
	48	}
	49
	50	nd = ncsi_find_dev(dev);
	51	ndp = nd ? TO_NCSI_DEV_PRIV(nd) : NULL;
	52
	53	dev_put(dev);
	54	return ndp;
	55	}
	56
	57	static int ncsi_write_channel_info(struct sk_buff *skb,
	58	struct ncsi_dev_priv *ndp,
	59	struct ncsi_channel *nc)
	60	{
062b3e1b	61	struct ncsi_channel_vlan_filter *ncf;
955dc68c	62	struct ncsi_channel_mode *m;
062b3e1b	63	struct nlattr *vid_nest;
955dc68c SMJ	64	int i;
	65
	66	nla_put_u32(skb, NCSI_CHANNEL_ATTR_ID, nc->id);
	67	m = &nc->modes[NCSI_MODE_LINK];
	68	nla_put_u32(skb, NCSI_CHANNEL_ATTR_LINK_STATE, m->data[2]);
	69	if (nc->state == NCSI_CHANNEL_ACTIVE)
	70	nla_put_flag(skb, NCSI_CHANNEL_ATTR_ACTIVE);
8d951a75	71	if (nc == nc->package->preferred_channel)
955dc68c SMJ	72	nla_put_flag(skb, NCSI_CHANNEL_ATTR_FORCED);
	73
	74	nla_put_u32(skb, NCSI_CHANNEL_ATTR_VERSION_MAJOR, nc->version.version);
	75	nla_put_u32(skb, NCSI_CHANNEL_ATTR_VERSION_MINOR, nc->version.alpha2);
	76	nla_put_string(skb, NCSI_CHANNEL_ATTR_VERSION_STR, nc->version.fw_name);
	77
ae0be8de	78	vid_nest = nla_nest_start_noflag(skb, NCSI_CHANNEL_ATTR_VLAN_LIST);
955dc68c SMJ	79	if (!vid_nest)
955dc68c SMJ	80	return -ENOMEM;
062b3e1b	81	ncf = &nc->vlan_filter;
955dc68c	82	i = -1;
062b3e1b SMJ	83	while ((i = find_next_bit((void *)&ncf->bitmap, ncf->n_vids,
	84	i + 1)) < ncf->n_vids) {
	85	if (ncf->vids[i])
955dc68c	86	nla_put_u16(skb, NCSI_CHANNEL_ATTR_VLAN_ID,
062b3e1b	87	ncf->vids[i]);
955dc68c SMJ	88	}
	89	nla_nest_end(skb, vid_nest);
	90
	91	return 0;
	92	}
	93
	94	static int ncsi_write_package_info(struct sk_buff *skb,
	95	struct ncsi_dev_priv *ndp, unsigned int id)
	96	{
	97	struct nlattr pnest, cnest, *nest;
	98	struct ncsi_package *np;
	99	struct ncsi_channel *nc;
	100	bool found;
	101	int rc;
	102
3d0371b3	103	if (id > ndp->package_num - 1) {
955dc68c SMJ	104	netdev_info(ndp->ndev.dev, "NCSI: No package with id %u\n", id);
	105	return -ENODEV;
	106	}
	107
	108	found = false;
	109	NCSI_FOR_EACH_PACKAGE(ndp, np) {
	110	if (np->id != id)
	111	continue;
ae0be8de	112	pnest = nla_nest_start_noflag(skb, NCSI_PKG_ATTR);
955dc68c SMJ	113	if (!pnest)
	114	return -ENOMEM;
	115	nla_put_u32(skb, NCSI_PKG_ATTR_ID, np->id);
8d951a75	116	if ((0x1 << np->id) == ndp->package_whitelist)
955dc68c	117	nla_put_flag(skb, NCSI_PKG_ATTR_FORCED);
ae0be8de	118	cnest = nla_nest_start_noflag(skb, NCSI_PKG_ATTR_CHANNEL_LIST);
955dc68c SMJ	119	if (!cnest) {
	120	nla_nest_cancel(skb, pnest);
	121	return -ENOMEM;
	122	}
	123	NCSI_FOR_EACH_CHANNEL(np, nc) {
ae0be8de	124	nest = nla_nest_start_noflag(skb, NCSI_CHANNEL_ATTR);
955dc68c SMJ	125	if (!nest) {
	126	nla_nest_cancel(skb, cnest);
	127	nla_nest_cancel(skb, pnest);
	128	return -ENOMEM;
	129	}
	130	rc = ncsi_write_channel_info(skb, ndp, nc);
	131	if (rc) {
	132	nla_nest_cancel(skb, nest);
	133	nla_nest_cancel(skb, cnest);
	134	nla_nest_cancel(skb, pnest);
	135	return rc;
	136	}
	137	nla_nest_end(skb, nest);
	138	}
	139	nla_nest_end(skb, cnest);
	140	nla_nest_end(skb, pnest);
	141	found = true;
	142	}
	143
	144	if (!found)
	145	return -ENODEV;
	146
	147	return 0;
	148	}
	149
	150	static int ncsi_pkg_info_nl(struct sk_buff msg, struct genl_info info)
	151	{
	152	struct ncsi_dev_priv *ndp;
	153	unsigned int package_id;
	154	struct sk_buff *skb;
	155	struct nlattr *attr;
	156	void *hdr;
	157	int rc;
	158
	159	if (!info \|\| !info->attrs)
	160	return -EINVAL;
	161
	162	if (!info->attrs[NCSI_ATTR_IFINDEX])
	163	return -EINVAL;
	164
	165	if (!info->attrs[NCSI_ATTR_PACKAGE_ID])
	166	return -EINVAL;
	167
	168	ndp = ndp_from_ifindex(genl_info_net(info),
	169	nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
	170	if (!ndp)
	171	return -ENODEV;
	172
	173	skb = genlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
	174	if (!skb)
	175	return -ENOMEM;
	176
	177	hdr = genlmsg_put(skb, info->snd_portid, info->snd_seq,
	178	&ncsi_genl_family, 0, NCSI_CMD_PKG_INFO);
	179	if (!hdr) {
50db64b0	180	kfree_skb(skb);
955dc68c SMJ	181	return -EMSGSIZE;
	182	}
	183
	184	package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]);
	185
ae0be8de	186	attr = nla_nest_start_noflag(skb, NCSI_ATTR_PACKAGE_LIST);
8daf1a2d CIK	187	if (!attr) {
	188	kfree_skb(skb);
	189	return -EMSGSIZE;
	190	}
955dc68c SMJ	191	rc = ncsi_write_package_info(skb, ndp, package_id);
	192
	193	if (rc) {
	194	nla_nest_cancel(skb, attr);
	195	goto err;
	196	}
	197
	198	nla_nest_end(skb, attr);
	199
	200	genlmsg_end(skb, hdr);
	201	return genlmsg_reply(skb, info);
	202
	203	err:
50db64b0	204	kfree_skb(skb);
955dc68c SMJ	205	return rc;
	206	}
	207
	208	static int ncsi_pkg_info_all_nl(struct sk_buff *skb,
	209	struct netlink_callback *cb)
	210	{
0f51f358	211	struct nlattr *attrs[NCSI_ATTR_MAX + 1];
955dc68c SMJ	212	struct ncsi_package np, package;
	213	struct ncsi_dev_priv *ndp;
	214	unsigned int package_id;
	215	struct nlattr *attr;
	216	void *hdr;
	217	int rc;
	218
8cb08174 JB	219	rc = genlmsg_parse_deprecated(cb->nlh, &ncsi_genl_family, attrs, NCSI_ATTR_MAX,
8cb08174 JB	220	ncsi_genl_policy, NULL);
955dc68c SMJ	221	if (rc)
	222	return rc;
	223
	224	if (!attrs[NCSI_ATTR_IFINDEX])
	225	return -EINVAL;
	226
	227	ndp = ndp_from_ifindex(get_net(sock_net(skb->sk)),
	228	nla_get_u32(attrs[NCSI_ATTR_IFINDEX]));
	229
	230	if (!ndp)
	231	return -ENODEV;
	232
	233	package_id = cb->args[0];
	234	package = NULL;
	235	NCSI_FOR_EACH_PACKAGE(ndp, np)
	236	if (np->id == package_id)
	237	package = np;
	238
	239	if (!package)
	240	return 0; /* done */
	241
	242	hdr = genlmsg_put(skb, NETLINK_CB(cb->skb).portid, cb->nlh->nlmsg_seq,
3d0371b3	243	&ncsi_genl_family, NLM_F_MULTI, NCSI_CMD_PKG_INFO);
955dc68c SMJ	244	if (!hdr) {
	245	rc = -EMSGSIZE;
	246	goto err;
	247	}
	248
ae0be8de	249	attr = nla_nest_start_noflag(skb, NCSI_ATTR_PACKAGE_LIST);
07660ca6 KL	250	if (!attr) {
	251	rc = -EMSGSIZE;
	252	goto err;
	253	}
955dc68c SMJ	254	rc = ncsi_write_package_info(skb, ndp, package->id);
	255	if (rc) {
	256	nla_nest_cancel(skb, attr);
	257	goto err;
	258	}
	259
	260	nla_nest_end(skb, attr);
	261	genlmsg_end(skb, hdr);
	262
	263	cb->args[0] = package_id + 1;
	264
	265	return skb->len;
	266	err:
	267	genlmsg_cancel(skb, hdr);
	268	return rc;
	269	}
	270
	271	static int ncsi_set_interface_nl(struct sk_buff msg, struct genl_info info)
	272	{
	273	struct ncsi_package np, package;
	274	struct ncsi_channel nc, channel;
	275	u32 package_id, channel_id;
	276	struct ncsi_dev_priv *ndp;
	277	unsigned long flags;
	278
	279	if (!info \|\| !info->attrs)
	280	return -EINVAL;
	281
	282	if (!info->attrs[NCSI_ATTR_IFINDEX])
	283	return -EINVAL;
	284
	285	if (!info->attrs[NCSI_ATTR_PACKAGE_ID])
	286	return -EINVAL;
	287
	288	ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
	289	nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
	290	if (!ndp)
	291	return -ENODEV;
	292
	293	package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]);
	294	package = NULL;
	295
955dc68c SMJ	296	NCSI_FOR_EACH_PACKAGE(ndp, np)
	297	if (np->id == package_id)
	298	package = np;
	299	if (!package) {
	300	/* The user has set a package that does not exist */
	301	return -ERANGE;
	302	}
	303
	304	channel = NULL;
8d951a75	305	if (info->attrs[NCSI_ATTR_CHANNEL_ID]) {
955dc68c SMJ	306	channel_id = nla_get_u32(info->attrs[NCSI_ATTR_CHANNEL_ID]);
955dc68c SMJ	307	NCSI_FOR_EACH_CHANNEL(package, nc)
8d951a75	308	if (nc->id == channel_id) {
955dc68c	309	channel = nc;
8d951a75 SMJ	310	break;
	311	}
	312	if (!channel) {
	313	netdev_info(ndp->ndev.dev,
	314	"NCSI: Channel %u does not exist!\n",
	315	channel_id);
	316	return -ERANGE;
	317	}
955dc68c SMJ	318	}
955dc68c SMJ	319
8d951a75 SMJ	320	spin_lock_irqsave(&ndp->lock, flags);
	321	ndp->package_whitelist = 0x1 << package->id;
	322	ndp->multi_package = false;
955dc68c SMJ	323	spin_unlock_irqrestore(&ndp->lock, flags);
955dc68c SMJ	324
8d951a75 SMJ	325	spin_lock_irqsave(&package->lock, flags);
	326	package->multi_channel = false;
	327	if (channel) {
	328	package->channel_whitelist = 0x1 << channel->id;
	329	package->preferred_channel = channel;
	330	} else {
	331	/* Allow any channel */
	332	package->channel_whitelist = UINT_MAX;
	333	package->preferred_channel = NULL;
	334	}
	335	spin_unlock_irqrestore(&package->lock, flags);
	336
	337	if (channel)
	338	netdev_info(ndp->ndev.dev,
	339	"Set package 0x%x, channel 0x%x as preferred\n",
	340	package_id, channel_id);
	341	else
	342	netdev_info(ndp->ndev.dev, "Set package 0x%x as preferred\n",
	343	package_id);
955dc68c	344
2878a2cf SMJ	345	/* Update channel configuration */
	346	if (!(ndp->flags & NCSI_DEV_RESET))
	347	ncsi_reset_dev(&ndp->ndev);
955dc68c SMJ	348
	349	return 0;
	350	}
	351
	352	static int ncsi_clear_interface_nl(struct sk_buff msg, struct genl_info info)
	353	{
	354	struct ncsi_dev_priv *ndp;
8d951a75	355	struct ncsi_package *np;
955dc68c SMJ	356	unsigned long flags;
	357
	358	if (!info \|\| !info->attrs)
	359	return -EINVAL;
	360
	361	if (!info->attrs[NCSI_ATTR_IFINDEX])
	362	return -EINVAL;
	363
	364	ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
	365	nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
	366	if (!ndp)
	367	return -ENODEV;
	368
8d951a75	369	/* Reset any whitelists and disable multi mode */
955dc68c	370	spin_lock_irqsave(&ndp->lock, flags);
8d951a75 SMJ	371	ndp->package_whitelist = UINT_MAX;
8d951a75 SMJ	372	ndp->multi_package = false;
955dc68c	373	spin_unlock_irqrestore(&ndp->lock, flags);
8d951a75 SMJ	374
	375	NCSI_FOR_EACH_PACKAGE(ndp, np) {
	376	spin_lock_irqsave(&np->lock, flags);
	377	np->multi_channel = false;
	378	np->channel_whitelist = UINT_MAX;
	379	np->preferred_channel = NULL;
	380	spin_unlock_irqrestore(&np->lock, flags);
	381	}
955dc68c SMJ	382	netdev_info(ndp->ndev.dev, "NCSI: Cleared preferred package/channel\n");
955dc68c SMJ	383
2878a2cf SMJ	384	/* Update channel configuration */
	385	if (!(ndp->flags & NCSI_DEV_RESET))
	386	ncsi_reset_dev(&ndp->ndev);
955dc68c SMJ	387
	388	return 0;
	389	}
	390
9771b8cc JLD	391	static int ncsi_send_cmd_nl(struct sk_buff msg, struct genl_info info)
	392	{
	393	struct ncsi_dev_priv *ndp;
	394	struct ncsi_pkt_hdr *hdr;
	395	struct ncsi_cmd_arg nca;
	396	unsigned char *data;
	397	u32 package_id;
	398	u32 channel_id;
	399	int len, ret;
	400
	401	if (!info \|\| !info->attrs) {
	402	ret = -EINVAL;
	403	goto out;
	404	}
	405
	406	if (!info->attrs[NCSI_ATTR_IFINDEX]) {
	407	ret = -EINVAL;
	408	goto out;
	409	}
	410
	411	if (!info->attrs[NCSI_ATTR_PACKAGE_ID]) {
	412	ret = -EINVAL;
	413	goto out;
	414	}
	415
	416	if (!info->attrs[NCSI_ATTR_CHANNEL_ID]) {
	417	ret = -EINVAL;
	418	goto out;
	419	}
	420
	421	if (!info->attrs[NCSI_ATTR_DATA]) {
	422	ret = -EINVAL;
	423	goto out;
	424	}
	425
	426	ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
	427	nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
	428	if (!ndp) {
	429	ret = -ENODEV;
	430	goto out;
	431	}
	432
	433	package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]);
	434	channel_id = nla_get_u32(info->attrs[NCSI_ATTR_CHANNEL_ID]);
	435
	436	if (package_id >= NCSI_MAX_PACKAGE \|\| channel_id >= NCSI_MAX_CHANNEL) {
	437	ret = -ERANGE;
	438	goto out_netlink;
	439	}
	440
	441	len = nla_len(info->attrs[NCSI_ATTR_DATA]);
	442	if (len < sizeof(struct ncsi_pkt_hdr)) {
	443	netdev_info(ndp->ndev.dev, "NCSI: no command to send %u\n",
	444	package_id);
	445	ret = -EINVAL;
	446	goto out_netlink;
	447	} else {
	448	data = (unsigned char *)nla_data(info->attrs[NCSI_ATTR_DATA]);
	449	}
	450
	451	hdr = (struct ncsi_pkt_hdr *)data;
	452
	453	nca.ndp = ndp;
	454	nca.package = (unsigned char)package_id;
455	nca.channel = (unsigned char)channel_id;
456	nca.type = hdr->type;
457	nca.req_flags = NCSI_REQ_FLAG_NETLINK_DRIVEN;
458	nca.info = info;
459	nca.payload = ntohs(hdr->length);
460	nca.data = data + sizeof(*hdr);
461
462	ret = ncsi_xmit_cmd(&nca);
463	out_netlink:
464	if (ret != 0) {
465	netdev_err(ndp->ndev.dev,
466	"NCSI: Error %d sending command\n",
467	ret);
468	ncsi_send_netlink_err(ndp->ndev.dev,
469	info->snd_seq,
470	info->snd_portid,
471	info->nlhdr,
472	ret);
473	}
474	out:
475	return ret;
476	}
477
478	int ncsi_send_netlink_rsp(struct ncsi_request *nr,
479	struct ncsi_package *np,
480	struct ncsi_channel *nc)
481	{
482	struct sk_buff *skb;
483	struct net *net;
484	void *hdr;
485	int rc;
486
487	net = dev_net(nr->rsp->dev);
488
489	skb = genlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
490	if (!skb)
491	return -ENOMEM;
492
493	hdr = genlmsg_put(skb, nr->snd_portid, nr->snd_seq,
494	&ncsi_genl_family, 0, NCSI_CMD_SEND_CMD);
495	if (!hdr) {
496	kfree_skb(skb);
497	return -EMSGSIZE;
498	}
499
500	nla_put_u32(skb, NCSI_ATTR_IFINDEX, nr->rsp->dev->ifindex);
501	if (np)
502	nla_put_u32(skb, NCSI_ATTR_PACKAGE_ID, np->id);
503	if (nc)
504	nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, nc->id);
505	else
506	nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, NCSI_RESERVED_CHANNEL);
507
508	rc = nla_put(skb, NCSI_ATTR_DATA, nr->rsp->len, (void *)nr->rsp->data);
509	if (rc)
510	goto err;
511
512	genlmsg_end(skb, hdr);
513	return genlmsg_unicast(net, skb, nr->snd_portid);
514
515	err:
516	kfree_skb(skb);
517	return rc;
518	}
519
520	int ncsi_send_netlink_timeout(struct ncsi_request *nr,
521	struct ncsi_package *np,
522	struct ncsi_channel *nc)
523	{
524	struct sk_buff *skb;
525	struct net *net;
526	void *hdr;
527
528	skb = genlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
529	if (!skb)
530	return -ENOMEM;
531
532	hdr = genlmsg_put(skb, nr->snd_portid, nr->snd_seq,
533	&ncsi_genl_family, 0, NCSI_CMD_SEND_CMD);
534	if (!hdr) {
535	kfree_skb(skb);
536	return -EMSGSIZE;
537	}
538
539	net = dev_net(nr->cmd->dev);
540
541	nla_put_u32(skb, NCSI_ATTR_IFINDEX, nr->cmd->dev->ifindex);
542
543	if (np)
544	nla_put_u32(skb, NCSI_ATTR_PACKAGE_ID, np->id);
545	else
546	nla_put_u32(skb, NCSI_ATTR_PACKAGE_ID,
547	NCSI_PACKAGE_INDEX((((struct ncsi_pkt_hdr *)
548	nr->cmd->data)->channel)));
549
550	if (nc)
551	nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, nc->id);
552	else
553	nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, NCSI_RESERVED_CHANNEL);
554
555	genlmsg_end(skb, hdr);
556	return genlmsg_unicast(net, skb, nr->snd_portid);
557	}
558
559	int ncsi_send_netlink_err(struct net_device *dev,
560	u32 snd_seq,
561	u32 snd_portid,
562	struct nlmsghdr *nlhdr,
563	int err)
564	{
565	struct nlmsghdr *nlh;
566	struct nlmsgerr *nle;
567	struct sk_buff *skb;
568	struct net *net;
569
570	skb = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
571	if (!skb)
572	return -ENOMEM;
573
574	net = dev_net(dev);
575
576	nlh = nlmsg_put(skb, snd_portid, snd_seq,
577	NLMSG_ERROR, sizeof(*nle), 0);
578	nle = (struct nlmsgerr *)nlmsg_data(nlh);
579	nle->error = err;
580	memcpy(&nle->msg, nlhdr, sizeof(*nlh));
581
582	nlmsg_end(skb, nlh);
583
584	return nlmsg_unicast(net->genl_sock, skb, snd_portid);
585	}
586
8d951a75 SMJ	587	static int ncsi_set_package_mask_nl(struct sk_buff *msg,
	588	struct genl_info *info)
	589	{
	590	struct ncsi_dev_priv *ndp;
	591	unsigned long flags;
	592	int rc;
	593
	594	if (!info \|\| !info->attrs)
	595	return -EINVAL;
	596
	597	if (!info->attrs[NCSI_ATTR_IFINDEX])
	598	return -EINVAL;
	599
	600	if (!info->attrs[NCSI_ATTR_PACKAGE_MASK])
	601	return -EINVAL;
	602
	603	ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
	604	nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
	605	if (!ndp)
	606	return -ENODEV;
	607
	608	spin_lock_irqsave(&ndp->lock, flags);
	609	if (nla_get_flag(info->attrs[NCSI_ATTR_MULTI_FLAG])) {
	610	if (ndp->flags & NCSI_DEV_HWA) {
	611	ndp->multi_package = true;
	612	rc = 0;
	613	} else {
	614	netdev_err(ndp->ndev.dev,
	615	"NCSI: Can't use multiple packages without HWA\n");
	616	rc = -EPERM;
	617	}
	618	} else {
	619	ndp->multi_package = false;
	620	rc = 0;
	621	}
	622
	623	if (!rc)
	624	ndp->package_whitelist =
	625	nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_MASK]);
	626	spin_unlock_irqrestore(&ndp->lock, flags);
	627
	628	if (!rc) {
	629	/* Update channel configuration */
	630	if (!(ndp->flags & NCSI_DEV_RESET))
	631	ncsi_reset_dev(&ndp->ndev);
	632	}
	633
	634	return rc;
	635	}
	636
	637	static int ncsi_set_channel_mask_nl(struct sk_buff *msg,
	638	struct genl_info *info)
	639	{
	640	struct ncsi_package np, package;
	641	struct ncsi_channel nc, channel;
	642	u32 package_id, channel_id;
	643	struct ncsi_dev_priv *ndp;
	644	unsigned long flags;
	645
	646	if (!info \|\| !info->attrs)
	647	return -EINVAL;
	648
	649	if (!info->attrs[NCSI_ATTR_IFINDEX])
	650	return -EINVAL;
651
652	if (!info->attrs[NCSI_ATTR_PACKAGE_ID])
653	return -EINVAL;
654
655	if (!info->attrs[NCSI_ATTR_CHANNEL_MASK])
656	return -EINVAL;
657
658	ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
659	nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
660	if (!ndp)
661	return -ENODEV;
662
663	package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]);
664	package = NULL;
665	NCSI_FOR_EACH_PACKAGE(ndp, np)
666	if (np->id == package_id) {
667	package = np;
668	break;
669	}
670	if (!package)
671	return -ERANGE;
672
673	spin_lock_irqsave(&package->lock, flags);
674
675	channel = NULL;
676	if (info->attrs[NCSI_ATTR_CHANNEL_ID]) {
677	channel_id = nla_get_u32(info->attrs[NCSI_ATTR_CHANNEL_ID]);
678	NCSI_FOR_EACH_CHANNEL(np, nc)
679	if (nc->id == channel_id) {
680	channel = nc;
681	break;
682	}
683	if (!channel) {
684	spin_unlock_irqrestore(&package->lock, flags);
685	return -ERANGE;
686	}
687	netdev_dbg(ndp->ndev.dev,
688	"NCSI: Channel %u set as preferred channel\n",
689	channel->id);
690	}
691
692	package->channel_whitelist =
693	nla_get_u32(info->attrs[NCSI_ATTR_CHANNEL_MASK]);
694	if (package->channel_whitelist == 0)
695	netdev_dbg(ndp->ndev.dev,
696	"NCSI: Package %u set to all channels disabled\n",
697	package->id);
698
699	package->preferred_channel = channel;
700
701	if (nla_get_flag(info->attrs[NCSI_ATTR_MULTI_FLAG])) {
702	package->multi_channel = true;
703	netdev_info(ndp->ndev.dev,
704	"NCSI: Multi-channel enabled on package %u\n",
705	package_id);
706	} else {
707	package->multi_channel = false;
708	}
709
710	spin_unlock_irqrestore(&package->lock, flags);
711
712	/* Update channel configuration */
713	if (!(ndp->flags & NCSI_DEV_RESET))
714	ncsi_reset_dev(&ndp->ndev);
715
716	return 0;
717	}
718
955dc68c SMJ	719	static const struct genl_ops ncsi_ops[] = {
	720	{
	721	.cmd = NCSI_CMD_PKG_INFO,
ef6243ac	722	.validate = GENL_DONT_VALIDATE_STRICT \| GENL_DONT_VALIDATE_DUMP,
955dc68c SMJ	723	.doit = ncsi_pkg_info_nl,
	724	.dumpit = ncsi_pkg_info_all_nl,
	725	.flags = 0,
	726	},
	727	{
	728	.cmd = NCSI_CMD_SET_INTERFACE,
ef6243ac	729	.validate = GENL_DONT_VALIDATE_STRICT \| GENL_DONT_VALIDATE_DUMP,
955dc68c SMJ	730	.doit = ncsi_set_interface_nl,
	731	.flags = GENL_ADMIN_PERM,
	732	},
	733	{
	734	.cmd = NCSI_CMD_CLEAR_INTERFACE,
ef6243ac	735	.validate = GENL_DONT_VALIDATE_STRICT \| GENL_DONT_VALIDATE_DUMP,
955dc68c SMJ	736	.doit = ncsi_clear_interface_nl,
	737	.flags = GENL_ADMIN_PERM,
	738	},
9771b8cc JLD	739	{
9771b8cc JLD	740	.cmd = NCSI_CMD_SEND_CMD,
ef6243ac	741	.validate = GENL_DONT_VALIDATE_STRICT \| GENL_DONT_VALIDATE_DUMP,
9771b8cc JLD	742	.doit = ncsi_send_cmd_nl,
	743	.flags = GENL_ADMIN_PERM,
	744	},
8d951a75 SMJ	745	{
8d951a75 SMJ	746	.cmd = NCSI_CMD_SET_PACKAGE_MASK,
ef6243ac	747	.validate = GENL_DONT_VALIDATE_STRICT \| GENL_DONT_VALIDATE_DUMP,
8d951a75 SMJ	748	.doit = ncsi_set_package_mask_nl,
	749	.flags = GENL_ADMIN_PERM,
	750	},
	751	{
	752	.cmd = NCSI_CMD_SET_CHANNEL_MASK,
ef6243ac	753	.validate = GENL_DONT_VALIDATE_STRICT \| GENL_DONT_VALIDATE_DUMP,
8d951a75 SMJ	754	.doit = ncsi_set_channel_mask_nl,
	755	.flags = GENL_ADMIN_PERM,
	756	},
955dc68c SMJ	757	};
	758
	759	static struct genl_family ncsi_genl_family __ro_after_init = {
	760	.name = "NCSI",
	761	.version = 0,
	762	.maxattr = NCSI_ATTR_MAX,
3b0f31f2	763	.policy = ncsi_genl_policy,
955dc68c SMJ	764	.module = THIS_MODULE,
	765	.ops = ncsi_ops,
	766	.n_ops = ARRAY_SIZE(ncsi_ops),
	767	};
	768
	769	int ncsi_init_netlink(struct net_device *dev)
	770	{
	771	int rc;
	772
	773	rc = genl_register_family(&ncsi_genl_family);
	774	if (rc)
	775	netdev_err(dev, "ncsi: failed to register netlink family\n");
	776
	777	return rc;
	778	}
	779
	780	int ncsi_unregister_netlink(struct net_device *dev)
	781	{
	782	int rc;
	783
	784	rc = genl_unregister_family(&ncsi_genl_family);
	785	if (rc)
	786	netdev_err(dev, "ncsi: failed to unregister netlink family\n");
	787
	788	return rc;
	789	}