Fix QinQ outer VLAN direction detection (#1147)

src/rust/lqos_sys/src/bpf/common/dissector.h

@@ -41,8 +41,7 @@ struct dissector_t
     __u16 l3offset;
     // Ethernet packet type once found (0 until then)
     __u16 eth_type;
-    // Current VLAN tag. If there are multiple tags, it will be
-    // the INNER tag.
+    // Outer VLAN ID, without priority or drop-eligible bits.
     __be16 current_vlan;
     __u16 src_port;
     __u16 dst_port;
@@ -79,6 +78,12 @@ struct pppoe_proto
 #define PPPOE_SES_HLEN 8
 #define PPP_IP 0x21
 #define PPP_IPV6 0x57
+#define LQOS_VLAN_VID_MASK 0x0FFF
+
+static __always_inline __be16 lqos_vlan_vid(__be16 vlan_tci)
+{
+    return bpf_htons(bpf_ntohs(vlan_tci) & LQOS_VLAN_VID_MASK);
+}
 
 // Representation of an MPLS label
 struct mpls_label
@@ -180,7 +185,10 @@ static __always_inline bool dissector_find_l3_offset(
                 return false;
             }
             struct vlan_hdr *vlan = (struct vlan_hdr *)(dissector->start + offset);
-            dissector->current_vlan = vlan->h_vlan_TCI;
+            __be16 vlan_tci = vlan->h_vlan_TCI;
+            if (offset == sizeof(struct ethhdr)) {
+                dissector->current_vlan = lqos_vlan_vid(vlan_tci);
+            }
             eth_type = bpf_ntohs(vlan->h_vlan_encapsulated_proto);
             offset += sizeof(struct vlan_hdr);
             // VLAN Redirection is requested, so lookup a detination and
@@ -190,10 +198,10 @@ static __always_inline bool dissector_find_l3_offset(
 #ifdef VERBOSE
                 bpf_debug("Searching for redirect %u:%u",
                           dissector->ctx->ingress_ifindex,
-                          bpf_ntohs(dissector->current_vlan));
+                          bpf_ntohs(vlan_tci));
 #endif
                 __u32 key = (dissector->ctx->ingress_ifindex << 16) |
-                            bpf_ntohs(dissector->current_vlan);
+                            bpf_ntohs(vlan_tci);
                 struct bifrost_vlan *vlan_info = NULL;
                 vlan_info = bpf_map_lookup_elem(&bifrost_vlan_map, &key);
                 if (vlan_info)

src/rust/lqos_sys/src/bpf/common/dissector_tc.h

@@ -31,7 +31,7 @@ struct tc_dissector_t
     struct in6_addr src_ip;
     // Destination IP, encoded by `ip_hash.h` functions.
     struct in6_addr dst_ip;
-    // Current VLAN detected.
+    // Outer VLAN ID, without priority or drop-eligible bits.
     // TODO: This can probably be removed since the packet dissector
     // now finds this.
     __be16 current_vlan;
@@ -53,7 +53,7 @@ static __always_inline bool tc_dissector_new(
     dissector->end = (void *)(long)ctx->data_end;
     dissector->ethernet_header = (struct ethhdr *)NULL;
     dissector->l3offset = 0;
-    dissector->current_vlan = bpf_htons(ctx->vlan_tci);
+    dissector->current_vlan = lqos_vlan_vid(bpf_htons(ctx->vlan_tci));
 
     // Check that there's room for an ethernet header
     if SKB_OVERFLOW (dissector->start, dissector->end, ethhdr)
@@ -110,8 +110,9 @@ static __always_inline bool tc_dissector_find_l3_offset(
             //bpf_debug("TC Found VLAN");
             struct vlan_hdr *vlan = (struct vlan_hdr *)
                 (dissector->start + offset);
-            // Calculated from the SKB
-            //dissector->current_vlan = vlan->h_vlan_TCI;
+            if (dissector->current_vlan == 0 && offset == sizeof(struct ethhdr)) {
+                dissector->current_vlan = lqos_vlan_vid(vlan->h_vlan_TCI);
+            }
             eth_type = bpf_ntohs(vlan->h_vlan_encapsulated_proto);
             offset += sizeof(struct vlan_hdr);
         }
@@ -221,4 +222,4 @@ static __always_inline bool tc_dissector_find_ip_header(
     default:
         return false;
     }
-}
+}

src/rust/lqos_sys/src/bpf/common/kprobe_helper.h

@@ -35,7 +35,9 @@ static __always_inline __be16 kprobe_scan_current_vlan(struct sk_buff *skb)
         if (bpf_probe_read_kernel(&vlan, sizeof(vlan), data + offset) < 0) {
             break;
         }
-        current_vlan = vlan.h_vlan_TCI;
+        if (i == 0) {
+            current_vlan = lqos_vlan_vid(vlan.h_vlan_TCI);
+        }
         eth_type = bpf_ntohs(vlan.h_vlan_encapsulated_proto);
         offset += sizeof(struct vlan_hdr_kprobe);
     }
@@ -61,7 +63,7 @@ static __always_inline __be16 kprobe_current_vlan(struct sk_buff *skb)
     // Prefer the skb metadata tag (matches TC egress behavior).
     __u16 vlan_tci = BPF_CORE_READ(skb, vlan_tci);
     if (vlan_tci) {
-        return bpf_htons(vlan_tci);
+        return lqos_vlan_vid(bpf_htons(vlan_tci));
     }
     // Fall back to scanning the L2 header for non-accelerated VLAN tags.
     return kprobe_scan_current_vlan(skb);