[mirror_ubuntu-bionic-kernel.git] / net / netfilter / nft_payload.c

/*
 * Copyright (c) 2008-2009 Patrick McHardy <kaber@trash.net>
 * Copyright (c) 2016 Pablo Neira Ayuso <pablo@netfilter.org>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * Development of this code funded by Astaro AG (http://www.astaro.com/)
 */

#include <linux/kernel.h>
#include <linux/if_vlan.h>
#include <linux/init.h>
#include <linux/module.h>
#include <linux/netlink.h>
#include <linux/netfilter.h>
#include <linux/netfilter/nf_tables.h>
#include <net/netfilter/nf_tables_core.h>
#include <net/netfilter/nf_tables.h>
/* For layer 4 checksum field offset. */
#include <linux/tcp.h>
#include <linux/udp.h>
#include <linux/icmpv6.h>

/* add vlan header into the user buffer for if tag was removed by offloads */
static bool
nft_payload_copy_vlan(u32 *d, const struct sk_buff *skb, u8 offset, u8 len)
{
	int mac_off = skb_mac_header(skb) - skb->data;
	u8 vlan_len, *vlanh, *dst_u8 = (u8 *) d;
	struct vlan_ethhdr veth;

	vlanh = (u8 *) &veth;
	if (offset < ETH_HLEN) {
		u8 ethlen = min_t(u8, len, ETH_HLEN - offset);

		if (skb_copy_bits(skb, mac_off, &veth, ETH_HLEN))
			return false;

		veth.h_vlan_proto = skb->vlan_proto;

		memcpy(dst_u8, vlanh + offset, ethlen);

		len -= ethlen;
		if (len == 0)
			return true;

		dst_u8 += ethlen;
		offset = ETH_HLEN;
	} else if (offset >= VLAN_ETH_HLEN) {
		offset -= VLAN_HLEN;
		goto skip;
	}

	veth.h_vlan_TCI = htons(skb_vlan_tag_get(skb));
	veth.h_vlan_encapsulated_proto = skb->protocol;

	vlanh += offset;

	vlan_len = min_t(u8, len, VLAN_ETH_HLEN - offset);
	memcpy(dst_u8, vlanh, vlan_len);

	len -= vlan_len;
	if (!len)
		return true;

	dst_u8 += vlan_len;
 skip:
	return skb_copy_bits(skb, offset + mac_off, dst_u8, len) == 0;
}

static void nft_payload_eval(const struct nft_expr *expr,
			     struct nft_regs *regs,
			     const struct nft_pktinfo *pkt)
{
	const struct nft_payload *priv = nft_expr_priv(expr);
	const struct sk_buff *skb = pkt->skb;
	u32 *dest = &regs->data[priv->dreg];
	int offset;

	dest[priv->len / NFT_REG32_SIZE] = 0;
	switch (priv->base) {
	case NFT_PAYLOAD_LL_HEADER:
		if (!skb_mac_header_was_set(skb))
			goto err;

		if (skb_vlan_tag_present(skb)) {
			if (!nft_payload_copy_vlan(dest, skb,
						   priv->offset, priv->len))
				goto err;
			return;
		}
		offset = skb_mac_header(skb) - skb->data;
		break;
	case NFT_PAYLOAD_NETWORK_HEADER:
		offset = skb_network_offset(skb);
		break;
	case NFT_PAYLOAD_TRANSPORT_HEADER:
		if (!pkt->tprot_set)
			goto err;
		offset = pkt->xt.thoff;
		break;
	default:
		BUG();
	}
	offset += priv->offset;

	if (skb_copy_bits(skb, offset, dest, priv->len) < 0)
		goto err;
	return;
err:
	regs->verdict.code = NFT_BREAK;
}

static const struct nla_policy nft_payload_policy[NFTA_PAYLOAD_MAX + 1] = {
	[NFTA_PAYLOAD_SREG]		= { .type = NLA_U32 },
	[NFTA_PAYLOAD_DREG]		= { .type = NLA_U32 },
	[NFTA_PAYLOAD_BASE]		= { .type = NLA_U32 },
	[NFTA_PAYLOAD_OFFSET]		= { .type = NLA_U32 },
	[NFTA_PAYLOAD_LEN]		= { .type = NLA_U32 },
	[NFTA_PAYLOAD_CSUM_TYPE]	= { .type = NLA_U32 },
	[NFTA_PAYLOAD_CSUM_OFFSET]	= { .type = NLA_U32 },
};

static int nft_payload_init(const struct nft_ctx *ctx,
			    const struct nft_expr *expr,
			    const struct nlattr * const tb[])
{
	struct nft_payload *priv = nft_expr_priv(expr);

	priv->base   = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE]));
	priv->offset = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_OFFSET]));
	priv->len    = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN]));
	priv->dreg   = nft_parse_register(tb[NFTA_PAYLOAD_DREG]);

	return nft_validate_register_store(ctx, priv->dreg, NULL,
					   NFT_DATA_VALUE, priv->len);
}

static int nft_payload_dump(struct sk_buff *skb, const struct nft_expr *expr)
{
	const struct nft_payload *priv = nft_expr_priv(expr);

	if (nft_dump_register(skb, NFTA_PAYLOAD_DREG, priv->dreg) ||
	    nla_put_be32(skb, NFTA_PAYLOAD_BASE, htonl(priv->base)) ||
	    nla_put_be32(skb, NFTA_PAYLOAD_OFFSET, htonl(priv->offset)) ||
	    nla_put_be32(skb, NFTA_PAYLOAD_LEN, htonl(priv->len)))
		goto nla_put_failure;
	return 0;

nla_put_failure:
	return -1;
}

static const struct nft_expr_ops nft_payload_ops = {
	.type		= &nft_payload_type,
	.size		= NFT_EXPR_SIZE(sizeof(struct nft_payload)),
	.eval		= nft_payload_eval,
	.init		= nft_payload_init,
	.dump		= nft_payload_dump,
};

const struct nft_expr_ops nft_payload_fast_ops = {
	.type		= &nft_payload_type,
	.size		= NFT_EXPR_SIZE(sizeof(struct nft_payload)),
	.eval		= nft_payload_eval,
	.init		= nft_payload_init,
	.dump		= nft_payload_dump,
};

static inline void nft_csum_replace(__sum16 *sum, __wsum fsum, __wsum tsum)
{
	*sum = csum_fold(csum_add(csum_sub(~csum_unfold(*sum), fsum), tsum));
	if (*sum == 0)
		*sum = CSUM_MANGLED_0;
}

static bool nft_payload_udp_checksum(struct sk_buff *skb, unsigned int thoff)
{
	struct udphdr *uh, _uh;

	uh = skb_header_pointer(skb, thoff, sizeof(_uh), &_uh);
	if (!uh)
		return false;

	return (__force bool)uh->check;
}

static int nft_payload_l4csum_offset(const struct nft_pktinfo *pkt,
				     struct sk_buff *skb,
				     unsigned int *l4csum_offset)
{
	switch (pkt->tprot) {
	case IPPROTO_TCP:
		*l4csum_offset = offsetof(struct tcphdr, check);
		break;
	case IPPROTO_UDP:
		if (!nft_payload_udp_checksum(skb, pkt->xt.thoff))
			return -1;
		/* Fall through. */
	case IPPROTO_UDPLITE:
		*l4csum_offset = offsetof(struct udphdr, check);
		break;
	case IPPROTO_ICMPV6:
		*l4csum_offset = offsetof(struct icmp6hdr, icmp6_cksum);
		break;
	default:
		return -1;
	}

	*l4csum_offset += pkt->xt.thoff;
	return 0;
}

static int nft_payload_l4csum_update(const struct nft_pktinfo *pkt,
				     struct sk_buff *skb,
				     __wsum fsum, __wsum tsum)
{
	int l4csum_offset;
	__sum16 sum;

	/* If we cannot determine layer 4 checksum offset or this packet doesn't
	 * require layer 4 checksum recalculation, skip this packet.
	 */
	if (nft_payload_l4csum_offset(pkt, skb, &l4csum_offset) < 0)
		return 0;

	if (skb_copy_bits(skb, l4csum_offset, &sum, sizeof(sum)) < 0)
		return -1;

	/* Checksum mangling for an arbitrary amount of bytes, based on
	 * inet_proto_csum_replace*() functions.
	 */
	if (skb->ip_summed != CHECKSUM_PARTIAL) {
		nft_csum_replace(&sum, fsum, tsum);
		if (skb->ip_summed == CHECKSUM_COMPLETE) {
			skb->csum = ~csum_add(csum_sub(~(skb->csum), fsum),
					      tsum);
		}
	} else {
		sum = ~csum_fold(csum_add(csum_sub(csum_unfold(sum), fsum),
					  tsum));
	}

	if (!skb_make_writable(skb, l4csum_offset + sizeof(sum)) ||
	    skb_store_bits(skb, l4csum_offset, &sum, sizeof(sum)) < 0)
		return -1;

	return 0;
}

static int nft_payload_csum_inet(struct sk_buff *skb, const u32 *src,
				 __wsum fsum, __wsum tsum, int csum_offset)
{
	__sum16 sum;

	if (skb_copy_bits(skb, csum_offset, &sum, sizeof(sum)) < 0)
		return -1;

	nft_csum_replace(&sum, fsum, tsum);
	if (!skb_make_writable(skb, csum_offset + sizeof(sum)) ||
	    skb_store_bits(skb, csum_offset, &sum, sizeof(sum)) < 0)
		return -1;

	return 0;
}

static void nft_payload_set_eval(const struct nft_expr *expr,
				 struct nft_regs *regs,
				 const struct nft_pktinfo *pkt)
{
	const struct nft_payload_set *priv = nft_expr_priv(expr);
	struct sk_buff *skb = pkt->skb;
	const u32 *src = &regs->data[priv->sreg];
	int offset, csum_offset;
	__wsum fsum, tsum;

	switch (priv->base) {
	case NFT_PAYLOAD_LL_HEADER:
		if (!skb_mac_header_was_set(skb))
			goto err;
		offset = skb_mac_header(skb) - skb->data;
		break;
	case NFT_PAYLOAD_NETWORK_HEADER:
		offset = skb_network_offset(skb);
		break;
	case NFT_PAYLOAD_TRANSPORT_HEADER:
		if (!pkt->tprot_set)
			goto err;
		offset = pkt->xt.thoff;
		break;
	default:
		BUG();
	}

	csum_offset = offset + priv->csum_offset;
	offset += priv->offset;

	if ((priv->csum_type == NFT_PAYLOAD_CSUM_INET || priv->csum_flags) &&
	    (priv->base != NFT_PAYLOAD_TRANSPORT_HEADER ||
	     skb->ip_summed != CHECKSUM_PARTIAL)) {
		fsum = skb_checksum(skb, offset, priv->len, 0);
		tsum = csum_partial(src, priv->len, 0);

		if (priv->csum_type == NFT_PAYLOAD_CSUM_INET &&
		    nft_payload_csum_inet(skb, src, fsum, tsum, csum_offset))
			goto err;

		if (priv->csum_flags &&
		    nft_payload_l4csum_update(pkt, skb, fsum, tsum) < 0)
			goto err;
	}

	if (!skb_make_writable(skb, max(offset + priv->len, 0)) ||
	    skb_store_bits(skb, offset, src, priv->len) < 0)
		goto err;

	return;
err:
	regs->verdict.code = NFT_BREAK;
}

static int nft_payload_set_init(const struct nft_ctx *ctx,
				const struct nft_expr *expr,
				const struct nlattr * const tb[])
{
	struct nft_payload_set *priv = nft_expr_priv(expr);

	priv->base        = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE]));
	priv->offset      = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_OFFSET]));
	priv->len         = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN]));
	priv->sreg        = nft_parse_register(tb[NFTA_PAYLOAD_SREG]);

	if (tb[NFTA_PAYLOAD_CSUM_TYPE])
		priv->csum_type =
			ntohl(nla_get_be32(tb[NFTA_PAYLOAD_CSUM_TYPE]));
	if (tb[NFTA_PAYLOAD_CSUM_OFFSET])
		priv->csum_offset =
			ntohl(nla_get_be32(tb[NFTA_PAYLOAD_CSUM_OFFSET]));
	if (tb[NFTA_PAYLOAD_CSUM_FLAGS]) {
		u32 flags;

		flags = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_CSUM_FLAGS]));
		if (flags & ~NFT_PAYLOAD_L4CSUM_PSEUDOHDR)
			return -EINVAL;

		priv->csum_flags = flags;
	}

	switch (priv->csum_type) {
	case NFT_PAYLOAD_CSUM_NONE:
	case NFT_PAYLOAD_CSUM_INET:
		break;
	default:
		return -EOPNOTSUPP;
	}

	return nft_validate_register_load(priv->sreg, priv->len);
}

static int nft_payload_set_dump(struct sk_buff *skb, const struct nft_expr *expr)
{
	const struct nft_payload_set *priv = nft_expr_priv(expr);

	if (nft_dump_register(skb, NFTA_PAYLOAD_SREG, priv->sreg) ||
	    nla_put_be32(skb, NFTA_PAYLOAD_BASE, htonl(priv->base)) ||
	    nla_put_be32(skb, NFTA_PAYLOAD_OFFSET, htonl(priv->offset)) ||
	    nla_put_be32(skb, NFTA_PAYLOAD_LEN, htonl(priv->len)) ||
	    nla_put_be32(skb, NFTA_PAYLOAD_CSUM_TYPE, htonl(priv->csum_type)) ||
	    nla_put_be32(skb, NFTA_PAYLOAD_CSUM_OFFSET,
			 htonl(priv->csum_offset)) ||
	    nla_put_be32(skb, NFTA_PAYLOAD_CSUM_FLAGS, htonl(priv->csum_flags)))
		goto nla_put_failure;
	return 0;

nla_put_failure:
	return -1;
}

static const struct nft_expr_ops nft_payload_set_ops = {
	.type		= &nft_payload_type,
	.size		= NFT_EXPR_SIZE(sizeof(struct nft_payload_set)),
	.eval		= nft_payload_set_eval,
	.init		= nft_payload_set_init,
	.dump		= nft_payload_set_dump,
};

static const struct nft_expr_ops *
nft_payload_select_ops(const struct nft_ctx *ctx,
		       const struct nlattr * const tb[])
{
	enum nft_payload_bases base;
	unsigned int offset, len;

	if (tb[NFTA_PAYLOAD_BASE] == NULL ||
	    tb[NFTA_PAYLOAD_OFFSET] == NULL ||
	    tb[NFTA_PAYLOAD_LEN] == NULL)
		return ERR_PTR(-EINVAL);

	base = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE]));
	switch (base) {
	case NFT_PAYLOAD_LL_HEADER:
	case NFT_PAYLOAD_NETWORK_HEADER:
	case NFT_PAYLOAD_TRANSPORT_HEADER:
		break;
	default:
		return ERR_PTR(-EOPNOTSUPP);
	}

	if (tb[NFTA_PAYLOAD_SREG] != NULL) {
		if (tb[NFTA_PAYLOAD_DREG] != NULL)
			return ERR_PTR(-EINVAL);
		return &nft_payload_set_ops;
	}

	if (tb[NFTA_PAYLOAD_DREG] == NULL)
		return ERR_PTR(-EINVAL);

	offset = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_OFFSET]));
	len    = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN]));

	if (len <= 4 && is_power_of_2(len) && IS_ALIGNED(offset, len) &&
	    base != NFT_PAYLOAD_LL_HEADER)
		return &nft_payload_fast_ops;
	else
		return &nft_payload_ops;
}

struct nft_expr_type nft_payload_type __read_mostly = {
	.name		= "payload",
	.select_ops	= nft_payload_select_ops,
	.policy		= nft_payload_policy,
	.maxattr	= NFTA_PAYLOAD_MAX,
	.owner		= THIS_MODULE,
};
Commit	Line	Data
96518518	1	/*
ef1f7df9	2	* Copyright (c) 2008-2009 Patrick McHardy <kaber@trash.net>
18140969	3	* Copyright (c) 2016 Pablo Neira Ayuso <pablo@netfilter.org>
96518518 PM	4	*
	5	* This program is free software; you can redistribute it and/or modify
	6	* it under the terms of the GNU General Public License version 2 as
	7	* published by the Free Software Foundation.
	8	*
	9	* Development of this code funded by Astaro AG (http://www.astaro.com/)
	10	*/
	11
	12	#include <linux/kernel.h>
8cfd23e6	13	#include <linux/if_vlan.h>
96518518 PM	14	#include <linux/init.h>
	15	#include <linux/module.h>
	16	#include <linux/netlink.h>
	17	#include <linux/netfilter.h>
	18	#include <linux/netfilter/nf_tables.h>
	19	#include <net/netfilter/nf_tables_core.h>
	20	#include <net/netfilter/nf_tables.h>
18140969 PNA	21	/* For layer 4 checksum field offset. */
	22	#include <linux/tcp.h>
	23	#include <linux/udp.h>
	24	#include <linux/icmpv6.h>
96518518	25
8cfd23e6 FW	26	/* add vlan header into the user buffer for if tag was removed by offloads */
	27	static bool
	28	nft_payload_copy_vlan(u32 d, const struct sk_buff skb, u8 offset, u8 len)
	29	{
	30	int mac_off = skb_mac_header(skb) - skb->data;
	31	u8 vlan_len, vlanh, dst_u8 = (u8 *) d;
	32	struct vlan_ethhdr veth;
	33
	34	vlanh = (u8 *) &veth;
	35	if (offset < ETH_HLEN) {
	36	u8 ethlen = min_t(u8, len, ETH_HLEN - offset);
	37
	38	if (skb_copy_bits(skb, mac_off, &veth, ETH_HLEN))
	39	return false;
	40
	41	veth.h_vlan_proto = skb->vlan_proto;
	42
	43	memcpy(dst_u8, vlanh + offset, ethlen);
	44
	45	len -= ethlen;
	46	if (len == 0)
	47	return true;
	48
	49	dst_u8 += ethlen;
	50	offset = ETH_HLEN;
	51	} else if (offset >= VLAN_ETH_HLEN) {
	52	offset -= VLAN_HLEN;
	53	goto skip;
	54	}
	55
	56	veth.h_vlan_TCI = htons(skb_vlan_tag_get(skb));
	57	veth.h_vlan_encapsulated_proto = skb->protocol;
	58
	59	vlanh += offset;
	60
	61	vlan_len = min_t(u8, len, VLAN_ETH_HLEN - offset);
	62	memcpy(dst_u8, vlanh, vlan_len);
	63
	64	len -= vlan_len;
	65	if (!len)
	66	return true;
	67
	68	dst_u8 += vlan_len;
	69	skip:
	70	return skb_copy_bits(skb, offset + mac_off, dst_u8, len) == 0;
	71	}
	72
96518518	73	static void nft_payload_eval(const struct nft_expr *expr,
a55e22e9	74	struct nft_regs *regs,
96518518 PM	75	const struct nft_pktinfo *pkt)
	76	{
	77	const struct nft_payload *priv = nft_expr_priv(expr);
	78	const struct sk_buff *skb = pkt->skb;
49499c3e	79	u32 *dest = &regs->data[priv->dreg];
96518518 PM	80	int offset;
96518518 PM	81
8cfd23e6	82	dest[priv->len / NFT_REG32_SIZE] = 0;
96518518 PM	83	switch (priv->base) {
	84	case NFT_PAYLOAD_LL_HEADER:
	85	if (!skb_mac_header_was_set(skb))
	86	goto err;
8cfd23e6 FW	87
	88	if (skb_vlan_tag_present(skb)) {
	89	if (!nft_payload_copy_vlan(dest, skb,
	90	priv->offset, priv->len))
	91	goto err;
	92	return;
	93	}
96518518 PM	94	offset = skb_mac_header(skb) - skb->data;
	95	break;
	96	case NFT_PAYLOAD_NETWORK_HEADER:
	97	offset = skb_network_offset(skb);
	98	break;
	99	case NFT_PAYLOAD_TRANSPORT_HEADER:
a20877b5 LZ	100	if (!pkt->tprot_set)
a20877b5 LZ	101	goto err;
c54032e0	102	offset = pkt->xt.thoff;
96518518 PM	103	break;
	104	default:
	105	BUG();
	106	}
	107	offset += priv->offset;
	108
fad136ea	109	if (skb_copy_bits(skb, offset, dest, priv->len) < 0)
96518518 PM	110	goto err;
	111	return;
	112	err:
a55e22e9	113	regs->verdict.code = NFT_BREAK;
96518518 PM	114	}
	115
	116	static const struct nla_policy nft_payload_policy[NFTA_PAYLOAD_MAX + 1] = {
7ec3f7b4 PM	117	[NFTA_PAYLOAD_SREG] = { .type = NLA_U32 },
	118	[NFTA_PAYLOAD_DREG] = { .type = NLA_U32 },
	119	[NFTA_PAYLOAD_BASE] = { .type = NLA_U32 },
	120	[NFTA_PAYLOAD_OFFSET] = { .type = NLA_U32 },
	121	[NFTA_PAYLOAD_LEN] = { .type = NLA_U32 },
	122	[NFTA_PAYLOAD_CSUM_TYPE] = { .type = NLA_U32 },
	123	[NFTA_PAYLOAD_CSUM_OFFSET] = { .type = NLA_U32 },
96518518 PM	124	};
	125
	126	static int nft_payload_init(const struct nft_ctx *ctx,
	127	const struct nft_expr *expr,
	128	const struct nlattr * const tb[])
	129	{
	130	struct nft_payload *priv = nft_expr_priv(expr);
96518518	131
c29b72e0	132	priv->base = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE]));
96518518 PM	133	priv->offset = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_OFFSET]));
96518518 PM	134	priv->len = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN]));
b1c96ed3	135	priv->dreg = nft_parse_register(tb[NFTA_PAYLOAD_DREG]);
96518518	136
1ec10212 PM	137	return nft_validate_register_store(ctx, priv->dreg, NULL,
1ec10212 PM	138	NFT_DATA_VALUE, priv->len);
96518518 PM	139	}
	140
	141	static int nft_payload_dump(struct sk_buff skb, const struct nft_expr expr)
	142	{
	143	const struct nft_payload *priv = nft_expr_priv(expr);
	144
b1c96ed3	145	if (nft_dump_register(skb, NFTA_PAYLOAD_DREG, priv->dreg) \|\|
96518518 PM	146	nla_put_be32(skb, NFTA_PAYLOAD_BASE, htonl(priv->base)) \|\|
	147	nla_put_be32(skb, NFTA_PAYLOAD_OFFSET, htonl(priv->offset)) \|\|
	148	nla_put_be32(skb, NFTA_PAYLOAD_LEN, htonl(priv->len)))
	149	goto nla_put_failure;
	150	return 0;
	151
	152	nla_put_failure:
	153	return -1;
	154	}
	155
ef1f7df9 PM	156	static const struct nft_expr_ops nft_payload_ops = {
ef1f7df9 PM	157	.type = &nft_payload_type,
96518518	158	.size = NFT_EXPR_SIZE(sizeof(struct nft_payload)),
96518518 PM	159	.eval = nft_payload_eval,
	160	.init = nft_payload_init,
	161	.dump = nft_payload_dump,
ef1f7df9 PM	162	};
ef1f7df9 PM	163
c29b72e0 PM	164	const struct nft_expr_ops nft_payload_fast_ops = {
	165	.type = &nft_payload_type,
	166	.size = NFT_EXPR_SIZE(sizeof(struct nft_payload)),
	167	.eval = nft_payload_eval,
	168	.init = nft_payload_init,
	169	.dump = nft_payload_dump,
	170	};
	171
18140969 PNA	172	static inline void nft_csum_replace(__sum16 *sum, __wsum fsum, __wsum tsum)
	173	{
	174	sum = csum_fold(csum_add(csum_sub(~csum_unfold(sum), fsum), tsum));
	175	if (*sum == 0)
	176	*sum = CSUM_MANGLED_0;
	177	}
	178
	179	static bool nft_payload_udp_checksum(struct sk_buff *skb, unsigned int thoff)
	180	{
	181	struct udphdr *uh, _uh;
	182
	183	uh = skb_header_pointer(skb, thoff, sizeof(_uh), &_uh);
	184	if (!uh)
	185	return false;
	186
5fd02ebe	187	return (__force bool)uh->check;
18140969 PNA	188	}
	189
	190	static int nft_payload_l4csum_offset(const struct nft_pktinfo *pkt,
	191	struct sk_buff *skb,
	192	unsigned int *l4csum_offset)
	193	{
	194	switch (pkt->tprot) {
	195	case IPPROTO_TCP:
	196	*l4csum_offset = offsetof(struct tcphdr, check);
	197	break;
	198	case IPPROTO_UDP:
	199	if (!nft_payload_udp_checksum(skb, pkt->xt.thoff))
	200	return -1;
	201	/* Fall through. */
	202	case IPPROTO_UDPLITE:
	203	*l4csum_offset = offsetof(struct udphdr, check);
	204	break;
	205	case IPPROTO_ICMPV6:
	206	*l4csum_offset = offsetof(struct icmp6hdr, icmp6_cksum);
	207	break;
	208	default:
	209	return -1;
	210	}
	211
	212	*l4csum_offset += pkt->xt.thoff;
	213	return 0;
	214	}
	215
	216	static int nft_payload_l4csum_update(const struct nft_pktinfo *pkt,
	217	struct sk_buff *skb,
	218	__wsum fsum, __wsum tsum)
	219	{
	220	int l4csum_offset;
	221	__sum16 sum;
	222
	223	/* If we cannot determine layer 4 checksum offset or this packet doesn't
	224	* require layer 4 checksum recalculation, skip this packet.
	225	*/
	226	if (nft_payload_l4csum_offset(pkt, skb, &l4csum_offset) < 0)
	227	return 0;
	228
	229	if (skb_copy_bits(skb, l4csum_offset, &sum, sizeof(sum)) < 0)
	230	return -1;
	231
	232	/* Checksum mangling for an arbitrary amount of bytes, based on
	233	* inet_proto_csum_replace*() functions.
	234	*/
	235	if (skb->ip_summed != CHECKSUM_PARTIAL) {
	236	nft_csum_replace(&sum, fsum, tsum);
	237	if (skb->ip_summed == CHECKSUM_COMPLETE) {
	238	skb->csum = ~csum_add(csum_sub(~(skb->csum), fsum),
	239	tsum);
	240	}
	241	} else {
	242	sum = ~csum_fold(csum_add(csum_sub(csum_unfold(sum), fsum),
	243	tsum));
	244	}
	245
	246	if (!skb_make_writable(skb, l4csum_offset + sizeof(sum)) \|\|
	247	skb_store_bits(skb, l4csum_offset, &sum, sizeof(sum)) < 0)
	248	return -1;
	249
	250	return 0;
	251	}
252
053d20f5 PNA	253	static int nft_payload_csum_inet(struct sk_buff skb, const u32 src,
	254	__wsum fsum, __wsum tsum, int csum_offset)
	255	{
	256	__sum16 sum;
	257
	258	if (skb_copy_bits(skb, csum_offset, &sum, sizeof(sum)) < 0)
	259	return -1;
	260
	261	nft_csum_replace(&sum, fsum, tsum);
	262	if (!skb_make_writable(skb, csum_offset + sizeof(sum)) \|\|
	263	skb_store_bits(skb, csum_offset, &sum, sizeof(sum)) < 0)
	264	return -1;
	265
	266	return 0;
	267	}
	268
7ec3f7b4 PM	269	static void nft_payload_set_eval(const struct nft_expr *expr,
	270	struct nft_regs *regs,
	271	const struct nft_pktinfo *pkt)
	272	{
	273	const struct nft_payload_set *priv = nft_expr_priv(expr);
	274	struct sk_buff *skb = pkt->skb;
	275	const u32 *src = &regs->data[priv->sreg];
	276	int offset, csum_offset;
	277	__wsum fsum, tsum;
7ec3f7b4 PM	278
	279	switch (priv->base) {
	280	case NFT_PAYLOAD_LL_HEADER:
	281	if (!skb_mac_header_was_set(skb))
	282	goto err;
	283	offset = skb_mac_header(skb) - skb->data;
	284	break;
	285	case NFT_PAYLOAD_NETWORK_HEADER:
	286	offset = skb_network_offset(skb);
	287	break;
	288	case NFT_PAYLOAD_TRANSPORT_HEADER:
a20877b5 LZ	289	if (!pkt->tprot_set)
a20877b5 LZ	290	goto err;
7ec3f7b4 PM	291	offset = pkt->xt.thoff;
	292	break;
	293	default:
	294	BUG();
	295	}
	296
	297	csum_offset = offset + priv->csum_offset;
	298	offset += priv->offset;
	299
053d20f5	300	if ((priv->csum_type == NFT_PAYLOAD_CSUM_INET \|\| priv->csum_flags) &&
7ec3f7b4 PM	301	(priv->base != NFT_PAYLOAD_TRANSPORT_HEADER \|\|
7ec3f7b4 PM	302	skb->ip_summed != CHECKSUM_PARTIAL)) {
7ec3f7b4 PM	303	fsum = skb_checksum(skb, offset, priv->len, 0);
7ec3f7b4 PM	304	tsum = csum_partial(src, priv->len, 0);
7ec3f7b4	305
053d20f5 PNA	306	if (priv->csum_type == NFT_PAYLOAD_CSUM_INET &&
053d20f5 PNA	307	nft_payload_csum_inet(skb, src, fsum, tsum, csum_offset))
7ec3f7b4	308	goto err;
18140969 PNA	309
	310	if (priv->csum_flags &&
	311	nft_payload_l4csum_update(pkt, skb, fsum, tsum) < 0)
	312	goto err;
7ec3f7b4 PM	313	}
	314
	315	if (!skb_make_writable(skb, max(offset + priv->len, 0)) \|\|
	316	skb_store_bits(skb, offset, src, priv->len) < 0)
	317	goto err;
	318
	319	return;
	320	err:
	321	regs->verdict.code = NFT_BREAK;
	322	}
	323
	324	static int nft_payload_set_init(const struct nft_ctx *ctx,
	325	const struct nft_expr *expr,
	326	const struct nlattr * const tb[])
	327	{
	328	struct nft_payload_set *priv = nft_expr_priv(expr);
	329
	330	priv->base = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE]));
	331	priv->offset = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_OFFSET]));
	332	priv->len = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN]));
	333	priv->sreg = nft_parse_register(tb[NFTA_PAYLOAD_SREG]);
	334
	335	if (tb[NFTA_PAYLOAD_CSUM_TYPE])
	336	priv->csum_type =
	337	ntohl(nla_get_be32(tb[NFTA_PAYLOAD_CSUM_TYPE]));
	338	if (tb[NFTA_PAYLOAD_CSUM_OFFSET])
	339	priv->csum_offset =
	340	ntohl(nla_get_be32(tb[NFTA_PAYLOAD_CSUM_OFFSET]));
18140969 PNA	341	if (tb[NFTA_PAYLOAD_CSUM_FLAGS]) {
	342	u32 flags;
	343
	344	flags = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_CSUM_FLAGS]));
	345	if (flags & ~NFT_PAYLOAD_L4CSUM_PSEUDOHDR)
	346	return -EINVAL;
	347
	348	priv->csum_flags = flags;
	349	}
7ec3f7b4 PM	350
	351	switch (priv->csum_type) {
	352	case NFT_PAYLOAD_CSUM_NONE:
	353	case NFT_PAYLOAD_CSUM_INET:
	354	break;
	355	default:
	356	return -EOPNOTSUPP;
	357	}
	358
	359	return nft_validate_register_load(priv->sreg, priv->len);
	360	}
	361
	362	static int nft_payload_set_dump(struct sk_buff skb, const struct nft_expr expr)
	363	{
	364	const struct nft_payload_set *priv = nft_expr_priv(expr);
	365
	366	if (nft_dump_register(skb, NFTA_PAYLOAD_SREG, priv->sreg) \|\|
	367	nla_put_be32(skb, NFTA_PAYLOAD_BASE, htonl(priv->base)) \|\|
	368	nla_put_be32(skb, NFTA_PAYLOAD_OFFSET, htonl(priv->offset)) \|\|
	369	nla_put_be32(skb, NFTA_PAYLOAD_LEN, htonl(priv->len)) \|\|
	370	nla_put_be32(skb, NFTA_PAYLOAD_CSUM_TYPE, htonl(priv->csum_type)) \|\|
	371	nla_put_be32(skb, NFTA_PAYLOAD_CSUM_OFFSET,
18140969 PNA	372	htonl(priv->csum_offset)) \|\|
18140969 PNA	373	nla_put_be32(skb, NFTA_PAYLOAD_CSUM_FLAGS, htonl(priv->csum_flags)))
7ec3f7b4 PM	374	goto nla_put_failure;
	375	return 0;
	376
	377	nla_put_failure:
	378	return -1;
	379	}
	380
	381	static const struct nft_expr_ops nft_payload_set_ops = {
	382	.type = &nft_payload_type,
	383	.size = NFT_EXPR_SIZE(sizeof(struct nft_payload_set)),
	384	.eval = nft_payload_set_eval,
	385	.init = nft_payload_set_init,
	386	.dump = nft_payload_set_dump,
	387	};
	388
0ca743a5 PNA	389	static const struct nft_expr_ops *
	390	nft_payload_select_ops(const struct nft_ctx *ctx,
	391	const struct nlattr * const tb[])
c29b72e0 PM	392	{
	393	enum nft_payload_bases base;
	394	unsigned int offset, len;
	395
7ec3f7b4	396	if (tb[NFTA_PAYLOAD_BASE] == NULL \|\|
c29b72e0 PM	397	tb[NFTA_PAYLOAD_OFFSET] == NULL \|\|
	398	tb[NFTA_PAYLOAD_LEN] == NULL)
	399	return ERR_PTR(-EINVAL);
	400
	401	base = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE]));
	402	switch (base) {
	403	case NFT_PAYLOAD_LL_HEADER:
	404	case NFT_PAYLOAD_NETWORK_HEADER:
	405	case NFT_PAYLOAD_TRANSPORT_HEADER:
	406	break;
	407	default:
	408	return ERR_PTR(-EOPNOTSUPP);
	409	}
	410
7ec3f7b4 PM	411	if (tb[NFTA_PAYLOAD_SREG] != NULL) {
	412	if (tb[NFTA_PAYLOAD_DREG] != NULL)
	413	return ERR_PTR(-EINVAL);
	414	return &nft_payload_set_ops;
	415	}
	416
	417	if (tb[NFTA_PAYLOAD_DREG] == NULL)
	418	return ERR_PTR(-EINVAL);
	419
c29b72e0	420	offset = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_OFFSET]));
45d9bcda	421	len = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN]));
c29b72e0	422
f627ed91 NA	423	if (len <= 4 && is_power_of_2(len) && IS_ALIGNED(offset, len) &&
f627ed91 NA	424	base != NFT_PAYLOAD_LL_HEADER)
c29b72e0 PM	425	return &nft_payload_fast_ops;
	426	else
	427	return &nft_payload_ops;
	428	}
	429
4e24877e	430	struct nft_expr_type nft_payload_type __read_mostly = {
ef1f7df9	431	.name = "payload",
c29b72e0	432	.select_ops = nft_payload_select_ops,
96518518 PM	433	.policy = nft_payload_policy,
96518518 PM	434	.maxattr = NFTA_PAYLOAD_MAX,
ef1f7df9	435	.owner = THIS_MODULE,
96518518	436	};