New multiple DSCP match by "-m dscp --dscp-multi value,value,..."

commit a6370996fd612a538474209195c3fbdd7dfeef76
Author: Kyeong Yoo <kyeong.yoo@alliedtelesis.co.nz>
Date:   Wed Jul 29 09:38:51 2015 +1200

    extensions: libxt_dscp: allow multiple DSCP value match (--dscp-multi)
    
    Current "dscp" match supports one DSCP value in a rule.
    
    This patch enhances "dscp" match with multiple values ("--dscp-multi" option):
    
      -m dscp --dscp-multi value,value,...
    
    DSCP value should be between 0 and 63 or valid DSCP name like AF11, etc.
    
    Reviewed-by: Luuk Paulussen <luuk.paulussen@alliedtelesis.co.nz>
    Reviewed-by: Chris Packham <chris.packham@alliedtelesis.co.nz>

diff --git a/extensions/libxt_dscp.c b/extensions/libxt_dscp.c
index 02b22a4..ebf8723 100644
--- a/extensions/libxt_dscp.c
+++ b/extensions/libxt_dscp.c
@@ -14,6 +14,7 @@ 
  */
 #include <stdio.h>
 #include <string.h>
+#include <stdlib.h>
 #include <xtables.h>
 #include <linux/netfilter/xt_dscp.h>
 
@@ -23,8 +24,10 @@ 
 enum {
 	O_DSCP = 0,
 	O_DSCP_CLASS,
+	O_DSCP_MULTI,
 	F_DSCP       = 1 << O_DSCP,
 	F_DSCP_CLASS = 1 << O_DSCP_CLASS,
+	F_DSCP_MULTI = 1 << O_DSCP_MULTI,
 };
 
 static void dscp_help(void)
@@ -36,20 +39,78 @@  static void dscp_help(void)
 "               		or in hex (ex: 0x20)\n"
 "[!] --dscp-class name		Match the DiffServ class. This value may\n"
 "				be any of the BE,EF, AFxx or CSx classes\n"
+"[!] --dscp-multi value[,value]	Match DSCP value(s) in decimal or in hex\n"
 "\n"
-"				These two options are mutually exclusive !\n");
+"				These three options are mutually exclusive !\n");
 }
 
 static const struct xt_option_entry dscp_opts[] = {
-	{.name = "dscp", .id = O_DSCP, .excl = F_DSCP_CLASS,
+	{.name = "dscp", .id = O_DSCP, .excl = F_DSCP_CLASS | F_DSCP_MULTI,
 	 .type = XTTYPE_UINT8, .min = 0, .max = XT_DSCP_MAX,
 	 .flags = XTOPT_INVERT | XTOPT_PUT,
 	 XTOPT_POINTER(struct xt_dscp_info, dscp)},
-	{.name = "dscp-class", .id = O_DSCP_CLASS, .excl = F_DSCP,
+	{.name = "dscp-class", .id = O_DSCP_CLASS, .excl = F_DSCP | F_DSCP_MULTI,
+	 .type = XTTYPE_STRING, .flags = XTOPT_INVERT},
+	{.name = "dscp-multi", .id = O_DSCP_MULTI, .excl = F_DSCP | F_DSCP_CLASS,
 	 .type = XTTYPE_STRING, .flags = XTOPT_INVERT},
 	XTOPT_TABLEEND,
 };
 
+static void dscp_multi_parse(struct xt_dscp_info *dinfo, const char *arg)
+{
+	char *buffer, *str, *next;
+	unsigned int dscp;
+
+	buffer = strdup(arg);
+	if (!buffer) xtables_error(OTHER_PROBLEM, "strdup failed");
+
+	for (str = buffer; str; str = next)
+	{
+		if ((next = strchr(str, ',')) != NULL)
+			*next++ = '\0';
+		/* Try to convert as a number first and then as a DSCP name.
+		 * Note class_to_dscp() calls exit() on error after printing error message. */
+		if (!xtables_strtoui(str, NULL, &dscp, 0, XT_DSCP_MAX))
+			dscp = class_to_dscp(str);
+		dscp_set_bit(dinfo->dscp_bitmap, dscp);
+	}
+	free(buffer);
+}
+
+static void dscp_multi_print(const struct xt_dscp_info *dinfo)
+{
+	unsigned char dscp;
+	int count = 0;
+
+	for (dscp = 0; dscp <= XT_DSCP_MAX; dscp++)
+	{
+		if (dscp_test_bit(dinfo->dscp_bitmap, dscp)) {
+			if (count == 0)
+				printf(" DSCP multi-match %s0x%02x", dinfo->invert ? "!" : "", dscp);
+			else
+				printf(",0x%02x", dscp);
+			count++;
+		}
+	}
+}
+
+static void dscp_multi_save(const struct xt_dscp_info *dinfo)
+{
+	unsigned char dscp;
+	int count = 0;
+
+	for (dscp = 0; dscp <= XT_DSCP_MAX; dscp++)
+	{
+		if (dscp_test_bit(dinfo->dscp_bitmap, dscp)) {
+			if (count == 0)
+				printf("%s --dscp-multi 0x%02x", dinfo->invert ? " !" : "", dscp);
+			else
+				printf(",0x%02x", dscp);
+			count++;
+		}
+	}
+}
+
 static void dscp_parse(struct xt_option_call *cb)
 {
 	struct xt_dscp_info *dinfo = cb->data;
@@ -65,6 +126,12 @@  static void dscp_parse(struct xt_option_call *cb)
 		if (cb->invert)
 			dinfo->invert = 1;
 		break;
+	case O_DSCP_MULTI:
+		dinfo->multi = 1;
+		dscp_multi_parse(dinfo, cb->arg);
+		if (cb->invert)
+			dinfo->invert = 1;
+		break;
 	}
 }
 
@@ -72,7 +139,7 @@  static void dscp_check(struct xt_fcheck_call *cb)
 {
 	if (cb->xflags == 0)
 		xtables_error(PARAMETER_PROBLEM,
-		           "DSCP match: Parameter --dscp is required");
+		           "DSCP match: Parameter --dscp or --dscp-multi is required");
 }
 
 static void
@@ -80,7 +147,10 @@  dscp_print(const void *ip, const struct xt_entry_match *match, int numeric)
 {
 	const struct xt_dscp_info *dinfo =
 		(const struct xt_dscp_info *)match->data;
-	printf(" DSCP match %s0x%02x", dinfo->invert ? "!" : "", dinfo->dscp);
+	if (dinfo->multi)
+		dscp_multi_print(dinfo);
+	else
+		printf(" DSCP match %s0x%02x", dinfo->invert ? "!" : "", dinfo->dscp);
 }
 
 static void dscp_save(const void *ip, const struct xt_entry_match *match)
@@ -88,7 +158,10 @@  static void dscp_save(const void *ip, const struct xt_entry_match *match)
 	const struct xt_dscp_info *dinfo =
 		(const struct xt_dscp_info *)match->data;
 
-	printf("%s --dscp 0x%02x", dinfo->invert ? " !" : "", dinfo->dscp);
+	if (dinfo->multi)
+		dscp_multi_save(dinfo);
+	else
+		printf("%s --dscp 0x%02x", dinfo->invert ? " !" : "", dinfo->dscp);
 }
 
 static struct xtables_match dscp_match = {
diff --git a/include/linux/netfilter/xt_dscp.h b/include/linux/netfilter/xt_dscp.h
index 15f8932..bea99fa 100644
--- a/include/linux/netfilter/xt_dscp.h
+++ b/include/linux/netfilter/xt_dscp.h
@@ -16,10 +16,17 @@ 
 #define XT_DSCP_SHIFT	2
 #define XT_DSCP_MAX	0x3f	/* 00111111 */
 
+#define dscp_set_bit(bmap, idx) \
+	(bmap[(idx) >> 5] |= 1U << (idx & 31))
+#define dscp_test_bit(bmap, idx) \
+	(((1U << (idx & 31)) & bmap[(idx) >> 5]) != 0)
+
 /* match info */
 struct xt_dscp_info {
 	__u8 dscp;
 	__u8 invert;
+	__u8 multi;
+	__u32 dscp_bitmap[(XT_DSCP_MAX >> 5) + 1];
 };
 
 struct xt_tos_match_info {