subsys/net/l2/ieee802154/ieee802154_frame.c

/*
 * Copyright (c) 2016 Intel Corporation.
 *
 * SPDX-License-Identifier: Apache-2.0
 */

/**
 * @file
 * @brief IEEE 802.15.4 MAC frame related functions implementation
 */

#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(net_ieee802154_frame, CONFIG_NET_L2_IEEE802154_LOG_LEVEL);

#include "ieee802154_frame.h"
#include "ieee802154_security.h"

#include <zephyr/net/net_core.h>
#include <zephyr/net/net_if.h>

#include <ipv6.h>
#include <nbr.h>

#define dbg_print_fs(fs)                                                                           \
	NET_DBG("fs(1): %u/%u/%u/%u/%u/%u", fs->fc.frame_type, fs->fc.security_enabled,            \
		fs->fc.frame_pending, fs->fc.ar, fs->fc.pan_id_comp, fs->fc.reserved);             \
	NET_DBG("fs(2): %u/%u/%u/%u/%u - %u", fs->fc.seq_num_suppr, fs->fc.ie_list,                \
		fs->fc.dst_addr_mode, fs->fc.frame_version, fs->fc.src_addr_mode, fs->sequence)

#define BUF_TIMEOUT K_MSEC(50)

#ifdef CONFIG_NET_L2_IEEE802154_SECURITY
const uint8_t level_2_tag_size[4] = {0, IEEE8021254_AUTH_TAG_LENGTH_32,
				     IEEE8021254_AUTH_TAG_LENGTH_64,
				     IEEE8021254_AUTH_TAG_LENGTH_128};
#endif

struct ieee802154_fcf_seq *ieee802154_validate_fc_seq(uint8_t *buf, uint8_t **p_buf,
						      uint8_t *length)
{
	struct ieee802154_fcf_seq *fs = (struct ieee802154_fcf_seq *)buf;

	dbg_print_fs(fs);

	/** Basic FC checks */
	if (fs->fc.frame_type == IEEE802154_FRAME_TYPE_RESERVED ||
	    fs->fc.frame_version >= IEEE802154_VERSION_RESERVED) {
		return NULL;
	}

	if (fs->fc.frame_type == IEEE802154_FRAME_TYPE_MULTIPURPOSE) {
		if (fs->fc.frame_version != 0) {
			return NULL;
		}
	} else {
		/** Only for versions 2003/2006 */
		if (fs->fc.frame_version < IEEE802154_VERSION_802154 &&
		    (fs->fc.dst_addr_mode == IEEE802154_ADDR_MODE_RESERVED ||
		     fs->fc.src_addr_mode == IEEE802154_ADDR_MODE_RESERVED ||
		     fs->fc.frame_type >= IEEE802154_FRAME_TYPE_RESERVED)) {
			return NULL;
		}
	}

	if (fs->fc.frame_type == IEEE802154_FRAME_TYPE_BEACON &&
	    (fs->fc.dst_addr_mode != IEEE802154_ADDR_MODE_NONE ||
	     fs->fc.src_addr_mode == IEEE802154_ADDR_MODE_NONE || fs->fc.pan_id_comp)) {
		/** See section 7.2.2.1.1 */
		return NULL;
	} else if (fs->fc.frame_type == IEEE802154_FRAME_TYPE_DATA &&
		   fs->fc.dst_addr_mode == IEEE802154_ADDR_MODE_NONE &&
		   fs->fc.src_addr_mode == IEEE802154_ADDR_MODE_NONE) {
		/** See section 7.2.2.2.1 */
		return NULL;
	} else if (fs->fc.frame_type == IEEE802154_FRAME_TYPE_MAC_COMMAND && fs->fc.frame_pending) {
		/** See section 7.3 */
		return NULL;
	}

#ifndef CONFIG_NET_L2_IEEE802154_SECURITY
	if (fs->fc.security_enabled) {
		return NULL;
	}
#endif

	if (p_buf) {
		*length -= IEEE802154_FCF_SEQ_LENGTH;
		*p_buf = buf + IEEE802154_FCF_SEQ_LENGTH;
	}

	return fs;
}

static inline bool validate_addr(uint8_t *buf, uint8_t **p_buf, uint8_t *length,
				 enum ieee802154_addressing_mode mode, bool pan_id_compression,
				 struct ieee802154_address_field **addr)
{
	uint8_t len = 0;

	*p_buf = buf;

	NET_DBG("Buf %p - mode %d - pan id comp %d", buf, mode, pan_id_compression);

	if (mode == IEEE802154_ADDR_MODE_NONE) {
		*addr = NULL;
		return true;
	}

	if (!pan_id_compression) {
		len = IEEE802154_PAN_ID_LENGTH;
	}

	if (mode == IEEE802154_ADDR_MODE_SHORT) {
		len += IEEE802154_SHORT_ADDR_LENGTH;
	} else {
		/* IEEE802154_ADDR_MODE_EXTENDED */
		len += IEEE802154_EXT_ADDR_LENGTH;
	}

	if (len > *length) {
		return false;
	}

	*p_buf += len;
	*length -= len;

	*addr = (struct ieee802154_address_field *)buf;

	return true;
}

#ifdef CONFIG_NET_L2_IEEE802154_SECURITY
struct ieee802154_aux_security_hdr *
ieee802154_validate_aux_security_hdr(uint8_t *buf, uint8_t **p_buf, uint8_t *length)
{
	struct ieee802154_aux_security_hdr *ash = (struct ieee802154_aux_security_hdr *)buf;
	uint8_t len = IEEE802154_SECURITY_CF_LENGTH + IEEE802154_SECURITY_FRAME_COUNTER_LENGTH;

	/* At least the asf is sized of: control field + frame counter */
	if (*length < len) {
		return NULL;
	}

	/* Only implicit key mode is supported for now */
	if (ash->control.key_id_mode != IEEE802154_KEY_ID_MODE_IMPLICIT) {
		return NULL;
	}

	/* Explicit key must have a key index != 0x00, see section 7.6.2.4.2 */
	switch (ash->control.key_id_mode) {
	case IEEE802154_KEY_ID_MODE_IMPLICIT:
		break;
	case IEEE802154_KEY_ID_MODE_INDEX:
		len += IEEE8021254_KEY_ID_FIELD_INDEX_LENGTH;
		if (*length < len) {
			return NULL;
		}

		if (!ash->kif.mode_1.key_index) {
			return NULL;
		}

		break;
	case IEEE802154_KEY_ID_MODE_SRC_4_INDEX:
		len += IEEE8021254_KEY_ID_FIELD_SRC_4_INDEX_LENGTH;
		if (*length < len) {
			return NULL;
		}

		if (!ash->kif.mode_2.key_index) {
			return NULL;
		}

		break;
	case IEEE802154_KEY_ID_MODE_SRC_8_INDEX:
		len += IEEE8021254_KEY_ID_FIELD_SRC_8_INDEX_LENGTH;
		if (*length < len) {
			return NULL;
		}

		if (!ash->kif.mode_3.key_index) {
			return NULL;
		}

		break;
	}

	*p_buf = buf + len;
	*length -= len;

	return ash;
}
#endif /* CONFIG_NET_L2_IEEE802154_SECURITY */

static inline bool validate_beacon(struct ieee802154_mpdu *mpdu, uint8_t *buf, uint8_t *length)
{
	struct ieee802154_beacon *b = (struct ieee802154_beacon *)buf;
	struct ieee802154_pas_spec *pas;
	uint8_t len = IEEE802154_BEACON_SF_SIZE + IEEE802154_BEACON_GTS_SPEC_SIZE;

	if (*length < len) {
		return false;
	}

	if (b->gts.desc_count) {
		len += IEEE802154_BEACON_GTS_DIR_SIZE +
		       b->gts.desc_count * IEEE802154_BEACON_GTS_SIZE;
	}

	if (*length < len) {
		return false;
	}

	pas = (struct ieee802154_pas_spec *)buf + len;

	len += IEEE802154_BEACON_PAS_SPEC_SIZE;
	if (*length < len) {
		return false;
	}

	if (pas->nb_sap || pas->nb_eap) {
		len += (pas->nb_sap * IEEE802154_SHORT_ADDR_LENGTH) +
		       (pas->nb_eap * IEEE802154_EXT_ADDR_LENGTH);
	}

	if (*length < len) {
		return false;
	}

	*length -= len;
	mpdu->beacon = b;

	return true;
}

static inline bool validate_mac_command_cfi_to_mhr(struct ieee802154_mhr *mhr, uint8_t ar,
						   uint8_t comp, uint8_t src_bf,
						   bool src_pan_brdcst_chk, uint8_t dst_bf,
						   bool dst_brdcst_chk)
{
	if (mhr->fs->fc.ar != ar || mhr->fs->fc.pan_id_comp != comp) {
		return false;
	}

	if (!(BIT(mhr->fs->fc.src_addr_mode) & src_bf) ||
	    !(BIT(mhr->fs->fc.dst_addr_mode) & dst_bf)) {
		return false;
	}

	/* This should be set only when comp == 0 */
	if (src_pan_brdcst_chk) {
		if (mhr->src_addr->plain.pan_id != IEEE802154_BROADCAST_PAN_ID) {
			return false;
		}
	}

	/* This should be set only when comp == 0 */
	if (dst_brdcst_chk) {
		/* broadcast address is symmetric so no need to swap byte order */
		if (mhr->dst_addr->plain.addr.short_addr != IEEE802154_BROADCAST_ADDRESS) {
			return false;
		}
	}

	return true;
}

static inline bool validate_mac_command(struct ieee802154_mpdu *mpdu, uint8_t *buf, uint8_t *length)
{
	struct ieee802154_command *c = (struct ieee802154_command *)buf;
	uint8_t len = IEEE802154_CMD_CFI_LENGTH;
	bool src_pan_brdcst_chk = false;
	bool dst_brdcst_chk = false;
	uint8_t comp = 0U;
	uint8_t ar = 0U;
	uint8_t src_bf, dst_bf;

	if (*length < len) {
		return false;
	}

	switch (c->cfi) {
	case IEEE802154_CFI_UNKNOWN:
		return false;
	case IEEE802154_CFI_ASSOCIATION_REQUEST:
		ar = 1U;
		len += IEEE802154_CMD_ASSOC_REQ_LENGTH;
		src_bf = BIT(IEEE802154_ADDR_MODE_EXTENDED);
		src_pan_brdcst_chk = true;
		dst_bf = BIT(IEEE802154_ADDR_MODE_SHORT) | BIT(IEEE802154_ADDR_MODE_EXTENDED);

		break;
	case IEEE802154_CFI_ASSOCIATION_RESPONSE:
		len += IEEE802154_CMD_ASSOC_RES_LENGTH;
		__fallthrough;
	case IEEE802154_CFI_DISASSOCIATION_NOTIFICATION:
		if (c->cfi == IEEE802154_CFI_DISASSOCIATION_NOTIFICATION) {
			len += IEEE802154_CMD_DISASSOC_NOTE_LENGTH;
		}
		__fallthrough;
	case IEEE802154_CFI_PAN_ID_CONLICT_NOTIFICATION:
		ar = 1U;
		comp = 1U;
		src_bf = BIT(IEEE802154_ADDR_MODE_EXTENDED);
		dst_bf = BIT(IEEE802154_ADDR_MODE_EXTENDED);

		break;
	case IEEE802154_CFI_DATA_REQUEST:
		ar = 1U;
		src_bf = BIT(IEEE802154_ADDR_MODE_SHORT) | BIT(IEEE802154_ADDR_MODE_EXTENDED);

		if (mpdu->mhr.fs->fc.dst_addr_mode == IEEE802154_ADDR_MODE_NONE) {
			dst_bf = BIT(IEEE802154_ADDR_MODE_NONE);
		} else {
			comp = 1U;
			dst_bf = BIT(IEEE802154_ADDR_MODE_SHORT) |
				 BIT(IEEE802154_ADDR_MODE_EXTENDED);
		}

		break;
	case IEEE802154_CFI_ORPHAN_NOTIFICATION:
		comp = 1U;
		src_bf = BIT(IEEE802154_ADDR_MODE_EXTENDED);
		dst_bf = BIT(IEEE802154_ADDR_MODE_SHORT);

		break;
	case IEEE802154_CFI_BEACON_REQUEST:
		src_bf = BIT(IEEE802154_ADDR_MODE_NONE);
		dst_bf = BIT(IEEE802154_ADDR_MODE_SHORT);
		dst_brdcst_chk = true;

		break;
	case IEEE802154_CFI_COORDINATOR_REALIGNEMENT:
		len += IEEE802154_CMD_COORD_REALIGN_LENGTH;
		src_bf = BIT(IEEE802154_ADDR_MODE_EXTENDED);

		if (mpdu->mhr.fs->fc.dst_addr_mode == IEEE802154_ADDR_MODE_SHORT) {
			dst_bf = BIT(IEEE802154_ADDR_MODE_SHORT);
			dst_brdcst_chk = true;
		} else {
			dst_bf = BIT(IEEE802154_ADDR_MODE_EXTENDED);
		}

		break;
	case IEEE802154_CFI_GTS_REQUEST:
		len += IEEE802154_GTS_REQUEST_LENGTH;
		ar = 1U;
		src_bf = BIT(IEEE802154_ADDR_MODE_SHORT);
		dst_bf = BIT(IEEE802154_ADDR_MODE_NONE);

		break;
	default:
		return false;
	}

	if (*length < len) {
		return false;
	}

	if (!validate_mac_command_cfi_to_mhr(&mpdu->mhr, ar, comp, src_bf,
					     src_pan_brdcst_chk, dst_bf,
					     dst_brdcst_chk)) {
		return false;
	}

	*length -= len;
	mpdu->command = c;

	return true;
}

static inline bool validate_payload_and_mfr(struct ieee802154_mpdu *mpdu, uint8_t *buf,
					    uint8_t *p_buf, uint8_t *length)
{
	uint8_t type = mpdu->mhr.fs->fc.frame_type;

	NET_DBG("Header size: %u, payload size %u", (uint32_t)(p_buf - buf), *length);

	if (type == IEEE802154_FRAME_TYPE_BEACON) {
		if (!validate_beacon(mpdu, p_buf, length)) {
			return false;
		}
	} else if (type == IEEE802154_FRAME_TYPE_DATA) {
		/** A data frame embeds a payload */
		if (*length == 0U) {
			return false;
		}

		mpdu->payload = (void *)p_buf;
	} else if (type == IEEE802154_FRAME_TYPE_ACK) {
		/** An ACK frame has no payload */
		if (*length) {
			return false;
		}

		mpdu->payload = NULL;
	} else {
		if (!validate_mac_command(mpdu, p_buf, length)) {
			return false;
		}
	}

	if (*length) {
		mpdu->mfr = (struct ieee802154_mfr *)(p_buf + *length);
	} else {
		mpdu->mfr = NULL;
	}

	return true;
}

bool ieee802154_validate_frame(uint8_t *buf, uint8_t length, struct ieee802154_mpdu *mpdu)
{
	uint8_t *p_buf = NULL;

	if (length > IEEE802154_MTU || length < IEEE802154_MIN_LENGTH) {
		NET_DBG("Wrong packet length: %d", length);
		return false;
	}

	mpdu->mhr.fs = ieee802154_validate_fc_seq(buf, &p_buf, &length);
	if (!mpdu->mhr.fs) {
		return false;
	}

	/* TODO: Support later version's frame types */
	if (mpdu->mhr.fs->fc.frame_type > IEEE802154_FRAME_TYPE_MAC_COMMAND) {
		return false;
	}

	if (!validate_addr(p_buf, &p_buf, &length, mpdu->mhr.fs->fc.dst_addr_mode, false,
			   &mpdu->mhr.dst_addr) ||
	    !validate_addr(p_buf, &p_buf, &length, mpdu->mhr.fs->fc.src_addr_mode,
			   (mpdu->mhr.fs->fc.pan_id_comp), &mpdu->mhr.src_addr)) {
		return false;
	}

#ifdef CONFIG_NET_L2_IEEE802154_SECURITY
	if (mpdu->mhr.fs->fc.security_enabled) {
		mpdu->mhr.aux_sec = ieee802154_validate_aux_security_hdr(p_buf, &p_buf, &length);
		if (!mpdu->mhr.aux_sec) {
			return false;
		}
	}
#endif

	return validate_payload_and_mfr(mpdu, buf, p_buf, &length);
}

uint8_t ieee802154_compute_header_and_authtag_size(struct net_if *iface, struct net_linkaddr *dst,
						   struct net_linkaddr *src)
{
	bool broadcast = !dst->addr;
	uint8_t hdr_len = sizeof(struct ieee802154_fcf_seq);

	/* PAN ID */
	hdr_len += IEEE802154_PAN_ID_LENGTH;

	/* Destination Address - see get_dst_addr_mode() */
	hdr_len += broadcast ? IEEE802154_SHORT_ADDR_LENGTH : dst->len;

	/* Source Address - see data_addr_to_fs_settings() */
	hdr_len += src->addr ? src->len : dst->len;

#ifdef CONFIG_NET_L2_IEEE802154_SECURITY
	if (broadcast) {
		NET_DBG("Broadcast packets are not being encrypted.");
		goto done;
	}

	struct ieee802154_context *ctx = (struct ieee802154_context *)net_if_l2_data(iface);

	k_sem_take(&ctx->ctx_lock, K_FOREVER);

	struct ieee802154_security_ctx *sec_ctx =
		&ctx->sec_ctx;
	if (sec_ctx->level == IEEE802154_SECURITY_LEVEL_NONE) {
		goto release;
	}

	/* Compute aux-sec hdr size and add it to hdr_len */
	hdr_len += IEEE802154_SECURITY_CF_LENGTH + IEEE802154_SECURITY_FRAME_COUNTER_LENGTH;

	switch (sec_ctx->key_mode) {
	case IEEE802154_KEY_ID_MODE_IMPLICIT:
		/* The only mode supported for now,
		 * generate_aux_securiy_hdr() will fail on other modes
		 */
		break;
	case IEEE802154_KEY_ID_MODE_INDEX:
		hdr_len += IEEE8021254_KEY_ID_FIELD_INDEX_LENGTH;
		break;
	case IEEE802154_KEY_ID_MODE_SRC_4_INDEX:
		hdr_len += IEEE8021254_KEY_ID_FIELD_SRC_4_INDEX_LENGTH;
		break;
	case IEEE802154_KEY_ID_MODE_SRC_8_INDEX:
		hdr_len += IEEE8021254_KEY_ID_FIELD_SRC_8_INDEX_LENGTH;
	}

	/* This is a _HACK_: As net_buf does not allow to reserve tailroom
	 * - here for authentication tag (see section 7.6.3.4.3) - it "reserves"
	 * it in headroom so the payload won't occupy all the left space
	 * and then when it will come to finalize the data frame it will
	 * reduce the reserved space by the tag size, move the payload
	 * foward accordingly, and only then: run the encryption/authentication
	 * which will fill the tag space in the end.
	 */
	if (sec_ctx->level < IEEE802154_SECURITY_LEVEL_ENC) {
		hdr_len += level_2_tag_size[sec_ctx->level];
	} else {
		hdr_len += level_2_tag_size[sec_ctx->level - 4U];
	}

release:
	k_sem_give(&ctx->ctx_lock);
done:
#endif /* CONFIG_NET_L2_IEEE802154_SECURITY */

	NET_DBG("Computed size of %u", hdr_len);

	return hdr_len;
}

static inline struct ieee802154_fcf_seq *generate_fcf_grounds(uint8_t **p_buf, bool ack)
{
	struct ieee802154_fcf_seq *fs;

	fs = (struct ieee802154_fcf_seq *)*p_buf;

	fs->fc.security_enabled = 0U;
	fs->fc.frame_pending = 0U;
	fs->fc.ar = ack;
	fs->fc.pan_id_comp = 0U;
	fs->fc.reserved = 0U;
	/* We support version 2006 only for now */
	fs->fc.seq_num_suppr = 0U;
	fs->fc.ie_list = 0U;
	fs->fc.frame_version = IEEE802154_VERSION_802154_2006;

	*p_buf += sizeof(struct ieee802154_fcf_seq);

	return fs;
}

static inline enum ieee802154_addressing_mode get_dst_addr_mode(struct net_linkaddr *dst,
								bool *broadcast)
{
	if (!dst->addr) {
		NET_DBG("Broadcast destination");
		*broadcast = true;
		return IEEE802154_ADDR_MODE_SHORT;
	}

	if (dst->len == IEEE802154_SHORT_ADDR_LENGTH) {
		uint16_t short_addr = ntohs(*(uint16_t *)(dst->addr));
		*broadcast = (short_addr == IEEE802154_BROADCAST_ADDRESS);
		return IEEE802154_ADDR_MODE_SHORT;
	} else {
		*broadcast = false;
	}

	if (dst->len == IEEE802154_EXT_ADDR_LENGTH) {
		return IEEE802154_ADDR_MODE_EXTENDED;
	}

	return IEEE802154_ADDR_MODE_NONE;
}

static inline bool data_addr_to_fs_settings(struct net_linkaddr *dst, struct ieee802154_fcf_seq *fs,
					    struct ieee802154_frame_params *params)
{
	bool broadcast;

	fs->fc.dst_addr_mode = get_dst_addr_mode(dst, &broadcast);
	if (fs->fc.dst_addr_mode != IEEE802154_ADDR_MODE_NONE) {
		fs->fc.pan_id_comp = 1U;

		if (broadcast) {
			params->dst.short_addr = IEEE802154_BROADCAST_ADDRESS;
			params->dst.len = IEEE802154_SHORT_ADDR_LENGTH;
			fs->fc.ar = 0U;
		} else if (dst->len == IEEE802154_SHORT_ADDR_LENGTH) {
			params->dst.short_addr = ntohs(*(uint16_t *)(dst->addr));
			params->dst.len = IEEE802154_SHORT_ADDR_LENGTH;
		} else {
			__ASSERT_NO_MSG(dst->len == IEEE802154_EXT_ADDR_LENGTH);
			params->dst.ext_addr = dst->addr;
			params->dst.len = IEEE802154_EXT_ADDR_LENGTH;
		}
	}

	if (params->short_addr) {
		fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_SHORT;
	} else {
		fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
	}

	return broadcast;
}

static uint8_t *generate_addressing_fields(struct ieee802154_context *ctx,
					   struct ieee802154_fcf_seq *fs,
					   struct ieee802154_frame_params *params, uint8_t *p_buf)
{
	struct ieee802154_address_field *af;

	/* destination address */
	if (fs->fc.dst_addr_mode != IEEE802154_ADDR_MODE_NONE) {
		af = (struct ieee802154_address_field *)p_buf;

		af->plain.pan_id = params->dst.pan_id;
		p_buf += IEEE802154_PAN_ID_LENGTH;

		if (fs->fc.dst_addr_mode == IEEE802154_ADDR_MODE_SHORT) {
			af->plain.addr.short_addr = sys_cpu_to_le16(params->dst.short_addr);
			p_buf += IEEE802154_SHORT_ADDR_LENGTH;
		} else {
			sys_memcpy_swap(af->plain.addr.ext_addr, params->dst.ext_addr,
					IEEE802154_EXT_ADDR_LENGTH);
			p_buf += IEEE802154_EXT_ADDR_LENGTH;
		}
	}

	/* source address */
	if (fs->fc.src_addr_mode == IEEE802154_ADDR_MODE_NONE) {
		return p_buf;
	}

	af = (struct ieee802154_address_field *)p_buf;
	struct ieee802154_address *src_addr;

	if (fs->fc.pan_id_comp) {
		src_addr = &af->comp.addr;
	} else {
		af->plain.pan_id = params->pan_id;
		src_addr = &af->plain.addr;
		p_buf += IEEE802154_PAN_ID_LENGTH;
	}

	if (fs->fc.src_addr_mode == IEEE802154_ADDR_MODE_SHORT) {
		src_addr->short_addr = sys_cpu_to_le16(params->short_addr);
		p_buf += IEEE802154_SHORT_ADDR_LENGTH;
	} else {
		memcpy(src_addr->ext_addr, ctx->ext_addr, IEEE802154_EXT_ADDR_LENGTH);
		p_buf += IEEE802154_EXT_ADDR_LENGTH;
	}

	return p_buf;
}

#ifdef CONFIG_NET_L2_IEEE802154_SECURITY
static uint8_t *generate_aux_security_hdr(struct ieee802154_security_ctx *sec_ctx, uint8_t *p_buf)
{
	struct ieee802154_aux_security_hdr *aux_sec;

	if (sec_ctx->level == IEEE802154_SECURITY_LEVEL_NONE) {
		return p_buf;
	}

	if (sec_ctx->key_mode != IEEE802154_KEY_ID_MODE_IMPLICIT) {
		/* TODO: it supports implicit mode only, for now */
		return NULL;
	}

	aux_sec = (struct ieee802154_aux_security_hdr *)p_buf;

	aux_sec->control.security_level = sec_ctx->level;
	aux_sec->control.key_id_mode = sec_ctx->key_mode;
	aux_sec->control.reserved = 0U;

	aux_sec->frame_counter = sys_cpu_to_le32(sec_ctx->frame_counter);

	return p_buf + IEEE802154_SECURITY_CF_LENGTH + IEEE802154_SECURITY_FRAME_COUNTER_LENGTH;
}
#endif /* CONFIG_NET_L2_IEEE802154_SECURITY */

bool ieee802154_create_data_frame(struct ieee802154_context *ctx, struct net_linkaddr *dst,
				  struct net_linkaddr *src, struct net_buf *buf, uint8_t hdr_len)
{
	struct ieee802154_frame_params params = {0};
	struct ieee802154_fcf_seq *fs;
	uint8_t *p_buf = buf->data;
	uint8_t *buf_start = p_buf;
	bool ret = false;
	bool broadcast;

	k_sem_take(&ctx->ctx_lock, K_FOREVER);

	fs = generate_fcf_grounds(&p_buf, ctx->ack_requested);

	fs->fc.frame_type = IEEE802154_FRAME_TYPE_DATA;
	fs->sequence = ctx->sequence++;

	params.dst.pan_id = ctx->pan_id;
	params.pan_id = ctx->pan_id;
	if (src->addr && src->len == IEEE802154_SHORT_ADDR_LENGTH) {
		params.short_addr = ntohs(*(uint16_t *)(src->addr));
		if (ctx->short_addr != params.short_addr) {
			goto out;
		}
	} else {
		if (src->len != IEEE802154_EXT_ADDR_LENGTH) {
			goto out;
		}

		uint8_t ext_addr_le[IEEE802154_EXT_ADDR_LENGTH];

		sys_memcpy_swap(ext_addr_le, src->addr, IEEE802154_EXT_ADDR_LENGTH);
		if (memcmp(ctx->ext_addr, ext_addr_le, src->len)) {
			goto out;
		}
	}

	broadcast = data_addr_to_fs_settings(dst, fs, &params);

	p_buf = generate_addressing_fields(ctx, fs, &params, p_buf);

#ifdef CONFIG_NET_L2_IEEE802154_SECURITY
	if (broadcast) {
		/* TODO: This may not always be correct. */
		NET_DBG("No security hdr needed: broadcasting");
		goto no_security_hdr;
	}

	if (ctx->sec_ctx.level == IEEE802154_SECURITY_LEVEL_NONE) {
		NET_WARN("IEEE 802.15.4 security is enabled but has not been configured.");
		goto no_security_hdr;
	}

	fs->fc.security_enabled = 1U;

	p_buf = generate_aux_security_hdr(&ctx->sec_ctx, p_buf);
	if (!p_buf) {
		NET_ERR("Unsupported key mode.");
		goto out;
	}

	uint8_t payload_len = buf->len - hdr_len;
	uint8_t level = ctx->sec_ctx.level;

	if (level >= IEEE802154_SECURITY_LEVEL_ENC) {
		level -= 4U;
	}

	uint8_t tag_size = level_2_tag_size[level];

	if (tag_size > 0) {
		/* If tagged, let's create tailroom for the tag by moving the payload left,
		 *see comment in ieee802154_compute_header_and_authtag_size().
		 */
		memmove(p_buf, buf_start + hdr_len, payload_len);
		hdr_len -= tag_size;
	}

	/* Let's encrypt/auth only in the end, if needed */
	if (!ieee802154_encrypt_auth(&ctx->sec_ctx, buf_start, hdr_len,
				     payload_len, tag_size, ctx->ext_addr)) {
		goto out;
	};

no_security_hdr:
#endif /* CONFIG_NET_L2_IEEE802154_SECURITY */
	if ((p_buf - buf_start) != hdr_len) {
		/* hdr_len was too small? We probably overwrote payload bytes */
		NET_ERR("Could not generate data frame %zu vs %u", (p_buf - buf_start), hdr_len);
		goto out;
	}

	dbg_print_fs(fs);

	ret = true;

out:
	k_sem_give(&ctx->ctx_lock);
	return ret;
}

#ifdef CONFIG_NET_L2_IEEE802154_RFD

static inline bool cfi_to_fs_settings(enum ieee802154_cfi cfi, struct ieee802154_fcf_seq *fs,
				      struct ieee802154_frame_params *params)
{
	switch (cfi) {
	case IEEE802154_CFI_DISASSOCIATION_NOTIFICATION:
		fs->fc.ar = 1U;
		fs->fc.pan_id_comp = 1U;

		__fallthrough;
	case IEEE802154_CFI_ASSOCIATION_REQUEST:
		fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;

		if (params->dst.len == IEEE802154_SHORT_ADDR_LENGTH) {
			fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_SHORT;
		} else {
			fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
		}

		break;
	case IEEE802154_CFI_ASSOCIATION_RESPONSE:
	case IEEE802154_CFI_PAN_ID_CONLICT_NOTIFICATION:
		fs->fc.ar = 1U;
		fs->fc.pan_id_comp = 1U;
		fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
		fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;

		break;
	case IEEE802154_CFI_DATA_REQUEST:
		fs->fc.ar = 1U;
		/* TODO: src/dst addr mode: see section 7.3.4 */

		break;
	case IEEE802154_CFI_ORPHAN_NOTIFICATION:
		fs->fc.pan_id_comp = 1U;
		fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
		fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_SHORT;

		break;
	case IEEE802154_CFI_BEACON_REQUEST:
		fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_NONE;
		fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_SHORT;
		break;
	case IEEE802154_CFI_COORDINATOR_REALIGNEMENT:
		fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
		/* TODO: ar and dst addr mode: see section 7.3.8 */

		break;
	case IEEE802154_CFI_GTS_REQUEST:
		fs->fc.ar = 1U;
		fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_SHORT;
		fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_NONE;

		break;
	default:
		return false;
	}

	return true;
}

static inline uint8_t mac_command_length(enum ieee802154_cfi cfi)
{
	uint8_t length = 1U; /* cfi is at least present */

	switch (cfi) {
	case IEEE802154_CFI_ASSOCIATION_REQUEST:
	case IEEE802154_CFI_DISASSOCIATION_NOTIFICATION:
	case IEEE802154_CFI_GTS_REQUEST:
		length += 1U;
		break;
	case IEEE802154_CFI_ASSOCIATION_RESPONSE:
		length += 3U;
		break;
	case IEEE802154_CFI_COORDINATOR_REALIGNEMENT:
		length += 8U;
		break;
	default:
		break;
	}

	return length;
}

struct net_pkt *ieee802154_create_mac_cmd_frame(struct net_if *iface, enum ieee802154_cfi type,
						struct ieee802154_frame_params *params)
{
	struct ieee802154_context *ctx = net_if_l2_data(iface);
	struct ieee802154_fcf_seq *fs;
	struct net_pkt *pkt = NULL;
	uint8_t *p_buf, *p_start;

	k_sem_take(&ctx->ctx_lock, K_FOREVER);

	/* It would be costly to compute the size when actual frame are never
	 * bigger than 125 bytes, so let's allocate that size as buffer.
	 */
	pkt = net_pkt_alloc_with_buffer(iface, IEEE802154_MTU, AF_UNSPEC, 0, BUF_TIMEOUT);
	if (!pkt) {
		goto out;
	}

	p_buf = net_pkt_data(pkt);
	p_start = p_buf;

	fs = generate_fcf_grounds(
		&p_buf, type == IEEE802154_CFI_BEACON_REQUEST ? false : ctx->ack_requested);

	fs->fc.frame_type = IEEE802154_FRAME_TYPE_MAC_COMMAND;
	fs->sequence = ctx->sequence;

	if (!cfi_to_fs_settings(type, fs, params)) {
		goto error;
	}

	p_buf = generate_addressing_fields(ctx, fs, params, p_buf);

	net_buf_add(pkt->buffer, p_buf - p_start);

	/* Let's insert the cfi */
	((struct ieee802154_command *)p_buf)->cfi = type;

	dbg_print_fs(fs);

	goto out;

error:
	net_pkt_unref(pkt);
	pkt = NULL;

out:
	k_sem_give(&ctx->ctx_lock);
	return pkt;
}

void ieee802154_mac_cmd_finalize(struct net_pkt *pkt, enum ieee802154_cfi type)
{
	net_buf_add(pkt->buffer, mac_command_length(type));
}

#endif /* CONFIG_NET_L2_IEEE802154_RFD */

#ifdef CONFIG_NET_L2_IEEE802154_ACK_REPLY
bool ieee802154_create_ack_frame(struct net_if *iface, struct net_pkt *pkt, uint8_t seq)
{
	uint8_t *p_buf = net_pkt_data(pkt);
	struct ieee802154_fcf_seq *fs;

	if (!p_buf) {
		return false;
	}

	fs = generate_fcf_grounds(&p_buf, false);

	fs->fc.dst_addr_mode = 0U;
	fs->fc.src_addr_mode = 0U;

	fs->fc.frame_type = IEEE802154_FRAME_TYPE_ACK;
	fs->sequence = seq;

	net_buf_add(pkt->buffer, IEEE802154_ACK_PKT_LENGTH);

	return true;
}
#endif /* CONFIG_NET_L2_IEEE802154_ACK_REPLY */

#ifdef CONFIG_NET_L2_IEEE802154_SECURITY
bool ieee802154_decipher_data_frame(struct net_if *iface, struct net_pkt *pkt,
				    struct ieee802154_mpdu *mpdu)
{
	struct ieee802154_context *ctx = net_if_l2_data(iface);
	bool ret = false;

	k_sem_take(&ctx->ctx_lock, K_FOREVER);

	uint8_t level = ctx->sec_ctx.level;

	if (!mpdu->mhr.fs->fc.security_enabled) {
		ret = true;
		goto out;
	}

	/* Section 7.2.3 (i) talks about "security level policy" conformance
	 * but such policy does not seem to be detailed. So let's assume both
	 * ends should have same security level.
	 */
	if (mpdu->mhr.aux_sec->control.security_level != level) {
		goto out;
	}

	if (level >= IEEE802154_SECURITY_LEVEL_ENC) {
		level -= 4U;
	}

	uint8_t tag_size = level_2_tag_size[level];
	uint8_t hdr_len = (uint8_t *)mpdu->payload - net_pkt_data(pkt);
	uint8_t payload_len = net_pkt_get_len(pkt) - hdr_len - tag_size;
	uint8_t ext_addr_le[IEEE802154_EXT_ADDR_LENGTH];

	/* TODO: Handle src short address.
	 * This will require to look up in nbr cache with short addr
	 * in order to get the extended address related to it.
	 */
	if (net_pkt_lladdr_src(pkt)->len != IEEE802154_EXT_ADDR_LENGTH) {
		NET_ERR("Decrypting packages with short source addresses is not supported.");
		goto out;
	}

	sys_memcpy_swap(ext_addr_le, net_pkt_lladdr_src(pkt)->addr, net_pkt_lladdr_src(pkt)->len);
	if (!ieee802154_decrypt_auth(&ctx->sec_ctx, net_pkt_data(pkt), hdr_len, payload_len,
				     tag_size, ext_addr_le,
				     sys_le32_to_cpu(mpdu->mhr.aux_sec->frame_counter))) {
		NET_ERR("Could not decipher the frame");
		goto out;
	}

	/* We remove tag size from buf's length, it is now useless. */
	pkt->buffer->len -= tag_size;

	ret = true;

out:
	k_sem_give(&ctx->ctx_lock);
	return ret;
}
#endif /* CONFIG_NET_L2_IEEE802154_SECURITY */