[PATCH nf-next v3] netfilter:nft_meta: add NFT_META_VLAN support

[wenxu]

From: wenxu <we...@ucloud.cn>

This patch provide a meta vlan to set the vlan tag of the packet.

for q-in-q outer vlan id 20:
meta vlan set 0x88a8:20

set the default 0x8100 vlan type with vlan id 20
meta vlan set 20

Signed-off-by: wenxu <we...@ucloud.cn>
---
 include/net/netfilter/nft_meta.h         |  5 ++++-
 include/uapi/linux/netfilter/nf_tables.h |  4 ++++
 net/netfilter/nft_meta.c                 | 27 +++++++++++++++++++++++++++
 3 files changed, 35 insertions(+), 1 deletion(-)

diff --git a/include/net/netfilter/nft_meta.h b/include/net/netfilter/nft_meta.h
index 5c69e9b..cb0f1e8 100644
--- a/include/net/netfilter/nft_meta.h
+++ b/include/net/netfilter/nft_meta.h
@@ -6,7 +6,10 @@ struct nft_meta {
        enum nft_meta_keys      key:8;
        union {
                enum nft_registers      dreg:8;
-               enum nft_registers      sreg:8;
+               struct {
+                       enum nft_registers      sreg:8;
+                       enum nft_registers      sreg2:8;
+               };
        };
 };
 
diff --git a/include/uapi/linux/netfilter/nf_tables.h 
b/include/uapi/linux/netfilter/nf_tables.h
index a0d1dbd..699524a 100644
--- a/include/uapi/linux/netfilter/nf_tables.h
+++ b/include/uapi/linux/netfilter/nf_tables.h
@@ -797,6 +797,7 @@ enum nft_exthdr_attributes {
  * @NFT_META_OIFKIND: packet output interface kind name 
(dev->rtnl_link_ops->kind)
  * @NFT_META_BRI_IIFPVID: packet input bridge port pvid
  * @NFT_META_BRI_IIFVPROTO: packet input bridge vlan proto
+ * @NFT_META_VLAN: packet vlan metadata
  */
 enum nft_meta_keys {
        NFT_META_LEN,
@@ -829,6 +830,7 @@ enum nft_meta_keys {
        NFT_META_OIFKIND,
        NFT_META_BRI_IIFPVID,
        NFT_META_BRI_IIFVPROTO,
+       NFT_META_VLAN,
 };
 
 /**
@@ -895,12 +897,14 @@ enum nft_hash_attributes {
  * @NFTA_META_DREG: destination register (NLA_U32)
  * @NFTA_META_KEY: meta data item to load (NLA_U32: nft_meta_keys)
  * @NFTA_META_SREG: source register (NLA_U32)
+ * @NFTA_META_SREG2: source register (NLA_U32)
  */
 enum nft_meta_attributes {
        NFTA_META_UNSPEC,
        NFTA_META_DREG,
        NFTA_META_KEY,
        NFTA_META_SREG,
+       NFTA_META_SREG2,
        __NFTA_META_MAX
 };
 #define NFTA_META_MAX          (__NFTA_META_MAX - 1)
diff --git a/net/netfilter/nft_meta.c b/net/netfilter/nft_meta.c
index 18a848b..901ad2e 100644
--- a/net/netfilter/nft_meta.c
+++ b/net/netfilter/nft_meta.c
@@ -271,6 +271,22 @@ void nft_meta_set_eval(const struct nft_expr *expr,
                skb->secmark = value;
                break;
 #endif
+       case NFT_META_VLAN: {
+               u32 *sreg2 = &regs->data[meta->sreg2];
+               u16 vlan_proto;
+               u16 vlan_tci;
+
+               vlan_tci = nft_reg_load16(sreg);
+               vlan_proto = nft_reg_load16(sreg2);
+
+               if (vlan_proto != ETH_P_8021Q && vlan_proto != ETH_P_8021AD)
+                       return;
+
+               skb_push_rcsum(skb, skb->mac_len);
+               skb_vlan_push(skb, htons(vlan_proto), vlan_tci & VLAN_VID_MASK);
+               skb_pull_rcsum(skb, skb->mac_len);
+               break;
+       }
        default:
                WARN_ON(1);
        }
@@ -281,6 +297,7 @@ void nft_meta_set_eval(const struct nft_expr *expr,
        [NFTA_META_DREG]        = { .type = NLA_U32 },
        [NFTA_META_KEY]         = { .type = NLA_U32 },
        [NFTA_META_SREG]        = { .type = NLA_U32 },
+       [NFTA_META_SREG2]       = { .type = NLA_U32 },
 };
 EXPORT_SYMBOL_GPL(nft_meta_policy);
 
@@ -432,6 +449,13 @@ int nft_meta_set_init(const struct nft_ctx *ctx,
        case NFT_META_PKTTYPE:
                len = sizeof(u8);
                break;
+       case NFT_META_VLAN:
+               len = sizeof(u16);
+               priv->sreg2 = nft_parse_register(tb[NFTA_META_SREG2]);
+               err = nft_validate_register_load(priv->sreg2, len);
+               if (err < 0)
+                       return err;
+               break;
        default:
                return -EOPNOTSUPP;
        }
@@ -457,6 +481,9 @@ int nft_meta_get_dump(struct sk_buff *skb,
                goto nla_put_failure;
        if (nft_dump_register(skb, NFTA_META_DREG, priv->dreg))
                goto nla_put_failure;
+       if (priv->key == NFT_META_VLAN &&
+           nft_dump_register(skb, NFTA_META_SREG2, priv->sreg2))
+               goto nla_put_failure;
        return 0;
 
 nla_put_failure:
-- 
1.8.3.1