From 2bf78196b242226f8778695fea922eb5341c0f21 Mon Sep 17 00:00:00 2001
From: Kevin Darbyshire-Bryant <ldir@darbyshire-bryant.me.uk>
Date: Fri, 1 Mar 2019 11:35:49 +0000
Subject: [PATCH] Automagically use & update DSCP contained in fwmark

and then I had a really bad idea!

Which is - if we're in fwmark & egress mode & we don't have a mark so
we've fallen through to DSCP, set a mark based on the DSCP.
That way, reply packets will be automagically fw marked for us on their
return as tc act connmark will have restored the mark which we can then
use.

With the 'icing' keyword, copy the dscp coded into the fwmark into the
real packets (the opposite of wash) in ingress mode.

The fwmark encodes the dscp thus (we steal the top byte)

ct->mark &= 0x00ffffff; - mask out top byte
ct->mark |= (0x01 | dscp) << 24; or in the DSCP (held in top 6 bits) and
set lowest bit as a 'cake set this fwmark' flag

options from tc:

fwmark - enable fwmark usage
getdscp - update conntrack mark with the DSCP if no existing CAKE mark
typically used with egress qdisc
setdscp - update DSCP on packets from CAKE mark typically used with
ingress qdisc

Signed-off-by: Kevin Darbyshire-Bryant <ldir@darbyshire-bryant.me.uk>
---
 pkt_sched.h |   2 +
 sch_cake.c  | 112 ++++++++++++++++++++++++++++++++++++++++++----------
 2 files changed, 94 insertions(+), 20 deletions(-)

diff --git a/pkt_sched.h b/pkt_sched.h
index a2f570c..e3c2ac4 100644
--- a/pkt_sched.h
+++ b/pkt_sched.h
@@ -879,6 +879,8 @@ enum {
 	TCA_CAKE_ACK_FILTER,
 	TCA_CAKE_SPLIT_GSO,
 	TCA_CAKE_FWMARK,
+	TCA_CAKE_GETDSCP,
+	TCA_CAKE_SETDSCP,
 	__TCA_CAKE_MAX
 };
 #define TCA_CAKE_MAX	(__TCA_CAKE_MAX - 1)
diff --git a/sch_cake.c b/sch_cake.c
index 052ac05..e27a8b1 100644
--- a/sch_cake.c
+++ b/sch_cake.c
@@ -269,7 +269,9 @@ enum {
 	CAKE_FLAG_INGRESS	   = BIT(2),
 	CAKE_FLAG_WASH		   = BIT(3),
 	CAKE_FLAG_SPLIT_GSO	   = BIT(4),
-	CAKE_FLAG_FWMARK	   = BIT(5)
+	CAKE_FLAG_FWMARK	   = BIT(5), /* USEFWMARK */
+	CAKE_FLAG_GETDSCP	   = BIT(6), /* STOREDSCP */
+	CAKE_FLAG_SETDSCP	   = BIT(7)  /* RESTOREDSCP */
 };
 
 /* COBALT operates the Codel and BLUE algorithms in parallel, in order to
@@ -1616,19 +1618,36 @@ static unsigned int cake_drop(struct Qdisc *sch, struct sk_buff **to_free)
 	return idx + (tin << 16);
 }
 
-static u8 cake_handle_diffserv(struct sk_buff *skb, u16 wash)
+static void cake_update_diffserv(struct sk_buff *skb, u8 dscp)
+{
+	switch (skb->protocol) {
+	case htons(ETH_P_IP):
+		if ((ipv4_get_dsfield(ip_hdr(skb)) & ~INET_ECN_MASK) != dscp)
+			ipv4_change_dsfield(ip_hdr(skb), INET_ECN_MASK, dscp);
+		break;
+	case htons(ETH_P_IPV6):
+		if ((ipv6_get_dsfield(ipv6_hdr(skb)) & ~INET_ECN_MASK) != dscp)
+			ipv6_change_dsfield(ipv6_hdr(skb), INET_ECN_MASK, dscp);
+		break;
+	default:
+		break;
+	}
+
+}
+
+static u8 cake_handle_diffserv(struct sk_buff *skb, bool wash)
 {
 	u8 dscp;
 
 	switch (skb->protocol) {
 	case htons(ETH_P_IP):
-		dscp = ipv4_get_dsfield(ip_hdr(skb)) >> 2;
+		dscp = ipv4_get_dsfield(ip_hdr(skb)) & ~INET_ECN_MASK;
 		if (wash && dscp)
 			ipv4_change_dsfield(ip_hdr(skb), INET_ECN_MASK, 0);
 		return dscp;
 
 	case htons(ETH_P_IPV6):
-		dscp = ipv6_get_dsfield(ipv6_hdr(skb)) >> 2;
+		dscp = ipv6_get_dsfield(ipv6_hdr(skb)) & ~INET_ECN_MASK;
 		if (wash && dscp)
 			ipv6_change_dsfield(ipv6_hdr(skb), INET_ECN_MASK, 0);
 		return dscp;
@@ -1642,37 +1661,68 @@ static u8 cake_handle_diffserv(struct sk_buff *skb, u16 wash)
 	}
 }
 
+static void cake_update_ct_mark(struct sk_buff *skb, u8 dscp)
+{
+#if IS_REACHABLE(CONFIG_NF_CONNTRACK)
+	enum ip_conntrack_info ctinfo;
+	struct nf_conn *ct;
+
+	ct = nf_ct_get(skb, &ctinfo);
+	if (!ct)
+		return;
+
+	ct->mark &= ~(0xff << 24);
+	ct->mark |= (0x01 | dscp) << 24;
+	nf_conntrack_event_cache(IPCT_MARK, ct);
+#endif
+}
+
 static struct cake_tin_data *cake_select_tin(struct Qdisc *sch,
 					     struct sk_buff *skb)
 {
 	struct cake_sched_data *q = qdisc_priv(sch);
-	u32 tin;
+	bool wash;
 	u8 dscp;
+	u8 tin;
 
-	/* Tin selection: Default to diffserv-based selection, allow overriding
-	 * using firewall marks or skb->priority.
-	 */
-	dscp = cake_handle_diffserv(skb,
-				    q->rate_flags & CAKE_FLAG_WASH);
+	wash = !!(q->rate_flags & CAKE_FLAG_WASH);
+
+	if (q->tin_mode == CAKE_DIFFSERV_BESTEFFORT) {
 
-	if (q->tin_mode == CAKE_DIFFSERV_BESTEFFORT)
 		tin = 0;
+		if (wash)
+			cake_update_diffserv(skb, 0);
 
-	else if (q->rate_flags & CAKE_FLAG_FWMARK && /* use fw mark */
-		 skb->mark &&
-		 skb->mark <= q->tin_cnt)
-		tin = q->tin_order[skb->mark - 1];
+	} else if (TC_H_MAJ(skb->priority) == sch->handle && /* use priority */
+		   TC_H_MIN(skb->priority) > 0 &&
+		   TC_H_MIN(skb->priority) <= q->tin_cnt) {
 
-	else if (TC_H_MAJ(skb->priority) == sch->handle &&
-		 TC_H_MIN(skb->priority) > 0 &&
-		 TC_H_MIN(skb->priority) <= q->tin_cnt)
 		tin = q->tin_order[TC_H_MIN(skb->priority) - 1];
+		if (wash)
+			cake_update_diffserv(skb, 0);
 
-	else {
-		tin = q->tin_index[dscp];
+	} else if (q->rate_flags & CAKE_FLAG_FWMARK && /* use fw mark */
+		   skb->mark & (0x01 << 24)) {
+
+		dscp = skb->mark >> 24 & ~INET_ECN_MASK;
+		tin = q->tin_index[dscp >> 2];
+
+		if (wash)
+			cake_update_diffserv(skb, 0);
+		else if (q->rate_flags & CAKE_FLAG_SETDSCP)
+			cake_update_diffserv(skb, dscp);
+
+	} else { /* fallback to DSCP */
+		/* extract the Diffserv Precedence field, if it exists */
+		/* and clear DSCP bits if washing */
+		dscp = cake_handle_diffserv(skb, wash);
+		tin = q->tin_index[dscp >> 2];
 
 		if (unlikely(tin >= q->tin_cnt))
 			tin = 0;
+
+		if (q->rate_flags & CAKE_FLAG_FWMARK && q->rate_flags & CAKE_FLAG_GETDSCP)
+			cake_update_ct_mark(skb, dscp);
 	}
 
 	return &q->tins[tin];
@@ -2760,6 +2810,20 @@ static int cake_change(struct Qdisc *sch, struct nlattr *opt,
 			q->rate_flags &= ~CAKE_FLAG_FWMARK;
 	}
 
+	if (tb[TCA_CAKE_GETDSCP]) {
+		if (!!nla_get_u32(tb[TCA_CAKE_GETDSCP]))
+			q->rate_flags |= CAKE_FLAG_GETDSCP;
+		else
+			q->rate_flags &= ~CAKE_FLAG_GETDSCP;
+	}
+
+	if (tb[TCA_CAKE_SETDSCP]) {
+		if (!!nla_get_u32(tb[TCA_CAKE_SETDSCP]))
+			q->rate_flags |= CAKE_FLAG_SETDSCP;
+		else
+			q->rate_flags &= ~CAKE_FLAG_SETDSCP;
+	}
+
 	if (q->tins) {
 		sch_tree_lock(sch);
 		cake_reconfigure(sch);
@@ -2944,6 +3008,14 @@ static int cake_dump(struct Qdisc *sch, struct sk_buff *skb)
 			!!(q->rate_flags & CAKE_FLAG_FWMARK)))
 		goto nla_put_failure;
 
+	if (nla_put_u32(skb, TCA_CAKE_GETDSCP,
+			!!(q->rate_flags & CAKE_FLAG_GETDSCP)))
+		goto nla_put_failure;
+
+	if (nla_put_u32(skb, TCA_CAKE_SETDSCP,
+			!!(q->rate_flags & CAKE_FLAG_SETDSCP)))
+		goto nla_put_failure;
+
 	return nla_nest_end(skb, opts);
 
 nla_put_failure:
-- 
2.17.2 (Apple Git-113)

