vlan_dev.c

linux 下对8021q协议的配置和实现的增强

	if (veth->h_vlan_proto != __constant_htons(ETH_P_8021Q)) {
		int orig_headroom = skb_headroom(skb);
		unsigned short veth_TCI;

		/* This is not a VLAN frame...but we can fix that! */
		VLAN_DEV_INFO(dev)->cnt_encap_on_xmit++;

#ifdef VLAN_DEBUG
		printk(VLAN_DBG "%s: proto to encap: 0x%hx (hbo)\n",
			__FUNCTION__, htons(veth->h_vlan_proto));
#endif
		/* Construct the second two bytes. This field looks something
		 * like:
		 * usr_priority: 3 bits	 (high bits)
		 * CFI		 1 bit
		 * VLAN ID	 12 bits (low bits)
		 */
		veth_TCI = VLAN_DEV_INFO(dev)->vlan_id;
		veth_TCI |= vlan_dev_get_egress_qos_mask(dev, skb);

		skb = __vlan_put_tag(skb, veth_TCI);
		if (!skb) {
			stats->tx_dropped++;
			return 0;
		}

		if (orig_headroom < VLAN_HLEN) {
			VLAN_DEV_INFO(dev)->cnt_inc_headroom_on_tx++;
		}
	}

#ifdef VLAN_DEBUG
	printk(VLAN_DBG "%s: about to send skb: %p to dev: %s\n",
		__FUNCTION__, skb, skb->dev->name);
	printk(VLAN_DBG "  %2hx.%2hx.%2hx.%2xh.%2hx.%2hx %2hx.%2hx.%2hx.%2hx.%2hx.%2hx %4hx %4hx %4hx\n",
	       veth->h_dest[0], veth->h_dest[1], veth->h_dest[2], veth->h_dest[3], veth->h_dest[4], veth->h_dest[5],
	       veth->h_source[0], veth->h_source[1], veth->h_source[2], veth->h_source[3], veth->h_source[4], veth->h_source[5],
	       veth->h_vlan_proto, veth->h_vlan_TCI, veth->h_vlan_encapsulated_proto);
#endif

	stats->tx_packets++; /* for statics only */
	stats->tx_bytes += skb->len;

	skb->dev = VLAN_DEV_INFO(dev)->real_dev;
	dev_queue_xmit(skb);

	return 0;
}

int vlan_dev_hwaccel_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
{
	struct net_device_stats *stats = vlan_dev_get_stats(dev);
	unsigned short veth_TCI;

	/* Construct the second two bytes. This field looks something
	 * like:
	 * usr_priority: 3 bits	 (high bits)
	 * CFI		 1 bit
	 * VLAN ID	 12 bits (low bits)
	 */
	veth_TCI = VLAN_DEV_INFO(dev)->vlan_id;
	veth_TCI |= vlan_dev_get_egress_qos_mask(dev, skb);
	skb = __vlan_hwaccel_put_tag(skb, veth_TCI);

	stats->tx_packets++;
	stats->tx_bytes += skb->len;

	skb->dev = VLAN_DEV_INFO(dev)->real_dev;
	dev_queue_xmit(skb);

	return 0;
}

int vlan_dev_change_mtu(struct net_device *dev, int new_mtu)
{
	/* TODO: gotta make sure the underlying layer can handle it,
	 * maybe an IFF_VLAN_CAPABLE flag for devices?
	 */
	if (VLAN_DEV_INFO(dev)->real_dev->mtu < new_mtu)
		return -ERANGE;

	dev->mtu = new_mtu;

	return 0;
}

int vlan_dev_set_ingress_priority(char *dev_name, __u32 skb_prio, short vlan_prio)
{
	struct net_device *dev = dev_get_by_name(dev_name);

	if (dev) {
		if (dev->priv_flags & IFF_802_1Q_VLAN) {
			/* see if a priority mapping exists.. */
			VLAN_DEV_INFO(dev)->ingress_priority_map[vlan_prio & 0x7] = skb_prio;
			dev_put(dev);
			return 0;
		}

		dev_put(dev);
	}
	return -EINVAL;
}

int vlan_dev_set_egress_priority(char *dev_name, __u32 skb_prio, short vlan_prio)
{
	struct net_device *dev = dev_get_by_name(dev_name);
	struct vlan_priority_tci_mapping *mp = NULL;
	struct vlan_priority_tci_mapping *np;
   
	if (dev) {
		if (dev->priv_flags & IFF_802_1Q_VLAN) {
			/* See if a priority mapping exists.. */
			mp = VLAN_DEV_INFO(dev)->egress_priority_map[skb_prio & 0xF];
			while (mp) {
				if (mp->priority == skb_prio) {
					mp->vlan_qos = ((vlan_prio << 13) & 0xE000);
					dev_put(dev);
					return 0;
				}
				mp = mp->next;
			}

			/* Create a new mapping then. */
			mp = VLAN_DEV_INFO(dev)->egress_priority_map[skb_prio & 0xF];
			np = kmalloc(sizeof(struct vlan_priority_tci_mapping), GFP_KERNEL);
			if (np) {
				np->next = mp;
				np->priority = skb_prio;
				np->vlan_qos = ((vlan_prio << 13) & 0xE000);
				VLAN_DEV_INFO(dev)->egress_priority_map[skb_prio & 0xF] = np;
				dev_put(dev);
				return 0;
			} else {
				dev_put(dev);
				return -ENOBUFS;
			}
		}
		dev_put(dev);
	}
	return -EINVAL;
}

/* Flags are defined in the vlan_dev_info class in include/linux/if_vlan.h file. */
int vlan_dev_set_vlan_flag(char *dev_name, __u32 flag, short flag_val)
{
	struct net_device *dev = dev_get_by_name(dev_name);

	if (dev) {
		if (dev->priv_flags & IFF_802_1Q_VLAN) {
			/* verify flag is supported */
			if (flag == 1) {
				if (flag_val) {
					VLAN_DEV_INFO(dev)->flags |= 1;
				} else {
					VLAN_DEV_INFO(dev)->flags &= ~1;
				}
				dev_put(dev);
				return 0;
			} else {
				printk(KERN_ERR  "%s: flag %i is not valid.\n",
					__FUNCTION__, (int)(flag));
				dev_put(dev);
				return -EINVAL;
			}
		} else {
			printk(KERN_ERR 
			       "%s: %s is not a vlan device, priv_flags: %hX.\n",
			       __FUNCTION__, dev->name, dev->priv_flags);
			dev_put(dev);
		}
	} else {
		printk(KERN_ERR  "%s: Could not find device: %s\n", 
			__FUNCTION__, dev_name);
	}

	return -EINVAL;
}


int vlan_dev_get_realdev_name(const char *dev_name, char* result)
{
	struct net_device *dev = dev_get_by_name(dev_name);
	int rv = 0;
	if (dev) {
		if (dev->priv_flags & IFF_802_1Q_VLAN) {
			strncpy(result, VLAN_DEV_INFO(dev)->real_dev->name, 23);
			rv = 0;
		} else {
			rv = -EINVAL;
		}
		dev_put(dev);
	} else {
		rv = -ENODEV;
	}
	return rv;
}

int vlan_dev_get_vid(const char *dev_name, unsigned short* result)
{
	struct net_device *dev = dev_get_by_name(dev_name);
	int rv = 0;
	if (dev) {
		if (dev->priv_flags & IFF_802_1Q_VLAN) {
			*result = VLAN_DEV_INFO(dev)->vlan_id;
			rv = 0;
		} else {
			rv = -EINVAL;
		}
		dev_put(dev);
	} else {
		rv = -ENODEV;
	}
	return rv;
}


int vlan_dev_set_mac_address(struct net_device *dev, void *addr_struct_p)
{
	struct sockaddr *addr = (struct sockaddr *)(addr_struct_p);
	int i;

	if (netif_running(dev))
		return -EBUSY;

	memcpy(dev->dev_addr, addr->sa_data, dev->addr_len);

	printk("%s: Setting MAC address to ", dev->name);
	for (i = 0; i < 6; i++)
		printk(" %2.2x", dev->dev_addr[i]);
	printk(".\n");

	if (memcmp(VLAN_DEV_INFO(dev)->real_dev->dev_addr,
		   dev->dev_addr,
		   dev->addr_len) != 0) {
		if (!(VLAN_DEV_INFO(dev)->real_dev->flags & IFF_PROMISC)) {
			int flgs = VLAN_DEV_INFO(dev)->real_dev->flags;

			/* Increment our in-use promiscuity counter */
			dev_set_promiscuity(VLAN_DEV_INFO(dev)->real_dev, 1);

			/* Make PROMISC visible to the user. */
			flgs |= IFF_PROMISC;
			printk("VLAN (%s):  Setting underlying device (%s) to promiscious mode.\n",
			       dev->name, VLAN_DEV_INFO(dev)->real_dev->name);
			dev_change_flags(VLAN_DEV_INFO(dev)->real_dev, flgs);
		}
	} else {
		printk("VLAN (%s):  Underlying device (%s) has same MAC, not checking promiscious mode.\n",
		       dev->name, VLAN_DEV_INFO(dev)->real_dev->name);
	}

	return 0;
}

static inline int vlan_dmi_equals(struct dev_mc_list *dmi1,
                                  struct dev_mc_list *dmi2)
{
	return ((dmi1->dmi_addrlen == dmi2->dmi_addrlen) &&
		(memcmp(dmi1->dmi_addr, dmi2->dmi_addr, dmi1->dmi_addrlen) == 0));
}

/** dmi is a single entry into a dev_mc_list, a single node.  mc_list is
 *  an entire list, and we'll iterate through it.
 */
static int vlan_should_add_mc(struct dev_mc_list *dmi, struct dev_mc_list *mc_list)
{
	struct dev_mc_list *idmi;

	for (idmi = mc_list; idmi != NULL; ) {
		if (vlan_dmi_equals(dmi, idmi)) {
			if (dmi->dmi_users > idmi->dmi_users)
				return 1;
			else
				return 0;
		} else {
			idmi = idmi->next;
		}
	}

	return 1;
}

static inline void vlan_destroy_mc_list(struct dev_mc_list *mc_list)
{
	struct dev_mc_list *dmi = mc_list;
	struct dev_mc_list *next;

	while(dmi) {
		next = dmi->next;
		kfree(dmi);
		dmi = next;
	}
}

static void vlan_copy_mc_list(struct dev_mc_list *mc_list, struct vlan_dev_info *vlan_info)
{
	struct dev_mc_list *dmi, *new_dmi;

	vlan_destroy_mc_list(vlan_info->old_mc_list);
	vlan_info->old_mc_list = NULL;

	for (dmi = mc_list; dmi != NULL; dmi = dmi->next) {
		new_dmi = kmalloc(sizeof(*new_dmi), GFP_ATOMIC);
		if (new_dmi == NULL) {
			printk(KERN_ERR "vlan: cannot allocate memory. "
			       "Multicast may not work properly from now.\n");
			return;
		}

		/* Copy whole structure, then make new 'next' pointer */
		*new_dmi = *dmi;
		new_dmi->next = vlan_info->old_mc_list;
		vlan_info->old_mc_list = new_dmi;
	}
}

static void vlan_flush_mc_list(struct net_device *dev)
{
	struct dev_mc_list *dmi = dev->mc_list;

	while (dmi) {
		printk(KERN_DEBUG "%s: del %.2x:%.2x:%.2x:%.2x:%.2x:%.2x mcast address from vlan interface\n",
		       dev->name,
		       dmi->dmi_addr[0],
		       dmi->dmi_addr[1],
		       dmi->dmi_addr[2],
		       dmi->dmi_addr[3],
		       dmi->dmi_addr[4],
		       dmi->dmi_addr[5]);
		dev_mc_delete(dev, dmi->dmi_addr, dmi->dmi_addrlen, 0);
		dmi = dev->mc_list;
	}

	/* dev->mc_list is NULL by the time we get here. */
	vlan_destroy_mc_list(VLAN_DEV_INFO(dev)->old_mc_list);
	VLAN_DEV_INFO(dev)->old_mc_list = NULL;
}

int vlan_dev_open(struct net_device *dev)
{
	if (!(VLAN_DEV_INFO(dev)->real_dev->flags & IFF_UP))
		return -ENETDOWN;

	return 0;
}

int vlan_dev_stop(struct net_device *dev)
{
	vlan_flush_mc_list(dev);
	return 0;
}

int vlan_dev_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
{
	struct net_device *real_dev = VLAN_DEV_INFO(dev)->real_dev;
	struct ifreq ifrr;
	int err = -EOPNOTSUPP;

	strncpy(ifrr.ifr_name, real_dev->name, IFNAMSIZ);
	ifrr.ifr_ifru = ifr->ifr_ifru;

	switch(cmd) {
	case SIOCGMIIPHY:
	case SIOCGMIIREG:
	case SIOCSMIIREG:
		if (real_dev->do_ioctl && netif_device_present(real_dev)) 
			err = real_dev->do_ioctl(real_dev, &ifrr, cmd);
		break;

	case SIOCETHTOOL:
		err = dev_ethtool(&ifrr);
	}

	if (!err) 
		ifr->ifr_ifru = ifrr.ifr_ifru;

	return err;
}

/** Taken from Gleb + Lennert's VLAN code, and modified... */
void vlan_dev_set_multicast_list(struct net_device *vlan_dev)
{
	struct dev_mc_list *dmi;
	struct net_device *real_dev;
	int inc;

	if (vlan_dev && (vlan_dev->priv_flags & IFF_802_1Q_VLAN)) {
		/* Then it's a real vlan device, as far as we can tell.. */
		real_dev = VLAN_DEV_INFO(vlan_dev)->real_dev;

		/* compare the current promiscuity to the last promisc we had.. */
		inc = vlan_dev->promiscuity - VLAN_DEV_INFO(vlan_dev)->old_promiscuity;
		if (inc) {
			printk(KERN_INFO "%s: dev_set_promiscuity(master, %d)\n",
			       vlan_dev->name, inc);
			dev_set_promiscuity(real_dev, inc); /* found in dev.c */
			VLAN_DEV_INFO(vlan_dev)->old_promiscuity = vlan_dev->promiscuity;
		}

		inc = vlan_dev->allmulti - VLAN_DEV_INFO(vlan_dev)->old_allmulti;
		if (inc) {
			printk(KERN_INFO "%s: dev_set_allmulti(master, %d)\n",
			       vlan_dev->name, inc);
			dev_set_allmulti(real_dev, inc); /* dev.c */
			VLAN_DEV_INFO(vlan_dev)->old_allmulti = vlan_dev->allmulti;
		}

		/* looking for addresses to add to master's list */
		for (dmi = vlan_dev->mc_list; dmi != NULL; dmi = dmi->next) {
			if (vlan_should_add_mc(dmi, VLAN_DEV_INFO(vlan_dev)->old_mc_list)) {
				dev_mc_add(real_dev, dmi->dmi_addr, dmi->dmi_addrlen, 0);
				printk(KERN_DEBUG "%s: add %.2x:%.2x:%.2x:%.2x:%.2x:%.2x mcast address to master interface\n",
				       vlan_dev->name,
				       dmi->dmi_addr[0],
				       dmi->dmi_addr[1],
				       dmi->dmi_addr[2],
				       dmi->dmi_addr[3],
				       dmi->dmi_addr[4],
				       dmi->dmi_addr[5]);
			}
		}

		/* looking for addresses to delete from master's list */
		for (dmi = VLAN_DEV_INFO(vlan_dev)->old_mc_list; dmi != NULL; dmi = dmi->next) {
			if (vlan_should_add_mc(dmi, vlan_dev->mc_list)) {
				/* if we think we should add it to the new list, then we should really
				 * delete it from the real list on the underlying device.
				 */
				dev_mc_delete(real_dev, dmi->dmi_addr, dmi->dmi_addrlen, 0);
				printk(KERN_DEBUG "%s: del %.2x:%.2x:%.2x:%.2x:%.2x:%.2x mcast address from master interface\n",
				       vlan_dev->name,
				       dmi->dmi_addr[0],
				       dmi->dmi_addr[1],
				       dmi->dmi_addr[2],
				       dmi->dmi_addr[3],
				       dmi->dmi_addr[4],
				       dmi->dmi_addr[5]);
			}
		}

		/* save multicast list */
		vlan_copy_mc_list(vlan_dev->mc_list, VLAN_DEV_INFO(vlan_dev));
	}
}