From: Samuel Mendoza-Jonas Date: Mon, 28 Aug 2017 06:18:42 +0000 (+1000) Subject: net/ncsi: Configure VLAN tag filter X-Git-Url: https://git.stricted.de/?a=commitdiff_plain;h=21acf63013ed3d6fce3176cc34b74064052a31b4;p=GitHub%2Fmoto-9609%2Fandroid_kernel_motorola_exynos9610.git net/ncsi: Configure VLAN tag filter 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 Signed-off-by: David S. Miller --- 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 @@ error: 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; }