[PATCH 04/16] net/stack: add support for VLAN filtering

Philippe Gerum rpm at xenomai.org
Thu Mar 21 18:13:18 CET 2019


From: Gilles Chanteperdrix <gilles.chanteperdrix at xenomai.org>

Signed-off-by: Philippe Gerum <rpm at xenomai.org>
---
 kernel/drivers/net/addons/cap.c               |  47 +-
 kernel/drivers/net/stack/Kconfig              |   7 +
 kernel/drivers/net/stack/Makefile             |   2 +
 kernel/drivers/net/stack/include/rtdev.h      |  19 +
 kernel/drivers/net/stack/include/rtif_vlan.h  | 105 +++
 kernel/drivers/net/stack/include/rtnet_port.h |   6 +-
 kernel/drivers/net/stack/include/rtskb.h      |   1 +
 kernel/drivers/net/stack/include/rtvlan.h     |  25 +
 kernel/drivers/net/stack/rtdev.c              |  32 +-
 kernel/drivers/net/stack/rtnet_chrdev.c       |   7 +-
 kernel/drivers/net/stack/rtnet_module.c       |   5 +
 kernel/drivers/net/stack/socket.c             |  16 +-
 kernel/drivers/net/stack/vlan.c               | 725 ++++++++++++++++++
 13 files changed, 967 insertions(+), 30 deletions(-)
 create mode 100644 kernel/drivers/net/stack/include/rtif_vlan.h
 create mode 100644 kernel/drivers/net/stack/include/rtvlan.h
 create mode 100644 kernel/drivers/net/stack/vlan.c

diff --git a/kernel/drivers/net/addons/cap.c b/kernel/drivers/net/addons/cap.c
index b901bedcf..e28f3dd0c 100644
--- a/kernel/drivers/net/addons/cap.c
+++ b/kernel/drivers/net/addons/cap.c
@@ -57,6 +57,25 @@ static struct tap_device_t {
 
 void rtcap_rx_hook(struct rtskb *rtskb)
 {
+	int                     ifindex;
+	int                     active;
+
+	if (rtskb->cap_flags & RTSKB_CAP_SHARED)
+		return;
+
+	ifindex = rtskb->rtdev->ifindex;
+	active  = tap_device[ifindex].present & (TAP_DEV | RTMAC_TAP_DEV);
+
+	if ((active & TAP_DEV)
+		&& !(tap_device[ifindex].tap_dev->flags & IFF_UP))
+		active &= ~TAP_DEV;
+	if ((active & RTMAC_TAP_DEV)
+		&& !(tap_device[ifindex].rtmac_tap_dev->flags & IFF_UP))
+		active &= ~RTMAC_TAP_DEV;
+
+	if (active == 0)
+		return;
+
 	if ((rtskb->cap_comp_skb = rtskb_pool_dequeue(&cap_pool)) == 0) {
 		tap_device[rtskb->rtdev->ifindex].tap_dev_stats.rx_dropped++;
 		return;
@@ -70,6 +89,7 @@ void rtcap_rx_hook(struct rtskb *rtskb)
 	rtskb->cap_next = NULL;
 
 	rtskb->cap_flags |= RTSKB_CAP_SHARED;
+	rtskb->cap_dev = rtskb->rtdev;
 
 	rtdm_nrtsig_pend(&cap_signal);
 }
@@ -78,16 +98,24 @@ int rtcap_xmit_hook(struct rtskb *rtskb, struct rtnet_device *rtdev)
 {
 	struct tap_device_t *tap_dev = &tap_device[rtskb->rtdev->ifindex];
 	rtdm_lockctx_t context;
+	int active;
 
-	if ((rtskb->cap_comp_skb = rtskb_pool_dequeue(&cap_pool)) == 0) {
+	active = tap_dev->present & XMIT_HOOK;
+	if (active && !(tap_dev->tap_dev->flags & IFF_UP))
+		active &= ~XMIT_HOOK;
+
+	if (!active
+		|| (rtskb->cap_flags & RTSKB_CAP_SHARED)
+		|| (rtskb->cap_comp_skb = rtskb_pool_dequeue(&cap_pool)) == 0) {
 		tap_dev->tap_dev_stats.rx_dropped++;
-		return tap_dev->orig_xmit(rtskb, rtdev);
+		goto done;
 	}
 
 	rtskb->cap_next = NULL;
 	rtskb->cap_start = rtskb->data;
 	rtskb->cap_len = rtskb->len;
 	rtskb->cap_flags |= RTSKB_CAP_SHARED;
+	rtskb->cap_dev = rtdev;
 
 	rtskb->time_stamp = rtdm_clock_read();
 
@@ -102,7 +130,7 @@ int rtcap_xmit_hook(struct rtskb *rtskb, struct rtnet_device *rtdev)
 	rtdm_lock_put_irqrestore(&rtcap_lock, context);
 
 	rtdm_nrtsig_pend(&cap_signal);
-
+done:
 	return tap_dev->orig_xmit(rtskb, rtdev);
 }
 
@@ -174,7 +202,7 @@ static void rtcap_signal_handler(rtdm_nrtsig_t * nrtsig, void *arg)
 
 		rtdm_lock_put_irqrestore(&rtcap_lock, context);
 
-		ifindex = rtskb->rtdev->ifindex;
+		ifindex = rtskb->cap_dev->ifindex;
 		active = tap_device[ifindex].present;
 
 		if (active) {
@@ -332,6 +360,7 @@ void cleanup_tap_devices(void)
 
 			unregister_netdev(tap_device[i].tap_dev);
 			free_netdev(tap_device[i].tap_dev);
+			tap_device[i].present = 0;
 		}
 }
 
@@ -474,19 +503,21 @@ void rtcap_cleanup(void)
 {
 	rtdm_lockctx_t context;
 
-	rtdm_nrtsig_destroy(&cap_signal);
-
 	/* unregister capturing handlers
 	 * (take lock to avoid any unloading code before handler was left) */
 	rtdm_lock_get_irqsave(&rtcap_lock, context);
 	rtcap_handler = NULL;
 	rtdm_lock_put_irqrestore(&rtcap_lock, context);
 
+	cleanup_tap_devices();
+
+	msleep(10);
+
+	rtdm_nrtsig_destroy(&cap_signal);
+
 	/* empty queue (should be already empty) */
 	rtcap_signal_handler(0, NULL /* we ignore them anyway */ );
 
-	cleanup_tap_devices();
-
 	rtskb_pool_release(&cap_pool);
 
 	printk("RTcap: unloaded\n");
diff --git a/kernel/drivers/net/stack/Kconfig b/kernel/drivers/net/stack/Kconfig
index 830cec5ad..47a6f290d 100644
--- a/kernel/drivers/net/stack/Kconfig
+++ b/kernel/drivers/net/stack/Kconfig
@@ -31,6 +31,13 @@ config XENO_DRIVERS_NET_RTWLAN
     on low-level access to 802.11-compliant adapters and is currently
     in an experimental stage.
 
+config XENO_DRIVERS_NET_VLAN
+    depends on XENO_DRIVERS_NET
+    bool "Real-Time VLAN"
+    select XENO_DRIVERS_NET_RTIPV4_IGMP
+    ---help---
+    Enables core support for real-time VLAN.
+
 comment "Protocols"
 
 source "drivers/xenomai/net/stack/ipv4/Kconfig"
diff --git a/kernel/drivers/net/stack/Makefile b/kernel/drivers/net/stack/Makefile
index d055dc2af..8873f7022 100644
--- a/kernel/drivers/net/stack/Makefile
+++ b/kernel/drivers/net/stack/Makefile
@@ -24,3 +24,5 @@ rtnet-y :=  \
 	eth.o
 
 rtnet-$(CONFIG_XENO_DRIVERS_NET_RTWLAN) += rtwlan.o
+
+rtnet-$(CONFIG_XENO_DRIVERS_NET_VLAN) += vlan.o
diff --git a/kernel/drivers/net/stack/include/rtdev.h b/kernel/drivers/net/stack/include/rtdev.h
index fe015c338..65c7ed3dc 100644
--- a/kernel/drivers/net/stack/include/rtdev.h
+++ b/kernel/drivers/net/stack/include/rtdev.h
@@ -153,6 +153,13 @@ struct rtnet_device {
 	int (*hard_start_xmit) (struct rtskb * skb, struct rtnet_device * dev);
 	int (*hw_reset) (struct rtnet_device * rtdev);
 	void (*set_multicast_list) (struct rtnet_device *rtdev);
+	void (*vlan_rx_add_vid)(struct rtnet_device *dev,
+				unsigned short vid);
+	void (*vlan_rx_kill_vid)(struct rtnet_device *dev,
+				unsigned short vid);
+#ifdef CONFIG_XENO_DRIVERS_NET_VLAN
+	struct list_head	vlan_link;
+#endif
 
 	/* Transmission hook, managed by the stack core, RTcap, and RTmac
 	 *
@@ -191,6 +198,18 @@ extern struct list_head event_hook_list;
 extern struct mutex rtnet_devices_nrt_lock;
 extern struct rtnet_device *rtnet_devices[];
 
+#ifdef CONFIG_XENO_DRIVERS_NET_VLAN
+extern struct rtnet_device *
+__rtdev_real_dev(struct rtnet_device *dev) __attribute__((pure));
+#else
+static inline struct rtnet_device *__rtdev_real_dev(struct rtnet_device *dev)
+{
+	return dev;
+}
+#endif
+#define rtdev_mc_list(dev) __rtdev_real_dev(dev)->mc_list
+#define rtdev_mc_count(dev) __rtdev_real_dev(dev)->mc_count
+
 int __rt_init_etherdev(struct rtnet_device *rtdev,
 		       unsigned int dev_pool_size, struct module *module);
 
diff --git a/kernel/drivers/net/stack/include/rtif_vlan.h b/kernel/drivers/net/stack/include/rtif_vlan.h
new file mode 100644
index 000000000..d1cc9fa4b
--- /dev/null
+++ b/kernel/drivers/net/stack/include/rtif_vlan.h
@@ -0,0 +1,105 @@
+/*
+ * VLAN		An implementation of 802.1Q VLAN tagging.
+ *
+ * Authors:	Ben Greear <greearb at candelatech.com>
+ *
+ *		This program is free software; you can redistribute it and/or
+ *		modify it under the terms of the GNU General Public License
+ *		as published by the Free Software Foundation; either version
+ *		2 of the License, or (at your option) any later version.
+ *
+ */
+#ifndef _RTNET_IF_VLAN_H_
+#define _RTNET_IF_VLAN_H_
+
+#include <rtdev.h>
+#include <rtskb.h>
+#include <uapi/linux/if_vlan.h>
+#include <linux/if_vlan.h>
+
+/* found in socket.c */
+extern void rtvlan_ioctl_set(int (*hook)(struct net *, void __user *));
+
+static inline bool is_rtvlan_dev(struct rtnet_device *dev)
+{
+	return !!(dev->priv_flags & IFF_802_1Q_VLAN);
+}
+
+struct rtvlan_pcpu_stats {
+	u64			rx_packets;
+	u64			rx_bytes;
+	u64			rx_multicast;
+	u64			tx_packets;
+	u64			tx_bytes;
+	seqcount_t		syncp;
+	u32			rx_errors;
+	u32			tx_dropped;
+};
+
+struct rtvlan_dev_priv {
+	unsigned int				nr_ingress_mappings;
+	u32					ingress_priority_map[8];
+	unsigned int				nr_egress_mappings;
+	struct vlan_priority_tci_mapping	*egress_priority_map[16];
+
+	__be16					vlan_proto;
+	u16					vlan_id;
+	u16					flags;
+
+	struct rtnet_device			*real_dev;
+	unsigned char				real_dev_addr[ETH_ALEN];
+
+	struct rtvlan_pcpu_stats __percpu	*vlan_pcpu_stats;
+	struct net_device_stats			stats;
+	unsigned int				nest_level;
+};
+
+static inline struct rtvlan_dev_priv *rtvlan_dev_priv(const struct rtnet_device *dev)
+{
+	return dev->priv;
+}
+
+static inline u16
+rtvlan_dev_get_egress_qos_mask(struct rtnet_device *dev, u32 skprio)
+{
+	struct vlan_priority_tci_mapping *mp;
+
+	smp_rmb(); /* coupled with smp_wmb() in rtvlan_dev_set_egress_priority() */
+
+	mp = rtvlan_dev_priv(dev)->egress_priority_map[(skprio & 0xF)];
+	while (mp) {
+		if (mp->priority == skprio) {
+			return mp->vlan_qos; /* This should already be shifted
+					      * to mask correctly with the
+					      * VLAN's TCI */
+		}
+		mp = mp->next;
+	}
+	return 0;
+}
+
+static inline void
+rtvlan_insert_tag(struct rtskb *skb, __be16 vlan_proto, u16 vlan_tci)
+{
+	struct vlan_ethhdr *veth;
+
+	veth = (struct vlan_ethhdr *)rtskb_push(skb, VLAN_HLEN);
+
+	/* Move the mac addresses to the beginning of the new header. */
+	memmove(skb->data, skb->data + VLAN_HLEN, 2 * ETH_ALEN);
+
+	/* first, the ethernet type */
+	veth->h_vlan_proto = vlan_proto;
+
+	/* now, the TCI */
+	veth->h_vlan_TCI = htons(vlan_tci);
+}
+
+static inline void
+rtvlan_put_tag(struct rtskb *skb, __be16 vlan_proto, u16 vlan_tci)
+{
+	rtvlan_insert_tag(skb, vlan_proto, vlan_tci);
+	skb->protocol = vlan_proto;
+}
+
+#endif /* !(_RTNET_IF_VLAN_H_) */
diff --git a/kernel/drivers/net/stack/include/rtnet_port.h b/kernel/drivers/net/stack/include/rtnet_port.h
index 37f907027..9f7e835a7 100644
--- a/kernel/drivers/net/stack/include/rtnet_port.h
+++ b/kernel/drivers/net/stack/include/rtnet_port.h
@@ -58,12 +58,12 @@ static inline int rtnetif_queue_stopped(struct rtnet_device *rtdev)
 
 static inline int rtnetif_running(struct rtnet_device *rtdev)
 {
-	return test_bit(__RTNET_LINK_STATE_START, &rtdev->link_state);
+	return test_bit(__RTNET_LINK_STATE_START, &__rtdev_real_dev(rtdev)->link_state);
 }
 
 static inline int rtnetif_device_present(struct rtnet_device *rtdev)
 {
-	return test_bit(__RTNET_LINK_STATE_PRESENT, &rtdev->link_state);
+	return test_bit(__RTNET_LINK_STATE_PRESENT, &__rtdev_real_dev(rtdev)->link_state);
 }
 
 static inline void rtnetif_device_detach(struct rtnet_device *rtdev)
@@ -99,7 +99,7 @@ static inline void rtnetif_carrier_off(struct rtnet_device *rtdev)
 
 static inline int rtnetif_carrier_ok(struct rtnet_device *rtdev)
 {
-	return !test_bit(__RTNET_LINK_STATE_NOCARRIER, &rtdev->link_state);
+	return !test_bit(__RTNET_LINK_STATE_NOCARRIER, &__rtdev_real_dev(rtdev)->link_state);
 }
 
 #define NIPQUAD(addr) \
diff --git a/kernel/drivers/net/stack/include/rtskb.h b/kernel/drivers/net/stack/include/rtskb.h
index 373dfedc5..36445ad08 100644
--- a/kernel/drivers/net/stack/include/rtskb.h
+++ b/kernel/drivers/net/stack/include/rtskb.h
@@ -214,6 +214,7 @@ struct rtskb {
 	unsigned char *cap_start;	/* start offset for capturing           */
 	unsigned int cap_len;	/* capture length of this rtskb         */
 	nanosecs_abs_t cap_rtmac_stamp;	/* RTmac enqueuing time            */
+        struct rtnet_device *cap_dev;	 /* Captured interface */
 #endif
 
 	struct list_head entry;	/* for global rtskb list */
diff --git a/kernel/drivers/net/stack/include/rtvlan.h b/kernel/drivers/net/stack/include/rtvlan.h
new file mode 100644
index 000000000..a6fd3df91
--- /dev/null
+++ b/kernel/drivers/net/stack/include/rtvlan.h
@@ -0,0 +1,25 @@
+#ifndef RTNET_RTVLAN_H
+#define RTNET_RTVLAN_H
+
+#ifdef CONFIG_XENO_DRIVERS_NET_VLAN
+void rtvlan_proto_init(void);
+
+void rtvlan_proto_release(void);
+
+int rtvlan_ioctl_handler(void __user *arg);
+#else
+static inline void rtvlan_proto_init(void)
+{
+}
+
+static inline void rtvlan_proto_release(void)
+{
+}
+
+static inline int rtvlan_ioctl_handler(void __user *arg)
+{
+	return -ENOSYS;
+}
+#endif
+
+#endif /* RTNET_RTVLAN_H */
diff --git a/kernel/drivers/net/stack/rtdev.c b/kernel/drivers/net/stack/rtdev.c
index e577d8147..a1bca2456 100644
--- a/kernel/drivers/net/stack/rtdev.c
+++ b/kernel/drivers/net/stack/rtdev.c
@@ -329,6 +329,9 @@ static void init_etherdev(struct rtnet_device *rtdev, struct module *module)
 	rtdev->flags = IFF_BROADCAST;	/* TODO: IFF_MULTICAST; */
 	rtdev->get_mtu = rt_hard_mtu;
 	rtdev->rt_owner = module;
+#ifdef CONFIG_XENO_DRIVERS_NET_VLAN
+	INIT_LIST_HEAD(&rtdev->vlan_link);
+#endif
 
 	memset(rtdev->broadcast, 0xFF, ETH_ALEN);
 	strcpy(rtdev->name, "rteth%d");
@@ -528,6 +531,15 @@ int rt_register_rtnetdev(struct rtnet_device *rtdev)
 	if (rtdev->vers < RTDEV_VERS_2_0)
 		return -EINVAL;
 
+#ifdef CONFIG_XENO_DRIVERS_NET_VLAN
+	if ((rtdev->features & NETIF_F_HW_VLAN_CTAG_FILTER) &&
+	    (!rtdev->vlan_rx_add_vid || !rtdev->vlan_rx_kill_vid)) {
+	    printk("RTnet: %s has VLAN hardware filtering but does not provide "
+		    "callbacks\n", rtdev->name);
+	    return -EINVAL;
+	}
+#endif
+
 	if (rtdev->features & NETIF_F_LLTX)
 		rtdev->start_xmit = rtdev->hard_start_xmit;
 	else
@@ -1005,7 +1017,7 @@ int rt_dev_mc_delete(struct rtnet_device *dev, void *addr, int alen, int glbl)
 
     rtdm_lock_get_irqsave(&dev->rtdev_lock, flags);
 
-    for (dmip = &dev->mc_list; (dmi = *dmip) != NULL; dmip = &dmi->next) {
+    for (dmip = &rtdev_mc_list(dev); (dmi = *dmip) != NULL; dmip = &dmi->next) {
         /*
          *  Find the entry we want to delete. The device could
          *  have variable length entries so check these too.
@@ -1025,7 +1037,7 @@ int rt_dev_mc_delete(struct rtnet_device *dev, void *addr, int alen, int glbl)
              *  Last user. So delete the entry.
              */
             *dmip = dmi->next;
-            dev->mc_count--;
+            rtdev_mc_count(dev)--;
 
             /*
              *  We have altered the list, so the card
@@ -1060,7 +1072,7 @@ int rt_dev_mc_add(struct rtnet_device *dev, void *addr, int alen, int glbl)
     dmi1 = rtdm_malloc(sizeof(*dmi1));
 
     rtdm_lock_get_irqsave(&dev->rtdev_lock, flags);
-    for (dmi = dev->mc_list; dmi != NULL; dmi = dmi->next) {
+    for (dmi = rtdev_mc_list(dev); dmi != NULL; dmi = dmi->next) {
         if (memcmp(dmi->dmi_addr, addr, dmi->dmi_addrlen) == 0 &&
             dmi->dmi_addrlen == alen) {
             if (glbl) {
@@ -1080,11 +1092,11 @@ int rt_dev_mc_add(struct rtnet_device *dev, void *addr, int alen, int glbl)
     }
     memcpy(dmi->dmi_addr, addr, alen);
     dmi->dmi_addrlen = alen;
-    dmi->next = dev->mc_list;
+    dmi->next = rtdev_mc_list(dev);
     dmi->dmi_users = 1;
     dmi->dmi_gusers = glbl ? 1 : 0;
-    dev->mc_list = dmi;
-    dev->mc_count++;
+    rtdev_mc_list(dev) = dmi;
+    rtdev_mc_count(dev)++;
 
     __rt_dev_mc_upload(dev);
 
@@ -1109,16 +1121,16 @@ void rt_dev_mc_discard(struct rtnet_device *dev)
 
     rtdm_lock_get_irqsave(&dev->rtdev_lock, flags);
 
-    while (dev->mc_list != NULL) {
-        struct rtdev_mc_list *tmp = dev->mc_list;
-        dev->mc_list = tmp->next;
+    while (rtdev_mc_list(dev) != NULL) {
+        struct rtdev_mc_list *tmp = rtdev_mc_list(dev);
+        rtdev_mc_list(dev) = tmp->next;
         if (tmp->dmi_users > tmp->dmi_gusers)
             printk("dev_mc_discard: multicast leakage! dmi_users=%d\n", tmp->dmi_users);
         rtdm_lock_put_irqrestore(&dev->rtdev_lock, flags);
         rtdm_free(tmp);
         rtdm_lock_get_irqsave(&dev->rtdev_lock, flags);
     }
-    dev->mc_count = 0;
+    rtdev_mc_count(dev) = 0;
 
     rtdm_lock_put_irqrestore(&dev->rtdev_lock, flags);
 }
diff --git a/kernel/drivers/net/stack/rtnet_chrdev.c b/kernel/drivers/net/stack/rtnet_chrdev.c
index d9a37e5a2..88bceaab8 100644
--- a/kernel/drivers/net/stack/rtnet_chrdev.c
+++ b/kernel/drivers/net/stack/rtnet_chrdev.c
@@ -30,6 +30,7 @@
 #include <linux/netdevice.h>
 #include <linux/spinlock.h>
 
+#include <rtnet_port.h>
 #include <rtnet_chrdev.h>
 #include <rtnet_internal.h>
 #include <ipv4/route.h>
@@ -126,10 +127,8 @@ static int rtnet_core_ioctl(struct rtnet_device *rtdev, unsigned int request,
 		cmd.args.info.mtu = rtdev->mtu;
 		cmd.args.info.flags = rtdev->flags;
 		if ((cmd.args.info.flags & IFF_UP)
-		    && (rtdev->link_state
-			& (RTNET_LINK_STATE_PRESENT
-			   | RTNET_LINK_STATE_NOCARRIER))
-		    == RTNET_LINK_STATE_PRESENT)
+		    && rtnetif_carrier_ok(rtdev)
+		    && rtnetif_device_present(rtdev))
 			cmd.args.info.flags |= IFF_RUNNING;
 
 		memcpy(cmd.args.info.dev_addr, rtdev->dev_addr, MAX_ADDR_LEN);
diff --git a/kernel/drivers/net/stack/rtnet_module.c b/kernel/drivers/net/stack/rtnet_module.c
index ed6037146..c0b7ff7f1 100644
--- a/kernel/drivers/net/stack/rtnet_module.c
+++ b/kernel/drivers/net/stack/rtnet_module.c
@@ -34,6 +34,7 @@
 #include <rtnet_rtpc.h>
 #include <stack_mgr.h>
 #include <rtwlan.h>
+#include <rtvlan.h>
 
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("RTnet stack core");
@@ -353,6 +354,8 @@ int __init rtnet_init(void)
 	if ((err = rtpc_init()) != 0)
 		goto err_out6;
 
+	rtvlan_proto_init();
+
 	rtnet_corectl_register();
 
 	return 0;
@@ -388,6 +391,8 @@ void __exit rtnet_release(void)
 {
 	rtnet_corectl_unregister();
 
+	rtvlan_proto_release();
+
 	rtpc_cleanup();
 
 	rtwlan_exit();
diff --git a/kernel/drivers/net/stack/socket.c b/kernel/drivers/net/stack/socket.c
index 832c96f00..92e33ce46 100644
--- a/kernel/drivers/net/stack/socket.c
+++ b/kernel/drivers/net/stack/socket.c
@@ -36,6 +36,8 @@
 #include <rtnet_internal.h>
 #include <rtnet_iovec.h>
 #include <rtnet_socket.h>
+#include <rtvlan.h>
+#include <rtnet_port.h>
 #include <ipv4/protocol.h>
 
 #define SKB_POOL_CLOSED     0
@@ -246,7 +248,8 @@ int rt_socket_if_ioctl(struct rtdm_fd *fd, int request, void __user * arg)
 	int ret = 0, size = 0, i;
 	short flags;
 
-	if (request == SIOCGIFCONF) {
+	switch (request) {
+	case SIOCGIFCONF: {
 		u_ifc = arg;
 		ifc = rtnet_get_arg(fd, &_ifc, u_ifc, sizeof(_ifc));
 		if (IS_ERR(ifc))
@@ -288,8 +291,13 @@ int rt_socket_if_ioctl(struct rtdm_fd *fd, int request, void __user * arg)
 		}
 
 		return rtnet_put_arg(fd, &u_ifc->ifc_len, &size, sizeof(size));
+	    }
+
+	case SIOCSIFVLAN:
+	  return rtvlan_ioctl_handler(arg);
 	}
 
+
 	u_ifr = arg;
 	ifr = rtnet_get_arg(fd, &_ifr, u_ifr, sizeof(_ifr));
 	if (IS_ERR(ifr))
@@ -316,10 +324,8 @@ int rt_socket_if_ioctl(struct rtdm_fd *fd, int request, void __user * arg)
 	case SIOCGIFFLAGS:
 		flags = rtdev->flags;
 		if ((ifr->ifr_flags & IFF_UP)
-		    && (rtdev->link_state
-			& (RTNET_LINK_STATE_PRESENT
-			   | RTNET_LINK_STATE_NOCARRIER))
-		    == RTNET_LINK_STATE_PRESENT)
+		    && rtnetif_carrier_ok(rtdev)
+		    && rtnetif_device_present(rtdev))
 			flags |= IFF_RUNNING;
 		ret = rtnet_put_arg(fd, &u_ifr->ifr_flags, &flags,
 				    sizeof(u_ifr->ifr_flags));
diff --git a/kernel/drivers/net/stack/vlan.c b/kernel/drivers/net/stack/vlan.c
new file mode 100644
index 000000000..7e8c389a4
--- /dev/null
+++ b/kernel/drivers/net/stack/vlan.c
@@ -0,0 +1,725 @@
+/*
+ * INET		802.1Q VLAN
+ *		Ethernet-type device handling.
+ *
+ * Authors:	Ben Greear <greearb at candelatech.com>
+ *		Please send support related email to: netdev at vger.kernel.org
+ *		VLAN Home Page: http://www.candelatech.com/~greear/vlan.html
+ *
+ * Fixes:
+ *		Fix for packet capture - Nick Eggleston <nick at dccinc.com>;
+ *		Add HW acceleration hooks - David S. Miller <davem at redhat.com>;
+ *		Correct all the locking - David S. Miller <davem at redhat.com>;
+ *		Use hash table for VLAN groups - David S. Miller <davem at redhat.com>
+ *
+ *		This program is free software; you can redistribute it and/or
+ *		modify it under the terms of the GNU General Public License
+ *		as published by the Free Software Foundation; either version
+ *		2 of the License, or (at your option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+
+#include <rtdev.h>
+#include <rtnet_port.h>
+#include <rtif_vlan.h>
+
+static unsigned short rtvlan_name_type = VLAN_NAME_TYPE_RAW_PLUS_VID_NO_PAD;
+
+struct rtnet_device *__rtdev_real_dev(struct rtnet_device *dev)
+{
+	if (is_rtvlan_dev(dev))
+		dev = rtvlan_dev_priv(dev)->real_dev;
+
+	return dev;
+}
+EXPORT_SYMBOL_GPL(__rtdev_real_dev);
+
+static inline u16 rtvlan_dev_vlan_id(const struct rtnet_device *dev)
+{
+	return rtvlan_dev_priv(dev)->vlan_id;
+}
+
+static void vlan_dev_set_rx_mode(struct rtnet_device *vlan_dev)
+{
+	struct rtvlan_dev_priv *vlan = rtvlan_dev_priv(vlan_dev);
+	struct rtnet_device *real_dev = vlan->real_dev;
+
+	rt_dev_mc_upload(real_dev);
+}
+
+/*
+ *	Create the VLAN header for an arbitrary protocol layer
+ *
+ *	saddr=NULL	means use device source address
+ *	daddr=NULL	means leave destination address (eg unresolved arp)
+ *
+ *  This is called when the SKB is moving down the stack towards the
+ *  physical devices.
+ */
+static int vlan_dev_hard_header(struct rtskb *skb, struct rtnet_device *dev,
+				unsigned short type,
+				void *daddr, void *saddr,
+				unsigned int len)
+{
+	struct rtvlan_dev_priv *vlan = rtvlan_dev_priv(dev);
+	struct vlan_hdr *vhdr;
+	unsigned int vhdrlen = 0;
+	u16 vlan_tci = 0;
+	int rc;
+
+	if (!(vlan->flags & VLAN_FLAG_REORDER_HDR)) {
+		vhdr = (struct vlan_hdr *) rtskb_push(skb, VLAN_HLEN);
+
+		vlan_tci = vlan->vlan_id;
+		vlan_tci |= rtvlan_dev_get_egress_qos_mask(dev, skb->priority);
+		vhdr->h_vlan_TCI = htons(vlan_tci);
+
+		/*
+		 *  Set the protocol type. For a packet of type ETH_P_802_3/2 we
+		 *  put the length in here instead.
+		 */
+		if (type != ETH_P_802_3 && type != ETH_P_802_2)
+			vhdr->h_vlan_encapsulated_proto = htons(type);
+		else
+			vhdr->h_vlan_encapsulated_proto = htons(len);
+
+		skb->protocol = vlan->vlan_proto;
+		type = ntohs(vlan->vlan_proto);
+		vhdrlen = VLAN_HLEN;
+	}
+
+	/* Before delegating work to the lower layer, enter our MAC-address */
+	if (saddr == NULL)
+		saddr = dev->dev_addr;
+
+	/* Now make the underlying real hard header */
+	dev = vlan->real_dev;
+	rc = dev->hard_header(skb, dev, type, daddr, saddr, len + vhdrlen);
+	if (rc > 0)
+		rc += vhdrlen;
+	return rc;
+}
+
+static netdev_tx_t vlan_dev_hard_start_xmit(struct rtskb *skb,
+					    struct rtnet_device *dev)
+{
+	struct rtvlan_dev_priv *vlan = rtvlan_dev_priv(dev);
+	struct vlan_ethhdr *veth = (struct vlan_ethhdr *)skb->data;
+	struct rtnet_device *real_dev = vlan->real_dev;
+	unsigned int len;
+	int ret;
+
+	/* 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 != vlan->vlan_proto ||
+		(vlan->flags & VLAN_FLAG_REORDER_HDR)) {
+		u16 vlan_tci;
+		vlan_tci = vlan->vlan_id;
+		vlan_tci |= rtvlan_dev_get_egress_qos_mask(dev, skb->priority);
+		rtvlan_put_tag(skb, vlan->vlan_proto, vlan_tci);
+	}
+
+	skb->rtdev = real_dev;
+	len = skb->len;
+
+	ret = real_dev->start_xmit(skb, real_dev);
+
+	if (likely(ret == 0)) {
+		struct rtvlan_pcpu_stats *stats;
+
+		stats = this_cpu_ptr(vlan->vlan_pcpu_stats);
+		raw_write_seqcount_begin(&stats->syncp);
+		stats->tx_packets++;
+		stats->tx_bytes += len;
+		raw_write_seqcount_end(&stats->syncp);
+	} else {
+		this_cpu_inc(vlan->vlan_pcpu_stats->tx_dropped);
+	}
+
+	return ret;
+}
+
+void vlan_dev_set_ingress_priority(const struct rtnet_device *dev,
+				   u32 skb_prio, u16 vlan_prio)
+{
+	struct rtvlan_dev_priv *vlan = rtvlan_dev_priv(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 rtnet_device *dev,
+				 u32 skb_prio, u16 vlan_prio)
+{
+	struct rtvlan_dev_priv *vlan = rtvlan_dev_priv(dev);
+	struct vlan_priority_tci_mapping *mp = NULL;
+	struct vlan_priority_tci_mapping *np;
+	u32 vlan_qos = (vlan_prio << VLAN_PRIO_SHIFT) & VLAN_PRIO_MASK;
+
+	/* 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;
+	/* Before inserting this element in hash table, make sure all its fields
+	 * are committed to memory.
+	 * coupled with smp_rmb() in vlan_dev_get_egress_qos_mask()
+	 */
+	smp_wmb();
+	vlan->egress_priority_map[skb_prio & 0xF] = np;
+	if (vlan_qos)
+		vlan->nr_egress_mappings++;
+	return 0;
+}
+
+static inline u32 vlan_get_ingress_priority(struct rtnet_device *dev,
+					    u16 vlan_tci)
+{
+	struct rtvlan_dev_priv *vip = rtvlan_dev_priv(dev);
+
+	return vip->ingress_priority_map[(vlan_tci >> VLAN_PRIO_SHIFT) & 0x7];
+}
+
+/* Flags are defined in the vlan_flags enum in include/linux/if_vlan.h file. */
+static int
+vlan_dev_change_flags(const struct rtnet_device *dev, u32 flags, u32 mask)
+{
+	struct rtvlan_dev_priv *vlan = rtvlan_dev_priv(dev);
+	u32 old_flags = vlan->flags;
+
+	if (mask & ~(VLAN_FLAG_REORDER_HDR | VLAN_FLAG_LOOSE_BINDING))
+		return -EINVAL;
+
+	vlan->flags = (old_flags & ~mask) | (flags & mask);
+
+	return 0;
+}
+
+static void
+vlan_dev_get_realdev_name(const struct rtnet_device *dev, char *result)
+{
+	strncpy(result, rtvlan_dev_priv(dev)->real_dev->name, 23);
+}
+
+static int vlan_dev_open(struct rtnet_device *dev)
+{
+	struct rtvlan_dev_priv *vlan = rtvlan_dev_priv(dev);
+	struct rtnet_device *real_dev = vlan->real_dev;
+
+	if (!(real_dev->flags & IFF_UP) &&
+	    !(vlan->flags & VLAN_FLAG_LOOSE_BINDING))
+		return -ENETDOWN;
+
+	ether_addr_copy(vlan->real_dev_addr, real_dev->dev_addr);
+
+	vlan_dev_set_rx_mode(dev);
+
+	if (rtnetif_carrier_ok(real_dev))
+		rtnetif_carrier_on(dev);
+	return 0;
+}
+
+static int vlan_dev_stop(struct rtnet_device *dev)
+{
+	rtnetif_carrier_off(dev);
+	return 0;
+}
+
+static struct net_device_stats *vlan_dev_get_stats(struct rtnet_device *dev)
+{
+	struct rtvlan_dev_priv *vlan = rtvlan_dev_priv(dev);
+	struct net_device_stats *stats = &vlan->stats;
+
+	memset(stats, '\0', sizeof(*stats));
+
+	if (rtvlan_dev_priv(dev)->vlan_pcpu_stats) {
+		struct rtvlan_pcpu_stats *p;
+		u32 rx_errors = 0, tx_dropped = 0;
+		int i;
+
+		for_each_possible_cpu(i) {
+			u64 rxpackets, rxbytes, rxmulticast, txpackets, txbytes;
+			unsigned int start;
+
+			p = per_cpu_ptr(rtvlan_dev_priv(dev)->vlan_pcpu_stats, i);
+			do {
+				start = raw_seqcount_begin(&p->syncp);
+				rxpackets	= p->rx_packets;
+				rxbytes		= p->rx_bytes;
+				rxmulticast	= p->rx_multicast;
+				txpackets	= p->tx_packets;
+				txbytes		= p->tx_bytes;
+			} while (read_seqcount_retry(&p->syncp, start));
+
+			stats->rx_packets	+= rxpackets;
+			stats->rx_bytes		+= rxbytes;
+			stats->multicast	+= rxmulticast;
+			stats->tx_packets	+= txpackets;
+			stats->tx_bytes		+= txbytes;
+			/* rx_errors & tx_dropped are u32 */
+			rx_errors	+= p->rx_errors;
+			tx_dropped	+= p->tx_dropped;
+		}
+		stats->rx_errors  = rx_errors;
+		stats->tx_dropped = tx_dropped;
+	}
+	return stats;
+}
+
+static int vlan_dev_init(struct rtnet_device *dev)
+{
+	struct rtvlan_dev_priv *vlan = rtvlan_dev_priv(dev);
+	struct rtnet_device *real_dev = vlan->real_dev;
+	int i;
+
+	rtnetif_carrier_off(dev);
+
+	/* IFF_BROADCAST|IFF_MULTICAST; ??? */
+	dev->flags  = real_dev->flags & ~(IFF_UP | IFF_PROMISC | IFF_ALLMULTI
+					| IFF_MASTER | IFF_SLAVE);
+	dev->link_state	 =
+		(real_dev->link_state & (1<<__RTNET_LINK_STATE_NOCARRIER)) |
+		(1<<__RTNET_LINK_STATE_PRESENT);
+
+	dev->features = NETIF_F_LLTX;
+
+	if (is_zero_ether_addr(dev->dev_addr))
+		ether_addr_copy(dev->dev_addr, real_dev->dev_addr);;
+	if (is_zero_ether_addr(dev->broadcast))
+		memcpy(dev->broadcast, real_dev->broadcast, dev->addr_len);
+
+	dev->open = vlan_dev_open;
+	dev->stop = vlan_dev_stop;
+	dev->hard_header_len = real_dev->hard_header_len + VLAN_HLEN;
+	dev->hard_header = vlan_dev_hard_header;
+	dev->hard_start_xmit = vlan_dev_hard_start_xmit;
+	dev->get_stats = vlan_dev_get_stats;
+	if (real_dev->set_multicast_list)
+		dev->set_multicast_list = vlan_dev_set_rx_mode;
+
+	vlan->vlan_pcpu_stats = alloc_percpu(typeof(*vlan->vlan_pcpu_stats));
+	if (!rtvlan_dev_priv(dev)->vlan_pcpu_stats)
+		return -ENOMEM;
+
+	for_each_possible_cpu(i) {
+		struct rtvlan_pcpu_stats *vlan_stat;
+		vlan_stat = per_cpu_ptr(vlan->vlan_pcpu_stats, i);
+		seqcount_init(&vlan_stat->syncp);
+	}
+
+	return 0;
+}
+
+static void vlan_dev_uninit(struct rtnet_device *dev)
+{
+	struct vlan_priority_tci_mapping *pm;
+	struct rtvlan_dev_priv *vlan = rtvlan_dev_priv(dev);
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(vlan->egress_priority_map); i++) {
+		while ((pm = vlan->egress_priority_map[i]) != NULL) {
+			vlan->egress_priority_map[i] = pm->next;
+			kfree(pm);
+		}
+	}
+}
+
+static void vlan_dev_free(struct rtnet_device *dev)
+{
+	struct rtvlan_dev_priv *vlan = rtvlan_dev_priv(dev);
+
+	vlan_dev_uninit(dev);
+	free_percpu(vlan->vlan_pcpu_stats);
+	vlan->vlan_pcpu_stats = NULL;
+	rt_unregister_rtnetdev(dev);
+	rt_rtdev_disconnect(dev);
+	rtdev_free(dev);
+}
+
+static bool vlan_hw_filter_capable(const struct rtnet_device *dev)
+{
+	return !!(dev->features & NETIF_F_HW_VLAN_CTAG_FILTER);
+}
+
+static struct rtnet_device *__rtvlan_find_dev(struct rtnet_device *dev, u16 vid)
+{
+	struct rtnet_device *vlan_dev;
+
+	if (vid == 0)
+		return dev;
+
+	list_for_each_entry(vlan_dev, &dev->vlan_link, vlan_link) {
+		struct rtvlan_dev_priv *vlan = rtvlan_dev_priv(vlan_dev);
+
+		if (vlan->vlan_id == vid)
+			return vlan_dev;
+	}
+
+	return NULL;
+}
+
+static struct rtnet_device *rtvlan_find_dev(struct rtnet_device *dev, u16 vid)
+{
+	struct rtnet_device *vlan_dev;
+	unsigned long flags;
+
+	rtdm_lock_get_irqsave(&dev->rtdev_lock, flags);
+	vlan_dev = __rtvlan_find_dev(dev, vid);
+	if (vlan_dev)
+		rtdev_reference(vlan_dev);
+	rtdm_lock_put_irqrestore(&dev->rtdev_lock, flags);
+
+	return vlan_dev;
+}
+
+static int rtvlan_vid_add(struct rtnet_device *dev)
+{
+	struct rtvlan_dev_priv *vlan = rtvlan_dev_priv(dev);
+	struct rtnet_device *real_dev = vlan->real_dev;
+	u16 vid = vlan->vlan_id;
+	unsigned long flags;
+	int err = 0;
+
+	rtdm_lock_get_irqsave(&dev->rtdev_lock, flags);
+	if (__rtvlan_find_dev(real_dev, vid)) {
+		err = -EEXIST;
+		goto out;
+	}
+
+	list_add(&dev->vlan_link, &real_dev->vlan_link);
+
+	if (vlan_hw_filter_capable(real_dev))
+		real_dev->vlan_rx_add_vid(real_dev, vid);
+  out:
+	rtdm_lock_put_irqrestore(&dev->rtdev_lock, flags);
+
+	return err;
+}
+
+static void rtvlan_vid_del(struct rtnet_device *dev)
+{
+	struct rtvlan_dev_priv *vlan = rtvlan_dev_priv(dev);
+	struct rtnet_device *real_dev = vlan->real_dev;
+	u16 vid = vlan->vlan_id;
+	unsigned long flags;
+
+	rtdm_lock_get_irqsave(&dev->rtdev_lock, flags);
+	if (__rtvlan_find_dev(real_dev, vid) != dev)
+		goto out;
+
+	list_del(&dev->vlan_link);
+
+	if (vlan_hw_filter_capable(real_dev))
+		real_dev->vlan_rx_kill_vid(real_dev, vid);
+  out:
+	rtdm_lock_put_irqrestore(&dev->rtdev_lock, flags);
+}
+
+void unregister_vlan_dev(struct rtnet_device *dev)
+{
+	struct rtvlan_dev_priv *vlan = rtvlan_dev_priv(dev);
+	struct rtnet_device *real_dev = vlan->real_dev;
+	u16 vlan_id = vlan->vlan_id;
+
+	if (vlan_id)
+		rtvlan_vid_del(dev);
+
+	/* Get rid of the vlan's reference to real_dev */
+	rtdev_dereference(real_dev);
+	vlan_dev_free(dev);
+}
+
+
+/*  Attach a VLAN device to a mac address (ie Ethernet Card).
+ *  Returns 0 if the device was created or a negative error code otherwise.
+ */
+static int register_vlan_device(struct rtnet_device *real_dev, u16 vlan_id)
+{
+	struct rtnet_device *new_dev;
+	struct rtvlan_dev_priv *vlan;
+	char name[IFNAMSIZ];
+	int err;
+
+	if (vlan_id >= VLAN_VID_MASK)
+		return -ERANGE;
+
+	/* Gotta set up the fields for the device.
+	   Only one type of device name supported
+	 */
+	switch (rtvlan_name_type) {
+	case VLAN_NAME_TYPE_RAW_PLUS_VID:
+		/* name will look like:	 eth1.0005 */
+		snprintf(name, IFNAMSIZ, "%s.%.4i", real_dev->name, vlan_id);
+		break;
+	case VLAN_NAME_TYPE_PLUS_VID_NO_PAD:
+		/* Put our vlan.VID in the name.
+		 * Name will look like:	 vlan5
+		 */
+		snprintf(name, IFNAMSIZ, "rtvlan%i", vlan_id);
+		break;
+	case VLAN_NAME_TYPE_RAW_PLUS_VID_NO_PAD:
+		/* Put our vlan.VID in the name.
+		 * Name will look like:	 eth0.5
+		 */
+		snprintf(name, IFNAMSIZ, "%s.%i", real_dev->name, vlan_id);
+		break;
+	case VLAN_NAME_TYPE_PLUS_VID:
+		/* Put our vlan.VID in the name.
+		 * Name will look like:	 vlan0005
+		 */
+	default:
+		snprintf(name, IFNAMSIZ, "rtvlan%.4i", vlan_id);
+	}
+
+	rtdev_reference(real_dev);
+
+	new_dev = rt_alloc_etherdev(sizeof(struct rtvlan_dev_priv), 0);
+	if (new_dev == NULL) {
+		err = -ENOBUFS;
+		goto err;
+	}
+	rtdev_alloc_name(new_dev, name);
+	rt_rtdev_connect(new_dev, &RTDEV_manager);
+	new_dev->vers = RTDEV_VERS_2_0;
+
+	new_dev->priv_flags |= IFF_802_1Q_VLAN;
+	new_dev->priv_flags &= ~(IFF_XMIT_DST_RELEASE | IFF_TX_SKB_SHARING);
+
+	memset(new_dev->broadcast, 0, ETH_ALEN);
+
+	/* need 4 bytes for extra VLAN header info,
+	 * hope the underlying device can handle it.
+	 */
+	new_dev->mtu = real_dev->mtu;
+	new_dev->priv_flags |= (real_dev->priv_flags & IFF_UNICAST_FLT);
+
+	vlan = rtvlan_dev_priv(new_dev);
+	vlan->vlan_proto = htons(ETH_P_8021Q);
+	vlan->vlan_id = vlan_id;
+	vlan->real_dev = real_dev;
+	vlan->flags = VLAN_FLAG_REORDER_HDR;
+
+	err = vlan_dev_init(new_dev);
+	if (err < 0)
+		goto out_free_newdev;
+
+	err = rtvlan_vid_add(new_dev);
+	if (err < 0)
+		goto out_free_newdev;
+
+	err = rt_register_rtnetdev(new_dev);
+	if (err > 0) {
+		err = -err;
+		goto out_free_newdev;
+	}
+
+	return 0;
+
+out_free_newdev:
+	rt_rtdev_disconnect(new_dev);
+	rtdev_free(new_dev);
+  err:
+	rtdev_dereference(real_dev);
+	return err;
+}
+
+/*
+ *	VLAN IOCTL handler.
+ *	o execute requested action or pass command to the device driver
+ *   arg is really a struct vlan_ioctl_args __user *.
+ */
+int rtvlan_ioctl_handler(void __user *arg)
+{
+	int err;
+	struct vlan_ioctl_args args;
+	struct rtnet_device *dev = NULL;
+
+	if (copy_from_user(&args, arg, sizeof(args)))
+		return -EFAULT;
+
+	/* Null terminate this sucker, just in case. */
+	args.device1[23] = 0;
+	args.u.device2[23] = 0;
+
+	switch (args.cmd) {
+	case SET_VLAN_INGRESS_PRIORITY_CMD:
+	case SET_VLAN_EGRESS_PRIORITY_CMD:
+	case SET_VLAN_FLAG_CMD:
+	case ADD_VLAN_CMD:
+	case DEL_VLAN_CMD:
+	case GET_VLAN_REALDEV_NAME_CMD:
+	case GET_VLAN_VID_CMD:
+		err = -ENODEV;
+		dev = rtdev_get_by_name(args.device1);
+		if (!dev)
+			goto out;
+
+		err = -EINVAL;
+		if (args.cmd != ADD_VLAN_CMD && !is_rtvlan_dev(dev))
+			goto out;
+	}
+
+	switch (args.cmd) {
+	case SET_VLAN_INGRESS_PRIORITY_CMD:
+		err = -EPERM;
+		vlan_dev_set_ingress_priority(dev,
+					      args.u.skb_priority,
+					      args.vlan_qos);
+		err = 0;
+		break;
+
+	case SET_VLAN_EGRESS_PRIORITY_CMD:
+		err = -EPERM;
+		err = vlan_dev_set_egress_priority(dev,
+						   args.u.skb_priority,
+						   args.vlan_qos);
+		break;
+
+	case SET_VLAN_FLAG_CMD:
+		err = -EPERM;
+		err = vlan_dev_change_flags(dev,
+					    args.vlan_qos ? args.u.flag : 0,
+					    args.u.flag);
+		break;
+
+	case SET_VLAN_NAME_TYPE_CMD:
+		err = -EPERM;
+		if ((args.u.name_type >= 0) &&
+		    (args.u.name_type < VLAN_NAME_TYPE_HIGHEST)) {
+			rtvlan_name_type = args.u.name_type;
+			err = 0;
+		} else {
+			err = -EINVAL;
+		}
+		break;
+
+	case ADD_VLAN_CMD:
+		err = -EPERM;
+		err = register_vlan_device(dev, args.u.VID);
+		break;
+
+	case DEL_VLAN_CMD:
+		err = -EPERM;
+		rtdev_dereference(dev); /*
+					 * Must dereference before unregistering
+					 * in order to avoid infinite loop in
+					 * rt_unregister_rtnetdev
+					 */
+		unregister_vlan_dev(dev);
+		return 0;
+
+	case GET_VLAN_REALDEV_NAME_CMD:
+		err = 0;
+		vlan_dev_get_realdev_name(dev, args.u.device2);
+		if (copy_to_user(arg, &args,
+				 sizeof(struct vlan_ioctl_args)))
+			err = -EFAULT;
+		break;
+
+	case GET_VLAN_VID_CMD:
+		err = 0;
+		args.u.VID = rtvlan_dev_vlan_id(dev);
+		if (copy_to_user(arg, &args,
+				 sizeof(struct vlan_ioctl_args)))
+		      err = -EFAULT;
+		break;
+
+	default:
+		err = -EOPNOTSUPP;
+		break;
+	}
+out:
+	if (dev)
+		rtdev_dereference(dev);
+
+	return err;
+}
+
+int rtvlan_proto_rx(struct rtskb *skb, struct rtpacket_type *pt)
+{
+	struct vlan_ethhdr *veth = (struct vlan_ethhdr *)skb->mac.raw;
+	struct rtvlan_pcpu_stats *rx_stats;
+	struct rtnet_device *vlan_dev;
+	struct rtvlan_dev_priv *vlan;
+	u16 vlan_tci;
+
+	vlan_tci = ntohs(veth->h_vlan_TCI);
+	vlan_dev = rtvlan_find_dev(skb->rtdev, vlan_tci & VLAN_VID_MASK);
+	if (!vlan_dev) {
+		kfree_rtskb(skb);
+		rtdev_dereference(skb->rtdev);
+		return 0;
+	}
+	vlan = rtvlan_dev_priv(vlan_dev);
+
+	skb->priority = vlan_get_ingress_priority(vlan_dev, vlan_tci);
+	skb->protocol = veth->h_vlan_encapsulated_proto;
+	skb->rtdev = vlan_dev;
+
+	if (skb->pkt_type == PACKET_OTHERHOST
+		&& ether_addr_equal(veth->h_dest, vlan_dev->dev_addr))
+		skb->pkt_type = PACKET_HOST;
+
+	if (vlan->flags & VLAN_FLAG_REORDER_HDR) {
+		memmove(skb->mac.raw + VLAN_HLEN, skb->mac.raw, 2 * ETH_ALEN);
+#ifdef CONFIG_XENO_DRIVERS_NET_ADDON_RTCAP
+		skb->cap_start += VLAN_HLEN;
+		skb->cap_len -= VLAN_HLEN;
+#endif
+	}
+	rtskb_pull(skb, VLAN_HLEN);
+
+	rx_stats = this_cpu_ptr(rtvlan_dev_priv(vlan_dev)->vlan_pcpu_stats);
+
+	raw_write_seqcount_begin(&rx_stats->syncp);
+	rx_stats->rx_packets++;
+	rx_stats->rx_bytes += skb->len;
+	if (skb->pkt_type == PACKET_MULTICAST)
+		rx_stats->rx_multicast++;
+	raw_write_seqcount_end(&rx_stats->syncp);
+
+	rt_stack_deliver(skb);
+	return 0;
+}
+
+struct rtpacket_type rtvlan_packet_type = {
+	.type = __constant_htons(ETH_P_8021Q),
+	.handler = rtvlan_proto_rx,
+};
+
+void rtvlan_proto_init(void)
+{
+	rtdev_add_pack(&rtvlan_packet_type);
+}
+
+void rtvlan_proto_release(void)
+{
+	rtdev_remove_pack(&rtvlan_packet_type);
+}
-- 
2.20.1




More information about the Xenomai mailing list