@@ -538,6 +538,61 @@ static void *packet_current_frame(struct packet_sock *po,
538
538
return packet_lookup_frame (po , rb , rb -> head , status );
539
539
}
540
540
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
+
541
596
static void prb_del_retire_blk_timer (struct tpacket_kbdq_core * pkc )
542
597
{
543
598
del_timer_sync (& pkc -> retire_blk_timer );
@@ -1007,10 +1062,16 @@ static void prb_clear_rxhash(struct tpacket_kbdq_core *pkc,
1007
1062
static void prb_fill_vlan_info (struct tpacket_kbdq_core * pkc ,
1008
1063
struct tpacket3_hdr * ppd )
1009
1064
{
1065
+ struct packet_sock * po = container_of (pkc , struct packet_sock , rx_ring .prb_bdqc );
1066
+
1010
1067
if (skb_vlan_tag_present (pkc -> skb )) {
1011
1068
ppd -> hv1 .tp_vlan_tci = skb_vlan_tag_get (pkc -> skb );
1012
1069
ppd -> hv1 .tp_vlan_tpid = ntohs (pkc -> skb -> vlan_proto );
1013
1070
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 ;
1014
1075
} else {
1015
1076
ppd -> hv1 .tp_vlan_tci = 0 ;
1016
1077
ppd -> hv1 .tp_vlan_tpid = 0 ;
@@ -2428,6 +2489,10 @@ static int tpacket_rcv(struct sk_buff *skb, struct net_device *dev,
2428
2489
h .h2 -> tp_vlan_tci = skb_vlan_tag_get (skb );
2429
2490
h .h2 -> tp_vlan_tpid = ntohs (skb -> vlan_proto );
2430
2491
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 ;
2431
2496
} else {
2432
2497
h .h2 -> tp_vlan_tci = 0 ;
2433
2498
h .h2 -> tp_vlan_tpid = 0 ;
@@ -2457,7 +2522,8 @@ static int tpacket_rcv(struct sk_buff *skb, struct net_device *dev,
2457
2522
sll -> sll_halen = dev_parse_header (skb , sll -> sll_addr );
2458
2523
sll -> sll_family = AF_PACKET ;
2459
2524
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 ;
2461
2527
sll -> sll_pkttype = skb -> pkt_type ;
2462
2528
if (unlikely (packet_sock_flag (po , PACKET_SOCK_ORIGDEV )))
2463
2529
sll -> sll_ifindex = orig_dev -> ifindex ;
@@ -3482,7 +3548,8 @@ static int packet_recvmsg(struct socket *sock, struct msghdr *msg, size_t len,
3482
3548
/* Original length was stored in sockaddr_ll fields */
3483
3549
origlen = PACKET_SKB_CB (skb )-> sa .origlen ;
3484
3550
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 ;
3486
3553
}
3487
3554
3488
3555
sock_recv_cmsgs (msg , sk , skb );
@@ -3539,6 +3606,21 @@ static int packet_recvmsg(struct socket *sock, struct msghdr *msg, size_t len,
3539
3606
aux .tp_vlan_tci = skb_vlan_tag_get (skb );
3540
3607
aux .tp_vlan_tpid = ntohs (skb -> vlan_proto );
3541
3608
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 ();
3542
3624
} else {
3543
3625
aux .tp_vlan_tci = 0 ;
3544
3626
aux .tp_vlan_tpid = 0 ;
0 commit comments