net/ncsi: Configure VLAN tag filter (21acf630) · Commits · 戴 / test

include/net/ncsi.h

+2 −0

Original line number	Original line	Diff line number	Diff line
	@@ -28,6 +28,8 @@ struct ncsi_dev {
	};		};

	#ifdef CONFIG_NET_NCSI		#ifdef CONFIG_NET_NCSI
			int ncsi_vlan_rx_add_vid(struct net_device *dev, __be16 proto, u16 vid);
			int ncsi_vlan_rx_kill_vid(struct net_device *dev, __be16 proto, u16 vid);
	struct ncsi_dev ncsi_register_dev(struct net_device dev,		struct ncsi_dev ncsi_register_dev(struct net_device dev,
	void (notifier)(struct ncsi_dev nd));		void (notifier)(struct ncsi_dev nd));
	int ncsi_start_dev(struct ncsi_dev *nd);		int ncsi_start_dev(struct ncsi_dev *nd);

net/ncsi/internal.h

+11 −0

Original line number	Original line	Diff line number	Diff line
	@@ -180,6 +180,7 @@ struct ncsi_channel {
	#define NCSI_CHANNEL_INACTIVE 1		#define NCSI_CHANNEL_INACTIVE 1
	#define NCSI_CHANNEL_ACTIVE 2		#define NCSI_CHANNEL_ACTIVE 2
	#define NCSI_CHANNEL_INVISIBLE 3		#define NCSI_CHANNEL_INVISIBLE 3
			bool reconfigure_needed;
	spinlock_t lock; /* Protect filters etc */		spinlock_t lock; /* Protect filters etc */
	struct ncsi_package *package;		struct ncsi_package *package;
	struct ncsi_channel_version version;		struct ncsi_channel_version version;
	@@ -235,6 +236,9 @@ enum {
	ncsi_dev_state_probe_dp,		ncsi_dev_state_probe_dp,
	ncsi_dev_state_config_sp = 0x0301,		ncsi_dev_state_config_sp = 0x0301,
	ncsi_dev_state_config_cis,		ncsi_dev_state_config_cis,
			ncsi_dev_state_config_clear_vids,
			ncsi_dev_state_config_svf,
			ncsi_dev_state_config_ev,
	ncsi_dev_state_config_sma,		ncsi_dev_state_config_sma,
	ncsi_dev_state_config_ebf,		ncsi_dev_state_config_ebf,
	#if IS_ENABLED(CONFIG_IPV6)		#if IS_ENABLED(CONFIG_IPV6)
	@@ -253,6 +257,12 @@ enum {
	ncsi_dev_state_suspend_done		ncsi_dev_state_suspend_done
	};		};

			struct vlan_vid {
			struct list_head list;
			__be16 proto;
			u16 vid;
			};

	struct ncsi_dev_priv {		struct ncsi_dev_priv {
	struct ncsi_dev ndev; /* Associated NCSI device */		struct ncsi_dev ndev; /* Associated NCSI device */
	unsigned int flags; /* NCSI device flags */		unsigned int flags; /* NCSI device flags */
	@@ -276,6 +286,7 @@ struct ncsi_dev_priv {
	struct work_struct work; /* For channel management */		struct work_struct work; /* For channel management */
	struct packet_type ptype; /* NCSI packet Rx handler */		struct packet_type ptype; /* NCSI packet Rx handler */
	struct list_head node; /* Form NCSI device list */		struct list_head node; /* Form NCSI device list */
			struct list_head vlan_vids; /* List of active VLAN IDs */
	};		};

	struct ncsi_cmd_arg {		struct ncsi_cmd_arg {

net/ncsi/ncsi-manage.c

+305 −3

Original line number	Original line	Diff line number	Diff line
	@@ -38,6 +38,25 @@ static inline int ncsi_filter_size(int table)
	return sizes[table];		return sizes[table];
	}		}

			u32 ncsi_get_filter(struct ncsi_channel nc, int table, int index)
			{
			struct ncsi_channel_filter *ncf;
			int size;

			ncf = nc->filters[table];
			if (!ncf)
			return NULL;

			size = ncsi_filter_size(table);
			if (size < 0)
			return NULL;

			return ncf->data + size * index;
			}

			/* Find the first active filter in a filter table that matches the given
			* data parameter. If data is NULL, this returns the first active filter.
			*/
	int ncsi_find_filter(struct ncsi_channel nc, int table, void data)		int ncsi_find_filter(struct ncsi_channel nc, int table, void data)
	{		{
	struct ncsi_channel_filter *ncf;		struct ncsi_channel_filter *ncf;
	@@ -58,7 +77,7 @@ int ncsi_find_filter(struct ncsi_channel nc, int table, void data)
	index = -1;		index = -1;
	while ((index = find_next_bit(bitmap, ncf->total, index + 1))		while ((index = find_next_bit(bitmap, ncf->total, index + 1))
	< ncf->total) {		< ncf->total) {
	if (!memcmp(ncf->data + size * index, data, size)) {		if (!data \|\| !memcmp(ncf->data + size * index, data, size)) {
	spin_unlock_irqrestore(&nc->lock, flags);		spin_unlock_irqrestore(&nc->lock, flags);
	return index;		return index;
	}		}
	@@ -639,6 +658,95 @@ error:
	nd->state = ncsi_dev_state_functional;		nd->state = ncsi_dev_state_functional;
	}		}

			/* Check the VLAN filter bitmap for a set filter, and construct a
			* "Set VLAN Filter - Disable" packet if found.
			*/
			static int clear_one_vid(struct ncsi_dev_priv ndp, struct ncsi_channel nc,
			struct ncsi_cmd_arg *nca)
			{
			int index;
			u32 *data;
			u16 vid;

			index = ncsi_find_filter(nc, NCSI_FILTER_VLAN, NULL);
			if (index < 0) {
			/* Filter table empty */
			return -1;
			}

			data = ncsi_get_filter(nc, NCSI_FILTER_VLAN, index);
			if (!data) {
			netdev_err(ndp->ndev.dev,
			"ncsi: failed to retrieve filter %d\n", index);
			/* Set the VLAN id to 0 - this will still disable the entry in
			* the filter table, but we won't know what it was.
			*/
			vid = 0;
			} else {
			vid = (u16 )data;
			}

			netdev_printk(KERN_DEBUG, ndp->ndev.dev,
			"ncsi: removed vlan tag %u at index %d\n",
			vid, index + 1);
			ncsi_remove_filter(nc, NCSI_FILTER_VLAN, index);

			nca->type = NCSI_PKT_CMD_SVF;
			nca->words[1] = vid;
			/* HW filter index starts at 1 */
			nca->bytes[6] = index + 1;
			nca->bytes[7] = 0x00;
			return 0;
			}

			/* Find an outstanding VLAN tag and constuct a "Set VLAN Filter - Enable"
			* packet.
			*/
			static int set_one_vid(struct ncsi_dev_priv ndp, struct ncsi_channel nc,
			struct ncsi_cmd_arg *nca)
			{
			struct vlan_vid *vlan = NULL;
			int index = 0;

			list_for_each_entry_rcu(vlan, &ndp->vlan_vids, list) {
			index = ncsi_find_filter(nc, NCSI_FILTER_VLAN, &vlan->vid);
			if (index < 0) {
			/* New tag to add */
			netdev_printk(KERN_DEBUG, ndp->ndev.dev,
			"ncsi: new vlan id to set: %u\n",
			vlan->vid);
			break;
			}
			netdev_printk(KERN_DEBUG, ndp->ndev.dev,
			"vid %u already at filter pos %d\n",
			vlan->vid, index);
			}

			if (!vlan \|\| index >= 0) {
			netdev_printk(KERN_DEBUG, ndp->ndev.dev,
			"no vlan ids left to set\n");
			return -1;
			}

			index = ncsi_add_filter(nc, NCSI_FILTER_VLAN, &vlan->vid);
			if (index < 0) {
			netdev_err(ndp->ndev.dev,
			"Failed to add new VLAN tag, error %d\n", index);
			return -1;
			}

			netdev_printk(KERN_DEBUG, ndp->ndev.dev,
			"ncsi: set vid %u in packet, index %u\n",
			vlan->vid, index + 1);
			nca->type = NCSI_PKT_CMD_SVF;
			nca->words[1] = vlan->vid;
			/* HW filter index starts at 1 */
			nca->bytes[6] = index + 1;
			nca->bytes[7] = 0x01;

			return 0;
			}

	static void ncsi_configure_channel(struct ncsi_dev_priv *ndp)		static void ncsi_configure_channel(struct ncsi_dev_priv *ndp)
	{		{
	struct ncsi_dev *nd = &ndp->ndev;		struct ncsi_dev *nd = &ndp->ndev;
	@@ -683,8 +791,11 @@ static void ncsi_configure_channel(struct ncsi_dev_priv *ndp)
	if (ret)		if (ret)
	goto error;		goto error;

	nd->state = ncsi_dev_state_config_sma;		nd->state = ncsi_dev_state_config_clear_vids;
	break;		break;
			case ncsi_dev_state_config_clear_vids:
			case ncsi_dev_state_config_svf:
			case ncsi_dev_state_config_ev:
	case ncsi_dev_state_config_sma:		case ncsi_dev_state_config_sma:
	case ncsi_dev_state_config_ebf:		case ncsi_dev_state_config_ebf:
	#if IS_ENABLED(CONFIG_IPV6)		#if IS_ENABLED(CONFIG_IPV6)
	@@ -699,11 +810,40 @@ static void ncsi_configure_channel(struct ncsi_dev_priv *ndp)
	nca.package = np->id;		nca.package = np->id;
	nca.channel = nc->id;		nca.channel = nc->id;

			/* Clear any active filters on the channel before setting */
			if (nd->state == ncsi_dev_state_config_clear_vids) {
			ret = clear_one_vid(ndp, nc, &nca);
			if (ret) {
			nd->state = ncsi_dev_state_config_svf;
			schedule_work(&ndp->work);
			break;
			}
			/* Repeat */
			nd->state = ncsi_dev_state_config_clear_vids;
			/* Add known VLAN tags to the filter */
			} else if (nd->state == ncsi_dev_state_config_svf) {
			ret = set_one_vid(ndp, nc, &nca);
			if (ret) {
			nd->state = ncsi_dev_state_config_ev;
			schedule_work(&ndp->work);
			break;
			}
			/* Repeat */
			nd->state = ncsi_dev_state_config_svf;
			/* Enable/Disable the VLAN filter */
			} else if (nd->state == ncsi_dev_state_config_ev) {
			if (list_empty(&ndp->vlan_vids)) {
			nca.type = NCSI_PKT_CMD_DV;
			} else {
			nca.type = NCSI_PKT_CMD_EV;
			nca.bytes[3] = NCSI_CAP_VLAN_NO;
			}
			nd->state = ncsi_dev_state_config_sma;
			} else if (nd->state == ncsi_dev_state_config_sma) {
	/* Use first entry in unicast filter table. Note that		/* Use first entry in unicast filter table. Note that
	* the MAC filter table starts from entry 1 instead of		* the MAC filter table starts from entry 1 instead of
	* 0.		* 0.
	*/		*/
	if (nd->state == ncsi_dev_state_config_sma) {
	nca.type = NCSI_PKT_CMD_SMA;		nca.type = NCSI_PKT_CMD_SMA;
	for (index = 0; index < 6; index++)		for (index = 0; index < 6; index++)
	nca.bytes[index] = dev->dev_addr[index];		nca.bytes[index] = dev->dev_addr[index];
	@@ -751,6 +891,25 @@ static void ncsi_configure_channel(struct ncsi_dev_priv *ndp)
	break;		break;
	case ncsi_dev_state_config_done:		case ncsi_dev_state_config_done:
	spin_lock_irqsave(&nc->lock, flags);		spin_lock_irqsave(&nc->lock, flags);
			if (nc->reconfigure_needed) {
			/* This channel's configuration has been updated
			* part-way during the config state - start the
			* channel configuration over
			*/
			nc->reconfigure_needed = false;
			nc->state = NCSI_CHANNEL_INACTIVE;
			spin_unlock_irqrestore(&nc->lock, flags);

			spin_lock_irqsave(&ndp->lock, flags);
			list_add_tail_rcu(&nc->link, &ndp->channel_queue);
			spin_unlock_irqrestore(&ndp->lock, flags);

			netdev_printk(KERN_DEBUG, dev,
			"Dirty NCSI channel state reset\n");
			ncsi_process_next_channel(ndp);
			break;
			}

	if (nc->modes[NCSI_MODE_LINK].data[2] & 0x1) {		if (nc->modes[NCSI_MODE_LINK].data[2] & 0x1) {
	hot_nc = nc;		hot_nc = nc;
	nc->state = NCSI_CHANNEL_ACTIVE;		nc->state = NCSI_CHANNEL_ACTIVE;
	@@ -1191,6 +1350,148 @@ static struct notifier_block ncsi_inet6addr_notifier = {
	};		};
	#endif /* CONFIG_IPV6 */		#endif /* CONFIG_IPV6 */

			static int ncsi_kick_channels(struct ncsi_dev_priv *ndp)
			{
			struct ncsi_dev *nd = &ndp->ndev;
			struct ncsi_channel *nc;
			struct ncsi_package *np;
			unsigned long flags;
			unsigned int n = 0;

			NCSI_FOR_EACH_PACKAGE(ndp, np) {
			NCSI_FOR_EACH_CHANNEL(np, nc) {
			spin_lock_irqsave(&nc->lock, flags);

			/* Channels may be busy, mark dirty instead of
			* kicking if;
			* a) not ACTIVE (configured)
			* b) in the channel_queue (to be configured)
			* c) it's ndev is in the config state
			*/
			if (nc->state != NCSI_CHANNEL_ACTIVE) {
			if ((ndp->ndev.state & 0xff00) ==
			ncsi_dev_state_config \|\|
			!list_empty(&nc->link)) {
			netdev_printk(KERN_DEBUG, nd->dev,
			"ncsi: channel %p marked dirty\n",
			nc);
			nc->reconfigure_needed = true;
			}
			spin_unlock_irqrestore(&nc->lock, flags);
			continue;
			}

			spin_unlock_irqrestore(&nc->lock, flags);

			ncsi_stop_channel_monitor(nc);
			spin_lock_irqsave(&nc->lock, flags);
			nc->state = NCSI_CHANNEL_INACTIVE;
			spin_unlock_irqrestore(&nc->lock, flags);

			spin_lock_irqsave(&ndp->lock, flags);
			list_add_tail_rcu(&nc->link, &ndp->channel_queue);
			spin_unlock_irqrestore(&ndp->lock, flags);

			netdev_printk(KERN_DEBUG, nd->dev,
			"ncsi: kicked channel %p\n", nc);
			n++;
			}
			}

			return n;
			}

			int ncsi_vlan_rx_add_vid(struct net_device *dev, __be16 proto, u16 vid)
			{
			struct ncsi_channel_filter *ncf;
			struct ncsi_dev_priv *ndp;
			unsigned int n_vids = 0;
			struct vlan_vid *vlan;
			struct ncsi_dev *nd;
			bool found = false;

			if (vid == 0)
			return 0;

			nd = ncsi_find_dev(dev);
			if (!nd) {
			netdev_warn(dev, "ncsi: No net_device?\n");
			return 0;
			}

			ndp = TO_NCSI_DEV_PRIV(nd);
			ncf = ndp->hot_channel->filters[NCSI_FILTER_VLAN];

			/* Add the VLAN id to our internal list */
			list_for_each_entry_rcu(vlan, &ndp->vlan_vids, list) {
			n_vids++;
			if (vlan->vid == vid) {
			netdev_printk(KERN_DEBUG, dev,
			"vid %u already registered\n", vid);
			return 0;
			}
			}

			if (n_vids >= ncf->total) {
			netdev_info(dev,
			"NCSI Channel supports up to %u VLAN tags but %u are already set\n",
			ncf->total, n_vids);
			return -EINVAL;
			}

			vlan = kzalloc(sizeof(*vlan), GFP_KERNEL);
			if (!vlan)
			return -ENOMEM;

			vlan->proto = proto;
			vlan->vid = vid;
			list_add_rcu(&vlan->list, &ndp->vlan_vids);

			netdev_printk(KERN_DEBUG, dev, "Added new vid %u\n", vid);

			found = ncsi_kick_channels(ndp) != 0;

			return found ? ncsi_process_next_channel(ndp) : 0;
			}

			int ncsi_vlan_rx_kill_vid(struct net_device *dev, __be16 proto, u16 vid)
			{
			struct vlan_vid vlan, tmp;
			struct ncsi_dev_priv *ndp;
			struct ncsi_dev *nd;
			bool found = false;

			if (vid == 0)
			return 0;

			nd = ncsi_find_dev(dev);
			if (!nd) {
			netdev_warn(dev, "ncsi: no net_device?\n");
			return 0;
			}

			ndp = TO_NCSI_DEV_PRIV(nd);

			/* Remove the VLAN id from our internal list */
			list_for_each_entry_safe(vlan, tmp, &ndp->vlan_vids, list)
			if (vlan->vid == vid) {
			netdev_printk(KERN_DEBUG, dev,
			"vid %u found, removing\n", vid);
			list_del_rcu(&vlan->list);
			found = true;
			kfree(vlan);
			}

			if (!found) {
			netdev_err(dev, "ncsi: vid %u wasn't registered!\n", vid);
			return -EINVAL;
			}

			found = ncsi_kick_channels(ndp) != 0;

			return found ? ncsi_process_next_channel(ndp) : 0;
			}

	struct ncsi_dev ncsi_register_dev(struct net_device dev,		struct ncsi_dev ncsi_register_dev(struct net_device dev,
	void (handler)(struct ncsi_dev ndev))		void (handler)(struct ncsi_dev ndev))
	{		{
	@@ -1215,6 +1516,7 @@ struct ncsi_dev ncsi_register_dev(struct net_device dev,
	nd->handler = handler;		nd->handler = handler;
	ndp->pending_req_num = 0;		ndp->pending_req_num = 0;
	INIT_LIST_HEAD(&ndp->channel_queue);		INIT_LIST_HEAD(&ndp->channel_queue);
			INIT_LIST_HEAD(&ndp->vlan_vids);
	INIT_WORK(&ndp->work, ncsi_dev_work);		INIT_WORK(&ndp->work, ncsi_dev_work);

	/* Initialize private NCSI device */		/* Initialize private NCSI device */

net/ncsi/ncsi-rsp.c

+8 −1

Original line number	Original line	Diff line number	Diff line
	@@ -694,7 +694,14 @@ static int ncsi_rsp_handler_gc(struct ncsi_request *nr)

	ncf->index = i;		ncf->index = i;
	ncf->total = cnt;		ncf->total = cnt;
			if (i == NCSI_FILTER_VLAN) {
			/* Set VLAN filters active so they are cleared in
			* first configuration state
			*/
			ncf->bitmap = U64_MAX;
			} else {
	ncf->bitmap = 0x0ul;		ncf->bitmap = 0x0ul;
			}
	nc->filters[i] = ncf;		nc->filters[i] = ncf;
	}		}

Admin message