Make use of the ndo_vlan_rx_{add,kill}_vid callbacks to have the NCSI
stack process new VLAN tags and configure the channel VLAN filter
appropriately.
Several VLAN tags can be set and a "Set VLAN Filter" packet must be sent
for each one, meaning the ncsi_dev_state_config_svf state must be
repeated. An internal list of VLAN tags is maintained, and compared
against the current channel's ncsi_channel_filter in order to keep track
within the state. VLAN filters are removed in a similar manner, with the
introduction of the ncsi_dev_state_config_clear_vids state. The maximum
number of VLAN tag filters is determined by the "Get Capabilities"
response from the channel.
Signed-off-by: Samuel Mendoza-Jonas <s...@mendozajonas.com>
---
v3: - Add comment describing change to ncsi_find_filter()
- Catch NULL in clear_one_vid() from ncsi_get_filter()
- Simplify state changes when kicking updated channel
include/net/ncsi.h | 2 +
net/ncsi/internal.h | 11 ++
net/ncsi/ncsi-manage.c | 308 ++++++++++++++++++++++++++++++++++++++++++++++++-
net/ncsi/ncsi-rsp.c | 9 +-
4 files changed, 326 insertions(+), 4 deletions(-)
diff --git a/include/net/ncsi.h b/include/net/ncsi.h
index 68680baac0fd..1f96af46df49 100644
--- a/include/net/ncsi.h
+++ b/include/net/ncsi.h
@@ -28,6 +28,8 @@ struct ncsi_dev {
};
#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,
void (*notifier)(struct ncsi_dev *nd));
int ncsi_start_dev(struct ncsi_dev *nd);
diff --git a/net/ncsi/internal.h b/net/ncsi/internal.h
index 1308a56f2591..af3d636534ef 100644
--- a/net/ncsi/internal.h
+++ b/net/ncsi/internal.h
@@ -180,6 +180,7 @@ struct ncsi_channel {
#define NCSI_CHANNEL_INACTIVE 1
#define NCSI_CHANNEL_ACTIVE 2
#define NCSI_CHANNEL_INVISIBLE 3
+ bool reconfigure_needed;
spinlock_t lock; /* Protect filters etc */
struct ncsi_package *package;
struct ncsi_channel_version version;
@@ -235,6 +236,9 @@ enum {
ncsi_dev_state_probe_dp,
ncsi_dev_state_config_sp = 0x0301,
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_ebf,
#if IS_ENABLED(CONFIG_IPV6)
@@ -253,6 +257,12 @@ enum {
ncsi_dev_state_suspend_done
};
+struct vlan_vid {
+ struct list_head list;
+ __be16 proto;
+ u16 vid;
+};
+
struct ncsi_dev_priv {
struct ncsi_dev ndev; /* Associated NCSI device */
unsigned int flags; /* NCSI device flags */
@@ -276,6 +286,7 @@ struct ncsi_dev_priv {
struct work_struct work; /* For channel management */
struct packet_type ptype; /* NCSI packet Rx handler */
struct list_head node; /* Form NCSI device list */
+ struct list_head vlan_vids; /* List of active VLAN IDs */
};
struct ncsi_cmd_arg {
diff --git a/net/ncsi/ncsi-manage.c b/net/ncsi/ncsi-manage.c
index a3bd5fa8ad09..11904b3b702d 100644
--- a/net/ncsi/ncsi-manage.c
+++ b/net/ncsi/ncsi-manage.c
@@ -38,6 +38,25 @@ static inline int ncsi_filter_size(int 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)
{
struct ncsi_channel_filter *ncf;
@@ -58,7 +77,7 @@ int ncsi_find_filter(struct ncsi_channel *nc, int table, void
*data)
index = -1;
while ((index = find_next_bit(bitmap, ncf->total, index + 1))
< 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);
return index;
}
@@ -639,6 +658,95 @@ static void ncsi_suspend_channel(struct ncsi_dev_priv *ndp)
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)
{
struct ncsi_dev *nd = &ndp->ndev;
@@ -683,8 +791,11 @@ static void ncsi_configure_channel(struct ncsi_dev_priv
*ndp)
if (ret)
goto error;
- nd->state = ncsi_dev_state_config_sma;
+ nd->state = ncsi_dev_state_config_clear_vids;
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_ebf:
#if IS_ENABLED(CONFIG_IPV6)
@@ -699,11 +810,40 @@ static void ncsi_configure_channel(struct ncsi_dev_priv
*ndp)
nca.package = np->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
* the MAC filter table starts from entry 1 instead of
* 0.
*/
- if (nd->state == ncsi_dev_state_config_sma) {
nca.type = NCSI_PKT_CMD_SMA;
for (index = 0; index < 6; index++)
nca.bytes[index] = dev->dev_addr[index];
@@ -751,6 +891,25 @@ static void ncsi_configure_channel(struct ncsi_dev_priv
*ndp)
break;
case ncsi_dev_state_config_done:
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) {
hot_nc = nc;
nc->state = NCSI_CHANNEL_ACTIVE;
@@ -1191,6 +1350,148 @@ static struct notifier_block ncsi_inet6addr_notifier = {
};
#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,
void (*handler)(struct ncsi_dev *ndev))
{
@@ -1215,6 +1516,7 @@ struct ncsi_dev *ncsi_register_dev(struct net_device *dev,
nd->handler = handler;
ndp->pending_req_num = 0;
INIT_LIST_HEAD(&ndp->channel_queue);
+ INIT_LIST_HEAD(&ndp->vlan_vids);
INIT_WORK(&ndp->work, ncsi_dev_work);
/* Initialize private NCSI device */
diff --git a/net/ncsi/ncsi-rsp.c b/net/ncsi/ncsi-rsp.c
index c1a191d790e2..265b9a892d41 100644
--- a/net/ncsi/ncsi-rsp.c
+++ b/net/ncsi/ncsi-rsp.c
@@ -694,7 +694,14 @@ static int ncsi_rsp_handler_gc(struct ncsi_request *nr)
ncf->index = i;
ncf->total = cnt;
- ncf->bitmap = 0x0ul;
+ 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;
+ }
nc->filters[i] = ncf;
}
--
2.14.0
