diff --git a/include/linux/if_link.h b/include/linux/if_link.h index f9032c8..d3fb5e6 100644 --- a/include/linux/if_link.h +++ b/include/linux/if_link.h @@ -164,6 +164,7 @@ enum IFLA_VLAN_FLAGS, IFLA_VLAN_EGRESS_QOS, IFLA_VLAN_INGRESS_QOS, + IFLA_VLAN_ETHERTYPE, __IFLA_VLAN_MAX, }; diff --git a/include/linux/if_vlan.h b/include/linux/if_vlan.h index a5cb0c3..11abb17 100644 --- a/include/linux/if_vlan.h +++ b/include/linux/if_vlan.h @@ -111,6 +111,7 @@ static inline void vlan_group_set_device(struct vlan_group *vg, #if defined(CONFIG_VLAN_8021Q) || defined(CONFIG_VLAN_8021Q_MODULE) extern struct net_device *vlan_dev_real_dev(const struct net_device *dev); extern u16 vlan_dev_vlan_id(const struct net_device *dev); +extern u16 vlan_dev_ethertype(const struct net_device *dev); extern int __vlan_hwaccel_rx(struct sk_buff *skb, struct vlan_group *grp, u16 vlan_tci, int polling); @@ -193,12 +194,12 @@ static inline struct sk_buff *__vlan_put_tag(struct sk_buff *skb, u16 vlan_tci) memmove(skb->data, skb->data + VLAN_HLEN, 2 * VLAN_ETH_ALEN); /* first, the ethernet type */ - veth->h_vlan_proto = htons(ETH_P_8021Q); + veth->h_vlan_proto = htons(vlan_dev_ethertype(skb->dev)); /* now, the TCI */ veth->h_vlan_TCI = htons(vlan_tci); - skb->protocol = htons(ETH_P_8021Q); + skb->protocol = htons(veth->h_vlan_proto); return skb; } @@ -247,7 +248,7 @@ static inline int __vlan_get_tag(const struct sk_buff *skb, u16 *vlan_tci) { struct vlan_ethhdr *veth = (struct vlan_ethhdr *)skb->data; - if (veth->h_vlan_proto != htons(ETH_P_8021Q)) { + if (veth->h_vlan_proto != htons(vlan_dev_ethertype(skb->dev))) { return -EINVAL; } @@ -299,6 +300,7 @@ static inline int vlan_get_tag(const struct sk_buff *skb, u16 *vlan_tci) /* Passed in vlan_ioctl_args structure to determine behaviour. */ enum vlan_ioctl_cmds { ADD_VLAN_CMD, + ADD_CUSTOM_VLAN_CMD, /* Allows specifying ethertype */ DEL_VLAN_CMD, SET_VLAN_INGRESS_PRIORITY_CMD, SET_VLAN_EGRESS_PRIORITY_CMD, @@ -307,7 +309,8 @@ enum vlan_ioctl_cmds { SET_VLAN_NAME_TYPE_CMD, SET_VLAN_FLAG_CMD, GET_VLAN_REALDEV_NAME_CMD, /* If this works, you know it's a VLAN device, btw */ - GET_VLAN_VID_CMD /* Get the VID of this VLAN (specified by name) */ + GET_VLAN_VID_CMD, /* Get the VID of this VLAN (specified by name) */ + GET_VLAN_ETHERTYPE_CMD, /* Get the ethertype of this VLAN */ }; enum vlan_flags { @@ -334,6 +337,11 @@ struct vlan_ioctl_args { unsigned int name_type; unsigned int bind_type; unsigned int flag; /* Matches vlan_dev_info flags */ + unsigned int ethertype; + struct { + int VID; + unsigned int ethertype; + } new; } u; short vlan_qos; diff --git a/net/8021q/vlan.c b/net/8021q/vlan.c index f0e335a..3bcb875 100644 --- a/net/8021q/vlan.c +++ b/net/8021q/vlan.c @@ -279,6 +279,13 @@ int register_vlan_dev(struct net_device *dev) if (real_dev->features & NETIF_F_HW_VLAN_FILTER) real_dev->vlan_rx_add_vid(real_dev, vlan_id); + if (vlan_dev_ethertype(dev) != vlan_packet_type.type) { + // FIXME: don't register multiple times! + struct packet_type *npt = kzalloc(sizeof(vlan_packet_type), GFP_KERNEL); + npt->type = htons(vlan_dev_ethertype(dev)); + dev_add_pack(npt); + } + return 0; out_uninit_applicant: @@ -293,7 +300,7 @@ out_free_group: /* 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 net_device *real_dev, u16 vlan_id) +static int register_custom_vlan_device(struct net_device *real_dev, u16 vlan_id, u16 ethertype) { struct net_device *new_dev; struct net *net = dev_net(real_dev); @@ -350,6 +357,7 @@ static int register_vlan_device(struct net_device *real_dev, u16 vlan_id) vlan_dev_info(new_dev)->real_dev = real_dev; vlan_dev_info(new_dev)->dent = NULL; vlan_dev_info(new_dev)->flags = VLAN_FLAG_REORDER_HDR; + vlan_dev_info(new_dev)->ethertype = ethertype; new_dev->rtnl_link_ops = &vlan_link_ops; err = register_vlan_dev(new_dev); @@ -363,6 +371,11 @@ out_free_newdev: return err; } +static int register_vlan_device(struct net_device *real_dev, u16 vlan_id) +{ + return register_custom_vlan_device(real_dev, vlan_id, ETH_P_8021Q); +} + static void vlan_sync_address(struct net_device *dev, struct net_device *vlandev) { @@ -561,6 +574,7 @@ static int vlan_ioctl_handler(struct net *net, void __user *arg) case DEL_VLAN_CMD: case GET_VLAN_REALDEV_NAME_CMD: case GET_VLAN_VID_CMD: + case GET_VLAN_ETHERTYPE_CMD: err = -ENODEV; dev = __dev_get_by_name(net, args.device1); if (!dev) @@ -623,6 +637,13 @@ static int vlan_ioctl_handler(struct net *net, void __user *arg) err = register_vlan_device(dev, args.u.VID); break; + case ADD_CUSTOM_VLAN_CMD: + err = -EPERM; + if (!capable(CAP_NET_ADMIN)) + break; + err = register_custom_vlan_device(dev, args.u.new.VID, args.u.new.ethertype); + break; + case DEL_VLAN_CMD: err = -EPERM; if (!capable(CAP_NET_ADMIN)) @@ -647,6 +668,14 @@ static int vlan_ioctl_handler(struct net *net, void __user *arg) err = -EFAULT; break; + case GET_VLAN_ETHERTYPE_CMD: + err = 0; + args.u.ethertype = vlan_dev_ethertype(dev); + if (copy_to_user(arg, &args, + sizeof(struct vlan_ioctl_args))) + err = -EFAULT; + break; + default: err = -EOPNOTSUPP; break; diff --git a/net/8021q/vlan.h b/net/8021q/vlan.h index a6603a4..431d1cf 100644 --- a/net/8021q/vlan.h +++ b/net/8021q/vlan.h @@ -29,6 +29,7 @@ struct vlan_priority_tci_mapping { * @dent: proc dir entry * @cnt_inc_headroom_on_tx: statistic - number of skb expansions on TX * @cnt_encap_on_xmit: statistic - number of skb encapsulations on TX + * @ethertype: EtherType */ struct vlan_dev_info { unsigned int nr_ingress_mappings; @@ -45,6 +46,8 @@ struct vlan_dev_info { struct proc_dir_entry *dent; unsigned long cnt_inc_headroom_on_tx; unsigned long cnt_encap_on_xmit; + + u16 ethertype; }; static inline struct vlan_dev_info *vlan_dev_info(const struct net_device *dev) diff --git a/net/8021q/vlan_core.c b/net/8021q/vlan_core.c index 68ced4b..6e7b10b 100644 --- a/net/8021q/vlan_core.c +++ b/net/8021q/vlan_core.c @@ -82,3 +82,9 @@ u16 vlan_dev_vlan_id(const struct net_device *dev) return vlan_dev_info(dev)->vlan_id; } EXPORT_SYMBOL_GPL(vlan_dev_vlan_id); + +u16 vlan_dev_ethertype(const struct net_device *dev) +{ + return vlan_dev_info(dev)->ethertype; +} +EXPORT_SYMBOL_GPL(vlan_dev_ethertype); diff --git a/net/8021q/vlan_dev.c b/net/8021q/vlan_dev.c index 8883e9c..fb5b3ee 100644 --- a/net/8021q/vlan_dev.c +++ b/net/8021q/vlan_dev.c @@ -273,8 +273,8 @@ static int vlan_dev_hard_header(struct sk_buff *skb, struct net_device *dev, else vhdr->h_vlan_encapsulated_proto = htons(len); - skb->protocol = htons(ETH_P_8021Q); - type = ETH_P_8021Q; + type = vlan_dev_ethertype(dev); + skb->protocol = htons(type); vhdrlen = VLAN_HLEN; } @@ -300,7 +300,7 @@ static int vlan_dev_hard_start_xmit(struct sk_buff *skb, struct net_device *dev) * 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) || + if (veth->h_vlan_proto != htons(vlan_dev_ethertype(dev)) || vlan_dev_info(dev)->flags & VLAN_FLAG_REORDER_HDR) { unsigned int orig_headroom = skb_headroom(skb); u16 vlan_tci; diff --git a/net/8021q/vlan_netlink.c b/net/8021q/vlan_netlink.c index e9c91dc..656a0e4 100644 --- a/net/8021q/vlan_netlink.c +++ b/net/8021q/vlan_netlink.c @@ -22,6 +22,7 @@ static const struct nla_policy vlan_policy[IFLA_VLAN_MAX + 1] = { [IFLA_VLAN_FLAGS] = { .len = sizeof(struct ifla_vlan_flags) }, [IFLA_VLAN_EGRESS_QOS] = { .type = NLA_NESTED }, [IFLA_VLAN_INGRESS_QOS] = { .type = NLA_NESTED }, + [IFLA_VLAN_ETHERTYPE] = { .type = NLA_U16 }, }; static const struct nla_policy vlan_map_policy[IFLA_VLAN_QOS_MAX + 1] = { @@ -40,6 +41,7 @@ static int vlan_validate(struct nlattr *tb[], struct nlattr *data[]) { struct ifla_vlan_flags *flags; u16 id; + u16 ethertype; int err; if (tb[IFLA_ADDRESS]) { @@ -70,6 +72,13 @@ static int vlan_validate(struct nlattr *tb[], struct nlattr *data[]) err = vlan_validate_qos_map(data[IFLA_VLAN_EGRESS_QOS]); if (err < 0) return err; + + if (data[IFLA_VLAN_ETHERTYPE]) { + ethertype = nla_get_u16(data[IFLA_VLAN_ETHERTYPE]); + if (ethertype && ethertype < 0x0800) + return -ERANGE; + } + return 0; } @@ -119,6 +128,10 @@ static int vlan_newlink(struct net_device *dev, vlan->vlan_id = nla_get_u16(data[IFLA_VLAN_ID]); vlan->real_dev = real_dev; vlan->flags = VLAN_FLAG_REORDER_HDR; + vlan->ethertype= nla_get_u16(data[IFLA_VLAN_ETHERTYPE]); + + if (!vlan->ethertype) + vlan->ethertype = ETH_P_8021Q; err = vlan_check_real_dev(real_dev, vlan->vlan_id); if (err < 0) @@ -150,6 +163,7 @@ static size_t vlan_get_size(const struct net_device *dev) struct vlan_dev_info *vlan = vlan_dev_info(dev); return nla_total_size(2) + /* IFLA_VLAN_ID */ + nla_total_size(2) + /* IFLA_VLAN_ETHERTYPE */ vlan_qos_map_size(vlan->nr_ingress_mappings) + vlan_qos_map_size(vlan->nr_egress_mappings); } @@ -205,6 +219,7 @@ static int vlan_fill_info(struct sk_buff *skb, const struct net_device *dev) } nla_nest_end(skb, nest); } + NLA_PUT_U16(skb, IFLA_VLAN_ETHERTYPE, vlan_dev_info(dev)->ethertype); return 0; nla_put_failure: