net: erspan: introduce erspan v2 for ip_gre

The patch adds support for erspan version 2.  Not all features are
supported in this patch.  The SGT (security group tag), GRA (timestamp
granularity), FT (frame type) are set to fixed value.  Only hardware
ID and direction are configurable.  Optional subheader is also not
supported.

Signed-off-by: William Tu <u9012063@gmail.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

5 files changed, 216 insertions, 18 deletions

diff --git a/include/net/erspan.h b/include/net/erspan.h
index 70c40c7c75b2..acdf6843095d 100644
--- a/include/net/erspan.h
+++ b/include/net/erspan.h
@@ -24,11 +24,29 @@
  * |      Reserved         |                  Index                |
  * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  *
+ *
+ *  ERSPAN Version 2 (Type III) header (12 octets [42:49])
+ *  0                   1                   2                   3
+ *  0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |  Ver  |          VLAN         | COS |BSO|T|     Session ID    |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |                          Timestamp                            |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |             SGT               |P|    FT   |   Hw ID   |D|Gra|O|
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *
+ *      Platform Specific SubHeader (8 octets, optional)
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |  Platf ID |               Platform Specific Info              |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |                  Platform Specific Info                       |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *
  * GRE proto ERSPAN type II = 0x88BE, type III = 0x22EB
  */
 
 #define ERSPAN_VERSION  0x1     /* ERSPAN type II */
-
 #define VER_MASK        0xf000
 #define VLAN_MASK       0x0fff
 #define COS_MASK        0xe000
@@ -37,6 +55,28 @@
 #define ID_MASK         0x03ff
 #define INDEX_MASK      0xfffff
 
+#define ERSPAN_VERSION2 0x2     /* ERSPAN type III*/
+#define BSO_MASK        EN_MASK
+#define SGT_MASK        0xffff0000
+#define P_MASK          0x8000
+#define FT_MASK         0x7c00
+#define HWID_MASK       0x03f0
+#define DIR_MASK        0x0008
+#define GRA_MASK        0x0006
+#define O_MASK          0x0001
+
+/* ERSPAN version 2 metadata header */
+struct erspan_md2 {
+        __be32 timestamp;
+        __be16 sgt;     /* security group tag */
+        __be16 flags;
+#define P_OFFSET        15
+#define FT_OFFSET       10
+#define HWID_OFFSET     4
+#define DIR_OFFSET      3
+#define GRA_OFFSET      1
+};
+
 enum erspan_encap_type {
         ERSPAN_ENCAP_NOVLAN = 0x0,      /* originally without VLAN tag */
         ERSPAN_ENCAP_ISL = 0x1,         /* originally ISL encapsulated */
@@ -48,8 +88,10 @@ enum erspan_encap_type {
 #define ERSPAN_V2_MDSIZE        8
 struct erspan_metadata {
         union {
-                __be32 index;   /* Version 1 (type II)*/
+                __be32 index;           /* Version 1 (type II)*/
+                struct erspan_md2 md2;  /* Version 2 (type III) */
         } u;
+        int version;
 };
 
 struct erspan_base_hdr {
@@ -58,6 +100,7 @@ struct erspan_base_hdr {
         __be16 session_id;
 #define COS_OFFSET  13
 #define EN_OFFSET   11
+#define BSO_OFFSET  EN_OFFSET
 #define T_OFFSET    10
 };
 
@@ -123,4 +166,77 @@ static inline void erspan_build_header(struct sk_buff *skb,
         ersmd->u.index = htonl(index & INDEX_MASK);
 }
 
+/* ERSPAN GRA: timestamp granularity
+ *   00b --> granularity = 100 microseconds
+ *   01b --> granularity = 100 nanoseconds
+ *   10b --> granularity = IEEE 1588
+ * Here we only support 100 microseconds.
+ */
+static inline __be32 erspan_get_timestamp(void)
+{
+        u64 h_usecs;
+        ktime_t kt;
+
+        kt = ktime_get_real();
+        h_usecs = ktime_divns(kt, 100 * NSEC_PER_USEC);
+
+        /* ERSPAN base header only has 32-bit,
+         * so it wraps around 4 days.
+         */
+        return htonl((u32)h_usecs);
+}
+
+static inline void erspan_build_header_v2(struct sk_buff *skb,
+                                          __be32 id, u8 direction, u16 hwid,
+                                          bool truncate, bool is_ipv4)
+{
+        struct ethhdr *eth = eth_hdr(skb);
+        struct erspan_base_hdr *ershdr;
+        struct erspan_metadata *md;
+        struct qtag_prefix {
+                __be16 eth_type;
+                __be16 tci;
+        } *qp;
+        u16 vlan_tci = 0;
+        u16 session_id;
+        u8 gra = 0; /* 100 usec */
+        u8 bso = 0; /* Bad/Short/Oversized */
+        u8 sgt = 0;
+        u8 tos;
+
+        tos = is_ipv4 ? ip_hdr(skb)->tos :
+                        (ipv6_hdr(skb)->priority << 4) +
+                        (ipv6_hdr(skb)->flow_lbl[0] >> 4);
+
+        /* Unlike v1, v2 does not have En field,
+         * so only extract vlan tci field.
+         */
+        if (eth->h_proto == htons(ETH_P_8021Q)) {
+                qp = (struct qtag_prefix *)(skb->data + 2 * ETH_ALEN);
+                vlan_tci = ntohs(qp->tci);
+        }
+
+        skb_push(skb, sizeof(*ershdr) + ERSPAN_V2_MDSIZE);
+        ershdr = (struct erspan_base_hdr *)skb->data;
+        memset(ershdr, 0, sizeof(*ershdr) + ERSPAN_V2_MDSIZE);
+
+        /* Build base header */
+        ershdr->ver_vlan = htons((vlan_tci & VLAN_MASK) |
+                                 (ERSPAN_VERSION2 << VER_OFFSET));
+        session_id = (u16)(ntohl(id) & ID_MASK) |
+                     ((tos_to_cos(tos) << COS_OFFSET) & COS_MASK) |
+                     (bso << BSO_OFFSET & BSO_MASK) |
+                     ((truncate << T_OFFSET) & T_MASK);
+        ershdr->session_id = htons(session_id);
+
+        /* Build metadata */
+        md = (struct erspan_metadata *)(ershdr + 1);
+        md->u.md2.timestamp = erspan_get_timestamp();
+        md->u.md2.sgt = htons(sgt);
+        md->u.md2.flags = htons(((1 << P_OFFSET) & P_MASK) |
+                                ((hwid << HWID_OFFSET) & HWID_MASK) |
+                                ((direction << DIR_OFFSET) & DIR_MASK) |
+                                ((gra << GRA_OFFSET) & GRA_MASK));
+}
+
 #endif
diff --git a/include/net/ip_tunnels.h b/include/net/ip_tunnels.h
index 24628f6b09bf..1f16773cfd76 100644
--- a/include/net/ip_tunnels.h
+++ b/include/net/ip_tunnels.h
@@ -116,8 +116,11 @@ struct ip_tunnel {
         u32             o_seqno;        /* The last output seqno */
         int             tun_hlen;       /* Precalculated header length */
 
-        /* This field used only by ERSPAN */
+        /* These four fields used only by ERSPAN */
         u32             index;          /* ERSPAN type II index */
+        u8              erspan_ver;     /* ERSPAN version */
+        u8              dir;            /* ERSPAN direction */
+        u16             hwid;           /* ERSPAN hardware ID */
 
         struct dst_cache dst_cache;
 
diff --git a/include/uapi/linux/if_ether.h b/include/uapi/linux/if_ether.h
index 3ee3bf7c8526..87b7529fcdfe 100644
--- a/include/uapi/linux/if_ether.h
+++ b/include/uapi/linux/if_ether.h
@@ -47,6 +47,7 @@
 #define ETH_P_PUP       0x0200          /* Xerox PUP packet             */
 #define ETH_P_PUPAT     0x0201          /* Xerox PUP Addr Trans packet  */
 #define ETH_P_TSN       0x22F0          /* TSN (IEEE 1722) packet       */
+#define ETH_P_ERSPAN2   0x22EB          /* ERSPAN version 2 (type III)  */
 #define ETH_P_IP        0x0800          /* Internet Protocol packet     */
 #define ETH_P_X25       0x0805          /* CCITT X.25                   */
 #define ETH_P_ARP       0x0806          /* Address Resolution packet    */
diff --git a/include/uapi/linux/if_tunnel.h b/include/uapi/linux/if_tunnel.h
index e68dadbd6d45..1b3d148c4560 100644
--- a/include/uapi/linux/if_tunnel.h
+++ b/include/uapi/linux/if_tunnel.h
@@ -137,6 +137,9 @@ enum {
         IFLA_GRE_IGNORE_DF,
         IFLA_GRE_FWMARK,
         IFLA_GRE_ERSPAN_INDEX,
+        IFLA_GRE_ERSPAN_VER,
+        IFLA_GRE_ERSPAN_DIR,
+        IFLA_GRE_ERSPAN_HWID,
         __IFLA_GRE_MAX,
 };
 
diff --git a/net/ipv4/ip_gre.c b/net/ipv4/ip_gre.c
index 3e37402147f3..004800b923c6 100644
--- a/net/ipv4/ip_gre.c
+++ b/net/ipv4/ip_gre.c
@@ -315,11 +315,26 @@ static int erspan_rcv(struct sk_buff *skb, struct tnl_ptk_info *tpi,
                                 return PACKET_REJECT;
 
                         memcpy(md, pkt_md, sizeof(*md));
+                        md->version = ver;
+
                         info = &tun_dst->u.tun_info;
                         info->key.tun_flags |= TUNNEL_ERSPAN_OPT;
                         info->options_len = sizeof(*md);
                 } else {
-                        tunnel->index = ntohl(pkt_md->u.index);
+                        tunnel->erspan_ver = ver;
+                        if (ver == 1) {
+                                tunnel->index = ntohl(pkt_md->u.index);
+                        } else {
+                                u16 md2_flags;
+                                u16 dir, hwid;
+
+                                md2_flags = ntohs(pkt_md->u.md2.flags);
+                                dir = (md2_flags & DIR_MASK) >> DIR_OFFSET;
+                                hwid = (md2_flags & HWID_MASK) >> HWID_OFFSET;
+                                tunnel->dir = dir;
+                                tunnel->hwid = hwid;
+                        }
+
                 }
 
                 skb_reset_mac_header(skb);
@@ -413,7 +428,8 @@ static int gre_rcv(struct sk_buff *skb)
         if (hdr_len < 0)
                 goto drop;
 
-        if (unlikely(tpi.proto == htons(ETH_P_ERSPAN))) {
+        if (unlikely(tpi.proto == htons(ETH_P_ERSPAN) ||
+                     tpi.proto == htons(ETH_P_ERSPAN2))) {
                 if (erspan_rcv(skb, &tpi, hdr_len) == PACKET_RCVD)
                         return 0;
         }
@@ -568,6 +584,7 @@ static void erspan_fb_xmit(struct sk_buff *skb, struct net_device *dev,
         bool truncate = false;
         struct flowi4 fl;
         int tunnel_hlen;
+        int version;
         __be16 df;
 
         tun_info = skb_tunnel_info(skb);
@@ -576,9 +593,13 @@ static void erspan_fb_xmit(struct sk_buff *skb, struct net_device *dev,
                 goto err_free_skb;
 
         key = &tun_info->key;
+        md = ip_tunnel_info_opts(tun_info);
+        if (!md)
+                goto err_free_rt;
 
         /* ERSPAN has fixed 8 byte GRE header */
-        tunnel_hlen = 8 + sizeof(struct erspan_base_hdr) + ERSPAN_V1_MDSIZE;
+        version = md->version;
+        tunnel_hlen = 8 + erspan_hdr_len(version);
 
         rt = prepare_fb_xmit(skb, dev, &fl, tunnel_hlen);
         if (!rt)
@@ -592,12 +613,23 @@ static void erspan_fb_xmit(struct sk_buff *skb, struct net_device *dev,
                 truncate = true;
         }
 
-        md = ip_tunnel_info_opts(tun_info);
-        if (!md)
-                goto err_free_rt;
+        if (version == 1) {
+                erspan_build_header(skb, tunnel_id_to_key32(key->tun_id),
+                                    ntohl(md->u.index), truncate, true);
+        } else if (version == 2) {
+                u16 md2_flags;
+                u8 direction;
+                u16 hwid;
 
-        erspan_build_header(skb, tunnel_id_to_key32(key->tun_id),
-                            ntohl(md->u.index), truncate, true);
+                md2_flags = ntohs(md->u.md2.flags);
+                direction = (md2_flags & DIR_MASK) >> DIR_OFFSET;
+                hwid = (md2_flags & HWID_MASK) >> HWID_OFFSET;
+
+                erspan_build_header_v2(skb, tunnel_id_to_key32(key->tun_id),
+                                       direction, hwid, truncate, true);
+        } else {
+                goto err_free_rt;
+        }
 
         gre_build_header(skb, 8, TUNNEL_SEQ,
                          htons(ETH_P_ERSPAN), 0, htonl(tunnel->o_seqno++));
@@ -699,8 +731,14 @@ static netdev_tx_t erspan_xmit(struct sk_buff *skb,
         }
 
         /* Push ERSPAN header */
-        erspan_build_header(skb, tunnel->parms.o_key, tunnel->index,
-                            truncate, true);
+        if (tunnel->erspan_ver == 1)
+                erspan_build_header(skb, tunnel->parms.o_key, tunnel->index,
+                                    truncate, true);
+        else
+                erspan_build_header_v2(skb, tunnel->parms.o_key,
+                                       tunnel->dir, tunnel->hwid,
+                                       truncate, true);
+
         tunnel->parms.o_flags &= ~TUNNEL_KEY;
         __gre_xmit(skb, dev, &tunnel->parms.iph, htons(ETH_P_ERSPAN));
         return NETDEV_TX_OK;
@@ -1172,13 +1210,32 @@ static int ipgre_netlink_parms(struct net_device *dev,
         if (data[IFLA_GRE_FWMARK])
                 *fwmark = nla_get_u32(data[IFLA_GRE_FWMARK]);
 
-        if (data[IFLA_GRE_ERSPAN_INDEX]) {
-                t->index = nla_get_u32(data[IFLA_GRE_ERSPAN_INDEX]);
+        if (data[IFLA_GRE_ERSPAN_VER]) {
+                t->erspan_ver = nla_get_u8(data[IFLA_GRE_ERSPAN_VER]);
 
-                if (t->index & ~INDEX_MASK)
+                if (t->erspan_ver != 1 && t->erspan_ver != 2)
                         return -EINVAL;
         }
 
+        if (t->erspan_ver == 1) {
+                if (data[IFLA_GRE_ERSPAN_INDEX]) {
+                        t->index = nla_get_u32(data[IFLA_GRE_ERSPAN_INDEX]);
+                        if (t->index & ~INDEX_MASK)
+                                return -EINVAL;
+                }
+        } else if (t->erspan_ver == 2) {
+                if (data[IFLA_GRE_ERSPAN_DIR]) {
+                        t->dir = nla_get_u8(data[IFLA_GRE_ERSPAN_DIR]);
+                        if (t->dir & ~(DIR_MASK >> DIR_OFFSET))
+                                return -EINVAL;
+                }
+                if (data[IFLA_GRE_ERSPAN_HWID]) {
+                        t->hwid = nla_get_u16(data[IFLA_GRE_ERSPAN_HWID]);
+                        if (t->hwid & ~(HWID_MASK >> HWID_OFFSET))
+                                return -EINVAL;
+                }
+        }
+
         return 0;
 }
 
@@ -1245,7 +1302,7 @@ static int erspan_tunnel_init(struct net_device *dev)
         tunnel->tun_hlen = 8;
         tunnel->parms.iph.protocol = IPPROTO_GRE;
         tunnel->hlen = tunnel->tun_hlen + tunnel->encap_hlen +
-                       sizeof(struct erspan_base_hdr) + ERSPAN_V1_MDSIZE;
+                       erspan_hdr_len(tunnel->erspan_ver);
         t_hlen = tunnel->hlen + sizeof(struct iphdr);
 
         dev->needed_headroom = LL_MAX_HEADER + t_hlen + 4;
@@ -1375,6 +1432,12 @@ static size_t ipgre_get_size(const struct net_device *dev)
                 nla_total_size(4) +
                 /* IFLA_GRE_ERSPAN_INDEX */
                 nla_total_size(4) +
+                /* IFLA_GRE_ERSPAN_VER */
+                nla_total_size(1) +
+                /* IFLA_GRE_ERSPAN_DIR */
+                nla_total_size(1) +
+                /* IFLA_GRE_ERSPAN_HWID */
+                nla_total_size(2) +
                 0;
 }
 
@@ -1417,9 +1480,18 @@ static int ipgre_fill_info(struct sk_buff *skb, const struct net_device *dev)
                         goto nla_put_failure;
         }
 
-        if (t->index)
+        if (nla_put_u8(skb, IFLA_GRE_ERSPAN_VER, t->erspan_ver))
+                goto nla_put_failure;
+
+        if (t->erspan_ver == 1) {
                 if (nla_put_u32(skb, IFLA_GRE_ERSPAN_INDEX, t->index))
                         goto nla_put_failure;
+        } else if (t->erspan_ver == 2) {
+                if (nla_put_u8(skb, IFLA_GRE_ERSPAN_DIR, t->dir))
+                        goto nla_put_failure;
+                if (nla_put_u16(skb, IFLA_GRE_ERSPAN_HWID, t->hwid))
+                        goto nla_put_failure;
+        }
 
         return 0;
 
@@ -1455,6 +1527,9 @@ static const struct nla_policy ipgre_policy[IFLA_GRE_MAX + 1] = {
         [IFLA_GRE_IGNORE_DF]    = { .type = NLA_U8 },
         [IFLA_GRE_FWMARK]       = { .type = NLA_U32 },
         [IFLA_GRE_ERSPAN_INDEX] = { .type = NLA_U32 },
+        [IFLA_GRE_ERSPAN_VER]   = { .type = NLA_U8 },
+        [IFLA_GRE_ERSPAN_DIR]   = { .type = NLA_U8 },
+        [IFLA_GRE_ERSPAN_HWID]  = { .type = NLA_U16 },
 };
 
 static struct rtnl_link_ops ipgre_link_ops __read_mostly = {