1 // SPDX-License-Identifier: GPL-2.0 2 #include <linux/skbuff.h> 3 #include <linux/netdevice.h> 4 #include <linux/if_vlan.h> 5 #include <linux/netpoll.h> 6 #include <linux/export.h> 7 #include "vlan.h" 8 9 bool vlan_do_receive(struct sk_buff **skbp) 10 { 11 struct sk_buff *skb = *skbp; 12 __be16 vlan_proto = skb->vlan_proto; 13 u16 vlan_id = skb_vlan_tag_get_id(skb); 14 struct net_device *vlan_dev; 15 struct vlan_pcpu_stats *rx_stats; 16 17 vlan_dev = vlan_find_dev(skb->dev, vlan_proto, vlan_id); 18 if (!vlan_dev) 19 return false; 20 21 skb = *skbp = skb_share_check(skb, GFP_ATOMIC); 22 if (unlikely(!skb)) 23 return false; 24 25 if (unlikely(!(vlan_dev->flags & IFF_UP))) { 26 kfree_skb(skb); 27 *skbp = NULL; 28 return false; 29 } 30 31 skb->dev = vlan_dev; 32 if (unlikely(skb->pkt_type == PACKET_OTHERHOST)) { 33 /* Our lower layer thinks this is not local, let's make sure. 34 * This allows the VLAN to have a different MAC than the 35 * underlying device, and still route correctly. */ 36 if (ether_addr_equal_64bits(eth_hdr(skb)->h_dest, vlan_dev->dev_addr)) 37 skb->pkt_type = PACKET_HOST; 38 } 39 40 if (!(vlan_dev_priv(vlan_dev)->flags & VLAN_FLAG_REORDER_HDR) && 41 !netif_is_macvlan_port(vlan_dev) && 42 !netif_is_bridge_port(vlan_dev)) { 43 unsigned int offset = skb->data - skb_mac_header(skb); 44 45 /* 46 * vlan_insert_tag expect skb->data pointing to mac header. 47 * So change skb->data before calling it and change back to 48 * original position later 49 */ 50 skb_push(skb, offset); 51 skb = *skbp = vlan_insert_tag(skb, skb->vlan_proto, 52 skb->vlan_tci); 53 if (!skb) 54 return false; 55 skb_pull(skb, offset + VLAN_HLEN); 56 skb_reset_mac_len(skb); 57 } 58 59 skb->priority = vlan_get_ingress_priority(vlan_dev, skb->vlan_tci); 60 skb->vlan_tci = 0; 61 62 rx_stats = this_cpu_ptr(vlan_dev_priv(vlan_dev)->vlan_pcpu_stats); 63 64 u64_stats_update_begin(&rx_stats->syncp); 65 rx_stats->rx_packets++; 66 rx_stats->rx_bytes += skb->len; 67 if (skb->pkt_type == PACKET_MULTICAST) 68 rx_stats->rx_multicast++; 69 u64_stats_update_end(&rx_stats->syncp); 70 71 return true; 72 } 73 74 /* Must be invoked with rcu_read_lock. */ 75 struct net_device *__vlan_find_dev_deep_rcu(struct net_device *dev, 76 __be16 vlan_proto, u16 vlan_id) 77 { 78 struct vlan_info *vlan_info = rcu_dereference(dev->vlan_info); 79 80 if (vlan_info) { 81 return vlan_group_get_device(&vlan_info->grp, 82 vlan_proto, vlan_id); 83 } else { 84 /* 85 * Lower devices of master uppers (bonding, team) do not have 86 * grp assigned to themselves. Grp is assigned to upper device 87 * instead. 88 */ 89 struct net_device *upper_dev; 90 91 upper_dev = netdev_master_upper_dev_get_rcu(dev); 92 if (upper_dev) 93 return __vlan_find_dev_deep_rcu(upper_dev, 94 vlan_proto, vlan_id); 95 } 96 97 return NULL; 98 } 99 EXPORT_SYMBOL(__vlan_find_dev_deep_rcu); 100 101 struct net_device *vlan_dev_real_dev(const struct net_device *dev) 102 { 103 struct net_device *ret = vlan_dev_priv(dev)->real_dev; 104 105 while (is_vlan_dev(ret)) 106 ret = vlan_dev_priv(ret)->real_dev; 107 108 return ret; 109 } 110 EXPORT_SYMBOL(vlan_dev_real_dev); 111 112 u16 vlan_dev_vlan_id(const struct net_device *dev) 113 { 114 return vlan_dev_priv(dev)->vlan_id; 115 } 116 EXPORT_SYMBOL(vlan_dev_vlan_id); 117 118 __be16 vlan_dev_vlan_proto(const struct net_device *dev) 119 { 120 return vlan_dev_priv(dev)->vlan_proto; 121 } 122 EXPORT_SYMBOL(vlan_dev_vlan_proto); 123 124 /* 125 * vlan info and vid list 126 */ 127 128 static void vlan_group_free(struct vlan_group *grp) 129 { 130 int i, j; 131 132 for (i = 0; i < VLAN_PROTO_NUM; i++) 133 for (j = 0; j < VLAN_GROUP_ARRAY_SPLIT_PARTS; j++) 134 kfree(grp->vlan_devices_arrays[i][j]); 135 } 136 137 static void vlan_info_free(struct vlan_info *vlan_info) 138 { 139 vlan_group_free(&vlan_info->grp); 140 kfree(vlan_info); 141 } 142 143 static void vlan_info_rcu_free(struct rcu_head *rcu) 144 { 145 vlan_info_free(container_of(rcu, struct vlan_info, rcu)); 146 } 147 148 static struct vlan_info *vlan_info_alloc(struct net_device *dev) 149 { 150 struct vlan_info *vlan_info; 151 152 vlan_info = kzalloc(sizeof(struct vlan_info), GFP_KERNEL); 153 if (!vlan_info) 154 return NULL; 155 156 vlan_info->real_dev = dev; 157 INIT_LIST_HEAD(&vlan_info->vid_list); 158 return vlan_info; 159 } 160 161 struct vlan_vid_info { 162 struct list_head list; 163 __be16 proto; 164 u16 vid; 165 int refcount; 166 }; 167 168 static bool vlan_hw_filter_capable(const struct net_device *dev, 169 const struct vlan_vid_info *vid_info) 170 { 171 if (vid_info->proto == htons(ETH_P_8021Q) && 172 dev->features & NETIF_F_HW_VLAN_CTAG_FILTER) 173 return true; 174 if (vid_info->proto == htons(ETH_P_8021AD) && 175 dev->features & NETIF_F_HW_VLAN_STAG_FILTER) 176 return true; 177 return false; 178 } 179 180 static struct vlan_vid_info *vlan_vid_info_get(struct vlan_info *vlan_info, 181 __be16 proto, u16 vid) 182 { 183 struct vlan_vid_info *vid_info; 184 185 list_for_each_entry(vid_info, &vlan_info->vid_list, list) { 186 if (vid_info->proto == proto && vid_info->vid == vid) 187 return vid_info; 188 } 189 return NULL; 190 } 191 192 static struct vlan_vid_info *vlan_vid_info_alloc(__be16 proto, u16 vid) 193 { 194 struct vlan_vid_info *vid_info; 195 196 vid_info = kzalloc(sizeof(struct vlan_vid_info), GFP_KERNEL); 197 if (!vid_info) 198 return NULL; 199 vid_info->proto = proto; 200 vid_info->vid = vid; 201 202 return vid_info; 203 } 204 205 static int __vlan_vid_add(struct vlan_info *vlan_info, __be16 proto, u16 vid, 206 struct vlan_vid_info **pvid_info) 207 { 208 struct net_device *dev = vlan_info->real_dev; 209 const struct net_device_ops *ops = dev->netdev_ops; 210 struct vlan_vid_info *vid_info; 211 int err; 212 213 vid_info = vlan_vid_info_alloc(proto, vid); 214 if (!vid_info) 215 return -ENOMEM; 216 217 if (vlan_hw_filter_capable(dev, vid_info)) { 218 if (netif_device_present(dev)) 219 err = ops->ndo_vlan_rx_add_vid(dev, proto, vid); 220 else 221 err = -ENODEV; 222 if (err) { 223 kfree(vid_info); 224 return err; 225 } 226 } 227 list_add(&vid_info->list, &vlan_info->vid_list); 228 vlan_info->nr_vids++; 229 *pvid_info = vid_info; 230 return 0; 231 } 232 233 int vlan_vid_add(struct net_device *dev, __be16 proto, u16 vid) 234 { 235 struct vlan_info *vlan_info; 236 struct vlan_vid_info *vid_info; 237 bool vlan_info_created = false; 238 int err; 239 240 ASSERT_RTNL(); 241 242 vlan_info = rtnl_dereference(dev->vlan_info); 243 if (!vlan_info) { 244 vlan_info = vlan_info_alloc(dev); 245 if (!vlan_info) 246 return -ENOMEM; 247 vlan_info_created = true; 248 } 249 vid_info = vlan_vid_info_get(vlan_info, proto, vid); 250 if (!vid_info) { 251 err = __vlan_vid_add(vlan_info, proto, vid, &vid_info); 252 if (err) 253 goto out_free_vlan_info; 254 } 255 vid_info->refcount++; 256 257 if (vlan_info_created) 258 rcu_assign_pointer(dev->vlan_info, vlan_info); 259 260 return 0; 261 262 out_free_vlan_info: 263 if (vlan_info_created) 264 kfree(vlan_info); 265 return err; 266 } 267 EXPORT_SYMBOL(vlan_vid_add); 268 269 static void __vlan_vid_del(struct vlan_info *vlan_info, 270 struct vlan_vid_info *vid_info) 271 { 272 struct net_device *dev = vlan_info->real_dev; 273 const struct net_device_ops *ops = dev->netdev_ops; 274 __be16 proto = vid_info->proto; 275 u16 vid = vid_info->vid; 276 int err; 277 278 if (vlan_hw_filter_capable(dev, vid_info)) { 279 if (netif_device_present(dev)) 280 err = ops->ndo_vlan_rx_kill_vid(dev, proto, vid); 281 else 282 err = -ENODEV; 283 if (err) { 284 pr_warn("failed to kill vid %04x/%d for device %s\n", 285 proto, vid, dev->name); 286 } 287 } 288 list_del(&vid_info->list); 289 kfree(vid_info); 290 vlan_info->nr_vids--; 291 } 292 293 void vlan_vid_del(struct net_device *dev, __be16 proto, u16 vid) 294 { 295 struct vlan_info *vlan_info; 296 struct vlan_vid_info *vid_info; 297 298 ASSERT_RTNL(); 299 300 vlan_info = rtnl_dereference(dev->vlan_info); 301 if (!vlan_info) 302 return; 303 304 vid_info = vlan_vid_info_get(vlan_info, proto, vid); 305 if (!vid_info) 306 return; 307 vid_info->refcount--; 308 if (vid_info->refcount == 0) { 309 __vlan_vid_del(vlan_info, vid_info); 310 if (vlan_info->nr_vids == 0) { 311 RCU_INIT_POINTER(dev->vlan_info, NULL); 312 call_rcu(&vlan_info->rcu, vlan_info_rcu_free); 313 } 314 } 315 } 316 EXPORT_SYMBOL(vlan_vid_del); 317 318 int vlan_vids_add_by_dev(struct net_device *dev, 319 const struct net_device *by_dev) 320 { 321 struct vlan_vid_info *vid_info; 322 struct vlan_info *vlan_info; 323 int err; 324 325 ASSERT_RTNL(); 326 327 vlan_info = rtnl_dereference(by_dev->vlan_info); 328 if (!vlan_info) 329 return 0; 330 331 list_for_each_entry(vid_info, &vlan_info->vid_list, list) { 332 err = vlan_vid_add(dev, vid_info->proto, vid_info->vid); 333 if (err) 334 goto unwind; 335 } 336 return 0; 337 338 unwind: 339 list_for_each_entry_continue_reverse(vid_info, 340 &vlan_info->vid_list, 341 list) { 342 vlan_vid_del(dev, vid_info->proto, vid_info->vid); 343 } 344 345 return err; 346 } 347 EXPORT_SYMBOL(vlan_vids_add_by_dev); 348 349 void vlan_vids_del_by_dev(struct net_device *dev, 350 const struct net_device *by_dev) 351 { 352 struct vlan_vid_info *vid_info; 353 struct vlan_info *vlan_info; 354 355 ASSERT_RTNL(); 356 357 vlan_info = rtnl_dereference(by_dev->vlan_info); 358 if (!vlan_info) 359 return; 360 361 list_for_each_entry(vid_info, &vlan_info->vid_list, list) 362 vlan_vid_del(dev, vid_info->proto, vid_info->vid); 363 } 364 EXPORT_SYMBOL(vlan_vids_del_by_dev); 365 366 bool vlan_uses_dev(const struct net_device *dev) 367 { 368 struct vlan_info *vlan_info; 369 370 ASSERT_RTNL(); 371 372 vlan_info = rtnl_dereference(dev->vlan_info); 373 if (!vlan_info) 374 return false; 375 return vlan_info->grp.nr_vlan_devs ? true : false; 376 } 377 EXPORT_SYMBOL(vlan_uses_dev); 378