vlan_dev.c

《嵌入式系统设计与实例开发实验教材二源码》Linux内核移植与编译实验

	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 != __constant_htons(ETH_P_8021Q)) {
		/* This is not a VLAN frame...but we can fix that! */
		unsigned short veth_TCI = 0;
		VLAN_DEV_INFO(dev)->cnt_encap_on_xmit++;

#ifdef VLAN_DEBUG
		printk(VLAN_DBG __FUNCTION__ ": proto to encap: 0x%hx (hbo)\n",
		       htons(veth->h_vlan_proto));
#endif

		if (skb_headroom(skb) < VLAN_HLEN) {
			struct sk_buff *sk_tmp = skb;
			skb = skb_realloc_headroom(sk_tmp, VLAN_HLEN);
			kfree_skb(sk_tmp);
			if (skb == NULL) {
				stats->tx_dropped++;
				return -ENOMEM;
			}
			VLAN_DEV_INFO(dev)->cnt_inc_headroom_on_tx++;
		} else {
			if (!(skb = skb_unshare(skb, GFP_ATOMIC))) {
				printk(KERN_ERR "vlan: failed to unshare skbuff\n");
				stats->tx_dropped++;
				return -ENOMEM;
			}
		}
		veth = (struct vlan_ethhdr *)skb_push(skb, VLAN_HLEN);

		/* Move the mac addresses to the beginning of the new header. */
		memmove(skb->data, skb->data + VLAN_HLEN, 12);

		/* first, the ethernet type */
		/* put_unaligned(__constant_htons(ETH_P_8021Q), &veth->h_vlan_proto); */
		veth->h_vlan_proto = __constant_htons(ETH_P_8021Q);

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

		veth->h_vlan_TCI = htons(veth_TCI);
	}

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

#ifdef VLAN_DEBUG
	printk(VLAN_DBG __FUNCTION__ ": about to send skb: %p to dev: %s\n",
	       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

	dev_queue_xmit(skb);
	stats->tx_packets++; /* for statics only */
	stats->tx_bytes += skb->len;
	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 new_mtu;
}

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_init(struct net_device *dev)
{
	/* TODO:  figure this out, maybe do nothing?? */
	return 0;
}

void vlan_dev_destruct(struct net_device *dev)
{
	if (dev) {
		vlan_flush_mc_list(dev);
		if (dev->priv) {
			dev_put(VLAN_DEV_INFO(dev)->real_dev);
			if (VLAN_DEV_INFO(dev)->dent) {
				printk(KERN_ERR __FUNCTION__ ": dent is NOT NULL!\n");

				/* If we ever get here, there is a serious bug
				 * that must be fixed.
				 */
			}

			kfree(dev->priv);

			VLAN_FMEM_DBG("dev->priv free, addr: %p\n", dev->priv);
			dev->priv = NULL;
		}
	}
}

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;
				}
			}

			/* 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 __FUNCTION__ ": flag %i is not valid.\n",
				       (int)(flag));
				dev_put(dev);
				return -EINVAL;
			}
		} else {
			printk(KERN_ERR __FUNCTION__
			       ": %s is not a vlan device, priv_flags: %hX.\n",
			       dev->name, dev->priv_flags);
			dev_put(dev);
		}
	} else {
		printk(KERN_ERR __FUNCTION__ ": Could not find device: %s\n", dev_name);
	}

	return -EINVAL;
}

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;
}

/** 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_INFO "%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_INFO "%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));
	}
}

/** 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.
 */
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;
}

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;
	}
}

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

	while (dmi) {
		dev_mc_delete(dev, dmi->dmi_addr, dmi->dmi_addrlen, 0);
		printk(KERN_INFO "%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]);
		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;
}