mac: implement Provisioned STS

Bug: 231581242
Signed-off-by: Jeremy Rocher <jeremy.rocher@qorvo.com>
Change-Id: I69b6108faeeca796e8cf3db5ec06dd26d4b99a84

31 files changed, 2901 insertions, 1026 deletions

diff --git a/kernel/net/mcps802154/Kbuild b/kernel/net/mcps802154/Kbuild
index f032cd7..1fd78c3 100644
--- a/kernel/net/mcps802154/Kbuild
+++ b/kernel/net/mcps802154/Kbuild
@@ -36,8 +36,6 @@ mcps802154-y := \
 
 mcps802154_region_fira-y := \
 	fira_access.o \
-	fira_aead.o \
-	fira_cmac.o \
 	fira_crypto.o \
 	fira_round_hopping_sequence.o \
 	fira_round_hopping_crypto.o \
@@ -49,7 +47,9 @@ mcps802154_region_fira-y := \
 	fira_session_fsm_init.o \
 	fira_session_fsm_idle.o \
 	fira_session_fsm_active.o \
-	fira_trace.o
+	fira_sts.o \
+	fira_trace.o \
+	mcps_crypto.o
 
 mcps802154_region_nfcc_coex-y := \
 	nfcc_coex_access.o \
diff --git a/kernel/net/mcps802154/fira_aead.c b/kernel/net/mcps802154/fira_aead.c
deleted file mode 100644
index 88669e8..0000000
--- a/kernel/net/mcps802154/fira_aead.c
+++ /dev/null
@@ -1,185 +0,0 @@
-/*
- * This file is part of the UWB stack for linux.
- *
- * Copyright (c) 2020-2021 Qorvo US, Inc.
- *
- * This software is provided under the GNU General Public License, version 2
- * (GPLv2), as well as under a Qorvo commercial license.
- *
- * You may choose to use this software under the terms of the GPLv2 License,
- * version 2 ("GPLv2"), as published by the Free Software Foundation.
- * You should have received a copy of the GPLv2 along with this program.  If
- * not, see <http://www.gnu.org/licenses/>.
- *
- * This program is distributed under the GPLv2 in the hope that it will be
- * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GPLv2 for more
- * details.
- *
- * If you cannot meet the requirements of the GPLv2, you may not use this
- * software for any purpose without first obtaining a commercial license from
- * Qorvo. Please contact Qorvo to inquire about licensing terms.
- */
-
-#include "fira_aead_impl.h"
-
-#include <asm/unaligned.h>
-#include <crypto/aes.h>
-#include <linux/errno.h>
-#include <linux/ieee802154.h>
-#include <linux/printk.h>
-#include <linux/string.h>
-#include <net/mcps802154_frame.h>
-
-#define FIRA_AEAD_AUTHSIZE 8
-
-int fira_aead_set_key(struct fira_aead *aead, const u8 *key)
-{
-	struct crypto_aead *tfm;
-	int r;
-
-	crypto_free_aead(aead->tfm);
-	aead->tfm = NULL;
-
-	tfm = crypto_alloc_aead("ccm(aes)", 0, CRYPTO_ALG_ASYNC);
-	if (IS_ERR(tfm)) {
-		if (PTR_ERR(tfm) == -ENOENT) {
-			pr_err("The crypto transform ccm(aes) seems to be missing."
-			       " Please check your kernel configuration.\n");
-		}
-		return PTR_ERR(tfm);
-	}
-
-	r = crypto_aead_setkey(tfm, key, AES_KEYSIZE_128);
-	if (r)
-		goto err_free_tfm;
-
-	r = crypto_aead_setauthsize(tfm, FIRA_AEAD_AUTHSIZE);
-	if (r)
-		goto err_free_tfm;
-
-	aead->tfm = tfm;
-
-	return 0;
-
-err_free_tfm:
-	crypto_free_aead(tfm);
-	return r;
-}
-
-static void fira_aead_fill_iv(u8 *iv, __le16 src_short_addr, u32 counter)
-{
-	u8 *ivp;
-
-	ivp = iv;
-	*ivp++ = sizeof(u16) - 1; /* Only set L', rest is filled by CCM. */
-	memset(ivp, 0,
-	       IEEE802154_EXTENDED_ADDR_LEN - IEEE802154_SHORT_ADDR_LEN);
-	ivp += IEEE802154_EXTENDED_ADDR_LEN - IEEE802154_SHORT_ADDR_LEN;
-	put_unaligned_be16(le16_to_cpu(src_short_addr), ivp);
-	ivp += IEEE802154_SHORT_ADDR_LEN;
-	put_unaligned_be32(counter, ivp);
-	ivp += sizeof(u32);
-	*ivp++ = IEEE802154_SCF_SECLEVEL_ENC_MIC64;
-	/* Filled by CCM. */
-	*ivp++ = 0;
-	*ivp++ = 0;
-}
-
-int fira_aead_encrypt(struct fira_aead *aead, struct sk_buff *skb,
-		      unsigned int header_len, __le16 src_short_addr,
-		      u32 counter)
-{
-	u8 iv[AES_BLOCK_SIZE];
-	struct scatterlist sg;
-	struct aead_request *req;
-	int r;
-
-	if (skb_tailroom(skb) < FIRA_AEAD_AUTHSIZE)
-		return -ENOBUFS;
-
-	fira_aead_fill_iv(iv, src_short_addr, counter);
-
-	req = aead_request_alloc(aead->tfm, GFP_ATOMIC);
-	if (!req)
-		return -ENOMEM;
-
-	skb->data[IEEE802154_FC_LEN + IEEE802154_SHORT_ADDR_LEN] =
-		IEEE802154_SCF_SECLEVEL_ENC_MIC64 |
-		IEEE802154_SCF_NO_FRAME_COUNTER;
-
-	sg_init_one(&sg, skb->data, skb->len + FIRA_AEAD_AUTHSIZE);
-
-	aead_request_set_callback(req, 0, NULL, NULL);
-	aead_request_set_crypt(req, &sg, &sg, skb->len - header_len, iv);
-	aead_request_set_ad(req, header_len);
-
-	r = crypto_aead_encrypt(req);
-
-	aead_request_free(req);
-
-	if (!r)
-		skb_put(skb, FIRA_AEAD_AUTHSIZE);
-	else
-		skb->data[IEEE802154_FC_LEN + IEEE802154_SHORT_ADDR_LEN] =
-			IEEE802154_SCF_NO_FRAME_COUNTER;
-
-	return r;
-}
-
-bool fira_aead_decrypt_scf_check(u8 scf)
-{
-	return scf == (IEEE802154_SCF_SECLEVEL_ENC_MIC64 |
-		       IEEE802154_SCF_NO_FRAME_COUNTER);
-}
-
-int fira_aead_decrypt_prepare(struct sk_buff *skb)
-{
-	if (skb->len < FIRA_AEAD_AUTHSIZE)
-		return -EBADMSG;
-	skb_trim(skb, skb->len - FIRA_AEAD_AUTHSIZE);
-	return 0;
-}
-
-int fira_aead_decrypt(struct fira_aead *aead, struct sk_buff *skb,
-		      unsigned int header_len, __le16 src_short_addr,
-		      u32 counter)
-{
-	u8 iv[AES_BLOCK_SIZE];
-	struct scatterlist sg;
-	struct aead_request *req;
-	u8 *header;
-	int r, payload_auth_len;
-
-	payload_auth_len = skb->len + FIRA_AEAD_AUTHSIZE;
-
-	fira_aead_fill_iv(iv, src_short_addr, counter);
-
-	req = aead_request_alloc(aead->tfm, GFP_ATOMIC);
-	if (!req)
-		return -ENOMEM;
-
-	header = skb->data - header_len;
-	sg_init_one(&sg, header, header_len + payload_auth_len);
-
-	aead_request_set_callback(req, 0, NULL, NULL);
-	aead_request_set_crypt(req, &sg, &sg, payload_auth_len, iv);
-	aead_request_set_ad(req, header_len);
-
-	r = crypto_aead_decrypt(req);
-
-	aead_request_free(req);
-
-	if (!r) {
-		header[IEEE802154_FC_LEN + IEEE802154_SHORT_ADDR_LEN] =
-			IEEE802154_SCF_NO_FRAME_COUNTER;
-	}
-
-	return r;
-}
-
-void fira_aead_destroy(struct fira_aead *aead)
-{
-	crypto_free_aead(aead->tfm);
-	aead->tfm = NULL;
-}
diff --git a/kernel/net/mcps802154/fira_aead.h b/kernel/net/mcps802154/fira_aead.h
deleted file mode 120000
index 747486d..0000000
--- a/kernel/net/mcps802154/fira_aead.h
+++ /dev/null
@@ -1 +0,0 @@
-../../../mac/fira_aead.h
\ No newline at end of file
diff --git a/kernel/net/mcps802154/fira_aead_impl.h b/kernel/net/mcps802154/fira_aead_impl.h
deleted file mode 100644
index 8a564de..0000000
--- a/kernel/net/mcps802154/fira_aead_impl.h
+++ /dev/null
@@ -1,41 +0,0 @@
-/*
- * This file is part of the UWB stack for linux.
- *
- * Copyright (c) 2020-2021 Qorvo US, Inc.
- *
- * This software is provided under the GNU General Public License, version 2
- * (GPLv2), as well as under a Qorvo commercial license.
- *
- * You may choose to use this software under the terms of the GPLv2 License,
- * version 2 ("GPLv2"), as published by the Free Software Foundation.
- * You should have received a copy of the GPLv2 along with this program.  If
- * not, see <http://www.gnu.org/licenses/>.
- *
- * This program is distributed under the GPLv2 in the hope that it will be
- * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GPLv2 for more
- * details.
- *
- * If you cannot meet the requirements of the GPLv2, you may not use this
- * software for any purpose without first obtaining a commercial license from
- * Qorvo. Please contact Qorvo to inquire about licensing terms.
- */
-
-#ifndef FIRA_AEAD_IMPL_H
-#define FIRA_AEAD_IMPL_H
-
-#include "fira_aead.h"
-
-#include <crypto/aead.h>
-
-/**
- * struct fira_aead - Context for payload encryption/decryption.
- */
-struct fira_aead {
-	/**
-	 * @tfm: Transformation context for payload.
-	 */
-	struct crypto_aead *tfm;
-};
-
-#endif /* FIRA_AEAD_IMPL_H */
diff --git a/kernel/net/mcps802154/fira_cmac.c b/kernel/net/mcps802154/fira_cmac.c
deleted file mode 100644
index b2e1437..0000000
--- a/kernel/net/mcps802154/fira_cmac.c
+++ /dev/null
@@ -1,89 +0,0 @@
-/*
- * This file is part of the UWB stack for linux.
- *
- * Copyright (c) 2020-2021 Qorvo US, Inc.
- *
- * This software is provided under the GNU General Public License, version 2
- * (GPLv2), as well as under a Qorvo commercial license.
- *
- * You may choose to use this software under the terms of the GPLv2 License,
- * version 2 ("GPLv2"), as published by the Free Software Foundation.
- * You should have received a copy of the GPLv2 along with this program.  If
- * not, see <http://www.gnu.org/licenses/>.
- *
- * This program is distributed under the GPLv2 in the hope that it will be
- * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GPLv2 for more
- * details.
- *
- * If you cannot meet the requirements of the GPLv2, you may not use this
- * software for any purpose without first obtaining a commercial license from
- * Qorvo. Please contact Qorvo to inquire about licensing terms.
- */
-
-#include "fira_cmac.h"
-
-#include <asm/unaligned.h>
-#include <linux/crypto.h>
-#include <crypto/hash.h>
-#include <linux/err.h>
-
-int fira_digest(const u8 *key, unsigned int key_len, const u8 *data,
-		unsigned int data_len, u8 *out)
-{
-	struct crypto_shash *tfm;
-	int r;
-
-	tfm = crypto_alloc_shash("cmac(aes)", 0, 0);
-	if (IS_ERR(tfm)) {
-		if (PTR_ERR(tfm) == -ENOENT) {
-			pr_err("The crypto transform cmac(aes) seems to be missing."
-			       " Please check your kernel configuration.\n");
-		}
-		return PTR_ERR(tfm);
-	}
-
-	r = crypto_shash_setkey(tfm, key, key_len);
-	if (r)
-		goto out;
-
-	do {
-		/* tfm need to be allocated for kernel < 4.20, so don't remove
-		 * this do..while block. */
-		SHASH_DESC_ON_STACK(desc, tfm);
-		desc->tfm = tfm;
-
-		r = crypto_shash_init(desc);
-		if (r)
-			goto out;
-
-		r = crypto_shash_finup(desc, data, data_len, out);
-	} while (0);
-
-out:
-	crypto_free_shash(tfm);
-	return r;
-}
-
-int fira_kdf(const u8 *input_key, unsigned int input_key_len, const char *label,
-	     const u8 *context, u8 *output_key, unsigned int output_key_len)
-{
-	u8 derivation_data[sizeof(u32) + FIRA_KDF_LABEL_LEN +
-			   FIRA_KDF_CONTEXT_LEN + sizeof(u32)];
-	u8 *p;
-
-	if (output_key_len != AES_KEYSIZE_128)
-		return -1;
-
-	p = derivation_data;
-	put_unaligned_be32(1, p);
-	p += sizeof(u32);
-	memcpy(p, label, FIRA_KDF_LABEL_LEN);
-	p += FIRA_KDF_LABEL_LEN;
-	memcpy(p, context, FIRA_KDF_CONTEXT_LEN);
-	p += FIRA_KDF_CONTEXT_LEN;
-	put_unaligned_be32(output_key_len * 8, p);
-
-	return fira_digest(input_key, input_key_len, derivation_data,
-			   sizeof(derivation_data), output_key);
-}
diff --git a/kernel/net/mcps802154/fira_cmac.h b/kernel/net/mcps802154/fira_cmac.h
deleted file mode 120000
index ec918e1..0000000
--- a/kernel/net/mcps802154/fira_cmac.h
+++ /dev/null
@@ -1 +0,0 @@
-../../../mac/fira_cmac.h
\ No newline at end of file
diff --git a/kernel/net/mcps802154/fira_sts.c b/kernel/net/mcps802154/fira_sts.c
new file mode 120000
index 0000000..84ac15f
--- /dev/null
+++ b/kernel/net/mcps802154/fira_sts.c
@@ -0,0 +1 @@
+../../../mac/fira_sts.c
\ No newline at end of file
diff --git a/kernel/net/mcps802154/fira_sts.h b/kernel/net/mcps802154/fira_sts.h
new file mode 120000
index 0000000..e93e356
--- /dev/null
+++ b/kernel/net/mcps802154/fira_sts.h
@@ -0,0 +1 @@
+../../../mac/fira_sts.h
\ No newline at end of file
diff --git a/kernel/net/mcps802154/mcps_crypto.c b/kernel/net/mcps802154/mcps_crypto.c
new file mode 100644
index 0000000..faef7a4
--- /dev/null
+++ b/kernel/net/mcps802154/mcps_crypto.c
@@ -0,0 +1,321 @@
+/*
+ * This file is part of the UWB stack for linux.
+ *
+ * Copyright (c) 2022 Qorvo US, Inc.
+ *
+ * This software is provided under the GNU General Public License, version 2
+ * (GPLv2), as well as under a Qorvo commercial license.
+ *
+ * You may choose to use this software under the terms of the GPLv2 License,
+ * version 2 ("GPLv2"), as published by the Free Software Foundation.
+ * You should have received a copy of the GPLv2 along with this program.  If
+ * not, see <http://www.gnu.org/licenses/>.
+ *
+ * This program is distributed under the GPLv2 in the hope that it will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GPLv2 for more
+ * details.
+ *
+ * If you cannot meet the requirements of the GPLv2, you may not use this
+ * software for any purpose without first obtaining a commercial license from
+ * Qorvo. Please contact Qorvo to inquire about licensing terms.
+ */
+
+#include <linux/crypto.h>
+#include <linux/scatterlist.h>
+#include <crypto/hash.h>
+#include <crypto/skcipher.h>
+#include <crypto/aead.h>
+#include <crypto/aes.h>
+
+#include "mcps_crypto.h"
+
+#if !(defined(CONFIG_CRYPTO_HASH2) && defined(CONFIG_CRYPTO_AEAD2))
+#error "required CONFIG_CRYPTO_HASH2 && CONFIG_CRYPTO_AEAD2"
+#endif
+
+#define FIRA_CRYPTO_AEAD_AUTHSIZE	8
+
+
+struct mcps_aes_ccm_star_128_ctx {
+	struct crypto_aead *tfm;
+};
+
+struct mcps_aes_ecb_128_ctx {
+	struct crypto_skcipher *tfm;
+	bool decrypt;
+};
+
+
+int mcps_crypto_cmac_aes_128_digest(const uint8_t *key, const uint8_t *data,
+		unsigned int data_len, uint8_t *out)
+{
+	struct crypto_shash *tfm;
+	int r;
+
+	tfm = crypto_alloc_shash("cmac(aes)", 0, 0);
+	if (IS_ERR(tfm)) {
+		if (PTR_ERR(tfm) == -ENOENT)
+			pr_err("The crypto transform cmac(aes) seems to be missing."
+			       " Please check your kernel configuration.\n");
+		return PTR_ERR(tfm);
+	}
+
+	r = crypto_shash_setkey(tfm, key, AES_KEYSIZE_128);
+	if (r != 0)
+		goto out;
+
+	do {
+		/* tfm need to be allocated for kernel < 4.20, so don't remove
+		 * this do..while block
+		 */
+		SHASH_DESC_ON_STACK(desc, tfm);
+
+		desc->tfm = tfm;
+
+		r = crypto_shash_init(desc);
+		if (r != 0)
+			goto out;
+
+		r = crypto_shash_finup(desc, data, data_len, out);
+	} while (0);
+
+out:
+	crypto_free_shash(tfm);
+
+	return r;
+}
+
+struct mcps_aes_ccm_star_128_ctx *mcps_crypto_aead_aes_ccm_star_128_create(void)
+{
+	struct mcps_aes_ccm_star_128_ctx *ctx;
+	int r;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		goto error;
+
+	ctx->tfm = crypto_alloc_aead("ccm(aes)", 0, 0);
+	if (IS_ERR(ctx->tfm)) {
+		if (PTR_ERR(ctx->tfm) == -ENOENT)
+			pr_err("The crypto transform ccm(aes) seems to be missing."
+			       " Please check your kernel configuration.\n");
+		goto error;
+	}
+
+	r = crypto_aead_setauthsize(ctx->tfm, FIRA_CRYPTO_AEAD_AUTHSIZE);
+	if (r != 0)
+		goto error;
+
+	return ctx;
+
+error:
+	mcps_crypto_aead_aes_ccm_star_128_destroy(ctx);
+
+	return NULL;
+}
+
+int mcps_crypto_aead_aes_ccm_star_128_set(struct mcps_aes_ccm_star_128_ctx *ctx,
+		const uint8_t *key)
+{
+	if (!ctx || !key)
+		return -EINVAL;
+
+	return crypto_aead_setkey(ctx->tfm, key, AES_KEYSIZE_128);
+}
+
+void mcps_crypto_aead_aes_ccm_star_128_destroy(struct mcps_aes_ccm_star_128_ctx *ctx)
+{
+	if (!ctx)
+		return;
+
+	crypto_free_aead(ctx->tfm);
+	kfree(ctx);
+}
+
+int mcps_crypto_aead_aes_ccm_star_128_encrypt(
+		struct mcps_aes_ccm_star_128_ctx *ctx, const uint8_t *nonce,
+		const uint8_t *header, unsigned int header_len,
+		uint8_t *data, unsigned int data_len,
+		uint8_t *mac, unsigned int mac_len)
+{
+	struct aead_request *req = NULL;
+	struct scatterlist sg[3];
+	u8 iv[AES_BLOCK_SIZE];
+	DECLARE_CRYPTO_WAIT(wait);
+	int r = -1;
+
+	if (!ctx || !nonce || !header || header_len <= 0 || !data ||
+			data_len <= 0 || !mac ||
+			mac_len != FIRA_CRYPTO_AEAD_AUTHSIZE) {
+		return -EINVAL;
+	}
+
+	req = aead_request_alloc(ctx->tfm, GFP_KERNEL);
+	if (!req) {
+		r = -ENOMEM;
+		goto end;
+	}
+
+	sg_init_table(sg, ARRAY_SIZE(sg));
+	sg_set_buf(&sg[0], header, header_len);
+	sg_set_buf(&sg[1], data, data_len);
+	sg_set_buf(&sg[2], mac, mac_len);
+
+	iv[0] = sizeof(u16) - 1;
+	memcpy(iv + 1, nonce, MCPS_CRYPTO_AES_CCM_STAR_NONCE_LEN);
+
+	aead_request_set_callback(req,
+			CRYPTO_TFM_REQ_MAY_BACKLOG | CRYPTO_TFM_REQ_MAY_SLEEP,
+			crypto_req_done, &wait);
+	aead_request_set_ad(req, header_len);
+	aead_request_set_crypt(req, sg, sg, data_len, iv);
+
+	r = crypto_wait_req(crypto_aead_encrypt(req), &wait);
+
+end:
+	aead_request_free(req);
+
+	return r;
+}
+
+int mcps_crypto_aead_aes_ccm_star_128_decrypt(
+		struct mcps_aes_ccm_star_128_ctx *ctx, const uint8_t *nonce,
+		const uint8_t *header, unsigned int header_len,
+		uint8_t *data, unsigned int data_len,
+		uint8_t *mac, unsigned int mac_len)
+{
+	struct aead_request *req = NULL;
+	struct scatterlist sg[3];
+	u8 iv[AES_BLOCK_SIZE];
+	DECLARE_CRYPTO_WAIT(wait);
+	int r = -1;
+
+	if (!ctx || !nonce || !header || header_len <= 0 || !data ||
+			data_len <= 0 || !mac ||
+			mac_len != FIRA_CRYPTO_AEAD_AUTHSIZE) {
+		return -EINVAL;
+	}
+
+	req = aead_request_alloc(ctx->tfm, GFP_KERNEL);
+	if (!req) {
+		r = -ENOMEM;
+		goto end;
+	}
+
+	iv[0] = sizeof(u16) - 1;
+	memcpy(iv + 1, nonce, MCPS_CRYPTO_AES_CCM_STAR_NONCE_LEN);
+
+	sg_init_table(sg, ARRAY_SIZE(sg));
+	sg_set_buf(&sg[0], header, header_len);
+	sg_set_buf(&sg[1], data, data_len);
+	sg_set_buf(&sg[2], mac, mac_len);
+
+	aead_request_set_callback(req,
+			CRYPTO_TFM_REQ_MAY_BACKLOG | CRYPTO_TFM_REQ_MAY_SLEEP,
+			crypto_req_done, &wait);
+	aead_request_set_ad(req, header_len);
+	aead_request_set_crypt(req, sg, sg, data_len + mac_len, iv);
+
+	r = crypto_wait_req(crypto_aead_decrypt(req), &wait);
+
+end:
+	aead_request_free(req);
+
+	return r;
+}
+
+struct mcps_aes_ecb_128_ctx *mcps_crypto_aes_ecb_128_create(void)
+{
+	struct mcps_aes_ecb_128_ctx *ctx;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		goto error;
+
+	ctx->tfm = crypto_alloc_skcipher("ecb(aes)", 0, 0);
+	if (IS_ERR(ctx->tfm)) {
+		if (PTR_ERR(ctx->tfm) == -ENOENT)
+			pr_err("The crypto transform ecb(aes) seems to be missing."
+			       " Please check your kernel configuration.\n");
+		goto error;
+	}
+
+	return ctx;
+
+error:
+	mcps_crypto_aes_ecb_128_destroy(ctx);
+
+	return NULL;
+}
+
+int mcps_crypto_aes_ecb_128_set_encrypt(struct mcps_aes_ecb_128_ctx *ctx,
+		const uint8_t *key)
+{
+	if (!ctx || !key)
+		return -EINVAL;
+
+	ctx->decrypt = false;
+
+	return crypto_skcipher_setkey(ctx->tfm, key, AES_KEYSIZE_128);
+}
+
+int mcps_crypto_aes_ecb_128_set_decrypt(struct mcps_aes_ecb_128_ctx *ctx,
+		const uint8_t *key)
+{
+	if (!ctx || !key)
+		return -EINVAL;
+
+	ctx->decrypt = true;
+
+	return crypto_skcipher_setkey(ctx->tfm, key, AES_KEYSIZE_128);
+}
+
+void mcps_crypto_aes_ecb_128_destroy(struct mcps_aes_ecb_128_ctx *ctx)
+{
+	if (!ctx)
+		return;
+
+	crypto_free_skcipher(ctx->tfm);
+	kfree(ctx);
+}
+
+int mcps_crypto_aes_ecb_128_encrypt(struct mcps_aes_ecb_128_ctx *ctx,
+		const uint8_t *data, unsigned int data_len, uint8_t *out)
+{
+	struct skcipher_request *req = NULL;
+	struct scatterlist sgin, sgout;
+	DECLARE_CRYPTO_WAIT(wait);
+	int r = -1;
+
+	if (!ctx || !data || data_len <= 0 || !out)
+		return -EINVAL;
+
+	/* round to full cipher block */
+	data_len = ((data_len - 1) & -AES_KEYSIZE_128) + AES_KEYSIZE_128;
+
+	req = skcipher_request_alloc(ctx->tfm, GFP_KERNEL);
+	if (!req) {
+		r = -ENOMEM;
+		goto end;
+	}
+
+	sg_init_one(&sgin, data, data_len);
+	sg_init_one(&sgout, out, data_len);
+	skcipher_request_set_callback(req,
+			CRYPTO_TFM_REQ_MAY_BACKLOG | CRYPTO_TFM_REQ_MAY_SLEEP,
+			crypto_req_done, &wait);
+	skcipher_request_set_crypt(req, &sgin, &sgout, data_len, NULL);
+
+	if (ctx->decrypt)
+		r = crypto_skcipher_decrypt(req);
+	else
+		r = crypto_skcipher_encrypt(req);
+	r = crypto_wait_req(r, &wait);
+
+end:
+	skcipher_request_free(req);
+
+	return r;
+}
+
diff --git a/kernel/net/mcps802154/mcps_crypto.h b/kernel/net/mcps802154/mcps_crypto.h
new file mode 100644
index 0000000..5a1a5c5
--- /dev/null
+++ b/kernel/net/mcps802154/mcps_crypto.h
@@ -0,0 +1,197 @@
+/*
+ * This file is part of the UWB stack for linux.
+ *
+ * Copyright (c) 2020-2022 Qorvo US, Inc.
+ *
+ * This software is provided under the GNU General Public License, version 2
+ * (GPLv2), as well as under a Qorvo commercial license.
+ *
+ * You may choose to use this software under the terms of the GPLv2 License,
+ * version 2 ("GPLv2"), as published by the Free Software Foundation.
+ * You should have received a copy of the GPLv2 along with this program.  If
+ * not, see <http://www.gnu.org/licenses/>.
+ *
+ * This program is distributed under the GPLv2 in the hope that it will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GPLv2 for more
+ * details.
+ *
+ * If you cannot meet the requirements of the GPLv2, you may not use this
+ * software for any purpose without first obtaining a commercial license from
+ * Qorvo. Please contact Qorvo to inquire about licensing terms.
+ */
+
+#ifndef MCPS_CRYPTO_H
+#define MCPS_CRYPTO_H
+
+#ifdef __KERNEL__
+#include <linux/types.h>
+#else
+#include <stdint.h>
+#endif
+
+#define MCPS_CRYPTO_AES_CCM_STAR_NONCE_LEN 13
+
+/**
+ * struct mcps_aes_ccm_star_128_ctx - Context containing AES-128-CCM* related
+ * information.
+ *
+ * This is an opaque structure left to the implementation.
+ */
+struct mcps_aes_ccm_star_128_ctx;
+
+/**
+ * struct mcps_aes_ecb_128_ctx - Context containing AES-128-ECB related
+ * information.
+ *
+ * This is an opaque structure left to the implementation.
+ */
+struct mcps_aes_ecb_128_ctx;
+
+
+/**
+ * mcps_crypto_cmac_aes_128_digest() - Compute a cmac AES 128.
+ * @key: AES key.
+ * @data: Input data.
+ * @data_len: Input data length in bytes.
+ * @out: Output hash, with length AES_BLOCK_SIZE.
+ *
+ * NOTE: This API should be implemented by platform.
+ *
+ * Return: 0 or error.
+ */
+int mcps_crypto_cmac_aes_128_digest(const uint8_t *key, const uint8_t *data,
+		unsigned int data_len, uint8_t *out);
+
+/**
+ * mcps_crypto_aead_aes_ccm_star_128_create() - Create a context using
+ * Authenticated Encryption Associated Data with AES CCM* 128.
+ *
+ * NOTE: This API should be implemented by platform.
+ *
+ * Return: The pointer to the context that will be used to encrypt & decrypt.
+ */
+struct mcps_aes_ccm_star_128_ctx *mcps_crypto_aead_aes_ccm_star_128_create(void);
+
+/**
+ * mcps_crypto_aead_aes_ccm_star_128_set() - Set a context using
+ * Authenticated Encryption Associated Data with AES CCM* 128.
+ * @ctx: Context.
+ * @key: AES key.
+ *
+ * NOTE: This API should be implemented by platform.
+ *
+ * Return: 0 or error.
+ */
+int mcps_crypto_aead_aes_ccm_star_128_set(struct mcps_aes_ccm_star_128_ctx *ctx, const uint8_t *key);
+
+/**
+ * mcps_crypto_aead_aes_ccm_star_128_destroy() - Destroy the Authenticated
+ * Encryption Associated Data with AES CCM* 128 context.
+ * @ctx: Context.
+ *
+ * NOTE: This API should be implemented by platform.
+ */
+void mcps_crypto_aead_aes_ccm_star_128_destroy(struct mcps_aes_ccm_star_128_ctx *ctx);
+
+/**
+ * mcps_crypto_aead_aes_ccm_star_128_encrypt() - Encrypt using Authenticated
+ * Encryption Associated Data with AES CCM* 128.
+ * @ctx: Context.
+ * @nonce: Nonce, with length MCPS_CRYPTO_AES_CCM_STAR_NONCE_LEN.
+ * @header: Header data.
+ * @header_len: Header length in bytes.
+ * @data: Data to encrypt, will be replaced with encrypted data.
+ * @data_len: Data length in bytes.
+ * @mac: AES CCM* MAC.
+ * @mac_len: AES CCM* MAC size in bytes.
+ *
+ * NOTE: This API should be implemented by platform.
+ *
+ * Return: 0 or error.
+ */
+int mcps_crypto_aead_aes_ccm_star_128_encrypt(
+		struct mcps_aes_ccm_star_128_ctx *ctx, const uint8_t *nonce,
+		const uint8_t *header, unsigned int header_len,
+		uint8_t *data, unsigned int data_len,
+		uint8_t *mac, unsigned int mac_len);
+
+/**
+ * mcps_crypto_aead_aes_ccm_star_128_decrypt() - Decrypt using Authenticated
+ * Encryption Associated Data with AES CCM* 128.
+ * @ctx: Context.
+ * @nonce: Nonce, with length MCPS_CRYPTO_AES_CCM_STAR_NONCE_LEN.
+ * @header: Header data.
+ * @header_len: Header length in bytes.
+ * @data: Data to decrypt, will be replaced with decrypted data.
+ * @data_len: Data length in bytes.
+ * @mac: AES CCM* MAC.
+ * @mac_len: AES CCM* MAC size in bytes.
+ *
+ * NOTE: This API should be implemented by platform. In case of mismatch
+ * between the MAC and calculated MAC, this function should return -EBADMSG.
+ *
+ * Return: 0 or error.
+ */
+int mcps_crypto_aead_aes_ccm_star_128_decrypt(
+		struct mcps_aes_ccm_star_128_ctx *ctx, const uint8_t *nonce,
+		const uint8_t *header, unsigned int header_len,
+		uint8_t *data, unsigned int data_len,
+		uint8_t *mac, unsigned int mac_len);
+
+/**
+ * mcps_crypto_aes_ecb_128_create() - Create a context using AES ECB 128.
+ *
+ * NOTE: This API should be implemented by platform.
+ *
+ * Return: The pointer to the context that will be used to encrypt & decrypt.
+ */
+struct mcps_aes_ecb_128_ctx *mcps_crypto_aes_ecb_128_create(void);
+
+/**
+ * mcps_crypto_aes_ecb_128_set_encrypt() - Set a context using
+ * Authenticated Encryption Associated Data with AES ECB* 128.
+ * @ctx: Context.
+ * @key: AES key.
+ *
+ * NOTE: This API should be implemented by platform.
+ *
+ * Return: 0 or error.
+ */
+int mcps_crypto_aes_ecb_128_set_encrypt(struct mcps_aes_ecb_128_ctx *ctx, const uint8_t *key);
+
+/**
+ * mcps_crypto_aes_ecb_128_set_decrypt() - Set a context using
+ * Authenticated Encryption Associated Data with AES ECB* 128.
+ * @ctx: Context.
+ * @key: AES key.
+ *
+ * NOTE: This API should be implemented by platform.
+ *
+ * Return: 0 or error.
+ */
+int mcps_crypto_aes_ecb_128_set_decrypt(struct mcps_aes_ecb_128_ctx *ctx, const uint8_t *key);
+
+/**
+ * mcps_crypto_aes_ecb_128_destroy() - Destroy the AES ECB 128 context.
+ * @ctx: Context.
+ *
+ * NOTE: This API should be implemented by platform.
+ */
+void mcps_crypto_aes_ecb_128_destroy(struct mcps_aes_ecb_128_ctx *ctx);
+
+/**
+ * mcps_crypto_aes_ecb_128_encrypt() - Encrypt using AES ECB 128.
+ * @ctx: Context.
+ * @data: Data to encrypt.
+ * @data_len: Data length in bytes, should be a multiple of AES_BLOCK_SIZE.
+ * @out: Ciphered data with same length as data.
+ *
+ * NOTE: This API should be implemented by platform.
+ *
+ * Return: 0 or error.
+ */
+int mcps_crypto_aes_ecb_128_encrypt(struct mcps_aes_ecb_128_ctx *ctx,
+		const uint8_t *data, unsigned int data_len, uint8_t *out);
+
+#endif /* MCPS_CRYPTO_H */
diff --git a/mac/fira_access.c b/mac/fira_access.c
index 0cddc41..ac7c50a 100644
--- a/mac/fira_access.c
+++ b/mac/fira_access.c
@@ -26,6 +26,7 @@
 #include "fira_session.h"
 #include "fira_frame.h"
 #include "fira_trace.h"
+#include "fira_sts.h"
 
 #include <asm/unaligned.h>
 #include <linux/string.h>
@@ -102,13 +103,9 @@ static void fira_access_setup_frame(struct fira_local *local,
 	bool is_first_frame = slot->message_id == FIRA_MESSAGE_ID_CONTROL;
 	bool request_rssi = session->params.report_rssi;
 	if (is_rframe) {
-		memcpy(sts_params->v, session->crypto.sts_v, AES_BLOCK_SIZE);
-		put_unaligned_be32(slot->index,
-				   sts_params->v + AES_BLOCK_SIZE / 2);
-		memcpy(sts_params->key,
-		       session->crypto.derived_authentication_key,
-		       AES_KEYSIZE_128);
-		/* Constant for the moment. */
+		fira_sts_get_sts_params(session, slot->index, sts_params->v,
+					sizeof(sts_params->v), sts_params->key,
+					sizeof(sts_params->key));
 		sts_params->n_segs = params->number_of_sts_segments;
 		sts_params->seg_len =
 			params->sts_length == FIRA_STS_LENGTH_128 ?
@@ -209,28 +206,27 @@ static void fira_access_setup_frame(struct fira_local *local,
 	}
 }
 
-static void fira_controlee_resync(struct fira_session *session, u32 sts_index,
-				  u32 timestamp_dtu)
+static void fira_controlee_resync(struct fira_session *session,
+				  u32 phy_sts_index, u32 timestamp_dtu)
 {
+	u32 block_idx, round_idx, slot_idx;
+	int block_start_timestamp_dtu;
 	const struct fira_session_params *params = &session->params;
-	/* Variable for session resync. */
-	int slots_per_block =
-		params->block_duration_dtu / params->slot_duration_dtu;
-	int sts_offset = sts_index - session->crypto.sts_index_init;
-	int block_index = sts_offset / slots_per_block;
-	int slot_index = sts_offset - block_index * slots_per_block;
-	int round_index = slot_index / params->round_duration_slots;
-	int block_start_dtu =
-		timestamp_dtu - slot_index * params->slot_duration_dtu;
+
+	fira_sts_convert_phy_sts_idx_to_time_indexes(
+		session, phy_sts_index, &block_idx, &round_idx, &slot_idx);
+	block_start_timestamp_dtu =
+		timestamp_dtu -
+		(round_idx * session->params.round_duration_slots + slot_idx) *
+			params->slot_duration_dtu;
 
 	/* Update the session. */
-	session->block_start_dtu = block_start_dtu;
-	session->block_index = block_index;
-	session->sts_index = sts_index - slot_index;
-	session->round_index = round_index;
+	session->block_start_dtu = block_start_timestamp_dtu;
+	session->block_index = block_idx;
+	session->round_index = round_idx;
 	session->controlee.synchronised = true;
 	session->controlee.next_round_index_valid = false;
-	session->controlee.block_index_sync = block_index;
+	session->controlee.block_index_sync = block_idx;
 }
 
 static bool fira_rx_sts_good(struct fira_local *local,
@@ -531,11 +527,13 @@ static void fira_rx_frame_control(struct fira_local *local,
 	struct mcps802154_ie_get_context ie_get = {};
 	const struct fira_session_params *params = NULL;
 	struct fira_session *session;
+	int header_len;
+	__le16 src_short_addr;
 	int last_slot_index = 0;
 	int offset_in_access_duration_dtu;
 	int left_duration_dtu;
 	unsigned n_slots;
-	u32 sts_index;
+	u32 phy_sts_index;
 	u8 *header;
 	int r;
 
@@ -551,14 +549,22 @@ static void fira_rx_frame_control(struct fira_local *local,
 	/* Read the header to capture the session context. */
 	header = skb->data;
 	session = fira_rx_frame_control_header_check(local, slot, skb, &ie_get,
-						     &sts_index);
+						     &phy_sts_index);
 	if (!session)
 		goto failed;
+
+	fira_controlee_resync(session, phy_sts_index, info->timestamp_dtu);
+
 	params = &session->params;
 	ri->rx_ctx = session->rx_ctx[0];
 
+	header_len = skb->data - header;
+	src_short_addr = slot->controller_tx ? local->dst_short_addr :
+					       slot->controlee->short_addr;
+
 	/* Continue to decode the frame. */
-	r = fira_frame_decrypt(local, session, slot, skb, skb->data - header);
+	r = fira_sts_decrypt_frame(session, skb, header_len, src_short_addr,
+				   slot->index);
 	if (r)
 		goto failed;
 	r = fira_frame_control_payload_check(local, skb, &ie_get, &n_slots,
@@ -567,7 +573,6 @@ static void fira_rx_frame_control(struct fira_local *local,
 	if (!r)
 		goto failed;
 
-	fira_controlee_resync(session, sts_index, info->timestamp_dtu);
 	left_duration_dtu =
 		access->duration_dtu - offset_in_access_duration_dtu;
 
@@ -763,6 +768,7 @@ static struct sk_buff *fira_tx_get_frame(struct mcps802154_access *access,
 	const struct fira_session_params *params = &session->params;
 	const struct fira_slot *slot = &local->slots[frame_idx];
 	struct sk_buff *skb;
+	int header_len;
 
 	trace_region_fira_tx_get_frame(session, slot->message_id);
 	if (params->rframe_config == FIRA_RFRAME_CONFIG_SP3 &&
@@ -800,7 +806,11 @@ static struct sk_buff *fira_tx_get_frame(struct mcps802154_access *access,
 		/* LCOV_EXCL_STOP */
 	}
 
-	if (fira_frame_encrypt(local, slot, skb)) {
+	header_len = mcps802154_ie_put_end(skb, false);
+	WARN_ON(header_len < 0);
+
+	if (fira_sts_encrypt_frame(local->current_session, skb, header_len,
+				   local->src_short_addr, slot->index)) {
 		kfree_skb(skb);
 		return NULL;
 	}
@@ -908,8 +918,6 @@ fira_get_access_controller(struct fira_local *local,
 	const struct fira_session_params *params = &session->params;
 	const struct fira_measurement_sequence_step *step =
 		fira_session_get_meas_seq_step(session);
-	int slots_per_block =
-		params->block_duration_dtu / params->slot_duration_dtu;
 	struct mcps802154_access *access = &local->access;
 	struct mcps802154_access_frame *frame;
 	struct mcps802154_sts_params *sts_params;
@@ -937,7 +945,6 @@ fira_get_access_controller(struct fira_local *local,
 	session->block_start_dtu = fsd->block_start_dtu;
 	session->block_index += fsd->add_blocks;
 	session->block_stride_len = params->block_stride_len;
-	session->sts_index += fsd->add_blocks * slots_per_block;
 	session->round_index = fsd->round_index;
 	session->controller.next_block_index =
 		session->block_index + session->block_stride_len + 1;
diff --git a/mac/fira_aead.h b/mac/fira_aead.h
deleted file mode 100644
index 8b9c968..0000000
--- a/mac/fira_aead.h
+++ /dev/null
@@ -1,96 +0,0 @@
-/*
- * This file is part of the UWB stack for linux.
- *
- * Copyright (c) 2020-2022 Qorvo US, Inc.
- *
- * This software is provided under the GNU General Public License, version 2
- * (GPLv2), as well as under a Qorvo commercial license.
- *
- * You may choose to use this software under the terms of the GPLv2 License,
- * version 2 ("GPLv2"), as published by the Free Software Foundation.
- * You should have received a copy of the GPLv2 along with this program.  If
- * not, see <http://www.gnu.org/licenses/>.
- *
- * This program is distributed under the GPLv2 in the hope that it will be
- * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GPLv2 for more
- * details.
- *
- * If you cannot meet the requirements of the GPLv2, you may not use this
- * software for any purpose without first obtaining a commercial license from
- * Qorvo. Please contact Qorvo to inquire about licensing terms.
- */
-
-#ifndef FIRA_AEAD_H
-#define FIRA_AEAD_H
-
-#include <linux/types.h>
-#include <linux/skbuff.h>
-
-struct fira_aead;
-
-/**
- * fira_aead_set_key() - Set key used for encryption/decryption.
- * @aead: Context.
- * @key: AES payload key (key size 128).
- *
- * Return: 0 or error.
- */
-int fira_aead_set_key(struct fira_aead *aead, const u8 *key);
-
-/**
- * fira_aead_encrypt() - Encrypt payload.
- * @aead: Context.
- * @skb: Buffer containing the frame to encrypt.
- * @header_len: Length of the MAC header, used for authentication.
- * @src_short_addr: Source short address.
- * @counter: Counter used for the nonce.
- *
- * Return: 0 or error.
- */
-int fira_aead_encrypt(struct fira_aead *aead, struct sk_buff *skb,
-		      unsigned int header_len, __le16 src_short_addr,
-		      u32 counter);
-
-/**
- * fira_aead_decrypt_scf_check() - Check security control field before
- * decryption.
- * @scf: Security control field.
- *
- * Return: true if good.
- */
-bool fira_aead_decrypt_scf_check(u8 scf);
-
-/**
- * fira_aead_decrypt_prepare() - Prepare the frame before decryption.
- * @skb: Buffer containing the frame to decrypt.
- *
- * Return: 0 or error.
- */
-int fira_aead_decrypt_prepare(struct sk_buff *skb);
-
-/**
- * fira_aead_decrypt() - Decrypt payload.
- * @aead: Context.
- * @skb: Buffer containing the frame to decrypt. MAC header should be present
- * before data.
- * @header_len: Length of the MAC header, used for authentication.
- * @src_short_addr: Source short address.
- * @counter: Counter used for the nonce.
- *
- * NOTE: This function must be called after calling fira_aead_decrypt_prepare.
- *
- * Return: 0 or error. In particular, -EBADMSG is returned if authentication tag
- * is wrong.
- */
-int fira_aead_decrypt(struct fira_aead *aead, struct sk_buff *skb,
-		      unsigned int header_len, __le16 src_short_addr,
-		      u32 counter);
-
-/**
- * fira_aead_destroy() - Release memory used for payload encryption/decryption.
- * @aead: Context to destroy.
- */
-void fira_aead_destroy(struct fira_aead *aead);
-
-#endif /* FIRA_AEAD_H */
diff --git a/mac/fira_cmac.h b/mac/fira_cmac.h
deleted file mode 100644
index 112874d..0000000
--- a/mac/fira_cmac.h
+++ /dev/null
@@ -1,60 +0,0 @@
-/*
- * This file is part of the UWB stack for linux.
- *
- * Copyright (c) 2020-2022 Qorvo US, Inc.
- *
- * This software is provided under the GNU General Public License, version 2
- * (GPLv2), as well as under a Qorvo commercial license.
- *
- * You may choose to use this software under the terms of the GPLv2 License,
- * version 2 ("GPLv2"), as published by the Free Software Foundation.
- * You should have received a copy of the GPLv2 along with this program.  If
- * not, see <http://www.gnu.org/licenses/>.
- *
- * This program is distributed under the GPLv2 in the hope that it will be
- * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GPLv2 for more
- * details.
- *
- * If you cannot meet the requirements of the GPLv2, you may not use this
- * software for any purpose without first obtaining a commercial license from
- * Qorvo. Please contact Qorvo to inquire about licensing terms.
- */
-
-#ifndef FIRA_CMAC_H
-#define FIRA_CMAC_H
-
-#include <crypto/aes.h>
-#include <linux/types.h>
-
-#define FIRA_KDF_LABEL_LEN 8
-#define FIRA_KDF_CONTEXT_LEN 16
-
-/**
- * fira_digest() - Compute a digest using cmac(aes).
- * @key: AES key.
- * @key_len: Length of AES key (AES_KEYSIZE_x).
- * @data: Input data.
- * @data_len: Input data length in octets.
- * @out: Output hash, with length AES_BLOCK_SIZE.
- *
- * Return: 0 or error.
- */
-int fira_digest(const u8 *key, unsigned int key_len, const u8 *data,
-		unsigned int data_len, u8 *out);
-
-/**
- * fira_kdf() - Key derivation function.
- * @input_key: AES input key.
- * @input_key_len: Length of AES input key.
- * @label: KDF label (8 bytes).
- * @context: KDF context (16 bytes).
- * @output_key: AES output key.
- * @output_key_len: Length of AES output key.
- *
- * Return: 0 or error.
- */
-int fira_kdf(const u8 *input_key, unsigned int input_key_len, const char *label,
-	     const u8 *context, u8 *output_key, unsigned int output_key_len);
-
-#endif /* FIRA_CMAC_H */
diff --git a/mac/fira_crypto.c b/mac/fira_crypto.c
index 35297e0..61b6a67 100644..100755
--- a/mac/fira_crypto.c
+++ b/mac/fira_crypto.c
@@ -7,9 +7,9 @@
  * (GPLv2), as well as under a Qorvo commercial license.
  *
  * You may choose to use this software under the terms of the GPLv2 License,
- * version 2 ("GPLv2"), as published by the Free Software Foundation.
- * You should have received a copy of the GPLv2 along with this program.  If
- * not, see <http://www.gnu.org/licenses/>.
+ * version 2 ("GPLv2"), as published by the Free Software Foundation. You should
+ * have received a copy of the GPLv2 along with this program.  If not, see
+ * <http://www.gnu.org/licenses/>.
  *
  * This program is distributed under the GPLv2 in the hope that it will be
  * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -21,297 +21,1475 @@
  * Qorvo. Please contact Qorvo to inquire about licensing terms.
  */
 
-#include "fira_crypto.h"
-
-#include "fira_cmac.h"
-#include "fira_region.h"
-#include "fira_session.h"
+#ifdef __KERNEL__
+#define pr_fmt(fmt) "%s:%s: " fmt, KBUILD_MODNAME, __func__
+#endif
 
-#include <asm/unaligned.h>
 #include <linux/errno.h>
-#include <linux/printk.h>
-#include <linux/skbuff.h>
 #include <linux/string.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <crypto/aes.h>
+
+#include <asm/unaligned.h>
+
+#include "fira_crypto.h"
 #include <net/mcps802154_frame.h>
+#include "mcps_crypto.h"
+
+#ifdef CONFIG_FIRA_CRYPTO_HAVE_SE
+#include "key_manager.h"
+#endif
+
+#ifdef __KERNEL__
+static inline void *platform_malloc(size_t s) { return kmalloc(s, GFP_KERNEL); }
+static inline void platform_free(void *p) { kfree(p); }
+#else
+#include "trace/define_trace_specific.h"
+#define pr_info(...) print_trace(__VA_ARGS__)
+#define pr_err(...) print_trace(__VA_ARGS__)
+#include "platform_alloc.h"
+#endif
+
+#define FIRA_CRYPTO_KDF_LABEL_LEN	8
+#define FIRA_CRYPTO_KDF_CONTEXT_LEN	16
+#define FIRA_CRYPTO_KEY_STS_MASK	0x7FFFFFFF
+
+#define FIRA_IE_VENDOR_OUI_LEN		3
+
+#define FIRA_CRYPTO_AEAD_AUTHSIZE	8
+
+struct fira_crypto {
+	u32 session_id;
+};
 
-#define FIRA_STATIC_STS_SESSION_KEY "StaticTSStaticTS"
+/**
+ * struct fira_crypto_aead - Context for payload encryption/decryption.
+ */
+struct fira_crypto_aead {
+	/**
+	 * @ctx: The context to be used during aead encryption & decryption.
+	 */
+	struct mcps_aes_ccm_star_128_ctx *ctx;
+};
 
 /**
- * fira_crypto_config_digest() - Compute session config digest.
- * @local: FiRa context.
- * @session: Session.
+ * struct fira_crypto - Context containing all crypto related
+ * information.
  *
- * Return: 0 or error.
+ * NOTE: It is regularly used by the FiRa region core to produce the STS
+ * parameters for a given a session and to encrypt/decrypt frames.
  */
-static int fira_crypto_config_digest(struct fira_local *local,
-				     struct fira_session *session)
+struct fira_crypto_base {
+	/**
+	 * @key_size: Size of the key used in the AES derivation.
+	 */
+	u8 key_size;
+
+	/**
+	 * @config_digest: Digest of the configuration, used as input for keys
+	 * derivation.
+	 */
+	u8 config_digest[AES_BLOCK_SIZE];
+
+	/**
+	 * @data_protection_key: Derived from the session key, the label
+	 * "DataPrtK" and the config_digest. The precise size is given by the
+	 * key_size.
+	 */
+	u8 data_protection_key[FIRA_KEY_SIZE_MIN];
+
+	/**
+	 * @derived_authentication_iv: Derived from data_protection_key, the
+	 * label "DerAuthI", the current value of crypto_sts_index, and the
+	 * config_digest. Used to compute the STS parameters for a slot.
+	 */
+	u8 derived_authentication_iv[AES_BLOCK_SIZE];
+
+	/**
+	 * @derived_authentication_key: Derived from data_protection_key, the
+	 * label "DerAuthK", the current value of crypto_sts_index and the
+	 * config_digest. Used to compute the STS parameters for a slot.
+	 */
+	u8 derived_authentication_key[FIRA_KEY_SIZE_MIN];
+
+	/**
+	 * @derived_payload_key: Derived from data_protection_key, the label
+	 * "DerPaylK", the current value of crypto_sts_index and the
+	 * config_digest. Used to encrypt/decrypt message PIE.
+	 */
+	u8 derived_payload_key[FIRA_KEY_SIZE_MIN];
+
+	/**
+	 * @aead: AEAD Context for payload encryption/decryption.
+	 */
+	struct fira_crypto_aead aead;
+};
+
+struct fira_crypto_ctx {
+	/**
+	 * @session_id: Id of the session using the fira_crypto.
+	 * This can also be a subsession key when this STS mode is active.
+	 */
+	u32 session_id;
+
+	/**
+	 * @ca_entry: Entry in list .
+	 */
+	struct list_head entry;
+
+	/**
+	 * @sts_config: The type of STS requested for this crypto.
+	 */
+	enum fira_sts_mode sts_config;
+
+	/**
+	 * @base: Common parameters between all types of crypto contexts.
+	 */
+	struct fira_crypto_base base;
+
+	/******* Dynamic STS Only **************/
+
+	/**
+	 * @ecb_ctx: AES ECB context
+	 */
+	struct mcps_aes_ecb_128_ctx *ecb_ctx;
+
+	/**
+	 * @privacy_key: Derived from the session key, the label
+	 * "PrivacyK" and the config_digest. Used to encrypt/decrypt message HIE.
+	 */
+	u8 privacy_key[FIRA_KEY_SIZE_MIN];
+
+	/******* Static STS Only **************/
+	/**
+	 * @vupper64: The vupper 64 to use when static STS is used.
+	 */
+	u8 vupper64[FIRA_VUPPER64_SIZE];
+};
+
+static LIST_HEAD(fira_crypto_ctx_list);
+
+static int fira_crypto_kdf(const u8 *input_key, unsigned int input_key_len,
+			const char *label, const u8 *context, u8 *output_key,
+			unsigned int output_key_len)
 {
-	u8 derivation_data[17];
-	u8 *p = derivation_data;
-	int slot_duration_us;
-	static const u8 zero_key[AES_KEYSIZE_128];
-	const struct mcps802154_channel *channel;
-
-	channel = mcps802154_get_current_channel(local->llhw);
-
-	slot_duration_us = session->params.slot_duration_dtu * 1000 /
-			   (local->llhw->dtu_freq_hz / 1000);
-
-	*p++ = session->params.ranging_round_usage;
-	*p++ = session->params.sts_config;
-	*p++ = session->params.multi_node_mode;
-	*p++ = session->params.channel_number ?: channel->channel;
-	put_unaligned_be16(slot_duration_us, p);
-	p += sizeof(u16);
-	*p++ = session->params.mac_fcs_type;
-	*p++ = session->params.rframe_config;
-	*p++ = session->params.preamble_code_index ?: channel->preamble_code;
-	*p++ = session->params.sfd_id;
-	*p++ = session->params.psdu_data_rate;
-	*p++ = session->params.preamble_duration;
-	*p++ = 3; /* Constant. */
-	put_unaligned_be32(session->id, p);
-
-	return fira_digest(zero_key, sizeof(zero_key), derivation_data,
-			   sizeof(derivation_data),
-			   session->crypto.config_digest);
+	u8 derivation_data[sizeof(u32) + FIRA_CRYPTO_KDF_LABEL_LEN +
+		FIRA_CRYPTO_KDF_CONTEXT_LEN + sizeof(u32)];
+	u8 *p;
+	int r;
+
+	if (input_key_len != AES_KEYSIZE_128) {
+		pr_err("input_key_len != AES_KEYSIZE_128");
+		return -EINVAL;
+	}
+
+	p = derivation_data;
+	put_unaligned_be32(1, p);
+	p += sizeof(u32);
+	memcpy(p, label, FIRA_CRYPTO_KDF_LABEL_LEN);
+	p += FIRA_CRYPTO_KDF_LABEL_LEN;
+	memcpy(p, context, FIRA_CRYPTO_KDF_CONTEXT_LEN);
+	p += FIRA_CRYPTO_KDF_CONTEXT_LEN;
+	put_unaligned_be32(output_key_len * 8, p);
+
+	r = mcps_crypto_cmac_aes_128_digest(input_key, derivation_data,
+			sizeof(derivation_data), output_key);
+
+	return r;
+}
+
+static int fira_crypto_aead_set_key(struct fira_crypto_aead *aead, const u8 *key)
+{
+	aead->ctx = mcps_crypto_aead_aes_ccm_star_128_create();
+
+	return aead->ctx ? mcps_crypto_aead_aes_ccm_star_128_set(aead->ctx, key) : -ENOMEM;
 }
 
-int fira_crypto_derive_per_session(struct fira_local *local,
-				   struct fira_session *session)
+static void fira_aead_fill_nonce(u8 *nonce, __le16 src_short_addr,
+				 u32 crypto_sts_index)
 {
-	struct fira_crypto *crypto = &session->crypto;
-	u8 sts_index_init_tmp[AES_KEYSIZE_128];
+	u8 *p;
+
+	p = nonce;
+	memset(p, 0, IEEE802154_EXTENDED_ADDR_LEN - IEEE802154_SHORT_ADDR_LEN);
+	p += IEEE802154_EXTENDED_ADDR_LEN - IEEE802154_SHORT_ADDR_LEN;
+	put_unaligned_be16(src_short_addr, p);
+	p += IEEE802154_SHORT_ADDR_LEN;
+	put_unaligned_be32(crypto_sts_index, p);
+	p += sizeof(u32);
+	*p++ = IEEE802154_SCF_SECLEVEL_ENC_MIC64;
+}
+
+static int fira_crypto_aead_encrypt(struct fira_crypto_aead *aead,
+		struct sk_buff *skb, unsigned int header_len,
+		__le16 src_short_addr, u32 crypto_sts_index)
+{
+	u8 nonce[MCPS_CRYPTO_AES_CCM_STAR_NONCE_LEN];
+	u8 *header = skb->data;
+	u8 *payload = skb->data + header_len;
+	const int payload_len = skb->len - header_len;
+	u8 *mac = skb->data + skb->len;
 	int r;
 
-	if (session->params.sts_config != FIRA_STS_CONFIG_STATIC)
-		return -EOPNOTSUPP;
+	fira_aead_fill_nonce(nonce, src_short_addr, crypto_sts_index);
 
-	r = fira_crypto_config_digest(local, session);
-	if (r)
-		goto out;
+	skb->data[IEEE802154_FC_LEN + IEEE802154_SHORT_ADDR_LEN] =
+		IEEE802154_SCF_SECLEVEL_ENC_MIC64 |
+		IEEE802154_SCF_NO_FRAME_COUNTER;
 
-	memcpy(session->crypto.session_key, FIRA_STATIC_STS_SESSION_KEY,
-	       AES_KEYSIZE_128);
-	session->crypto.key_size = AES_KEYSIZE_128;
+	r = mcps_crypto_aead_aes_ccm_star_128_encrypt(
+		aead->ctx, nonce, header, header_len, payload, payload_len, mac,
+		FIRA_CRYPTO_AEAD_AUTHSIZE);
 
-	r = fira_kdf(crypto->session_key, crypto->key_size, "DataPrtK",
-		     crypto->config_digest, crypto->data_protection_key,
-		     crypto->key_size);
-	if (r)
-		goto out;
+	if (!r)
+		skb_put(skb, FIRA_CRYPTO_AEAD_AUTHSIZE);
+	else
+		skb->data[IEEE802154_FC_LEN + IEEE802154_SHORT_ADDR_LEN] |=
+			IEEE802154_SCF_NO_FRAME_COUNTER;
 
-	r = fira_kdf(crypto->data_protection_key, crypto->key_size, "StsIndIn",
-		     crypto->config_digest, sts_index_init_tmp,
-		     AES_KEYSIZE_128);
-	if (r)
-		goto out;
-	crypto->sts_index_init =
-		get_unaligned_be32(sts_index_init_tmp + AES_KEYSIZE_128 -
-				   sizeof(u32)) &
-		0x7fffffff;
-out:
-	memzero_explicit(sts_index_init_tmp, sizeof(sts_index_init_tmp));
 	return r;
 }
 
-int fira_crypto_derive_per_rotation(struct fira_local *local,
-				    struct fira_session *session, u32 sts_index)
+static int fira_crypto_aead_decrypt(struct fira_crypto_aead *aead,
+		struct sk_buff *skb, unsigned int header_len,
+		__le16 src_short_addr, u32 crypto_sts_index)
 {
-	struct fira_crypto *crypto = &session->crypto;
-	u8 context[AES_BLOCK_SIZE];
-	u8 derived_authentication_iv[AES_BLOCK_SIZE];
-	u32 crypto_sts_index;
-	u32 sts_v_counter;
-	u8 *sts_v;
+	u8 nonce[MCPS_CRYPTO_AES_CCM_STAR_NONCE_LEN];
+	u8 *header;
+	u8 *payload;
+	unsigned int payload_len;
+	u8 *mac;
+	int r;
+
+	header = skb->data - header_len;
+	payload = skb->data;
+	payload_len = skb->len;
+	mac = skb->data + payload_len;
+
+	fira_aead_fill_nonce(nonce, src_short_addr, crypto_sts_index);
+
+	r = mcps_crypto_aead_aes_ccm_star_128_decrypt(
+		aead->ctx, nonce, header, header_len, payload, payload_len, mac,
+		FIRA_CRYPTO_AEAD_AUTHSIZE);
+
+	if (!r) {
+		header[IEEE802154_FC_LEN + IEEE802154_SHORT_ADDR_LEN] |=
+			IEEE802154_SCF_NO_FRAME_COUNTER;
+	}
+
+	memzero_explicit(mac, FIRA_CRYPTO_AEAD_AUTHSIZE);
+
+	return r;
+}
+
+static void fira_crypto_aead_destroy(struct fira_crypto_aead *aead)
+{
+	mcps_crypto_aead_aes_ccm_star_128_destroy(aead->ctx);
+}
+
+/*! ----------------------------------------------------------------------------------------------
+ * @brief This function returns the fira crypto context relative to a sessionID
+ *
+ * input parameters:
+ * @param session_id - sessionId to articulate the research on.
+ *
+ * output parameters
+ *
+ * return NULL if error or struct fira_crypto_ctx pointer.
+ */
+struct fira_crypto_ctx *get_session(u32 session_id)
+{
+	struct fira_crypto_ctx *session;
+
+	list_for_each_entry(session, &fira_crypto_ctx_list, entry) {
+		if (session->session_id == session_id)
+			return session;
+	}
+
+	return NULL;
+}
+
+static void remove_session(struct fira_crypto_ctx *session)
+{
+	fira_crypto_aead_destroy(&session->base.aead);
+	mcps_crypto_aes_ecb_128_destroy(session->ecb_ctx);
+	list_del(&session->entry);
+	/* Wipe all derived keys */
+	memzero_explicit(session, sizeof(*session));
+	platform_free(session);
+}
+
+/*! ----------------------------------------------------------------------------------------------
+ * @brief This function is used to initialise the FIRA crypto backend
+ *
+ * input parameters:
+ * @param key_manager - key manager to use. Not used for the moment.
+ *
+ * output parameters
+ *
+ * return 0 if no error.
+ */
+int fira_crypto_init(void *key_manager)
+{
+#ifdef CONFIG_FIRA_CRYPTO_HAVE_SE
+	/* Opens the UBWS - SE secure channel */
+	return key_manager_init(NULL);
+#else
+	return 0;
+#endif
+}
+
+/************************************** FIRA STS API FCTS ***************************************/
+
+int fira_crypto_context_init(const struct fira_crypto_params *params,
+		struct fira_crypto **crypto)
+{
+	int status = -1;
 	int r;
+	struct fira_crypto *fira_crypto_ext;
+	struct fira_crypto_ctx *fira_crypto_ctx;
+	u8 session_key[AES_KEYSIZE_128];
+
+	/* checks the sessionId is not already allocated */
+	fira_crypto_ctx = get_session(params->session_id);
+	if (fira_crypto_ctx) {
+		pr_err("Crypto context already exists for session id %u\n", params->session_id);
+		/* Remove the context */
+		remove_session(fira_crypto_ctx);
+	}
+
+	fira_crypto_ext = platform_malloc(sizeof(*fira_crypto_ext));
+	memset(fira_crypto_ext, 0, sizeof(*fira_crypto_ext));
+	fira_crypto_ctx = platform_malloc(sizeof(*fira_crypto_ctx));
+	memset(fira_crypto_ctx, 0, sizeof(*fira_crypto_ctx));
+	if (fira_crypto_ctx && fira_crypto_ext) {
+		fira_crypto_ctx->session_id = params->session_id;
+		fira_crypto_ext->session_id = params->session_id;
+		/* Add this context in the global list */
+		list_add(&fira_crypto_ctx->entry, &fira_crypto_ctx_list);
+		status = 0;
+	} else {
+		pr_err("Crypto context initialisation failed. Not enough memory !\n");
+	}
+
+	if (status)
+		return status;
+
+	/* Retrieve session key */
+	switch (params->sts_config) {
+	case FIRA_STS_MODE_STATIC:
+		memcpy(session_key, "StaticTSStaticTS", AES_KEYSIZE_128);
+		fira_crypto_ctx->base.key_size = AES_KEYSIZE_128;
+		memcpy(fira_crypto_ctx->vupper64, params->vupper64, FIRA_VUPPER64_SIZE);
+		break;
+
+#ifdef CONFIG_FIRA_CRYPTO_HAVE_SE
+	case FIRA_STS_MODE_DYNAMIC:
+	case FIRA_STS_MODE_DYNAMIC_INDIVIDUAL_KEY:
+		status = key_manager_consume_key(params->session_id,
+				session_key, AES_KEYSIZE_128);
+		fira_crypto_ctx->base.key_size = AES_KEYSIZE_128;
+		break;
+#endif // CONFIG_FIRA_CRYPTO_HAVE_SE
+
+	case FIRA_STS_MODE_PROVISIONED:
+	case FIRA_STS_MODE_PROVISIONED_INDIVIDUAL_KEY:
+		if (params->prov_session_key) {
+			memcpy(session_key, params->prov_session_key,
+					params->prov_session_key_len);
+			fira_crypto_ctx->base.key_size = params->prov_session_key_len;
+		} else {
+			/* Session key not set */
+			pr_err("Session key not provisioned !\n");
+			status = -1;
+		}
+		break;
+
+	default:
+		/* Bad value */
+		pr_err("STS config unknown !\n");
+		status = -1;
+	}
+
+	fira_crypto_ctx->sts_config = params->sts_config;
 
-	/* Zero for Static STS. */
-	crypto_sts_index = 0;
+	if (!status) {
+		/* Compute Config Digest */
+		static const u8 zero_key[AES_KEYSIZE_128];
 
-	memcpy(context, crypto->config_digest + sizeof(u32),
-	       AES_BLOCK_SIZE - sizeof(u32));
-	put_unaligned_be32(crypto_sts_index,
-			   context + AES_BLOCK_SIZE - sizeof(u32));
+		r = mcps_crypto_cmac_aes_128_digest(zero_key,
+				params->concat_params,
+				params->concat_params_size,
+				fira_crypto_ctx->base.config_digest);
+		if (r)
+			goto error_out;
 
-	r = fira_kdf(crypto->data_protection_key, crypto->key_size, "DerAuthI",
-		     context, derived_authentication_iv, AES_KEYSIZE_128);
+		/* Compute secDataProtectionKey */
+		r = fira_crypto_kdf(session_key,
+				fira_crypto_ctx->base.key_size,
+				"DataPrtK",
+				fira_crypto_ctx->base.config_digest,
+				fira_crypto_ctx->base.data_protection_key,
+				FIRA_KEY_SIZE_MIN);
+		if (r)
+			goto error_out;
+
+		if (params->sts_config == FIRA_STS_MODE_STATIC) {
+			/* rotate keys only once for static_sts */
+			r = fira_crypto_rotate_elements(
+					fira_crypto_ext,
+					0);
+		} else {
+
+			/* Compute secPrivacy Key and setup AES ECB context */
+			r = fira_crypto_kdf(session_key,
+					fira_crypto_ctx->base.key_size,
+					"PrivacyK",
+					fira_crypto_ctx->base.config_digest,
+					fira_crypto_ctx->privacy_key,
+					FIRA_KEY_SIZE_MIN);
+
+		}
+		if (r)
+			goto error_out;
+	}
+
+	/* Wipe session key */
+	memzero_explicit(session_key, AES_KEYSIZE_128);
+
+	*crypto = fira_crypto_ext;
+
+	return status;
+
+error_out:
+	/* Wipe session key */
+	memzero_explicit(session_key, AES_KEYSIZE_128);
+	*crypto = NULL;
+	remove_session(fira_crypto_ctx);
+	platform_free(fira_crypto_ext);
+	return r;
+}
+
+/**
+ * fira_crypto_dynamic_deinit() - De-initialize a dynamic STS context.
+ * @session_id: Pointer to the session id.
+ */
+void fira_crypto_context_deinit(struct fira_crypto *crypto)
+{
+	u32 fira_session_id = crypto->session_id;
+	struct fira_crypto_ctx *fira_crypto_ctx = get_session(fira_session_id);
+
+	if (fira_crypto_ctx) {
+		/* Remove the context */
+		remove_session(fira_crypto_ctx);
+	} else {
+		/* The context doesn't exist */
+		pr_err("Crypto context unknown for session id %u\n", fira_session_id);
+	}
+	platform_free(crypto);
+}
+
+/**
+ * fira_crypto_rotate_elements() - Rotate the crypto elements contained in the
+ * crypto context.
+ *
+ * NOTE: After calling this function, all active crypto elements will be the latest
+ * rotated ones.
+ *
+ * @crypto: The context containing the elements to rotate.
+ * @crypto_sts_index: The crypto STS index to use to rotate the elements.
+ *
+ * Return: 0 or error.
+ */
+int fira_crypto_rotate_elements(struct fira_crypto *crypto,
+				const u32 crypto_sts_index)
+{
+	int r = 0;
+	u8 context[AES_BLOCK_SIZE];
+	u32 fira_session_id = crypto->session_id;
+	struct fira_crypto_ctx *fira_crypto_ctx = get_session(fira_session_id);
+
+	memcpy(context, fira_crypto_ctx->base.config_digest + sizeof(u32),
+			AES_BLOCK_SIZE - sizeof(u32));
+	put_unaligned_be32(crypto_sts_index, context + AES_BLOCK_SIZE -
+			sizeof(u32));
+
+	r = fira_crypto_kdf(fira_crypto_ctx->base.data_protection_key,
+			fira_crypto_ctx->base.key_size,
+			"DerAuthI", context,
+			fira_crypto_ctx->base.derived_authentication_iv,
+			fira_crypto_ctx->base.key_size);
 	if (r)
-		goto out;
-	sts_v = crypto->sts_v;
-	memcpy(sts_v, session->params.vupper64, FIRA_VUPPER64_SIZE);
-	sts_v += FIRA_VUPPER64_SIZE;
-	memset(sts_v, 0, sizeof(u32));
-	sts_v += sizeof(u32);
-	sts_v_counter = get_unaligned_be32(derived_authentication_iv +
-					   AES_BLOCK_SIZE - sizeof(u32)) &
-			0x7fffffff;
-	put_unaligned_be32(sts_v_counter, sts_v);
-
-	r = fira_kdf(crypto->data_protection_key, crypto->key_size, "DerAuthK",
-		     context, crypto->derived_authentication_key,
-		     AES_KEYSIZE_128);
+		goto error_out;
+
+	r = fira_crypto_kdf(fira_crypto_ctx->base.data_protection_key,
+			fira_crypto_ctx->base.key_size,
+			"DerAuthK", context,
+			fira_crypto_ctx->base.derived_authentication_key,
+			fira_crypto_ctx->base.key_size);
 	if (r)
-		goto out;
+		goto error_out;
 
-	r = fira_kdf(crypto->data_protection_key, crypto->key_size, "DerPaylK",
-		     context, crypto->derived_payload_key, AES_KEYSIZE_128);
+	r = fira_crypto_kdf(fira_crypto_ctx->base.data_protection_key,
+			fira_crypto_ctx->base.key_size,
+			"DerPaylK", context,
+			fira_crypto_ctx->base.derived_payload_key,
+			fira_crypto_ctx->base.key_size);
 	if (r)
-		goto out;
+		goto error_out;
 
-	r = fira_aead_set_key(&crypto->aead, crypto->derived_payload_key);
+	if (fira_crypto_ctx->base.aead.ctx == NULL)
+		r = fira_crypto_aead_set_key(&fira_crypto_ctx->base.aead,
+				fira_crypto_ctx->base.derived_payload_key);
 
-out:
+error_out:
 	memzero_explicit(context, sizeof(context));
-	memzero_explicit(derived_authentication_iv,
-			 sizeof(derived_authentication_iv));
 	return r;
 }
 
-#ifndef CONFIG_MCPS802154_DISABLE_AUTO_TEST
-
-int fira_crypto_test(void)
+/**
+ * fira_crypto_build_phy_sts_index_init() - Build the phy STS index init value
+ * related to the given crypto context.
+ *
+ * @crypto: The context to use to compute the phy STS index init value.
+ * @phy_sts_index_init: The pointer where the computed value will be stored.
+ *
+ * Return: 0 or error.
+ */
+int fira_crypto_build_phy_sts_index_init(struct fira_crypto *crypto,
+					 u32 *phy_sts_index_init)
 {
-	/* LCOV_EXCL_START */
-	static const u8 zero_key[AES_KEYSIZE_128];
-	struct sk_buff *skb = NULL;
+	u32 fira_session_id = crypto->session_id;
+	struct fira_crypto_ctx *fira_crypto_ctx = get_session(fira_session_id);
 	int r;
-	struct fira_round_hopping_sequence round_hopping_sequence;
-
-	static const u8 digest_data[] = { 0x02, 0x00, 0x00, 0x09, 0x07, 0xD0,
-					  0x00, 0x03, 0x0a, 0x02, 0x00, 0x01,
-					  0x03, 0x01, 0x23, 0x45, 0x67 };
-	u8 digest[AES_BLOCK_SIZE];
-	static const u8 digest_expect[] = { 0xa0, 0x43, 0x90, 0xcf, 0x8a, 0x33,
-					    0xf6, 0xeb, 0x7e, 0x2f, 0xc3, 0x78,
-					    0x87, 0xb6, 0xb2, 0xa3 };
-
-	static const u8 frame_key[] = { 0xa5, 0x5f, 0xab, 0x83, 0xb6, 0x20,
-					0xf9, 0xf6, 0xa4, 0x7c, 0xdb, 0x72,
-					0x91, 0x7c, 0x73, 0x8a };
-	static const u8 frame[] = {
-		0x49, 0x2b, 0xa2, 0xaa, 0x20, 0x13, 0x00, 0xff, 0x18, 0x5a,
-		0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x67, 0x45,
-		0x23, 0x01, 0x78, 0xbe, 0x9b, 0x0b, 0x00, 0x3f, 0x1b, 0x90,
-		0xff, 0x18, 0x5a, 0x03, 0x05, 0x00, 0x00, 0x03, 0x42, 0x55,
-		0x01, 0x04, 0x44, 0x55, 0x03, 0x07, 0x42, 0x55, 0x05, 0x09,
-		0x42, 0x55, 0x09, 0x0a, 0x44, 0x55, 0x0b
-	};
-	static const u8 frame_enc[] = {
-		0x49, 0x2b, 0xa2, 0xaa, 0x26, 0x13, 0x00, 0xff, 0x18, 0x5a,
-		0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x67, 0x45,
-		0x23, 0x01, 0x78, 0xbe, 0x9b, 0x0b, 0x00, 0x3f, 0xcb, 0xa4,
-		0xfd, 0x37, 0xd1, 0x99, 0x44, 0x88, 0x7c, 0x2b, 0xec, 0x2e,
-		0x1a, 0x99, 0x8e, 0x80, 0x61, 0x7c, 0x44, 0xb5, 0xe8, 0xe3,
-		0xf3, 0x35, 0x3a, 0xb9, 0xf2, 0x29, 0x1b, 0x80, 0x4b, 0xba,
-		0xe1, 0xa9, 0x2a, 0x20, 0x28
-	};
-	const unsigned int frame_header_len = 28;
-	const __le16 frame_src_short_addr = 0xaaa1;
-	const u32 frame_counter = 0;
-	struct fira_aead aead = {};
-
-	/* Test digest. */
-	r = fira_digest(zero_key, AES_KEYSIZE_128, digest_data,
-			sizeof(digest_data), digest);
-	if (r != 0 || memcmp(digest, digest_expect, sizeof(digest)) != 0) {
-		pr_err("fira_digest test failed: r = %d\n", r);
-		print_hex_dump(KERN_ERR, "digest:        ", DUMP_PREFIX_NONE,
-			       16, 1, digest, sizeof(digest), false);
-		print_hex_dump(KERN_ERR, "digest_expect: ", DUMP_PREFIX_NONE,
-			       16, 1, digest_expect, sizeof(digest_expect),
-			       false);
+	u8 phy_sts_index_init_tmp[AES_KEYSIZE_128];
+
+	r = fira_crypto_kdf(fira_crypto_ctx->base.data_protection_key,
+			fira_crypto_ctx->base.key_size,
+			"StsIndIn", fira_crypto_ctx->base.config_digest,
+			phy_sts_index_init_tmp,
+			fira_crypto_ctx->base.key_size);
+	if (r)
+		goto error_out;
+
+	*phy_sts_index_init =
+		get_unaligned_be32(phy_sts_index_init_tmp +
+				fira_crypto_ctx->base.key_size - sizeof(u32)) &
+		FIRA_CRYPTO_KEY_STS_MASK;
+	return 0;
+
+error_out:
+	memzero_explicit(phy_sts_index_init_tmp,
+			 sizeof(phy_sts_index_init_tmp));
+	return r;
+
+}
+
+/**
+ * fira_crypto_dynamic_get_sts_params() - Get STS parameters for a given slot
+ * using a dynamic STS configuration.
+ * @crypto: The context to use to get the STS parameters.
+ * @crypto_sts_index: The crypto STS index related to the slot request slot.
+ * @sts_v: Output buffer to store the STS V.
+ * @sts_v_size: Size of the STS V buffer.
+ * @sts_key: Output buffer to store the STS key.
+ * @sts_key_size: Size of the STS key buffer.
+ *
+ * Return: 0 or error.
+ */
+int fira_crypto_get_sts_params(struct fira_crypto *crypto,
+		const u32 crypto_sts_index, u8 *sts_v, u32 sts_v_size,
+		u8 *sts_key, u32 sts_key_size)
+{
+	u32 fira_session_id = crypto->session_id;
+	struct fira_crypto_ctx *fira_crypto_ctx = get_session(fira_session_id);
+	u8 *vupper64 = NULL;
+	u32 v_counter;
+	u8 *sts_v_temp;
+
+	if (fira_crypto_ctx->sts_config == FIRA_STS_MODE_STATIC)
+		vupper64 = fira_crypto_ctx->vupper64;
+	else
+		vupper64 = fira_crypto_ctx->base.derived_authentication_iv;
+
+	if (sts_v_size < AES_BLOCK_SIZE || sts_key_size < AES_KEYSIZE_128)
 		return -EINVAL;
+
+	sts_v_temp = sts_v;
+
+	/* Concatenate the 128 bits of sts_v
+	 * sts_v = vupper64 | crypto_sts_index | v_counter
+	 */
+	memcpy(sts_v_temp, vupper64, FIRA_VUPPER64_SIZE);
+	sts_v_temp += FIRA_VUPPER64_SIZE;
+	put_unaligned_be32(crypto_sts_index, sts_v_temp);
+	sts_v_temp += sizeof(crypto_sts_index);
+	v_counter = get_unaligned_be32(
+			fira_crypto_ctx->base.derived_authentication_iv +
+			AES_BLOCK_SIZE - sizeof(v_counter)) &
+		FIRA_CRYPTO_KEY_STS_MASK;
+	put_unaligned_be32(v_counter, sts_v_temp);
+
+	memcpy(sts_key, fira_crypto_ctx->base.derived_authentication_key,
+			fira_crypto_ctx->base.key_size);
+
+	return 0;
+}
+
+/**
+ * fira_crypto_encrypt_frame() - Encrypt a 802154 frame using a given context.
+ *
+ * NOTE: The src address is given as an argument as it is a part of the nonce needed
+ * to encrypt the frame and it is not present in the 802154 frame.
+ * The length of the header is given because only the payload is encrypted even if
+ * the encryption algorithm needs the whole 802154 frame.
+ * Encryption is done in-place.
+ * When called this function shall increase the size of the skb of
+ * FIRA_CRYPTO_AEAD_AUTHSIZE and set the correct bits in the 802154 frame SCF.
+ *
+ * @crypto: The context to use to encrypt the frame.
+ * @skb: The buffer containing the whole frame, skb->data points to the start of
+ * the 802154 frame header.
+ * @header_len: The length of the 802154 frame header. Can be used to find the
+ * position of the 802154 frame payload relative to skb->data.
+ * @src_short_addr: The short source address attached to the frame.
+ * @crypto_sts_index: The crypto STS index attached to the frame.
+ *
+ * Return: 0 or error.
+ */
+int fira_crypto_encrypt_frame(struct fira_crypto *crypto, struct sk_buff *skb,
+		int header_len, __le16 src_short_addr, u32 crypto_sts_index)
+{
+	u32 fira_session_id = crypto->session_id;
+	struct fira_crypto_ctx *fira_crypto_ctx = get_session(fira_session_id);
+
+	return fira_crypto_aead_encrypt(&fira_crypto_ctx->base.aead, skb, header_len,
+					src_short_addr, crypto_sts_index);
+}
+
+/**
+ * fira_crypto_decrypt_frame() - Decrypt a 802154 frame using a given context.
+ *
+ * NOTE: The src address is given as an argument as it is a part of the nonce needed
+ * to decrypt the frame and it is not present in the 802154 frame.
+ * The length of the header is given because only the payload is encrypted even if
+ * the encryption algorithm needs the whole 802154 frame.
+ * Decryption is done in-place.
+ * When called, this function shall reduce the
+ * size of the skb of FIRA_CRYPTO_AEAD_AUTHSIZE and verify the correct bits in the
+ * 802154 frame SCF.
+ *
+ * @crypto: The crypto to use to decrypt the frame.
+ * @skb: The buffer containing the whole frame, skb->data points to the start of
+ * the 802154 frame payload.
+ * @header_len: The length of the 802154 frame header. Can be used to find the
+ * start of the 802154 frame payload relative to skb->data.
+ * @src_short_addr: The short source address attached to the frame.
+ * @crypto_sts_index: The crypto STS index attached to the frame.
+ *
+ * Return: 0 or error.
+ */
+int fira_crypto_decrypt_frame(struct fira_crypto *crypto, struct sk_buff *skb,
+		int header_len, __le16 src_short_addr, u32 crypto_sts_index)
+{
+	u32 fira_session_id = crypto->session_id;
+	struct fira_crypto_ctx *fira_crypto_ctx = get_session(fira_session_id);
+
+	return fira_crypto_aead_decrypt(&fira_crypto_ctx->base.aead, skb, header_len,
+					src_short_addr, crypto_sts_index);
+}
+
+/**
+ * fira_crypto_encrypt_hie() - Encrypt the HIE in a FiRa 802154 frame.
+ * @crypto: The context to use to get the STS parameters.
+ * @skb: Buffer containing the frame to encrypt.
+ * @hie_offset: Offset to the start of the HIE (relating to skb->data) to encrypt.
+ * @hie_len: Length of the HIE to encrypt.
+ *
+ * Return: 0 or error.
+ */
+int fira_crypto_encrypt_hie(struct fira_crypto *crypto, struct sk_buff *skb,
+		int hie_offset, int hie_len)
+{
+	u32 fira_session_id = crypto->session_id;
+	struct fira_crypto_ctx *fira_crypto_ctx = get_session(fira_session_id);
+	int rc;
+
+	if (fira_crypto_ctx->sts_config == FIRA_STS_MODE_STATIC)
+		return 0;
+
+	fira_crypto_ctx->ecb_ctx = mcps_crypto_aes_ecb_128_create();
+	if (!fira_crypto_ctx->ecb_ctx ||
+		mcps_crypto_aes_ecb_128_set_encrypt(
+			fira_crypto_ctx->ecb_ctx,
+			fira_crypto_ctx->privacy_key))
+		return -1;
+
+	rc = mcps_crypto_aes_ecb_128_encrypt(fira_crypto_ctx->ecb_ctx,
+			(const uint8_t *)(skb->data + hie_offset +
+				IEEE802154_IE_HEADER_LEN +
+				FIRA_IE_VENDOR_OUI_LEN),
+			(unsigned int)hie_len - IEEE802154_IE_HEADER_LEN -
+				FIRA_IE_VENDOR_OUI_LEN,
+			(uint8_t *)(skb->data + hie_offset +
+				IEEE802154_IE_HEADER_LEN +
+				FIRA_IE_VENDOR_OUI_LEN));
+
+	mcps_crypto_aes_ecb_128_destroy(fira_crypto_ctx->ecb_ctx);
+	fira_crypto_ctx->ecb_ctx = NULL;
+
+	return rc;
+}
+
+/**
+ * fira_crypto_decrypt_hie() - Decrypt the HIE in a FiRa 802154 frame.
+ * @crypto: The context to use to get the STS parameters.
+ * @skb: Buffer containing the frame to decrypt.
+ * @hie_offset: Offset to the start of the HIE (relative to skb->data) to decrypt.
+ * @hie_len: Length of the HIE to decrypt.
+ *
+ * Return: 0 or error.
+ */
+int fira_crypto_decrypt_hie(struct fira_crypto *crypto, struct sk_buff *skb,
+		int hie_offset, int hie_len)
+{
+	u32 fira_session_id = crypto->session_id;
+	struct fira_crypto_ctx *fira_crypto_ctx = get_session(fira_session_id);
+	int rc;
+
+	if (fira_crypto_ctx->sts_config == FIRA_STS_MODE_STATIC)
+		return 0;
+
+	fira_crypto_ctx->ecb_ctx = mcps_crypto_aes_ecb_128_create();
+	if (!fira_crypto_ctx->ecb_ctx ||
+			mcps_crypto_aes_ecb_128_set_decrypt(
+				fira_crypto_ctx->ecb_ctx,
+				fira_crypto_ctx->privacy_key))
+		return -1;
+
+	rc = mcps_crypto_aes_ecb_128_encrypt(fira_crypto_ctx->ecb_ctx,
+			(const uint8_t *)(skb->data + hie_offset),
+			(unsigned int)hie_len,
+			(uint8_t *)(skb->data + hie_offset));
+
+	mcps_crypto_aes_ecb_128_destroy(fira_crypto_ctx->ecb_ctx);
+	fira_crypto_ctx->ecb_ctx = NULL;
+
+	return rc;
+}
+
+/**
+ * fira_crypto_get_capabilities() - Get capabilities of the platform used.
+ *
+ * Return:
+ *    bit 0 : FIRA_STS_MODE_STATIC supported
+ *    bit 1 : FIRA_STS_MODE_DYNAMIC supported
+ *    bit 2 : FIRA_STS_MODE_DYNAMIC_INDIVIDUAL_KEY supported
+ *    bit 3 : FIRA_STS_MODE_PROVISIONED supported
+ *    bit 4 : FIRA_STS_MODE_PROVISIONED_INDIVIDUAL_KEY supported
+ *    other : not used
+ */
+u32 fira_crypto_get_capabilities(void)
+{
+	u32 status = 0;
+
+	status += STS_CAP(STATIC);
+	status += STS_CAP(PROVISIONED);
+
+#ifdef CONFIG_FIRA_CRYPTO_HAVE_SE
+	status += STS_CAP(DYNAMIC);
+#endif
+
+	return status;
+}
+
+int fira_crypto_prepare_decrypt(struct fira_crypto *crypto, struct sk_buff *skb)
+{
+	u8 scf;
+	u8 *p;
+
+	p = skb->data - (IEEE802154_FC_LEN + IEEE802154_SHORT_ADDR_LEN +
+			 IEEE802154_SCF_LEN);
+	scf = p[IEEE802154_FC_LEN + IEEE802154_SHORT_ADDR_LEN];
+	if (!(scf == (IEEE802154_SCF_SECLEVEL_ENC_MIC64 |
+		      IEEE802154_SCF_NO_FRAME_COUNTER)) ||
+	    (skb->len < FIRA_CRYPTO_AEAD_AUTHSIZE))
+		return -EBADMSG;
+	skb_trim(skb, skb->len - FIRA_CRYPTO_AEAD_AUTHSIZE);
+
+	return 0;
+}
+
+static bool compare_bufs(const void *a, size_t alen, const void *b, size_t blen)
+{
+	if (alen != blen || memcmp(a, b, alen) != 0) {
+#ifdef __KERNEL__
+		print_hex_dump(KERN_ERR, "a: ", DUMP_PREFIX_OFFSET, 16, 1, a,
+				alen, false);
+		print_hex_dump(KERN_ERR, "b: ", DUMP_PREFIX_OFFSET, 16, 1, b,
+				blen, false);
+#endif
+		return false;
+	}
+
+	return true;
+}
+
+/**
+ * fira_crypto_test_static()
+ * Run the FIRA CONSORTIUM UWB MAC TECHNICAL REQUIREMENTS
+ * version 1.3.0 test vectors for Static STS
+ *
+ * NOTE: This APis used for unit tests only.
+ *
+ * Return: 0 if ok
+ */
+static int fira_crypto_test_static(void)
+{
+	/* Static STS */
+	static const u8 config[] = {
+		0x02, 0x00, 0x00, 0x09, 0x07, 0xd0, 0x00, 0x03,
+		0x0a, 0x02, 0x00, 0x01, 0x03, 0x01, 0x23, 0x45,
+		0x67
+	};
+	static const u8 vUpp[] = {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07};
+	static const struct fira_crypto_params param = {
+		.session_id = 0x01234567,
+		.sts_config = FIRA_STS_MODE_STATIC,
+		.concat_params = config,
+		.concat_params_size = sizeof(config),
+		.vupper64 = vUpp
+	};
+	static const u8 configDigest[] = {
+		0xa0, 0x43, 0x90, 0xcf, 0x8a, 0x33, 0xf6, 0xeb,
+		0x7e, 0x2f, 0xc3, 0x78, 0x87, 0xb6, 0xb2, 0xa3
+	};
+	static const u8 secDataProtectionKey[] = {
+		0xf3, 0x21, 0x6c, 0x87, 0xd0, 0xc6, 0x93, 0x2e,
+		0x39, 0x57, 0xb4, 0x81, 0xfa, 0xb8, 0xb2, 0x09
+	};
+	static const u8 derived_authentication_iv[] = {
+		0x8b, 0x54, 0x37, 0x6e, 0x7c, 0xd7, 0xa5, 0xd6,
+		0x6b, 0xd1, 0x20, 0x00, 0x97, 0x27, 0x41, 0x19
+	};
+	static const u8 derived_authentication_key[] = {
+		0xdd, 0x98, 0x97, 0xf2, 0xb8, 0x5c, 0x9d, 0xc8,
+		0xa7, 0xde, 0xc0, 0x1c, 0xca, 0x5b, 0x61, 0xdb
+	};
+	static const u8 derived_payload_key[] = {
+		0xa5, 0x5f, 0xab, 0x83, 0xb6, 0x20, 0xf9, 0xf6,
+		0xa4, 0x7c, 0xdb, 0x72, 0x91, 0x7c, 0x73, 0x8a
+	};
+	static const u8 sts_v_ref[] = {
+		0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
+		0x00, 0x00, 0x00, 0x00, 0x17, 0x27, 0x41, 0x19
+	};
+	/* build the RCM Frame */
+	/* build the header */
+	static const u8 RCM[] = {
+		0x49, 0x2b, 0xa2, 0xaa, 0x26, 0x13, 0x00, 0xff,
+		0x18, 0x5a, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+		0x08, 0x08, 0x67, 0x45, 0x23, 0x01, 0x78, 0xbe,
+		0x9b, 0x0b, 0x00, 0x3f, 0x1b, 0x90, 0xff, 0x18,
+		0x5a, 0x03, 0x05, 0x00, 0x00, 0x03, 0x42, 0x55,
+		0x01, 0x04, 0x44, 0x55, 0x03, 0x07, 0x42, 0x55,
+		0x05, 0x09, 0x42, 0x55, 0x09, 0x0a, 0x44, 0x55,
+		0x0b
+	};
+	static const u8 RCMRef[] = {
+		0x49, 0x2b, 0xa2, 0xaa, 0x26, 0x13, 0x00, 0xff,
+		0x18, 0x5a, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+		0x08, 0x08, 0x67, 0x45, 0x23, 0x01, 0x78, 0xbe,
+		0x9b, 0x0b, 0x00, 0x3f, 0xcb, 0xa4, 0xfd, 0x37,
+		0xd1, 0x99, 0x44, 0x88, 0x7c, 0x2b, 0xec, 0x2e,
+		0x1a, 0x99, 0x8e, 0x80, 0x61, 0x7c, 0x44, 0xb5,
+		0xe8, 0xe3, 0xf3, 0x35, 0x3a, 0xb9, 0xf2, 0x29,
+		0x1b, 0x80, 0x4b, 0xba, 0xe1, 0xa9, 0x2a, 0x20,
+		0x28
+	};
+	static const u8 Header[] = {
+		0x49, 0x2b, 0xa2, 0xaa, 0x26, 0x13, 0x00, 0xff,
+		0x18, 0x5a, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+		0x08, 0x08, 0x67, 0x45, 0x23, 0x01, 0x78, 0xbe,
+		0x9b, 0x0b
+	};
+	static const u8 HeaderRef[] = {
+		0x49, 0x2b, 0xa2, 0xaa, 0x26, 0x13, 0x00, 0xff,
+		0x18, 0x5a, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+		0x08, 0x08, 0x67, 0x45, 0x23, 0x01, 0x78, 0xbe,
+		0x9b, 0x0b
+	};
+	/* Decrypt Frame */
+	static const u8 RCM_Rcv_Ref[] = {
+		0x49, 0x2b, 0xa2, 0xaa, 0x26, 0x13, 0x00, 0xff,
+		0x18, 0x5a, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+		0x08, 0x08, 0x67, 0x45, 0x23, 0x01, 0x78, 0xbe,
+		0x9b, 0x0b, 0x00, 0x3f, 0x1b, 0x90, 0xff, 0x18,
+		0x5a, 0x03, 0x05, 0x00, 0x00, 0x03, 0x42, 0x55,
+		0x01, 0x04, 0x44, 0x55, 0x03, 0x07, 0x42, 0x55,
+		0x05, 0x09, 0x42, 0x55, 0x09, 0x0a, 0x44, 0x55,
+		0x0b
+	};
+	static const u8 Frame_Rcv[] = {
+		0x49, 0x2b, 0xa2, 0xaa, 0x26, 0x13, 0x00, 0xff,
+		0x18, 0x5a, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+		0x08, 0x08, 0x67, 0x45, 0x23, 0x01, 0x78, 0xbe,
+		0x9b, 0x0b, 0x00, 0x3f, 0xcb, 0xa4, 0xfd, 0x37,
+		0xd1, 0x99, 0x44, 0x88, 0x7c, 0x2b, 0xec, 0x2e,
+		0x1a, 0x99, 0x8e, 0x80, 0x61, 0x7c, 0x44, 0xb5,
+		0xe8, 0xe3, 0xf3, 0x35, 0x3a, 0xb9, 0xf2, 0x29,
+		0x1b, 0x80, 0x4b, 0xba, 0xe1, 0xa9, 0x2a, 0x20,
+		0x28
+	};
+	static const u8 Header_Rcv[] = {
+		0x49, 0x2b, 0xa2, 0xaa, 0x26, 0x13, 0x00, 0xff,
+		0x18, 0x5a, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+		0x08, 0x08, 0x67, 0x45, 0x23, 0x01, 0x78, 0xbe,
+		0x9b, 0x0b
+	};
+	static const u8 HeaderRef_Rcv[] = {
+		0x49, 0x2b, 0xa2, 0xaa, 0x26, 0x13, 0x00, 0xff,
+		0x18, 0x5a, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+		0x08, 0x08, 0x67, 0x45, 0x23, 0x01, 0x78, 0xbe,
+		0x9b, 0x0b
+	};
+	int err = -1, r;
+	struct fira_crypto *crypto = NULL;
+	struct fira_crypto_ctx *fira_crypto_ctx;
+	u32 phy_sts_index_init = 0;
+	const u32 crypto_sts_index = 0;
+	u8 sts_v[16];
+	u8 sts_key[16];
+	struct sk_buff *skb = NULL;
+
+	r = fira_crypto_context_init(&param, &crypto);
+	if (r != 0 || !crypto || crypto->session_id != param.session_id) {
+		pr_err("fira_crypto_context_init fail: %d\n", r);
+		goto end;
+	}
+
+	fira_crypto_ctx = get_session(param.session_id);
+	if (!fira_crypto_ctx) {
+		pr_err("cannot get session\n");
+		goto end;
+	}
+
+	if (!compare_bufs(configDigest, sizeof(configDigest),
+				fira_crypto_ctx->base.config_digest,
+				sizeof(fira_crypto_ctx->base.config_digest))) {
+		pr_err("compare configDigest fail\n");
+		goto end;
+	}
+
+	if (!compare_bufs(secDataProtectionKey, sizeof(secDataProtectionKey),
+				fira_crypto_ctx->base.data_protection_key,
+				sizeof(fira_crypto_ctx->base.data_protection_key))) {
+		pr_err("compare secDataProtectionKey fail\n");
+		goto end;
+	}
+
+	if (!compare_bufs(derived_authentication_iv,
+				sizeof(derived_authentication_iv),
+				fira_crypto_ctx->base.derived_authentication_iv,
+				sizeof(fira_crypto_ctx->base.derived_authentication_iv))) {
+		pr_err("compare derived_authentication_iv fail\n");
+		goto end;
 	}
 
-	/* Test AEAD. */
-	r = fira_aead_set_key(&aead, frame_key);
+	if (!compare_bufs(derived_authentication_key,
+				sizeof(derived_authentication_key),
+				fira_crypto_ctx->base.derived_authentication_key,
+				sizeof(fira_crypto_ctx->base.derived_authentication_key))) {
+		pr_err("compare derived_authentication_key fail\n");
+		goto end;
+	}
+
+	if (!compare_bufs(derived_payload_key, sizeof(derived_payload_key),
+				fira_crypto_ctx->base.derived_payload_key,
+				sizeof(fira_crypto_ctx->base.derived_payload_key))) {
+		pr_err("compare derived_payload_key fail\n");
+		goto end;
+	}
+
+	r = fira_crypto_build_phy_sts_index_init(crypto, &phy_sts_index_init);
 	if (r != 0) {
-		pr_err("fira_aead_set_key test failed: r = %d\n", r);
-		return -EINVAL;
+		pr_err("fira_crypto_build_phy_sts_index_init fail: %d\n", r);
+		goto end;
+	}
+	if (phy_sts_index_init != 0x0b9bbe78) {
+		pr_err("phy_sts_index_init fail\n");
+		goto end;
 	}
 
-	/* AEAD enc. */
-	skb = alloc_skb(sizeof(frame_enc), GFP_KERNEL);
+	r = fira_crypto_get_sts_params(crypto, crypto_sts_index, sts_v,
+				sizeof(sts_v), sts_key, sizeof(sts_key));
+	if (r != 0) {
+		pr_err("fira_crypto_get_sts_params fail: %d\n", r);
+		goto end;
+	}
+	if (!compare_bufs(derived_authentication_key,
+				sizeof(derived_authentication_key),
+				sts_key, sizeof(sts_key))) {
+		pr_err("compare sts_key fail\n");
+		goto end;
+	}
+
+	if (!compare_bufs(sts_v_ref, sizeof(sts_v_ref), sts_v, sizeof(sts_v))) {
+		pr_err("compare sts_v fail\n");
+		goto end;
+	}
+
+	skb = alloc_skb(128, GFP_KERNEL);
 	if (!skb) {
-		r = -ENOMEM;
-		goto out;
+		pr_err("cannot allocate skb\n");
+		goto end;
 	}
-	skb_put_data(skb, frame, sizeof(frame));
 
-	r = fira_aead_encrypt(&aead, skb, frame_header_len,
-			      frame_src_short_addr, frame_counter);
-	if (r != 0 || skb->len != sizeof(frame_enc) ||
-	    memcmp(skb->data, frame_enc, sizeof(frame_enc)) != 0) {
-		pr_err("fira_aead_encrypt test failed: r = %d\n", r);
-		print_hex_dump(KERN_ERR, "frame_enc: ", DUMP_PREFIX_NONE, 16, 1,
-			       skb->data, skb->len, false);
-		print_hex_dump(KERN_ERR, "expect:    ", DUMP_PREFIX_NONE, 16, 1,
-			       frame_enc, sizeof(frame_enc), false);
-		r = -EINVAL;
-		goto out;
+	skb_put_data(skb, Header, sizeof(Header));
+
+	/* Encrypt Header first (NOP in Static STS) */
+	r = fira_crypto_encrypt_hie(crypto, skb, 5, 21);
+	if (r != 0) {
+		pr_err("fira_crypto_encrypt_hie fail: %d\n", r);
+		goto end;
+	}
+	if (!compare_bufs(HeaderRef, sizeof(HeaderRef), skb->data, skb->len)) {
+		pr_err("fira_crypto_encrypt_hie compare HeaderRef fail\n");
+		goto end;
 	}
 
-	/* AEAD dec. */
 	kfree_skb(skb);
-	skb = alloc_skb(sizeof(frame_enc), GFP_KERNEL);
+
+	skb = alloc_skb(128, GFP_KERNEL);
 	if (!skb) {
-		r = -ENOMEM;
-		goto out;
-	}
-	skb_put_data(skb, frame_enc, sizeof(frame_enc));
-	skb_pull(skb, frame_header_len);
-
-	/* Prepare cannot fail. */
-	fira_aead_decrypt_prepare(skb);
-	r = fira_aead_decrypt(&aead, skb, frame_header_len,
-			      frame_src_short_addr, frame_counter);
-	skb_push(skb, frame_header_len);
-	if (r != 0 || skb->len != sizeof(frame) ||
-	    memcmp(skb->data, frame, sizeof(frame)) != 0) {
-		pr_err("fira_aead_decrypt test failed: r = %d\n", r);
-		print_hex_dump(KERN_ERR, "frame:  ", DUMP_PREFIX_NONE, 16, 1,
-			       skb->data, skb->len, false);
-		print_hex_dump(KERN_ERR, "expect: ", DUMP_PREFIX_NONE, 16, 1,
-			       frame, sizeof(frame), false);
-		r = -EINVAL;
-		goto out;
-	}
-
-	/* AEAD dec bad tag. */
+		pr_err("cannot allocate skb\n");
+		goto end;
+	}
+
+	skb_put_data(skb, RCM, sizeof(RCM));
+
+	r = fira_crypto_encrypt_frame(crypto, skb, 28, 0xaaa1, 0);
+	if (r != 0) {
+		pr_err("fira_crypto_encrypt_frame fail: %d\n", r);
+		goto end;
+	}
+
+	if (!compare_bufs(RCMRef, sizeof(RCMRef), skb->data, skb->len)) {
+		pr_err("fira_crypto_encrypt_frame compare RCMRef fail\n");
+		goto end;
+	}
+
 	kfree_skb(skb);
-	skb = alloc_skb(sizeof(frame_enc), GFP_KERNEL);
+
+	skb = alloc_skb(128, GFP_KERNEL);
 	if (!skb) {
-		r = -ENOMEM;
-		goto out;
+		pr_err("cannot allocate skb\n");
+		goto end;
 	}
-	skb_put_data(skb, frame_enc, sizeof(frame_enc));
-	skb_pull(skb, frame_header_len);
-	skb->data[skb->len - 1]++;
 
-	/* Prepare cannot fail. */
-	fira_aead_decrypt_prepare(skb);
-	r = fira_aead_decrypt(&aead, skb, frame_header_len,
-			      frame_src_short_addr, frame_counter);
-	if (r != -EBADMSG) {
-		pr_err("fira_aead_decrypt bad msg test failed: r = %d\n", r);
-		r = -EINVAL;
-		goto out;
+	skb_put_data(skb, Frame_Rcv, sizeof(Frame_Rcv));
+	skb_pull(skb, 28); /* skip header */
+
+	skb_trim(skb, skb->len - FIRA_CRYPTO_AEAD_AUTHSIZE);
+	r = fira_crypto_decrypt_frame(crypto, skb, 28, 0xaaa1, 0);
+	if (r != 0) {
+		pr_err("fira_crypto_decrypt_frame fail: %d\n", r);
+		goto end;
 	}
 
-	/* Test ecb(aes) presence for hopping. */
-	r = fira_round_hopping_crypto_init(&round_hopping_sequence);
-	if (r)
-		goto out;
-	fira_round_hopping_crypto_destroy(&round_hopping_sequence);
+	skb_push(skb, 28); /* restore header */
+
+	if (!compare_bufs(RCM_Rcv_Ref, sizeof(RCM_Rcv_Ref), skb->data, skb->len)) {
+		pr_err("fira_crypto_decrypt_frame compare RCM_Rcv_Ref fail\n");
+		goto end;
+	}
 
-	r = 0;
-out:
 	kfree_skb(skb);
-	fira_aead_destroy(&aead);
 
-	/* LCOV_EXCL_STOP */
-	return r;
+	skb = alloc_skb(128, GFP_KERNEL);
+	if (!skb) {
+		pr_err("cannot allocate skb\n");
+		goto end;
+	}
+
+	skb_put_data(skb, Header_Rcv, sizeof(Header_Rcv));
+
+	/* Decrypt header (NOP in Static STS) */
+	r = fira_crypto_decrypt_hie(crypto, skb, 10, 16);
+	if (r != 0) {
+		pr_err("fira_crypto_decrypt_hie fail: %d\n", r);
+		goto end;
+	}
+	if (!compare_bufs(HeaderRef_Rcv, sizeof(HeaderRef_Rcv), skb->data,
+				skb->len)) {
+		pr_err("fira_crypto_decrypt_hie compare HeaderRef_Rcv fail\n");
+		goto end;
+	}
+
+	err = 0;
+
+	pr_info("Static STS tests success\n");
+
+end:
+	if (skb)
+		kfree_skb(skb);
+	if (crypto)
+		fira_crypto_context_deinit(crypto);
+
+	return err;
 }
 
-#endif /* !CONFIG_MCPS802154_DISABLE_AUTO_TEST */
+/**
+ * fira_crypto_test_provisioned()
+ * Run the FIRA CONSORTIUM UWB MAC TECHNICAL REQUIREMENTS
+ * version 1.3.0 test vectors for Dynamic STS (Provisioned STS is ran instead of
+ * pure dynamic)
+ *
+ * NOTE: This APis used for unit tests only.
+ *
+ * Return: 0 if ok
+ */
+static int fira_crypto_test_provisioned(void)
+{
+	/* Provisioned STS (equivalent to D-STS) */
+	static const u8 config_P_STS[] = {
+		0x02, 0x01, 0x00, 0x09, 0x07, 0xD0, 0x00, 0x03,
+		0x0a, 0x02, 0x00, 0x01, 0x03, 0x01, 0x23, 0x45,
+		0x67
+	};
+	static const u8 sessionKey[] = {
+		0x44, 0x79, 0x6e, 0x61, 0x6d, 0x69, 0x63, 0x53,
+		0x54, 0x53, 0x4e, 0x6f, 0x52, 0x6f, 0x74, 0x30
+	};
+	static const struct fira_crypto_params param_p_sts = {
+		.session_id = 0x01234567,
+		.sts_config = FIRA_STS_MODE_PROVISIONED,
+		.concat_params = config_P_STS,
+		.concat_params_size = sizeof(config_P_STS),
+		.prov_session_key = sessionKey,
+		.prov_session_key_len = sizeof(sessionKey)
+	};
+	static const u8 configDigest_p_sts[] = {
+		0x08, 0x93, 0x66, 0xba, 0xfb, 0x3b, 0x24, 0xbf,
+		0xd2, 0x93, 0x33, 0x77, 0x61, 0xb8, 0x8f, 0xc3
+	};
+	static const u8 secDataPrivacyKey_p_sts[] = {
+		0x3a, 0x4b, 0xab, 0x18, 0x74, 0x4a, 0xee, 0x93,
+		0x86, 0x50, 0xf1, 0xa0, 0x3f, 0x58, 0x5a, 0x49
+	};
+	static const u8 secDataProtectionKey_p_sts[] = {
+		0x67, 0xf7, 0x02, 0x7e, 0xa6, 0x2d, 0x84, 0xa5,
+		0xe1, 0xa8, 0xd7, 0xb8, 0xb8, 0xac, 0xae, 0xaf
+	};
+	static const u8 derived_authentication_iv_p_sts[] = {
+		0xfa, 0x32, 0x6f, 0xed, 0x87, 0xd2, 0xef, 0x7e,
+		0xb6, 0x80, 0xb2, 0xd6, 0xd1, 0x19, 0xa9, 0xb8
+	};
+	static const u8 derived_authentication_key_p_sts[] = {
+		0x91, 0xa2, 0xde, 0x58, 0xff, 0x3b, 0x5e, 0x85,
+		0x15, 0x33, 0x58, 0xd6, 0x15, 0x64, 0x64, 0xff
+	};
+	static const u8 derived_payload_key_p_sts[] = {
+		0x97, 0xe4, 0xab, 0x69, 0x61, 0x77, 0xbb, 0x39,
+		0x92, 0x77, 0xb8, 0x35, 0x9f, 0xa5, 0x5d, 0x19
+	};
+	static const u8 sts_v_ref_p_sts[] = {
+		0xfa, 0x32, 0x6f, 0xed, 0x87, 0xd2, 0xef, 0x7e,
+		0x04, 0x1f, 0x3b, 0xa0, 0x51, 0x19, 0xa9, 0xb8
+	};
+	/* build the RCM Frame */
+	static const u8 RCM_p_sts[] = {
+		0x49, 0x2b, 0xa2, 0xaa, 0x26, 0x13, 0x00, 0xff,
+		0x18, 0x5a, 0x84, 0x6c, 0xa4, 0x3c, 0x52, 0xfb,
+		0xb0, 0x2b, 0x56, 0xa9, 0x87, 0x9d, 0xb0, 0x4e,
+		0x4e, 0x03, 0x00, 0x3f, 0x1b, 0x90, 0xff, 0x18,
+		0x5a, 0x03, 0x05, 0x00, 0x00, 0x03, 0x42, 0x55,
+		0x01, 0x04, 0x44, 0x55, 0x03, 0x07, 0x42, 0x55,
+		0x05, 0x09, 0x42, 0x55, 0x09, 0x0a, 0x44, 0x55,
+		0x0b
+	};
+	static const u8 RCMRef_p_sts[] = {
+		0x49, 0x2b, 0xa2, 0xaa, 0x26, 0x13, 0x00, 0xff,
+		0x18, 0x5a, 0x84, 0x6c, 0xa4, 0x3c, 0x52, 0xfb,
+		0xb0, 0x2b, 0x56, 0xa9, 0x87, 0x9d, 0xb0, 0x4e,
+		0x4e, 0x03, 0x00, 0x3f, 0x82, 0x76, 0xe0, 0x44,
+		0xf3, 0x78, 0xab, 0xbe, 0xd2, 0x39, 0x86, 0x7e,
+		0xd2, 0xfe, 0x5c, 0x9d, 0xcd, 0x13, 0x1d, 0x1f,
+		0x63, 0x38, 0xf1, 0xf7,	0x9d, 0xb1, 0x84, 0x71,
+		0x72, 0x7a, 0x10, 0xfc, 0x80, 0x04, 0x7e, 0xdb,
+		0x0f
+	};
+	static const u8 Header_p_sts[] = {
+		0x49, 0x2b, 0xa2, 0xaa, 0x26, 0x13, 0x00, 0xff,
+		0x18, 0x5a, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+		0x08, 0x08, 0x67, 0x45, 0x23, 0x01, 0xa0, 0x3b,
+		0x1f, 0x04
+	};
+	static const u8 HeaderRef_p_sts[] = {
+		0x49, 0x2b, 0xa2, 0xaa, 0x26, 0x13, 0x00, 0xff,
+		0x18, 0x5a, 0x84, 0x6c, 0xa4, 0x3c, 0x52, 0xfb,
+		0xb0, 0x2b, 0x56, 0xa9, 0x87, 0x9d, 0xb0, 0x4e,
+		0x4e, 0x03
+	};
+	/* Decrypt Frame */
+	static const u8 Header_RCM_p_sts_Rcv[] = {
+		0x49, 0x2b, 0xa2, 0xaa, 0x26, 0x13, 0x00, 0xff,
+		0x18, 0x5a, 0x84, 0x6c, 0xa4, 0x3c, 0x52, 0xfb,
+		0xb0, 0x2b, 0x56, 0xa9, 0x87, 0x9d, 0xb0, 0x4e,
+		0x4e, 0x03, 0x00, 0x3f, 0x82, 0x76, 0xe0, 0x44,
+		0xf3, 0x78, 0xab, 0xbe, 0xd2, 0x39, 0x86, 0x7e,
+		0xd2, 0xfe, 0x5c, 0x9d, 0xcd, 0x13, 0x1d, 0x1f,
+		0x63, 0x38, 0xf1, 0xf7, 0x9d, 0xb1, 0x84, 0x71,
+		0x72, 0x7a, 0x10, 0xfc, 0x80, 0x04, 0x7e, 0xdb,
+		0x0f
+	};
+	static const u8 RCMRef_p_sts_Rcv[] = {
+		0x49, 0x2b, 0xa2, 0xaa, 0x26, 0x13, 0x00, 0xff,
+		0x18, 0x5a, 0x84, 0x6c, 0xa4, 0x3c, 0x52, 0xfb,
+		0xb0, 0x2b, 0x56, 0xa9, 0x87, 0x9d, 0xb0, 0x4e,
+		0x4e, 0x03, 0x00, 0x3f, 0x1b, 0x90, 0xff, 0x18,
+		0x5a, 0x03, 0x05, 0x00, 0x00, 0x03, 0x42, 0x55,
+		0x01, 0x04, 0x44, 0x55, 0x03, 0x07, 0x42, 0x55,
+		0x05, 0x09, 0x42, 0x55, 0x09, 0x0a, 0x44, 0x55,
+		0x0b
+	};
+	static const u8 Header_p_sts_Rcv[] = {
+		0x49, 0x2b, 0xa2, 0xaa, 0x26, 0x13, 0x00, 0xff,
+		0x18, 0x5a, 0x84, 0x6c, 0xa4, 0x3c, 0x52, 0xfb,
+		0xb0, 0x2b, 0x56, 0xa9, 0x87, 0x9d, 0xb0, 0x4e,
+		0x4e, 0x03
+	};
+	static const u8 HeaderRef_p_sts_Rcv[] = {
+		0x49, 0x2b, 0xa2, 0xaa, 0x26, 0x13, 0x00, 0xff,
+		0x18, 0x5a, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+		0x08, 0x08, 0x67, 0x45, 0x23, 0x01, 0xa0, 0x3b,
+		0x1f, 0x04
+	};
+	int err = -1, r;
+	struct fira_crypto *crypto = NULL;
+	struct fira_crypto_ctx *fira_crypto_ctx;
+	u32 phy_sts_index_init = 0;
+	u32 crypto_sts_index_p_sts = 0;
+	u8 sts_v[16];
+	u8 sts_key[16];
+	struct sk_buff *skb = NULL;
+
+	r = fira_crypto_context_init(&param_p_sts, &crypto);
+	if (r != 0 || !crypto || crypto->session_id != param_p_sts.session_id) {
+		pr_err("fira_crypto_context_init fail: %d\n", r);
+		goto end;
+	}
+
+	fira_crypto_ctx = get_session(param_p_sts.session_id);
+	if (!fira_crypto_ctx) {
+		pr_err("cannot get session\n");
+		goto end;
+	}
+
+	if (!compare_bufs(configDigest_p_sts, sizeof(configDigest_p_sts),
+				fira_crypto_ctx->base.config_digest,
+				sizeof(fira_crypto_ctx->base.config_digest))) {
+		pr_err("compare configDigest_p_sts fail\n");
+		goto end;
+	}
+
+	if (!compare_bufs(secDataPrivacyKey_p_sts,
+				sizeof(secDataPrivacyKey_p_sts),
+				fira_crypto_ctx->privacy_key,
+				sizeof(fira_crypto_ctx->privacy_key))) {
+		pr_err("compare secDataPrivacyKey_p_sts fail\n");
+		goto end;
+	}
+
+	if (!compare_bufs(secDataProtectionKey_p_sts,
+				sizeof(secDataProtectionKey_p_sts),
+				fira_crypto_ctx->base.data_protection_key,
+				sizeof(fira_crypto_ctx->base.data_protection_key))) {
+		pr_err("compare secDataProtectionKey fail\n");
+		goto end;
+	}
+
+	r = fira_crypto_build_phy_sts_index_init(crypto, &phy_sts_index_init);
+	if (r != 0) {
+		pr_err("fira_crypto_build_phy_sts_index_init fail: %d\n", r);
+		goto end;
+	}
+	if (phy_sts_index_init != 0x041f3ba0) {
+		pr_err("phy_sts_index_init fail\n");
+		goto end;
+	}
+
+	r = fira_crypto_rotate_elements(crypto, phy_sts_index_init);
+	if (r != 0) {
+		pr_err("fira_crypto_rotate_elements fail: %d\n", r);
+		goto end;
+	}
+
+	if (!compare_bufs(derived_authentication_iv_p_sts,
+				sizeof(derived_authentication_iv_p_sts),
+				fira_crypto_ctx->base.derived_authentication_iv,
+				sizeof(fira_crypto_ctx->base.derived_authentication_iv))) {
+		pr_err("compare derived_authentication_iv_p_sts fail\n");
+		goto end;
+	}
+
+	if (!compare_bufs(derived_authentication_key_p_sts,
+				sizeof(derived_authentication_key_p_sts),
+				fira_crypto_ctx->base.derived_authentication_key,
+				sizeof(fira_crypto_ctx->base.derived_authentication_key))) {
+		pr_err("compare derived_authentication_key_p_sts fail\n");
+		goto end;
+	}
+
+	if (!compare_bufs(derived_payload_key_p_sts,
+				sizeof(derived_payload_key_p_sts),
+				fira_crypto_ctx->base.derived_payload_key,
+				sizeof(fira_crypto_ctx->base.derived_payload_key))) {
+		pr_err("compare derived_payload_key fail\n");
+		goto end;
+	}
+
+	/* Return STS parameters slot 0 */
+	crypto_sts_index_p_sts = 0x041f3ba0;
+	r = fira_crypto_get_sts_params(crypto, crypto_sts_index_p_sts, sts_v,
+				sizeof(sts_v), sts_key, sizeof(sts_key));
+	if (r != 0) {
+		pr_err("fira_crypto_get_sts_params fail: %d\n", r);
+		goto end;
+	}
+	if (!compare_bufs(derived_authentication_key_p_sts,
+				sizeof(derived_authentication_key_p_sts),
+				sts_key, sizeof(sts_key))) {
+		pr_err("compare sts_key Fail\n");
+		goto end;
+	}
+	if (!compare_bufs(sts_v_ref_p_sts, sizeof(sts_v_ref_p_sts), sts_v,
+				sizeof(sts_v))) {
+		pr_err("compare sts_v_ref_p_sts Fail\n");
+		goto end;
+	}
+
+	skb = alloc_skb(128, GFP_KERNEL);
+	if (!skb) {
+		pr_err("cannot allocate skb\n");
+		goto end;
+	}
+
+	skb_put_data(skb, Header_p_sts, sizeof(Header_p_sts));
+
+	/* Encrypt Header first */
+	r = fira_crypto_encrypt_hie(crypto, skb, 5, 16);
+	if (r != 0) {
+		pr_err("fira_crypto_encrypt_hie fail: %d\n", r);
+		goto end;
+	}
+	if (!compare_bufs(HeaderRef_p_sts, sizeof(HeaderRef_p_sts), skb->data,
+				skb->len)) {
+		pr_err("compare HeaderRef_p_sts fail\n");
+		goto end;
+	}
+
+	kfree_skb(skb);
+
+	skb = alloc_skb(128, GFP_KERNEL);
+	if (!skb) {
+		pr_err("cannot allocate skb\n");
+		goto end;
+	}
+
+	skb_put_data(skb, RCM_p_sts, sizeof(RCM_p_sts));
+
+	r = fira_crypto_encrypt_frame(crypto, skb, 28, 0xaaa1, 0x041f3ba0);
+	if (r != 0) {
+		pr_err("fira_crypto_encrypt_frame fail: %d\n", r);
+		goto end;
+	}
+	if (!compare_bufs(RCMRef_p_sts, sizeof(RCMRef_p_sts), skb->data,
+				skb->len)) {
+		pr_err("compare RCMRef_p_sts fail\n");
+		goto end;
+	}
+
+	kfree_skb(skb);
+
+	skb = alloc_skb(128, GFP_KERNEL);
+	if (!skb) {
+		pr_err("cannot allocate skb\n");
+		goto end;
+	}
+
+	skb_put_data(skb, Header_RCM_p_sts_Rcv, sizeof(Header_RCM_p_sts_Rcv));
+	skb_pull(skb, 28); /* skip header */
+
+	skb_trim(skb, skb->len - FIRA_CRYPTO_AEAD_AUTHSIZE);
+	r = fira_crypto_decrypt_frame(crypto, skb, 28, 0xaaa1, 0x041f3ba0);
+	if (r != 0) {
+		pr_err("fira_crypto_decrypt_frame fail: %d\n", r);
+		goto end;
+	}
+
+	skb_push(skb, 28); /* restore header */
+
+	if (!compare_bufs(RCMRef_p_sts_Rcv, sizeof(RCMRef_p_sts_Rcv), skb->data,
+				skb->len)) {
+		pr_err("compare RCMRef_p_sts_Rcv fail\n");
+		goto end;
+	}
+
+	kfree_skb(skb);
+
+	skb = alloc_skb(128, GFP_KERNEL);
+	if (!skb) {
+		pr_err("cannot allocate skb\n");
+		goto end;
+	}
+
+	skb_put_data(skb, Header_p_sts_Rcv, sizeof(Header_p_sts_Rcv));
+
+	/* Decrypt header */
+	r = fira_crypto_decrypt_hie(crypto, skb, 10, 16);
+	if (r != 0) {
+		pr_err("fira_crypto_decrypt_hie fail: %d\n", r);
+		goto end;
+	}
+	if (!compare_bufs(HeaderRef_p_sts_Rcv, sizeof(HeaderRef_p_sts_Rcv),
+				skb->data, skb->len)) {
+		pr_err("compare HeaderRef_p_sts_Rcv fail\n");
+		goto end;
+	}
+
+	err = 0;
+
+	pr_info("Provisioned STS tests success\n");
+
+end:
+	if (skb)
+		kfree_skb(skb);
+	if (crypto)
+		fira_crypto_context_deinit(crypto);
+
+	return err;
+}
+
+/**
+ * fira_crypto_test() - Run the FIRA CONSORTIUM UWB MAC TECHNICAL REQUIREMENTS
+ * version 1.3.0 test vectors for Static STS and Dynamic STS (Provisioned STS is
+ * ran instead of pure dynnamic)
+ *
+ *
+ * NOTE: This APis used for unit tests only.
+ *
+ * Return: 0 if ok,
+ */
+int fira_crypto_test(void)
+{
+	int r = 0;
+
+	r = fira_crypto_test_static() || r;
+	r = fira_crypto_test_provisioned() || r;
+
+	return r ? -1 : 0;
+}
diff --git a/mac/fira_crypto.h b/mac/fira_crypto.h
index fa8d524..8fd2f8d 100644
--- a/mac/fira_crypto.h
+++ b/mac/fira_crypto.h
@@ -24,108 +24,230 @@
 #ifndef NET_MCPS802154_FIRA_CRYPTO_H
 #define NET_MCPS802154_FIRA_CRYPTO_H
 
-#include <crypto/aes.h>
-#include <linux/types.h>
+#include <linux/skbuff.h>
+#include <linux/kernel.h>
 
-#include "fira_aead_impl.h"
+#include <linux/errno.h>
+#include <asm/unaligned.h>
+#include <linux/string.h>
+#include <linux/ieee802154.h>
+#include <linux/printk.h>
+#include <net/fira_region_params.h>
 
-struct fira_local;
-struct fira_session;
+struct fira_crypto;
 
 /**
- * struct fira_crypto - Crypto context for sessions. This contains sensitive data
- * and must be handled specially to avoid leaking information.
+ * fira_crypto_init() - Callback to initialize crypto module implementation.
+ *
+ * @key_manager: Handler to key manager.
+ *
+ * Return: 0 or error.
  */
-struct fira_crypto {
-	/**
-	 * @session_key: Session key. This is a constant for static STS. Size is
-	 * given by @key_size.
-	 */
-	u8 session_key[AES_KEYSIZE_256];
-	/**
-	 * @data_protection_key: Data protection key, used to derive other
-	 * material. Size is given by @key_size.
-	 */
-	u8 data_protection_key[AES_KEYSIZE_256];
+int fira_crypto_init(void *key_manager);
+
+/**
+ * struct fira_crypto_params - Arguments grouping structure for the crypto context
+ * initialization function.
+ */
+struct fira_crypto_params {
 	/**
-	 * @sts_v: STS V, composed of the derived authentication initialization
-	 * vector, V upper 64 (for static STS) and STS index, used for STS generation.
-	 *
-	 * STS index must be updated for each frame.
+	 * @session_id: Id of the session using the fira_crypto.
+	 * This can also be a subsession key when this STS mode is active.
 	 */
-	u8 sts_v[AES_BLOCK_SIZE];
+	u32 session_id;
 	/**
-	 * @derived_authentication_key: Derived authentication key, used for STS
-	 * generation.
+	 * @sts_config: The type of STS requested for this crypto.
 	 */
-	u8 derived_authentication_key[AES_KEYSIZE_128];
+	enum fira_sts_mode sts_config;
 	/**
-	 * @derived_payload_key: Derived payload key, used to encrypt frame
-	 * payload.
+	 * @concat_params: The concatenated parameters of the session according
+	 * to the FiRa specs.
 	 */
-	u8 derived_payload_key[AES_KEYSIZE_128];
+	const u8 *concat_params;
 	/**
-	 * @config_digest: Digest of the configuration, used as input for key
-	 * derivation.
+	 * @concat_params_size: The size of the concatenated parameters.
 	 */
-	u8 config_digest[AES_BLOCK_SIZE];
+	int concat_params_size;
 	/**
-	 * @sts_index_init: Initial value of the STS index, ignore MSB.
+	 * @vupper64: The vupper 64 to use when static STS is used.
 	 */
-	u32 sts_index_init;
+	const u8 *vupper64;
 	/**
-	 * @key_size: Size of the session key and data protection key. All other
-	 * keys are 128 bit.
+	 * @prov_session_key: The session key when provisioned STS is used.
 	 */
-	int key_size;
+	const u8 *prov_session_key;
 	/**
-	 * @aead: Context for payload encryption/decryption.
+	 * @prov_session_key_len: The length of the session key when provisioned STS is used.
 	 */
-	struct fira_aead aead;
+	u8 prov_session_key_len;
 };
 
 /**
- * fira_crypto_derive_per_session() - Prepare crypto material per session.
- * @local: FiRa context.
- * @session: Session.
+ * fira_crypto_get_capabilities() - Query FiRa STS capabilities
  *
- * Prepare everything which is generated once per session.
+ * Return: FiRa crypto backend capabilities as a bitfield
+ * (see &enum fira_sts_mode).
+ */
+u32 fira_crypto_get_capabilities(void);
+
+/**
+ * fira_crypto_context_init() - Initialize a crypto context containing the crypto
+ * elements for a session.
+ * @crypto_params: Parameters to initialize the crypto context.
+ * @crypto: The initialized crypto context.
  *
  * Return: 0 or error.
  */
-int fira_crypto_derive_per_session(struct fira_local *local,
-				   struct fira_session *session);
+int fira_crypto_context_init(const struct fira_crypto_params *crypto_params,
+			     struct fira_crypto **crypto);
+
+/**
+ * fira_crypto_context_deinit() - Deinitialize a crypto context.
+ * @crypto: The crypto context to deinitialize.
+ */
+void fira_crypto_context_deinit(struct fira_crypto *crypto);
 
 /**
- * fira_crypto_derive_per_rotation() - Prepare crypto material per rotation.
- * @local: FiRa context.
- * @session: Session.
- * @sts_index: STS index at time of rotation. Ignored for static STS.
+ * fira_crypto_rotate_elements() - Rotate the crypto elements contained in the
+ * crypto context.
+ *
+ * NOTE: After calling this function, all active crypto elements will be the latest
+ * rotated ones.
  *
- * Prepare keys which are generated at initialization and on key rotation.
+ * @crypto: The context containing the elements to rotate.
+ * @crypto_sts_index: The crypto STS index to use to rotate the elements.
  *
  * Return: 0 or error.
  */
-int fira_crypto_derive_per_rotation(struct fira_local *local,
-				    struct fira_session *session,
-				    u32 sts_index);
+int fira_crypto_rotate_elements(struct fira_crypto *crypto,
+				const u32 crypto_sts_index);
 
-#ifndef CONFIG_MCPS802154_DISABLE_AUTO_TEST
+/**
+ * fira_crypto_build_phy_sts_index_init() - Build the phy STS index init value
+ * related to the given crypto context.
+ *
+ * @crypto: The context to use to compute the phy STS index init value.
+ * @phy_sts_index_init: The pointer where the computed value will be stored.
+ *
+ * Return: 0 or error.
+ */
+int fira_crypto_build_phy_sts_index_init(struct fira_crypto *crypto,
+					 u32 *phy_sts_index_init);
 
 /**
- * fira_crypto_test() - Autotest for crypto.
+ * fira_crypto_get_sts_params() - Build and get the STS parameters according to
+ * a specific crypto context.
+ *
+ * NOTE: The elements built are the STS value and the STS key. Their construction
+ * depends on the STS config and is described in the FiRa MAC specification.
+ *
+ * @crypto: The context to use to build the STS parameters.
+ * @crypto_sts_index: The crypto STS index to use to build the STS parameters.
+ * @sts_v: The output buffer for STS V.
+ * @sts_v_size: The size of the output buffer for STS V.
+ * @sts_key: The output buffer for STS key.
+ * @sts_key_size: The size of the output buffer for STS key.
  *
  * Return: 0 or error.
  */
-int fira_crypto_test(void);
+int fira_crypto_get_sts_params(struct fira_crypto *crypto, u32 crypto_sts_index,
+			       u8 *sts_v, u32 sts_v_size, u8 *sts_key,
+			       u32 sts_key_size);
 
-#else
+/**
+ * fira_crypto_prepare_decrypt() - Prepare skb for header decryption and verification.
+ * @crypto: The crypto context used to decrypt the frame.
+ * @skb: Buffer containing the frame to decrypt.
+ *
+ * Return: 0 or error.
+ */
+int fira_crypto_prepare_decrypt(struct fira_crypto *crypto,
+				struct sk_buff *skb);
 
-static inline int fira_crypto_test(void)
-{
-	return 0;
-}
+/**
+ * fira_crypto_encrypt_frame() - Encrypt a 802154 frame using a given context.
+ *
+ * NOTE: The src address is given as an argument as it is a part of the nonce needed
+ * to encrypt the frame and it is not present in the 802154 frame.
+ * The length of the header is given because only the payload is encrypted even if
+ * the encryption algorithm needs the whole 802154 frame.
+ * Encryption is done in-place.
+ * When called this function shall increase the size of the skb of
+ * FIRA_CRYPTO_AEAD_AUTHSIZE and set the correct bits in the 802154 frame SCF.
+ *
+ * @crypto: The context to use to encrypt the frame.
+ * @skb: The buffer containing the whole frame, skb->data points to the start of
+ * the 802154 frame header.
+ * @header_len: The length of the 802154 frame header. Can be used to find the
+ * position of the 802154 frame payload relative to skb->data.
+ * @src_short_addr: The short source address attached to the frame.
+ * @crypto_sts_index: The crypto STS index attached to the frame.
+ *
+ * Return: 0 or error.
+ */
+int fira_crypto_encrypt_frame(struct fira_crypto *crypto, struct sk_buff *skb,
+			      int header_len, __le16 src_short_addr,
+			      u32 crypto_sts_index);
 
-#endif
+/**
+ * fira_crypto_decrypt_frame() - Decrypt a 802154 frame using a given context.
+ *
+ * NOTE: The src address is given as an argument as it is a part of the nonce needed
+ * to decrypt the frame and it is not present in the 802154 frame.
+ * The length of the header is given because only the payload is encrypted even if
+ * the encryption algorithm needs the whole 802154 frame.
+ * Decryption is done in-place.
+ * When called, this function shall reduce the
+ * size of the skb of FIRA_CRYPTO_AEAD_AUTHSIZE and verify the correct bits in the
+ * 802154 frame SCF.
+ *
+ * @crypto: The crypto to use to decrypt the frame.
+ * @skb: The buffer containing the whole frame, skb->data points to the start of
+ * the 802154 frame payload.
+ * @header_len: The length of the 802154 frame header. Can be used to find the
+ * start of the 802154 frame payload relative to skb->data.
+ * @src_short_addr: The short source address attached to the frame.
+ * @crypto_sts_index: The crypto STS index attached to the frame.
+ *
+ * Return: 0 or error.
+ */
+int fira_crypto_decrypt_frame(struct fira_crypto *crypto, struct sk_buff *skb,
+			      int header_len, __le16 src_short_addr,
+			      u32 crypto_sts_index);
+
+/**
+ * fira_crypto_encrypt_hie() - Encrypt a 802154 header using a given context.
+ *
+ * @crypto: The crypto to use to encrypt the frame.
+ * @skb: The buffer containing the whole frame, skb->data points to the start of
+ * the 802154 frame header.
+ * @hie_offset: Offset of the FiRa HIE relative to skb->data.
+ * @hie_len: The length of the FiRa HIE.
+ *
+ * Return: 0 or error.
+ */
+int fira_crypto_encrypt_hie(struct fira_crypto *crypto, struct sk_buff *skb,
+			    int hie_offset, int hie_len);
+
+/**
+ * fira_crypto_decrypt_hie() - Decrypt a 802154 header using a given context.
+ *
+ * @crypto: The crypto to use to encrypt the frame.
+ * @skb: The buffer containing the whole frame, skb->data points to the start of
+ * the 802154 frame payload.
+ * @hie_offset: Offset of the FiRa HIE relative to skb->data.
+ * @hie_len: The length of 802154 header.
+ *
+ * Return: 0 or error.
+ */
+int fira_crypto_decrypt_hie(struct fira_crypto *crypto, struct sk_buff *skb,
+			    int hie_offset, int hie_len);
+
+/**
+ * fira_crypto_test() - Autotest for FiRa crypto.
+ *
+ * Return: 0 or error.
+ */
+int fira_crypto_test(void);
 
 #endif /* NET_MCPS802154_FIRA_CRYPTO_H */
diff --git a/mac/fira_frame.c b/mac/fira_frame.c
index d8e2546..09367d5 100644
--- a/mac/fira_frame.c
+++ b/mac/fira_frame.c
@@ -23,6 +23,7 @@
 
 #include "fira_frame.h"
 #include "fira_session.h"
+#include "fira_crypto.h"
 #include "fira_trace.h"
 
 #include <asm/unaligned.h>
@@ -35,62 +36,6 @@
 
 #include "warn_return.h"
 
-#define FIRA_IE_VENDOR_OUI_LEN 3
-#define FIRA_IE_HEADER_PADDING_LEN 8
-#define FIRA_IE_HEADER_SESSION_ID_LEN 4
-#define FIRA_IE_HEADER_STS_INDEX_LEN 4
-#define FIRA_IE_HEADER_LEN                                     \
-	(FIRA_IE_VENDOR_OUI_LEN + FIRA_IE_HEADER_PADDING_LEN + \
-	 FIRA_IE_HEADER_SESSION_ID_LEN + FIRA_IE_HEADER_STS_INDEX_LEN)
-
-#define FIRA_IE_PAYLOAD_CONTROL_LEN(n_mngt) \
-	(FIRA_IE_VENDOR_OUI_LEN + 4 + 4 * (n_mngt))
-#define FIRA_IE_PAYLOAD_MEASUREMENT_REPORT_TYPE1_LEN(round_index_present, \
-						     n_reply_time)        \
-	(FIRA_IE_VENDOR_OUI_LEN + 2 + 2 * (round_index_present) + 4 +     \
-	 6 * (n_reply_time))
-#define FIRA_IE_PAYLOAD_MEASUREMENT_REPORT_TYPE2_LEN(             \
-	round_index_present, reply_time_present, n_reply_time)    \
-	(FIRA_IE_VENDOR_OUI_LEN + 3 + 2 * (round_index_present) + \
-	 4 * (reply_time_present) + 6 * (n_reply_time))
-#define FIRA_IE_PAYLOAD_RESULT_REPORT_LEN(tof_present, aoa_azimuth_present, \
-					  aoa_elevation_present,            \
-					  aoa_fom_present)                  \
-	(FIRA_IE_VENDOR_OUI_LEN + 2 + 4 * (tof_present) +                   \
-	 2 * (aoa_azimuth_present) + 2 * (aoa_elevation_present) +          \
-	 (aoa_fom_present) *                                                \
-		 (1 * (aoa_azimuth_present) + 1 * (aoa_elevation_present)))
-
-#define FIRA_MIC_LEVEL 64
-#define FIRA_MIC_LEN (FIRA_MIC_LEVEL / 8)
-
-/* 3 IE headers in the frame : vendor IE, header terminator and payload. */
-#define FIRA_FRAME_WITHOUT_PAYLOAD_LEN                                        \
-	(IEEE802154_FC_LEN + IEEE802154_SCF_LEN + IEEE802154_SHORT_ADDR_LEN + \
-	 3 * IEEE802154_IE_HEADER_LEN + FIRA_IE_HEADER_LEN + FIRA_MIC_LEN +   \
-	 IEEE802154_FCS_LEN)
-
-#define FIRA_IE_VENDOR_OUI 0x5a18ff
-#define FIRA_IE_HEADER_PADDING 0x08
-
-#define FIRA_MNGT_RANGING_ROLE (1 << 0)
-#define FIRA_MNGT_SLOT_INDEX (0xff << 1)
-#define FIRA_MNGT_SHORT_ADDR (0xffff << 9)
-#define FIRA_MNGT_MESSAGE_ID (0xf << 25)
-#define FIRA_MNGT_STOP (1 << 29)
-#define FIRA_MNGT_RESERVED (0x3U << 30)
-
-#define FIRA_MEASUREMENT_REPORT_CONTROL_HOPPING_MODE (1 << 0)
-#define FIRA_MEASUREMENT_REPORT_CONTROL_ROUND_INDEX_PRESENT (1 << 1)
-#define FIRA_MEASUREMENT_REPORT_CONTROL_N_REPLY_TIME (0x3f << 2)
-
-#define FIRA_MEASUREMENT_REPORT_CONTROL_REPLY_TIME_PRESENT (1 << 0)
-
-#define FIRA_RESULT_REPORT_CONTROL_TOF_PRESENT (1 << 0)
-#define FIRA_RESULT_REPORT_CONTROL_AOA_AZIMUTH_PRESENT (1 << 1)
-#define FIRA_RESULT_REPORT_CONTROL_AOA_ELEVATION_PRESENT (1 << 2)
-#define FIRA_RESULT_REPORT_CONTROL_AOA_FOM_PRESENT (1 << 3)
-
 bool fira_frame_check_n_controlees(const struct fira_session *session,
 				   size_t n_controlees, bool active)
 {
@@ -142,6 +87,7 @@ void fira_frame_header_put(const struct fira_local *local,
 		  (IEEE802154_ADDR_NONE << IEEE802154_FC_SAMODE_SHIFT));
 	u8 *p;
 	int i;
+	u8 *p_hie;
 
 	p = skb_put(skb, IEEE802154_FC_LEN + IEEE802154_SHORT_ADDR_LEN +
 				 IEEE802154_SCF_LEN);
@@ -151,6 +97,7 @@ void fira_frame_header_put(const struct fira_local *local,
 	p += IEEE802154_SHORT_ADDR_LEN;
 	*p = IEEE802154_SCF_NO_FRAME_COUNTER;
 
+	p_hie = skb->data + skb->len;
 	mcps802154_ie_put_begin(skb);
 	p = mcps802154_ie_put_header_ie(skb, IEEE802154_IE_HEADER_VENDOR_ID,
 					FIRA_IE_HEADER_LEN);
@@ -160,8 +107,9 @@ void fira_frame_header_put(const struct fira_local *local,
 		*p++ = FIRA_IE_HEADER_PADDING;
 	put_unaligned_le32(session->id, p);
 	p += FIRA_IE_HEADER_SESSION_ID_LEN;
-	put_unaligned_le32(
-		fira_session_get_round_sts_index(session) + slot->index, p);
+	put_unaligned_le32(fira_sts_get_phy_sts_index(session, slot->index), p);
+	fira_sts_encrypt_hie(local->current_session, skb, p_hie - skb->data,
+			     FIRA_IE_HEADER_LEN + IEEE802154_IE_HEADER_LEN);
 }
 
 static u8 *fira_frame_common_payload_put(struct sk_buff *skb, unsigned int len,
@@ -401,14 +349,18 @@ void fira_frame_rframe_payload_put(struct fira_local *local,
 bool fira_frame_header_check(const struct fira_local *local,
 			     struct sk_buff *skb,
 			     struct mcps802154_ie_get_context *ie_get,
-			     u32 *sts_index, u32 *session_id)
+			     u32 *phy_sts_index, u32 *session_id)
 {
+	struct fira_session *session = local->current_session;
 	u16 fc = (IEEE802154_FC_TYPE_DATA | IEEE802154_FC_SECEN |
 		  IEEE802154_FC_INTRA_PAN | IEEE802154_FC_NO_SEQ |
 		  IEEE802154_FC_IE_PRESENT |
 		  (IEEE802154_ADDR_SHORT << IEEE802154_FC_DAMODE_SHIFT) |
 		  (2 << IEEE802154_FC_VERSION_SHIFT) |
 		  (IEEE802154_ADDR_NONE << IEEE802154_FC_SAMODE_SHIFT));
+	u8 ciphered_hie[FIRA_IE_HEADER_PADDING_LEN +
+			FIRA_IE_HEADER_SESSION_ID_LEN +
+			FIRA_IE_HEADER_STS_INDEX_LEN] = { 0 };
 	bool fira_header_seen = false;
 	int r;
 	u8 *p;
@@ -416,18 +368,15 @@ bool fira_frame_header_check(const struct fira_local *local,
 	p = skb->data;
 	if (!skb_pull(skb, IEEE802154_FC_LEN + IEEE802154_SHORT_ADDR_LEN +
 				   IEEE802154_SCF_LEN) ||
-	    get_unaligned_le16(p) != fc ||
-	    !fira_aead_decrypt_scf_check(
-		    p[IEEE802154_FC_LEN + IEEE802154_SHORT_ADDR_LEN]))
+	    get_unaligned_le16(p) != fc)
 		return false;
 
-	if (fira_aead_decrypt_prepare(skb))
+	if (fira_sts_prepare_decrypt(session, skb))
 		return false;
 
 	for (r = mcps802154_ie_get(skb, ie_get); r == 0 && !ie_get->in_payload;
 	     r = mcps802154_ie_get(skb, ie_get)) {
 		p = skb->data;
-		skb_pull(skb, ie_get->len);
 		ie_get->mlme_len = 0;
 
 		if (ie_get->id == IEEE802154_IE_HEADER_VENDOR_ID &&
@@ -437,21 +386,37 @@ bool fira_frame_header_check(const struct fira_local *local,
 			vendor = get_unaligned_le24(p);
 			p += FIRA_IE_VENDOR_OUI_LEN;
 			if (vendor != FIRA_IE_VENDOR_OUI)
-				continue;
+				goto next;
 			if (fira_header_seen)
-				return false;
+				goto hie_error;
 			if (ie_get->len != FIRA_IE_HEADER_LEN)
-				return false;
+				goto hie_error;
+
+			memcpy(ciphered_hie, skb->data + FIRA_IE_VENDOR_OUI_LEN,
+			       sizeof(ciphered_hie));
+			if (fira_sts_decrypt_hie(
+				    session, skb, FIRA_IE_VENDOR_OUI_LEN,
+				    ie_get->len - FIRA_IE_VENDOR_OUI_LEN))
+				goto hie_error;
 			p += FIRA_IE_HEADER_PADDING_LEN;
 			*session_id = get_unaligned_le32(p);
 			p += FIRA_IE_HEADER_SESSION_ID_LEN;
-			*sts_index = get_unaligned_le32(p);
+			*phy_sts_index = get_unaligned_le32(p);
 			p += FIRA_IE_HEADER_STS_INDEX_LEN;
 			fira_header_seen = true;
+			memcpy(skb->data + FIRA_IE_VENDOR_OUI_LEN, ciphered_hie,
+			       ie_get->len - FIRA_IE_VENDOR_OUI_LEN);
+			memzero_explicit(ciphered_hie, sizeof(ciphered_hie));
 		}
+	next:
+		skb_pull(skb, ie_get->len);
 	}
 
 	return r >= 0 && fira_header_seen;
+
+hie_error:
+	skb_pull(skb, ie_get->len);
+	return false;
 }
 
 static bool fira_frame_control_read(struct fira_local *local, u8 *p,
@@ -895,45 +860,16 @@ bool fira_frame_rframe_payload_check(struct fira_local *local,
 	return r >= 0;
 }
 
-int fira_frame_encrypt(struct fira_local *local, const struct fira_slot *slot,
-		       struct sk_buff *skb)
-{
-	struct fira_session *session = local->current_session;
-	int header_len;
-
-	/* No payload, can not fail. */
-	header_len = mcps802154_ie_put_end(skb, false);
-	WARN_RETURN_ON(header_len < 0, header_len);
-
-	return fira_aead_encrypt(&session->crypto.aead, skb, header_len,
-				 local->src_short_addr, slot->index);
-}
-
-int fira_frame_decrypt(struct fira_local *local, struct fira_session *session,
-		       const struct fira_slot *slot, struct sk_buff *skb,
-		       unsigned int header_len)
-{
-	__le16 src_short_addr;
-
-	if (slot->controller_tx)
-		src_short_addr = local->dst_short_addr;
-	else
-		src_short_addr = slot->controlee->short_addr;
-
-	return fira_aead_decrypt(&session->crypto.aead, skb, header_len,
-				 src_short_addr, slot->index);
-}
-
 struct fira_session *fira_rx_frame_control_header_check(
 	struct fira_local *local, const struct fira_slot *slot,
 	struct sk_buff *skb, struct mcps802154_ie_get_context *ie_get,
-	u32 *sts_index)
+	u32 *phy_sts_index)
 {
 	const struct fira_session *session = local->current_session;
 	struct fira_session *session_found = NULL;
 	u32 session_id;
 
-	if (!fira_frame_header_check(local, skb, ie_get, sts_index,
+	if (!fira_frame_header_check(local, skb, ie_get, phy_sts_index,
 				     &session_id))
 		return NULL;
 	if (session->id == session_id) {
@@ -975,22 +911,26 @@ int fira_frame_header_check_decrypt(struct fira_local *local,
 				    struct mcps802154_ie_get_context *ie_get)
 {
 	struct fira_session *session = local->current_session;
-	unsigned header_len;
-	u32 sts_index;
+	int header_len;
+	__le16 src_short_addr;
+	u32 phy_sts_index;
 	u32 session_id;
 	u8 *header;
 
 	header = skb->data;
 
-	if (!fira_frame_header_check(local, skb, ie_get, &sts_index,
+	if (!fira_frame_header_check(local, skb, ie_get, &phy_sts_index,
 				     &session_id))
 		return -EBADMSG;
 	if (session_id != session->id)
 		return -EBADMSG;
-	if (sts_index !=
-	    fira_session_get_round_sts_index(session) + slot->index)
+
+	if (phy_sts_index != fira_sts_get_phy_sts_index(session, slot->index))
 		return -EBADMSG;
 
 	header_len = skb->data - header;
-	return fira_frame_decrypt(local, session, slot, skb, header_len);
+	src_short_addr = slot->controller_tx ? local->dst_short_addr :
+					       slot->controlee->short_addr;
+	return fira_sts_decrypt_frame(session, skb, header_len, src_short_addr,
+				      slot->index);
 }
diff --git a/mac/fira_frame.h b/mac/fira_frame.h
index 1638e06..f37620f 100644
--- a/mac/fira_frame.h
+++ b/mac/fira_frame.h
@@ -33,6 +33,62 @@ struct sk_buff;
 struct mcps802154_ie_get_context;
 struct fira_session_params;
 
+#define FIRA_IE_VENDOR_OUI_LEN 3
+#define FIRA_IE_HEADER_PADDING_LEN 8
+#define FIRA_IE_HEADER_SESSION_ID_LEN 4
+#define FIRA_IE_HEADER_STS_INDEX_LEN 4
+#define FIRA_IE_HEADER_LEN                                     \
+	(FIRA_IE_VENDOR_OUI_LEN + FIRA_IE_HEADER_PADDING_LEN + \
+	 FIRA_IE_HEADER_SESSION_ID_LEN + FIRA_IE_HEADER_STS_INDEX_LEN)
+
+#define FIRA_IE_PAYLOAD_CONTROL_LEN(n_mngt) \
+	(FIRA_IE_VENDOR_OUI_LEN + 4 + 4 * (n_mngt))
+#define FIRA_IE_PAYLOAD_MEASUREMENT_REPORT_TYPE1_LEN(round_index_present, \
+						     n_reply_time)        \
+	(FIRA_IE_VENDOR_OUI_LEN + 2 + 2 * (round_index_present) + 4 +     \
+	 6 * (n_reply_time))
+#define FIRA_IE_PAYLOAD_MEASUREMENT_REPORT_TYPE2_LEN(             \
+	round_index_present, reply_time_present, n_reply_time)    \
+	(FIRA_IE_VENDOR_OUI_LEN + 3 + 2 * (round_index_present) + \
+	 4 * (reply_time_present) + 6 * (n_reply_time))
+#define FIRA_IE_PAYLOAD_RESULT_REPORT_LEN(tof_present, aoa_azimuth_present, \
+					  aoa_elevation_present,            \
+					  aoa_fom_present)                  \
+	(FIRA_IE_VENDOR_OUI_LEN + 2 + 4 * (tof_present) +                   \
+	 2 * (aoa_azimuth_present) + 2 * (aoa_elevation_present) +          \
+	 (aoa_fom_present) *                                                \
+		 (1 * (aoa_azimuth_present) + 1 * (aoa_elevation_present)))
+
+#define FIRA_MIC_LEVEL 64
+#define FIRA_MIC_LEN (FIRA_MIC_LEVEL / 8)
+
+/* 3 IE headers in the frame : vendor IE, header terminator and payload. */
+#define FIRA_FRAME_WITHOUT_PAYLOAD_LEN                                        \
+	(IEEE802154_FC_LEN + IEEE802154_SCF_LEN + IEEE802154_SHORT_ADDR_LEN + \
+	 3 * IEEE802154_IE_HEADER_LEN + FIRA_IE_HEADER_LEN + FIRA_MIC_LEN +   \
+	 IEEE802154_FCS_LEN)
+
+#define FIRA_IE_VENDOR_OUI 0x5a18ff
+#define FIRA_IE_HEADER_PADDING 0x08
+
+#define FIRA_MNGT_RANGING_ROLE (1 << 0)
+#define FIRA_MNGT_SLOT_INDEX (0xff << 1)
+#define FIRA_MNGT_SHORT_ADDR (0xffff << 9)
+#define FIRA_MNGT_MESSAGE_ID (0xf << 25)
+#define FIRA_MNGT_STOP (1 << 29)
+#define FIRA_MNGT_RESERVED (0x3U << 30)
+
+#define FIRA_MEASUREMENT_REPORT_CONTROL_HOPPING_MODE (1 << 0)
+#define FIRA_MEASUREMENT_REPORT_CONTROL_ROUND_INDEX_PRESENT (1 << 1)
+#define FIRA_MEASUREMENT_REPORT_CONTROL_N_REPLY_TIME (0x3f << 2)
+
+#define FIRA_MEASUREMENT_REPORT_CONTROL_REPLY_TIME_PRESENT (1 << 0)
+
+#define FIRA_RESULT_REPORT_CONTROL_TOF_PRESENT (1 << 0)
+#define FIRA_RESULT_REPORT_CONTROL_AOA_AZIMUTH_PRESENT (1 << 1)
+#define FIRA_RESULT_REPORT_CONTROL_AOA_ELEVATION_PRESENT (1 << 2)
+#define FIRA_RESULT_REPORT_CONTROL_AOA_FOM_PRESENT (1 << 3)
+
 /**
  * fira_frame_check_n_controlees() - Check the number of wanted
  * controlees.
@@ -106,7 +162,7 @@ void fira_frame_rframe_payload_put(struct fira_local *local,
  * @local: FiRa context.
  * @skb: Frame buffer.
  * @ie_get: Context used to read IE, must be zero initialized.
- * @sts_index: STS index read from header.
+ * @phy_sts_index: STS index read from header.
  * @session_id: Session id read from header.
  *
  * Return: true if header is correct.
@@ -114,7 +170,7 @@ void fira_frame_rframe_payload_put(struct fira_local *local,
 bool fira_frame_header_check(const struct fira_local *local,
 			     struct sk_buff *skb,
 			     struct mcps802154_ie_get_context *ie_get,
-			     u32 *sts_index, u32 *session_id);
+			     u32 *phy_sts_index, u32 *session_id);
 
 /**
  * fira_frame_control_payload_check() - Check FiRa frame payload for a control
@@ -177,45 +233,20 @@ bool fira_frame_rframe_payload_check(struct fira_local *local,
 				     struct mcps802154_ie_get_context *ie_get);
 
 /**
- * fira_frame_encrypt() - Terminate a frame and encrypt.
- * @local: FiRa context.
- * @slot: Corresponding slot.
- * @skb: Frame buffer.
- *
- * Return: 0 or error.
- */
-int fira_frame_encrypt(struct fira_local *local, const struct fira_slot *slot,
-		       struct sk_buff *skb);
-
-/**
- * fira_frame_decrypt() - Decrypt payload.
- * @local: FiRa context.
- * @session: Session.
- * @slot: Corresponding slot.
- * @skb: Frame buffer, with header in front of data.
- * @header_len: Length of the MAC header, used for authentication.
- *
- * Return: 0 or error, -EBADMSG if not authenticated.
- */
-int fira_frame_decrypt(struct fira_local *local, struct fira_session *session,
-		       const struct fira_slot *slot, struct sk_buff *skb,
-		       unsigned int header_len);
-
-/**
  * fira_rx_frame_control_header_check() - Check control frame and consume
  * header.
  * @local: FiRa context.
  * @slot: Corresponding slot.
  * @skb: Frame buffer.
  * @ie_get: Context used to read IE, must be zero initialized.
- * @sts_index: STS index received.
+ * @phy_sts_index: STS index received.
  *
  * Return: Session context or NULL.
  */
 struct fira_session *fira_rx_frame_control_header_check(
 	struct fira_local *local, const struct fira_slot *slot,
 	struct sk_buff *skb, struct mcps802154_ie_get_context *ie_get,
-	u32 *sts_index);
+	u32 *phy_sts_index);
 
 /**
  * fira_frame_header_check_decrypt() - Check and consume header, and decrypt
diff --git a/mac/fira_region.c b/mac/fira_region.c
index 6e89bb1..9e4bf53 100644
--- a/mac/fira_region.c
+++ b/mac/fira_region.c
@@ -32,10 +32,11 @@
 #include "fira_region_call.h"
 #include "fira_access.h"
 #include "fira_session.h"
-
+#include "fira_crypto.h"
 #include "warn_return.h"
 
 static struct mcps802154_region_ops fira_region_ops;
+static bool do_crypto_selftest_on_module_init;
 
 static void fira_report_event(struct work_struct *work)
 {
@@ -68,6 +69,7 @@ static struct mcps802154_region *fira_open(struct mcps802154_llhw *llhw)
 	INIT_WORK(&local->report_work, fira_report_event);
 	/* FIXME: Hack to simplify unit test, which is borderline. */
 	local->block_duration_rx_margin_ppm = UWB_BLOCK_DURATION_MARGIN_PPM;
+	fira_crypto_init(NULL);
 	return &local->region;
 }
 
@@ -459,10 +461,8 @@ void fira_check_all_missed_ranging(struct fira_local *local,
 
 int __init fira_region_init(void)
 {
-	int r;
-
-	r = fira_crypto_test();
-	WARN_RETURN(r);
+	if (do_crypto_selftest_on_module_init)
+		WARN_RETURN(fira_crypto_test());
 
 	return mcps802154_region_register(&fira_region_ops);
 }
@@ -472,6 +472,7 @@ void __exit fira_region_exit(void)
 	mcps802154_region_unregister(&fira_region_ops);
 }
 
+module_param_named(crypto_selftest, do_crypto_selftest_on_module_init, bool, 0644);
 module_init(fira_region_init);
 module_exit(fira_region_exit);
 
diff --git a/mac/fira_region_call.c b/mac/fira_region_call.c
index cb9f462..8e324aa 100644
--- a/mac/fira_region_call.c
+++ b/mac/fira_region_call.c
@@ -34,6 +34,7 @@
 #include "fira_access.h"
 #include "fira_region_call.h"
 #include "fira_trace.h"
+#include "fira_sts.h"
 
 static const struct nla_policy fira_call_nla_policy[FIRA_CALL_ATTR_MAX + 1] = {
 	[FIRA_CALL_ATTR_SESSION_ID] = { .type = NLA_U32 },
@@ -97,7 +98,7 @@ static const struct nla_policy fira_session_param_nla_policy[FIRA_SESSION_PARAM_
 		NLA_POLICY_MAX(NLA_U8, FIRA_BOOLEAN_MAX),
 	[FIRA_SESSION_PARAM_ATTR_MEASUREMENT_SEQUENCE] = { .type = NLA_NESTED_ARRAY },
 	[FIRA_SESSION_PARAM_ATTR_STS_CONFIG] =
-		NLA_POLICY_MAX(NLA_U8, FIRA_STS_CONFIG_DYNAMIC_INDIVIDUAL_KEY),
+		NLA_POLICY_MAX(NLA_U8, FIRA_STS_MODE_PROVISIONED_INDIVIDUAL_KEY),
 	[FIRA_SESSION_PARAM_ATTR_SUB_SESSION_ID] = { .type = NLA_U32 },
 	[FIRA_SESSION_PARAM_ATTR_VUPPER64] =
 		NLA_POLICY_EXACT_LEN(FIRA_VUPPER64_SIZE),
@@ -431,6 +432,7 @@ static int fira_session_set_parameters(struct fira_local *local, u32 session_id,
 	r = check_parameter_proximity_range(&session->params, attrs);
 	if (r)
 		return r;
+
 	if (attrs[FIRA_SESSION_PARAM_ATTR_MEASUREMENT_SEQUENCE]) {
 		r = fira_session_params_set_measurement_sequence(
 			attrs[FIRA_SESSION_PARAM_ATTR_MEASUREMENT_SEQUENCE],
@@ -508,6 +510,15 @@ static int fira_session_set_parameters(struct fira_local *local, u32 session_id,
 	}
 	/* STS and crypto parameters. */
 	PMEMCPY(VUPPER64, vupper64);
+	if (attrs[FIRA_SESSION_PARAM_ATTR_SESSION_KEY]) {
+		struct nlattr *attr =
+			attrs[FIRA_SESSION_PARAM_ATTR_SESSION_KEY];
+		memcpy(p->session_key, nla_data(attr), nla_len(attr));
+		p->session_key_len = nla_len(attr);
+	}
+	P(STS_CONFIG, sts_config, u8, x);
+	P(KEY_ROTATION, key_rotation, u8, x);
+	P(KEY_ROTATION_RATE, key_rotation_rate, u8, x);
 	/* Report parameters. */
 	P(AOA_RESULT_REQ, aoa_result_req, u8, !!x);
 	P(REPORT_TOF, report_tof, u8, !!x);
@@ -519,7 +530,7 @@ static int fira_session_set_parameters(struct fira_local *local, u32 session_id,
 	P(DATA_VENDOR_OUI, data_vendor_oui, u32, x);
 
 	PMEMNCPY(DATA_PAYLOAD, data_payload, data_payload_len);
-	/* Increment sequence number if a new data is received. */
+	/* Increment payload sequence number if a new data is received. */
 	if (attrs[FIRA_SESSION_PARAM_ATTR_DATA_PAYLOAD])
 		p->data_payload_seq++;
 	/* Diagnostics */
@@ -746,6 +757,9 @@ static int fira_session_get_parameters(struct fira_local *local, u32 session_id)
 		goto nla_put_failure;
 	/* STS and crypto parameters. */
 	PMEMCPY(VUPPER64, vupper64);
+	P(STS_CONFIG, sts_config, u8, x);
+	P(KEY_ROTATION, key_rotation, u8, x);
+	P(KEY_ROTATION_RATE, key_rotation_rate, u8, x);
 	/* Report parameters. */
 	P(AOA_RESULT_REQ, aoa_result_req, u8, !!x);
 	P(REPORT_TOF, report_tof, u8, !!x);
@@ -999,6 +1013,7 @@ int fira_get_capabilities(struct fira_local *local,
 	struct sk_buff *msg;
 	struct nlattr *capabilities;
 	u64 hw_flags = local->llhw->flags;
+	u32 sts_caps = fira_crypto_get_capabilities();
 
 	if (!info)
 		return 0;
@@ -1032,6 +1047,11 @@ int fira_get_capabilities(struct fira_local *local,
 		if (hw_flags & (hw_flag)) \
 			F(name);          \
 	} while (0)
+#define S(mode)                                             \
+	do {                                                \
+		if (sts_caps & (1 << FIRA_STS_MODE_##mode)) \
+			F(STS_##mode);                      \
+	} while (0)
 
 	/* Main session capabilities. */
 	P(FIRA_PHY_VERSION_RANGE, u32, 0x01010101);
@@ -1046,8 +1066,6 @@ int fira_get_capabilities(struct fira_local *local,
 	/* Behaviour. */
 	F(ROUND_HOPPING);
 	F(BLOCK_STRIDING);
-	/* STS and crypto capabilities. */
-	F(STS_CONFIG_STATIC);
 	/* Radio. */
 	P(CHANNEL_NUMBER, u16,
 	  local->llhw->hw->phy->supported
@@ -1080,6 +1098,12 @@ int fira_get_capabilities(struct fira_local *local,
 	/* Antenna. */
 	P(RX_ANTENNA_PAIRS, u32, local->llhw->rx_antenna_pairs);
 	P(TX_ANTENNAS, u32, local->llhw->tx_antennas);
+	/* STS and crypto capabilities. */
+	S(STATIC);
+	S(DYNAMIC);
+	S(DYNAMIC_INDIVIDUAL_KEY);
+	S(PROVISIONED);
+	S(PROVISIONED_INDIVIDUAL_KEY);
 	/* Report. */
 	C(AOA_AZIMUTH, MCPS802154_LLHW_AOA_AZIMUTH);
 	C(AOA_AZIMUTH_FULL, MCPS802154_LLHW_AOA_AZIMUTH_FULL);
diff --git a/mac/fira_session.c b/mac/fira_session.c
index 8373d0d..cca4252 100644
--- a/mac/fira_session.c
+++ b/mac/fira_session.c
@@ -32,7 +32,6 @@
 #include <net/fira_region_nl.h>
 
 #include "fira_session.h"
-#include "fira_crypto.h"
 #include "fira_round_hopping_sequence.h"
 #include "fira_access.h"
 #include "fira_frame.h"
@@ -551,6 +550,7 @@ struct fira_session *fira_session_new(struct fira_local *local, u32 session_id)
 	params->round_hopping = false;
 	params->priority = FIRA_PRIORITY_DEFAULT;
 	params->sts_length = FIRA_STS_LENGTH_64;
+	params->sts_config = FIRA_STS_MODE_STATIC;
 	params->rframe_config = FIRA_RFRAME_CONFIG_SP3;
 	params->preamble_duration = FIRA_PREAMBULE_DURATION_64;
 	params->sfd_id = FIRA_SFD_ID_2;
@@ -605,11 +605,6 @@ void fira_session_free(struct fira_local *local, struct fira_session *session)
 	}
 	fira_session_fsm_uninit(local, session);
 	fira_round_hopping_sequence_destroy(session);
-	fira_aead_destroy(&session->crypto.aead);
-	/*
-	 * The session structure contains the Crypto context. This needs to be
-	 * cleared.
-	 */
 	kfree(session->rx_ctx[0]);
 	kfree_sensitive(session);
 }
diff --git a/mac/fira_session.h b/mac/fira_session.h
index b23c8ab..7a2241e 100644
--- a/mac/fira_session.h
+++ b/mac/fira_session.h
@@ -26,6 +26,7 @@
 
 #include "fira_session_fsm.h"
 #include "fira_region.h"
+#include "fira_sts.h"
 #include "fira_crypto.h"
 #include "fira_round_hopping_crypto_impl.h"
 
@@ -66,7 +67,7 @@ struct fira_controlee {
 	 * @sub_session_key_len: Length of the sub-session key used by
 	 * the controlee.
 	 */
-	__u16 sub_session_key_len;
+	__u8 sub_session_key_len;
 	/**
 	 * @sub_session_key: Sub-session key used by the controlee.
 	 */
@@ -131,8 +132,12 @@ struct fira_session_params {
 	enum fira_prf_mode prf_mode;
 	enum fira_phr_data_rate phr_data_rate;
 	/* STS and crypto. */
-	enum fira_sts_config sts_config;
+	enum fira_sts_mode sts_config;
 	u8 vupper64[FIRA_VUPPER64_SIZE];
+	u8 session_key_len;
+	u8 session_key[FIRA_KEY_SIZE_MAX];
+	bool key_rotation;
+	u8 key_rotation_rate;
 	bool aoa_result_req;
 	bool report_tof;
 	bool report_aoa_azimuth;
@@ -231,10 +236,6 @@ struct fira_session {
 	 */
 	int next_round_index;
 	/**
-	 * @sts_index: STS index value on the last access.
-	 */
-	u32 sts_index;
-	/**
 	* @stop_request: Session has been requested to stop.
 	*/
 	bool stop_request;
@@ -253,10 +254,7 @@ struct fira_session {
 	 * Counter reset on ranging round success.
 	 */
 	int n_ranging_round_retry;
-	/**
-	 * @crypto: Crypto context.
-	 */
-	struct fira_crypto crypto;
+
 	/**
 	 * @round_hopping_sequence: Round hopping sequence generation context.
 	 */
@@ -351,6 +349,25 @@ struct fira_session {
 	 * @rx_ctx: Custom rx context for all controlees.
 	 */
 	void *rx_ctx[FIRA_CONTROLEES_MAX];
+	/**
+	 * @crypto: crypto related variables.
+	 */
+	struct fira_crypto *crypto;
+	/**
+	 * @sts: sts related variables.
+	 */
+	struct {
+		/**
+		 * @phy_sts_index_init: Initial phy_sts_index deduced at context init.
+		 */
+		u32 phy_sts_index_init;
+
+		/**
+		 * @last_rotation_block_index: index to the last block where the
+		 * rotation occurred.
+		 */
+		u32 last_rotation_block_index;
+	} sts;
 };
 
 /**
@@ -516,21 +533,6 @@ bool fira_session_is_ready(const struct fira_local *local,
 			   const struct fira_session *session);
 
 /**
- * fira_session_get_round_sts_index() - Get current round's STS index.
- * @session: Session.
- *
- * Return: The STS of the first slot of the current round.
- */
-static inline u32
-fira_session_get_round_sts_index(const struct fira_session *session)
-{
-	const struct fira_session_params *p = &session->params;
-
-	return session->sts_index +
-	       session->round_index * p->round_duration_slots;
-}
-
-/**
  * fira_session_get_meas_seq_step() - Get current measurement step.
  * @session: Session.
  *
diff --git a/mac/fira_session_fsm.c b/mac/fira_session_fsm.c
index 3a9a086..3d1d478 100644
--- a/mac/fira_session_fsm.c
+++ b/mac/fira_session_fsm.c
@@ -76,9 +76,8 @@ fira_session_get_state_id(const struct fira_session *session)
 int fira_session_fsm_check_parameters(const struct fira_session *session,
 				      struct nlattr **attrs)
 {
-	if (session->state->check_parameters)
-		return session->state->check_parameters(session, attrs);
-	return 0;
+	WARN_ON(!session->state->check_parameters);
+	return session->state->check_parameters(session, attrs);
 }
 
 void fira_session_fsm_parameters_updated(struct fira_local *local,
diff --git a/mac/fira_session_fsm_active.c b/mac/fira_session_fsm_active.c
index 75a1c1b..feff6c9 100644
--- a/mac/fira_session_fsm_active.c
+++ b/mac/fira_session_fsm_active.c
@@ -247,12 +247,9 @@ static void forward_to_next_ranging(struct fira_session *session, int n_ranging)
 	int blocks_per_ranging = session->block_stride_len + 1;
 	int add_blocks = n_ranging * blocks_per_ranging;
 	int duration_dtu = add_blocks * params->block_duration_dtu;
-	int slots_per_block =
-		params->block_duration_dtu / params->slot_duration_dtu;
 
 	session->block_index += add_blocks;
 	session->block_start_dtu += duration_dtu;
-	session->sts_index += add_blocks * slots_per_block;
 	session->n_ranging_round_retry += n_ranging;
 }
 
@@ -277,21 +274,25 @@ static void ranging_round_done(struct fira_local *local,
 	case FIRA_DEVICE_TYPE_CONTROLLER:
 		/* Update controlee's states between two ranging round. */
 		fira_session_update_controlees(local, session);
+		fira_sts_rotate_keys(session);
 		break;
 	case FIRA_DEVICE_TYPE_CONTROLEE:
 		/* Did the controlee's access lose the synchronisation? */
 		session->controlee.synchronised =
 			is_controlee_synchronised(local, session);
+		if (session->controlee.synchronised)
+			fira_sts_rotate_keys(session);
 		break;
 	}
 
 	fira_session_report(local, session, report_info);
 
-	if (report_info->stopped)
+	if (report_info->stopped) {
 		fira_session_fsm_change_state(local, session,
 					      &fira_session_fsm_idle);
-	else
+	} else {
 		forward_to_next_ranging(session, 1);
+	}
 }
 
 static void fira_session_fsm_active_enter(struct fira_local *local,
@@ -321,6 +322,7 @@ static void fira_session_fsm_active_enter(struct fira_local *local,
 static void fira_session_fsm_active_leave(struct fira_local *local,
 					  struct fira_session *session)
 {
+	fira_sts_deinit(session);
 	list_move(&session->entry, &local->inactive_sessions);
 	fira_session_restart_controlees(session);
 }
diff --git a/mac/fira_session_fsm_idle.c b/mac/fira_session_fsm_idle.c
index d8235cf..fcbcf58 100644
--- a/mac/fira_session_fsm_idle.c
+++ b/mac/fira_session_fsm_idle.c
@@ -63,6 +63,7 @@ static int fira_session_fsm_idle_start(struct fira_local *local,
 	u32 now_dtu;
 	int r;
 	int i;
+	int slot_duration_us;
 
 	trace_region_fira_session_params(session, params);
 	for (i = 0; i < params->meas_seq.n_steps; i++) {
@@ -71,13 +72,9 @@ static int fira_session_fsm_idle_start(struct fira_local *local,
 		step = &params->meas_seq.steps[i];
 		trace_region_fira_session_meas_seq_params(session, step, i);
 	}
+	slot_duration_us = (session->params.slot_duration_dtu * 1000) /
+			   (local->llhw->dtu_freq_hz / 1000);
 
-	r = fira_crypto_derive_per_session(local, session);
-	if (r)
-		return r;
-	r = fira_crypto_derive_per_rotation(local, session, 0);
-	if (r)
-		return r;
 	r = mcps802154_get_current_timestamp_dtu(local->llhw, &now_dtu);
 	if (r)
 		return r;
@@ -86,10 +83,15 @@ static int fira_session_fsm_idle_start(struct fira_local *local,
 	session->event_portid = info->snd_portid;
 	session->block_start_valid = false;
 	session->block_index = 0;
-	session->sts_index = session->crypto.sts_index_init;
+	session->round_index = 0;
 	session->controlee.synchronised = false;
 	session->last_access_timestamp_dtu = now_dtu;
 
+	r = fira_sts_init(session, slot_duration_us,
+			  mcps802154_get_current_channel(local->llhw));
+	if (r)
+		return r;
+
 	/* Set radio parameters. */
 	switch (params->prf_mode) {
 	case FIRA_PRF_MODE_BPRF:
@@ -128,9 +130,40 @@ static int fira_session_fsm_idle_start(struct fira_local *local,
 	return 0;
 }
 
+/* not static: shared with init state */
+int fira_session_fsm_idle_check_parameters(const struct fira_session *session,
+					   struct nlattr **attrs)
+{
+	enum fira_session_param_attrs i;
+
+	for (i = FIRA_SESSION_PARAM_ATTR_UNSPEC + 1;
+	     i <= FIRA_SESSION_PARAM_ATTR_MAX; i++) {
+		const struct nlattr *attr = attrs[i];
+
+		if (!attr)
+			/* Attribute not provided. */
+			continue;
+
+		switch (i) {
+		case FIRA_SESSION_PARAM_ATTR_STS_CONFIG:
+			if (fira_crypto_get_capabilities() &
+			    (1 << nla_get_u8(attr)))
+				continue;
+			else
+				return -EINVAL;
+			break;
+		/* no check on other parameters */
+		default:
+			break;
+		}
+	}
+	return 0;
+}
+
 const struct fira_session_fsm_state fira_session_fsm_idle = {
 	.id = FIRA_SESSION_STATE_ID_IDLE,
 	.parameters_updated = fira_session_fsm_idle_parameters_updated,
 	.controlee_list_updated = fira_session_fsm_idle_controlee_list_updated,
 	.start = fira_session_fsm_idle_start,
+	.check_parameters = fira_session_fsm_idle_check_parameters,
 };
diff --git a/mac/fira_session_fsm_init.c b/mac/fira_session_fsm_init.c
index c277721..5c12e62 100644
--- a/mac/fira_session_fsm_init.c
+++ b/mac/fira_session_fsm_init.c
@@ -65,9 +65,13 @@ fira_session_fsm_init_controlee_list_updated(struct fira_local *local,
 	}
 }
 
+int fira_session_fsm_idle_check_parameters(const struct fira_session *session,
+					   struct nlattr **attrs);
+
 const struct fira_session_fsm_state fira_session_fsm_init = {
 	.id = FIRA_SESSION_STATE_ID_INIT,
 	.enter = fira_session_fsm_init_enter,
 	.parameters_updated = fira_session_fsm_init_parameters_updated,
 	.controlee_list_updated = fira_session_fsm_init_controlee_list_updated,
+	.check_parameters = fira_session_fsm_idle_check_parameters,
 };
diff --git a/mac/fira_sts.c b/mac/fira_sts.c
new file mode 100644
index 0000000..20e8142
--- /dev/null
+++ b/mac/fira_sts.c
@@ -0,0 +1,264 @@
+/*
+* This file is part of the UWB stack for linux.
+*
+* Copyright (c) 2022 Qorvo US, Inc.
+*
+* This software is provided under the GNU General Public License, version 2
+* (GPLv2), as well as under a Qorvo commercial license.
+*
+* You may choose to use this software under the terms of the GPLv2 License,
+* version 2 ("GPLv2"), as published by the Free Software Foundation.
+* You should have received a copy of the GPLv2 along with this program.  If
+* not, see <http://www.gnu.org/licenses/>.
+* not, see <http://www.gnu.org/licenses/>.
+*
+* This program is distributed under the GPLv2 in the hope that it will be
+* useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GPLv2 for more
+* details.
+*
+* If you cannot meet the requirements of the GPLv2, you may not use this
+* software for any purpose without first obtaining a commercial license from
+* Qorvo. Please contact Qorvo to inquire about licensing terms.
+*/
+
+#include "fira_session.h"
+#include "fira_sts.h"
+#include "fira_crypto.h"
+
+#include <asm/unaligned.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+
+#define FIRA_CONCATENATED_PARAMS_SIZE 17
+
+/**
+* fira_sts_concatenate_params() - Concatenate the session parameters to compute
+* the digest.
+* @session: FiRa session for which we need to concatenate the parameter.
+* @channel: Channel parameter coming from the LLHW.
+* @slot_duration_us: duration of a FiRa slot in us (according to session config).
+* @concat_params: output buffer.
+* @concat_params_size: size of the output buffer.
+*/
+static void
+fira_sts_concatenate_params(const struct fira_session *session,
+			    const struct mcps802154_channel *channel,
+			    int slot_duration_us, u8 *concat_params,
+			    u8 concat_params_size)
+{
+	u8 *p;
+
+	p = concat_params;
+	*p++ = session->params.ranging_round_usage;
+	*p++ = session->params.sts_config;
+	*p++ = session->params.multi_node_mode;
+	*p++ = session->params.channel_number != 0 ?
+		       session->params.channel_number :
+		       channel->channel;
+	put_unaligned_be16(slot_duration_us, p);
+	p += sizeof(u16);
+	*p++ = session->params.mac_fcs_type;
+	*p++ = session->params.rframe_config;
+	*p++ = session->params.preamble_code_index != 0 ?
+		       session->params.preamble_code_index :
+		       channel->preamble_code;
+	*p++ = session->params.sfd_id;
+	*p++ = session->params.psdu_data_rate;
+	*p++ = session->params.preamble_duration;
+	*p++ = 0x03;
+	put_unaligned_be32(session->id, p);
+}
+
+/**
+* fira_sts_get_crypto_sts_index() - Compute the current crypto STS index.
+* @session: The session for which the crypto sts index is needed
+* @slot_index: index to the current slot.
+*
+* Return: crypto_sts_index depending on the sts mode.
+*/
+static u32 fira_sts_get_crypto_sts_index(struct fira_session *session,
+					 u32 slot_index)
+{
+	if (session->params.sts_config == FIRA_STS_MODE_STATIC) {
+		return slot_index;
+	}
+	return fira_sts_get_phy_sts_index(session, slot_index);
+}
+
+int fira_sts_init(struct fira_session *session, int slot_duration_us,
+		  const struct mcps802154_channel *current_channel)
+{
+	int r = 0;
+
+	u32 crypto_sts_index;
+	u8 concat_params[FIRA_CONCATENATED_PARAMS_SIZE];
+	struct fira_crypto_params crypto_params;
+
+	fira_sts_concatenate_params(session, current_channel, slot_duration_us,
+				    concat_params, sizeof(concat_params));
+
+	crypto_params.session_id = session->id;
+	crypto_params.sts_config = session->params.sts_config;
+	crypto_params.concat_params = concat_params;
+	crypto_params.concat_params_size = sizeof(concat_params);
+	crypto_params.vupper64 = session->params.vupper64;
+	crypto_params.prov_session_key = session->params.session_key;
+	crypto_params.prov_session_key_len = session->params.session_key_len;
+
+	r = fira_crypto_context_init(&crypto_params, &session->crypto);
+	if (r)
+		return r;
+
+	r = fira_crypto_build_phy_sts_index_init(
+		session->crypto, &session->sts.phy_sts_index_init);
+	if (r)
+		goto error_out;
+
+	session->sts.last_rotation_block_index = 0;
+	crypto_sts_index = fira_sts_get_crypto_sts_index(session, 0);
+	r = fira_crypto_rotate_elements(session->crypto, crypto_sts_index);
+	if (r)
+		goto error_out;
+
+	return 0;
+
+error_out:
+	fira_crypto_context_deinit(session->crypto);
+	session->crypto = NULL;
+	return r;
+}
+
+void fira_sts_deinit(struct fira_session *session)
+{
+	if (session->crypto)
+		fira_crypto_context_deinit(session->crypto);
+}
+
+int fira_sts_rotate_keys(struct fira_session *session)
+{
+	const struct fira_session_params *params = &session->params;
+	u32 rotation_period;
+	u32 n_slots_per_block;
+	u32 crypto_sts_index;
+	bool time_to_rotate;
+	bool rotation_after_resync;
+	int rotation_block_index;
+	int r = 0;
+
+	if (params->sts_config != FIRA_STS_MODE_STATIC &&
+	    params->key_rotation) {
+		/* Key rotation is triggered after rotation_period expires or
+		 * by a resync at controlee side.
+		 */
+		rotation_period = (1 << params->key_rotation_rate);
+		time_to_rotate = (session->block_index -
+				  session->sts.last_rotation_block_index) >=
+				 rotation_period;
+		rotation_after_resync = session->block_index <
+					session->sts.last_rotation_block_index;
+		if (time_to_rotate || rotation_after_resync) {
+			n_slots_per_block = (params->block_duration_dtu /
+					     params->slot_duration_dtu);
+			/* Remove extra blocks following resynchronization
+			 * rotation_block_index should be power of 2 and multiple of
+			 * rotation_period.
+			 *
+			 * crypto_sts_index shall be calculated at the block triggering rotation.
+			 */
+			rotation_block_index =
+				session->block_index -
+				(session->block_index % rotation_period);
+			crypto_sts_index =
+				session->sts.phy_sts_index_init +
+				(rotation_block_index * n_slots_per_block);
+			r = fira_crypto_rotate_elements(session->crypto,
+							crypto_sts_index);
+			session->sts.last_rotation_block_index =
+				rotation_block_index;
+		}
+	}
+
+	return r;
+}
+
+int fira_sts_get_sts_params(struct fira_session *session, u32 slot_index,
+			    u8 *sts_v, u32 sts_v_size, u8 *sts_key,
+			    u32 sts_key_size)
+{
+	u32 crypto_sts_index =
+		fira_sts_get_crypto_sts_index(session, slot_index);
+	return fira_crypto_get_sts_params(session->crypto, crypto_sts_index,
+					  sts_v, sts_v_size, sts_key,
+					  sts_key_size);
+}
+
+u32 fira_sts_get_phy_sts_index(const struct fira_session *session,
+			       const u32 slot_index)
+{
+	return session->sts.phy_sts_index_init +
+	       (session->block_index * (session->params.block_duration_dtu /
+					session->params.slot_duration_dtu)) +
+	       (session->round_index * session->params.round_duration_slots) +
+	       slot_index;
+}
+
+int fira_sts_convert_phy_sts_idx_to_time_indexes(
+	const struct fira_session *session, const u32 current_phy_sts_index,
+	u32 *block_idx, u32 *round_idx, u32 *slot_idx)
+{
+	u32 remaining_slots, absolute_phy_sts_index, n_slots_per_block;
+	const struct fira_session_params *params = &session->params;
+
+	if (current_phy_sts_index < session->sts.phy_sts_index_init)
+		return -EINVAL;
+	n_slots_per_block =
+		params->block_duration_dtu / params->slot_duration_dtu;
+	absolute_phy_sts_index =
+		current_phy_sts_index - session->sts.phy_sts_index_init;
+	*block_idx = (u32)(absolute_phy_sts_index / n_slots_per_block);
+	remaining_slots = absolute_phy_sts_index % n_slots_per_block;
+	*round_idx = (u32)(remaining_slots / params->round_duration_slots);
+	*slot_idx = remaining_slots % params->round_duration_slots;
+
+	return 0;
+}
+
+int fira_sts_prepare_decrypt(struct fira_session *session, struct sk_buff *skb)
+{
+	return fira_crypto_prepare_decrypt(session->crypto, skb);
+}
+
+int fira_sts_encrypt_frame(struct fira_session *session, struct sk_buff *skb,
+			   int header_len, __le16 src_short_addr,
+			   u32 slot_index)
+{
+	u32 crypto_sts_index =
+		fira_sts_get_crypto_sts_index(session, slot_index);
+	return fira_crypto_encrypt_frame(session->crypto, skb, header_len,
+					 src_short_addr, crypto_sts_index);
+}
+
+int fira_sts_decrypt_frame(struct fira_session *session, struct sk_buff *skb,
+			   int header_len, __le16 src_short_addr,
+			   u32 slot_index)
+{
+	u32 crypto_sts_index =
+		fira_sts_get_crypto_sts_index(session, slot_index);
+	return fira_crypto_decrypt_frame(session->crypto, skb, header_len,
+					 src_short_addr, crypto_sts_index);
+}
+
+int fira_sts_decrypt_hie(struct fira_session *session, struct sk_buff *skb,
+			 int hie_offset, int hie_len)
+{
+	return fira_crypto_decrypt_hie(session->crypto, skb, hie_offset,
+				       hie_len);
+}
+
+int fira_sts_encrypt_hie(struct fira_session *session, struct sk_buff *skb,
+			 int hie_offset, int hie_len)
+{
+	return fira_crypto_encrypt_hie(session->crypto, skb, hie_offset,
+				       hie_len);
+}
diff --git a/mac/fira_sts.h b/mac/fira_sts.h
new file mode 100644
index 0000000..2d96540
--- /dev/null
+++ b/mac/fira_sts.h
@@ -0,0 +1,171 @@
+/*
+* This file is part of the UWB stack for linux.
+*
+* Copyright (c) 2022 Qorvo US, Inc.
+*
+* This software is provided under the GNU General Public License, version 2
+* (GPLv2), as well as under a Qorvo commercial license.
+*
+* You may choose to use this software under the terms of the GPLv2 License,
+* version 2 ("GPLv2"), as published by the Free Software Foundation.
+* You should have received a copy of the GPLv2 along with this program.  If
+* not, see <http://www.gnu.org/licenses/>.
+* not, see <http://www.gnu.org/licenses/>.
+*
+* This program is distributed under the GPLv2 in the hope that it will be
+* useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GPLv2 for more
+* details.
+*
+* If you cannot meet the requirements of the GPLv2, you may not use this
+* software for any purpose without first obtaining a commercial license from
+* Qorvo. Please contact Qorvo to inquire about licensing terms.
+*/
+
+#ifndef NET_MCPS802154_FIRA_STS_H
+#define NET_MCPS802154_FIRA_STS_H
+
+#include <linux/types.h>
+#include <linux/list.h>
+#include <linux/ieee802154.h>
+
+#include <net/fira_region_params.h>
+#include <net/mcps802154_frame.h>
+
+struct fira_session;
+struct fira_local;
+
+/**
+ * fira_sts_init() - Initialization of STS context for a given
+ * session.
+ * @session: The session for which the context shall be initialized.
+ * @slot_duration_us: Duration of a slot in us.
+ * @current_channel: Current channel used by the LLHW.
+ *
+ * Return: 0 or error.
+ */
+int fira_sts_init(struct fira_session *session, int slot_duration_us,
+		  const struct mcps802154_channel *current_channel);
+
+/**
+ * fira_sts_deinit() - Deinitialize STS context and release its resources.
+ * @session: The session for which the STS shall be de-initialized.
+ */
+void fira_sts_deinit(struct fira_session *session);
+
+/**
+ * fira_sts_rotate_keys() - To verify and rotate crypto keys if needed.
+ * @session: The session for which the sts params are requested.
+ *
+ * Return: 0 or error.
+ */
+int fira_sts_rotate_keys(struct fira_session *session);
+
+/**
+ * fira_sts_get_sts_params() - To fetch sts_params in order to configure the
+ * current frame.
+ * @session: The session for which the sts params are requested.
+ * @slot_index: The index of the slot for which the STS shall be computed.
+ * @sts_v: STS Vector to be filled using the context.
+ * @sts_v_size: Size of the requested STS vector.
+ * @sts_key: STS Key to be set using the context.
+ * @sts_key_size: Size of the requested STS key.
+ *
+ * Return: 0 or error.
+ */
+int fira_sts_get_sts_params(struct fira_session *session, const u32 slot_index,
+			    u8 *sts_v, u32 sts_v_size, u8 *sts_key,
+			    u32 sts_key_size);
+
+/**
+* fira_sts_get_phy_sts_index() - Computes the phy sts index related to
+* a giver slot of a given session.
+* @session: The session for which the phy sts index is needed.
+* @slot_index: The index of the slot index for which the STS index is needed.
+*
+* Return: phy_sts_index for the current slot.
+*/
+u32 fira_sts_get_phy_sts_index(const struct fira_session *session,
+			       const u32 slot_index);
+
+/**
+ * fira_sts_convert_phy_sts_idx_to_time_indexes() - Convert a given phy
+ * sts index to the corresponding time related indexes (block, round and slot
+ * indexes).
+ * @session: The session to for which the indexes are needed.
+ * @current_phy_sts_index: phy_sts_index used for time synchronization.
+ * @block_idx: The block index pointed by the given phy sts index.
+ * @round_idx: The block index pointed by the given phy sts index.
+ * @slot_idx: The block index pointed by the given phy sts index.
+ *
+ * Return: 0 or error.
+ */
+int fira_sts_convert_phy_sts_idx_to_time_indexes(
+	const struct fira_session *session, const u32 current_phy_sts_index,
+	u32 *block_idx, u32 *round_idx, u32 *slot_idx);
+
+/**
+* fira_sts_prepare_decrypt() - Prepare skb for header decryption and verification.
+* @session: The session to for which the indexes are needed.
+* @skb: Buffer containing the frame to decrypt.
+*
+* Return: 0 or error.
+*/
+int fira_sts_prepare_decrypt(struct fira_session *session, struct sk_buff *skb);
+
+/**
+* fira_sts_encrypt_frame() - Encrypt the given FiRa 802154 frame.
+* @session: The session to use to encrypt the frame.
+* @skb: Buffer containing the frame to encrypt.
+* @header_len: Length of the 802154 header. Can be used to find the start of the
+* payload (relative to skb->data).
+* @src_short_addr: Source short address.
+* @slot_index: The slot index of the frame to encrypt.
+*
+* Return: 0 or error.
+*/
+int fira_sts_encrypt_frame(struct fira_session *session, struct sk_buff *skb,
+			   int header_len, __le16 src_short_addr,
+			   const u32 slot_index);
+
+/**
+* fira_sts_decrypt_frame() - Decrypt the given FiRa 802154 frame.
+* @session: The session to use to decrypt the frame.
+* @skb: Buffer containing the frame to decrypt.
+* @header_len: Length of the 802154 header. Used to find the start of the
+* frame payload (relative to skb->data).
+* @src_short_addr: Source short address.
+* @slot_index: The slot index of the frame to decrypt.
+*
+* Return: 0 or error.
+*/
+int fira_sts_decrypt_frame(struct fira_session *session, struct sk_buff *skb,
+			   int header_len, __le16 src_short_addr,
+			   u32 slot_index);
+
+/**
+* fira_sts_encrypt_hie() - Encrypt the HIE stored in a FiRa 802154
+* frame.
+* @session: The session to attached to the HIE.
+* @skb: Buffer containing the frame to encrypt.
+* @hie_offset: Offset to the start of the HIE (relative to skb->data) to encrypt.
+* @hie_len: Length of the HIE to encrypt.
+*
+* Return: 0 or error.
+*/
+int fira_sts_encrypt_hie(struct fira_session *session, struct sk_buff *skb,
+			 int hie_offset, int hie_len);
+
+/**
+* fira_sts_decrypt_hie() - Decrypt the HIE stored in a FiRa 802154 frame.
+* @session: The session attached to the HIE
+* @skb: Buffer containing the frame to decrypt.
+* @hie_offset: Offset to the start of the HIE (relative to skb->data) to decrypt.
+* @hie_len: Length of the HIE to decrypt.
+*
+* Return: 0 or error.
+*/
+int fira_sts_decrypt_hie(struct fira_session *session, struct sk_buff *skb,
+			 int hie_offset, int hie_len);
+
+#endif /* NET_MCPS802154_FIRA_STS_H */
diff --git a/mac/fira_trace.h b/mac/fira_trace.h
index cfca35f..58410a4 100644
--- a/mac/fira_trace.h
+++ b/mac/fira_trace.h
@@ -117,13 +117,17 @@ TRACE_DEFINE_ENUM(FIRA_PHR_DATA_RATE_6M81);
 TRACE_DEFINE_ENUM(FIRA_MAC_FCS_TYPE_CRC_16);
 TRACE_DEFINE_ENUM(FIRA_MAC_FCS_TYPE_CRC_32);
 
-#define FIRA_STS_CONFIG_SYMBOLS                  \
-	{ FIRA_STS_CONFIG_STATIC, "static" },    \
-	{ FIRA_STS_CONFIG_DYNAMIC, "dynamic" },  \
-	{ FIRA_STS_CONFIG_DYNAMIC_INDIVIDUAL_KEY, "dynamic_individual_key" }
-TRACE_DEFINE_ENUM(FIRA_STS_CONFIG_STATIC);
-TRACE_DEFINE_ENUM(FIRA_STS_CONFIG_DYNAMIC);
-TRACE_DEFINE_ENUM(FIRA_STS_CONFIG_DYNAMIC_INDIVIDUAL_KEY);
+#define FIRA_STS_MODE_SYMBOLS                  \
+	{ FIRA_STS_MODE_STATIC, "static" },    \
+	{ FIRA_STS_MODE_DYNAMIC, "dynamic" },  \
+	{ FIRA_STS_MODE_DYNAMIC_INDIVIDUAL_KEY, "dynamic_individual_key" },  \
+	{ FIRA_STS_MODE_PROVISIONED, "provisioned" },  \
+	{ FIRA_STS_MODE_PROVISIONED_INDIVIDUAL_KEY, "provisioned_individual_key" }
+TRACE_DEFINE_ENUM(FIRA_STS_MODE_STATIC);
+TRACE_DEFINE_ENUM(FIRA_STS_MODE_DYNAMIC);
+TRACE_DEFINE_ENUM(FIRA_STS_MODE_DYNAMIC_INDIVIDUAL_KEY);
+TRACE_DEFINE_ENUM(FIRA_STS_MODE_PROVISIONED);
+TRACE_DEFINE_ENUM(FIRA_STS_MODE_PROVISIONED_INDIVIDUAL_KEY);
 
 #define FIRA_MESSAGE_TYPE                              \
 	{ FIRA_MESSAGE_ID_RANGING_INITIATION, "RIM" }, \
@@ -265,8 +269,12 @@ TRACE_EVENT(region_fira_session_params,
 		__field(enum fira_sts_segments, number_of_sts_segments)
 		__field(enum fira_psdu_data_rate, psdu_data_rate)
 		__field(enum fira_mac_fcs_type, mac_fcs_type)
-		__field(enum fira_sts_config, sts_config)
+		__field(enum fira_sts_mode, sts_config)
 		__array(u8, vupper64, FIRA_VUPPER64_SIZE)
+		__field(u32, session_key_len)
+		__array(u8, session_key, FIRA_KEY_SIZE_MIN)
+		__field(bool, key_rotation)
+		__field(int, key_rotation_rate)
 		__field(bool, aoa_result_req)
 		__field(bool, report_tof)
 		__field(bool, report_aoa_azimuth)
@@ -298,7 +306,11 @@ TRACE_EVENT(region_fira_session_params,
 		__entry->psdu_data_rate = params->psdu_data_rate;
 		__entry->mac_fcs_type = params->mac_fcs_type;
 		__entry->sts_config = params->sts_config;
-		memcpy(__entry->vupper64, params->vupper64, FIRA_VUPPER64_SIZE);
+		memcpy(__entry->vupper64, params->vupper64, sizeof(params->vupper64));
+		__entry->session_key_len = params->session_key_len;
+		memcpy(__entry->session_key, params->session_key, params->session_key_len);
+		__entry->key_rotation = params->key_rotation;
+		__entry->key_rotation_rate = params->key_rotation_rate;
 		__entry->aoa_result_req = params->aoa_result_req;
 		__entry->report_tof = params->report_tof;
 		__entry->report_aoa_azimuth = params->report_aoa_azimuth;
@@ -312,7 +324,9 @@ TRACE_EVENT(region_fira_session_params,
 		  "max_rr_retry=%d round_duration_slots=%d round_hopping=%d "
 		  "priority=%d channel_number=%d preamble_code_index=%d rframe_config=%s "
 		  "preamble_duration=%s sfd_id=%d number_of_sts_segments=%s psdu_data_rate=%s mac_fcs_type=%s "
-		  "sts_config=%s vupper64=%s aoa_result_req=%d report_tof=%d report_aoa_azimuth=%d "
+		  "sts_config=%s vupper64=%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x session_key_len=%d "
+		  "session_key=%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x "
+		  "key_rotation=%d key_rotation_rate=%d aoa_result_req=%d report_tof=%d report_aoa_azimuth=%d "
 		  "report_aoa_elevation=%d report_aoa_fom=%d diagnostics=%d",
 		  FIRA_SESSION_PR_ARG,
 		  __print_symbolic(__entry->device_type, FIRA_DEVICE_TYPE_SYMBOLS),
@@ -336,8 +350,16 @@ TRACE_EVENT(region_fira_session_params,
 		  __print_symbolic(__entry->number_of_sts_segments, FIRA_STS_SEGMENTS_SYMBOLS),
 		  __print_symbolic(__entry->psdu_data_rate, FIRA_PSDU_DATA_RATE_SYMBOLS),
 		  __print_symbolic(__entry->mac_fcs_type, FIRA_MAC_FCS_TYPE_CRC_SYMBOLS),
-		  __print_symbolic(__entry->sts_config, FIRA_STS_CONFIG_SYMBOLS),
-		  __print_hex(__entry->vupper64, FIRA_VUPPER64_SIZE),
+		  __print_symbolic(__entry->sts_config, FIRA_STS_MODE_SYMBOLS),
+		  __entry->vupper64[0], __entry->vupper64[1], __entry->vupper64[2], __entry->vupper64[3],
+		  __entry->vupper64[4], __entry->vupper64[5], __entry->vupper64[6], __entry->vupper64[7],
+		  __entry->session_key_len,
+		  __entry->session_key[0], __entry->session_key[1], __entry->session_key[2], __entry->session_key[3],
+		  __entry->session_key[4], __entry->session_key[5], __entry->session_key[6], __entry->session_key[7],
+		  __entry->session_key[8], __entry->session_key[9], __entry->session_key[10], __entry->session_key[11],
+		  __entry->session_key[12], __entry->session_key[13], __entry->session_key[14], __entry->session_key[15],
+		  __entry->key_rotation,
+		  __entry->key_rotation_rate,
 		  __entry->aoa_result_req,
 		  __entry->report_tof,
 		  __entry->report_aoa_azimuth,
diff --git a/mac/include/net/fira_region_nl.h b/mac/include/net/fira_region_nl.h
index 21f7655..a78788c 100644
--- a/mac/include/net/fira_region_nl.h
+++ b/mac/include/net/fira_region_nl.h
@@ -166,12 +166,16 @@ enum fira_call {
  *	Number of antenna pairs for RX.
  * @FIRA_CAPABILITY_ATTR_TX_ANTENNAS:
  *	Number of antennas for TX.
- * @FIRA_CAPABILITY_ATTR_STS_CONFIG_STATIC:
+ * @FIRA_CAPABILITY_ATTR_STS_STATIC:
  *	Static STS supported.
- * @FIRA_CAPABILITY_ATTR_STS_CONFIG_DYNAMIC:
+ * @FIRA_CAPABILITY_ATTR_STS_DYNAMIC:
  *	Dynamic STS supported.
- * @FIRA_CAPABILITY_ATTR_STS_CONFIG_DYNAMIC_INDIVIDUAL:
+ * @FIRA_CAPABILITY_ATTR_STS_DYNAMIC_INDIVIDUAL_KEY:
  *	Dynamic STS for controlee individual keys supported.
+ * @FIRA_CAPABILITY_ATTR_STS_PROVISIONED:
+ *	Provisioned STS supported.
+ * @FIRA_CAPABILITY_ATTR_STS_PROVISIONED_INDIVIDUAL_KEY:
+ *	Provisioned STS for controlee individual keys supported.
  * @FIRA_CAPABILITY_ATTR_AOA_AZIMUTH:
  *	AoA in azimuth supported.
  * @FIRA_CAPABILITY_ATTR_AOA_AZIMUTH_FULL:
@@ -235,9 +239,11 @@ enum fira_capability_attrs {
 	FIRA_CAPABILITY_ATTR_RX_ANTENNA_PAIRS,
 	FIRA_CAPABILITY_ATTR_TX_ANTENNAS,
 	/* STS and crypto capabilities. */
-	FIRA_CAPABILITY_ATTR_STS_CONFIG_STATIC,
-	FIRA_CAPABILITY_ATTR_STS_CONFIG_DYNAMIC,
-	FIRA_CAPABILITY_ATTR_STS_CONFIG_DYNAMIC_INDIVIDUAL,
+	FIRA_CAPABILITY_ATTR_STS_STATIC,
+	FIRA_CAPABILITY_ATTR_STS_DYNAMIC,
+	FIRA_CAPABILITY_ATTR_STS_DYNAMIC_INDIVIDUAL_KEY,
+	FIRA_CAPABILITY_ATTR_STS_PROVISIONED,
+	FIRA_CAPABILITY_ATTR_STS_PROVISIONED_INDIVIDUAL_KEY,
 	/* Report. */
 	FIRA_CAPABILITY_ATTR_AOA_AZIMUTH,
 	FIRA_CAPABILITY_ATTR_AOA_AZIMUTH_FULL,
@@ -359,16 +365,17 @@ enum fira_call_attrs {
  * @FIRA_SESSION_PARAM_ATTR_MEASUREMENT_SEQUENCE:
  * 	Sequence of measurement steps. Configures antenna flexibility.
  * @FIRA_SESSION_PARAM_ATTR_STS_CONFIG:
- *	Static STS (0, default), dynamic STS (1) or dynamic STS for controlee
- *	individual keys (2)
+ *	Static STS (0, default), Dynamic STS (1), Dynamic STS for controlee
+ *	individual keys (2), Provisioned STS (3), Provisioned STS for controlee
+ *	individual keys (4). See &enum fira_sts_mode.
  * @FIRA_SESSION_PARAM_ATTR_SUB_SESSION_ID:
  *	For dynamic STS for controlee individual key, sub session ID [controlee only]
  * @FIRA_SESSION_PARAM_ATTR_VUPPER64:
  *	vUpper64 for static STS (UCI: STATIC_STS_IV | VENDOR_ID)
  * @FIRA_SESSION_PARAM_ATTR_SESSION_KEY:
- *	For dynamic STS, session key (not in UCI)
+ *	For provisioned sts only, session key.
  * @FIRA_SESSION_PARAM_ATTR_SUB_SESSION_KEY:
- *	For dynamic STS for controlee individual keys, sub session key [controlee only]
+ *	For dynamic or provisioned STS, sub session key [controlee only]
  * @FIRA_SESSION_PARAM_ATTR_KEY_ROTATION:
  *	Disable (0, default) or enabled (1)
  * @FIRA_SESSION_PARAM_ATTR_KEY_ROTATION_RATE:
diff --git a/mac/include/net/fira_region_params.h b/mac/include/net/fira_region_params.h
index a5bebaa..9cade01 100644
--- a/mac/include/net/fira_region_params.h
+++ b/mac/include/net/fira_region_params.h
@@ -27,6 +27,7 @@
 #include <linux/types.h>
 
 #define FIRA_VUPPER64_SIZE 8
+#define FIRA_STS_VUPPER64_OFFSET 8
 #define FIRA_KEY_SIZE_MAX 32
 #define FIRA_KEY_SIZE_MIN 16
 #define FIRA_CONTROLEES_MAX 8
@@ -250,18 +251,29 @@ enum fira_rssi_report_type {
 };
 
 /**
- * enum fira_sts_config - Scrambled Timestamp Sequence configuration.
- * @FIRA_STS_CONFIG_STATIC: Use a static STS configuration.
- * @FIRA_STS_CONFIG_DYNAMIC: Use a dynamic STS configuration.
- * @FIRA_STS_CONFIG_DYNAMIC_INDIVIDUAL_KEY: Use a dynamic STS configuration
- * with an individual key.
+ * enum fira_sts_mode - Scrambled Timestamp Sequence modes.
+ *
+ * @FIRA_STS_MODE_STATIC: Static STS mode.
+ * @FIRA_STS_MODE_DYNAMIC: Use a dynamic STS mode.
+ * @FIRA_STS_MODE_DYNAMIC_INDIVIDUAL_KEY: Use a dynamic STS mode
+ * with individual controlee key.
+ * @FIRA_STS_MODE_PROVISIONED: Use a provisioned STS mode.
+ * @FIRA_STS_MODE_PROVISIONED_INDIVIDUAL_KEY: Use a provisioned STS
+ * mode with individual controlee key.
  */
-enum fira_sts_config {
-	FIRA_STS_CONFIG_STATIC,
-	FIRA_STS_CONFIG_DYNAMIC,
-	FIRA_STS_CONFIG_DYNAMIC_INDIVIDUAL_KEY,
+enum fira_sts_mode {
+	FIRA_STS_MODE_STATIC = 0,
+	FIRA_STS_MODE_DYNAMIC = 1,
+	FIRA_STS_MODE_DYNAMIC_INDIVIDUAL_KEY = 2,
+	FIRA_STS_MODE_PROVISIONED = 3,
+	FIRA_STS_MODE_PROVISIONED_INDIVIDUAL_KEY = 4,
 };
 
+/*
+ * Get the capabilities bitfield value corresponding to given STS mode.
+ */
+#define STS_CAP(mode) (1 << (FIRA_STS_MODE_##mode))
+
 /**
  * enum fira_ranging_status - The ranging status.
  * @FIRA_STATUS_RANGING_INTERNAL_ERROR: Implementation specific error.
@@ -340,7 +352,7 @@ enum fira_ranging_diagnostics_frame_report_flags {
 	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORT_RSSIS = 1 << 0,
 	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORT_AOAS = 1 << 1,
 	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORT_CIRS = 1 << 2,
-	__FIRA_RANGING_DIAGNOSTICS_FRAME_REPORT_AFTER_LAST = 1U << 31,
+	__FIRA_RANGING_DIAGNOSTICS_FRAME_REPORT_AFTER_LAST = 1 << 31,
 };
 
 /**
diff --git a/mac/trace.h b/mac/trace.h
index a8e2011..3808554 100644
--- a/mac/trace.h
+++ b/mac/trace.h
@@ -503,6 +503,8 @@ TRACE_EVENT(llhw_set_sts_params,
 		__field(int, seg_len)
 		__field(int, sp2_tx_gap_4chips)
 		__array(int, sp2_rx_gap_4chips, MCPS802154_STS_N_SEGS_MAX)
+		__array(u8, key, AES_BLOCK_SIZE)
+		__array(u8, v, AES_BLOCK_SIZE)
 		),
 	TP_fast_assign(
 		LOCAL_ASSIGN;
@@ -511,13 +513,25 @@ TRACE_EVENT(llhw_set_sts_params,
 		__entry->sp2_tx_gap_4chips = params->sp2_tx_gap_4chips;
 		memcpy(__entry->sp2_rx_gap_4chips, params->sp2_rx_gap_4chips,
 		       sizeof(params->sp2_rx_gap_4chips));
+		memcpy(__entry->key, params->key, sizeof(params->key));
+		memcpy(__entry->v, params->v, sizeof(params->v));
 		),
 	TP_printk(LOCAL_PR_FMT " n_segs=%d seg_len=%d sp2_tx_gap_4chips=%d"
-		  " sp2_rx_gap_4chips=%d,%d,%d,%d",
+		  " sp2_rx_gap_4chips=%d,%d,%d,%d"
+		  " sts_key=%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x"
+		  " sts_v=%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x",
 		  LOCAL_PR_ARG, __entry->n_segs, __entry->seg_len,
 		  __entry->sp2_tx_gap_4chips, __entry->sp2_rx_gap_4chips[0],
 		  __entry->sp2_rx_gap_4chips[1], __entry->sp2_rx_gap_4chips[2],
-		  __entry->sp2_rx_gap_4chips[3])
+		  __entry->sp2_rx_gap_4chips[3], __entry->key[0], __entry->key[1],
+		  __entry->key[2], __entry->key[3], __entry->key[4], __entry->key[5],
+		  __entry->key[6], __entry->key[7], __entry->key[8], __entry->key[9],
+		  __entry->key[10], __entry->key[11], __entry->key[12], __entry->key[13],
+		  __entry->key[14], __entry->key[15], __entry->v[0], __entry->v[1],
+		  __entry->v[2], __entry->v[3], __entry->v[4], __entry->v[5],
+		  __entry->v[6], __entry->v[7], __entry->v[8], __entry->v[9],
+		  __entry->v[10], __entry->v[11], __entry->v[12], __entry->v[13],
+		  __entry->v[14], __entry->v[15])
 	);
 
 TRACE_EVENT(llhw_rx_get_measurement,