@@ -538,6 +538,61 @@ static void *packet_current_frame(struct packet_sock *po,
538538 return packet_lookup_frame (po , rb , rb -> head , status );
539539}
540540
541+ static u16 vlan_get_tci (struct sk_buff * skb , struct net_device * dev )
542+ {
543+ u8 * skb_orig_data = skb -> data ;
544+ int skb_orig_len = skb -> len ;
545+ struct vlan_hdr vhdr , * vh ;
546+ unsigned int header_len ;
547+
548+ if (!dev )
549+ return 0 ;
550+
551+ /* In the SOCK_DGRAM scenario, skb data starts at the network
552+ * protocol, which is after the VLAN headers. The outer VLAN
553+ * header is at the hard_header_len offset in non-variable
554+ * length link layer headers. If it's a VLAN device, the
555+ * min_header_len should be used to exclude the VLAN header
556+ * size.
557+ */
558+ if (dev -> min_header_len == dev -> hard_header_len )
559+ header_len = dev -> hard_header_len ;
560+ else if (is_vlan_dev (dev ))
561+ header_len = dev -> min_header_len ;
562+ else
563+ return 0 ;
564+
565+ skb_push (skb , skb -> data - skb_mac_header (skb ));
566+ vh = skb_header_pointer (skb , header_len , sizeof (vhdr ), & vhdr );
567+ if (skb_orig_data != skb -> data ) {
568+ skb -> data = skb_orig_data ;
569+ skb -> len = skb_orig_len ;
570+ }
571+ if (unlikely (!vh ))
572+ return 0 ;
573+
574+ return ntohs (vh -> h_vlan_TCI );
575+ }
576+
577+ static __be16 vlan_get_protocol_dgram (struct sk_buff * skb )
578+ {
579+ __be16 proto = skb -> protocol ;
580+
581+ if (unlikely (eth_type_vlan (proto ))) {
582+ u8 * skb_orig_data = skb -> data ;
583+ int skb_orig_len = skb -> len ;
584+
585+ skb_push (skb , skb -> data - skb_mac_header (skb ));
586+ proto = __vlan_get_protocol (skb , proto , NULL );
587+ if (skb_orig_data != skb -> data ) {
588+ skb -> data = skb_orig_data ;
589+ skb -> len = skb_orig_len ;
590+ }
591+ }
592+
593+ return proto ;
594+ }
595+
541596static void prb_del_retire_blk_timer (struct tpacket_kbdq_core * pkc )
542597{
543598 del_timer_sync (& pkc -> retire_blk_timer );
@@ -1007,10 +1062,16 @@ static void prb_clear_rxhash(struct tpacket_kbdq_core *pkc,
10071062static void prb_fill_vlan_info (struct tpacket_kbdq_core * pkc ,
10081063 struct tpacket3_hdr * ppd )
10091064{
1065+ struct packet_sock * po = container_of (pkc , struct packet_sock , rx_ring .prb_bdqc );
1066+
10101067 if (skb_vlan_tag_present (pkc -> skb )) {
10111068 ppd -> hv1 .tp_vlan_tci = skb_vlan_tag_get (pkc -> skb );
10121069 ppd -> hv1 .tp_vlan_tpid = ntohs (pkc -> skb -> vlan_proto );
10131070 ppd -> tp_status = TP_STATUS_VLAN_VALID | TP_STATUS_VLAN_TPID_VALID ;
1071+ } else if (unlikely (po -> sk .sk_type == SOCK_DGRAM && eth_type_vlan (pkc -> skb -> protocol ))) {
1072+ ppd -> hv1 .tp_vlan_tci = vlan_get_tci (pkc -> skb , pkc -> skb -> dev );
1073+ ppd -> hv1 .tp_vlan_tpid = ntohs (pkc -> skb -> protocol );
1074+ ppd -> tp_status = TP_STATUS_VLAN_VALID | TP_STATUS_VLAN_TPID_VALID ;
10141075 } else {
10151076 ppd -> hv1 .tp_vlan_tci = 0 ;
10161077 ppd -> hv1 .tp_vlan_tpid = 0 ;
@@ -2428,6 +2489,10 @@ static int tpacket_rcv(struct sk_buff *skb, struct net_device *dev,
24282489 h .h2 -> tp_vlan_tci = skb_vlan_tag_get (skb );
24292490 h .h2 -> tp_vlan_tpid = ntohs (skb -> vlan_proto );
24302491 status |= TP_STATUS_VLAN_VALID | TP_STATUS_VLAN_TPID_VALID ;
2492+ } else if (unlikely (sk -> sk_type == SOCK_DGRAM && eth_type_vlan (skb -> protocol ))) {
2493+ h .h2 -> tp_vlan_tci = vlan_get_tci (skb , skb -> dev );
2494+ h .h2 -> tp_vlan_tpid = ntohs (skb -> protocol );
2495+ status |= TP_STATUS_VLAN_VALID | TP_STATUS_VLAN_TPID_VALID ;
24312496 } else {
24322497 h .h2 -> tp_vlan_tci = 0 ;
24332498 h .h2 -> tp_vlan_tpid = 0 ;
@@ -2457,7 +2522,8 @@ static int tpacket_rcv(struct sk_buff *skb, struct net_device *dev,
24572522 sll -> sll_halen = dev_parse_header (skb , sll -> sll_addr );
24582523 sll -> sll_family = AF_PACKET ;
24592524 sll -> sll_hatype = dev -> type ;
2460- sll -> sll_protocol = skb -> protocol ;
2525+ sll -> sll_protocol = (sk -> sk_type == SOCK_DGRAM ) ?
2526+ vlan_get_protocol_dgram (skb ) : skb -> protocol ;
24612527 sll -> sll_pkttype = skb -> pkt_type ;
24622528 if (unlikely (packet_sock_flag (po , PACKET_SOCK_ORIGDEV )))
24632529 sll -> sll_ifindex = orig_dev -> ifindex ;
@@ -3482,7 +3548,8 @@ static int packet_recvmsg(struct socket *sock, struct msghdr *msg, size_t len,
34823548 /* Original length was stored in sockaddr_ll fields */
34833549 origlen = PACKET_SKB_CB (skb )-> sa .origlen ;
34843550 sll -> sll_family = AF_PACKET ;
3485- sll -> sll_protocol = skb -> protocol ;
3551+ sll -> sll_protocol = (sock -> type == SOCK_DGRAM ) ?
3552+ vlan_get_protocol_dgram (skb ) : skb -> protocol ;
34863553 }
34873554
34883555 sock_recv_cmsgs (msg , sk , skb );
@@ -3539,6 +3606,21 @@ static int packet_recvmsg(struct socket *sock, struct msghdr *msg, size_t len,
35393606 aux .tp_vlan_tci = skb_vlan_tag_get (skb );
35403607 aux .tp_vlan_tpid = ntohs (skb -> vlan_proto );
35413608 aux .tp_status |= TP_STATUS_VLAN_VALID | TP_STATUS_VLAN_TPID_VALID ;
3609+ } else if (unlikely (sock -> type == SOCK_DGRAM && eth_type_vlan (skb -> protocol ))) {
3610+ struct sockaddr_ll * sll = & PACKET_SKB_CB (skb )-> sa .ll ;
3611+ struct net_device * dev ;
3612+
3613+ rcu_read_lock ();
3614+ dev = dev_get_by_index_rcu (sock_net (sk ), sll -> sll_ifindex );
3615+ if (dev ) {
3616+ aux .tp_vlan_tci = vlan_get_tci (skb , dev );
3617+ aux .tp_vlan_tpid = ntohs (skb -> protocol );
3618+ aux .tp_status |= TP_STATUS_VLAN_VALID | TP_STATUS_VLAN_TPID_VALID ;
3619+ } else {
3620+ aux .tp_vlan_tci = 0 ;
3621+ aux .tp_vlan_tpid = 0 ;
3622+ }
3623+ rcu_read_unlock ();
35423624 } else {
35433625 aux .tp_vlan_tci = 0 ;
35443626 aux .tp_vlan_tpid = 0 ;
0 commit comments