netfilter: nftables_offload: special ethertype handling for VLAN

The nftables offload parser sets FLOW_DISSECTOR_KEY_BASIC .n_proto to the
ethertype field in in the ethertype frame. However:

- FLOW_DISSECTOR_KEY_BASIC .n_proto field always stores either IPv4 or IPv6
  ethertypes.
- FLOW_DISSECTOR_KEY_VLAN .vlan_tpid stores either the 802.1q and 802.1ad
  ethertypes. Same as for C-VLAN.

This function adjusts the flow dissector to handle three scenarios:

1) FLOW_DISSECTOR_KEY_VLAN and FLOW_DISSECTOR_KEY_CVLAN are set. Then,
   transfer the .n_proto field to FLOW_DISSECTOR_KEY_VLAN .tpid, and
   FLOW_DISSECTOR_KEY_VLAN .tpid to FLOW_DISSECTOR_KEY_CVLAN .tpid.
   Finally set .n_proto to FLOW_DISSECTOR_KEY_CVLAN .tpid.
2) FLOW_DISSECTOR_KEY_VLAN is set. Swap the .n_proto and the
   FLOW_DISSECTOR_KEY_VLAN .tpid fields.
3) ethertype is set to 802.1q or 802.1ad, in this case, transfer the .n_proto
   field to FLOW_DISSECTOR_KEY_VLAN .vlan_tpid.

Fixes: a82055a ("netfilter: nft_payload: add VLAN offload support")
Signed-off-by: Pablo Neira Ayuso <pablo@netfilter.org>

net/netfilter/nf_tables_offload.c

@@ -47,6 +47,45 @@ void nft_flow_rule_set_addr_type(struct nft_flow_rule *flow,
 		offsetof(struct nft_flow_key, control);
 }
 
+struct nft_offload_ethertype {
+	__be16 value;
+	__be16 mask;
+};
+
+void nft_flow_rule_transfer_vlan(struct nft_offload_ctx *ctx,
+				 struct nft_flow_rule *flow)
+{
+	struct nft_flow_match *match = &flow->match;
+	struct nft_offload_ethertype ethertype = {
+		.value	= match->key.basic.n_proto,
+		.mask	= match->mask.basic.n_proto,
+	};
+
+	if ((flow->match.dissector.used_keys &
+	     (BIT(FLOW_DISSECTOR_KEY_VLAN) | BIT(FLOW_DISSECTOR_KEY_CVLAN))) ==
+	    (BIT(FLOW_DISSECTOR_KEY_VLAN) | BIT(FLOW_DISSECTOR_KEY_CVLAN))) {
+		match->key.basic.n_proto = match->key.cvlan.vlan_tpid;
+		match->mask.basic.n_proto = match->mask.cvlan.vlan_tpid;
+		match->key.cvlan.vlan_tpid = match->key.vlan.vlan_tpid;
+		match->mask.cvlan.vlan_tpid = match->mask.vlan.vlan_tpid;
+		match->key.vlan.vlan_tpid = ethertype.value;
+		match->mask.vlan.vlan_tpid = ethertype.mask;
+	} else if (flow->match.dissector.used_keys & (BIT(FLOW_DISSECTOR_KEY_VLAN))) {
+		match->key.basic.n_proto = match->key.vlan.vlan_tpid;
+		match->mask.basic.n_proto = match->mask.vlan.vlan_tpid;
+		match->key.vlan.vlan_tpid = ethertype.value;
+		match->mask.vlan.vlan_tpid = ethertype.mask;
+	} else if (match->key.basic.n_proto == htons(ETH_P_8021Q) ||
+		   match->key.basic.n_proto == htons(ETH_P_8021AD)) {
+		match->key.vlan.vlan_tpid = ethertype.value;
+		match->mask.vlan.vlan_tpid = ethertype.mask;
+		match->dissector.used_keys |= BIT(FLOW_DISSECTOR_KEY_VLAN);
+		match->key.basic.n_proto = 0;
+		match->mask.basic.n_proto = 0;
+		match->dissector.used_keys &= ~BIT(FLOW_DISSECTOR_KEY_BASIC);
+	}
+}
+
 struct nft_flow_rule *nft_flow_rule_create(struct net *net,
 					   const struct nft_rule *rule)
 {
@@ -91,6 +130,8 @@ struct nft_flow_rule *nft_flow_rule_create(struct net *net,
 
 		expr = nft_expr_next(expr);
 	}
+	nft_flow_rule_transfer_vlan(ctx, flow);
+
 	flow->proto = ctx->dep.l3num;
 	kfree(ctx);