vlan_core.c 9.1 KB

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