[IPSEC]: Implement DSCP decapsulation

This patch adds DSCP decapsulation for IPsec.  This is enabled by
a per-state flag which is off by default.  Leaving it off by default
maintains compatibility and is also good for performance reasons.

I decided to not implement a toggle on the output path since not
encapsulating the DSCP can and should be done by netfilter.
Signed-off-by: Herbert Xu <herbert@gondor.apana.org.au>
Signed-off-by: David S. Miller <davem@davemloft.net>

include/linux/pfkeyv2.h

@@ -245,6 +245,7 @@ struct sadb_x_nat_t_port {
 
 /* Security Association flags */
 #define SADB_SAFLAGS_PFS	1
+#define SADB_SAFLAGS_DECAP_DSCP	0x40000000
 #define SADB_SAFLAGS_NOECN	0x80000000
 
 /* Security Association states */

include/linux/xfrm.h

@@ -190,6 +190,7 @@ struct xfrm_usersa_info {
 	__u8				replay_window;
 	__u8				flags;
 #define XFRM_STATE_NOECN	1
+#define XFRM_STATE_DECAP_DSCP	2
 };
 
 struct xfrm_usersa_id {

include/net/inet_ecn.h

@@ -78,6 +78,12 @@ static inline void IP_ECN_clear(struct iphdr *iph)
 	iph->tos &= ~INET_ECN_MASK;
 }
 
+static inline void ipv4_copy_dscp(struct iphdr *outer, struct iphdr *inner)
+{
+	u32 dscp = ipv4_get_dsfield(outer) & ~INET_ECN_MASK;
+	ipv4_change_dsfield(inner, INET_ECN_MASK, dscp);
+}
+
 struct ipv6hdr;
 
 static inline void IP6_ECN_set_ce(struct ipv6hdr *iph)

net/ipv4/xfrm4_input.c

@@ -101,6 +101,8 @@ int xfrm4_rcv_encap(struct sk_buff *skb, __u16 encap_type)
 			if (skb_cloned(skb) &&
 			    pskb_expand_head(skb, 0, 0, GFP_ATOMIC))
 				goto drop;
+			if (x->props.flags & XFRM_STATE_DECAP_DSCP)
+				ipv4_copy_dscp(iph, skb->h.ipiph);
 			if (!(x->props.flags & XFRM_STATE_NOECN))
 				ipip_ecn_decapsulate(skb);
 			skb->mac.raw = memmove(skb->data - skb->mac_len,

net/ipv6/xfrm6_input.c

@@ -88,6 +88,8 @@ int xfrm6_rcv_spi(struct sk_buff **pskb, unsigned int *nhoffp, u32 spi)
 			if (skb_cloned(skb) &&
 			    pskb_expand_head(skb, 0, 0, GFP_ATOMIC))
 				goto drop;
+			if (x->props.flags & XFRM_STATE_DECAP_DSCP)
+				ipv6_copy_dscp(skb->nh.ipv6h, skb->h.ipv6h);
 			if (!(x->props.flags & XFRM_STATE_NOECN))
 				ipip6_ecn_decapsulate(skb);
 			skb->mac.raw = memmove(skb->data - skb->mac_len,

net/key/af_key.c

@@ -683,6 +683,8 @@ static struct sk_buff * pfkey_xfrm_state2msg(struct xfrm_state *x, int add_keys,
 	sa->sadb_sa_flags = 0;
 	if (x->props.flags & XFRM_STATE_NOECN)
 		sa->sadb_sa_flags |= SADB_SAFLAGS_NOECN;
+	if (x->props.flags & XFRM_STATE_DECAP_DSCP)
+		sa->sadb_sa_flags |= SADB_SAFLAGS_DECAP_DSCP;
 
 	/* hard time */
 	if (hsc & 2) {
@@ -965,6 +967,8 @@ static struct xfrm_state * pfkey_msg2xfrm_state(struct sadb_msg *hdr,
 	x->props.replay_window = sa->sadb_sa_replay;
 	if (sa->sadb_sa_flags & SADB_SAFLAGS_NOECN)
 		x->props.flags |= XFRM_STATE_NOECN;
+	if (sa->sadb_sa_flags & SADB_SAFLAGS_DECAP_DSCP)
+		x->props.flags |= XFRM_STATE_DECAP_DSCP;
 
 	lifetime = (struct sadb_lifetime*) ext_hdrs[SADB_EXT_LIFETIME_HARD-1];
 	if (lifetime != NULL) {