vlan_dev.c

来自「linux 内核源代码」· C语言 代码 · 共 734 行 · 第 1/2 页

	if (!(VLAN_DEV_INFO(dev)->flags & VLAN_FLAG_REORDER_HDR))
		build_vlan_header = 1;

	if (build_vlan_header) {
		vhdr = (struct vlan_hdr *) skb_push(skb, VLAN_HLEN);

		/* build the four bytes that make this a VLAN header. */

		/* Now, 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);

		vhdr->h_vlan_TCI = htons(veth_TCI);

		/*
		 *  Set the protocol type.
		 *  For a packet of type ETH_P_802_3 we put the length in here instead.
		 *  It is up to the 802.2 layer to carry protocol information.
		 */

		if (type != ETH_P_802_3) {
			vhdr->h_vlan_encapsulated_proto = htons(type);
		} else {
			vhdr->h_vlan_encapsulated_proto = htons(len);
		}

		skb->protocol = htons(ETH_P_8021Q);
		skb_reset_network_header(skb);
	}

	/* Before delegating work to the lower layer, enter our MAC-address */
	if (saddr == NULL)
		saddr = dev->dev_addr;

	dev = VLAN_DEV_INFO(dev)->real_dev;

	/* MPLS can send us skbuffs w/out enough space.	 This check will grow the
	 * skb if it doesn't have enough headroom.  Not a beautiful solution, so
	 * I'll tick a counter so that users can know it's happening...	 If they
	 * care...
	 */

	/* NOTE:  This may still break if the underlying device is not the final
	 * device (and thus there are more headers to add...)  It should work for
	 * good-ole-ethernet though.
	 */
	if (skb_headroom(skb) < dev->hard_header_len) {
		struct sk_buff *sk_tmp = skb;
		skb = skb_realloc_headroom(sk_tmp, dev->hard_header_len);
		kfree_skb(sk_tmp);
		if (skb == NULL) {
			struct net_device_stats *stats = vlan_dev_get_stats(vdev);
			stats->tx_dropped++;
			return -ENOMEM;
		}
		VLAN_DEV_INFO(vdev)->cnt_inc_headroom_on_tx++;
#ifdef VLAN_DEBUG
		printk(VLAN_DBG "%s: %s: had to grow skb.\n", __FUNCTION__, vdev->name);
#endif
	}

	if (build_vlan_header) {
		/* Now make the underlying real hard header */
		rc = dev_hard_header(skb, dev, ETH_P_8021Q, daddr, saddr,
				     len + VLAN_HLEN);
		if (rc > 0)
			rc += VLAN_HLEN;
		else if (rc < 0)
			rc -= VLAN_HLEN;
	} else
		/* If here, then we'll just make a normal looking ethernet frame,
		 * but, the hard_start_xmit method will insert the tag (it has to
		 * be able to do this for bridged and other skbs that don't come
		 * down the protocol stack in an orderly manner.
		 */
		rc = dev_hard_header(skb, dev, type, daddr, saddr, len);

	return rc;
}

int vlan_dev_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
{
	struct net_device_stats *stats = vlan_dev_get_stats(dev);
	struct vlan_ethhdr *veth = (struct vlan_ethhdr *)(skb->data);

	/* Handle non-VLAN frames if they are sent to us, for example by DHCP.
	 *
	 * NOTE: THIS ASSUMES DIX ETHERNET, SPECIFICALLY NOT SUPPORTING
	 * OTHER THINGS LIKE FDDI/TokenRing/802.3 SNAPs...
	 */

	if (veth->h_vlan_proto != htons(ETH_P_8021Q) ||
		VLAN_DEV_INFO(dev)->flags & VLAN_FLAG_REORDER_HDR) {
		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;
}

void vlan_dev_set_ingress_priority(const struct net_device *dev,
				   u32 skb_prio, short vlan_prio)
{
	struct vlan_dev_info *vlan = VLAN_DEV_INFO(dev);

	if (vlan->ingress_priority_map[vlan_prio & 0x7] && !skb_prio)
		vlan->nr_ingress_mappings--;
	else if (!vlan->ingress_priority_map[vlan_prio & 0x7] && skb_prio)
		vlan->nr_ingress_mappings++;

	vlan->ingress_priority_map[vlan_prio & 0x7] = skb_prio;
}

int vlan_dev_set_egress_priority(const struct net_device *dev,
				 u32 skb_prio, short vlan_prio)
{
	struct vlan_dev_info *vlan = VLAN_DEV_INFO(dev);
	struct vlan_priority_tci_mapping *mp = NULL;
	struct vlan_priority_tci_mapping *np;
	u32 vlan_qos = (vlan_prio << 13) & 0xE000;

	/* See if a priority mapping exists.. */
	mp = vlan->egress_priority_map[skb_prio & 0xF];
	while (mp) {
		if (mp->priority == skb_prio) {
			if (mp->vlan_qos && !vlan_qos)
				vlan->nr_egress_mappings--;
			else if (!mp->vlan_qos && vlan_qos)
				vlan->nr_egress_mappings++;
			mp->vlan_qos = vlan_qos;
			return 0;
		}
		mp = mp->next;
	}

	/* Create a new mapping then. */
	mp = vlan->egress_priority_map[skb_prio & 0xF];
	np = kmalloc(sizeof(struct vlan_priority_tci_mapping), GFP_KERNEL);
	if (!np)
		return -ENOBUFS;

	np->next = mp;
	np->priority = skb_prio;
	np->vlan_qos = vlan_qos;
	vlan->egress_priority_map[skb_prio & 0xF] = np;
	if (vlan_qos)
		vlan->nr_egress_mappings++;
	return 0;
}

/* Flags are defined in the vlan_flags enum in include/linux/if_vlan.h file. */
int vlan_dev_set_vlan_flag(const struct net_device *dev,
			   u32 flag, short flag_val)
{
	/* verify flag is supported */
	if (flag == VLAN_FLAG_REORDER_HDR) {
		if (flag_val) {
			VLAN_DEV_INFO(dev)->flags |= VLAN_FLAG_REORDER_HDR;
		} else {
			VLAN_DEV_INFO(dev)->flags &= ~VLAN_FLAG_REORDER_HDR;
		}
		return 0;
	}
	printk(KERN_ERR "%s: flag %i is not valid.\n", __FUNCTION__, flag);
	return -EINVAL;
}

void vlan_dev_get_realdev_name(const struct net_device *dev, char *result)
{
	strncpy(result, VLAN_DEV_INFO(dev)->real_dev->name, 23);
}

void vlan_dev_get_vid(const struct net_device *dev, unsigned short *result)
{
	*result = VLAN_DEV_INFO(dev)->vlan_id;
}

int vlan_dev_open(struct net_device *dev)
{
	struct vlan_dev_info *vlan = VLAN_DEV_INFO(dev);
	struct net_device *real_dev = vlan->real_dev;
	int err;

	if (!(real_dev->flags & IFF_UP))
		return -ENETDOWN;

	if (compare_ether_addr(dev->dev_addr, real_dev->dev_addr)) {
		err = dev_unicast_add(real_dev, dev->dev_addr, ETH_ALEN);
		if (err < 0)
			return err;
	}
	memcpy(vlan->real_dev_addr, real_dev->dev_addr, ETH_ALEN);

	if (dev->flags & IFF_ALLMULTI)
		dev_set_allmulti(real_dev, 1);
	if (dev->flags & IFF_PROMISC)
		dev_set_promiscuity(real_dev, 1);

	return 0;
}

int vlan_dev_stop(struct net_device *dev)
{
	struct net_device *real_dev = VLAN_DEV_INFO(dev)->real_dev;

	dev_mc_unsync(real_dev, dev);
	if (dev->flags & IFF_ALLMULTI)
		dev_set_allmulti(real_dev, -1);
	if (dev->flags & IFF_PROMISC)
		dev_set_promiscuity(real_dev, -1);

	if (compare_ether_addr(dev->dev_addr, real_dev->dev_addr))
		dev_unicast_delete(real_dev, dev->dev_addr, dev->addr_len);

	return 0;
}

int vlan_set_mac_address(struct net_device *dev, void *p)
{
	struct net_device *real_dev = VLAN_DEV_INFO(dev)->real_dev;
	struct sockaddr *addr = p;
	int err;

	if (!is_valid_ether_addr(addr->sa_data))
		return -EADDRNOTAVAIL;

	if (!(dev->flags & IFF_UP))
		goto out;

	if (compare_ether_addr(addr->sa_data, real_dev->dev_addr)) {
		err = dev_unicast_add(real_dev, addr->sa_data, ETH_ALEN);
		if (err < 0)
			return err;
	}

	if (compare_ether_addr(dev->dev_addr, real_dev->dev_addr))
		dev_unicast_delete(real_dev, dev->dev_addr, ETH_ALEN);

out:
	memcpy(dev->dev_addr, addr->sa_data, ETH_ALEN);
	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;
	}

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

	return err;
}

void vlan_change_rx_flags(struct net_device *dev, int change)
{
	struct net_device *real_dev = VLAN_DEV_INFO(dev)->real_dev;

	if (change & IFF_ALLMULTI)
		dev_set_allmulti(real_dev, dev->flags & IFF_ALLMULTI ? 1 : -1);
	if (change & IFF_PROMISC)
		dev_set_promiscuity(real_dev, dev->flags & IFF_PROMISC ? 1 : -1);
}

/** Taken from Gleb + Lennert's VLAN code, and modified... */
void vlan_dev_set_multicast_list(struct net_device *vlan_dev)
{
	dev_mc_sync(VLAN_DEV_INFO(vlan_dev)->real_dev, vlan_dev);
}