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;
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;
}
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;
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)
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];
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;
};
#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))
{
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 */
projects / GitHub / moto-9609 / android_kernel_motorola_exynos9610.git / commitdiff