ipfw: fix matching and setting DSCP value for IPv6

Matching for DSCP codes has used incorrect bits. Use IPV6_DSCP()
macro for matching opcodes to fix this. Also this leads to always
use value from a mbuf instead of cached value.

Previously different opcodes have used both cached in f_id value
and stored in the mbuf, and it did not always work after setdscp
action, since cached value was not updated.

Update IPv6 flowid value cached in the f_id.flow_id6 when we do
modification of DSCP value in O_SETDSCP opcode, it may be used by
external modules.

Also added logging support for O_SETDSCP opcode.

Reviewed by:	kp
MFC after:	1 week
Differential Revision: https://reviews.freebsd.org/D34807

2 files changed, 17 insertions, 17 deletions

diff --git a/sys/netpfil/ipfw/ip_fw2.c b/sys/netpfil/ipfw/ip_fw2.c
index bcf775009d25..7775b519270f 100644
--- a/sys/netpfil/ipfw/ip_fw2.c
+++ b/sys/netpfil/ipfw/ip_fw2.c
@@ -2064,11 +2064,9 @@ do {								\
 					else if (vidx == 6 /* dscp */) {
 						if (is_ipv4)
 							key = ip->ip_tos >> 2;
-						else {
-							key = args->f_id.flow_id6;
-							key = (key & 0x0f) << 2 |
-							    (key & 0xf000) >> 14;
-						}
+						else
+							key = IPV6_DSCP(
+							    (struct ip6_hdr *)ip) >> 2;
 						key &= 0x3f;
 					} else if (vidx == 2 /* dst-port */ ||
 					    vidx == 3 /* src-port */) {
@@ -2328,11 +2326,9 @@ do {								\
 				if (is_ipv4)
 					x = ip->ip_tos >> 2;
 				else if (is_ipv6) {
-					uint8_t *v;
-					v = &((struct ip6_hdr *)ip)->ip6_vfc;
-					x = (*v & 0x0F) << 2;
-					v++;
-					x |= *v >> 6;
+					x = IPV6_DSCP(
+					    (struct ip6_hdr *)ip) >> 2;
+					x &= 0x3f;
 				} else
 					break;
 
@@ -3139,12 +3135,13 @@ do {								\
 					ip->ip_sum = cksum_adjust(ip->ip_sum,
 					    old, *(uint16_t *)ip);
 				} else if (is_ipv6) {
-					uint8_t *v;
+					/* update cached value */
+					args->f_id.flow_id6 =
+					    ntohl(*(uint32_t *)ip) & ~0x0FC00000;
+					args->f_id.flow_id6 |= code << 22;
 
-					v = &((struct ip6_hdr *)ip)->ip6_vfc;
-					*v = (*v & 0xF0) | (code >> 2);
-					v++;
-					*v = (*v & 0x3F) | ((code & 0x03) << 6);
+					*((uint32_t *)ip) =
+					    htonl(args->f_id.flow_id6);
 				} else
 					break;
 
diff --git a/sys/netpfil/ipfw/ip_fw_log.c b/sys/netpfil/ipfw/ip_fw_log.c
index 3e375b06cd76..d230d243a0f7 100644
--- a/sys/netpfil/ipfw/ip_fw_log.c
+++ b/sys/netpfil/ipfw/ip_fw_log.c
@@ -156,8 +156,7 @@ ipfw_log(struct ip_fw_chain *chain, struct ip_fw *f, u_int hlen,
 				altq->qid);
 			cmd += F_LEN(cmd);
 		}
-		if (cmd->opcode == O_PROB || cmd->opcode == O_TAG ||
-		    cmd->opcode == O_SETDSCP)
+		if (cmd->opcode == O_PROB || cmd->opcode == O_TAG)
 			cmd += F_LEN(cmd);
 
 		action = action2;
@@ -202,6 +201,10 @@ ipfw_log(struct ip_fw_chain *chain, struct ip_fw *f, u_int hlen,
 			snprintf(SNPARGS(action2, 0), "Tee %d",
 				TARG(cmd->arg1, divert));
 			break;
+		case O_SETDSCP:
+			snprintf(SNPARGS(action2, 0), "SetDscp %d",
+				TARG(cmd->arg1, dscp) & 0x3F);
+			break;
 		case O_SETFIB:
 			snprintf(SNPARGS(action2, 0), "SetFib %d",
 				TARG(cmd->arg1, fib) & 0x7FFF);