[apple/xnu.git] / bsd / netinet6 / esp_chachapoly.c

/*
 * Copyright (c) 2017 Apple Inc. All rights reserved.
 *
 * @APPLE_OSREFERENCE_LICENSE_HEADER_START@
 *
 * This file contains Original Code and/or Modifications of Original Code
 * as defined in and that are subject to the Apple Public Source License
 * Version 2.0 (the 'License'). You may not use this file except in
 * compliance with the License. The rights granted to you under the License
 * may not be used to create, or enable the creation or redistribution of,
 * unlawful or unlicensed copies of an Apple operating system, or to
 * circumvent, violate, or enable the circumvention or violation of, any
 * terms of an Apple operating system software license agreement.
 *
 * Please obtain a copy of the License at
 * http://www.opensource.apple.com/apsl/ and read it before using this file.
 *
 * The Original Code and all software distributed under the License are
 * distributed on an 'AS IS' basis, WITHOUT WARRANTY OF ANY KIND, EITHER
 * EXPRESS OR IMPLIED, AND APPLE HEREBY DISCLAIMS ALL SUCH WARRANTIES,
 * INCLUDING WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE, QUIET ENJOYMENT OR NON-INFRINGEMENT.
 * Please see the License for the specific language governing rights and
 * limitations under the License.
 *
 * @APPLE_OSREFERENCE_LICENSE_HEADER_END@
 */

#include <sys/param.h>
#include <sys/systm.h>
#include <sys/socket.h>
#include <sys/queue.h>
#include <sys/syslog.h>
#include <sys/errno.h>
#include <sys/mbuf.h>
#include <sys/mcache.h>
#include <mach/vm_param.h>
#include <kern/locks.h>
#include <string.h>
#include <net/if.h>
#include <net/route.h>
#include <net/net_osdep.h>
#include <netinet6/ipsec.h>
#include <netinet6/esp.h>
#include <netinet6/esp_chachapoly.h>
#include <netkey/key.h>
#include <netkey/keydb.h>
#include <corecrypto/cc.h>
#include <libkern/crypto/chacha20poly1305.h>

#define ESP_CHACHAPOLY_SALT_LEN         4
#define ESP_CHACHAPOLY_KEY_LEN          32
#define ESP_CHACHAPOLY_NONCE_LEN        12

// The minimum alignment is documented in KALLOC_LOG2_MINALIGN
// which isn't accessible from here. Current minimum is 8.
_Static_assert(_Alignof(chacha20poly1305_ctx) <= 8,
    "Alignment guarantee is broken");

#if (((8 * (ESP_CHACHAPOLY_KEY_LEN + ESP_CHACHAPOLY_SALT_LEN)) != ESP_CHACHAPOLY_KEYBITS_WITH_SALT) || \
        (ESP_CHACHAPOLY_KEY_LEN != CCCHACHA20_KEY_NBYTES) || \
        (ESP_CHACHAPOLY_NONCE_LEN != CCCHACHA20POLY1305_NONCE_NBYTES))
#error "Invalid sizes"
#endif

extern lck_mtx_t *sadb_mutex;

typedef struct _esp_chachapoly_ctx {
	chacha20poly1305_ctx ccp_ctx;
	uint8_t ccp_salt[ESP_CHACHAPOLY_SALT_LEN];
	bool ccp_implicit_iv;
} esp_chachapoly_ctx_s, *esp_chachapoly_ctx_t;


#define ESP_ASSERT(_cond, _format, ...)                                                                                 \
	do {                                                                                                                                            \
	        if (!(_cond)) {                                                                                                                 \
	                panic("%s:%d " _format, __FUNCTION__, __LINE__, ##__VA_ARGS__);         \
	        }                                                                                                                                               \
	} while (0)

#define ESP_CHECK_ARG(_arg) ESP_ASSERT(_arg != NULL, #_arg " is NULL")

#define _esp_log(_level, _format, ...)  \
	log(_level, "%s:%d " _format, __FUNCTION__, __LINE__, ##__VA_ARGS__)
#define esp_log_err(_format, ...) _esp_log(LOG_ERR, _format, ##__VA_ARGS__)
#define esp_log_default(_format, ...) _esp_log(LOG_NOTICE, _format, ##__VA_ARGS__)

#define _esp_packet_log(_level, _format, ...)  \
	ipseclog((_level, "%s:%d " _format, __FUNCTION__, __LINE__, ##__VA_ARGS__))
#define esp_packet_log_err(_format, ...) _esp_packet_log(LOG_ERR, _format, ##__VA_ARGS__)

int
esp_chachapoly_mature(struct secasvar *sav)
{
	const struct esp_algorithm *algo;

	ESP_CHECK_ARG(sav);

	if ((sav->flags & SADB_X_EXT_OLD) != 0) {
		esp_log_err("ChaChaPoly is incompatible with SADB_X_EXT_OLD, SPI 0x%08x",
		    ntohl(sav->spi));
		return 1;
	}
	if ((sav->flags & SADB_X_EXT_DERIV) != 0) {
		esp_log_err("ChaChaPoly is incompatible with SADB_X_EXT_DERIV, SPI 0x%08x",
		    ntohl(sav->spi));
		return 1;
	}

	if (sav->alg_enc != SADB_X_EALG_CHACHA20POLY1305) {
		esp_log_err("ChaChaPoly unsupported algorithm %d, SPI 0x%08x",
		    sav->alg_enc, ntohl(sav->spi));
		return 1;
	}

	if (sav->key_enc == NULL) {
		esp_log_err("ChaChaPoly key is missing, SPI 0x%08x",
		    ntohl(sav->spi));
		return 1;
	}

	algo = esp_algorithm_lookup(sav->alg_enc);
	if (algo == NULL) {
		esp_log_err("ChaChaPoly lookup failed for algorithm %d, SPI 0x%08x",
		    sav->alg_enc, ntohl(sav->spi));
		return 1;
	}

	if (sav->key_enc->sadb_key_bits != ESP_CHACHAPOLY_KEYBITS_WITH_SALT) {
		esp_log_err("ChaChaPoly invalid key length %d bits, SPI 0x%08x",
		    sav->key_enc->sadb_key_bits, ntohl(sav->spi));
		return 1;
	}

	esp_log_default("ChaChaPoly Mature SPI 0x%08x%s %s dir %u state %u mode %u",
	    ntohl(sav->spi),
	    (((sav->flags & SADB_X_EXT_IIV) != 0) ? " IIV" : ""),
	    ((sav->sah->ipsec_if != NULL) ? if_name(sav->sah->ipsec_if) : "NONE"),
	    sav->sah->dir, sav->sah->state, sav->sah->saidx.mode);

	return 0;
}

size_t
esp_chachapoly_schedlen(__unused const struct esp_algorithm *algo)
{
	return sizeof(esp_chachapoly_ctx_s);
}

int
esp_chachapoly_schedule(__unused const struct esp_algorithm *algo,
    struct secasvar *sav)
{
	esp_chachapoly_ctx_t esp_ccp_ctx;
	int rc = 0;

	ESP_CHECK_ARG(sav);
	if (_KEYLEN(sav->key_enc) != ESP_CHACHAPOLY_KEY_LEN + ESP_CHACHAPOLY_SALT_LEN) {
		esp_log_err("ChaChaPoly Invalid key len %u, SPI 0x%08x",
		    _KEYLEN(sav->key_enc), ntohl(sav->spi));
		return EINVAL;
	}
	LCK_MTX_ASSERT(sadb_mutex, LCK_MTX_ASSERT_OWNED);

	esp_ccp_ctx = (esp_chachapoly_ctx_t)sav->sched;
	esp_ccp_ctx->ccp_implicit_iv = ((sav->flags & SADB_X_EXT_IIV) != 0);

	if (sav->ivlen != (esp_ccp_ctx->ccp_implicit_iv ? 0 : ESP_CHACHAPOLY_IV_LEN)) {
		esp_log_err("ChaChaPoly Invalid ivlen %u, SPI 0x%08x",
		    sav->ivlen, ntohl(sav->spi));
		return EINVAL;
	}

	rc = chacha20poly1305_init(&esp_ccp_ctx->ccp_ctx,
	    (const uint8_t *)_KEYBUF(sav->key_enc));
	if (rc != 0) {
		esp_log_err("ChaChaPoly chacha20poly1305_init failed %d, SPI 0x%08x",
		    rc, ntohl(sav->spi));
		return rc;
	}

	memcpy(esp_ccp_ctx->ccp_salt,
	    (const uint8_t *)_KEYBUF(sav->key_enc) + ESP_CHACHAPOLY_KEY_LEN,
	    sizeof(esp_ccp_ctx->ccp_salt));


	esp_log_default("ChaChaPoly Schedule SPI 0x%08x%s %s dir %u state %u mode %u",
	    ntohl(sav->spi), (esp_ccp_ctx->ccp_implicit_iv ? " IIV" : ""),
	    ((sav->sah->ipsec_if != NULL) ? if_name(sav->sah->ipsec_if) : "NONE"),
	    sav->sah->dir, sav->sah->state, sav->sah->saidx.mode);

	return 0;
}

int
esp_chachapoly_ivlen(const struct esp_algorithm *algo,
    struct secasvar *sav)
{
	ESP_CHECK_ARG(algo);

	if (sav != NULL &&
	    ((sav->sched != NULL && ((esp_chachapoly_ctx_t)sav->sched)->ccp_implicit_iv) ||
	    ((sav->flags & SADB_X_EXT_IIV) != 0))) {
		return 0;
	} else {
		return algo->ivlenval;
	}
}

int
esp_chachapoly_encrypt_finalize(struct secasvar *sav,
    unsigned char *tag,
    size_t tag_bytes)
{
	esp_chachapoly_ctx_t esp_ccp_ctx;
	int rc = 0;

	ESP_CHECK_ARG(sav);
	ESP_CHECK_ARG(tag);
	if (tag_bytes != ESP_CHACHAPOLY_ICV_LEN) {
		esp_log_err("ChaChaPoly Invalid tag_bytes %zu, SPI 0x%08x",
		    tag_bytes, ntohl(sav->spi));
		return EINVAL;
	}

	esp_ccp_ctx = (esp_chachapoly_ctx_t)sav->sched;
	rc = chacha20poly1305_finalize(&esp_ccp_ctx->ccp_ctx, tag);
	if (rc != 0) {
		esp_log_err("ChaChaPoly chacha20poly1305_finalize failed %d, SPI 0x%08x",
		    rc, ntohl(sav->spi));
		return rc;
	}
	return 0;
}

int
esp_chachapoly_decrypt_finalize(struct secasvar *sav,
    unsigned char *tag,
    size_t tag_bytes)
{
	esp_chachapoly_ctx_t esp_ccp_ctx;
	int rc = 0;

	ESP_CHECK_ARG(sav);
	ESP_CHECK_ARG(tag);
	if (tag_bytes != ESP_CHACHAPOLY_ICV_LEN) {
		esp_log_err("ChaChaPoly Invalid tag_bytes %zu, SPI 0x%08x",
		    tag_bytes, ntohl(sav->spi));
		return EINVAL;
	}

	esp_ccp_ctx = (esp_chachapoly_ctx_t)sav->sched;
	rc = chacha20poly1305_verify(&esp_ccp_ctx->ccp_ctx, tag);
	if (rc != 0) {
		esp_packet_log_err("ChaChaPoly chacha20poly1305_verify failed %d, SPI 0x%08x",
		    rc, ntohl(sav->spi));
		return rc;
	}
	return 0;
}

int
esp_chachapoly_encrypt(struct mbuf *m, // head of mbuf chain
    size_t off,                                        // offset to ESP header
    __unused size_t plen,
    struct secasvar *sav,
    __unused const struct esp_algorithm *algo,
    int ivlen)
{
	struct mbuf *s = m; // this mbuf
	int32_t soff = 0; // offset from the head of mbuf chain (m) to head of this mbuf (s)
	int32_t sn = 0; // offset from the head of this mbuf (s) to the body
	uint8_t *sp; // buffer of a given encryption round
	size_t len; // length of a given encryption round
	const int32_t ivoff = (int32_t)off + (int32_t)sizeof(struct newesp); // IV offset
	const size_t bodyoff = ivoff + ivlen; // body offset
	int rc = 0; // return code of corecrypto operations
	struct newesp esp_hdr; // ESP header for AAD
	_Static_assert(sizeof(esp_hdr) == 8, "Bad size");
	uint32_t nonce[ESP_CHACHAPOLY_NONCE_LEN / 4]; // ensure 32bit alignment
	_Static_assert(sizeof(nonce) == ESP_CHACHAPOLY_NONCE_LEN, "Bad nonce length");
	esp_chachapoly_ctx_t esp_ccp_ctx;

	ESP_CHECK_ARG(m);
	ESP_CHECK_ARG(sav);

	esp_ccp_ctx = (esp_chachapoly_ctx_t)sav->sched;

	if (ivlen != (esp_ccp_ctx->ccp_implicit_iv ? 0 : ESP_CHACHAPOLY_IV_LEN)) {
		m_freem(m);
		esp_log_err("ChaChaPoly Invalid ivlen %u, SPI 0x%08x",
		    ivlen, ntohl(sav->spi));
		return EINVAL;
	}
	if (sav->ivlen != ivlen) {
		m_freem(m);
		esp_log_err("ChaChaPoly Invalid sav->ivlen %u, SPI 0x%08x",
		    sav->ivlen, ntohl(sav->spi));
		return EINVAL;
	}

	// check if total packet length is enough to contain ESP + IV
	if (m->m_pkthdr.len < bodyoff) {
		esp_log_err("ChaChaPoly Packet too short %d < %zu, SPI 0x%08x",
		    m->m_pkthdr.len, bodyoff, ntohl(sav->spi));
		m_freem(m);
		return EINVAL;
	}

	rc = chacha20poly1305_reset(&esp_ccp_ctx->ccp_ctx);
	if (rc != 0) {
		m_freem(m);
		esp_log_err("ChaChaPoly chacha20poly1305_reset failed %d, SPI 0x%08x",
		    rc, ntohl(sav->spi));
		return rc;
	}

	// esp_hdr is used for nonce and AAD
	m_copydata(m, (int)off, sizeof(esp_hdr), (void *)&esp_hdr);

	// RFC 7634 dictates that the 12 byte nonce must be
	// the 4 byte salt followed by the 8 byte IV.
	// The IV MUST be non-repeating but does not need to be unpredictable,
	// so we use 4 bytes of 0 followed by the 4 byte ESP sequence number.
	// this allows us to use implicit IV -- draft-ietf-ipsecme-implicit-iv
	// Note that sav->seq is zero here so we must get esp_seq from esp_hdr
	memcpy(nonce, esp_ccp_ctx->ccp_salt, ESP_CHACHAPOLY_SALT_LEN);
	memset(((uint8_t *)nonce) + ESP_CHACHAPOLY_SALT_LEN, 0, 4);
	memcpy(((uint8_t *)nonce) + ESP_CHACHAPOLY_SALT_LEN + 4,
	    &esp_hdr.esp_seq, sizeof(esp_hdr.esp_seq));

	_Static_assert(4 + sizeof(esp_hdr.esp_seq) == ESP_CHACHAPOLY_IV_LEN,
	    "Bad IV length");
	_Static_assert(ESP_CHACHAPOLY_SALT_LEN + ESP_CHACHAPOLY_IV_LEN == sizeof(nonce),
	    "Bad nonce length");

	rc = chacha20poly1305_setnonce(&esp_ccp_ctx->ccp_ctx, (uint8_t *)nonce);
	if (rc != 0) {
		m_freem(m);
		esp_log_err("ChaChaPoly chacha20poly1305_setnonce failed %d, SPI 0x%08x",
		    rc, ntohl(sav->spi));
		return rc;
	}

	if (!esp_ccp_ctx->ccp_implicit_iv) {
		memcpy(sav->iv, ((uint8_t *)nonce) + ESP_CHACHAPOLY_SALT_LEN, ESP_CHACHAPOLY_IV_LEN);
		m_copyback(m, ivoff, ivlen, sav->iv);
	}
	cc_clear(sizeof(nonce), nonce);

	// Set Additional Authentication Data (AAD)
	rc = chacha20poly1305_aad(&esp_ccp_ctx->ccp_ctx,
	    sizeof(esp_hdr),
	    (void *)&esp_hdr);
	if (rc != 0) {
		m_freem(m);
		esp_log_err("ChaChaPoly chacha20poly1305_aad failed %d, SPI 0x%08x",
		    rc, ntohl(sav->spi));
		return rc;
	}

	// skip headers/IV
	while (s != NULL && soff < bodyoff) {
		if (soff + s->m_len > bodyoff) {
			sn = bodyoff - soff;
			break;
		}

		soff += s->m_len;
		s = s->m_next;
	}

	while (s != NULL && soff < m->m_pkthdr.len) {
		len = (size_t)(s->m_len - sn);
		if (len == 0) {
			// skip empty mbufs
			continue;
		}
		sp = mtod(s, uint8_t *) + sn;

		rc = chacha20poly1305_encrypt(&esp_ccp_ctx->ccp_ctx,
		    len, sp, sp);
		if (rc != 0) {
			m_freem(m);
			esp_log_err("ChaChaPoly chacha20poly1305_encrypt failed %d, SPI 0x%08x",
			    rc, ntohl(sav->spi));
			return rc;
		}

		sn = 0;
		soff += s->m_len;
		s = s->m_next;
	}
	if (s == NULL && soff != m->m_pkthdr.len) {
		m_freem(m);
		esp_log_err("ChaChaPoly not enough mbufs %d %d, SPI 0x%08x",
		    soff, m->m_pkthdr.len, ntohl(sav->spi));
		return EFBIG;
	}
	return 0;
}

int
esp_chachapoly_decrypt(struct mbuf *m, // head of mbuf chain
    size_t off,                                        // offset to ESP header
    struct secasvar *sav,
    __unused const struct esp_algorithm *algo,
    int ivlen)
{
	struct mbuf *s = m; // this mbuf
	int32_t soff = 0; // offset from the head of mbuf chain (m) to head of this mbuf (s)
	int32_t sn = 0; // offset from the head of this mbuf (s) to the body
	uint8_t *sp; // buffer of a given encryption round
	size_t len; // length of a given encryption round
	const int32_t ivoff = (int32_t)off + (int32_t)sizeof(struct newesp); // IV offset
	const int32_t bodyoff = ivoff + ivlen; // body offset
	int rc = 0; // return code of corecrypto operations
	struct newesp esp_hdr; // ESP header for AAD
	_Static_assert(sizeof(esp_hdr) == 8, "Bad size");
	uint32_t nonce[ESP_CHACHAPOLY_NONCE_LEN / 4]; // ensure 32bit alignment
	_Static_assert(sizeof(nonce) == ESP_CHACHAPOLY_NONCE_LEN, "Bad nonce length");
	esp_chachapoly_ctx_t esp_ccp_ctx;

	ESP_CHECK_ARG(m);
	ESP_CHECK_ARG(sav);

	esp_ccp_ctx = (esp_chachapoly_ctx_t)sav->sched;

	if (ivlen != (esp_ccp_ctx->ccp_implicit_iv ? 0 : ESP_CHACHAPOLY_IV_LEN)) {
		m_freem(m);
		esp_log_err("ChaChaPoly Invalid ivlen %u, SPI 0x%08x",
		    ivlen, ntohl(sav->spi));
		return EINVAL;
	}
	if (sav->ivlen != ivlen) {
		m_freem(m);
		esp_log_err("ChaChaPoly Invalid sav->ivlen %u, SPI 0x%08x",
		    sav->ivlen, ntohl(sav->spi));
		return EINVAL;
	}

	// check if total packet length is enough to contain ESP + IV
	if (m->m_pkthdr.len < bodyoff) {
		esp_packet_log_err("ChaChaPoly Packet too short %d < %u, SPI 0x%08x",
		    m->m_pkthdr.len, bodyoff, ntohl(sav->spi));
		m_freem(m);
		return EINVAL;
	}

	rc = chacha20poly1305_reset(&esp_ccp_ctx->ccp_ctx);
	if (rc != 0) {
		m_freem(m);
		esp_log_err("ChaChaPoly chacha20poly1305_reset failed %d, SPI 0x%08x",
		    rc, ntohl(sav->spi));
		return rc;
	}

	m_copydata(m, (int)off, sizeof(esp_hdr), (void *)&esp_hdr);

	// RFC 7634 dictates that the 12 byte nonce must be
	// the 4 byte salt followed by the 8 byte IV.
	memcpy(nonce, esp_ccp_ctx->ccp_salt, ESP_CHACHAPOLY_SALT_LEN);
	if (esp_ccp_ctx->ccp_implicit_iv) {
		// IV is implicit (4 zero bytes followed by the ESP sequence number)
		memset(((uint8_t *)nonce) + ESP_CHACHAPOLY_SALT_LEN, 0, 4);
		memcpy(((uint8_t *)nonce) + ESP_CHACHAPOLY_SALT_LEN + 4,
		    &esp_hdr.esp_seq, sizeof(esp_hdr.esp_seq));
		_Static_assert(4 + sizeof(esp_hdr.esp_seq) == ESP_CHACHAPOLY_IV_LEN, "Bad IV length");
	} else {
		// copy IV from packet
		m_copydata(m, ivoff, ESP_CHACHAPOLY_IV_LEN, ((uint8_t *)nonce) + ESP_CHACHAPOLY_SALT_LEN);
	}
	_Static_assert(ESP_CHACHAPOLY_SALT_LEN + ESP_CHACHAPOLY_IV_LEN == sizeof(nonce),
	    "Bad nonce length");

	rc = chacha20poly1305_setnonce(&esp_ccp_ctx->ccp_ctx, (uint8_t *)nonce);
	if (rc != 0) {
		m_freem(m);
		esp_log_err("ChaChaPoly chacha20poly1305_setnonce failed %d, SPI 0x%08x",
		    rc, ntohl(sav->spi));
		return rc;
	}
	cc_clear(sizeof(nonce), nonce);

	// Set Additional Authentication Data (AAD)
	rc = chacha20poly1305_aad(&esp_ccp_ctx->ccp_ctx,
	    sizeof(esp_hdr),
	    (void *)&esp_hdr);
	if (rc != 0) {
		m_freem(m);
		esp_log_err("ChaChaPoly chacha20poly1305_aad failed %d, SPI 0x%08x",
		    rc, ntohl(sav->spi));
		return rc;
	}

	// skip headers/IV
	while (s != NULL && soff < bodyoff) {
		if (soff + s->m_len > bodyoff) {
			sn = bodyoff - soff;
			break;
		}

		soff += s->m_len;
		s = s->m_next;
	}

	while (s != NULL && soff < m->m_pkthdr.len) {
		len = (size_t)(s->m_len - sn);
		if (len == 0) {
			// skip empty mbufs
			continue;
		}
		sp = mtod(s, uint8_t *) + sn;

		rc = chacha20poly1305_decrypt(&esp_ccp_ctx->ccp_ctx,
		    len, sp, sp);
		if (rc != 0) {
			m_freem(m);
			esp_packet_log_err("chacha20poly1305_decrypt failed %d, SPI 0x%08x",
			    rc, ntohl(sav->spi));
			return rc;
		}

		sn = 0;
		soff += s->m_len;
		s = s->m_next;
	}
	if (s == NULL && soff != m->m_pkthdr.len) {
		m_freem(m);
		esp_packet_log_err("not enough mbufs %d %d, SPI 0x%08x",
		    soff, m->m_pkthdr.len, ntohl(sav->spi));
		return EFBIG;
	}
	return 0;
}
Commit	Line	Data
5ba3f43e A	1	/*
	2	* Copyright (c) 2017 Apple Inc. All rights reserved.
	3	*
	4	* @APPLE_OSREFERENCE_LICENSE_HEADER_START@
	5	*
	6	* This file contains Original Code and/or Modifications of Original Code
	7	* as defined in and that are subject to the Apple Public Source License
	8	* Version 2.0 (the 'License'). You may not use this file except in
	9	* compliance with the License. The rights granted to you under the License
	10	* may not be used to create, or enable the creation or redistribution of,
	11	* unlawful or unlicensed copies of an Apple operating system, or to
	12	* circumvent, violate, or enable the circumvention or violation of, any
	13	* terms of an Apple operating system software license agreement.
	14	*
	15	* Please obtain a copy of the License at
	16	* http://www.opensource.apple.com/apsl/ and read it before using this file.
	17	*
	18	* The Original Code and all software distributed under the License are
	19	* distributed on an 'AS IS' basis, WITHOUT WARRANTY OF ANY KIND, EITHER
	20	* EXPRESS OR IMPLIED, AND APPLE HEREBY DISCLAIMS ALL SUCH WARRANTIES,
	21	* INCLUDING WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY,
	22	* FITNESS FOR A PARTICULAR PURPOSE, QUIET ENJOYMENT OR NON-INFRINGEMENT.
	23	* Please see the License for the specific language governing rights and
	24	* limitations under the License.
	25	*
	26	* @APPLE_OSREFERENCE_LICENSE_HEADER_END@
	27	*/
	28
	29	#include <sys/param.h>
	30	#include <sys/systm.h>
	31	#include <sys/socket.h>
	32	#include <sys/queue.h>
	33	#include <sys/syslog.h>
	34	#include <sys/errno.h>
	35	#include <sys/mbuf.h>
	36	#include <sys/mcache.h>
	37	#include <mach/vm_param.h>
	38	#include <kern/locks.h>
	39	#include <string.h>
	40	#include <net/if.h>
	41	#include <net/route.h>
	42	#include <net/net_osdep.h>
	43	#include <netinet6/ipsec.h>
	44	#include <netinet6/esp.h>
	45	#include <netinet6/esp_chachapoly.h>
	46	#include <netkey/key.h>
	47	#include <netkey/keydb.h>
	48	#include <corecrypto/cc.h>
	49	#include <libkern/crypto/chacha20poly1305.h>
	50
0a7de745 A	51	#define ESP_CHACHAPOLY_SALT_LEN 4
	52	#define ESP_CHACHAPOLY_KEY_LEN 32
	53	#define ESP_CHACHAPOLY_NONCE_LEN 12
5ba3f43e A	54
	55	// The minimum alignment is documented in KALLOC_LOG2_MINALIGN
	56	// which isn't accessible from here. Current minimum is 8.
	57	_Static_assert(_Alignof(chacha20poly1305_ctx) <= 8,
0a7de745	58	"Alignment guarantee is broken");
5ba3f43e	59
0a7de745 A	60	#if (((8 * (ESP_CHACHAPOLY_KEY_LEN + ESP_CHACHAPOLY_SALT_LEN)) != ESP_CHACHAPOLY_KEYBITS_WITH_SALT) \|\| \
	61	(ESP_CHACHAPOLY_KEY_LEN != CCCHACHA20_KEY_NBYTES) \|\| \
	62	(ESP_CHACHAPOLY_NONCE_LEN != CCCHACHA20POLY1305_NONCE_NBYTES))
5ba3f43e A	63	#error "Invalid sizes"
	64	#endif
	65
	66	extern lck_mtx_t *sadb_mutex;
	67
	68	typedef struct _esp_chachapoly_ctx {
	69	chacha20poly1305_ctx ccp_ctx;
	70	uint8_t ccp_salt[ESP_CHACHAPOLY_SALT_LEN];
	71	bool ccp_implicit_iv;
	72	} esp_chachapoly_ctx_s, *esp_chachapoly_ctx_t;
	73
	74
0a7de745 A	75	#define ESP_ASSERT(_cond, _format, ...) \
	76	do { \
	77	if (!(_cond)) { \
	78	panic("%s:%d " _format, __FUNCTION__, __LINE__, ##__VA_ARGS__); \
	79	} \
5ba3f43e A	80	} while (0)
5ba3f43e A	81
d9a64523	82	#define ESP_CHECK_ARG(_arg) ESP_ASSERT(_arg != NULL, #_arg " is NULL")
5ba3f43e A	83
	84	#define _esp_log(_level, _format, ...) \
	85	log(_level, "%s:%d " _format, __FUNCTION__, __LINE__, ##__VA_ARGS__)
	86	#define esp_log_err(_format, ...) _esp_log(LOG_ERR, _format, ##__VA_ARGS__)
d9a64523	87	#define esp_log_default(_format, ...) _esp_log(LOG_NOTICE, _format, ##__VA_ARGS__)
5ba3f43e A	88
	89	#define _esp_packet_log(_level, _format, ...) \
	90	ipseclog((_level, "%s:%d " _format, __FUNCTION__, __LINE__, ##__VA_ARGS__))
	91	#define esp_packet_log_err(_format, ...) _esp_packet_log(LOG_ERR, _format, ##__VA_ARGS__)
	92
	93	int
	94	esp_chachapoly_mature(struct secasvar *sav)
	95	{
	96	const struct esp_algorithm *algo;
	97
	98	ESP_CHECK_ARG(sav);
	99
	100	if ((sav->flags & SADB_X_EXT_OLD) != 0) {
d9a64523	101	esp_log_err("ChaChaPoly is incompatible with SADB_X_EXT_OLD, SPI 0x%08x",
0a7de745	102	ntohl(sav->spi));
5ba3f43e A	103	return 1;
	104	}
	105	if ((sav->flags & SADB_X_EXT_DERIV) != 0) {
d9a64523	106	esp_log_err("ChaChaPoly is incompatible with SADB_X_EXT_DERIV, SPI 0x%08x",
0a7de745	107	ntohl(sav->spi));
5ba3f43e A	108	return 1;
	109	}
	110
	111	if (sav->alg_enc != SADB_X_EALG_CHACHA20POLY1305) {
d9a64523	112	esp_log_err("ChaChaPoly unsupported algorithm %d, SPI 0x%08x",
0a7de745	113	sav->alg_enc, ntohl(sav->spi));
5ba3f43e A	114	return 1;
	115	}
	116
	117	if (sav->key_enc == NULL) {
d9a64523	118	esp_log_err("ChaChaPoly key is missing, SPI 0x%08x",
0a7de745	119	ntohl(sav->spi));
5ba3f43e A	120	return 1;
	121	}
	122
	123	algo = esp_algorithm_lookup(sav->alg_enc);
	124	if (algo == NULL) {
d9a64523	125	esp_log_err("ChaChaPoly lookup failed for algorithm %d, SPI 0x%08x",
0a7de745	126	sav->alg_enc, ntohl(sav->spi));
5ba3f43e A	127	return 1;
	128	}
	129
	130	if (sav->key_enc->sadb_key_bits != ESP_CHACHAPOLY_KEYBITS_WITH_SALT) {
d9a64523	131	esp_log_err("ChaChaPoly invalid key length %d bits, SPI 0x%08x",
0a7de745	132	sav->key_enc->sadb_key_bits, ntohl(sav->spi));
5ba3f43e A	133	return 1;
	134	}
	135
d9a64523	136	esp_log_default("ChaChaPoly Mature SPI 0x%08x%s %s dir %u state %u mode %u",
0a7de745 A	137	ntohl(sav->spi),
	138	(((sav->flags & SADB_X_EXT_IIV) != 0) ? " IIV" : ""),
	139	((sav->sah->ipsec_if != NULL) ? if_name(sav->sah->ipsec_if) : "NONE"),
	140	sav->sah->dir, sav->sah->state, sav->sah->saidx.mode);
d9a64523	141
5ba3f43e A	142	return 0;
	143	}
	144
f427ee49	145	size_t
5ba3f43e A	146	esp_chachapoly_schedlen(__unused const struct esp_algorithm *algo)
	147	{
	148	return sizeof(esp_chachapoly_ctx_s);
	149	}
	150
	151	int
	152	esp_chachapoly_schedule(__unused const struct esp_algorithm *algo,
0a7de745	153	struct secasvar *sav)
5ba3f43e A	154	{
	155	esp_chachapoly_ctx_t esp_ccp_ctx;
	156	int rc = 0;
	157
	158	ESP_CHECK_ARG(sav);
5ba3f43e	159	if (_KEYLEN(sav->key_enc) != ESP_CHACHAPOLY_KEY_LEN + ESP_CHACHAPOLY_SALT_LEN) {
d9a64523	160	esp_log_err("ChaChaPoly Invalid key len %u, SPI 0x%08x",
0a7de745	161	_KEYLEN(sav->key_enc), ntohl(sav->spi));
5ba3f43e A	162	return EINVAL;
	163	}
	164	LCK_MTX_ASSERT(sadb_mutex, LCK_MTX_ASSERT_OWNED);
	165
	166	esp_ccp_ctx = (esp_chachapoly_ctx_t)sav->sched;
d9a64523 A	167	esp_ccp_ctx->ccp_implicit_iv = ((sav->flags & SADB_X_EXT_IIV) != 0);
	168
	169	if (sav->ivlen != (esp_ccp_ctx->ccp_implicit_iv ? 0 : ESP_CHACHAPOLY_IV_LEN)) {
	170	esp_log_err("ChaChaPoly Invalid ivlen %u, SPI 0x%08x",
0a7de745	171	sav->ivlen, ntohl(sav->spi));
d9a64523 A	172	return EINVAL;
d9a64523 A	173	}
5ba3f43e A	174
5ba3f43e A	175	rc = chacha20poly1305_init(&esp_ccp_ctx->ccp_ctx,
0a7de745	176	(const uint8_t *)_KEYBUF(sav->key_enc));
5ba3f43e	177	if (rc != 0) {
d9a64523	178	esp_log_err("ChaChaPoly chacha20poly1305_init failed %d, SPI 0x%08x",
0a7de745	179	rc, ntohl(sav->spi));
5ba3f43e A	180	return rc;
	181	}
	182
	183	memcpy(esp_ccp_ctx->ccp_salt,
0a7de745 A	184	(const uint8_t *)_KEYBUF(sav->key_enc) + ESP_CHACHAPOLY_KEY_LEN,
0a7de745 A	185	sizeof(esp_ccp_ctx->ccp_salt));
5ba3f43e	186
d9a64523 A	187
d9a64523 A	188	esp_log_default("ChaChaPoly Schedule SPI 0x%08x%s %s dir %u state %u mode %u",
0a7de745 A	189	ntohl(sav->spi), (esp_ccp_ctx->ccp_implicit_iv ? " IIV" : ""),
	190	((sav->sah->ipsec_if != NULL) ? if_name(sav->sah->ipsec_if) : "NONE"),
	191	sav->sah->dir, sav->sah->state, sav->sah->saidx.mode);
5ba3f43e A	192
	193	return 0;
	194	}
	195
d9a64523 A	196	int
d9a64523 A	197	esp_chachapoly_ivlen(const struct esp_algorithm *algo,
0a7de745	198	struct secasvar *sav)
d9a64523 A	199	{
	200	ESP_CHECK_ARG(algo);
	201
	202	if (sav != NULL &&
0a7de745 A	203	((sav->sched != NULL && ((esp_chachapoly_ctx_t)sav->sched)->ccp_implicit_iv) \|\|
0a7de745 A	204	((sav->flags & SADB_X_EXT_IIV) != 0))) {
d9a64523 A	205	return 0;
	206	} else {
	207	return algo->ivlenval;
	208	}
	209	}
	210
5ba3f43e A	211	int
5ba3f43e A	212	esp_chachapoly_encrypt_finalize(struct secasvar *sav,
0a7de745	213	unsigned char *tag,
f427ee49	214	size_t tag_bytes)
5ba3f43e A	215	{
	216	esp_chachapoly_ctx_t esp_ccp_ctx;
	217	int rc = 0;
	218
	219	ESP_CHECK_ARG(sav);
	220	ESP_CHECK_ARG(tag);
	221	if (tag_bytes != ESP_CHACHAPOLY_ICV_LEN) {
f427ee49	222	esp_log_err("ChaChaPoly Invalid tag_bytes %zu, SPI 0x%08x",
0a7de745	223	tag_bytes, ntohl(sav->spi));
5ba3f43e A	224	return EINVAL;
	225	}
	226
	227	esp_ccp_ctx = (esp_chachapoly_ctx_t)sav->sched;
	228	rc = chacha20poly1305_finalize(&esp_ccp_ctx->ccp_ctx, tag);
	229	if (rc != 0) {
d9a64523	230	esp_log_err("ChaChaPoly chacha20poly1305_finalize failed %d, SPI 0x%08x",
0a7de745	231	rc, ntohl(sav->spi));
5ba3f43e A	232	return rc;
	233	}
	234	return 0;
	235	}
	236
	237	int
	238	esp_chachapoly_decrypt_finalize(struct secasvar *sav,
0a7de745	239	unsigned char *tag,
f427ee49	240	size_t tag_bytes)
5ba3f43e A	241	{
	242	esp_chachapoly_ctx_t esp_ccp_ctx;
	243	int rc = 0;
	244
	245	ESP_CHECK_ARG(sav);
	246	ESP_CHECK_ARG(tag);
	247	if (tag_bytes != ESP_CHACHAPOLY_ICV_LEN) {
f427ee49	248	esp_log_err("ChaChaPoly Invalid tag_bytes %zu, SPI 0x%08x",
0a7de745	249	tag_bytes, ntohl(sav->spi));
5ba3f43e A	250	return EINVAL;
	251	}
	252
	253	esp_ccp_ctx = (esp_chachapoly_ctx_t)sav->sched;
	254	rc = chacha20poly1305_verify(&esp_ccp_ctx->ccp_ctx, tag);
	255	if (rc != 0) {
d9a64523	256	esp_packet_log_err("ChaChaPoly chacha20poly1305_verify failed %d, SPI 0x%08x",
0a7de745	257	rc, ntohl(sav->spi));
5ba3f43e A	258	return rc;
	259	}
	260	return 0;
	261	}
	262
	263	int
	264	esp_chachapoly_encrypt(struct mbuf *m, // head of mbuf chain
0a7de745 A	265	size_t off, // offset to ESP header
	266	__unused size_t plen,
	267	struct secasvar *sav,
	268	__unused const struct esp_algorithm *algo,
	269	int ivlen)
5ba3f43e A	270	{
	271	struct mbuf *s = m; // this mbuf
	272	int32_t soff = 0; // offset from the head of mbuf chain (m) to head of this mbuf (s)
	273	int32_t sn = 0; // offset from the head of this mbuf (s) to the body
	274	uint8_t *sp; // buffer of a given encryption round
	275	size_t len; // length of a given encryption round
	276	const int32_t ivoff = (int32_t)off + (int32_t)sizeof(struct newesp); // IV offset
f427ee49	277	const size_t bodyoff = ivoff + ivlen; // body offset
5ba3f43e A	278	int rc = 0; // return code of corecrypto operations
	279	struct newesp esp_hdr; // ESP header for AAD
	280	_Static_assert(sizeof(esp_hdr) == 8, "Bad size");
d9a64523 A	281	uint32_t nonce[ESP_CHACHAPOLY_NONCE_LEN / 4]; // ensure 32bit alignment
d9a64523 A	282	_Static_assert(sizeof(nonce) == ESP_CHACHAPOLY_NONCE_LEN, "Bad nonce length");
5ba3f43e A	283	esp_chachapoly_ctx_t esp_ccp_ctx;
	284
	285	ESP_CHECK_ARG(m);
	286	ESP_CHECK_ARG(sav);
d9a64523 A	287
	288	esp_ccp_ctx = (esp_chachapoly_ctx_t)sav->sched;
	289
	290	if (ivlen != (esp_ccp_ctx->ccp_implicit_iv ? 0 : ESP_CHACHAPOLY_IV_LEN)) {
5ba3f43e	291	m_freem(m);
d9a64523	292	esp_log_err("ChaChaPoly Invalid ivlen %u, SPI 0x%08x",
0a7de745	293	ivlen, ntohl(sav->spi));
5ba3f43e A	294	return EINVAL;
5ba3f43e A	295	}
d9a64523	296	if (sav->ivlen != ivlen) {
5ba3f43e	297	m_freem(m);
d9a64523	298	esp_log_err("ChaChaPoly Invalid sav->ivlen %u, SPI 0x%08x",
0a7de745	299	sav->ivlen, ntohl(sav->spi));
5ba3f43e A	300	return EINVAL;
	301	}
	302
5ba3f43e A	303	// check if total packet length is enough to contain ESP + IV
5ba3f43e A	304	if (m->m_pkthdr.len < bodyoff) {
d9a64523	305	esp_log_err("ChaChaPoly Packet too short %d < %zu, SPI 0x%08x",
0a7de745	306	m->m_pkthdr.len, bodyoff, ntohl(sav->spi));
5ba3f43e A	307	m_freem(m);
	308	return EINVAL;
	309	}
	310
	311	rc = chacha20poly1305_reset(&esp_ccp_ctx->ccp_ctx);
	312	if (rc != 0) {
	313	m_freem(m);
d9a64523	314	esp_log_err("ChaChaPoly chacha20poly1305_reset failed %d, SPI 0x%08x",
0a7de745	315	rc, ntohl(sav->spi));
5ba3f43e A	316	return rc;
	317	}
	318
d9a64523 A	319	// esp_hdr is used for nonce and AAD
	320	m_copydata(m, (int)off, sizeof(esp_hdr), (void *)&esp_hdr);
	321
5ba3f43e A	322	// RFC 7634 dictates that the 12 byte nonce must be
	323	// the 4 byte salt followed by the 8 byte IV.
	324	// The IV MUST be non-repeating but does not need to be unpredictable,
	325	// so we use 4 bytes of 0 followed by the 4 byte ESP sequence number.
d9a64523 A	326	// this allows us to use implicit IV -- draft-ietf-ipsecme-implicit-iv
d9a64523 A	327	// Note that sav->seq is zero here so we must get esp_seq from esp_hdr
5ba3f43e	328	memcpy(nonce, esp_ccp_ctx->ccp_salt, ESP_CHACHAPOLY_SALT_LEN);
d9a64523 A	329	memset(((uint8_t *)nonce) + ESP_CHACHAPOLY_SALT_LEN, 0, 4);
d9a64523 A	330	memcpy(((uint8_t *)nonce) + ESP_CHACHAPOLY_SALT_LEN + 4,
0a7de745	331	&esp_hdr.esp_seq, sizeof(esp_hdr.esp_seq));
d9a64523 A	332
d9a64523 A	333	_Static_assert(4 + sizeof(esp_hdr.esp_seq) == ESP_CHACHAPOLY_IV_LEN,
0a7de745	334	"Bad IV length");
5ba3f43e	335	_Static_assert(ESP_CHACHAPOLY_SALT_LEN + ESP_CHACHAPOLY_IV_LEN == sizeof(nonce),
0a7de745	336	"Bad nonce length");
5ba3f43e	337
d9a64523	338	rc = chacha20poly1305_setnonce(&esp_ccp_ctx->ccp_ctx, (uint8_t *)nonce);
5ba3f43e A	339	if (rc != 0) {
5ba3f43e A	340	m_freem(m);
d9a64523	341	esp_log_err("ChaChaPoly chacha20poly1305_setnonce failed %d, SPI 0x%08x",
0a7de745	342	rc, ntohl(sav->spi));
5ba3f43e A	343	return rc;
	344	}
	345
	346	if (!esp_ccp_ctx->ccp_implicit_iv) {
d9a64523	347	memcpy(sav->iv, ((uint8_t *)nonce) + ESP_CHACHAPOLY_SALT_LEN, ESP_CHACHAPOLY_IV_LEN);
5ba3f43e A	348	m_copyback(m, ivoff, ivlen, sav->iv);
	349	}
	350	cc_clear(sizeof(nonce), nonce);
	351
	352	// Set Additional Authentication Data (AAD)
5ba3f43e	353	rc = chacha20poly1305_aad(&esp_ccp_ctx->ccp_ctx,
0a7de745 A	354	sizeof(esp_hdr),
0a7de745 A	355	(void *)&esp_hdr);
5ba3f43e A	356	if (rc != 0) {
5ba3f43e A	357	m_freem(m);
d9a64523	358	esp_log_err("ChaChaPoly chacha20poly1305_aad failed %d, SPI 0x%08x",
0a7de745	359	rc, ntohl(sav->spi));
5ba3f43e A	360	return rc;
	361	}
	362
	363	// skip headers/IV
	364	while (s != NULL && soff < bodyoff) {
	365	if (soff + s->m_len > bodyoff) {
	366	sn = bodyoff - soff;
	367	break;
	368	}
	369
	370	soff += s->m_len;
	371	s = s->m_next;
	372	}
	373
	374	while (s != NULL && soff < m->m_pkthdr.len) {
	375	len = (size_t)(s->m_len - sn);
	376	if (len == 0) {
	377	// skip empty mbufs
	378	continue;
	379	}
	380	sp = mtod(s, uint8_t *) + sn;
	381
	382	rc = chacha20poly1305_encrypt(&esp_ccp_ctx->ccp_ctx,
0a7de745	383	len, sp, sp);
5ba3f43e A	384	if (rc != 0) {
5ba3f43e A	385	m_freem(m);
d9a64523	386	esp_log_err("ChaChaPoly chacha20poly1305_encrypt failed %d, SPI 0x%08x",
0a7de745	387	rc, ntohl(sav->spi));
5ba3f43e A	388	return rc;
	389	}
	390
	391	sn = 0;
	392	soff += s->m_len;
	393	s = s->m_next;
	394	}
	395	if (s == NULL && soff != m->m_pkthdr.len) {
	396	m_freem(m);
d9a64523	397	esp_log_err("ChaChaPoly not enough mbufs %d %d, SPI 0x%08x",
0a7de745	398	soff, m->m_pkthdr.len, ntohl(sav->spi));
5ba3f43e A	399	return EFBIG;
	400	}
	401	return 0;
	402	}
	403
	404	int
	405	esp_chachapoly_decrypt(struct mbuf *m, // head of mbuf chain
0a7de745 A	406	size_t off, // offset to ESP header
	407	struct secasvar *sav,
	408	__unused const struct esp_algorithm *algo,
	409	int ivlen)
5ba3f43e A	410	{
	411	struct mbuf *s = m; // this mbuf
	412	int32_t soff = 0; // offset from the head of mbuf chain (m) to head of this mbuf (s)
	413	int32_t sn = 0; // offset from the head of this mbuf (s) to the body
	414	uint8_t *sp; // buffer of a given encryption round
	415	size_t len; // length of a given encryption round
	416	const int32_t ivoff = (int32_t)off + (int32_t)sizeof(struct newesp); // IV offset
d9a64523	417	const int32_t bodyoff = ivoff + ivlen; // body offset
5ba3f43e A	418	int rc = 0; // return code of corecrypto operations
	419	struct newesp esp_hdr; // ESP header for AAD
	420	_Static_assert(sizeof(esp_hdr) == 8, "Bad size");
d9a64523 A	421	uint32_t nonce[ESP_CHACHAPOLY_NONCE_LEN / 4]; // ensure 32bit alignment
d9a64523 A	422	_Static_assert(sizeof(nonce) == ESP_CHACHAPOLY_NONCE_LEN, "Bad nonce length");
5ba3f43e A	423	esp_chachapoly_ctx_t esp_ccp_ctx;
	424
	425	ESP_CHECK_ARG(m);
	426	ESP_CHECK_ARG(sav);
d9a64523 A	427
	428	esp_ccp_ctx = (esp_chachapoly_ctx_t)sav->sched;
	429
	430	if (ivlen != (esp_ccp_ctx->ccp_implicit_iv ? 0 : ESP_CHACHAPOLY_IV_LEN)) {
5ba3f43e	431	m_freem(m);
d9a64523	432	esp_log_err("ChaChaPoly Invalid ivlen %u, SPI 0x%08x",
0a7de745	433	ivlen, ntohl(sav->spi));
5ba3f43e A	434	return EINVAL;
5ba3f43e A	435	}
d9a64523	436	if (sav->ivlen != ivlen) {
5ba3f43e	437	m_freem(m);
d9a64523	438	esp_log_err("ChaChaPoly Invalid sav->ivlen %u, SPI 0x%08x",
0a7de745	439	sav->ivlen, ntohl(sav->spi));
5ba3f43e A	440	return EINVAL;
	441	}
	442
5ba3f43e A	443	// check if total packet length is enough to contain ESP + IV
5ba3f43e A	444	if (m->m_pkthdr.len < bodyoff) {
94ff46dc	445	esp_packet_log_err("ChaChaPoly Packet too short %d < %u, SPI 0x%08x",
0a7de745	446	m->m_pkthdr.len, bodyoff, ntohl(sav->spi));
5ba3f43e A	447	m_freem(m);
	448	return EINVAL;
	449	}
	450
	451	rc = chacha20poly1305_reset(&esp_ccp_ctx->ccp_ctx);
	452	if (rc != 0) {
	453	m_freem(m);
d9a64523	454	esp_log_err("ChaChaPoly chacha20poly1305_reset failed %d, SPI 0x%08x",
0a7de745	455	rc, ntohl(sav->spi));
5ba3f43e A	456	return rc;
	457	}
	458
	459	m_copydata(m, (int)off, sizeof(esp_hdr), (void *)&esp_hdr);
	460
	461	// RFC 7634 dictates that the 12 byte nonce must be
	462	// the 4 byte salt followed by the 8 byte IV.
	463	memcpy(nonce, esp_ccp_ctx->ccp_salt, ESP_CHACHAPOLY_SALT_LEN);
	464	if (esp_ccp_ctx->ccp_implicit_iv) {
	465	// IV is implicit (4 zero bytes followed by the ESP sequence number)
d9a64523 A	466	memset(((uint8_t *)nonce) + ESP_CHACHAPOLY_SALT_LEN, 0, 4);
d9a64523 A	467	memcpy(((uint8_t *)nonce) + ESP_CHACHAPOLY_SALT_LEN + 4,
0a7de745	468	&esp_hdr.esp_seq, sizeof(esp_hdr.esp_seq));
5ba3f43e A	469	_Static_assert(4 + sizeof(esp_hdr.esp_seq) == ESP_CHACHAPOLY_IV_LEN, "Bad IV length");
	470	} else {
	471	// copy IV from packet
d9a64523	472	m_copydata(m, ivoff, ESP_CHACHAPOLY_IV_LEN, ((uint8_t *)nonce) + ESP_CHACHAPOLY_SALT_LEN);
5ba3f43e A	473	}
5ba3f43e A	474	_Static_assert(ESP_CHACHAPOLY_SALT_LEN + ESP_CHACHAPOLY_IV_LEN == sizeof(nonce),
0a7de745	475	"Bad nonce length");
5ba3f43e	476
d9a64523	477	rc = chacha20poly1305_setnonce(&esp_ccp_ctx->ccp_ctx, (uint8_t *)nonce);
5ba3f43e A	478	if (rc != 0) {
5ba3f43e A	479	m_freem(m);
d9a64523	480	esp_log_err("ChaChaPoly chacha20poly1305_setnonce failed %d, SPI 0x%08x",
0a7de745	481	rc, ntohl(sav->spi));
5ba3f43e A	482	return rc;
	483	}
	484	cc_clear(sizeof(nonce), nonce);
	485
	486	// Set Additional Authentication Data (AAD)
	487	rc = chacha20poly1305_aad(&esp_ccp_ctx->ccp_ctx,
0a7de745 A	488	sizeof(esp_hdr),
0a7de745 A	489	(void *)&esp_hdr);
5ba3f43e A	490	if (rc != 0) {
5ba3f43e A	491	m_freem(m);
d9a64523	492	esp_log_err("ChaChaPoly chacha20poly1305_aad failed %d, SPI 0x%08x",
0a7de745	493	rc, ntohl(sav->spi));
5ba3f43e A	494	return rc;
	495	}
	496
	497	// skip headers/IV
	498	while (s != NULL && soff < bodyoff) {
	499	if (soff + s->m_len > bodyoff) {
	500	sn = bodyoff - soff;
	501	break;
	502	}
	503
	504	soff += s->m_len;
	505	s = s->m_next;
	506	}
	507
	508	while (s != NULL && soff < m->m_pkthdr.len) {
	509	len = (size_t)(s->m_len - sn);
	510	if (len == 0) {
	511	// skip empty mbufs
	512	continue;
	513	}
	514	sp = mtod(s, uint8_t *) + sn;
	515
	516	rc = chacha20poly1305_decrypt(&esp_ccp_ctx->ccp_ctx,
0a7de745	517	len, sp, sp);
5ba3f43e A	518	if (rc != 0) {
5ba3f43e A	519	m_freem(m);
d9a64523	520	esp_packet_log_err("chacha20poly1305_decrypt failed %d, SPI 0x%08x",
0a7de745	521	rc, ntohl(sav->spi));
5ba3f43e A	522	return rc;
	523	}
	524
	525	sn = 0;
	526	soff += s->m_len;
	527	s = s->m_next;
	528	}
	529	if (s == NULL && soff != m->m_pkthdr.len) {
	530	m_freem(m);
d9a64523	531	esp_packet_log_err("not enough mbufs %d %d, SPI 0x%08x",
0a7de745	532	soff, m->m_pkthdr.len, ntohl(sav->spi));
5ba3f43e A	533	return EFBIG;
	534	}
	535	return 0;
	536	}