[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>

6 files changed, 16 insertions, 0 deletions

diff --git a/include/linux/pfkeyv2.h b/include/linux/pfkeyv2.h
index a48a3ecc7c21..e6b519220245 100644
--- a/include/linux/pfkeyv2.h
+++ b/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 */
diff --git a/include/linux/xfrm.h b/include/linux/xfrm.h
index 2e22a996f623..f0df02ae68a4 100644
--- a/include/linux/xfrm.h
+++ b/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 {
diff --git a/include/net/inet_ecn.h b/include/net/inet_ecn.h
index 0bde1b6d5ced..6e2ee16546ec 100644
--- a/include/net/inet_ecn.h
+++ b/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)
diff --git a/net/ipv4/xfrm4_input.c b/net/ipv4/xfrm4_input.c
index f074d4930636..47e54d4218df 100644
--- a/net/ipv4/xfrm4_input.c
+++ b/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,
diff --git a/net/ipv6/xfrm6_input.c b/net/ipv6/xfrm6_input.c
index 45702e4d429c..28c29d78338e 100644
--- a/net/ipv6/xfrm6_input.c
+++ b/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,
diff --git a/net/key/af_key.c b/net/key/af_key.c
index b059fa2931f0..ed9d9bebdd35 100644
--- a/net/key/af_key.c
+++ b/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) {