Snap for 9489562 from d030aa0d4a7f0f3b31b0d2801ada36a69d438ae5 to android13-gs-pixel-5.10-releaseandroid-t-qpr2-beta-3.2_r0.4android-13.0.0_r0.65android-13.0.0_r0.63android-gs-raviole-5.10-t-qpr2-beta-3.2

Change-Id: I68c68c45c7ca01ec0a623f4dfd42c3aeb5835891

138 files changed, 12588 insertions, 6206 deletions

diff --git a/kernel/drivers/net/ieee802154/dw3000-overlay.dts b/kernel/drivers/net/ieee802154/dw3000-overlay.dts
index 39cb0da..775913a 100644
--- a/kernel/drivers/net/ieee802154/dw3000-overlay.dts
+++ b/kernel/drivers/net/ieee802154/dw3000-overlay.dts
@@ -76,4 +76,3 @@
         panid = <&dw3000>,"decawave,panid;0";
     };
 };
-
diff --git a/kernel/drivers/net/ieee802154/dw3000.h b/kernel/drivers/net/ieee802154/dw3000.h
index 5f03416..4495a2d 100644
--- a/kernel/drivers/net/ieee802154/dw3000.h
+++ b/kernel/drivers/net/ieee802154/dw3000.h
@@ -191,6 +191,7 @@ enum dw3000_ciagdiag_reg_select {
  * @tx_fctrl: Transmit frame control
  * @rx_timeout_pac: Preamble detection timeout period in units of PAC size
  *  symbols
+ * @rx_frame_timeout_dly: reception frame timeout period in units of dly.
  * @w4r_time: Wait-for-response time (RX after TX delay)
  * @sts_key: STS Key
  * @sts_iv: STS IV
@@ -208,6 +209,7 @@ struct dw3000_local_data {
 	u16 max_frames_len;
 	s16 ststhreshold;
 	u16 rx_timeout_pac;
+	u32 rx_frame_timeout_dly;
 	u32 tx_fctrl;
 	u32 w4r_time;
 	u8 sts_key[AES_KEYSIZE_128];
@@ -217,12 +219,18 @@ struct dw3000_local_data {
 /* Statistics items */
 enum dw3000_stats_items {
 	DW3000_STATS_RX_GOOD,
-	DW3000_STATS_RX_TO,
 	DW3000_STATS_RX_ERROR,
+	DW3000_STATS_RX_TO,
 	__DW3000_STATS_COUNT
 };
 
-/* DW3000 statistics */
+/**
+ * struct dw3000_stats - DW3000 statistics
+ * @count: Count per-items
+ * @rssi: Last RSSI data per type
+ * @enabled: Stats enabled flag
+ * @indexes: Free-running index per-items
+ */
 struct dw3000_stats {
 	/* Total stats */
 	u16 count[__DW3000_STATS_COUNT];
@@ -230,6 +238,7 @@ struct dw3000_stats {
 	struct dw3000_rssi rssi[DW3000_RSSI_REPORTS_MAX];
 	/* Stats on/off */
 	bool enabled;
+	u16 indexes[__DW3000_STATS_COUNT];
 };
 
 /* Maximum skb length
@@ -436,6 +445,7 @@ enum config_changed_flags {
  * @cur_state: current state defined by enum power_state
  * @tx_adjust: TX time adjustment based on frame length
  * @rx_start: RX start date in DTU for RX time adjustment
+ * @interrupts: Hardware interrupts count on the device.
  */
 struct dw3000_power {
 	struct sysfs_power_stats stats[DW3000_PWR_MAX];
@@ -443,6 +453,7 @@ struct dw3000_power {
 	int cur_state;
 	int tx_adjust;
 	u32 rx_start;
+	atomic64_t interrupts;
 };
 
 /**
@@ -451,8 +462,8 @@ struct dw3000_power {
  * @config_changed: bitfield of configuration changed during DEEP-SLEEP
  * @frame_idx: saved frame index to use for deferred TX/RX
  * @tx_skb: saved frame to transmit for deferred TX
- * @tx_info: saved info to use for deferred TX
- * @rx_info: saved parameter for deferred RX
+ * @tx_config: saved config to use for deferred TX
+ * @rx_config: saved parameter for deferred RX
  * @regbackup: registers backup to detect diff
  * @compare_work: deferred registers backup compare work
  */
@@ -462,8 +473,8 @@ struct dw3000_deep_sleep_state {
 	int frame_idx;
 	struct sk_buff *tx_skb;
 	union {
-		struct mcps802154_tx_frame_info tx_info;
-		struct mcps802154_rx_info rx_info;
+		struct mcps802154_tx_frame_config tx_config;
+		struct mcps802154_rx_frame_config rx_config;
 	};
 #ifdef CONFIG_DW3000_DEBUG
 	void *regbackup;
@@ -496,6 +507,23 @@ struct dw3000_cir_data;
 #define DW3000_MAX_QUEUED_SPI_XFER 32
 #define DW3000_QUEUED_SPI_BUFFER_SZ 2048
 
+/* Number of samples to average. */
+#define DW3000_NB_AVERAGE 1
+
+/**
+ * struct dw3000_rx_ctx - Custom rx context use with rx_init/rx_get_measurement.
+ * @pdoa_rad_q11: Array of PDoA measurements.
+ * @aoa_rad_q11: Array of AoA measurements.
+ * @index: Index for pdoa_rad_q11 and aoa_rad_q11 arrays.
+ * @sample_valid_nb: Number of valid measurements in array, used for average.
+ */
+struct dw3000_rx_ctx {
+	int pdoa_rad_q11[DW3000_NB_AVERAGE];
+	int aoa_rad_q11[DW3000_NB_AVERAGE];
+	int index;
+	int sample_valid_nb;
+};
+
 /**
  * struct dw3000 - main DW3000 device structure
  * @spi: pointer to corresponding spi device
@@ -543,6 +571,7 @@ struct dw3000_cir_data;
  * @coex_interval_us: Minimum interval between two operations in us
  *		      under which WiFi coexistence GPIO is kept active
  * @coex_gpio: WiFi coexistence GPIO, >= 0 if activated
+ * @coex_enabled: WiFi coexistence activation
  * @coex_status: WiFi coexistence GPIO status, 1 if activated
  * @lna_pa_mode: LNA/PA configuration to use
  * @autoack: auto-ack status, true if activated
@@ -557,6 +586,7 @@ struct dw3000_cir_data;
  * @restricted_channels: bit field of restricted channels
  * @tx_rf2: parameter to enable the tx on rf2 port
  * @cir_data_changed: true if buffer data have been reallocated
+ * @full_cia_read: CIA registers fully loaded into cir_data struct
  * @cir_data: allocated CIR exploitation data
  * @msg_queue: SPI message holding transfer queue
  * @msg_queue_xfer: next transfer available
@@ -647,6 +677,7 @@ struct dw3000 {
 	unsigned coex_margin_us;
 	unsigned coex_interval_us;
 	s8 coex_gpio;
+	bool coex_enabled;
 	int coex_status;
 	/* LNA/PA mode */
 	s8 lna_pa_mode;
@@ -674,6 +705,7 @@ struct dw3000 {
 	u8 tx_rf2;
 	/* Channel impulse response data */
 	bool cir_data_changed;
+	bool full_cia_read;
 	struct dw3000_cir_data *cir_data;
 	/* SPI message holding transfers queue */
 	struct spi_message *msg_queue;
@@ -682,7 +714,8 @@ struct dw3000 {
 	/* Buffer for queued transfers */
 	char *msg_queue_buf;
 	char *msg_queue_buf_pos;
-
+	/* dw3000 thread clamp value  */
+	int min_clamp_value;
 	/* Insert new fields before this line */
 
 	/* Shared message protected by a mutex */
diff --git a/kernel/drivers/net/ieee802154/dw3000_calib.c b/kernel/drivers/net/ieee802154/dw3000_calib.c
index 31fc047..31bd4a7 100644
--- a/kernel/drivers/net/ieee802154/dw3000_calib.c
+++ b/kernel/drivers/net/ieee802154/dw3000_calib.c
@@ -26,7 +26,7 @@
 /* clang-format off */
 #define CHAN_PRF_PARAMS (4 * DW3000_CALIBRATION_PRF_MAX)
 #define ANT_CHAN_PARAMS (CHAN_PRF_PARAMS * DW3000_CALIBRATION_CHANNEL_MAX)
-#define ANT_OTHER_PARAMS (3) /* port, selector_gpio... */
+#define ANT_OTHER_PARAMS (4) /* port, selector_gpio... */
 #define ANTPAIR_CHAN_PARAMS (2 * DW3000_CALIBRATION_CHANNEL_MAX + 1)
 
 #define OTHER_PARAMS (13) /* xtal_trim, temperature_reference,
@@ -38,7 +38,7 @@
 
 #define MAX_CALIB_KEYS ((ANTMAX * (ANT_CHAN_PARAMS + ANT_OTHER_PARAMS)) + \
 			(ANTPAIR_MAX * ANTPAIR_CHAN_PARAMS) +		\
-			(DW3000_CALIBRATION_CHANNEL_MAX) +		\
+			(DW3000_CALIBRATION_CHANNEL_MAX * 2) +		\
 			OTHER_PARAMS)
 
 #define DW_OFFSET(m) offsetof(struct dw3000, m)
@@ -61,7 +61,8 @@
 	PRF_CAL_INFO(ant[x].ch[1], 1),		\
 	CAL_INFO(ant[x].port),			\
 	CAL_INFO(ant[x].selector_gpio),		\
-	CAL_INFO(ant[x].selector_gpio_value)
+	CAL_INFO(ant[x].selector_gpio_value),	\
+	CAL_INFO(ant[x].caps)
 
 #define ANTPAIR_CAL_INFO(x,y)					\
 	CAL_INFO(antpair[ANTPAIR_IDX(x, y)].ch[0].pdoa_offset),	\
@@ -90,7 +91,9 @@ static const struct {
 	ANTPAIR_CAL_INFO(2,3),
 	/* chY.* */
 	CAL_INFO(ch[0].pll_locking_code),
+	CAL_INFO(ch[0].wifi_coex_enabled),
 	CAL_INFO(ch[1].pll_locking_code),
+	CAL_INFO(ch[1].wifi_coex_enabled),
 	/* other with direct access in struct dw3000 */
 	DW_INFO(txconfig.smart),
 	DW_INFO(auto_sleep_margin_us),
@@ -122,7 +125,8 @@ static const struct {
 	PRF_CAL_LABEL(x, 9, 64),		\
 	"ant" #x ".port",			\
 	"ant" #x ".selector_gpio",		\
-	"ant" #x ".selector_gpio_value"
+	"ant" #x ".selector_gpio_value",	\
+	"ant" #x ".caps"
 
 #define PDOA_CAL_LABEL(a, b, c)				\
 	"ant" #a ".ant" #b ".ch" #c ".pdoa_offset",	\
@@ -150,7 +154,9 @@ static const char *const dw3000_calib_keys[MAX_CALIB_KEYS + 1] = {
 	ANTPAIR_CAL_LABEL(2,3),
 	/* chY.* */
 	"ch5.pll_locking_code",
+	"ch5.wifi_coex-enabled",
 	"ch9.pll_locking_code",
+	"ch9.wifi_coex-enabled",
 	/* other */
 	"smart_tx_power",
 	"auto_sleep_margin",
@@ -323,6 +329,9 @@ int dw3000_calib_update_config(struct dw3000 *dw)
 	/* Shortcut pointers to reduce line length. */
 	ant_calib_prf = &ant_calib->ch[chanidx].prf[prfidx];
 
+	/* WiFi coexistence according to current channel */
+	dw->coex_enabled = dw->calib_data.ch[chanidx].wifi_coex_enabled;
+
 	/* Update TX configuration */
 	txconfig->power = ant_calib_prf->tx_power ? ant_calib_prf->tx_power :
 						    0xfefefefe;
diff --git a/kernel/drivers/net/ieee802154/dw3000_calib.h b/kernel/drivers/net/ieee802154/dw3000_calib.h
index c51ae17..b136b4c 100644
--- a/kernel/drivers/net/ieee802154/dw3000_calib.h
+++ b/kernel/drivers/net/ieee802154/dw3000_calib.h
@@ -82,10 +82,12 @@ extern const dw3000_pdoa_lut_t dw3000_default_lut_ch9;
 /**
  * struct dw3000_channel_calib - per-channel dependent calibration parameters
  * @pll_locking_code: PLL locking code
+ * @wifi_coex_enabled: WiFi coexistence activation
  */
 struct dw3000_channel_calib {
 	/* chY.pll_locking_code */
 	u32 pll_locking_code;
+	bool wifi_coex_enabled;
 };
 
 /**
@@ -109,6 +111,7 @@ struct dw3000_antenna_calib_prf {
  * @port: port value this antenna belong to (0 for RF1, 1 for RF2)
  * @selector_gpio: GPIO number to select this antenna
  * @selector_gpio_value: GPIO value to select this antenna
+ * @caps: antenna capabilities
  */
 struct dw3000_antenna_calib {
 	/* antX.chY.prfZ.* */
@@ -116,7 +119,7 @@ struct dw3000_antenna_calib {
 		struct dw3000_antenna_calib_prf prf[DW3000_CALIBRATION_PRF_MAX];
 	} ch[DW3000_CALIBRATION_CHANNEL_MAX];
 	/* antX.* */
-	u8 port, selector_gpio, selector_gpio_value;
+	u8 port, selector_gpio, selector_gpio_value, caps;
 };
 
 /**
diff --git a/kernel/drivers/net/ieee802154/dw3000_chip.h b/kernel/drivers/net/ieee802154/dw3000_chip.h
index 136a2c9..5871ea0 100644
--- a/kernel/drivers/net/ieee802154/dw3000_chip.h
+++ b/kernel/drivers/net/ieee802154/dw3000_chip.h
@@ -25,6 +25,7 @@
 
 /* Forward declaration */
 struct dw3000;
+struct dw3000_rssi;
 
 /**
  * enum dw3000_chip_register_flags - flags for the register declaration
@@ -117,6 +118,7 @@ struct dw3000_chip_register_priv {
  * @kick_ops_table_on_wakeup: kick the desired operating parameter set table
  * @kick_dgc_on_wakeup: kick the DGC upon wakeup from sleep
  * @get_registers: Return known registers table and it's size
+ * @compute_rssi: Uses the parameters to compute RSSI of current frame
  */
 struct dw3000_chip_ops {
 	int (*softreset)(struct dw3000 *dw);
@@ -137,6 +139,8 @@ struct dw3000_chip_ops {
 	int (*kick_dgc_on_wakeup)(struct dw3000 *dw);
 	const struct dw3000_chip_register *(*get_registers)(struct dw3000 *dw,
 							    size_t *count);
+	u32 (*compute_rssi)(struct dw3000 *dw, struct dw3000_rssi *rssi,
+			    bool rx_tune, u8 sts);
 };
 
 /**
diff --git a/kernel/drivers/net/ieee802154/dw3000_chip_c0.c b/kernel/drivers/net/ieee802154/dw3000_chip_c0.c
index 18033d6..2401504 100644
--- a/kernel/drivers/net/ieee802154/dw3000_chip_c0.c
+++ b/kernel/drivers/net/ieee802154/dw3000_chip_c0.c
@@ -344,6 +344,61 @@ static int dw3000_c0_kick_dgc_on_wakeup(struct dw3000 *dw)
 				   dgc_sel | DW3000_NVM_CFG_DGC_KICK_BIT_MASK);
 }
 
+/**
+ * dw3000_c0_compute_rssi() - Compute RSSI from its composites
+ * @dw: the DW device
+ * @rssi: RSSI composites
+ * @rx_tune: state of RX_TUNE_EN bit leads to use dgc_dec value or not
+ * @sts: current sts mode
+ *
+ * Because RSSI cannot be positive in our case (would mean that signals have
+ * been amplified) we return an unsigned integer considered as always negative.
+ *
+ * Return: 0 on error, else the RSSI in absolute value and as an integer.
+ * expressed in dBm, Q32.0.
+ */
+static u32 dw3000_c0_compute_rssi(struct dw3000 *dw, struct dw3000_rssi *rssi,
+				  bool rx_tune, u8 sts)
+{
+	/* Details are logged into UWB-3455
+	 * DW3700 User Manual v0.3 and 4 section 4.7.2 gives that RSSI
+	 * can be computed using this formula:
+	 * rssi = 10 * log10 ((cir_pwr * 2^21) / pacc_cnt ^ 2) + 6D - A(prf)
+	 * But log10 isn't implemented ; let's use ilog2 instead, because it is
+	 * easy to do on binary numbers. Thus, formula becomes:
+	 * rssi = 3*log2(cir_pwr) - 6*log2(pacc_cnt) + 3*log2(2^21) + 6*D - A(prf)
+	 * Notice that 3*log2(2^21) +6*D - A(prf) can be pre-computed.
+	 * Factor 3 comes from ln(2) / ln(10) almost equal to 3 / 10 ; this is an
+	 * approximation done in equation turning log10 into log2 to avoid floating point
+	 */
+
+	/* u32 is used because ilog2 macro cannot work on bitfield */
+	u32 pwr = rssi->cir_pwr;
+	u32 cnt = rssi->pacc_cnt;
+	s32 r, rssi_constant;
+
+	/* Do not consider bad packets */
+	if (unlikely(!pwr || !cnt))
+		return 0;
+
+	rssi_constant = DW3000_RSSI_CONSTANT + 6 * rx_tune * rssi->dgc_dec;
+
+	if (!rssi->prf_64mhz) {
+		rssi_constant -= DW3000_RSSI_OFFSET_PRF16;
+	} else
+		rssi_constant -= ((sts == DW3000_STS_MODE_OFF) ?
+					  DW3000_RSSI_OFFSET_PRF64_IPATOV :
+					  DW3000_RSSI_OFFSET_PRF64_STS);
+
+	r = 3 * ilog2(pwr) - 6 * ilog2(cnt) + rssi_constant;
+	if (unlikely(r > 0)) {
+		dev_err(dw->dev, "bad rssi value. Forced to 0\n");
+		r = 0;
+	}
+
+	return (u32)-r;
+}
+
 const struct dw3000_chip_ops dw3000_chip_c0_ops = {
 	.softreset = dw3000_c0_softreset,
 	.init = dw3000_c0_init,
@@ -360,4 +415,5 @@ const struct dw3000_chip_ops dw3000_chip_c0_ops = {
 	.kick_ops_table_on_wakeup = dw3000_c0_kick_ops_table_on_wakeup,
 	.kick_dgc_on_wakeup = dw3000_c0_kick_dgc_on_wakeup,
 	.get_registers = dw3000_c0_get_registers,
+	.compute_rssi = dw3000_c0_compute_rssi,
 };
diff --git a/kernel/drivers/net/ieee802154/dw3000_chip_c0.h b/kernel/drivers/net/ieee802154/dw3000_chip_c0.h
index cc784e7..25734f9 100644
--- a/kernel/drivers/net/ieee802154/dw3000_chip_c0.h
+++ b/kernel/drivers/net/ieee802154/dw3000_chip_c0.h
@@ -38,4 +38,11 @@
  * when a calibration from scratch is executed */
 #define DW3000_C0_PLL_CALIBRATION_FROM_SCRATCH_DELAY_US (400)
 
+/* RSSI constants */
+#define DW3000_RSSI_OFFSET_PRF64_STS 121
+#define DW3000_RSSI_OFFSET_PRF64_IPATOV 122
+#define DW3000_RSSI_OFFSET_PRF16 114
+#define DW3000_RSSI_CONSTANT \
+	63 /* 3 * log2(2^21) because log2 used instead of log10 */
+
 #endif /* __DW3000_CHIP_C0_H */
diff --git a/kernel/drivers/net/ieee802154/dw3000_chip_d0.c b/kernel/drivers/net/ieee802154/dw3000_chip_d0.c
index 10e9643..8bf3fb8 100644
--- a/kernel/drivers/net/ieee802154/dw3000_chip_d0.c
+++ b/kernel/drivers/net/ieee802154/dw3000_chip_d0.c
@@ -259,6 +259,61 @@ static int dw3000_d0_pll_calibration_from_scratch(struct dw3000 *dw)
 	return rc;
 }
 
+/**
+ * dw3000_d0_compute_rssi() - Compute RSSI from its composites
+ * @dw: the DW device
+ * @rssi: RSSI composites
+ * @rx_tune: state of RX_TUNE_EN bit leads to use dgc_dec value or not
+ * @sts: current sts mode
+ *
+ * Because RSSI cannot be positive in our case (would mean that signals have
+ * been amplified) we return an unsigned integer considered as always negative.
+ *
+ * Return: 0 on error, else the RSSI in absolute value and as an integer.
+ * expressed in dBm, Q32.0.
+ */
+u32 dw3000_d0_compute_rssi(struct dw3000 *dw, struct dw3000_rssi *rssi,
+			   bool rx_tune, u8 sts)
+{
+	/* Details are logged into UWB-3455
+	 * DW3700 User Manual v0.4 section 4.7.2 gives that RSSI
+	 * can be computed using this formula:
+	 * rssi = 10 * log10 ((cir_pwr * 2^17) / pacc_cnt ^ 2) + 6D - A(prf)
+	 * But log10 isn't implemented ; let's use ilog2 instead, because it is
+	 * easy to do on binary numbers. Thus, formula becomes:
+	 * rssi = 3*log2(cir_pwr) - 6*log2(pacc_cnt) + 3*log2(2^17) + 6*D - A(prf)
+	 * Notice that 3*log2(2^17) +6*D - A(prf) can be pre-computed.
+	 * Factor 3 comes from ln(2) / ln(10) almost equal to 3 / 10 ; this is an
+	 * approximation done in equation turning log10 into log2 to avoid floating point
+	 */
+
+	/* u32 is used because ilog2 macro cannot work on bitfield */
+	u32 pwr = rssi->cir_pwr;
+	u32 cnt = rssi->pacc_cnt;
+	s32 r, rssi_constant;
+
+	/* Do not consider bad packets */
+	if (unlikely(!pwr || !cnt))
+		return 0;
+
+	rssi_constant = DW3000_RSSI_CONSTANT + 6 * rx_tune * rssi->dgc_dec;
+
+	if (!rssi->prf_64mhz) {
+		rssi_constant -= DW3000_RSSI_OFFSET_PRF16;
+	} else
+		rssi_constant -= ((sts == DW3000_STS_MODE_OFF) ?
+					  DW3000_RSSI_OFFSET_PRF64_IPATOV :
+					  DW3000_RSSI_OFFSET_PRF64_STS);
+
+	r = 3 * ilog2(pwr) - 6 * ilog2(cnt) + rssi_constant;
+	if (unlikely(r > 0)) {
+		dev_err(dw->dev, "bad rssi value. Forced to 0\n");
+		r = 0;
+	}
+
+	return (u32)-r;
+}
+
 const struct dw3000_chip_ops dw3000_chip_d0_ops = {
 	.softreset = dw3000_d0_softreset,
 	.init = dw3000_d0_init,
@@ -272,4 +327,5 @@ const struct dw3000_chip_ops dw3000_chip_d0_ops = {
 	.prog_pll_coarse_code = dw3000_c0_prog_pll_coarse_code,
 	.set_mrxlut = dw3000_c0_set_mrxlut,
 	.get_registers = dw3000_d0_get_registers,
+	.compute_rssi = dw3000_d0_compute_rssi,
 };
diff --git a/kernel/drivers/net/ieee802154/dw3000_chip_d0.h b/kernel/drivers/net/ieee802154/dw3000_chip_d0.h
index ac4d22e..4804623 100644
--- a/kernel/drivers/net/ieee802154/dw3000_chip_d0.h
+++ b/kernel/drivers/net/ieee802154/dw3000_chip_d0.h
@@ -38,4 +38,11 @@
  * when a calibration from scratch is executed */
 #define DW3000_D0_PLL_CALIBRATION_FROM_SCRATCH_DELAY_US (400)
 
+/* RSSI constants */
+#define DW3000_RSSI_OFFSET_PRF64_STS 121
+#define DW3000_RSSI_OFFSET_PRF64_IPATOV 122
+#define DW3000_RSSI_OFFSET_PRF16 114
+#define DW3000_RSSI_CONSTANT \
+	51 /* 3 * log2(2^17) because log2 used instead of log10 */
+
 #endif /* __DW3000_CHIP_D0_H */
diff --git a/kernel/drivers/net/ieee802154/dw3000_chip_e0.c b/kernel/drivers/net/ieee802154/dw3000_chip_e0.c
index ff2079a..1605bf0 100644
--- a/kernel/drivers/net/ieee802154/dw3000_chip_e0.c
+++ b/kernel/drivers/net/ieee802154/dw3000_chip_e0.c
@@ -32,6 +32,8 @@ int dw3000_d0_init(struct dw3000 *dw);
 int dw3000_d0_coex_init(struct dw3000 *dw);
 const struct dw3000_chip_register *dw3000_d0_get_registers(struct dw3000 *dw,
 							   size_t *count);
+u32 dw3000_d0_compute_rssi(struct dw3000 *dw, struct dw3000_rssi *rssi,
+			   bool rx_tune, u8 sts);
 
 const u32 *dw3000_e0_get_config_mrxlut_chan(struct dw3000 *dw, u8 channel)
 {
@@ -706,4 +708,5 @@ const struct dw3000_chip_ops dw3000_chip_e0_ops = {
 	.pll_coarse_code = dw3000_e0_pll_coarse_code,
 	.set_mrxlut = dw3000_e0_set_mrxlut,
 	.get_registers = dw3000_d0_get_registers,
+	.compute_rssi = dw3000_d0_compute_rssi,
 };
diff --git a/kernel/drivers/net/ieee802154/dw3000_cir.h b/kernel/drivers/net/ieee802154/dw3000_cir.h
index 9a87ffc..d66429c 100644
--- a/kernel/drivers/net/ieee802154/dw3000_cir.h
+++ b/kernel/drivers/net/ieee802154/dw3000_cir.h
@@ -54,7 +54,7 @@ struct dw3000_cir_record {
  * @fp_power3: first path power component f3
  * @offset: user defined offset
  * @fp_index: index of first path record in CIR register
- * @tdoa: tdoa raw value
+ * @pdoa: pdoa raw value
  * @acc: number of symbols accumulated in CIR
  * @type: CIR type field
  * @dummy: store the dummy byte firstly received at each CIR memory reading
@@ -73,7 +73,7 @@ struct dw3000_cir_data {
 	u32 fp_power3;
 	s32 offset;
 	u16 fp_index;
-	s16 tdoa;
+	u16 pdoa;
 	u16 acc;
 	u8 type;
 	u8 dummy;
diff --git a/kernel/drivers/net/ieee802154/dw3000_coex.h b/kernel/drivers/net/ieee802154/dw3000_coex.h
index 2f39dc6..c2dc6b7 100644
--- a/kernel/drivers/net/ieee802154/dw3000_coex.h
+++ b/kernel/drivers/net/ieee802154/dw3000_coex.h
@@ -76,7 +76,7 @@ static inline int dw3000_coex_start(struct dw3000 *dw, bool *trx_delayed,
 	int delay_us;
 	int timer_us = 0;
 
-	if (dw->coex_gpio < 0)
+	if (dw->coex_gpio < 0 || !dw->coex_enabled)
 		return 0;
 	/* Add a margin for required SPI transactions to the coex delay time
 	 * to ensure GPIO change at right time. */
@@ -120,7 +120,7 @@ static inline int dw3000_coex_start(struct dw3000 *dw, bool *trx_delayed,
  */
 static inline int dw3000_coex_stop(struct dw3000 *dw)
 {
-	if (dw->coex_gpio < 0)
+	if (dw->coex_gpio < 0 || !dw->coex_enabled)
 		return 0;
 
 	trace_dw3000_coex_gpio_stop(dw, dw->coex_status);
diff --git a/kernel/drivers/net/ieee802154/dw3000_core.c b/kernel/drivers/net/ieee802154/dw3000_core.c
index 6905c3c..f950be0 100644
--- a/kernel/drivers/net/ieee802154/dw3000_core.c
+++ b/kernel/drivers/net/ieee802154/dw3000_core.c
@@ -26,6 +26,7 @@
 #include <linux/delay.h>
 #include <linux/interrupt.h>
 #include <linux/bitfield.h>
+#include <linux/log2.h>
 
 #include "dw3000.h"
 #include "dw3000_core.h"
@@ -186,8 +187,8 @@ const struct dw3000_chip_version dw3000_chip_versions[] = {
 #define DW3000_TXRXSWITCH_TX 0x01011100
 #define DW3000_TXRXSWITCH_AUTO 0x1C000000
 
-#define DW3000_RF_TXCTRL_CH5 0x1C071134UL
-#define DW3000_RF_TXCTRL_CH9 0x1C010034UL
+#define DW3000_RF_TXCTRL_CH5 0x1C081134UL
+#define DW3000_RF_TXCTRL_CH9 0x1C020034UL
 #define DW3000_RF_TXCTRL_LO_B2 0x0E
 #define DW3000_RF_PLL_CFG_CH5 0x1F3C
 #define DW3000_RF_PLL_CFG_CH9 0x0F3C
@@ -296,6 +297,12 @@ enum ciadiag_dbl_options {
 /* Disable CIA diagnostic. CIACONFIG's bit-4 in RX_ANTENNA_DELAY + 1 */
 #define DW3000_CIA_CONFIG_DIAG_OFF (0x1 << 4)
 
+/* LDO VOUT value */
+#define DW3000_RF_LDO_VOUT 0x0D7FFFFFUL
+
+/* PLL common value  */
+#define DW3000_RF_PLL_COMMON 0xE104
+
 struct dw3000_ciadiag_reg_info {
 	u32 diag1;
 	u32 diag12;
@@ -1602,51 +1609,62 @@ MODULE_PARM_DESC(stats_enabled,
 /**
  * dw3000_rx_store_rssi() - Get RSSI data from last good RX frame
  * @dw: the DW device
+ * @rssi: points to current rssi record in stats structure
+ * @pkt_sts: sts mode of current packet
  *
  * The RSSI data must be read before incrementing counter.
  * It requires at least one received frame to get any data from
  * register. Both 'cir_pwr' and 'pacc_cnt' values cannot be null
- * regarding the RSSI formula in the DW3700 User Manual v0.1 section 4.6.2.
+ * regarding the RSSI formula in the DW3700 User Manual v0.3 section 4.7.2.
  *
  * Return: 0 on success, else a negative error code.
  */
-static int dw3000_rx_store_rssi(struct dw3000 *dw)
+int dw3000_rx_store_rssi(struct dw3000 *dw, struct dw3000_rssi *rssi,
+			 u8 pkt_sts)
 {
 	struct dw3000_config *config = &dw->config;
-	struct dw3000_stats *stats = &dw->stats;
 	/* Only read RSSI after good RX frame */
-	int idx = stats->count[DW3000_STATS_RX_GOOD] - 1;
-	bool sts = _dw3000_sts_is_enabled(dw);
-	const char *chip_name = dw3000_get_chip_name(dw);
-	struct dw3000_rssi *rssi;
 	int rc;
 	u32 cir_pwr;
 	u16 pacc_cnt;
 	u8 dgc_dec;
+	u32 diag1_addr;
 
 	/* No data available */
-	if (idx < 0)
-		return -EAGAIN;
-	/* Get RSSI data pointer to store data */
-	rssi = &stats->rssi[idx];
-	/* Read CIR power value */
-	rc = dw3000_reg_read32(
-		dw, _ciadiag_reg_info[dw->data.ciadiag_reg_select].diag1, 0,
-		&cir_pwr);
-	if (unlikely(rc))
-		return rc;
-	/* Read preamble accumulation count value */
-	rc = dw3000_reg_read16(
-		dw, _ciadiag_reg_info[dw->data.ciadiag_reg_select].diag12, 0,
-		&pacc_cnt);
-	if (unlikely(rc))
-		return rc;
-	/* Avoid "nan" and "-inf" in userspace when calculating average RSSI */
-	if (cir_pwr == 0 || pacc_cnt == 0) {
-		dev_err(dw->dev,
-			"one or both values from CIA registers are null\n");
+	if (!rssi)
 		return -EAGAIN;
+	/* Get required data to calculate RSSI */
+	if (dw->full_cia_read) {
+		/* Get values from already retrieved CIA registers */
+		diag1_addr = (pkt_sts == DW3000_STS_MODE_OFF) ?
+				     (DW3000_DB_DIAG_IP_DIAG1 >> 2) :
+				     (DW3000_DB_DIAG_STS_DIAG1 >> 2);
+		cir_pwr = dw->cir_data->ciaregs[diag1_addr];
+		pacc_cnt = dw->cir_data->acc;
+	} else {
+		/* Read CIR power value */
+		rc = dw3000_reg_read32(
+			dw,
+			_ciadiag_reg_info[dw->data.ciadiag_reg_select].diag1, 0,
+			&cir_pwr);
+		if (unlikely(rc))
+			return rc;
+		/* Read preamble accumulation count value */
+		rc = dw3000_reg_read16(
+			dw,
+			_ciadiag_reg_info[dw->data.ciadiag_reg_select].diag12,
+			0, &pacc_cnt);
+		if (unlikely(rc))
+			return rc;
+		/* Reset minidiag to allow a new measure */
+		rc = dw3000_reg_modify32(dw, DW3000_CIA_CONF_ID, 0,
+					 ~DW3000_CIA_CONF_MINDIAG_BIT_MASK,
+					 0x0);
+		if (unlikely(rc))
+			return rc;
 	}
+
+	/* Store in provided buffer */
 	rssi->cir_pwr = cir_pwr;
 	rssi->pacc_cnt = pacc_cnt;
 	rssi->prf_64mhz = ((config->rxCode >= 9) && (config->rxCode <= 24));
@@ -1657,31 +1675,88 @@ static int dw3000_rx_store_rssi(struct dw3000 *dw)
 		return rc;
 	rssi->dgc_dec = dgc_dec;
 
-	trace_dw3000_rx_rssi(dw, chip_name, sts, rssi->cir_pwr, rssi->pacc_cnt,
-			     rssi->prf_64mhz, rssi->dgc_dec);
 	return 0;
 }
 
 /**
- * dw3000_rx_stats_inc() - Increment statistics
- * @dw: the DW device
- * @item: statistics item
+ * dw3000_rx_stats_inc() - Increment statistics.
+ * @dw: The DW device.
+ * @item: Statistics item.
+ * @rssi: The RSSI information to store.
  *
  * Return: 0 on success, else a negative error code.
  */
-static inline int dw3000_rx_stats_inc(struct dw3000 *dw,
-				      const enum dw3000_stats_items item)
+int dw3000_rx_stats_inc(struct dw3000 *dw, const enum dw3000_stats_items item,
+			struct dw3000_rssi *rssi)
 {
+	struct dw3000_stats *stats = &dw->stats;
+	const char *chip_name = dw3000_get_chip_name(dw);
+	bool sts_enabled = _dw3000_sts_is_enabled(dw);
+	u16 idx;
+
+	/* Increment per-item counter */
+	stats->count[item]++;
+
+	/* Save provided RSSI data */
+	if (!rssi)
+		return 0;
+
+	/* Retrieve current idx */
+	idx = stats->indexes[item];
+	/* RSSI array is divided in two parts, RX_GOOD at first */
+	idx += (DW3000_RSSI_REPORTS_MAX >> 1) * (item == DW3000_STATS_RX_ERROR);
+	stats->rssi[idx] = *rssi;
+
+	/* Update where to store next RSSI */
+	stats->indexes[item]++;
+	if (stats->indexes[item] >= (DW3000_RSSI_REPORTS_MAX >> 1))
+		stats->indexes[item] = 0;
+	/* TODO(UWB-3455): Shall we reset count[item] too to maintain current behaviour?
+	 *                 See also do_tm_cmd_get_rx_diag() function.
+	 */
+
+	/* Trace RSSI data*/
+	trace_dw3000_rx_rssi(dw, chip_name, sts_enabled, rssi->cir_pwr,
+			     rssi->pacc_cnt, rssi->prf_64mhz, rssi->dgc_dec);
+	return 0;
+}
+
+/**
+ * dw3000_rx_calc_rssi() - RSSI computation.
+ * @dw: The DW device.
+ * @rssi: The RSSI related informations.
+ * @info: the MCPS information structure to fill with RSSI.
+ * @sts: Current STS mode.
+ *
+ * This function checks if RSSI is required: by explicit MAC request or by
+ * stats, then retrieve, store and output it by tracepoint
+ * or in a structure field.
+ *
+ * Return: 0 on success, else a negative error code.
+ */
+int dw3000_rx_calc_rssi(struct dw3000 *dw, struct dw3000_rssi *rssi,
+			struct mcps802154_rx_frame_info *info, u8 sts)
+{
+	bool rssi_required = info->flags & MCPS802154_RX_FRAME_INFO_RSSI;
 	int rc = 0;
-	if (dw->stats.enabled) {
-		if (dw->stats.count[item] >= DW3000_RSSI_REPORTS_MAX)
-			dw->stats.count[item] = 0;
-		dw->stats.count[item]++;
-		if (item == DW3000_STATS_RX_GOOD) {
-			rc = dw3000_rx_store_rssi(dw);
-		}
-	}
-	return rc;
+	bool rx_tune;
+	u8 reg;
+
+	if (!rssi_required)
+		return 0;
+
+	/* Get RX_TUNE_EN bit required by the RSSI formula */
+	/* TODO: move this in dw3000_configure_dgc() to cache this value in
+	   struct dw3000_local_data and avoid this read. */
+	rc = dw3000_reg_read8(dw, DW3000_DGC_CFG_ID, 0, &reg);
+	if (rc)
+		return rc;
+	rx_tune = reg & DW3000_DGC_CFG_RX_TUNE_EN_BIT_MASK;
+	/* Compute RSSI and give the result to the upper layer in Q7.1 */
+	info->rssi = dw->chip_ops->compute_rssi(dw, rssi, rx_tune, sts) << 1;
+	if (!info->rssi)
+		info->flags &= ~MCPS802154_RX_FRAME_INFO_RSSI;
+	return 0;
 }
 
 static int dw3000_power_supply_one(struct regulator *regulator, bool onoff)
@@ -1727,6 +1802,12 @@ static int dw3000_power_supply(struct dw3000 *dw, int onoff)
 			onoff ? "enable" : "disable", rc);
 		return rc;
 	}
+	/* Set 2p5 reg to 2.7V  */
+	rc = regulator_set_voltage(power->regulator_2p5, 2700000, 2700000);
+	if (rc < 0) {
+		dev_err(dw->dev, "regulator failed to set voltage :%d\n", rc);
+		return rc;
+	}
 
 	rc = dw3000_power_supply_one(power->regulator_vdd, onoff);
 	if (rc < 0) {
@@ -1962,6 +2043,37 @@ static inline int dw3000_setpreambledetecttimeout(struct dw3000 *dw,
 	return 0;
 }
 
+/**
+ * dw3000_setrxtimeout() - Set the reception timeout
+ * @dw: the DW device
+ * @timeout_dly: reception total time timeout in dly unit.
+ *
+ * timeout value 0 will disable the timeout.
+ *
+ * Return: zero on success, else a negative error code.
+ */
+static inline int dw3000_setrxtimeout(struct dw3000 *dw, u32 timeout_dly)
+{
+	struct dw3000_local_data *local = &dw->data;
+	int rc;
+	if (local->rx_frame_timeout_dly == timeout_dly)
+		return 0;
+	if (timeout_dly) {
+		rc = dw3000_reg_write32(dw, DW3000_RX_FWTO_ID, 0, timeout_dly);
+		if (unlikely(rc))
+			return rc;
+		rc = dw3000_reg_or16(dw, DW3000_SYS_CFG_ID, 0,
+				     DW3000_SYS_CFG_RXWTOE_BIT_MASK);
+	} else {
+		rc = dw3000_reg_and16(dw, DW3000_SYS_CFG_ID, 0,
+				      (u16)~DW3000_SYS_CFG_RXWTOE_BIT_MASK);
+	}
+	if (unlikely(rc))
+		return rc;
+	local->rx_frame_timeout_dly = timeout_dly;
+	return 0;
+}
+
 static inline int dw3000_setdelayedtrxtime(struct dw3000 *dw, u32 starttime)
 {
 	u32 sys_starttime = dw3000_dtu_to_sys_time(dw, starttime);
@@ -2052,6 +2164,7 @@ static int do_wakeup(struct dw3000 *dw, const void *in, void *out)
 
 int dw3000_deepsleep_wakeup_now(struct dw3000 *dw,
 				dw3000_idle_timeout_cb idle_timeout_cb,
+				u32 timestamp_dtu,
 				enum operational_state next_operational_state)
 {
 	struct dw3000_deep_sleep_state *dss = &dw->deep_sleep_state;
@@ -2063,6 +2176,7 @@ int dw3000_deepsleep_wakeup_now(struct dw3000 *dw,
 
 	dss->next_operational_state = next_operational_state;
 	dw->idle_timeout_cb = idle_timeout_cb;
+	dw->idle_timeout_dtu = timestamp_dtu;
 	return 0;
 }
 
@@ -2155,6 +2269,7 @@ int dw3000_idle_cancel_timer(struct dw3000 *dw)
 		return -ENOENT;
 	/* Ensure wakeup ISR don't call the mcps802154_timer_expired() */
 	dw->idle_timeout_cb = NULL;
+	dw->idle_timeout = false;
 	return 0;
 }
 
@@ -2213,7 +2328,7 @@ int dw3000_check_operational_state(struct dw3000 *dw, int delay_dtu,
 		dw->deep_sleep_state.next_operational_state);
 
 	if (dw->current_operational_state == DW3000_OP_STATE_OFF)
-		return -EIO;
+		return -ENODEV;
 	/* In deep sleep or wake up in progress, we can store parameters only
 	   if no other operation queued. */
 	if ((dw->current_operational_state < DW3000_OP_STATE_IDLE_PLL) &&
@@ -2239,7 +2354,8 @@ int dw3000_check_operational_state(struct dw3000 *dw, int delay_dtu,
 		rc = dw3000_wakeup(dw);
 		if (unlikely(rc))
 			return rc;
-		/* fallthrough */
+		/* Use kernel defined statement which exist since kernel 5.4. */
+		fallthrough;
 	case DW3000_OP_STATE_WAKE_UP:
 		/* Inform caller to save parameters. Stored operation will redo
 		   deep sleep if needed. */
@@ -2410,7 +2526,7 @@ stop_coex:
 /**
  * dw3000_do_rx_enable() - handle RX enable MCPS operation
  * @dw: the DW device to put in RX mode
- * @info: RX enable parameters from MCPS
+ * @config: RX enable parameters from MCPS
  * @frame_idx: Frame index in a continuous block
  *
  * This function is called to execute all required operation to enable RX on
@@ -2428,13 +2544,15 @@ stop_coex:
  * Return: 0 on success, else a negative error code.
  */
 int dw3000_do_rx_enable(struct dw3000 *dw,
-			const struct mcps802154_rx_info *info, int frame_idx)
+			const struct mcps802154_rx_frame_config *config,
+			int frame_idx)
 {
 	struct dw3000_deep_sleep_state *dss = &dw->deep_sleep_state;
 	struct mcps802154_llhw *llhw = dw->llhw;
 	u32 cur_time_dtu = 0;
 	u32 rx_date_dtu = 0;
 	u32 timeout_pac = 0;
+	u32 frame_timeout_dly = 0;
 	bool rx_delayed = true;
 	int delay_dtu = 0;
 	bool can_sync = false;
@@ -2442,27 +2560,19 @@ int dw3000_do_rx_enable(struct dw3000 *dw,
 	bool pdoa_enabled;
 	u8 sts_mode;
 
-	trace_dw3000_mcps_rx_enable(dw, info->flags, info->timeout_dtu);
+	trace_dw3000_mcps_rx_enable(dw, config->flags, config->timeout_dtu);
 
 	/* Ensure CFO is checked if responder wait first frame of round. */
-	dw->data.check_cfo = !!(info->flags & MCPS802154_RX_INFO_RANGING_ROUND);
+	dw->data.check_cfo =
+		!!(config->flags & MCPS802154_RX_FRAME_CONFIG_RANGING_ROUND);
 	/* Calculate the transfer date. */
-	if (info->flags & MCPS802154_RX_INFO_TIMESTAMP_DTU) {
+	if (config->flags & MCPS802154_RX_FRAME_CONFIG_TIMESTAMP_DTU) {
 		rx_date_dtu =
-			info->timestamp_dtu - DW3000_RX_ENABLE_STARTUP_DTU;
+			config->timestamp_dtu - DW3000_RX_ENABLE_STARTUP_DTU;
 	} else {
 		/* Receive immediately. */
 		rx_delayed = false;
 	}
-	/* Calculate the timeout. */
-	if (info->timeout_dtu == 0) {
-		timeout_pac = dw->pre_timeout_pac;
-	} else if (info->timeout_dtu != -1) {
-		timeout_pac = dw->pre_timeout_pac +
-			      dtu_to_pac(llhw, info->timeout_dtu);
-	} else {
-		/* No timeout. */
-	}
 
 	if (rx_delayed) {
 		cur_time_dtu = dw3000_get_dtu_time(dw);
@@ -2483,47 +2593,73 @@ int dw3000_do_rx_enable(struct dw3000 *dw,
 	/* For delayed RX, where delay_dtu != 0, enter/leave deep sleep */
 	rc = dw3000_check_operational_state(dw, delay_dtu, can_sync);
 	if (rc) {
-		/* Handle error cases first */
-		if (rc < 0)
+		/*
+		 * Handle error cases first :
+		 *  - Do not fail if we are in deep sleep/wakeup state
+		 */
+		if (rc < 0 && rc != -EIO)
 			return rc;
 		/* Save parameters to activate RX delayed when
 		   wakeup later */
 
 		dw->wakeup_done_cb = dw3000_wakeup_done_to_rx;
 		dss->next_operational_state = DW3000_OP_STATE_RX;
-		dss->rx_info = *info;
+		dss->rx_config = *config;
 		dss->frame_idx = frame_idx;
 		return 0;
 	}
 	/* All operation below require the DW chip is in IDLE_PLL state */
 
+	/* Calculate the preamble timeout. */
+	if (config->timeout_dtu == 0) {
+		timeout_pac = dw->pre_timeout_pac;
+	} else if (config->timeout_dtu != -1) {
+		timeout_pac = dw->pre_timeout_pac +
+			      dtu_to_pac(llhw, config->timeout_dtu);
+	} else {
+		/* No timeout. */
+	}
+
+	/* Add the frame timeout if needed. */
+	if (timeout_pac && config->frame_timeout_dtu) {
+		frame_timeout_dly =
+			dtu_to_dly(llhw, config->frame_timeout_dtu) +
+			pac_to_dly(llhw, timeout_pac);
+	}
+
 	/* Start SPI queuing mode to minimise number of SPI messages
 	   Queue will be flushed by dw3000_tx_frame() itself. */
 	dw3000_spi_queue_start(dw);
 
 	/* Enable STS */
-	pdoa_enabled = !!(info->flags & MCPS802154_RX_INFO_RANGING_PDOA);
-	sts_mode = FIELD_GET(MCPS802154_RX_INFO_STS_MODE_MASK, info->flags);
+	pdoa_enabled =
+		!!(config->flags & MCPS802154_RX_FRAME_CONFIG_RANGING_PDOA);
+	sts_mode = FIELD_GET(MCPS802154_RX_FRAME_CONFIG_STS_MODE_MASK,
+			     config->flags);
 	rc = dw3000_set_sts_pdoa(dw, sts_mode,
 				 sts_to_pdoa(sts_mode, pdoa_enabled));
 	if (unlikely(rc))
 		goto fail;
 	/* Ensure correct RX antennas are selected. */
-	rc = dw3000_set_rx_antennas(dw, info->ant_set_id, pdoa_enabled);
+	rc = dw3000_set_rx_antennas(dw, config->ant_set_id, pdoa_enabled);
 	if (unlikely(rc))
 		goto fail;
-	if (info->flags & MCPS802154_RX_INFO_AACK) {
+	if (config->flags & MCPS802154_RX_FRAME_CONFIG_AACK) {
 		dw3000_enable_autoack(dw, false);
 	} else {
 		dw3000_disable_autoack(dw, false);
 	}
+	rc = dw3000_setrxtimeout(dw, frame_timeout_dly);
+	if (unlikely(rc))
+		goto fail;
 	rc = dw3000_rx_enable(dw, rx_delayed, rx_date_dtu, timeout_pac);
 	if (unlikely(rc))
 		goto fail;
 
 	/* Store ranging clock requirement for next operation */
 	dw->need_ranging_clock =
-		(info->flags & MCPS802154_RX_INFO_KEEP_RANGING_CLOCK) != 0;
+		(config->flags &
+		 MCPS802154_RX_FRAME_CONFIG_KEEP_RANGING_CLOCK) != 0;
 
 fail:
 	if (rc)
@@ -2537,6 +2673,7 @@ static irqreturn_t dw3000_irq_handler(int irq, void *context)
 {
 	struct dw3000 *dw = context;
 
+	atomic64_inc(&dw->power.interrupts);
 	dw3000_enqueue_irq(dw);
 
 	return IRQ_HANDLED;
@@ -3323,7 +3460,8 @@ int dw3000_tx_frame(struct dw3000 *dw, struct sk_buff *skb, bool tx_delayed,
 	}
 
 	if (skb) {
-		len = skb->len + IEEE802154_FCS_LEN;
+		/* FCS is already included while pctt is enabled */
+		len = skb->len + (dw->pctt.enabled ? 0 : IEEE802154_FCS_LEN);
 		/* Write frame properties to the transmit frame control register */
 		if (WARN_ON(len > dw->data.max_frames_len))
 			return -EINVAL;
@@ -3364,8 +3502,7 @@ int dw3000_tx_frame(struct dw3000 *dw, struct sk_buff *skb, bool tx_delayed,
 		if (unlikely(rc))
 			goto stop_coex;
 		/* W4R mode are handled by TX event IRQ handler */
-		dw3000_power_stats(dw, DW3000_PWR_TX,
-				   skb ? skb->len + IEEE802154_FCS_LEN : 0);
+		dw3000_power_stats(dw, DW3000_PWR_TX, len);
 		return 0;
 	}
 
@@ -3386,8 +3523,7 @@ int dw3000_tx_frame(struct dw3000 *dw, struct sk_buff *skb, bool tx_delayed,
 		goto stop_coex;
 
 	/* W4R mode are handled by TX event IRQ handler */
-	dw3000_power_stats(dw, DW3000_PWR_TX,
-			   skb ? skb->len + IEEE802154_FCS_LEN : 0);
+	dw3000_power_stats(dw, DW3000_PWR_TX, len);
 	/* Check if late */
 	rc = dw3000_check_hpdwarn(dw);
 	if (unlikely(rc)) {
@@ -3407,7 +3543,7 @@ stop_coex:
 /**
  * dw3000_do_tx_frame() - handle TX frame MCPS operation
  * @dw: the device on which transmit frame
- * @info: TX parameters from MCPS
+ * @config: TX parameters from MCPS
  * @skb: the frame to transmit
  * @frame_idx: Frame index in a continuous block
  *
@@ -3427,7 +3563,7 @@ stop_coex:
  * Return: 0 on success, else a negative error code.
  */
 int dw3000_do_tx_frame(struct dw3000 *dw,
-		       const struct mcps802154_tx_frame_info *info,
+		       const struct mcps802154_tx_frame_config *config,
 		       struct sk_buff *skb, int frame_idx)
 {
 	struct dw3000_deep_sleep_state *dss = &dw->deep_sleep_state;
@@ -3443,16 +3579,16 @@ int dw3000_do_tx_frame(struct dw3000 *dw,
 	int rc;
 	u8 sts_mode;
 
-	trace_dw3000_mcps_tx_frame(dw, info->flags, skb ? skb->len : 0);
+	trace_dw3000_mcps_tx_frame(dw, config->flags, skb ? skb->len : 0);
 
 	/* Calculate the transfer date.*/
-	if (info->flags & MCPS802154_TX_FRAME_TIMESTAMP_DTU) {
-		tx_date_dtu = info->timestamp_dtu + llhw->shr_dtu;
+	if (config->flags & MCPS802154_TX_FRAME_CONFIG_TIMESTAMP_DTU) {
+		tx_date_dtu = config->timestamp_dtu + llhw->shr_dtu;
 	} else {
 		/* Send immediately. */
 		tx_delayed = false;
 	}
-	if (info->flags & MCPS802154_TX_FRAME_RANGING)
+	if (config->flags & MCPS802154_TX_FRAME_CONFIG_RANGING)
 		ranging = true;
 
 	if (tx_delayed) {
@@ -3482,7 +3618,7 @@ int dw3000_do_tx_frame(struct dw3000 *dw,
 		   wakeup later */
 		dw->wakeup_done_cb = dw3000_wakeup_done_to_tx;
 		dss->next_operational_state = DW3000_OP_STATE_TX;
-		dss->tx_info = *info;
+		dss->tx_config = *config;
 		dss->tx_skb = skb;
 		dss->frame_idx = frame_idx;
 		return 0;
@@ -3493,35 +3629,48 @@ int dw3000_do_tx_frame(struct dw3000 *dw,
 	   Queue will be flushed by dw3000_tx_frame() itself. */
 	dw3000_spi_queue_start(dw);
 
+	/* Oscillate XTAL around calibrated value to maximise successful PDoA probability */
+	if (DW3000_XTAL_BIAS &&
+	    (config->flags & MCPS802154_TX_FRAME_CONFIG_RANGING_ROUND)) {
+		dw->data.xtal_bias = (dw->data.xtal_bias > 0 ?
+					      -DW3000_XTAL_BIAS :
+					      DW3000_XTAL_BIAS);
+		rc = dw3000_prog_xtrim(dw);
+		if (unlikely(rc))
+			goto fail;
+	}
 	/* Enable STS */
-	sts_mode = FIELD_GET(MCPS802154_TX_FRAME_STS_MODE_MASK, info->flags);
+	sts_mode = FIELD_GET(MCPS802154_TX_FRAME_CONFIG_STS_MODE_MASK,
+			     config->flags);
 	rc = dw3000_set_sts_pdoa(
 		dw, sts_mode,
 		sts_to_pdoa(sts_mode,
-			    info->flags & MCPS802154_TX_FRAME_RANGING_PDOA));
+			    config->flags &
+				    MCPS802154_TX_FRAME_CONFIG_RANGING_PDOA));
 	if (unlikely(rc))
 		goto fail;
 	/* Ensure correct TX antenna is selected. */
-	rc = dw3000_set_tx_antenna(dw, info->ant_set_id);
+	rc = dw3000_set_tx_antenna(dw, config->ant_set_id);
 	if (unlikely(rc))
 		goto fail;
 
-	if (info->rx_enable_after_tx_dtu > 0) {
+	if (config->rx_enable_after_tx_dtu > 0) {
 		/* Disable auto-ack if it was previously enabled. */
 		dw3000_disable_autoack(dw, false);
 		/* Calculate the after tx rx delay. */
-		rx_delay_dly = dtu_to_dly(llhw, info->rx_enable_after_tx_dtu) -
-			       DW3000_RX_ENABLE_STARTUP_DLY;
+		rx_delay_dly =
+			dtu_to_dly(llhw, config->rx_enable_after_tx_dtu) -
+			DW3000_RX_ENABLE_STARTUP_DLY;
 		rx_delay_dly = rx_delay_dly >= 0 ? rx_delay_dly : 0;
 		/* Calculate the after tx rx timeout. */
-		if (info->rx_enable_after_tx_timeout_dtu == 0) {
+		if (config->rx_enable_after_tx_timeout_dtu == 0) {
 			rx_timeout_pac = dw->pre_timeout_pac;
-		} else if (info->rx_enable_after_tx_timeout_dtu != -1) {
+		} else if (config->rx_enable_after_tx_timeout_dtu != -1) {
 			rx_timeout_pac =
 				dw->pre_timeout_pac +
 				dtu_to_pac(
 					llhw,
-					info->rx_enable_after_tx_timeout_dtu);
+					config->rx_enable_after_tx_timeout_dtu);
 		} else {
 			/* No timeout. */
 		}
@@ -3533,7 +3682,8 @@ int dw3000_do_tx_frame(struct dw3000 *dw,
 
 	/* Store ranging clock requirement for next operation */
 	dw->need_ranging_clock =
-		(info->flags & MCPS802154_TX_FRAME_KEEP_RANGING_CLOCK) != 0;
+		(config->flags &
+		 MCPS802154_TX_FRAME_CONFIG_KEEP_RANGING_CLOCK) != 0;
 fail:
 	if (rc)
 		/* Ensure SPI queuing mode disabled on error */
@@ -3596,6 +3746,8 @@ static int dw3000_rx_frame(struct dw3000 *dw,
 			rc = -ENOMEM;
 			goto err_spi;
 		}
+		if (dw->pctt.enabled)
+			len += IEEE802154_FCS_LEN;
 		buffer = skb_put(skb, len);
 		/* Directly read data from the IC to the buffer */
 		rc = dw3000_rx_read_data(dw, buffer, len, 0);
@@ -3614,7 +3766,7 @@ static int dw3000_rx_frame(struct dw3000 *dw,
 	spin_unlock_irqrestore(&rx->lock, flags);
 	/* Print the received frame in hexadecimal characters */
 	if (unlikely(DEBUG)) {
-		dev_dbg(dw->dev, "frame info: len=%lu, rxflags=0x%.2x", len,
+		dev_dbg(dw->dev, "frame config: len=%lu, rxflags=0x%.2x", len,
 			data->rx_flags);
 		if (skb)
 			print_hex_dump_bytes("dw3000: frame data: ",
@@ -4113,6 +4265,11 @@ static inline int dw3000_configure_rf(struct dw3000 *dw)
 	rc = dw3000_reg_write16(dw, DW3000_PLL_CFG_ID, 0, rf_pll_cfg);
 	if (rc)
 		return rc;
+
+	rc = dw3000_reg_write16(dw, DW3000_PLL_COMMON_ID, 0, DW3000_RF_PLL_COMMON);
+	if (rc)
+		return rc;
+
 	rc = dw3000_reg_write8(dw, DW3000_LDO_RLOAD_ID, 1,
 			       DW3000_LDO_RLOAD_VAL_B1);
 	if (rc)
@@ -4250,6 +4407,8 @@ int dw3000_configure_chan(struct dw3000 *dw)
 	rc = dw3000_configure_rf(dw);
 	if (rc)
 		return rc;
+	/* Disable AGC */
+	dw3000_reg_modify32(dw, DW3000_AGC_CFG_ID, 0, DW3000_AGC_DIS_MASK, 0);
 	/* Configure DGC. */
 	return dw3000_configure_dgc(dw);
 }
@@ -4682,7 +4841,7 @@ static int dw3000_wakeup_done_to_tx(struct dw3000 *dw)
 
 	trace_dw3000_wakeup_done_to_tx(dw);
 	dss->next_operational_state = dw->current_operational_state;
-	return dw3000_do_tx_frame(dw, &dss->tx_info, dss->tx_skb,
+	return dw3000_do_tx_frame(dw, &dss->tx_config, dss->tx_skb,
 				  dss->frame_idx);
 }
 
@@ -4698,7 +4857,7 @@ static int dw3000_wakeup_done_to_rx(struct dw3000 *dw)
 
 	trace_dw3000_wakeup_done_to_rx(dw);
 	dss->next_operational_state = dw->current_operational_state;
-	return dw3000_do_rx_enable(dw, &dss->rx_info, dss->frame_idx);
+	return dw3000_do_rx_enable(dw, &dss->rx_config, dss->frame_idx);
 }
 
 /**
@@ -4715,15 +4874,26 @@ static int dw3000_wakeup_done_to_idle(struct dw3000 *dw)
 	trace_dw3000_wakeup_done_to_idle(dw);
 	if (dw->idle_timeout) {
 		u32 now_dtu = dw3000_get_dtu_time(dw);
-		int idle_duration_us =
-			DTU_TO_US(dw->idle_timeout_dtu - now_dtu);
+		int idle_duration_dtu = dw->idle_timeout_dtu - now_dtu;
 
 		/* TODO:
 		 * 2 timers implemented (idle, deep_sleep),
 		 * or a better FSM (enter, leave, events, state)
-		 * should help to follow/repect timestamps expected. */
-		dw3000_wakeup_timer_start(dw, idle_duration_us);
-		return 0;
+		 * should help to follow/respect timestamp expected. */
+		/* WORKAROUND: On a delayed wake-up (by CPU high load),
+		 * the idle_timeout_dtu date can be in the past. And to avoid
+		 * to program a timer in the past, process the idle_timeout_cb
+		 * immediately when duration is negative or equal to 0.
+		 * For NFCC_COEX, the NFC software will enter in "LATE"
+		 * processing, and return immediately too. Thus the CPU high
+		 * load will not broken dw3000 driver and mac layer. */
+		if (idle_duration_dtu > 0) {
+			int idle_duration_us = DTU_TO_US(idle_duration_dtu);
+
+			dw3000_wakeup_timer_start(dw, idle_duration_us);
+			return 0;
+		}
+		trace_dw3000_wakeup_done_to_idle_late(dw);
 	}
 	return dw3000_enqueue_generic(dw, &cmd);
 }
@@ -4765,16 +4935,19 @@ int dw3000_idle(struct dw3000 *dw, bool timestamp, u32 timestamp_dtu,
 	dw->idle_timeout_dtu = timestamp_dtu;
 	if (timestamp) {
 		int deepsleep_delay_us;
+		int idle_delay_dtu;
 		int idle_delay_us;
 		bool is_sleeping = dw->current_operational_state ==
 				   DW3000_OP_STATE_DEEP_SLEEP;
 
 		cur_time_dtu = dw3000_get_dtu_time(dw);
-		idle_delay_us = DTU_TO_US(timestamp_dtu - cur_time_dtu);
-		if (idle_delay_us < 0) {
+		idle_delay_dtu =
+			timestamp_dtu - cur_time_dtu - dw->llhw->anticip_dtu;
+		if (idle_delay_dtu < 0) {
 			rc = -ETIME;
 			goto eof;
 		}
+		idle_delay_us = DTU_TO_US(idle_delay_dtu);
 		/* Warning: timestamp_dtu have already taken in consideration
 		 * WakeUp latency! */
 		deepsleep_delay_us = idle_delay_us;
@@ -4786,6 +4959,7 @@ int dw3000_idle(struct dw3000 *dw, bool timestamp, u32 timestamp_dtu,
 		 * which haven't the same timeout_dtu! */
 		if (deepsleep_delay_us < 0) {
 			dw3000_deepsleep_wakeup_now(dw, idle_timeout_cb,
+						    dw->idle_timeout_dtu,
 						    DW3000_OP_STATE_MAX);
 			rc = 0;
 			goto eof;
@@ -5058,6 +5232,8 @@ static int dw3000_configure(struct dw3000 *dw)
 
 	/* Update configuration dependent timings */
 	dw3000_update_timings(dw);
+	/* update VOUT */
+	rc = dw3000_reg_write32(dw, DW3000_LDO_VOUT_ID, 0, DW3000_RF_LDO_VOUT);
 	return rc;
 }
 
@@ -5144,7 +5320,7 @@ int dw3000_set_shortaddr(struct dw3000 *dw, __le16 val)
 }
 
 /**
- * dw3000_configure_hw_addr_filt() - Set device's hardware address filtering 
+ * dw3000_configure_hw_addr_filt() - Set device's hardware address filtering
  * parameters
  * @dw: the DW device
  * @changed: bitfields of flags indicating parameters which have changed
@@ -5574,6 +5750,23 @@ int dw3000_disable_autoack(struct dw3000 *dw, bool force)
 }
 
 /**
+ * dw3000_enable_auto_fcs() - Enable or disable the automatic FCS adding
+ * @dw: the DW device
+ * @on: enable the auto FCS adding if true, otherwise disable it
+ *
+ * Return: zero on success, else a negative error code.
+ */
+int dw3000_enable_auto_fcs(struct dw3000 *dw, bool on)
+{
+	if (!on) {
+		return dw3000_reg_or32(dw, DW3000_SYS_CFG_ID, 0,
+				       DW3000_SYS_CFG_DIS_FCS_TX_BIT_MASK);
+	}
+	return dw3000_reg_and32(dw, DW3000_SYS_CFG_ID, 0,
+				(~DW3000_SYS_CFG_DIS_FCS_TX_BIT_MASK));
+}
+
+/**
  * dw3000_otp_read32() - 32 bits OTP memory read.
  * @dw: the DW device
  * @addr: address in the OTP memory
@@ -6589,22 +6782,17 @@ int dw3000_read_frame_cir_data(struct dw3000 *dw,
 	/* Get packet type */
 	cir->type = stsMode;
 
-	/* Get tdoa */
-	if (info->flags & MCPS802154_RX_FRAME_INFO_RANGING_PDOA) {
-		cir->tdoa = info->ranging_pdoa_rad_q11;
-	} else {
-		cir->tdoa = 0;
-	}
-
 	/* CIA algorithm have to finish before results reading */
 	if (!(dw->rx.flags & DW3000_RX_FLAG_CIA)) {
 		rc = -ENODATA;
 		goto read_frame_cir_error;
 	}
 
+	dw->full_cia_read = false;
 	rc = dw3000_read_ciaregs(dw);
 	if (rc)
 		goto read_frame_cir_error;
+	dw->full_cia_read = true;
 	dw3000_read_fp_power(dw, stsMode);
 	dw3000_read_cir_acc(dw, stsMode);
 	rc = dw3000_acc_clken(dw, true);
@@ -7106,6 +7294,10 @@ void dw3000_init_config(struct dw3000 *dw)
 	/* Set default antenna ports configuration */
 	dw->calib_data.ant[0].port = 0;
 	dw->calib_data.ant[1].port = 1;
+	/* By default WiFi Coex is enabled */
+	dw->coex_enabled = true;
+	for (i = 0; i < DW3000_CALIBRATION_CHANNEL_MAX; i++)
+		dw->calib_data.ch[i].wifi_coex_enabled = true;
 	/* Ensure power stats timing start at load time */
 	dw->power.cur_state = DW3000_PWR_OFF;
 	dw->power.stats[DW3000_PWR_OFF].count = 1;
@@ -7218,8 +7410,8 @@ static inline int dw3000_isr_handle_spi_ready(struct dw3000 *dw,
 			   dw3000_dtu_to_ktime(dw, dw->sleep_enter_dtu);
 		next_date_dtu = dss->next_operational_state ==
 						DW3000_OP_STATE_RX ?
-					dss->rx_info.timestamp_dtu :
-					dss->tx_info.timestamp_dtu;
+					dss->rx_config.timestamp_dtu :
+					dss->tx_config.timestamp_dtu;
 		trace_dw3000_wakeup_done(dw, sleep_ns / 1000,
 					 dw->sleep_enter_dtu, next_date_dtu,
 					 dss->next_operational_state);
@@ -7282,6 +7474,9 @@ static inline int dw3000_isr_handle_spi_ready(struct dw3000 *dw,
 
 	/* TODO: So, just add below this line more required unsaved registers
 	 * setup. */
+	 rc = dw3000_reg_write32(dw, DW3000_LDO_VOUT_ID, 0, DW3000_RF_LDO_VOUT);
+	 if (rc)
+		 return rc;
 
 setuperror:
 #ifdef CONFIG_DW3000_DEBUG
@@ -7298,7 +7493,7 @@ setuperror:
 			if (wakeup_done_cb == dw3000_wakeup_done_to_tx)
 				mcps802154_tx_too_late(dw->llhw);
 			else
-				mcps802154_rx_too_late(dw->llhw);
+				 mcps802154_rx_too_late(dw->llhw);
 
 			rc = 0;
 		}
@@ -7392,10 +7587,6 @@ static inline int dw3000_isr_handle_rx_call_handler(struct dw3000 *dw,
 	u32 eof_dtu;
 	int rc;
 
-	/* Report statistics */
-	rc = dw3000_rx_stats_inc(dw, DW3000_STATS_RX_GOOD);
-	if (unlikely(rc))
-		return rc;
 	/* Store LDE/STS RX errors in rx_flags */
 	if (isr->status & DW3000_SYS_STATUS_CIAERR_BIT_MASK)
 		isr->rx_flags |= DW3000_RX_FLAG_CER;
@@ -7411,7 +7602,7 @@ static inline int dw3000_isr_handle_rx_call_handler(struct dw3000 *dw,
 	if (unlikely(rc))
 		return rc;
 	isr->rx_flags |= DW3000_RX_FLAG_TS; /* don't read it again later */
-	eof_dtu = (u32)(isr->ts_rctu / DW3000_RCTU_PER_DTU) +
+	eof_dtu = dw3000_sys_time_rctu_to_dtu(dw, isr->ts_rctu) +
 		  dw3000_frame_duration_dtu(dw, isr->datalength, false);
 	/* Update power statistics */
 	dw3000_power_stats(dw, DW3000_PWR_IDLE, eof_dtu);
@@ -7478,11 +7669,11 @@ static inline int dw3000_isr_handle_rxto_event(struct dw3000 *dw, u32 status)
 		return 0;
 	/* Update power statistics */
 	end_dtu = dw->power.rx_start + (dw->data.rx_timeout_pac + 1) *
-					       dw->chips_per_pac *
+					       dw->chips_per_pac /
 					       DW3000_CHIP_PER_DTU;
 	dw3000_power_stats(dw, DW3000_PWR_IDLE, end_dtu);
 	/* Report statistics */
-	rc = dw3000_rx_stats_inc(dw, DW3000_STATS_RX_TO);
+	rc = dw3000_rx_stats_inc(dw, DW3000_STATS_RX_TO, NULL);
 	if (unlikely(rc))
 		goto err;
 	/* Inform upper layer */
@@ -7500,7 +7691,6 @@ static inline int dw3000_isr_handle_rxerr_event(struct dw3000 *dw, u32 status)
 {
 	struct mcps802154_llhw *llhw = dw->llhw;
 	enum mcps802154_rx_error_type error;
-	int rc;
 
 	/* If rx_disable() callback was called, we can't call mcps802154_rx_error() */
 	if (dw3000_rx_busy(dw, true))
@@ -7508,7 +7698,11 @@ static inline int dw3000_isr_handle_rxerr_event(struct dw3000 *dw, u32 status)
 	/* Update power statistics */
 	dw3000_power_stats(dw, DW3000_PWR_IDLE, 0);
 	/* Map error to mcps802154_rx_error_type enum */
-	if (status & DW3000_SYS_STATUS_RXSTO_BIT_MASK) {
+	if (status & (DW3000_SYS_STATUS_RXPTO_BIT_MASK |
+		      DW3000_SYS_STATUS_RXFTO_BIT_MASK)) {
+		dev_dbg(dw->dev, "rx preamble timeout\n");
+		error = MCPS802154_RX_ERROR_TIMEOUT;
+	} else if (status & DW3000_SYS_STATUS_RXSTO_BIT_MASK) {
 		dev_dbg(dw->dev, "rx sfd timeout\n");
 		error = MCPS802154_RX_ERROR_SFD_TIMEOUT;
 	} else if (status & DW3000_SYS_STATUS_ARFE_BIT_MASK) {
@@ -7522,7 +7716,7 @@ static inline int dw3000_isr_handle_rxerr_event(struct dw3000 *dw, u32 status)
 		error = MCPS802154_RX_ERROR_BAD_CKSUM;
 	} else if (status & DW3000_SYS_STATUS_RXPHE_BIT_MASK) {
 		dev_dbg(dw->dev, "rx phr error\n");
-		error = MCPS802154_RX_ERROR_OTHER;
+		error = MCPS802154_RX_ERROR_PHR_DECODE;
 	} else if (status & DW3000_SYS_STATUS_RXFSL_BIT_MASK) {
 		dev_dbg(dw->dev, "rx sync loss\n");
 		error = MCPS802154_RX_ERROR_OTHER;
@@ -7530,15 +7724,11 @@ static inline int dw3000_isr_handle_rxerr_event(struct dw3000 *dw, u32 status)
 		dev_dbg(dw->dev, "rx error 0x%x\n", status);
 		error = MCPS802154_RX_ERROR_OTHER;
 	}
-	/* Report statistics */
-	rc = dw3000_rx_stats_inc(dw, DW3000_STATS_RX_ERROR);
-	if (unlikely(rc))
-		goto err;
 	/* Report RX error event */
 	mcps802154_rx_error(llhw, error);
-err:
+
 	WARN_ON_ONCE(dw3000_rx_busy(dw, false));
-	return rc;
+	return 0;
 }
 
 static inline int dw3000_isr_handle_tx_event(struct dw3000 *dw,
@@ -7931,7 +8121,8 @@ static ssize_t dw3000_sysfs_show(struct kobject *kobj,
 			"Run state:\n\tcount:\t%llu\n\tdur ns:\t%llu\n"
 			"Idle state:\n\tcount:\t%llu\n\tdur ns:\t%llu\n"
 			"Tx state:\n\tcount:\t%llu\n\tdur ns:\t%llu\n"
-			"Rx state:\n\tcount:\t%llu\n\tdur ns:\t%llu\n",
+			"Rx state:\n\tcount:\t%llu\n\tdur ns:\t%llu\n"
+			"Interrupts:\n\tcount:\t%lld\n",
 			dw->power.stats[DW3000_PWR_OFF].count,
 			dw->power.stats[DW3000_PWR_OFF].dur,
 			dw->power.stats[DW3000_PWR_DEEPSLEEP].count,
@@ -7940,7 +8131,8 @@ static ssize_t dw3000_sysfs_show(struct kobject *kobj,
 			dw->power.stats[DW3000_PWR_RUN].dur,
 			dw->power.stats[DW3000_PWR_IDLE].count, idle_dur,
 			dw->power.stats[DW3000_PWR_TX].count, tx_ns,
-			dw->power.stats[DW3000_PWR_RX].count, rx_ns);
+			dw->power.stats[DW3000_PWR_RX].count, rx_ns,
+			(s64)atomic64_read(&dw->power.interrupts));
 	return ret;
 }
 
@@ -7957,6 +8149,7 @@ static ssize_t dw3000_sysfs_store(struct kobject *kobj,
 		dw->power.stats[cstate].count = 1;
 		if (dw->power.cur_state > DW3000_PWR_RUN)
 			dw->power.stats[dw->power.cur_state].count = 1;
+		atomic64_set(&dw->power.interrupts, 0);
 	}
 	return length;
 }
@@ -7985,3 +8178,189 @@ void dw3000_sysfs_remove(struct dw3000 *dw)
 		kobject_del(&dw->sysfs_power_dir);
 	}
 }
+
+/**
+ * dw3000_nfcc_coex_prepare_config() - Prepare the configuration before nfcc
+ * coex.
+ * @dw: The DW device.
+ *
+ * Return: Zero on success, else a negative error code.
+ */
+int dw3000_nfcc_coex_prepare_config(struct dw3000 *dw)
+{
+	int rc;
+
+	trace_dw3000_nfcc_coex_prepare_config(dw);
+
+	/* Disable any rx or tx command in progress. */
+	rc = dw3000_rx_disable(dw);
+	if (rc)
+		return rc;
+
+	/* May need to resync to avoid drift. */
+	rc = dw3000_may_resync(dw);
+	if (rc)
+		return rc;
+
+	/* Reset Wait-for-Response Time. */
+	rc = dw3000_setrxaftertxdelay(dw, 0);
+	if (rc)
+		return rc;
+
+	/* Reset the reception timeout. */
+	rc = dw3000_setrxtimeout(dw, 0);
+	if (rc)
+		return rc;
+
+	/*
+	 * Disable RXPTO behavior for two reasons:
+	 * - CCC firmware doesn't manage it. As the RXPTO_EN is false,
+	 *   then ccc is blocked.
+	 * - Update cached value in dw3000 context for next use.
+	 */
+	return dw3000_setpreambledetecttimeout(dw, 0);
+}
+
+/**
+ * dw3000_nfcc_coex_restore_config() - Restore the configuration after nfcc
+ * coex.
+ * @dw: The DW device.
+ *
+ * Some cache is reset to force the reconfiguration.
+ * Some RF parameters are reconfigured.
+ *
+ * Return: Zero on success, else a negative error code.
+ */
+int dw3000_nfcc_coex_restore_config(struct dw3000 *dw)
+{
+	struct dw3000_local_data *local = &dw->data;
+	struct dw3000_config *config = &dw->config;
+	int rc;
+
+	trace_dw3000_nfcc_coex_restore_config(dw);
+
+	/*
+	 * We want to restore the configuration after nfcc slot.
+	 */
+
+	/*
+	 * Clear security registers related cache.
+	 * The STS parameters will be reset during "set_sts_params" call.
+	 */
+	memset(local->sts_key, 0, AES_KEYSIZE_128);
+	memset(local->sts_iv, 0, AES_BLOCK_SIZE);
+	dw->config.stsLength = 0;
+
+	/* Clear all cache variables to force the reconfiguration. */
+	local->rx_timeout_pac = 0;
+	local->w4r_time = 0;
+	local->ack_time = 0;
+	local->tx_fctrl = 0;
+	local->rx_frame_timeout_dly = 0;
+	local->ack_time = 0;
+
+	/* Configure the SYS_CFG register. */
+	rc = dw3000_configure_sys_cfg(dw, config);
+	if (rc)
+		return rc;
+
+	/* WiFi coexistence initialisation. */
+	rc = dw3000_coex_init(dw);
+	if (rc)
+		return rc;
+
+	/* Configure antenna selection GPIO if any. */
+	rc = dw3000_config_antenna_gpios(dw);
+	if (rc)
+		return rc;
+
+	/*
+	 * Reset cached antenna config to ensure GPIO are reconfigured
+	 * correctly.
+	 */
+	dw->config.ant[0] = -1;
+	dw->config.ant[1] = -1;
+
+	/* Select the events that will trigger an interrupt. */
+	rc = dw3000_set_interrupt(dw, DW3000_SYS_STATUS_TRX,
+				  DW3000_ENABLE_INT_ONLY);
+	if (rc)
+		return rc;
+
+	/*
+	 * PLL already is locked but some RF parameters could be changed.
+	 * So we reprogram the Xtal, the DGC, the ADC, ...
+	 */
+
+	/* Xtal trim could be changed. */
+	rc = dw3000_prog_xtrim(dw);
+	if (rc)
+		return rc;
+
+	/* Configure PLL coarse code, if needed. */
+	if (dw->chip_ops->prog_pll_coarse_code) {
+		rc = dw->chip_ops->prog_pll_coarse_code(dw);
+		if (rc) {
+			dev_err(dw->dev, "device coarse code setup has failed (%d)\n", rc);
+			return rc;
+		}
+	}
+
+	/* Configure delays. */
+	rc = dw3000_set_antenna_delay(dw, 0);
+	if (rc)
+		return rc;
+
+	rc = dw3000_reconfigure_hw_addr_filt(dw);
+	if (rc)
+		return rc;
+
+	/* Do some device specific initialisation if any. */
+	rc = dw->chip_ops->init(dw);
+	if (rc) {
+		dev_err(dw->dev, "device chip specific init has failed (%d)\n",
+			rc);
+		return rc;
+	}
+
+	/* Reconfigure all dependent channels. */
+	rc = dw3000_configure_chan(dw);
+	if (rc)
+		return rc;
+
+	rc = dw3000_pgf_cal(dw, 1);
+	if (rc)
+		return rc;
+
+	/* Calibrate ADC offset, if needed, after DGC configuration and after PLL lock. */
+	if (dw->chip_ops->adc_offset_calibration) {
+		rc = dw->chip_ops->adc_offset_calibration(dw);
+		if (rc)
+			return rc;
+	}
+
+	/* Setup TX preamble size, data rate and SDF timeout count. */
+	rc = dw3000_configure_preamble_length_and_datarate(dw, false);
+	if (rc)
+		return rc;
+
+	/* PHR rate. */
+	rc = dw3000_configure_phr_rate(dw);
+	if (rc)
+		return rc;
+
+	/*
+	 * Ensure STS fields are double-buffered if enabled, also enable stats
+	 * if configured in module parameters.
+	 */
+	rc = dw3000_configure_ciadiag(dw, dw->stats.enabled,
+				      dw->data.dblbuffon ?
+					      DW3000_CIA_DIAG_LOG_DBL_MID :
+					      DW3000_CIA_DIAG_LOG_DBL_OFF);
+	if (rc) {
+		dev_err(dw->dev, "device CIA DIAG setup has failed (%d)\n", rc);
+		return rc;
+	}
+
+	return 0;
+}
diff --git a/kernel/drivers/net/ieee802154/dw3000_core.h b/kernel/drivers/net/ieee802154/dw3000_core.h
index 6294d40..bf936b9 100644
--- a/kernel/drivers/net/ieee802154/dw3000_core.h
+++ b/kernel/drivers/net/ieee802154/dw3000_core.h
@@ -199,7 +199,7 @@ enum spi_modes {
 /* Size of RX LUT configuration tables */
 #define DW3000_CONFIGMRXLUT_MAX 7
 #define DW3000_DGC_CFG 0x38
-#define DW3000_DGC_CFG0 0x10000240
+#define DW3000_DGC_CFG0 0x00000240
 #define DW3000_DGC_CFG1 0x1a491248
 #define DW3000_DGC_CFG2 0x2db248db
 
@@ -374,6 +374,7 @@ int dw3000_configure_sts_iv(struct dw3000 *dw, const u8 *iv);
 int dw3000_load_sts_iv(struct dw3000 *dw);
 int dw3000_configure_sys_cfg(struct dw3000 *dw, struct dw3000_config *config);
 int dw3000_configure_hw_addr_filt(struct dw3000 *dw, unsigned long changed);
+int dw3000_enable_auto_fcs(struct dw3000 *dw, bool on);
 
 int dw3000_clear_sys_status(struct dw3000 *dw, u32 clear_bits);
 int dw3000_clear_dss_status(struct dw3000 *dw, u8 clear_bits);
@@ -383,12 +384,20 @@ int dw3000_read_rdb_status(struct dw3000 *dw, u8 *status);
 int dw3000_read_sys_status(struct dw3000 *dw, u32 *status);
 int dw3000_read_sys_time(struct dw3000 *dw, u32 *sys_time);
 
+int dw3000_rx_store_rssi(struct dw3000 *dw, struct dw3000_rssi *rssi,
+			 u8 pkt_sts);
+int dw3000_rx_calc_rssi(struct dw3000 *dw, struct dw3000_rssi *rssi,
+			struct mcps802154_rx_frame_info *info, u8 pkt_sts);
+int dw3000_rx_stats_inc(struct dw3000 *dw, const enum dw3000_stats_items item,
+			struct dw3000_rssi *rssi);
+
 u32 dw3000_get_dtu_time(struct dw3000 *dw);
 
 int dw3000_forcetrxoff(struct dw3000 *dw);
 
 int dw3000_do_rx_enable(struct dw3000 *dw,
-			const struct mcps802154_rx_info *info, int frame_idx);
+			const struct mcps802154_rx_frame_config *config,
+			int frame_idx);
 int dw3000_rx_enable(struct dw3000 *dw, bool rx_delayed, u32 date_dtu,
 		     u32 timeout_pac);
 int dw3000_rx_disable(struct dw3000 *dw);
@@ -400,9 +409,9 @@ void dw3000_rx_stats_clear(struct dw3000 *dw);
 int dw3000_enable_autoack(struct dw3000 *dw, bool force);
 int dw3000_disable_autoack(struct dw3000 *dw, bool force);
 
-struct mcps802154_tx_frame_info;
+struct mcps802154_tx_frame_config;
 int dw3000_do_tx_frame(struct dw3000 *dw,
-		       const struct mcps802154_tx_frame_info *info,
+		       const struct mcps802154_tx_frame_config *config,
 		       struct sk_buff *skb, int frame_idx);
 
 int dw3000_tx_setcwtone(struct dw3000 *dw, bool on);
@@ -446,6 +455,7 @@ int dw3000_idle(struct dw3000 *dw, bool timestamp, u32 timestamp_dtu,
 		enum operational_state next_operational_state);
 int dw3000_deepsleep_wakeup_now(struct dw3000 *dw,
 				dw3000_idle_timeout_cb idle_timeout_cb,
+				u32 timestamp_dtu,
 				enum operational_state next_operational_state);
 int dw3000_can_deep_sleep(struct dw3000 *dw, int delay_us);
 int dw3000_trace_rssi_info(struct dw3000 *dw, u32 regid, char *chipver);
@@ -453,6 +463,8 @@ int dw3000_trace_rssi_info(struct dw3000 *dw, u32 regid, char *chipver);
 int dw3000_testmode_continuous_tx_start(struct dw3000 *dw, u32 frame_length,
 					u32 rate);
 int dw3000_testmode_continuous_tx_stop(struct dw3000 *dw);
+int dw3000_nfcc_coex_prepare_config(struct dw3000 *dw);
+int dw3000_nfcc_coex_restore_config(struct dw3000 *dw);
 
 /* Preamble length related information. */
 struct dw3000_plen_info {
@@ -498,6 +510,21 @@ static inline int dw3000_compute_shr_dtu(struct dw3000 *dw)
 	return shr_symb * chip_per_symb / DW3000_CHIP_PER_DTU;
 }
 
+static inline int compute_shr_dtu_from_conf(
+	const struct mcps802154_hrp_uwb_params *hrp_uwb_params)
+{
+	const int preamble_symb = hrp_uwb_params->psr;
+	const int chip_per_symb =
+		_prf_info[hrp_uwb_params->prf == MCPS802154_PRF_64 ?
+				  DW3000_PRF_64M :
+				  DW3000_PRF_16M]
+			.chip_per_symb;
+	/* The only possible sfd number of symbols is 8. */
+	const int sfd_symb = 8;
+	const int shr_symb = preamble_symb + sfd_symb;
+	return shr_symb * chip_per_symb / DW3000_CHIP_PER_DTU;
+}
+
 static inline int dw3000_compute_symbol_dtu(struct dw3000 *dw)
 {
 	const int chip_per_symb =
@@ -654,6 +681,23 @@ static inline u32 dw3000_sys_time_to_dtu(struct dw3000 *dw, u32 sys_time,
 }
 
 /**
+ * dw3000_sys_time_rctu_to_dtu() - compute current DTU time from RCTU.
+ * @dw: the DW device.
+ * @timestamp_rctu: The DW device RX_STAMP register value in RCTU to convert to DTU.
+ * The RCTU, Ranging Counter Time Unit, is approximately 15.65 picoseconds long.
+ *
+ * Return: The corresponding DTU time.
+ */
+static inline u32 dw3000_sys_time_rctu_to_dtu(struct dw3000 *dw,
+					      u64 timestamp_rctu)
+{
+	u32 sys_time = (u32)(timestamp_rctu / DW3000_RCTU_PER_SYS);
+	u32 dtu_near = dw3000_get_dtu_time(dw) - DW3000_DTU_FREQ;
+
+	return dw3000_sys_time_to_dtu(dw, sys_time, dtu_near);
+}
+
+/**
  * dw3000_reset_rctu_conv_state() - reset RCTU converter
  * @dw: the DW device
  *
@@ -676,6 +720,13 @@ static inline void dw3000_resync_rctu_conv_state(struct dw3000 *dw)
 		dw->rctu_conv.state = ALIGNED;
 }
 
+static inline int pac_to_dly(struct mcps802154_llhw *llhw, int pac)
+{
+	struct dw3000 *dw = llhw->priv;
+
+	return (pac * dw->chips_per_pac / DW3000_CHIP_PER_DLY);
+}
+
 static inline int dtu_to_pac(struct mcps802154_llhw *llhw, int timeout_dtu)
 {
 	struct dw3000 *dw = llhw->priv;
diff --git a/kernel/drivers/net/ieee802154/dw3000_core_reg.h b/kernel/drivers/net/ieee802154/dw3000_core_reg.h
index 7ba4a8b..8847bf8 100644
--- a/kernel/drivers/net/ieee802154/dw3000_core_reg.h
+++ b/kernel/drivers/net/ieee802154/dw3000_core_reg.h
@@ -137,6 +137,9 @@
 #define DW3000_TX_FCTRL_TXFLEN_BIT_LEN (10U)
 #define DW3000_TX_FCTRL_TXFLEN_BIT_MASK 0x3ffU
 
+/* register RX_FWTO */
+#define DW3000_RX_FWTO_ID 0x34
+
 /* register SYS_ENABLE_LO */
 #define DW3000_SYS_ENABLE_LO_ID 0x3c
 #define DW3000_SYS_ENABLE_LO_LEN (4U)
@@ -646,6 +649,7 @@
 #define DW3000_AGC_CFG_ID 0x30014
 #define DW3000_AGC_CFG_LEN (4U)
 #define DW3000_AGC_CFG_MASK 0xFFFFFFFFUL
+#define DW3000_AGC_DIS_MASK 0xFFFFFFFEUL
 
 /* Register DGC_CFG. */
 #define DW3000_DGC_CFG_ID 0x30018
@@ -1365,6 +1369,9 @@
 #define DW3000_LDO_CTRL_LDO_VDDMS1_EN_BIT_LEN (1U)
 #define DW3000_LDO_CTRL_LDO_VDDMS1_EN_BIT_MASK 0x1U
 
+/* register LDO_VOUT*/
+#define DW3000_LDO_VOUT_ID  0x7004C
+
 /* register LDO_RLOAD */
 #define DW3000_LDO_RLOAD_ID 0x70050
 
@@ -1399,6 +1406,11 @@
 /* register PLL_CFG */
 #define DW3000_PLL_CFG_ID 0x90000
 
+/* register PLL_COMMON  */
+#define DW3000_PLL_COMMON_ID 0x90010
+#define DW3000_PLL_COMMON_LEN (2U)
+#define DW3000_PLL_COMMON_MASK 0x0000FFFFUL
+
 /* register XTAL */
 #define DW3000_XTAL_ID 0x90014
 #define DW3000_XTAL_TRIM_BIT_OFFSET (0U)
diff --git a/kernel/drivers/net/ieee802154/dw3000_core_tests.c b/kernel/drivers/net/ieee802154/dw3000_core_tests.c
index 15f3f85..ed69b63 100644
--- a/kernel/drivers/net/ieee802154/dw3000_core_tests.c
+++ b/kernel/drivers/net/ieee802154/dw3000_core_tests.c
@@ -5,6 +5,7 @@ static u64 kunit_get_boottime_ns(void);
 
 /* Replace ktime_get_boottime_ns calls to kunit_get_boottime_ns calls. */
 #define ktime_get_boottime_ns kunit_get_boottime_ns
+#define dw3000_get_dtu_time kunit_dw3000_get_dtu_time
 #define trace_dw3000_power_stats(dw, state, boot_time_ns, len_or_date) \
 	do {                                                           \
 	} while (0)
@@ -118,6 +119,7 @@ const struct dw3000_prf_info _prf_info[] = {
 
 /* Static variable declarations */
 static u64 kunit_boot_time_ns;
+static u32 kunit_dtu_time;
 
 /**
  * kunit_get_boottime_ns() - ktime_get_boottime_ns replacement for tests
@@ -130,6 +132,17 @@ static u64 kunit_get_boottime_ns(void)
 	return kunit_boot_time_ns;
 }
 
+/**
+ * kunit_dw3000_get_dtu_time() - dw3000_get_dtu_time kunit wrapper
+ * @dw: the DW device
+ *
+ * Return: The current simulated DTU time.
+ */
+u32 kunit_dw3000_get_dtu_time(struct dw3000 *dw)
+{
+	return kunit_dtu_time;
+}
+
 /* Define the test cases. */
 
 static void dw3000_ktime_to_dtu_test_basic(struct kunit *test)
@@ -213,6 +226,52 @@ static void dw3000_sys_time_to_dtu_test_basic(struct kunit *test)
 			dw3000_sys_time_to_dtu(dw, 0x13ea64u, dtu_near));
 }
 
+static void dw3000_sys_time_rctu_to_dtu_test_basic(struct kunit *test)
+{
+	struct dw3000 *dw = kunit_kzalloc(test, sizeof(*dw), GFP_KERNEL);
+
+	/* Ensure allocation succeeded. */
+	KUNIT_ASSERT_NOT_ERR_OR_NULL(test, dw);
+
+	/* Tests with dtu_sync == 0, sys_time_sync == 0 and dtu_near == 0.
+	 * Set kunit_dtu_time to DW3000_DTU_FREQ to get dtu_near == 0 in
+	 * dw3000_sys_time_rctu_to_dtu(). */
+	kunit_dtu_time = DW3000_DTU_FREQ;
+	KUNIT_EXPECT_EQ(test, 0u, dw3000_sys_time_rctu_to_dtu(dw, 0));
+	KUNIT_EXPECT_EQ(test, 1u,
+			dw3000_sys_time_rctu_to_dtu(dw, DW3000_RCTU_PER_DTU));
+
+	/* Tests with dtu_near == 10000.
+	 * Set kunit_dtu_time to DW3000_DTU_FREQ + 10000 to get dtu_near == 10000
+	 * in dw3000_sys_time_rctu_to_dtu(). */
+	kunit_dtu_time = DW3000_DTU_FREQ + 10000;
+	KUNIT_EXPECT_EQ(
+		test, 1005u,
+		dw3000_sys_time_rctu_to_dtu(dw, 1005 * DW3000_RCTU_PER_DTU));
+
+	/* Tests with dtu_sync == 1000000/16 & sys_time_sync == 1000000 */
+	dw->sys_time_sync = 1000000;
+	dw->dtu_sync = 1000000 >> 4;
+	KUNIT_EXPECT_EQ(
+		test, 10000u,
+		dw3000_sys_time_rctu_to_dtu(dw, 10000 * DW3000_RCTU_PER_DTU));
+
+	/* Tests with real values from traces:
+	 * timestamp_rctu: 63852263355
+	 * dtu_sync: 13025
+	 * sys_time_sync: 5414
+	 * dtu_near: 4349
+	 * dw3000_sys_time_rctu_to_dtu() return: 15601618
+	 *
+	 * Set kunit_dtu_time to DW3000_DTU_FREQ + 4349 to get dtu_near == 4349
+	 * in dw3000_sys_time_rctu_to_dtu(). */
+	kunit_dtu_time = DW3000_DTU_FREQ + 4349;
+	dw->dtu_sync = 13025;
+	dw->sys_time_sync = 5414;
+	KUNIT_EXPECT_EQ(test, 15601618u,
+			dw3000_sys_time_rctu_to_dtu(dw, 63852263355));
+}
+
 static void power_stats_test_setup(struct dw3000 *dw)
 {
 	struct dw3000_power *pwr = &dw->power;
@@ -441,6 +500,7 @@ static struct kunit_case dw3000_core_test_cases[] = {
 	KUNIT_CASE(dw3000_dtu_to_ktime_test_basic),
 	KUNIT_CASE(dw3000_dtu_to_sys_time_test_basic),
 	KUNIT_CASE(dw3000_sys_time_to_dtu_test_basic),
+	KUNIT_CASE(dw3000_sys_time_rctu_to_dtu_test_basic),
 	KUNIT_CASE(dw3000_power_stats_test_basic),
 	KUNIT_CASE(dw3000_power_stats_test_tx),
 	KUNIT_CASE(dw3000_power_stats_test_rx),
diff --git a/kernel/drivers/net/ieee802154/dw3000_debugfs.c b/kernel/drivers/net/ieee802154/dw3000_debugfs.c
index c2f6b2d..33b88c1 100644
--- a/kernel/drivers/net/ieee802154/dw3000_debugfs.c
+++ b/kernel/drivers/net/ieee802154/dw3000_debugfs.c
@@ -762,10 +762,12 @@ int dw3000_debugsfs_init(struct dw3000 *dw)
  */
 void dw3000_debugfs_remove(struct dw3000 *dw)
 {
-	struct dw3000_debugfs_file *cur;
-
-	list_for_each_entry (cur, &dw->debugfs.dbgfile_list, ll) {
+	while (!list_empty(&dw->debugfs.dbgfile_list)) {
+		struct dw3000_debugfs_file *cur =
+			list_first_entry(&dw->debugfs.dbgfile_list,
+					struct dw3000_debugfs_file, ll);
 		debugfs_remove(cur->file);
+		list_del(&cur->ll);
 		kfree(cur);
 	}
 
diff --git a/kernel/drivers/net/ieee802154/dw3000_mcps.c b/kernel/drivers/net/ieee802154/dw3000_mcps.c
index 4fc6875..f1810a7 100644
--- a/kernel/drivers/net/ieee802154/dw3000_mcps.c
+++ b/kernel/drivers/net/ieee802154/dw3000_mcps.c
@@ -37,6 +37,7 @@
 #include "dw3000_pctt_mcps.h"
 #include "dw3000_coex.h"
 #include "dw3000_cir.h"
+#include "dw3000_power_stats.h"
 
 static int completion_active(struct completion *completion)
 {
@@ -47,12 +48,14 @@ static int completion_active(struct completion *completion)
 #endif
 }
 
-static inline u32 timestamp_rctu_to_dtu(struct dw3000 *dw, u64 timestamp_rctu);
 static inline u64 timestamp_rctu_to_rmarker_rctu(struct dw3000 *dw,
 						 u64 timestamp_rctu,
 						 u32 rmarker_dtu);
 
-static inline u32 tx_rmarker_offset(struct dw3000 *dw, int ant_set_id)
+static inline u32
+tx_rmarker_offset(struct dw3000 *dw,
+		  const struct mcps802154_channel *channel_params,
+		  int ant_set_id)
 {
 	struct dw3000_config *config = &dw->config;
 	const struct dw3000_antenna_calib *ant_calib;
@@ -60,6 +63,9 @@ static inline u32 tx_rmarker_offset(struct dw3000 *dw, int ant_set_id)
 	int chanidx;
 	int prfidx;
 	s8 ant_idx1, ant_idx2;
+	int chan = channel_params ? channel_params->channel : config->chan;
+	int pcode = channel_params ? channel_params->preamble_code :
+				     config->txCode;
 
 	if (ant_set_id < 0 || ant_set_id >= ANTSET_ID_MAX) {
 		dev_err(dw->dev,
@@ -86,10 +92,10 @@ static inline u32 tx_rmarker_offset(struct dw3000 *dw, int ant_set_id)
 
 	ant_calib = &dw->calib_data.ant[ant_idx1];
 
-	chanidx = config->chan == 9 ? DW3000_CALIBRATION_CHANNEL_9 :
-				      DW3000_CALIBRATION_CHANNEL_5;
-	prfidx = config->txCode >= 9 ? DW3000_CALIBRATION_PRF_64MHZ :
-				       DW3000_CALIBRATION_PRF_16MHZ;
+	chanidx = chan == 9 ? DW3000_CALIBRATION_CHANNEL_9 :
+			      DW3000_CALIBRATION_CHANNEL_5;
+	prfidx = pcode >= 9 ? DW3000_CALIBRATION_PRF_64MHZ :
+			      DW3000_CALIBRATION_PRF_16MHZ;
 
 	ant_calib_prf = &ant_calib->ch[chanidx].prf[prfidx];
 
@@ -240,7 +246,7 @@ static void stop(struct mcps802154_llhw *llhw)
 
 struct do_tx_frame_params {
 	struct sk_buff *skb;
-	const struct mcps802154_tx_frame_info *info;
+	const struct mcps802154_tx_frame_config *config;
 	int frame_idx;
 };
 
@@ -249,30 +255,30 @@ static int do_tx_frame(struct dw3000 *dw, const void *in, void *out)
 	const struct do_tx_frame_params *params =
 		(const struct do_tx_frame_params *)in;
 
-	return dw3000_do_tx_frame(dw, params->info, params->skb,
+	return dw3000_do_tx_frame(dw, params->config, params->skb,
 				  params->frame_idx);
 }
 
 static int tx_frame(struct mcps802154_llhw *llhw, struct sk_buff *skb,
-		    const struct mcps802154_tx_frame_info *info, int frame_idx,
-		    int next_delay_dtu)
+		    const struct mcps802154_tx_frame_config *config,
+		    int frame_idx, int next_delay_dtu)
 {
 	struct dw3000 *dw = llhw->priv;
 	struct do_tx_frame_params params = { .skb = skb,
-					     .info = info,
+					     .config = config,
 					     .frame_idx = frame_idx };
 	struct dw3000_stm_command cmd = { do_tx_frame, &params, NULL };
 
 	/* Check data : no data if SP3, must have data otherwise */
-	if (((info->flags & MCPS802154_TX_FRAME_STS_MODE_MASK) ==
-	     MCPS802154_TX_FRAME_SP3) != !skb)
+	if (((config->flags & MCPS802154_TX_FRAME_CONFIG_STS_MODE_MASK) ==
+	     MCPS802154_TX_FRAME_CONFIG_SP3) != !skb)
 		return -EINVAL;
 
 	return dw3000_enqueue_generic(dw, &cmd);
 }
 
 struct do_rx_frame_params {
-	const struct mcps802154_rx_info *info;
+	const struct mcps802154_rx_frame_config *config;
 	int frame_idx;
 };
 
@@ -281,15 +287,15 @@ static int do_rx_enable(struct dw3000 *dw, const void *in, void *out)
 	const struct do_rx_frame_params *params =
 		(const struct do_rx_frame_params *)in;
 
-	return dw3000_do_rx_enable(dw, params->info, params->frame_idx);
+	return dw3000_do_rx_enable(dw, params->config, params->frame_idx);
 }
 
 static int rx_enable(struct mcps802154_llhw *llhw,
-		     const struct mcps802154_rx_info *info, int frame_idx,
-		     int next_delay_dtu)
+		     const struct mcps802154_rx_frame_config *config,
+		     int frame_idx, int next_delay_dtu)
 {
 	struct dw3000 *dw = llhw->priv;
-	struct do_rx_frame_params params = { .info = info,
+	struct do_rx_frame_params params = { .config = config,
 					     .frame_idx = frame_idx };
 	struct dw3000_stm_command cmd = { do_rx_enable, &params, NULL };
 
@@ -387,6 +393,30 @@ static int get_ranging_sts_fom(struct mcps802154_llhw *llhw,
 	/* DW3000 only support one STS segment. */
 	info->ranging_sts_fom[0] =
 		clamp(1 + sts_acc_qual * 254 / sts_acc_max, 1, 255);
+	/* Set FoM of all other segments to maximum value so that they do not
+	 * cause quality check failure. */
+	memset(&info->ranging_sts_fom[1], 0xFF, MCPS802154_STS_N_SEGS_MAX - 1);
+	return ret;
+}
+
+static int rx_get_rssi(struct dw3000 *dw, struct mcps802154_rx_frame_info *info,
+		       const enum dw3000_stats_items item)
+{
+	struct dw3000_config *config = &dw->config;
+	int ret = 0;
+
+	if (dw->stats.enabled || info->flags & MCPS802154_RX_FRAME_INFO_RSSI) {
+		struct dw3000_rssi rssi;
+		u8 sts = config->stsMode & DW3000_STS_BASIC_MODES_MASK;
+		ret = dw3000_rx_store_rssi(dw, &rssi, sts);
+		if (ret) {
+			info->flags &= ~MCPS802154_RX_FRAME_INFO_RSSI;
+			return ret;
+		}
+		if (dw->stats.enabled)
+			dw3000_rx_stats_inc(dw, item, &rssi);
+		ret = dw3000_rx_calc_rssi(dw, &rssi, info, sts);
+	}
 	return ret;
 }
 
@@ -437,7 +467,7 @@ static int rx_get_frame(struct mcps802154_llhw *llhw, struct sk_buff **skb,
 				MCPS802154_RX_FRAME_INFO_RANGING_STS_TIMESTAMP_RCTU);
 		else {
 			u32 rmarker_dtu =
-				timestamp_rctu_to_dtu(dw, timestamp_rctu);
+				dw3000_sys_time_rctu_to_dtu(dw, timestamp_rctu);
 			u64 rmarker_rctu = timestamp_rctu_to_rmarker_rctu(
 				dw, timestamp_rctu, rmarker_dtu);
 			info->timestamp_rctu =
@@ -470,12 +500,6 @@ static int rx_get_frame(struct mcps802154_llhw *llhw, struct sk_buff **skb,
 		if (unlikely(ret))
 			goto error;
 	}
-	/* In case of PDoA. */
-	if (info->flags & MCPS802154_RX_FRAME_INFO_RANGING_PDOA) {
-		info->ranging_pdoa_rad_q11 = dw3000_read_pdoa(dw);
-		info->ranging_aoa_rad_q11 =
-			dw3000_pdoa_to_aoa_lut(dw, info->ranging_pdoa_rad_q11);
-	}
 	/* In case of STS */
 	if (info->flags & MCPS802154_RX_FRAME_INFO_RANGING_STS_TIMESTAMP_RCTU) {
 		u64 sts_ts_rctu;
@@ -519,6 +543,17 @@ static int rx_get_frame(struct mcps802154_llhw *llhw, struct sk_buff **skb,
 		info->ranging_tracking_interval_rctu = 1 << 26;
 	}
 
+	/* If dbgfs file is opened & waiting for data, fill structure and wake-up reading process */
+	if (dw->cir_data && completion_active(&dw->cir_data->complete)) {
+		ret = dw3000_read_frame_cir_data(dw, info, pkt_ts);
+		if (ret)
+			goto error;
+	}
+	/* Report statistics and if required process RSSI */
+	ret = rx_get_rssi(dw, info, DW3000_STATS_RX_GOOD);
+	if (ret)
+		goto error;
+
 	/* Keep only implemented. */
 	info->flags &= (MCPS802154_RX_FRAME_INFO_TIMESTAMP_RCTU |
 			MCPS802154_RX_FRAME_INFO_TIMESTAMP_DTU |
@@ -527,16 +562,9 @@ static int rx_get_frame(struct mcps802154_llhw *llhw, struct sk_buff **skb,
 			MCPS802154_RX_FRAME_INFO_RANGING_PDOA_FOM |
 			MCPS802154_RX_FRAME_INFO_RANGING_STS_FOM |
 			MCPS802154_RX_FRAME_INFO_RANGING_STS_TIMESTAMP_RCTU |
-			MCPS802154_RX_FRAME_INFO_RANGING_OFFSET);
+			MCPS802154_RX_FRAME_INFO_RANGING_OFFSET |
+			MCPS802154_RX_FRAME_INFO_RSSI);
 	trace_dw3000_return_int_u32(dw, info->flags, *skb ? (*skb)->len : 0);
-
-	/* If dbgfs file is opened & waiting for data, fill structure and wake-up reading process */
-	if (dw->cir_data && completion_active(&dw->cir_data->complete)) {
-		ret = dw3000_read_frame_cir_data(dw, info, pkt_ts);
-		if (ret)
-			goto error;
-	}
-
 	return 0;
 
 error:
@@ -559,15 +587,65 @@ static int rx_get_error_frame(struct mcps802154_llhw *llhw,
 	if (info->flags & MCPS802154_RX_FRAME_INFO_TIMESTAMP_RCTU) {
 		if (dw3000_read_rx_timestamp(dw, &info->timestamp_rctu))
 			info->flags &= ~MCPS802154_RX_FRAME_INFO_TIMESTAMP_RCTU;
-	} else {
-		/* Not implemented */
-		info->flags = 0;
 	}
+	/* Report statistics and if required process RSSI */
+	ret = rx_get_rssi(dw, info, DW3000_STATS_RX_ERROR);
+	if (ret)
+		goto error;
+	/* Keep only implemented. */
+	info->flags &= (MCPS802154_RX_FRAME_INFO_TIMESTAMP_RCTU |
+			MCPS802154_RX_FRAME_INFO_RSSI);
+
 error:
 	trace_dw3000_return_int_u32(dw, ret, info->flags);
 	return ret;
 }
 
+/**
+ * rx_get_measurement - Update measurement.
+ * @llhw: Low-level device pointer.
+ * @rx_ctx: Custom rx context (can be NULL).
+ * @info: Measurement to update.
+ *
+ * Return: 0 or error.
+ */
+static int rx_get_measurement(struct mcps802154_llhw *llhw, void *rx_ctx,
+			      struct mcps802154_rx_measurement_info *info)
+{
+	struct dw3000 *dw = llhw->priv;
+
+	if (info->flags & MCPS802154_RX_MEASUREMENTS_AOAS) {
+		info->aoas[0].pdoa_rad_q11 = dw3000_read_pdoa(dw);
+		info->aoas[0].aoa_rad_q11 =
+			dw3000_pdoa_to_aoa_lut(dw, info->aoas[0].pdoa_rad_q11);
+		info->n_aoas = 1;
+	}
+
+	/* TODO: UWB-4961 Usage of a mcps802154_rx_frame_info is a
+	 * workaround used until rx_get_rssi() can be fully removed
+	 * from rx_get_frame(). */
+	if (info->flags & MCPS802154_RX_MEASUREMENTS_RSSIS) {
+		struct mcps802154_rx_frame_info frame_info;
+		int ret;
+
+		frame_info.flags = MCPS802154_RX_FRAME_INFO_RSSI;
+		ret = rx_get_rssi(dw, &frame_info, DW3000_STATS_RX_GOOD);
+		if (ret) {
+			info->n_rssis = 0;
+		} else {
+			info->rssis_q1[0] = frame_info.rssi;
+			/* Only one RSSI computed per frame */
+			info->n_rssis = 1;
+		}
+	}
+
+	/* Keep only implemented. */
+	info->flags &= MCPS802154_RX_MEASUREMENTS_AOAS |
+		       MCPS802154_RX_MEASUREMENTS_RSSIS;
+
+	return 0;
+}
+
 static int dw3000_handle_idle_timeout(struct dw3000 *dw)
 {
 	/* MCPS feeback must be done outside driver kthread. */
@@ -660,14 +738,6 @@ static int get_current_timestamp_dtu(struct mcps802154_llhw *llhw,
 	return ret;
 }
 
-static inline u32 timestamp_rctu_to_dtu(struct dw3000 *dw, u64 timestamp_rctu)
-{
-	u32 sys_time = (u32)(timestamp_rctu / DW3000_RCTU_PER_SYS);
-	u32 dtu_near = dw3000_get_dtu_time(dw) - DW3000_DTU_FREQ;
-
-	return dw3000_sys_time_to_dtu(dw, sys_time, dtu_near);
-}
-
 static inline u64 timestamp_rctu_to_rmarker_rctu(struct dw3000 *dw,
 						 u64 timestamp_rctu,
 						 u32 rmarker_dtu)
@@ -696,15 +766,20 @@ static inline u64 timestamp_rctu_to_rmarker_rctu(struct dw3000 *dw,
 	return rmarker_rctu;
 }
 
-static u64 tx_timestamp_dtu_to_rmarker_rctu(struct mcps802154_llhw *llhw,
-					    u32 tx_timestamp_dtu,
-					    int ant_set_id)
+static u64 tx_timestamp_dtu_to_rmarker_rctu(
+	struct mcps802154_llhw *llhw, u32 tx_timestamp_dtu,
+	const struct mcps802154_hrp_uwb_params *hrp_uwb_params,
+	const struct mcps802154_channel *channel_params, int ant_set_id)
 {
 	struct dw3000 *dw = llhw->priv;
 	struct dw3000_rctu_conv *rctu = &dw->rctu_conv;
 
-	const u32 rmarker_dtu = tx_timestamp_dtu + llhw->shr_dtu;
-	const u32 ant_offset = tx_rmarker_offset(dw, ant_set_id);
+	const u32 shr_dtu = hrp_uwb_params ?
+				    compute_shr_dtu_from_conf(hrp_uwb_params) :
+				    llhw->shr_dtu;
+	const u32 rmarker_dtu = tx_timestamp_dtu + shr_dtu;
+	const u32 ant_offset =
+		tx_rmarker_offset(dw, channel_params, ant_set_id);
 	u64 rmarker_rctu;
 	s64 rmarker_diff_dtu;
 
@@ -822,9 +897,11 @@ static int do_set_hrp_uwb_params(struct dw3000 *dw, const void *in, void *out)
 		dss->config_changed |= changed;
 		return 0;
 	}
-	if (changed & DW3000_PREAMBLE_LENGTH_CHANGED) {
-		/* Reconfigure preamble size and SDF timeout count */
-		rc = dw3000_configure_preamble_length_and_datarate(dw, false);
+	if (changed &
+	    (DW3000_PREAMBLE_LENGTH_CHANGED | DW3000_DATA_RATE_CHANGED)) {
+		/* reconfigure data rate and preamble size if needed. */
+		rc = dw3000_configure_preamble_length_and_datarate(
+			dw, !(changed & DW3000_PREAMBLE_LENGTH_CHANGED));
 		if (rc)
 			return rc;
 	}
@@ -839,16 +916,69 @@ static int do_set_hrp_uwb_params(struct dw3000 *dw, const void *in, void *out)
 		if (rc)
 			return rc;
 	}
-	/* If DW3000_PREAMBLE_LENGTH_CHANGED is set, data rate is already configured, skip it. */
-	if ((changed & DW3000_DATA_RATE_CHANGED) &&
-	    !(changed & DW3000_PREAMBLE_LENGTH_CHANGED))
-		rc = dw3000_configure_preamble_length_and_datarate(dw, true);
+
+	if (changed & (DW3000_SFD_CHANGED | DW3000_PREAMBLE_LENGTH_CHANGED))
+		dw3000_update_timings(dw);
 
 	return rc;
 }
 
-int set_hrp_uwb_params(struct mcps802154_llhw *llhw, int prf, int psr,
-		       int sfd_selector, int phr_rate, int data_rate)
+static int check_hrp_uwb_params(struct mcps802154_llhw *llhw,
+				const struct mcps802154_hrp_uwb_params *params)
+{
+	switch (params->prf) {
+	case MCPS802154_PRF_16:
+	case MCPS802154_PRF_64:
+		break;
+	case MCPS802154_PRF_125:
+	case MCPS802154_PRF_250:
+		return -ENOTSUPP;
+	default:
+		return -EINVAL;
+	}
+	switch (params->psr) {
+	case MCPS802154_PSR_32:
+	case MCPS802154_PSR_64:
+	case MCPS802154_PSR_128:
+	case MCPS802154_PSR_256:
+	case MCPS802154_PSR_1024:
+	case MCPS802154_PSR_4096:
+		break;
+	case MCPS802154_PSR_16:
+	case MCPS802154_PSR_24:
+	case MCPS802154_PSR_48:
+	case MCPS802154_PSR_96:
+		return -ENOTSUPP;
+	default:
+		return -EINVAL;
+	}
+	switch (params->sfd_selector) {
+	case MCPS802154_SFD_4A:
+	case MCPS802154_SFD_4Z_8:
+		break;
+	case MCPS802154_SFD_4Z_4:
+	case MCPS802154_SFD_4Z_16:
+	case MCPS802154_SFD_4Z_32:
+		return -ENOTSUPP;
+	default:
+		return -EINVAL;
+	}
+	switch (params->data_rate) {
+	case MCPS802154_DATA_RATE_6M81:
+	case MCPS802154_DATA_RATE_850K:
+		break;
+	case MCPS802154_DATA_RATE_7M80:
+	case MCPS802154_DATA_RATE_27M2:
+	case MCPS802154_DATA_RATE_31M2:
+		return -ENOTSUPP;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+int set_hrp_uwb_params(struct mcps802154_llhw *llhw,
+		       const struct mcps802154_hrp_uwb_params *params)
 {
 	unsigned long changed = 0;
 	struct dw3000 *dw = llhw->priv;
@@ -856,6 +986,7 @@ int set_hrp_uwb_params(struct mcps802154_llhw *llhw, int prf, int psr,
 	struct dw3000_stm_command cmd = { do_set_hrp_uwb_params, &changed,
 					  NULL };
 	int ret;
+	int psr, sfd_selector, phr_hi_rate, data_rate;
 
 	/* The prf parameter is not used. This is due to the specificity of
 	 * the DW3000 chip where the prf is not programmed explicitly,
@@ -864,44 +995,58 @@ int set_hrp_uwb_params(struct mcps802154_llhw *llhw, int prf, int psr,
 	 */
 
 	/* Check parameters early */
-	switch (psr) {
-	case DW3000_PLEN_64:
-	case DW3000_PLEN_32:
-	case DW3000_PLEN_72:
-	case DW3000_PLEN_128:
-	case DW3000_PLEN_256:
-	case DW3000_PLEN_512:
-	case DW3000_PLEN_1024:
-	case DW3000_PLEN_1536:
-	case DW3000_PLEN_2048:
-	case DW3000_PLEN_4096:
+	ret = check_hrp_uwb_params(llhw, params);
+	if (ret)
+		return ret;
+
+	switch (params->psr) {
+	case MCPS802154_PSR_32:
+		psr = DW3000_PLEN_32;
+		break;
+	case MCPS802154_PSR_128:
+		psr = DW3000_PLEN_128;
+		break;
+	case MCPS802154_PSR_256:
+		psr = DW3000_PLEN_256;
+		break;
+	case MCPS802154_PSR_1024:
+		psr = DW3000_PLEN_1024;
+		break;
+	case MCPS802154_PSR_4096:
+		psr = DW3000_PLEN_4096;
 		break;
 	default:
-		return -EINVAL;
+		psr = DW3000_PLEN_64;
+		break;
 	}
 
-	switch (sfd_selector) {
-	case DW3000_SFD_TYPE_STD:
-	case DW3000_SFD_TYPE_DW_8:
-	case DW3000_SFD_TYPE_DW_16:
-	case DW3000_SFD_TYPE_4Z:
+	switch (params->sfd_selector) {
+	case MCPS802154_SFD_4A:
+		sfd_selector = DW3000_SFD_TYPE_STD;
 		break;
 	default:
-		return -EINVAL;
+		sfd_selector = DW3000_SFD_TYPE_4Z;
+		break;
 	}
 
-	if (phr_rate != DW3000_PHRRATE_STD && phr_rate != DW3000_PHRRATE_DTA)
-		return -EINVAL;
+	switch (params->data_rate) {
+	case MCPS802154_DATA_RATE_850K:
+		data_rate = DW3000_BR_850K;
+		break;
+	default:
+		data_rate = DW3000_BR_6M8;
+		break;
+	}
 
-	if (data_rate != DW3000_BR_850K && data_rate != DW3000_BR_6M8)
-		return -EINVAL;
+	phr_hi_rate = params->phr_hi_rate ? DW3000_PHRRATE_DTA :
+					    DW3000_PHRRATE_STD;
 
 	/* Detect configuration change(s) */
 	if (config->txPreambLength != psr)
 		changed |= DW3000_PREAMBLE_LENGTH_CHANGED;
 	if (config->sfdType != sfd_selector)
 		changed |= DW3000_SFD_CHANGED;
-	if (config->phrRate != phr_rate)
+	if (config->phrRate != phr_hi_rate)
 		changed |= DW3000_PHR_RATE_CHANGED;
 	if (config->dataRate != data_rate)
 		changed |= DW3000_DATA_RATE_CHANGED;
@@ -911,7 +1056,7 @@ int set_hrp_uwb_params(struct mcps802154_llhw *llhw, int prf, int psr,
 	/* Update configuration structure */
 	config->txPreambLength = psr;
 	config->sfdType = sfd_selector;
-	config->phrRate = phr_rate;
+	config->phrRate = phr_hi_rate;
 	config->dataRate = data_rate;
 
 	/* Reconfigure the chip with it if needed */
@@ -1138,13 +1283,13 @@ static int do_vendor_cmd(struct dw3000 *dw, const void *in, void *out)
 	const struct do_vendor_params *params = in;
 
 	switch (params->subcmd) {
-	case DW3000_VENDOR_CMD_PCTT_SETUP_HW:
+	case LLHW_VENDOR_CMD_PCTT_SETUP_HW:
 		return dw3000_pctt_vendor_cmd(dw, params->vendor_id,
 					      params->subcmd, params->data,
 					      params->data_len);
-	case DW3000_VENDOR_CMD_NFCC_COEX_HANDLE_ACCESS:
-	case DW3000_VENDOR_CMD_NFCC_COEX_GET_ACCESS_INFORMATION:
-	case DW3000_VENDOR_CMD_NFCC_COEX_STOP:
+	case LLHW_VENDOR_CMD_NFCC_COEX_HANDLE_ACCESS:
+	case LLHW_VENDOR_CMD_NFCC_COEX_GET_ACCESS_INFORMATION:
+	case LLHW_VENDOR_CMD_NFCC_COEX_STOP:
 		return dw3000_nfcc_coex_vendor_cmd(dw, params->vendor_id,
 						   params->subcmd, params->data,
 						   params->data_len);
@@ -1176,6 +1321,66 @@ static int vendor_cmd(struct mcps802154_llhw *llhw, u32 vendor_id, u32 subcmd,
 	return dw3000_enqueue_generic(dw, &cmd);
 }
 
+static int get_antenna_caps(struct mcps802154_llhw *llhw, int ant_idx,
+			    uint32_t *caps)
+{
+	struct dw3000 *dw = llhw->priv;
+	const struct dw3000_antenna_calib *ant_calib;
+
+	if (ant_idx < 0 || ant_idx >= ANTMAX) {
+		dev_err(dw->dev, "Bad antenna number (%d)\n", ant_idx);
+		return -EINVAL;
+	}
+
+	ant_calib = &dw->calib_data.ant[ant_idx];
+	*caps = ant_calib->caps;
+	return 0;
+}
+
+/**
+ * get_power_stats() - Forward vendor commands processing to dw3000 function.
+ * @llhw: Low-level hardware without MCPS.
+ * @pwr_stats: mcps802154_power_stats structure to be filled.
+ *
+ * Return: 0 on success or negative error code.
+ */
+static int get_power_stats(struct mcps802154_llhw *llhw,
+			   struct mcps802154_power_stats *pwr_stats)
+{
+	struct dw3000 *dw = llhw->priv;
+	u64 idle_dur, rx_ns, tx_ns;
+
+	/* Update the power statistics if needed. */
+	if (dw->power.cur_state <= DW3000_PWR_IDLE)
+		dw3000_power_stats(dw, dw->power.cur_state, 0);
+	/* TX/RX are kept in DTU unit. Convert it here to limit conversion error */
+	rx_ns = dw->power.stats[DW3000_PWR_RX].dur * 10000 /
+		(DW3000_DTU_FREQ / 100000);
+	tx_ns = dw->power.stats[DW3000_PWR_TX].dur * 10000 /
+		(DW3000_DTU_FREQ / 100000);
+	idle_dur = dw->power.stats[DW3000_PWR_RUN].dur - tx_ns - rx_ns;
+
+	pwr_stats->power_state_stats[MCPS802154_PWR_STATE_OFF].dur =
+		dw->power.stats[DW3000_PWR_OFF].dur;
+	pwr_stats->power_state_stats[MCPS802154_PWR_STATE_OFF].count =
+		dw->power.stats[DW3000_PWR_OFF].count;
+	pwr_stats->power_state_stats[MCPS802154_PWR_STATE_SLEEP].dur =
+		dw->power.stats[DW3000_PWR_DEEPSLEEP].dur;
+	pwr_stats->power_state_stats[MCPS802154_PWR_STATE_SLEEP].count =
+		dw->power.stats[DW3000_PWR_DEEPSLEEP].count;
+	pwr_stats->power_state_stats[MCPS802154_PWR_STATE_IDLE].dur = idle_dur;
+	pwr_stats->power_state_stats[MCPS802154_PWR_STATE_IDLE].count =
+		dw->power.stats[DW3000_PWR_IDLE].count;
+	pwr_stats->power_state_stats[MCPS802154_PWR_STATE_RX].dur = rx_ns;
+	pwr_stats->power_state_stats[MCPS802154_PWR_STATE_RX].count =
+		dw->power.stats[DW3000_PWR_RX].count;
+	pwr_stats->power_state_stats[MCPS802154_PWR_STATE_TX].dur = tx_ns;
+	pwr_stats->power_state_stats[MCPS802154_PWR_STATE_TX].count =
+		dw->power.stats[DW3000_PWR_TX].count;
+	pwr_stats->interrupts = atomic64_read(&dw->power.interrupts);
+	return 0;
+}
+
 static const struct mcps802154_ops dw3000_mcps_ops = {
 	.start = start,
 	.stop = stop,
@@ -1184,6 +1389,7 @@ static const struct mcps802154_ops dw3000_mcps_ops = {
 	.rx_disable = rx_disable,
 	.rx_get_frame = rx_get_frame,
 	.rx_get_error_frame = rx_get_error_frame,
+	.rx_get_measurement = rx_get_measurement,
 	.idle = idle,
 	.reset = reset,
 	.get_current_timestamp_dtu = get_current_timestamp_dtu,
@@ -1192,6 +1398,7 @@ static const struct mcps802154_ops dw3000_mcps_ops = {
 	.compute_frame_duration_dtu = compute_frame_duration_dtu,
 	.set_channel = set_channel,
 	.set_hrp_uwb_params = set_hrp_uwb_params,
+	.check_hrp_uwb_params = check_hrp_uwb_params,
 	.set_sts_params = set_sts_params,
 	.set_hw_addr_filt = set_hw_addr_filt,
 	.set_txpower = set_txpower,
@@ -1202,6 +1409,8 @@ static const struct mcps802154_ops dw3000_mcps_ops = {
 	.get_calibration = get_calibration,
 	.list_calibration = list_calibration,
 	.vendor_cmd = vendor_cmd,
+	.get_antenna_caps = get_antenna_caps,
+	.get_power_stats = get_power_stats,
 	MCPS802154_TESTMODE_CMD(dw3000_tm_cmd)
 };
 
@@ -1229,7 +1438,18 @@ struct dw3000 *dw3000_mcps_alloc(struct device *dev)
 	llhw->hw->flags =
 		(IEEE802154_HW_TX_OMIT_CKSUM | IEEE802154_HW_AFILT |
 		 IEEE802154_HW_PROMISCUOUS | IEEE802154_HW_RX_OMIT_CKSUM);
-	llhw->flags = llhw->hw->flags;
+	llhw->flags =
+		(MCPS802154_LLHW_BPRF | MCPS802154_LLHW_DATA_RATE_850K |
+		 MCPS802154_LLHW_DATA_RATE_6M81 |
+		 MCPS802154_LLHW_PHR_DATA_RATE_850K |
+		 MCPS802154_LLHW_PHR_DATA_RATE_6M81 | MCPS802154_LLHW_PRF_16 |
+		 MCPS802154_LLHW_PRF_64 | MCPS802154_LLHW_PSR_32 |
+		 MCPS802154_LLHW_PSR_64 | MCPS802154_LLHW_PSR_128 |
+		 MCPS802154_LLHW_PSR_256 | MCPS802154_LLHW_PSR_1024 |
+		 MCPS802154_LLHW_PSR_4096 | MCPS802154_LLHW_SFD_4A |
+		 MCPS802154_LLHW_SFD_4Z_8 | MCPS802154_LLHW_STS_SEGMENT_1 |
+		 MCPS802154_LLHW_AOA_AZIMUTH | MCPS802154_LLHW_AOA_ELEVATION |
+		 MCPS802154_LLHW_AOA_FOM);
 
 	llhw->hw->phy->supported.channels[4] = DW3000_SUPPORTED_CHANNELS;
 
@@ -1254,6 +1474,8 @@ struct dw3000 *dw3000_mcps_alloc(struct device *dev)
 	llhw->hw->phy->current_channel = dw->config.chan;
 	llhw->hw->phy->current_page = 4;
 	llhw->current_preamble_code = dw->config.txCode;
+	/* AoA/PDoA filtering. */
+	llhw->rx_ctx_size = sizeof(struct dw3000_rx_ctx);
 
 	return dw;
 }
@@ -1266,8 +1488,9 @@ void dw3000_mcps_free(struct dw3000 *dw)
 {
 	dev_dbg(dw->dev, "%s called\n", __func__);
 	if (dw->llhw) {
-		mcps802154_free_llhw(dw->llhw);
+		struct mcps802154_llhw *llhw = dw->llhw;
 		dw->llhw = NULL;
+		mcps802154_free_llhw(llhw);
 	}
 }
 
diff --git a/kernel/drivers/net/ieee802154/dw3000_nfcc_coex.h b/kernel/drivers/net/ieee802154/dw3000_nfcc_coex.h
index 4e04239..0bffafc 100644
--- a/kernel/drivers/net/ieee802154/dw3000_nfcc_coex.h
+++ b/kernel/drivers/net/ieee802154/dw3000_nfcc_coex.h
@@ -27,7 +27,6 @@
 #include <net/vendor_cmd.h>
 
 /* Main defines */
-#define DW3000_NFCC_COEX_VER_ID 2
 #define DW3000_NFCC_COEX_SIGNATURE_STR "QORVO"
 #define DW3000_NFCC_COEX_SIGNATURE_LEN 5
 #define DW3000_NFCC_COEX_MAX_NB_TLV 12
@@ -51,6 +50,19 @@
 #define DW3000_NFCC_COEX_DTU_PER_UUS_POWER 4 /* To use with left shift. */
 
 /**
+ * enum dw3000_nfcc_coex_send - Type of message to send.
+ *
+ * @DW3000_NFCC_COEX_SEND_CLK_SYNC: Clock sync message.
+ * @DW3000_NFCC_COEX_SEND_CLK_OFFSET: Clock offset message.
+ * @DW3000_NFCC_COEX_SEND_STOP: Stop message.
+ */
+enum dw3000_nfcc_coex_send {
+	DW3000_NFCC_COEX_SEND_CLK_SYNC,
+	DW3000_NFCC_COEX_SEND_CLK_OFFSET,
+	DW3000_NFCC_COEX_SEND_STOP,
+};
+
+/**
  * struct dw3000_nfcc_coex_msg - Message read/write from/to scratch memory.
  */
 struct dw3000_nfcc_coex_msg {
@@ -122,7 +134,7 @@ struct dw3000_nfcc_coex {
 	/**
 	 * @access_info: Access information to provide to upper layer.
 	 */
-	struct dw3000_vendor_cmd_nfcc_coex_get_access_info access_info;
+	struct llhw_vendor_cmd_nfcc_coex_get_access_info access_info;
 	/**
 	 * @session_time0_dtu: Timestamp used as reference between NFCC and AP.
 	 */
@@ -156,9 +168,9 @@ struct dw3000_nfcc_coex {
 	 */
 	bool configured;
 	/**
-	 * @sync_time_needed: True when clock_sync frame must be send.
+	 * @send: Type of message to send.
 	 */
-	bool sync_time_needed;
+	enum dw3000_nfcc_coex_send send;
 	/**
 	 * @first_rx_message: False after the first valid msg received.
 	 */
@@ -167,6 +179,10 @@ struct dw3000_nfcc_coex {
 	 * @watchdog_timer: Watchdog timer to detect spi not bring back.
 	 */
 	struct timer_list watchdog_timer;
+	/**
+	 * @version: Protocol version to use.
+	 */
+	u8 version;
 };
 
 #endif /* __DW3000_NFCC_COEX_H */
diff --git a/kernel/drivers/net/ieee802154/dw3000_nfcc_coex_core.c b/kernel/drivers/net/ieee802154/dw3000_nfcc_coex_core.c
index 702c8a3..9d8aef9 100644
--- a/kernel/drivers/net/ieee802154/dw3000_nfcc_coex_core.c
+++ b/kernel/drivers/net/ieee802154/dw3000_nfcc_coex_core.c
@@ -30,7 +30,10 @@
 
 #include <linux/module.h>
 
-unsigned dw3000_nfcc_coex_margin_dtu = DW3000_ANTICIP_DTU;
+/* dw3000_nfcc_coex_margin_dtu:
+ * - Can't be bigger than ANTICIP_DTU (trouble with CLOCK_SYNC).
+ * - Lower than 4ms is really dangerous. */
+unsigned dw3000_nfcc_coex_margin_dtu = US_TO_DTU(16000);
 module_param_named(nfcc_coex_margin_dtu, dw3000_nfcc_coex_margin_dtu, uint,
 		   0444);
 MODULE_PARM_DESC(
@@ -117,7 +120,7 @@ static int dw3000_nfcc_coex_disable_SPIxMAVAIL_interrupts(struct dw3000 *dw)
 static void dw3000_nfcc_coex_update_access_info(
 	struct dw3000 *dw, const struct dw3000_nfcc_coex_buffer *buffer)
 {
-	struct dw3000_vendor_cmd_nfcc_coex_get_access_info *access_info =
+	struct llhw_vendor_cmd_nfcc_coex_get_access_info *access_info =
 		&dw->nfcc_coex.access_info;
 	struct dw3000_nfcc_coex_rx_msg_info rx_msg_info = {};
 	int r;
@@ -132,9 +135,12 @@ static void dw3000_nfcc_coex_update_access_info(
 	access_info->stop = !rx_msg_info.next_slot_found;
 	access_info->watchdog_timeout = false;
 	if (rx_msg_info.next_slot_found) {
+		/* Request the handle earlier to the mac layer. */
 		access_info->next_timestamp_dtu =
-			rx_msg_info.next_timestamp_dtu;
-		access_info->next_duration_dtu = rx_msg_info.next_duration_dtu;
+			rx_msg_info.next_timestamp_dtu -
+			dw3000_nfcc_coex_margin_dtu;
+		access_info->next_duration_dtu = rx_msg_info.next_duration_dtu +
+						 dw3000_nfcc_coex_margin_dtu;
 	}
 	return;
 
@@ -168,11 +174,13 @@ int dw3000_nfcc_coex_configure(struct dw3000 *dw)
 			return r;
 		}
 	}
-	r = dw3000_rx_disable(dw);
+	r = dw3000_nfcc_coex_prepare_config(dw);
 	if (r) {
-		trace_dw3000_nfcc_coex_warn(dw, "rx disable failed");
+		trace_dw3000_nfcc_coex_warn(dw,
+					    "prepare the configuration fails");
 		return r;
 	}
+
 	r = dw3000_nfcc_coex_enable_SPIxMAVAIL_interrupts(dw);
 	if (r) {
 		trace_dw3000_nfcc_coex_err(
@@ -334,12 +342,10 @@ void dw3000_nfcc_coex_init(struct dw3000 *dw)
  * dw3000_nfcc_coex_enable() - Enable NFCC coexistence.
  * @dw: Driver context.
  * @channel: Channel number (5 or 9).
- * @sync_time_needed: True when it's the first access.
  *
  * Return: 0 on success, else an error.
  */
-int dw3000_nfcc_coex_enable(struct dw3000 *dw, u8 channel,
-			    bool sync_time_needed)
+int dw3000_nfcc_coex_enable(struct dw3000 *dw, u8 channel)
 {
 	struct dw3000_nfcc_coex *nfcc_coex = &dw->nfcc_coex;
 
@@ -349,7 +355,6 @@ int dw3000_nfcc_coex_enable(struct dw3000 *dw, u8 channel,
 
 	/* Save current channel. */
 	nfcc_coex->original_channel = dw->config.chan;
-	nfcc_coex->sync_time_needed = sync_time_needed;
 	nfcc_coex->configured = false;
 	nfcc_coex->enabled = true;
 	/* Set the new channel. */
@@ -385,7 +390,9 @@ int dw3000_nfcc_coex_disable(struct dw3000 *dw)
 			if (r)
 				return r;
 		}
-		dw->nfcc_coex.configured = false;
+		r = dw3000_nfcc_coex_restore_config(dw);
+		if (r)
+			return r;
 	}
 	return 0;
 }
diff --git a/kernel/drivers/net/ieee802154/dw3000_nfcc_coex_core.h b/kernel/drivers/net/ieee802154/dw3000_nfcc_coex_core.h
index 15fb442..26bfa64 100644
--- a/kernel/drivers/net/ieee802154/dw3000_nfcc_coex_core.h
+++ b/kernel/drivers/net/ieee802154/dw3000_nfcc_coex_core.h
@@ -34,8 +34,7 @@ int dw3000_nfcc_coex_cancel_watchdog(struct dw3000 *dw);
 int dw3000_nfcc_coex_spi1_avail(struct dw3000 *dw);
 int dw3000_nfcc_coex_idle_timeout(struct dw3000 *dw);
 void dw3000_nfcc_coex_init(struct dw3000 *dw);
-int dw3000_nfcc_coex_enable(struct dw3000 *dw, u8 channel,
-			    bool sync_time_needed);
+int dw3000_nfcc_coex_enable(struct dw3000 *dw, u8 channel);
 int dw3000_nfcc_coex_disable(struct dw3000 *dw);
 int dw3000_nfcc_coex_configure(struct dw3000 *dw);
 
diff --git a/kernel/drivers/net/ieee802154/dw3000_nfcc_coex_mcps.c b/kernel/drivers/net/ieee802154/dw3000_nfcc_coex_mcps.c
index 6e759ed..2214859 100644
--- a/kernel/drivers/net/ieee802154/dw3000_nfcc_coex_mcps.c
+++ b/kernel/drivers/net/ieee802154/dw3000_nfcc_coex_mcps.c
@@ -30,10 +30,48 @@
 
 #define DW3000_NFCC_COEX_WATCHDOG_DEFAULT_DURATION_MS 24000
 
+static int dw3000_nfcc_coex_wakeup_and_send(struct dw3000 *dw,
+					    s32 idle_duration_dtu,
+					    u32 send_timestamp_dtu)
+{
+	struct dw3000_nfcc_coex *nfcc_coex = &dw->nfcc_coex;
+	int r;
+
+	trace_dw3000_nfcc_coex_wakeup_and_send(
+		dw, nfcc_coex->send, idle_duration_dtu, send_timestamp_dtu);
+
+	if (idle_duration_dtu > dw->llhw->anticip_dtu) {
+		r = dw3000_idle(dw, true, send_timestamp_dtu,
+				dw3000_nfcc_coex_idle_timeout,
+				DW3000_OP_STATE_MAX);
+		goto wakeup_and_send_end;
+	} else if (dw->current_operational_state ==
+		   DW3000_OP_STATE_DEEP_SLEEP) {
+		r = dw3000_deepsleep_wakeup_now(dw,
+						dw3000_nfcc_coex_idle_timeout,
+						send_timestamp_dtu,
+						DW3000_OP_STATE_MAX);
+		goto wakeup_and_send_end;
+	}
+
+	r = dw3000_nfcc_coex_configure(dw);
+	if (r)
+		goto wakeup_and_send_end;
+	r = dw3000_nfcc_coex_message_send(dw);
+	if (r)
+		goto wakeup_and_send_end;
+	return 0;
+
+wakeup_and_send_end:
+	if (r)
+		dw3000_nfcc_coex_disable(dw);
+	return r;
+}
+
 /**
  * dw3000_nfcc_coex_handle_access() - handle access to provide to NFCC.
  * @dw: Driver context.
- * @data: Adress of handle access information.
+ * @data: Address of handle access information.
  * @data_len: Number of byte of the data object.
  *
  * Return: 0 on success, else an error.
@@ -41,10 +79,10 @@
 static int dw3000_nfcc_coex_handle_access(struct dw3000 *dw, void *data,
 					  int data_len)
 {
-	const struct dw3000_vendor_cmd_nfcc_coex_handle_access *info = data;
+	const struct llhw_vendor_cmd_nfcc_coex_handle_access *info = data;
 	struct dw3000_nfcc_coex *nfcc_coex = &dw->nfcc_coex;
 	const u32 dtu_per_ms = dw->llhw->dtu_freq_hz / 1000;
-	u32 now_dtu, message_send_timestamp_dtu;
+	u32 now_dtu;
 	s32 idle_duration_dtu;
 	int r;
 
@@ -56,19 +94,32 @@ static int dw3000_nfcc_coex_handle_access(struct dw3000 *dw, void *data,
 		return -EBUSY;
 	}
 
-	r = dw3000_nfcc_coex_enable(dw, info->chan, info->start);
+	r = dw3000_nfcc_coex_enable(dw, info->chan);
 	if (r)
 		return r;
 
 	now_dtu = dw3000_get_dtu_time(dw);
-	message_send_timestamp_dtu =
-		info->timestamp_dtu - dw3000_nfcc_coex_margin_dtu;
-	idle_duration_dtu = message_send_timestamp_dtu - now_dtu;
+	idle_duration_dtu = info->timestamp_dtu - now_dtu;
 
 	trace_dw3000_nfcc_coex_handle_access(dw, info, idle_duration_dtu);
-	/* Save start session date, to retrieve MSB bits lost for next date. */
-	nfcc_coex->access_start_dtu = info->timestamp_dtu;
-	/* Build the when spi must be released. */
+	nfcc_coex->send = info->start ? DW3000_NFCC_COEX_SEND_CLK_SYNC :
+					DW3000_NFCC_COEX_SEND_CLK_OFFSET;
+	if (info->start) {
+		nfcc_coex->version = info->version;
+		/*
+		 * Save first start session date, to retrieve MSB bits lost
+		 * for next received timestamp through session_time0_dtu.
+		 * It's saved because between session_time0_dtu and
+		 * access_start_dtu, a deep sleep can occur, and so
+		 * `dtu_to_sys_time` must be call after the wake up.
+		 * Between access_start_dtu and now, the duration can be less
+		 * than 2 ms (fira slot) in multi-region feature.
+		 * So the margin is like a second anticip dtu to add to provide
+		 * time for NFC handling.
+		 */
+		nfcc_coex->access_start_dtu =
+			info->timestamp_dtu + dw3000_nfcc_coex_margin_dtu;
+	}
 	nfcc_coex->watchdog_timer.expires =
 		jiffies +
 		msecs_to_jiffies((info->timestamp_dtu - now_dtu) / dtu_per_ms +
@@ -76,38 +127,14 @@ static int dw3000_nfcc_coex_handle_access(struct dw3000 *dw, void *data,
 	add_timer(&nfcc_coex->watchdog_timer);
 
 	/* Send message and so release the SPI close to the nfc_coex_margin. */
-	message_send_timestamp_dtu =
-		info->timestamp_dtu - dw3000_nfcc_coex_margin_dtu;
-	if (idle_duration_dtu > 0) {
-		r = dw3000_idle(dw, true, message_send_timestamp_dtu,
-				dw3000_nfcc_coex_idle_timeout,
-				DW3000_OP_STATE_MAX);
-		goto handle_access_end;
-	} else if (dw->current_operational_state ==
-		   DW3000_OP_STATE_DEEP_SLEEP) {
-		r = dw3000_deepsleep_wakeup_now(
-			dw, dw3000_nfcc_coex_idle_timeout, DW3000_OP_STATE_MAX);
-		goto handle_access_end;
-	}
-
-	r = dw3000_nfcc_coex_configure(dw);
-	if (r)
-		goto handle_access_end;
-	r = dw3000_nfcc_coex_message_send(dw);
-	if (r)
-		goto handle_access_end;
-	return 0;
-
-handle_access_end:
-	if (r)
-		dw3000_nfcc_coex_disable(dw);
-	return r;
+	return dw3000_nfcc_coex_wakeup_and_send(dw, idle_duration_dtu,
+						info->timestamp_dtu);
 }
 
 /**
  * dw3000_nfcc_coex_get_access_information() - Forward access info cached.
  * @dw: Driver context.
- * @data: Adress where to write access information.
+ * @data: Address where to write access information.
  * @data_len: Number of byte of the data object.
  *
  * Return: 0 on success, else an error.
@@ -115,7 +142,7 @@ handle_access_end:
 static int dw3000_nfcc_coex_get_access_information(struct dw3000 *dw,
 						   void *data, int data_len)
 {
-	const struct dw3000_vendor_cmd_nfcc_coex_get_access_info *access_info =
+	const struct llhw_vendor_cmd_nfcc_coex_get_access_info *access_info =
 		&dw->nfcc_coex.access_info;
 
 	if (!data || data_len != sizeof(*access_info))
@@ -128,22 +155,60 @@ static int dw3000_nfcc_coex_get_access_information(struct dw3000 *dw,
 /**
  * dw3000_nfcc_coex_stop() - Stop NFCC.
  * @dw: Driver context.
+ * @data: Address of stop information.
+ * @data_len: Number of byte of the data object.
  *
  * Return: 0 on success, else an error.
  */
-static int dw3000_nfcc_coex_stop(struct dw3000 *dw)
+static int dw3000_nfcc_coex_stop(struct dw3000 *dw, void *data, int data_len)
 {
+	const struct llhw_vendor_cmd_nfcc_coex_stop *info = data;
+	struct dw3000_nfcc_coex *nfcc_coex = &dw->nfcc_coex;
+	const u32 dtu_per_ms = dw->llhw->dtu_freq_hz / 1000;
+	u32 now_dtu, send_timestamp_dtu;
+	s32 idle_duration_dtu;
 	int r;
 
-	/* Cancel the idle timeout, and ignore the deepsleep state. */
-	r = dw3000_idle_cancel_timer(dw);
-	if (r)
-		return r;
-	/* Cancel the watchdog which have a bigger timeout. */
-	r = dw3000_nfcc_coex_cancel_watchdog(dw);
+	if (data_len && data_len != sizeof(*info))
+		return -EINVAL;
+
+	if (timer_pending(&nfcc_coex->watchdog_timer)) {
+		trace_dw3000_nfcc_coex_err(dw, "watchdog timer is pending");
+		return -EBUSY;
+	}
+
+	r = dw3000_nfcc_coex_enable(dw, dw->config.chan);
 	if (r)
 		return r;
-	return dw3000_nfcc_coex_disable(dw);
+
+	nfcc_coex->send = DW3000_NFCC_COEX_SEND_STOP;
+
+	if (info) {
+		now_dtu = dw3000_get_dtu_time(dw);
+		send_timestamp_dtu = info->timestamp_dtu;
+		idle_duration_dtu = send_timestamp_dtu - now_dtu;
+		nfcc_coex->watchdog_timer.expires =
+			jiffies +
+			msecs_to_jiffies(
+				(info->timestamp_dtu - now_dtu) / dtu_per_ms +
+				DW3000_NFCC_COEX_WATCHDOG_DEFAULT_DURATION_MS);
+		nfcc_coex->version = info->version;
+	} else {
+		send_timestamp_dtu = 0;
+		idle_duration_dtu = 0;
+		nfcc_coex->watchdog_timer.expires =
+			jiffies +
+			msecs_to_jiffies(
+				DW3000_NFCC_COEX_WATCHDOG_DEFAULT_DURATION_MS);
+		/* Cancel wakeup timer launch by idle() */
+		dw3000_idle_cancel_timer(dw);
+	}
+
+	add_timer(&nfcc_coex->watchdog_timer);
+
+	/* Send message and so release the SPI close to the nfc_coex_margin. */
+	return dw3000_nfcc_coex_wakeup_and_send(dw, idle_duration_dtu,
+						send_timestamp_dtu);
 }
 
 /**
@@ -165,13 +230,13 @@ int dw3000_nfcc_coex_vendor_cmd(struct dw3000 *dw, u32 vendor_id, u32 subcmd,
 		return -EOPNOTSUPP;
 
 	switch (subcmd) {
-	case DW3000_VENDOR_CMD_NFCC_COEX_HANDLE_ACCESS:
+	case LLHW_VENDOR_CMD_NFCC_COEX_HANDLE_ACCESS:
 		return dw3000_nfcc_coex_handle_access(dw, data, data_len);
-	case DW3000_VENDOR_CMD_NFCC_COEX_GET_ACCESS_INFORMATION:
+	case LLHW_VENDOR_CMD_NFCC_COEX_GET_ACCESS_INFORMATION:
 		return dw3000_nfcc_coex_get_access_information(dw, data,
 							       data_len);
-	case DW3000_VENDOR_CMD_NFCC_COEX_STOP:
-		return dw3000_nfcc_coex_stop(dw);
+	case LLHW_VENDOR_CMD_NFCC_COEX_STOP:
+		return dw3000_nfcc_coex_stop(dw, data, data_len);
 	default:
 		return -EINVAL;
 	}
diff --git a/kernel/drivers/net/ieee802154/dw3000_nfcc_coex_msg.c b/kernel/drivers/net/ieee802154/dw3000_nfcc_coex_msg.c
index c0f2172..18b74be 100644
--- a/kernel/drivers/net/ieee802154/dw3000_nfcc_coex_msg.c
+++ b/kernel/drivers/net/ieee802154/dw3000_nfcc_coex_msg.c
@@ -33,6 +33,7 @@
 #define TLV_U32_LEN (4 + 1) /* u32 + ack/nack. */
 #define TLV_SLOTS_LEN(nbslots) \
 	(1 + (8 * (nbslots)) + 1) /* nslots + slots + ack/nack. */
+#define TLV_SLOTS_LIST_SIZE_MAX (1 + (8 * (TLV_MAX_NB_SLOTS)))
 #define MSG_NEXT_TLV(buffer, offset) \
 	(struct dw3000_nfcc_coex_tlv *)((buffer)->msg.tlvs + (offset))
 
@@ -64,11 +65,11 @@ void dw3000_nfcc_coex_header_put(struct dw3000 *dw,
 {
 	struct dw3000_nfcc_coex_msg *msg = &buffer->msg;
 
-	trace_dw3000_nfcc_coex_header_put(dw, DW3000_NFCC_COEX_VER_ID,
+	trace_dw3000_nfcc_coex_header_put(dw, dw->nfcc_coex.version,
 					  dw->nfcc_coex.tx_seq_num);
 	memcpy(msg->signature, DW3000_NFCC_COEX_SIGNATURE_STR,
 	       DW3000_NFCC_COEX_SIGNATURE_LEN);
-	msg->ver_id = DW3000_NFCC_COEX_VER_ID;
+	msg->ver_id = dw->nfcc_coex.version;
 	msg->seqnum = dw->nfcc_coex.tx_seq_num;
 	msg->nb_tlv = 0;
 	buffer->tlvs_len = 0;
@@ -139,6 +140,24 @@ static void dw3000_nfcc_coex_clock_offset_payload_put(
 }
 
 /**
+ * dw3000_nfcc_coex_stop_session_payload_put() - Fill stop session payload.
+ * @dw: Driver context.
+ * @buffer: Buffer to set with help of handle_access.
+ * @session_id: Session id to stop.
+ */
+static void dw3000_nfcc_coex_stop_session_payload_put(
+	struct dw3000 *dw, struct dw3000_nfcc_coex_buffer *buffer,
+	u32 session_id)
+{
+	trace_dw3000_nfcc_coex_stop_session_payload_put(dw, session_id);
+
+	dw3000_nfcc_coex_header_put(dw, buffer);
+
+	dw3000_nfcc_coex_tlv_u32_put(
+		buffer, DW3000_NFCC_COEX_TLV_TYPE_STOP_SESSION, session_id);
+}
+
+/**
  * dw3000_nfcc_coex_message_send() - Write message for NFCC and release SPI1.
  * @dw: Driver context.
  *
@@ -150,17 +169,25 @@ int dw3000_nfcc_coex_message_send(struct dw3000 *dw)
 	/* Build the absolute sys time offset. */
 	u32 offset_sys_time =
 		(dw->dtu_sync << DW3000_DTU_PER_SYS_POWER) - dw->sys_time_sync;
+	u32 clock_offset_sys_time;
 
-	if (dw->nfcc_coex.sync_time_needed) {
-		dw->nfcc_coex.sync_time_needed = false;
+	switch (dw->nfcc_coex.send) {
+	case DW3000_NFCC_COEX_SEND_CLK_SYNC:
 		dw3000_nfcc_coex_clock_sync_payload_put(dw, &buffer);
-	} else {
+		break;
+	default:
+	case DW3000_NFCC_COEX_SEND_CLK_OFFSET:
 		/* Compute the clock correction to forward to NFCC. */
-		u32 clock_offset_sys_time =
+		clock_offset_sys_time =
 			offset_sys_time - dw->nfcc_coex.prev_offset_sys_time;
 		/* Build the message with the clock update to forward. */
 		dw3000_nfcc_coex_clock_offset_payload_put(
 			dw, &buffer, -clock_offset_sys_time);
+		break;
+	case DW3000_NFCC_COEX_SEND_STOP:
+		dw3000_nfcc_coex_stop_session_payload_put(
+			dw, &buffer, DW3000_NFCC_COEX_SESSION_ID_DEFAULT);
+		break;
 	}
 
 	dw->nfcc_coex.prev_offset_sys_time = offset_sys_time;
@@ -189,7 +216,7 @@ dw3000_nfcc_coex_header_check(struct dw3000 *dw,
 		return -EINVAL;
 	}
 	/* Check AP_NFCC Interface Version ID. */
-	if (msg->ver_id != DW3000_NFCC_COEX_VER_ID) {
+	if (msg->ver_id != dw->nfcc_coex.version) {
 		return -EINVAL;
 	}
 	/* Read number of TLVs. */
@@ -246,6 +273,9 @@ dw3000_nfcc_coex_tlvs_check(struct dw3000 *dw,
 			/* Reject a new TLV with same type. Behavior not defined. */
 			if (slot_list)
 				return -EINVAL;
+			/* Check if the tlv size isn't exceeding the list max size */
+			if (tlv->len > TLV_SLOTS_LIST_SIZE_MAX)
+				return -EINVAL;
 			slot_list = (const struct dw3000_nfcc_coex_tlv_slot_list
 					     *)&tlv->tlv;
 			/* Update rx_msg_info. */
diff --git a/kernel/drivers/net/ieee802154/dw3000_nfcc_coex_msg.h b/kernel/drivers/net/ieee802154/dw3000_nfcc_coex_msg.h
index 001de1d..4214088 100644
--- a/kernel/drivers/net/ieee802154/dw3000_nfcc_coex_msg.h
+++ b/kernel/drivers/net/ieee802154/dw3000_nfcc_coex_msg.h
@@ -28,6 +28,7 @@
 #include "dw3000.h"
 
 #define TLV_MAX_NB_SLOTS 4
+#define DW3000_NFCC_COEX_SESSION_ID_DEFAULT 0
 
 /**
  * enum dw3000_nfcc_coex_tlv_type - TLVs types.
@@ -43,6 +44,8 @@
  *	Indicate error condition.
  * @DW3000_NFCC_COEX_TLV_TYPE_SLOT_LIST_UUS:
  *	Indicate the UWB clock offset in V2 protocol.
+ * @DW3000_NFCC_COEX_TLV_TYPE_STOP_SESSION:
+ *	Indicate the stop session in V3 protocol.
  */
 enum dw3000_nfcc_coex_tlv_type {
 	DW3000_NFCC_COEX_TLV_TYPE_UNSPEC,
@@ -52,6 +55,7 @@ enum dw3000_nfcc_coex_tlv_type {
 	DW3000_NFCC_COEX_TLV_TYPE_TLV_UWBCNT_OFFS,
 	DW3000_NFCC_COEX_TLV_TYPE_ERROR,
 	DW3000_NFCC_COEX_TLV_TYPE_SLOT_LIST_UUS,
+	DW3000_NFCC_COEX_TLV_TYPE_STOP_SESSION
 };
 
 /**
diff --git a/kernel/drivers/net/ieee802154/dw3000_pctt_mcps.c b/kernel/drivers/net/ieee802154/dw3000_pctt_mcps.c
index 429fe52..400080c 100644
--- a/kernel/drivers/net/ieee802154/dw3000_pctt_mcps.c
+++ b/kernel/drivers/net/ieee802154/dw3000_pctt_mcps.c
@@ -29,7 +29,7 @@
 int dw3000_pctt_vendor_cmd(struct dw3000 *dw, u32 vendor_id, u32 subcmd,
 			   void *data, size_t data_len)
 {
-	struct dw3000_vendor_cmd_pctt_setup_hw *info = data;
+	struct llhw_vendor_cmd_pctt_setup_hw *info = data;
 	struct dw3000_config *config = &dw->config;
 	int rc;
 
@@ -62,5 +62,5 @@ int dw3000_pctt_vendor_cmd(struct dw3000 *dw, u32 vendor_id, u32 subcmd,
 			return rc;
 	}
 	dw->pctt.enabled = !!info;
-	return 0;
+	return dw3000_enable_auto_fcs(dw, !dw->pctt.enabled);
 }
diff --git a/kernel/drivers/net/ieee802154/dw3000_spi.c b/kernel/drivers/net/ieee802154/dw3000_spi.c
index 30f0561..d710868 100644
--- a/kernel/drivers/net/ieee802154/dw3000_spi.c
+++ b/kernel/drivers/net/ieee802154/dw3000_spi.c
@@ -116,7 +116,7 @@ static int dw3000_spi_probe(struct spi_device *spi)
 	hrtimer_init(&dw->idle_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
 	dw->idle_timer.function = dw3000_idle_timeout;
 
-	dev_info(dw->dev, "Loading driver...2022_03_04");
+	dev_info(dw->dev, "Loading driver...");
 	dw3000_sysfs_init(dw);
 
 	/* Setup SPI parameters */
@@ -141,16 +141,7 @@ static int dw3000_spi_probe(struct spi_device *spi)
 	rc = spi_setup(spi);
 	if (rc != 0)
 		goto err_spi_setup;
-#if (KERNEL_VERSION(5, 5, 0) <= LINUX_VERSION_CODE)
-	/* Setup SPI controller CS timings */
-	{
-		struct spi_delay dly = { .unit = SPI_DELAY_UNIT_NSECS,
-					 .value = 0 };
-		rc = spi_set_cs_timing(spi, &dly, &dly, &dly);
-		if (rc != 0)
-			goto err_spi_setup;
-	}
-#endif
+
 	/* Request and setup regulators if availables*/
 	dw3000_setup_regulators(dw);
 
@@ -185,13 +176,6 @@ static int dw3000_spi_probe(struct spi_device *spi)
 	if (rc != 0)
 		goto err_setup_irq;
 
-	/* Register MCPS 802.15.4 device */
-	rc = dw3000_mcps_register(dw);
-	if (rc != 0) {
-		dev_err(&spi->dev, "could not register: %d\n", rc);
-		goto err_register_hw;
-	}
-
 	/*
 	 * Initialize PM QoS. Using the default latency won't change anything
 	 * to the QoS list
@@ -208,17 +192,25 @@ static int dw3000_spi_probe(struct spi_device *spi)
 	if (rc != 0)
 		goto err_debugfs;
 
+	/* Register MCPS 802.15.4 device */
+	rc = dw3000_mcps_register(dw);
+	if (rc != 0) {
+		dev_err(&spi->dev, "could not register: %d\n", rc);
+		goto err_register_hw;
+	}
+
 	/* All is ok */
 	return 0;
 
+err_register_hw:
+	dw3000_debugfs_remove(dw);
 err_debugfs:
 err_state_start:
 	dw3000_pm_qos_remove_request(dw);
-	dw3000_mcps_unregister(dw);
-err_register_hw:
 err_setup_irq:
 	dw3000_state_stop(dw);
 err_state_init:
+	dw3000_transfers_free(dw);
 err_transfers_init:
 err_setup_gpios:
 err_spi_setup:
@@ -228,6 +220,7 @@ err_qos_latency:
 err_thread_cpu:
 err_wifi_coex:
 	dw3000_mcps_free(dw);
+	spi_set_drvdata(spi, NULL);
 err_alloc_hw:
 	return rc;
 }
@@ -246,26 +239,24 @@ static int dw3000_spi_remove(struct spi_device *spi)
 {
 	struct dw3000 *dw = spi_get_drvdata(spi);
 
-	dw3000_cir_data_alloc_count(dw, 0);
-
-	dw3000_sysfs_remove(dw);
-
-	dw3000_debugfs_remove(dw);
+	if (dw == NULL)
+		/* Error during probe, all already freed */
+		return 0;
 
 	dev_dbg(dw->dev, "unloading...");
 
+	/* Remove sysfs files */
+	dw3000_debugfs_remove(dw);
+	dw3000_sysfs_remove(dw);
 	/* Unregister subsystems */
 	dw3000_mcps_unregister(dw);
-
-	dw3000_pm_qos_remove_request(dw);
-
 	/* Stop state machine */
 	dw3000_state_stop(dw);
-
+	dw3000_pm_qos_remove_request(dw);
 	/* Free pre-computed SPI messages */
 	dw3000_transfers_free(dw);
-
 	/* Release the mcps 802.15.4 device */
+	dw3000_cir_data_alloc_count(dw, 0);
 	dw3000_mcps_free(dw);
 
 	return 0;
diff --git a/kernel/drivers/net/ieee802154/dw3000_stm.c b/kernel/drivers/net/ieee802154/dw3000_stm.c
index 40da614..5b85643 100644
--- a/kernel/drivers/net/ieee802154/dw3000_stm.c
+++ b/kernel/drivers/net/ieee802154/dw3000_stm.c
@@ -21,21 +21,50 @@
  * Qorvo. Please contact Qorvo to inquire about licensing terms.
  */
 #include <linux/version.h>
+#include <linux/module.h>
 #include <linux/workqueue.h>
 #include <linux/sched.h>
 #include <linux/mutex.h>
+#include <linux/of.h>
 
 #include "dw3000.h"
 #include "dw3000_core.h"
 
-#define DW3000_MIN_CLAMP_VALUE	170
+#define DW3000_MIN_CLAMP_VALUE 460
 
 /* First version with sched_setattr_nocheck: v4.16-rc1~164^2~5 */
 #if (KERNEL_VERSION(4, 11, 0) <= LINUX_VERSION_CODE)
 #include <uapi/linux/sched/types.h>
 #endif
 
-static inline int dw3000_set_sched_attr(struct task_struct *p)
+static int dw3000_min_clamp_value = 0;
+
+module_param_named(min_clamp_value, dw3000_min_clamp_value, int, 0644);
+MODULE_PARM_DESC(min_clamp_value, "Sets the minimum cpu frequency the dw3000 thread must run at");
+
+
+static void dw3000_get_clamp_from_dt(struct dw3000 *dw) {
+	int dt_clamp = DW3000_MIN_CLAMP_VALUE;
+	int ret;
+
+	if (!dw->dev->of_node)
+		return;
+	/* debug value is priority  */
+	if (dw3000_min_clamp_value) {
+		dw->min_clamp_value = dw3000_min_clamp_value;
+		dev_info(dw->dev, "using debug min clamp=%d\n", dw->min_clamp_value);
+		return;
+	}
+
+	ret = of_property_read_u32(dw->dev->of_node, "min_clamp", &dt_clamp);
+	if (ret) {
+		dev_err(dw->dev, "error reading dt_clamp ret=%d\n", ret);
+	}
+	dw->min_clamp_value = dt_clamp;
+	dev_info(dw->dev, "dt_clamp=%d\n", dw->min_clamp_value);
+}
+
+static inline int dw3000_set_sched_attr(struct dw3000 *dw, struct task_struct *p)
 {
 #if (KERNEL_VERSION(5, 9, 0) > LINUX_VERSION_CODE)
 	struct sched_param sched_par = { .sched_priority = MAX_RT_PRIO - 2 };
@@ -45,7 +74,7 @@ static inline int dw3000_set_sched_attr(struct task_struct *p)
 	struct sched_attr attr = { .sched_policy = SCHED_FIFO,
 				   .sched_priority = MAX_RT_PRIO - 2,
 				   .sched_flags = SCHED_FLAG_UTIL_CLAMP_MIN,
-				   .sched_util_min = DW3000_MIN_CLAMP_VALUE };
+				   .sched_util_min = dw->min_clamp_value };
 	return sched_setattr_nocheck(p, &attr);
 #endif
 }
@@ -75,7 +104,7 @@ int dw3000_enqueue_generic(struct dw3000 *dw, struct dw3000_stm_command *cmd)
 		return cmd->cmd(dw, cmd->in, cmd->out);
 	}
 
-	/* Mutex is used in dw3000_enqueue_generic() 
+	/* Mutex is used in dw3000_enqueue_generic()
 	* This protection will work with the spinlock in order to allow
 	* the CPU to sleep and avoid ressources wasting during spinning
 	*/
@@ -302,14 +331,18 @@ int dw3000_state_init(struct dw3000 *dw, int cpu)
 	stm->mthread = kthread_create(dw3000_event_thread, dw, "dw3000-%s",
 				      dev_name(dw->dev));
 	if (IS_ERR(stm->mthread)) {
-		return PTR_ERR(stm->mthread);
+		int err = PTR_ERR(stm->mthread);
+		stm->mthread = NULL;
+		return err;
 	}
+	get_task_struct(stm->mthread);
 	if (cpu >= 0)
 		kthread_bind(stm->mthread, (unsigned)cpu);
 	dw->dw3000_pid = stm->mthread->pid;
 
 	/* Increase thread priority */
-	rc = dw3000_set_sched_attr(stm->mthread);
+	dw3000_get_clamp_from_dt(dw);
+	rc = dw3000_set_sched_attr(dw, stm->mthread);
 	if (rc)
 		dev_err(dw->dev, "dw3000_set_sched_attr failed: %d\n", rc);
 	return 0;
@@ -360,8 +393,13 @@ int dw3000_state_stop(struct dw3000 *dw)
 {
 	struct dw3000_state *stm = &dw->stm;
 
+	if (stm->mthread == NULL)
+		return 0; /* already stopped or not created yet */
+
 	/* Stop state machine thread */
 	kthread_stop(stm->mthread);
+	put_task_struct(stm->mthread);
+	stm->mthread = NULL;
 
 	dev_dbg(dw->dev, "state machine stopped\n");
 	return 0;
diff --git a/kernel/drivers/net/ieee802154/dw3000_testmode.c b/kernel/drivers/net/ieee802154/dw3000_testmode.c
index e5e2e3d..1097d2d 100644
--- a/kernel/drivers/net/ieee802154/dw3000_testmode.c
+++ b/kernel/drivers/net/ieee802154/dw3000_testmode.c
@@ -105,11 +105,17 @@ static int do_tm_cmd_get_rx_diag(struct dw3000 *dw, const void *in, void *out)
 {
 	const struct do_tm_cmd_params *params = in;
 	struct dw3000_stats *stats = &dw->stats;
-	size_t rssi_len =
-		stats->count[DW3000_STATS_RX_GOOD] * sizeof(struct dw3000_rssi);
 	struct sk_buff *nl_skb;
+	size_t rssi_len;
+	int count = stats->count[DW3000_STATS_RX_GOOD];
 	int rc;
 
+	/* TODO: we don't send RSSI data for error frames. We should change this. */
+	rssi_len = count < (DW3000_RSSI_REPORTS_MAX << 1) ?
+			   count :
+			   DW3000_RSSI_REPORTS_MAX << 1;
+	rssi_len *= sizeof(struct dw3000_rssi);
+
 	/**
 	 * Allocate netlink message. The approximated size includes
 	 * the testmode's command id and data.
@@ -320,8 +326,8 @@ static int do_tm_cmd_stop_cont_tx(struct dw3000 *dw, const void *in, void *out)
 	return dw3000_testmode_continuous_tx_stop(dw);
 }
 
-static int do_tm_cmd_set_hrp_params(struct dw3000 *dw, const void *in,
-				    void *out)
+static int do_tm_cmd_set_hrp_uwb_params(struct dw3000 *dw, const void *in,
+					void *out)
 {
 	const struct do_tm_cmd_params *params = in;
 	u32 psr;
@@ -398,7 +404,7 @@ int dw3000_tm_cmd(struct mcps802154_llhw *llhw, void *data, int len)
 		[DW3000_TM_CMD_START_CONTINUOUS_TX] = do_tm_cmd_start_cont_tx,
 		[DW3000_TM_CMD_STOP_CONTINUOUS_TX] = do_tm_cmd_stop_cont_tx,
 		[DW3000_TM_CMD_DEEP_SLEEP] = do_tm_cmd_deep_sleep,
-		[DW3000_TM_CMD_SET_HRP_PARAMS] = do_tm_cmd_set_hrp_params,
+		[DW3000_TM_CMD_SET_HRP_PARAMS] = do_tm_cmd_set_hrp_uwb_params,
 		[DW3000_TM_CMD_SET_CHANNEL] = do_tm_cmd_set_channel,
 	};
 	u32 tm_cmd;
diff --git a/kernel/drivers/net/ieee802154/dw3000_trc.h b/kernel/drivers/net/ieee802154/dw3000_trc.h
index 268e7f6..29943e8 100644
--- a/kernel/drivers/net/ieee802154/dw3000_trc.h
+++ b/kernel/drivers/net/ieee802154/dw3000_trc.h
@@ -201,29 +201,30 @@ TRACE_DEFINE_ENUM(DW3000_PWR_TX);
 #define DW_SYS_STATUS_FLAGS_PR_ARG __entry->status
 #endif
 
-#define RX_INFO_FLAGS_ENTRY __field(u8, flags)
-#define RX_INFO_FLAGS_ASSIGN entry->flags = flags
-#define RX_INFO_FLAGS_PR_FMT "flags: %s"
+#define RX_FRAME_CONFIG_FLAGS_ENTRY __field(u8, flags)
+#define RX_FRAME_CONFIG_FLAGS_ASSIGN entry->flags = flags
+#define RX_FRAME_CONFIG_FLAGS_PR_FMT "flags: %s"
 
-#define mcps802154_rx_info_name(name)            \
-	{                                        \
-		MCPS802154_RX_INFO_##name, #name \
+#define mcps802154_rx_frame_config_name(name)            \
+	{                                                \
+		MCPS802154_RX_FRAME_CONFIG_##name, #name \
 	}
 
 /* clang-format off */
-#define RX_INFO_FLAGS                                \
-	mcps802154_rx_info_name(TIMESTAMP_DTU),      \
-	mcps802154_rx_info_name(AACK),               \
-	mcps802154_rx_info_name(RANGING),            \
-	mcps802154_rx_info_name(KEEP_RANGING_CLOCK), \
-	mcps802154_rx_info_name(RANGING_PDOA),       \
-	mcps802154_rx_info_name(SP3),                \
-	mcps802154_rx_info_name(SP2),                \
-	mcps802154_rx_info_name(SP1),                \
-	mcps802154_rx_info_name(STS_MODE_MASK)
+#define RX_FRAME_CONFIG_FLAGS                                \
+	mcps802154_rx_frame_config_name(TIMESTAMP_DTU),      \
+	mcps802154_rx_frame_config_name(AACK),               \
+	mcps802154_rx_frame_config_name(RANGING),            \
+	mcps802154_rx_frame_config_name(KEEP_RANGING_CLOCK), \
+	mcps802154_rx_frame_config_name(RANGING_PDOA),       \
+	mcps802154_rx_frame_config_name(SP3),                \
+	mcps802154_rx_frame_config_name(SP2),                \
+	mcps802154_rx_frame_config_name(SP1),                \
+	mcps802154_rx_frame_config_name(STS_MODE_MASK)
 /* clang-format on */
 
-#define RX_INFO_FLAGS_PR_ARG __print_flags(__entry->flags, "|", RX_INFO_FLAGS)
+#define RX_FRAME_CONFIG_FLAGS_PR_ARG \
+	__print_flags(__entry->flags, "|", RX_FRAME_CONFIG_FLAGS)
 
 #define RX_FRAME_INFO_FLAGS_ENTRY __field(u16, flags)
 #define RX_FRAME_INFO_FLAGS_ASSIGN entry->flags = flags
@@ -252,29 +253,29 @@ TRACE_DEFINE_ENUM(DW3000_PWR_TX);
 #define RX_FRAME_INFO_FLAGS_PR_ARG \
 	__print_flags(__entry->flags, "|", RX_FRAME_INFO_FLAGS)
 
-#define TX_FRAME_INFO_FLAGS_ENTRY __field(u8, flags)
-#define TX_FRAME_INFO_FLAGS_ASSIGN entry->flags = flags
-#define TX_FRAME_INFO_FLAGS_PR_FMT "flags: %s"
+#define TX_FRAME_CONFIG_FLAGS_ENTRY __field(u8, flags)
+#define TX_FRAME_CONFIG_FLAGS_ASSIGN entry->flags = flags
+#define TX_FRAME_CONFIG_FLAGS_PR_FMT "flags: %s"
 
-#define mcps802154_tx_frame_info_name(name)       \
-	{                                         \
-		MCPS802154_TX_FRAME_##name, #name \
+#define mcps802154_tx_frame_config_name(name)            \
+	{                                                \
+		MCPS802154_TX_FRAME_CONFIG_##name, #name \
 	}
 
 /* clang-format off */
-#define TX_FRAME_INFO_FLAGS						\
-	mcps802154_tx_frame_info_name(TIMESTAMP_DTU),			\
-	mcps802154_tx_frame_info_name(CCA),				\
-	mcps802154_tx_frame_info_name(RANGING),				\
-	mcps802154_tx_frame_info_name(KEEP_RANGING_CLOCK),		\
-	mcps802154_tx_frame_info_name(SP3),				\
-	mcps802154_tx_frame_info_name(SP2),				\
-	mcps802154_tx_frame_info_name(SP1),				\
-	mcps802154_tx_frame_info_name(STS_MODE_MASK)
+#define TX_FRAME_CONFIG_FLAGS						\
+	mcps802154_tx_frame_config_name(TIMESTAMP_DTU),			\
+	mcps802154_tx_frame_config_name(CCA),				\
+	mcps802154_tx_frame_config_name(RANGING),				\
+	mcps802154_tx_frame_config_name(KEEP_RANGING_CLOCK),		\
+	mcps802154_tx_frame_config_name(SP3),				\
+	mcps802154_tx_frame_config_name(SP2),				\
+	mcps802154_tx_frame_config_name(SP1),				\
+	mcps802154_tx_frame_config_name(STS_MODE_MASK)
 /* clang-format on */
 
-#define TX_FRAME_INFO_FLAGS_PR_ARG \
-	__print_flags(__entry->flags, "|", TX_FRAME_INFO_FLAGS)
+#define TX_FRAME_CONFIG_FLAGS_PR_ARG \
+	__print_flags(__entry->flags, "|", TX_FRAME_CONFIG_FLAGS)
 
 #define dw3000_nfcc_coex_tlv_type_name(name)            \
 	{                                               \
@@ -315,6 +316,26 @@ TRACE_DEFINE_ENUM(DW3000_NFCC_COEX_TLV_TYPE_SLOT_LIST_UUS);
 TRACE_DEFINE_ENUM(DW3000_DSS_STAT_SPI1_AVAIL_BIT_MASK);
 TRACE_DEFINE_ENUM(DW3000_DSS_STAT_SPI2_AVAIL_BIT_MASK);
 
+#define dw3000_nfcc_coex_send_name(name)            \
+	{                                           \
+		DW3000_NFCC_COEX_SEND_##name, #name \
+	}
+
+#define DW3000_NFCC_COEX_SEND_ENTRY __field(enum dw3000_nfcc_coex_send, send)
+#define DW3000_NFCC_COEX_SEND_ASSIGN __entry->send = send
+#define DW3000_NFCC_COEX_SEND_PR_FMT "send: %s"
+/* clang-format off */
+#define DW3000_NFCC_COEX_SEND                \
+	dw3000_nfcc_coex_send_name(CLK_SYNC),   \
+	dw3000_nfcc_coex_send_name(CLK_OFFSET), \
+	dw3000_nfcc_coex_send_name(STOP)
+/* clang-format on */
+TRACE_DEFINE_ENUM(DW3000_NFCC_COEX_SEND_CLK_SYNC);
+TRACE_DEFINE_ENUM(DW3000_NFCC_COEX_SEND_CLK_OFFSET);
+TRACE_DEFINE_ENUM(DW3000_NFCC_COEX_SEND_STOP);
+#define DW3000_NFCC_COEX_SEND_ARG \
+	__print_symbolic(__entry->send, DW3000_NFCC_COEX_SEND)
+
 /* We don't want clang-format to modify the following events definition!
    Look at net/wireless/trace.h for the required format. */
 /* clang-format off */
@@ -407,16 +428,16 @@ TRACE_EVENT(dw3000_mcps_tx_frame,
 	TP_ARGS(dw, flags, len),
 	TP_STRUCT__entry(
 		DW_ENTRY
-		TX_FRAME_INFO_FLAGS_ENTRY
+		TX_FRAME_CONFIG_FLAGS_ENTRY
 		__field(u16, len)
 	),
 	TP_fast_assign(
 		DW_ASSIGN;
-		TX_FRAME_INFO_FLAGS_ASSIGN;
+		TX_FRAME_CONFIG_FLAGS_ASSIGN;
 		__entry->len = len;
 	),
-	TP_printk(DW_PR_FMT ", " TX_FRAME_INFO_FLAGS_PR_FMT ", skb->len: %d",
-		  DW_PR_ARG, TX_FRAME_INFO_FLAGS_PR_ARG,__entry->len)
+	TP_printk(DW_PR_FMT ", " TX_FRAME_CONFIG_FLAGS_PR_FMT ", skb->len: %d",
+		  DW_PR_ARG, TX_FRAME_CONFIG_FLAGS_PR_ARG,__entry->len)
 );
 
 TRACE_EVENT(dw3000_mcps_tx_frame_too_late,
@@ -442,16 +463,16 @@ TRACE_EVENT(dw3000_mcps_rx_enable,
 	TP_ARGS(dw, flags, timeout),
 	TP_STRUCT__entry(
 		DW_ENTRY
-		RX_INFO_FLAGS_ENTRY
+		RX_FRAME_CONFIG_FLAGS_ENTRY
 		__field(int, timeout)
 	),
 	TP_fast_assign(
 		DW_ASSIGN;
-		RX_INFO_FLAGS_ASSIGN;
+		RX_FRAME_CONFIG_FLAGS_ASSIGN;
 		__entry->timeout = timeout;
 	),
-	TP_printk(DW_PR_FMT ", " RX_INFO_FLAGS_PR_FMT ", timeout: %d",
-		  DW_PR_ARG, RX_INFO_FLAGS_PR_ARG, __entry->timeout)
+	TP_printk(DW_PR_FMT ", " RX_FRAME_CONFIG_FLAGS_PR_FMT ", timeout: %d",
+		  DW_PR_ARG, RX_FRAME_CONFIG_FLAGS_PR_ARG, __entry->timeout)
 );
 
 TRACE_EVENT(dw3000_mcps_rx_enable_too_late,
@@ -521,6 +542,11 @@ DEFINE_EVENT(dw_only_evt, dw3000_wakeup_done_to_idle,
 	TP_ARGS(dw)
 );
 
+DEFINE_EVENT(dw_only_evt, dw3000_wakeup_done_to_idle_late,
+	TP_PROTO(struct dw3000 *dw),
+	TP_ARGS(dw)
+);
+
 TRACE_EVENT(dw3000_idle,
 	TP_PROTO(struct dw3000 *dw, bool timeout, u32 timeout_dtu,
 		 enum operational_state next_operational_state),
@@ -1172,6 +1198,21 @@ TRACE_EVENT(dw3000_nfcc_coex_clock_offset_payload_put,
 		  __entry->clock_offset_sys_time)
 );
 
+TRACE_EVENT(dw3000_nfcc_coex_stop_session_payload_put,
+	TP_PROTO(struct dw3000 *dw, u32 session_id),
+	TP_ARGS(dw, session_id),
+	TP_STRUCT__entry(
+		DW_ENTRY
+		__field(u32, session_id)
+	),
+	TP_fast_assign(
+		DW_ASSIGN;
+		__entry->session_id = session_id;
+	),
+	TP_printk(DW_PR_FMT ", session_id %d", DW_PR_ARG,
+		  __entry->session_id)
+);
+
 TRACE_EVENT(dw3000_nfcc_coex_rx_msg_info,
 	TP_PROTO(struct dw3000 *dw, u32 next_timestamp_dtu,
 		 int next_duration_dtu),
@@ -1238,7 +1279,7 @@ TRACE_EVENT(dw3000_nfcc_coex_tlv_check,
 );
 
 TRACE_EVENT(dw3000_nfcc_coex_handle_access,
-	TP_PROTO(struct dw3000 *dw, const struct dw3000_vendor_cmd_nfcc_coex_handle_access *info,
+	TP_PROTO(struct dw3000 *dw, const struct llhw_vendor_cmd_nfcc_coex_handle_access *info,
 		 s32 idle_duration_dtu),
 	TP_ARGS(dw, info, idle_duration_dtu),
 	TP_STRUCT__entry(
@@ -1266,6 +1307,30 @@ TRACE_EVENT(dw3000_nfcc_coex_handle_access,
 		  __entry->duration_dtu, __entry->chan)
 );
 
+TRACE_EVENT(dw3000_nfcc_coex_wakeup_and_send,
+	TP_PROTO(struct dw3000 *dw, enum dw3000_nfcc_coex_send send,
+		 s32 idle_duration_dtu, u32 send_timestamp_dtu),
+	TP_ARGS(dw, send, idle_duration_dtu, send_timestamp_dtu),
+	TP_STRUCT__entry(
+		DW_ENTRY
+		DW3000_NFCC_COEX_SEND_ENTRY
+		__field(s32, idle_duration_dtu)
+		__field(u32, send_timestamp_dtu)
+	),
+	TP_fast_assign(
+		DW_ASSIGN;
+		DW3000_NFCC_COEX_SEND_ASSIGN,
+		__entry->idle_duration_dtu = idle_duration_dtu;
+		__entry->send_timestamp_dtu = send_timestamp_dtu;
+	),
+	TP_printk(DW_PR_FMT ", " DW3000_NFCC_COEX_SEND_PR_FMT
+		  ", idle_duration_dtu: %d, send_timestamp_dtu: 0x%08x",
+		  DW_PR_ARG,
+		  DW3000_NFCC_COEX_SEND_ARG,
+		  __entry->idle_duration_dtu,
+		  __entry->send_timestamp_dtu)
+);
+
 TRACE_EVENT(dw3000_nfcc_coex_err,
 	TP_PROTO(struct dw3000 *dw, const char *err),
 	TP_ARGS(dw, err),
@@ -1436,6 +1501,30 @@ TRACE_EVENT(dw3000_read_clockoffset,
 		  __entry->cfo)
 	);
 
+TRACE_EVENT(dw3000_nfcc_coex_prepare_config,
+	TP_PROTO(struct dw3000 *dw),
+	TP_ARGS(dw),
+	TP_STRUCT__entry(
+		DW_ENTRY
+		),
+	TP_fast_assign(
+		DW_ASSIGN;
+		),
+	TP_printk(DW_PR_FMT, DW_PR_ARG)
+	);
+
+TRACE_EVENT(dw3000_nfcc_coex_restore_config,
+	TP_PROTO(struct dw3000 *dw),
+	TP_ARGS(dw),
+	TP_STRUCT__entry(
+		DW_ENTRY
+		),
+	TP_fast_assign(
+		DW_ASSIGN;
+		),
+	TP_printk(DW_PR_FMT, DW_PR_ARG)
+	);
+
 /* clang-format on */
 #endif /* !__DW3000_TRACE || TRACE_HEADER_MULTI_READ */
 
diff --git a/kernel/drivers/net/ieee802154/dw3000_txpower_adjustment.c b/kernel/drivers/net/ieee802154/dw3000_txpower_adjustment.c
index d80e854..1b2ae14 100644
--- a/kernel/drivers/net/ieee802154/dw3000_txpower_adjustment.c
+++ b/kernel/drivers/net/ieee802154/dw3000_txpower_adjustment.c
@@ -325,6 +325,7 @@ static u32 adjust_tx_power(u16 frame_duration_us, u32 ref_tx_power, u8 channel,
 {
 	u32 adjusted_tx_power;
 	u16 target_boost = 0;
+	u16 base_target_boost = 0;
 	u16 current_boost = 0;
 	u16 best_boost_abs = 0;
 	u16 best_boost = 0;
@@ -332,7 +333,7 @@ static u32 adjust_tx_power(u16 frame_duration_us, u32 ref_tx_power, u8 channel,
 	u16 lower_limit = 0;
 
 	const u8 *lut = NULL;
-	uint8_t ref_tx_power_byte[4]; /* txpwr of each segments (UM 8.2.2.20) */
+	uint8_t ref_tx_power_byte[4]; /* txpwr of each segment (UM 8.2.2.20) */
 	uint8_t adj_tx_power_byte[4];
 	uint8_t adj_tx_power_boost[4];
 	u8 best_index;
@@ -341,29 +342,31 @@ static u32 adjust_tx_power(u16 frame_duration_us, u32 ref_tx_power, u8 channel,
 	u8 ref_fine_gain;
 	bool within_margin_flag;
 	bool reached_max_fine_gain_flag;
+	bool shortcut_optim_flag;
 	u8 unlock;
-	u8 i;
+	u8 i, j;
 	int k;
 	int ret = 0;
 
-	target_boost = calculate_power_boost(frame_duration_us);
+	base_target_boost = calculate_power_boost(frame_duration_us);
 	if (th_boost) {
-		*th_boost = target_boost;
+		*th_boost = base_target_boost;
 	}
 	switch (channel) {
 	case 5:
 		lut = fine_gain_lut_chan5;
-		if (target_boost >= MAX_BOOST_CH5)
-			target_boost = MAX_BOOST_CH5;
+		if (base_target_boost > MAX_BOOST_CH5)
+			base_target_boost = MAX_BOOST_CH5;
 		break;
 	default:
 		lut = fine_gain_lut_chan9;
-		if (target_boost >= MAX_BOOST_CH9)
-			target_boost = MAX_BOOST_CH9;
+		if (base_target_boost > MAX_BOOST_CH9)
+			base_target_boost = MAX_BOOST_CH9;
 		break;
 	}
 
 	for (k = 0; k < 4; k++) {
+		target_boost = base_target_boost;
 		current_boost = 0;
 		best_boost_abs = 0;
 		best_boost = 0;
@@ -371,6 +374,7 @@ static u32 adjust_tx_power(u16 frame_duration_us, u32 ref_tx_power, u8 channel,
 		best_coarse_gain = 0;
 		within_margin_flag = false;
 		reached_max_fine_gain_flag = false;
+		shortcut_optim_flag = false;
 		unlock = 0;
 		i = 0;
 		ref_tx_power_byte[k] = (u8)(ref_tx_power >> (k << 3));
@@ -382,10 +386,14 @@ static u32 adjust_tx_power(u16 frame_duration_us, u32 ref_tx_power, u8 channel,
 		i = ref_fine_gain;
 
 		/* Avoid re-doing the same math four times */
-		if (k > 0 && (ref_tx_power_byte[k] == ref_tx_power_byte[0])) {
-			adj_tx_power_byte[k] = adj_tx_power_byte[0];
-			continue;
+		for (j = 0; !shortcut_optim_flag && (j < k); j++) {
+			if (ref_tx_power_byte[k] == ref_tx_power_byte[j]) {
+				adj_tx_power_byte[k] = adj_tx_power_byte[j];
+				shortcut_optim_flag = true;
+			}
 		}
+		if (shortcut_optim_flag)
+			continue;
 
 		/* PHR power must be 6dB lower than PSDU */
 		if (k == 1) {
@@ -458,8 +466,7 @@ static u32 adjust_tx_power(u16 frame_duration_us, u32 ref_tx_power, u8 channel,
 
 			/* Corner case: when fine gain setting is very low, it can happened that
 			 * current boost is already larger than target_boost but not within margin.
-			 * Then, just return current solution.
-			 */
+			 * Then, just return current solution. */
 			if (current_boost >= upper_limit &&
 			    !reached_max_fine_gain_flag) {
 				break;
diff --git a/kernel/drivers/net/ieee802154/mcps802154_fake.c b/kernel/drivers/net/ieee802154/mcps802154_fake.c
index ae8dcad..082b20b 100644
--- a/kernel/drivers/net/ieee802154/mcps802154_fake.c
+++ b/kernel/drivers/net/ieee802154/mcps802154_fake.c
@@ -75,8 +75,8 @@ static void stop(struct mcps802154_llhw *llhw)
 }
 
 static int tx_frame(struct mcps802154_llhw *llhw, struct sk_buff *skb,
-		    const struct mcps802154_tx_frame_info *info, int frame_idx,
-		    int next_delay_dtu)
+		    const struct mcps802154_tx_frame_config *config,
+		    int frame_idx, int next_delay_dtu)
 {
 	if (!started) {
 		pr_err("fake_mcps: %s called and not started\n", __func__);
@@ -90,8 +90,8 @@ static int tx_frame(struct mcps802154_llhw *llhw, struct sk_buff *skb,
 }
 
 static int rx_enable(struct mcps802154_llhw *llhw,
-		     const struct mcps802154_rx_info *info, int frame_idx,
-		     int next_delay_dtu)
+		     const struct mcps802154_rx_frame_config *info,
+		     int frame_idx, int next_delay_dtu)
 {
 	if (!started) {
 		pr_err("fake_mcps: %s called and not started\n", __func__);
@@ -238,9 +238,10 @@ static int get_current_timestamp_dtu(struct mcps802154_llhw *llhw,
 	return 0;
 }
 
-static u64 tx_timestamp_dtu_to_rmarker_rctu(struct mcps802154_llhw *llhw,
-					    u32 tx_timestamp_dtu,
-					    int ant_set_id)
+static u64 tx_timestamp_dtu_to_rmarker_rctu(
+	struct mcps802154_llhw *llhw, u32 tx_timestamp_dtu,
+	const struct mcps802154_hrp_uwb_params *hrp_uwb_params,
+	const struct mcps802154_channel *channel_params, int ant_set_id)
 {
 	if (!started) {
 		pr_err("fake_mcps: %s called and not started\n", __func__);
@@ -281,8 +282,8 @@ static int set_channel(struct mcps802154_llhw *llhw, u8 page, u8 channel,
 	return 0;
 }
 
-static int set_hrp_uwb_params(struct mcps802154_llhw *llhw, int prf, int psr,
-			      int sfd_selector, int phr_rate, int data_rate)
+static int set_hrp_uwb_params(struct mcps802154_llhw *llhw,
+			      const struct mcps802154_hrp_uwb_params *params)
 {
 	if (!started) {
 		pr_err("fake_mcps: %s called and not started\n", __func__);
@@ -434,6 +435,17 @@ static int __init fake_init(void)
 	driver_llhw->hw->flags =
 		(IEEE802154_HW_TX_OMIT_CKSUM | IEEE802154_HW_AFILT |
 		 IEEE802154_HW_PROMISCUOUS | IEEE802154_HW_RX_OMIT_CKSUM);
+	driver_llhw->flags =
+		(MCPS802154_LLHW_BPRF | MCPS802154_LLHW_DATA_RATE_6M81 |
+		 MCPS802154_LLHW_PHR_DATA_RATE_850K |
+		 MCPS802154_LLHW_PHR_DATA_RATE_6M81 | MCPS802154_LLHW_PRF_16 |
+		 MCPS802154_LLHW_PRF_64 | MCPS802154_LLHW_PSR_32 |
+		 MCPS802154_LLHW_PSR_64 | MCPS802154_LLHW_PSR_128 |
+		 MCPS802154_LLHW_PSR_256 | MCPS802154_LLHW_PSR_1024 |
+		 MCPS802154_LLHW_PSR_4096 | MCPS802154_LLHW_SFD_4A |
+		 MCPS802154_LLHW_SFD_4Z_8 | MCPS802154_LLHW_STS_SEGMENT_1 |
+		 MCPS802154_LLHW_AOA_AZIMUTH | MCPS802154_LLHW_AOA_ELEVATION |
+		 MCPS802154_LLHW_AOA_FOM);
 	/* UWB High band 802.15.4a-2007. */
 	driver_llhw->hw->phy->supported.channels[4] |= 0xffe0;
 
diff --git a/kernel/include/net/idle_region_nl.h b/kernel/include/net/idle_region_nl.h
new file mode 120000
index 0000000..d1e5a9c
--- /dev/null
+++ b/kernel/include/net/idle_region_nl.h
@@ -0,0 +1 @@
+../../../mac/include/net/idle_region_nl.h
\ No newline at end of file
diff --git a/kernel/include/net/mcps_skb_frag.h b/kernel/include/net/mcps_skb_frag.h
new file mode 120000
index 0000000..55fa1c3
--- /dev/null
+++ b/kernel/include/net/mcps_skb_frag.h
@@ -0,0 +1 @@
+../../../mac/include/net/mcps_skb_frag.h
\ No newline at end of file
diff --git a/kernel/include/net/simple_ranging_region_nl.h b/kernel/include/net/simple_ranging_region_nl.h
deleted file mode 120000
index 79bdc0f..0000000
--- a/kernel/include/net/simple_ranging_region_nl.h
+++ /dev/null
@@ -1 +0,0 @@
-../../../mac/include/net/simple_ranging_region_nl.h
\ No newline at end of file
diff --git a/kernel/net/mcps802154/Kbuild b/kernel/net/mcps802154/Kbuild
index f6ba8c8..1fd78c3 100644
--- a/kernel/net/mcps802154/Kbuild
+++ b/kernel/net/mcps802154/Kbuild
@@ -1,4 +1,8 @@
-obj-m := mcps802154.o mcps802154_region_fira.o \
+ifndef CONFIG_MCPS802154
+CONFIG_MCPS802154:=m
+endif
+
+obj-$(CONFIG_MCPS802154) := mcps802154.o mcps802154_region_fira.o \
 	 mcps802154_region_nfcc_coex.o \
 	 mcps802154_region_pctt.o \
 
@@ -9,31 +13,29 @@ mcps802154-y := \
 	default_region.o \
 	endless_scheduler.o \
 	on_demand_scheduler.o \
+	idle_region.o \
 	fproc.o \
 	fproc_broken.o \
 	fproc_multi.o \
 	fproc_vendor.o \
 	fproc_nothing.o \
+	fproc_idle.o \
 	fproc_rx.o \
 	fproc_stopped.o \
 	fproc_tx.o \
 	frame.o \
 	ie.o \
 	mcps_main.o \
+	mcps_skb_frag.o \
 	nl.o \
 	ops.o \
 	regions.o \
-	simple_ranging_region.o \
 	schedule.o \
 	schedulers.o \
 	trace.o
 
-mcps802154-$(CONFIG_MCPS802154_TESTMODE) += ping_pong_region.o
-
 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 \
@@ -41,7 +43,13 @@ mcps802154_region_fira-y := \
 	fira_region.o \
 	fira_region_call.o \
 	fira_session.o \
-	fira_trace.o
+	fira_session_fsm.o \
+	fira_session_fsm_init.o \
+	fira_session_fsm_idle.o \
+	fira_session_fsm_active.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_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_round_hopping_crypto.c b/kernel/net/mcps802154/fira_round_hopping_crypto.c
index 2728b21..efb8259 100644
--- a/kernel/net/mcps802154/fira_round_hopping_crypto.c
+++ b/kernel/net/mcps802154/fira_round_hopping_crypto.c
@@ -27,7 +27,7 @@
 #include <linux/scatterlist.h>
 
 int fira_round_hopping_crypto_encrypt(
-	struct fira_round_hopping_sequence *round_hopping_sequence,
+	const struct fira_round_hopping_sequence *round_hopping_sequence,
 	const u8 *data, u8 *out)
 {
 	struct scatterlist sg;
diff --git a/kernel/net/mcps802154/fira_session_fsm.c b/kernel/net/mcps802154/fira_session_fsm.c
new file mode 120000
index 0000000..b815ae3
--- /dev/null
+++ b/kernel/net/mcps802154/fira_session_fsm.c
@@ -0,0 +1 @@
+../../../mac/fira_session_fsm.c
\ No newline at end of file
diff --git a/kernel/net/mcps802154/fira_session_fsm.h b/kernel/net/mcps802154/fira_session_fsm.h
new file mode 120000
index 0000000..5506507
--- /dev/null
+++ b/kernel/net/mcps802154/fira_session_fsm.h
@@ -0,0 +1 @@
+../../../mac/fira_session_fsm.h
\ No newline at end of file
diff --git a/kernel/net/mcps802154/fira_session_fsm_active.c b/kernel/net/mcps802154/fira_session_fsm_active.c
new file mode 120000
index 0000000..63f915b
--- /dev/null
+++ b/kernel/net/mcps802154/fira_session_fsm_active.c
@@ -0,0 +1 @@
+../../../mac/fira_session_fsm_active.c
\ No newline at end of file
diff --git a/kernel/net/mcps802154/fira_session_fsm_active.h b/kernel/net/mcps802154/fira_session_fsm_active.h
new file mode 120000
index 0000000..7654f0a
--- /dev/null
+++ b/kernel/net/mcps802154/fira_session_fsm_active.h
@@ -0,0 +1 @@
+../../../mac/fira_session_fsm_active.h
\ No newline at end of file
diff --git a/kernel/net/mcps802154/fira_session_fsm_idle.c b/kernel/net/mcps802154/fira_session_fsm_idle.c
new file mode 120000
index 0000000..4725342
--- /dev/null
+++ b/kernel/net/mcps802154/fira_session_fsm_idle.c
@@ -0,0 +1 @@
+../../../mac/fira_session_fsm_idle.c
\ No newline at end of file
diff --git a/kernel/net/mcps802154/fira_session_fsm_idle.h b/kernel/net/mcps802154/fira_session_fsm_idle.h
new file mode 120000
index 0000000..5182a72
--- /dev/null
+++ b/kernel/net/mcps802154/fira_session_fsm_idle.h
@@ -0,0 +1 @@
+../../../mac/fira_session_fsm_idle.h
\ No newline at end of file
diff --git a/kernel/net/mcps802154/fira_session_fsm_init.c b/kernel/net/mcps802154/fira_session_fsm_init.c
new file mode 120000
index 0000000..41149be
--- /dev/null
+++ b/kernel/net/mcps802154/fira_session_fsm_init.c
@@ -0,0 +1 @@
+../../../mac/fira_session_fsm_init.c
\ No newline at end of file
diff --git a/kernel/net/mcps802154/fira_session_fsm_init.h b/kernel/net/mcps802154/fira_session_fsm_init.h
new file mode 120000
index 0000000..f520adb
--- /dev/null
+++ b/kernel/net/mcps802154/fira_session_fsm_init.h
@@ -0,0 +1 @@
+../../../mac/fira_session_fsm_init.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/fproc_idle.c b/kernel/net/mcps802154/fproc_idle.c
new file mode 120000
index 0000000..430ae47
--- /dev/null
+++ b/kernel/net/mcps802154/fproc_idle.c
@@ -0,0 +1 @@
+../../../mac/fproc_idle.c
\ No newline at end of file
diff --git a/kernel/net/mcps802154/idle_region.c b/kernel/net/mcps802154/idle_region.c
new file mode 120000
index 0000000..6e6ca32
--- /dev/null
+++ b/kernel/net/mcps802154/idle_region.c
@@ -0,0 +1 @@
+../../../mac/idle_region.c
\ No newline at end of file
diff --git a/kernel/net/mcps802154/idle_region.h b/kernel/net/mcps802154/idle_region.h
new file mode 120000
index 0000000..fe7992b
--- /dev/null
+++ b/kernel/net/mcps802154/idle_region.h
@@ -0,0 +1 @@
+../../../mac/idle_region.h
\ No newline at end of file
diff --git a/kernel/net/mcps802154/mcps802154_fproc.h b/kernel/net/mcps802154/mcps802154_fproc.h
new file mode 120000
index 0000000..add11c0
--- /dev/null
+++ b/kernel/net/mcps802154/mcps802154_fproc.h
@@ -0,0 +1 @@
+../../../mac/mcps802154_fproc.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/kernel/net/mcps802154/mcps_skb_frag.c b/kernel/net/mcps802154/mcps_skb_frag.c
new file mode 120000
index 0000000..cfdbfe1
--- /dev/null
+++ b/kernel/net/mcps802154/mcps_skb_frag.c
@@ -0,0 +1 @@
+../../../mac/mcps_skb_frag.c
\ No newline at end of file
diff --git a/kernel/net/mcps802154/nl.c b/kernel/net/mcps802154/nl.c
index d04eee6..80e520b 100644
--- a/kernel/net/mcps802154/nl.c
+++ b/kernel/net/mcps802154/nl.c
@@ -40,9 +40,6 @@
 #define nla_strscpy nla_strlcpy
 #endif
 
-/* Used to report ranging result, this should later be different per device. */
-static u32 ranging_report_portid;
-
 static struct genl_family mcps802154_nl_family;
 
 static const struct nla_policy
@@ -62,14 +59,6 @@ static const struct nla_policy
 		[MCPS802154_REGION_ATTR_CALL_PARAMS] = { .type = NLA_NESTED },
 	};
 
-static const struct nla_policy mcps802154_nl_ranging_request_policy
-	[MCPS802154_RANGING_REQUEST_ATTR_MAX + 1] = {
-		[MCPS802154_RANGING_REQUEST_ATTR_ID] = { .type = NLA_U32 },
-		[MCPS802154_RANGING_REQUEST_ATTR_FREQUENCY_HZ] = { .type = NLA_U32 },
-		[MCPS802154_RANGING_REQUEST_ATTR_PEER] = { .type = NLA_U64 },
-		[MCPS802154_RANGING_REQUEST_ATTR_REMOTE_PEER] = { .type = NLA_U64 },
-	};
-
 static const struct nla_policy mcps802154_nl_policy[MCPS802154_ATTR_MAX + 1] = {
 	[MCPS802154_ATTR_HW] = { .type = NLA_U32 },
 	[MCPS802154_ATTR_WPAN_PHY_NAME] = ATTR_STRING_POLICY,
@@ -81,12 +70,11 @@ static const struct nla_policy mcps802154_nl_policy[MCPS802154_ATTR_MAX + 1] = {
 	[MCPS802154_ATTR_SCHEDULER_CALL_PARAMS] = { .type = NLA_NESTED },
 	[MCPS802154_ATTR_SCHEDULER_REGION_CALL] = { .type = NLA_NESTED },
 	[MCPS802154_ATTR_CALIBRATIONS] = { .type = NLA_NESTED },
+	[MCPS802154_ATTR_PWR_STATS] = { .type = NLA_NESTED },
 
 #ifdef CONFIG_MCPS802154_TESTMODE
 	[MCPS802154_ATTR_TESTDATA] = { .type = NLA_NESTED },
 #endif
-	[MCPS802154_ATTR_RANGING_REQUESTS] =
-		NLA_POLICY_NESTED_ARRAY(mcps802154_nl_ranging_request_policy),
 };
 
 /**
@@ -403,6 +391,26 @@ static int mcps802154_nl_call_region(struct sk_buff *skb,
 	return r;
 }
 
+/**
+ * mcps802154_nl_close_scheduler() - Close current scheduler and its regions.
+ * @skb: Request message.
+ * @info: Request information.
+ *
+ * Return: 0 or error.
+ */
+static int mcps802154_nl_close_scheduler(struct sk_buff *skb,
+					 struct genl_info *info)
+{
+	struct mcps802154_local *local = info->user_ptr[0];
+	mutex_lock(&local->fsm_lock);
+	local->cur_cmd_info = info;
+	mcps802154_ca_close(local);
+	local->cur_cmd_info = NULL;
+	mutex_unlock(&local->fsm_lock);
+
+	return 0;
+}
+
 struct sk_buff *
 mcps802154_region_call_alloc_reply_skb(struct mcps802154_llhw *llhw,
 				       struct mcps802154_region *region,
@@ -634,243 +642,9 @@ int mcps802154_testmode_reply(struct mcps802154_llhw *llhw, struct sk_buff *skb)
 	return genlmsg_reply(skb, local->cur_cmd_info);
 }
 EXPORT_SYMBOL(mcps802154_testmode_reply);
-
-/**
- * mcps802154_nl_send_ping_pong_report() - Append ping_pong result to a netlink
- * message.
- * @local: MCPS private data.
- * @msg: Message to write to.
- * @portid: Destination port address.
- * @id: ping_pong identifier.
- * @t_0: t_0 of ping pong
- * @t_3: t_3 of ping pong
- * @t_4: t_4 of ping pong
- *
- * Return: 0 or error.
- */
-static int mcps802154_nl_send_ping_pong_report(struct mcps802154_local *local,
-					       struct sk_buff *msg, u32 portid,
-					       int id, u64 t_0, u64 t_3,
-					       u64 t_4)
-{
-	void *hdr;
-	struct nlattr *result;
-
-	hdr = genlmsg_put(msg, ranging_report_portid, 0, &mcps802154_nl_family,
-			  0, MCPS802154_CMD_PING_PONG_REPORT);
-	if (!hdr)
-		return -ENOBUFS;
-
-	if (nla_put_u32(msg, MCPS802154_ATTR_HW, local->hw_idx))
-		goto error;
-
-	result = nla_nest_start(msg, MCPS802154_ATTR_PING_PONG_RESULT);
-	if (!result)
-		goto error;
-
-	if (nla_put_u32(msg, MCPS802154_PING_PONG_RESULT_ATTR_ID, id) ||
-	    nla_put_u64_64bit(msg, MCPS802154_PING_PONG_RESULT_ATTR_T_0, t_0,
-			      0) ||
-	    nla_put_u64_64bit(msg, MCPS802154_PING_PONG_RESULT_ATTR_T_3, t_3,
-			      0) ||
-	    nla_put_u64_64bit(msg, MCPS802154_PING_PONG_RESULT_ATTR_T_4, t_4,
-			      0))
-		goto error;
-
-	nla_nest_end(msg, result);
-
-	genlmsg_end(msg, hdr);
-	return 0;
-error:
-	genlmsg_cancel(msg, hdr);
-	return -EMSGSIZE;
-}
-
-int mcps802154_nl_ping_pong_report(struct mcps802154_llhw *llhw, int id,
-				   u64 t_0, u64 t_3, u64 t_4)
-{
-	struct mcps802154_local *local = llhw_to_local(llhw);
-	struct sk_buff *msg;
-	int r;
-	if (ranging_report_portid == 0)
-		return -ECONNREFUSED;
-	msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
-	if (!msg)
-		return -ENOMEM;
-
-	if (mcps802154_nl_send_ping_pong_report(
-		    local, msg, ranging_report_portid, id, t_0, t_3, t_4)) {
-		nlmsg_free(msg);
-		return -ENOBUFS;
-	}
-
-	r = genlmsg_unicast(wpan_phy_net(local->hw->phy), msg,
-			    ranging_report_portid);
-	if (r == -ECONNREFUSED) {
-		ranging_report_portid = 0;
-	}
-	return r;
-}
 #endif
 
 /**
- * mcps802154_nl_set_ranging_requests() - Set ranging requests for a device.
- * @skb: Request message.
- * @info: Request information.
- *
- * Return: 0 or error.
- */
-static int mcps802154_nl_set_ranging_requests(struct sk_buff *skb,
-					      struct genl_info *info)
-{
-	struct mcps802154_local *local = info->user_ptr[0];
-	struct nlattr *request;
-	struct nlattr *attrs[MCPS802154_RANGING_REQUEST_ATTR_MAX + 1];
-	struct mcps802154_nl_ranging_request
-		requests[MCPS802154_NL_RANGING_REQUESTS_MAX];
-	unsigned int n_requests = 0;
-	int r, rem;
-
-	if (!local->ca.scheduler || !local->ca.scheduler->ops->ranging_setup)
-		return -EOPNOTSUPP;
-
-	if (!info->attrs[MCPS802154_ATTR_RANGING_REQUESTS])
-		return -EINVAL;
-
-	nla_for_each_nested (
-		request, info->attrs[MCPS802154_ATTR_RANGING_REQUESTS], rem) {
-		if (n_requests >= MCPS802154_NL_RANGING_REQUESTS_MAX)
-			return -EINVAL;
-
-		r = nla_parse_nested(attrs, MCPS802154_RANGING_REQUEST_ATTR_MAX,
-				     request,
-				     mcps802154_nl_ranging_request_policy,
-				     info->extack);
-		if (r)
-			return r;
-
-		if (!attrs[MCPS802154_RANGING_REQUEST_ATTR_ID] ||
-		    !attrs[MCPS802154_RANGING_REQUEST_ATTR_FREQUENCY_HZ] ||
-		    !attrs[MCPS802154_RANGING_REQUEST_ATTR_PEER])
-			return -EINVAL;
-
-		requests[n_requests].id =
-			nla_get_s32(attrs[MCPS802154_RANGING_REQUEST_ATTR_ID]);
-		requests[n_requests].frequency_hz = nla_get_s32(
-			attrs[MCPS802154_RANGING_REQUEST_ATTR_FREQUENCY_HZ]);
-		requests[n_requests].peer_extended_addr = nla_get_le64(
-			attrs[MCPS802154_RANGING_REQUEST_ATTR_PEER]);
-		requests[n_requests].remote_peer_extended_addr = 0;
-
-		if (attrs[MCPS802154_RANGING_REQUEST_ATTR_REMOTE_PEER])
-			requests[n_requests]
-				.remote_peer_extended_addr = nla_get_le64(
-				attrs[MCPS802154_RANGING_REQUEST_ATTR_REMOTE_PEER]);
-
-		n_requests++;
-	}
-
-	mutex_lock(&local->fsm_lock);
-	r = local->ca.scheduler->ops->ranging_setup(local->ca.scheduler,
-						    requests, n_requests);
-	mutex_unlock(&local->fsm_lock);
-	if (r)
-		return r;
-
-	/* TODO: store per device. */
-	ranging_report_portid = info->snd_portid;
-
-	return 0;
-}
-
-/**
- * mcps802154_nl_send_ranging_report() - Append ranging result to a netlink
- * message.
- * @local: MCPS private data.
- * @msg: Message to write to.
- * @portid: Destination port address.
- * @id: Ranging identifier.
- * @report: Phase Differences Of Arrival and Time of Flight.
- *
- * Return: 0 or error.
- */
-static int mcps802154_nl_send_ranging_report(
-	struct mcps802154_local *local, struct sk_buff *msg, u32 portid, int id,
-	const struct mcps802154_nl_ranging_report *report)
-{
-	void *hdr;
-	struct nlattr *result;
-
-	hdr = genlmsg_put(msg, ranging_report_portid, 0, &mcps802154_nl_family,
-			  0, MCPS802154_CMD_RANGING_REPORT);
-	if (!hdr)
-		return -ENOBUFS;
-
-	if (nla_put_u32(msg, MCPS802154_ATTR_HW, local->hw_idx))
-		goto error;
-
-	result = nla_nest_start(msg, MCPS802154_ATTR_RANGING_RESULT);
-	if (!result)
-		goto error;
-
-	if (nla_put_u32(msg, MCPS802154_RANGING_RESULT_ATTR_ID, id) ||
-	    nla_put_s32(msg, MCPS802154_RANGING_RESULT_ATTR_TOF_RCTU,
-			report->tof_rctu) ||
-	    nla_put_s32(msg, MCPS802154_RANGING_RESULT_ATTR_LOCAL_PDOA_RAD_Q11,
-			report->local_pdoa_rad_q11) ||
-	    nla_put_s32(msg, MCPS802154_RANGING_RESULT_ATTR_REMOTE_PDOA_RAD_Q11,
-			report->remote_pdoa_rad_q11))
-		goto error;
-	if (!report->is_same_rx_ant_set_id) {
-		if ((nla_put_s32(
-			     msg,
-			     MCPS802154_RANGING_RESULT_ATTR_LOCAL_PDOA_ELEVATION_RAD_Q11,
-			     report->local_pdoa_elevation_rad_q11) ||
-		     nla_put_s32(
-			     msg,
-			     MCPS802154_RANGING_RESULT_ATTR_REMOTE_PDOA_ELEVATION_RAD_Q11,
-			     report->remote_pdoa_elevation_rad_q11)))
-			goto error;
-	}
-
-	nla_nest_end(msg, result);
-	genlmsg_end(msg, hdr);
-	return 0;
-error:
-	genlmsg_cancel(msg, hdr);
-	return -EMSGSIZE;
-}
-
-int mcps802154_nl_ranging_report(
-	struct mcps802154_llhw *llhw, int id,
-	const struct mcps802154_nl_ranging_report *report)
-{
-	struct mcps802154_local *local = llhw_to_local(llhw);
-	struct sk_buff *msg;
-	int r;
-
-	if (ranging_report_portid == 0)
-		return -ECONNREFUSED;
-
-	msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
-	if (!msg)
-		return -ENOMEM;
-
-	if (mcps802154_nl_send_ranging_report(local, msg, ranging_report_portid,
-					      id, report)) {
-		nlmsg_free(msg);
-		return -ENOBUFS;
-	}
-
-	r = genlmsg_unicast(wpan_phy_net(local->hw->phy), msg,
-			    ranging_report_portid);
-	if (r == -ECONNREFUSED)
-		ranging_report_portid = 0;
-
-	return r;
-}
-
-/**
  * mcps802154_nl_put_calibration() - put on calibration in msg.
  * @msg: Request message.
  * @key: calibration name
@@ -1161,6 +935,130 @@ failure:
 	return err;
 }
 
+/**
+ * mcps802154_nl_put_pwr_stats_state() - Put a power statistic state on a netlink message.
+ * @msg: Netlink message.
+ * @state: Related power statistic state.
+ * @time: Duration of this state.
+ * @count: Transitions count to this state.
+ *
+ * Return: 0 or error.
+ */
+static int
+mcps802154_nl_put_pwr_stats_state(struct sk_buff *msg,
+				  enum mcps802154_pwr_stats_attrs state,
+				  u32 time, u32 count)
+{
+	struct nlattr *nl_pwr_stats_state;
+
+	nl_pwr_stats_state = nla_nest_start(msg, state);
+	if (!nl_pwr_stats_state)
+		return -EMSGSIZE;
+	if (nla_put_u32(msg, MCPS802154_PWR_STATS_STATE_ATTR_TIME, time))
+		return -EMSGSIZE;
+	if (nla_put_u32(msg, MCPS802154_PWR_STATS_STATE_ATTR_COUNT, count))
+		return -EMSGSIZE;
+	nla_nest_end(msg, nl_pwr_stats_state);
+	return 0;
+}
+
+/**
+ * mcps802154_nl_get_pwr_stats() - Get power statistics.
+ * @skb: Request message.
+ * @info: Request information.
+ *
+ * Return: 0 or error.
+ */
+static int mcps802154_nl_get_pwr_stats(struct sk_buff *skb,
+				       struct genl_info *info)
+{
+	struct mcps802154_local *local = info->user_ptr[0];
+	struct mcps802154_llhw *llhw = &local->llhw;
+	struct sk_buff *msg;
+	void *hdr;
+	struct mcps802154_power_stats pwr_stats;
+	struct nlattr *nl_pwr_stats;
+	int rc;
+
+	if (!local->ops->get_power_stats)
+		return -EOPNOTSUPP;
+
+	/* Get the power statistics from the low level hardware driver. */
+	rc = local->ops->get_power_stats(llhw, &pwr_stats);
+	if (rc)
+		return rc;
+
+	/* Build the response netlink message. */
+	msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+	if (!msg)
+		return -ENOMEM;
+
+	hdr = genlmsg_put(msg, info->snd_portid, info->snd_seq,
+			  &mcps802154_nl_family, 0,
+			  MCPS802154_CMD_GET_PWR_STATS);
+	if (!hdr) {
+		rc = -ENOBUFS;
+		goto failure;
+	}
+
+	nl_pwr_stats = nla_nest_start(msg, MCPS802154_ATTR_PWR_STATS);
+	if (!nl_pwr_stats)
+		goto nla_put_failure;
+
+	/* Process the SLEEP state. */
+	rc = mcps802154_nl_put_pwr_stats_state(
+		msg, MCPS802154_PWR_STATS_ATTR_SLEEP,
+		pwr_stats.power_state_stats[MCPS802154_PWR_STATE_SLEEP].dur /
+			1000000,
+		pwr_stats.power_state_stats[MCPS802154_PWR_STATE_SLEEP].count);
+	if (rc)
+		goto nla_put_failure;
+
+	/* Process the IDLE state. */
+	rc = mcps802154_nl_put_pwr_stats_state(
+		msg, MCPS802154_PWR_STATS_ATTR_IDLE,
+		pwr_stats.power_state_stats[MCPS802154_PWR_STATE_IDLE].dur /
+			1000000,
+		pwr_stats.power_state_stats[MCPS802154_PWR_STATE_IDLE].count);
+	if (rc)
+		goto nla_put_failure;
+
+	/* Process the RX state. */
+	rc = mcps802154_nl_put_pwr_stats_state(
+		msg, MCPS802154_PWR_STATS_ATTR_RX,
+		pwr_stats.power_state_stats[MCPS802154_PWR_STATE_RX].dur /
+			1000000,
+		pwr_stats.power_state_stats[MCPS802154_PWR_STATE_RX].count);
+	if (rc)
+		goto nla_put_failure;
+
+	/* Process the TX state. */
+	rc = mcps802154_nl_put_pwr_stats_state(
+		msg, MCPS802154_PWR_STATS_ATTR_TX,
+		pwr_stats.power_state_stats[MCPS802154_PWR_STATE_TX].dur /
+			1000000,
+		pwr_stats.power_state_stats[MCPS802154_PWR_STATE_TX].count);
+	if (rc)
+		goto nla_put_failure;
+
+	/* Process the interrupts count. */
+	if (nla_put_u32(msg, MCPS802154_PWR_STATS_ATTR_INTERRUPTS,
+			pwr_stats.interrupts)) {
+		rc = -EMSGSIZE;
+		goto nla_put_failure;
+	}
+
+	nla_nest_end(msg, nl_pwr_stats);
+	genlmsg_end(msg, hdr);
+	return genlmsg_reply(msg, info);
+
+nla_put_failure:
+	genlmsg_cancel(msg, hdr);
+failure:
+	nlmsg_free(msg);
+	return rc;
+}
+
 enum mcps802154_nl_internal_flags {
 	MCPS802154_NL_NEED_HW = 1,
 };
@@ -1265,6 +1163,12 @@ static const struct genl_ops mcps802154_nl_ops[] = {
 		.internal_flags = MCPS802154_NL_NEED_HW,
 	},
 	{
+		.cmd = MCPS802154_CMD_CLOSE_SCHEDULER,
+		.doit = mcps802154_nl_close_scheduler,
+		.flags = GENL_ADMIN_PERM,
+		.internal_flags = MCPS802154_NL_NEED_HW,
+	},
+	{
 		.cmd = MCPS802154_CMD_SET_SCHEDULER_REGIONS,
 		.doit = mcps802154_nl_generic_set_params,
 		.flags = GENL_ADMIN_PERM,
@@ -1291,12 +1195,6 @@ static const struct genl_ops mcps802154_nl_ops[] = {
 	},
 #endif
 	{
-		.cmd = MCPS802154_CMD_SET_RANGING_REQUESTS,
-		.doit = mcps802154_nl_set_ranging_requests,
-		.flags = GENL_ADMIN_PERM,
-		.internal_flags = MCPS802154_NL_NEED_HW,
-	},
-	{
 		.cmd = MCPS802154_CMD_SET_CALIBRATIONS,
 		.doit = mcps802154_nl_set_calibration,
 		.flags = GENL_ADMIN_PERM,
@@ -1314,6 +1212,12 @@ static const struct genl_ops mcps802154_nl_ops[] = {
 		.flags = GENL_ADMIN_PERM,
 		.internal_flags = MCPS802154_NL_NEED_HW,
 	},
+	{
+		.cmd = MCPS802154_CMD_GET_PWR_STATS,
+		.doit = mcps802154_nl_get_pwr_stats,
+		.flags = GENL_ADMIN_PERM,
+		.internal_flags = MCPS802154_NL_NEED_HW,
+	},
 };
 
 static struct genl_family mcps802154_nl_family __ro_after_init = {
diff --git a/kernel/net/mcps802154/nl.h b/kernel/net/mcps802154/nl.h
index eecab90..8bfe3bb 100644
--- a/kernel/net/mcps802154/nl.h
+++ b/kernel/net/mcps802154/nl.h
@@ -24,72 +24,6 @@
 #ifndef MCPS802154_NL_H
 #define MCPS802154_NL_H
 
-#include <linux/types.h>
-
-struct mcps802154_llhw;
-
-#define MCPS802154_NL_RANGING_REQUESTS_MAX 16
-
-struct mcps802154_nl_ranging_request {
-	int id;
-	int frequency_hz;
-	__le64 peer_extended_addr;
-	__le64 remote_peer_extended_addr;
-};
-
-/**
- * struct mcps802154_nl_ranging_report - Measures report.
- */
-struct mcps802154_nl_ranging_report {
-	/** @tof_rctu: Time of Flight, or INT_MIN. */
-	int tof_rctu;
-	/** @local_pdoa_rad_q11: Local Phase Difference Of Arrival, or INT_MIN. */
-	int local_pdoa_rad_q11;
-	/** @remote_pdoa_rad_q11: Remote Phase Difference  Of Arrival, or INT_MIN. */
-	int remote_pdoa_rad_q11;
-	/** @local_pdoa_elevation_rad_q11: Local Phase Difference Of Arrival, or INT_MIN. */
-	int local_pdoa_elevation_rad_q11;
-	/** @remote_pdoa_elevation_rad_q11: Remote Phase Difference Of Arrival, or INT_MIN. */
-	int remote_pdoa_elevation_rad_q11;
-	/** @is_same_rx_ant_set_id: Has azimuth and elevation AoA been done with same antennas set? */
-	bool is_same_rx_ant_set_id;
-};
-
-/**
- * mcps802154_nl_ranging_report() - Report a ranging result, called from ranging
- * code.
- * @llhw: Low-level device pointer.
- * @id: Ranging identifier.
- * @report: Phase Difference Of Arrival and Time of Flight.
- *
- * If this returns -ECONNREFUSED, the receiver is not listening anymore, ranging
- * can be stopped.
- *
- * Return: 0 or error.
- */
-int mcps802154_nl_ranging_report(
-	struct mcps802154_llhw *llhw, int id,
-	const struct mcps802154_nl_ranging_report *report);
-
-#ifdef CONFIG_MCPS802154_TESTMODE
-/**
- * mcps802154_nl_ping_pong_report() - Report a ping pong result, called from
- * factory tests code.
- * @llhw: Low-level device pointer.
- * @id: ping pong identifier.
- * @t_0: t_0 of ping pong
- * @t_3: t_3 of ping pong
- * @t_4: t_4 of ping pong
- *
- * If this returns -ECONNREFUSED, the receiver is not listening anymore,
- * ping pong can be stopped.
- *
- * Return: 0 or error.
- */
-int mcps802154_nl_ping_pong_report(struct mcps802154_llhw *llhw, int id,
-				   u64 t_0, u64 t_3, u64 t_4);
-#endif
-
 int mcps802154_nl_init(void);
 void mcps802154_nl_exit(void);
 
diff --git a/kernel/net/mcps802154/ping_pong_region.c b/kernel/net/mcps802154/ping_pong_region.c
deleted file mode 100644
index 42135d0..0000000
--- a/kernel/net/mcps802154/ping_pong_region.c
+++ /dev/null
@@ -1,581 +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 <asm/unaligned.h>
-#include <linux/slab.h>
-#include <linux/errno.h>
-#include <linux/ieee802154.h>
-#include <net/af_ieee802154.h>
-#include <net/mcps802154_schedule.h>
-#include <net/mcps802154_frame.h>
-#include <net/simple_ranging_region_nl.h>
-#include "nl.h"
-#include "warn_return.h"
-#include "ping_pong_region.h"
-
-/* t_reply1 in RCTU
- * t_reply1 is 400 microseconds, in RCTU:
- * 400000000/15.6500400641026 = 25559039.99999994
- */
-#define TWR_FACTORY_TEST_T_REPLY1_RCTU (25559040ull * 2)
-
-/* t_reply2 in RCTU
- * t_reply2 is 17000 microseconds, in RCTU:
- * 17000000000/15.6500400641026 = 1086259199.999998
- */
-#define TWR_FACTORY_TEST_T_REPLY2_RCTU (1086259200ull)
-
-#define TWR_FACTORY_TEST_FUNCTION_CODE_POLL 0x0110
-#define TWR_FACTORY_TEST_FUNCTION_CODE_RESP 0x0111
-#define TWR_FACTORY_TEST_FUNCTION_CODE_FINAL 0x0112
-
-#define PING_PONG_FRAME_SIZE 2
-
-#define PING_PONG_FRAME_HEADER_SIZE                                       \
-	(IEEE802154_FC_LEN + IEEE802154_SEQ_LEN + IEEE802154_PAN_ID_LEN + \
-	 IEEE802154_SHORT_ADDR_LEN * 2)
-#define PING_PONG_FRAME_MAX_SIZE \
-	(PING_PONG_FRAME_HEADER_SIZE + PING_PONG_FRAME_SIZE)
-
-enum twr_factory_tests_frames {
-	TWR_FACTORY_TEST_FRAME_POLL,
-	TWR_FACTORY_TEST_FRAME_RESP,
-	TWR_FACTORY_TEST_FRAME_FINAL,
-	N_TWR_FACTORY_TEST_FRAMES,
-};
-
-struct ping_pong_initiator_time {
-	u64 t_0;
-	u64 t_3;
-	u64 t_4;
-};
-
-struct ping_pong_local {
-	struct mcps802154_scheduler scheduler;
-	struct mcps802154_llhw *llhw;
-	struct mcps802154_region region_init_active;
-	struct mcps802154_region region_init_idle;
-	struct mcps802154_region region_resp;
-	struct mcps802154_access access;
-	struct mcps802154_access_frame frames[N_TWR_FACTORY_TEST_FRAMES];
-	struct mcps802154_nl_ranging_request ping_pong_request;
-	int region_init_active_duration_dtu;
-	bool enable_init_tx;
-	bool is_responder;
-	struct ping_pong_initiator_time initiator_time;
-};
-
-static inline struct ping_pong_local *
-scheduler_to_ping_pong_local(const struct mcps802154_scheduler *scheduler)
-{
-	return container_of(scheduler, struct ping_pong_local, scheduler);
-}
-
-static inline struct ping_pong_local *
-region_init_active_to_ping_pong_local(struct mcps802154_region *region)
-{
-	return container_of(region, struct ping_pong_local, region_init_active);
-}
-
-static inline struct ping_pong_local *
-region_init_idle_to_ping_pong_local(struct mcps802154_region *region)
-{
-	return container_of(region, struct ping_pong_local, region_init_idle);
-}
-
-static inline struct ping_pong_local *
-region_resp_to_ping_pong_local(struct mcps802154_region *region)
-{
-	return container_of(region, struct ping_pong_local, region_resp);
-}
-
-static inline struct ping_pong_local *
-access_to_ping_pong_local(const struct mcps802154_access *access)
-{
-	return container_of(access, struct ping_pong_local, access);
-}
-
-static void ping_pong_report(struct ping_pong_local *local, u64 t_0, u64 t_3,
-			     u64 t_4)
-{
-	struct mcps802154_nl_ranging_request *request =
-		&local->ping_pong_request;
-
-	mcps802154_nl_ping_pong_report(local->llhw, request->id, t_0, t_3, t_4);
-	/* Disable TX and force schedule update to change the region */
-	local->enable_init_tx = false;
-	mcps802154_schedule_invalidate(local->llhw);
-}
-
-void ping_pong_frame_header_fill_buf(u8 *buf, __le16 pan_id, __le16 dst,
-				     __le16 src)
-{
-	u16 fc = (IEEE802154_FC_TYPE_DATA | IEEE802154_FC_INTRA_PAN |
-		  (IEEE802154_ADDR_SHORT << IEEE802154_FC_DAMODE_SHIFT) |
-		  (1 << IEEE802154_FC_VERSION_SHIFT) |
-		  (IEEE802154_ADDR_SHORT << IEEE802154_FC_SAMODE_SHIFT));
-	u8 seq = 0;
-	size_t pos = 0;
-
-	put_unaligned_le16(fc, buf + pos);
-	pos += IEEE802154_FC_LEN;
-	buf[pos] = seq;
-	pos += IEEE802154_SEQ_LEN;
-	memcpy(buf + pos, &pan_id, sizeof(pan_id));
-	pos += IEEE802154_PAN_ID_LEN;
-	memcpy(buf + pos, &dst, sizeof(dst));
-	pos += IEEE802154_SHORT_ADDR_LEN;
-	memcpy(buf + pos, &src, sizeof(src));
-}
-
-void ping_pong_frame_header_put(struct sk_buff *skb, __le16 pan_id, __le16 dst,
-				__le16 src)
-{
-	ping_pong_frame_header_fill_buf(
-		skb_put(skb, PING_PONG_FRAME_HEADER_SIZE), pan_id, dst, src);
-}
-
-static void ping_pong_resp_rx_frame(struct mcps802154_access *access,
-				    int frame_idx, struct sk_buff *skb,
-				    const struct mcps802154_rx_frame_info *info,
-				    enum mcps802154_rx_error_type error)
-{
-	struct ping_pong_local *local = access_to_ping_pong_local(access);
-	struct mcps802154_nl_ranging_request *request =
-		&local->ping_pong_request;
-	u32 resp_tx_start_dtu;
-
-	if (!skb) {
-		/* In case of NULL skb, to avoid the next TX, we adjust
-		 * the frames count of region access. */
-		access->n_frames = frame_idx + 1;
-		return;
-	}
-	request->peer_extended_addr =
-		get_unaligned_le64(skb->data + PING_PONG_FRAME_HEADER_SIZE -
-				   IEEE802154_SHORT_ADDR_LEN);
-	resp_tx_start_dtu =
-		info->timestamp_dtu +
-		TWR_FACTORY_TEST_T_REPLY1_RCTU / local->llhw->dtu_rctu;
-	/* Set the timings for the next frames. */
-	access->frames[TWR_FACTORY_TEST_FRAME_RESP].tx_frame_info.timestamp_dtu =
-		resp_tx_start_dtu;
-	access->frames[TWR_FACTORY_TEST_FRAME_FINAL].rx.info.timestamp_dtu =
-		resp_tx_start_dtu +
-		TWR_FACTORY_TEST_T_REPLY2_RCTU / local->llhw->dtu_rctu;
-
-	kfree_skb(skb);
-	return;
-}
-
-static struct sk_buff *
-ping_pong_resp_tx_get_frame(struct mcps802154_access *access, int frame_idx)
-{
-	struct ping_pong_local *local = access_to_ping_pong_local(access);
-	struct mcps802154_nl_ranging_request *request =
-		&local->ping_pong_request;
-	struct sk_buff *skb = mcps802154_frame_alloc(
-		local->llhw, PING_PONG_FRAME_MAX_SIZE, GFP_KERNEL);
-	/* Extended address from mcps802154_nl_ranging_request are used as
-	 * short address */
-	ping_pong_frame_header_put(skb, mcps802154_get_pan_id(local->llhw),
-				   request->peer_extended_addr,
-				   mcps802154_get_short_addr(local->llhw));
-
-	if (WARN_ON(frame_idx != TWR_FACTORY_TEST_FRAME_RESP))
-		return skb;
-
-	skb_put_u8(skb, (u8)TWR_FACTORY_TEST_FUNCTION_CODE_RESP);
-	skb_put_u8(skb, (u8)(TWR_FACTORY_TEST_FUNCTION_CODE_RESP >> 8));
-	return skb;
-}
-
-static void ping_pong_tx_return(struct mcps802154_access *access, int frame_idx,
-				struct sk_buff *skb,
-				enum mcps802154_access_tx_return_reason reason)
-{
-	kfree_skb(skb);
-}
-
-struct mcps802154_access_ops ping_pong_resp_access_ops = {
-	.rx_frame = ping_pong_resp_rx_frame,
-	.tx_get_frame = ping_pong_resp_tx_get_frame,
-	.tx_return = ping_pong_tx_return,
-};
-
-static struct mcps802154_access *
-ping_pong_resp_get_access(struct mcps802154_region *region,
-			  u32 next_timestamp_dtu, int next_in_region_dtu,
-			  int region_duration_dtu)
-{
-	struct ping_pong_local *local = region_resp_to_ping_pong_local(region);
-	struct mcps802154_access *access = &local->access;
-	u32 start_dtu = next_timestamp_dtu;
-
-	access->method = MCPS802154_ACCESS_METHOD_MULTI;
-	access->ops = &ping_pong_resp_access_ops;
-	access->n_frames = ARRAY_SIZE(local->frames);
-	access->frames = local->frames;
-
-	access->frames[TWR_FACTORY_TEST_FRAME_POLL].is_tx = false;
-	access->frames[TWR_FACTORY_TEST_FRAME_POLL].rx.info.timestamp_dtu =
-		start_dtu;
-	access->frames[TWR_FACTORY_TEST_FRAME_POLL].rx.info.timeout_dtu = -1;
-	access->frames[TWR_FACTORY_TEST_FRAME_POLL].rx.info.flags =
-		MCPS802154_RX_INFO_TIMESTAMP_DTU | MCPS802154_RX_INFO_RANGING |
-		MCPS802154_RX_INFO_KEEP_RANGING_CLOCK;
-	access->frames[TWR_FACTORY_TEST_FRAME_POLL].rx.frame_info_flags_request =
-		MCPS802154_RX_FRAME_INFO_TIMESTAMP_DTU;
-
-	access->frames[TWR_FACTORY_TEST_FRAME_RESP].is_tx = true;
-	access->frames[TWR_FACTORY_TEST_FRAME_RESP].tx_frame_info.flags =
-		MCPS802154_TX_FRAME_TIMESTAMP_DTU |
-		MCPS802154_TX_FRAME_RANGING |
-		MCPS802154_TX_FRAME_KEEP_RANGING_CLOCK;
-	access->frames[TWR_FACTORY_TEST_FRAME_RESP]
-		.tx_frame_info.rx_enable_after_tx_dtu = 0;
-	access->frames[TWR_FACTORY_TEST_FRAME_RESP]
-		.tx_frame_info.rx_enable_after_tx_timeout_dtu = 0;
-
-	access->frames[TWR_FACTORY_TEST_FRAME_FINAL].is_tx = false;
-	access->frames[TWR_FACTORY_TEST_FRAME_FINAL].rx.info.flags =
-		MCPS802154_RX_INFO_TIMESTAMP_DTU | MCPS802154_RX_INFO_RANGING;
-	access->frames[TWR_FACTORY_TEST_FRAME_FINAL].rx.info.timeout_dtu = 0;
-	access->frames[TWR_FACTORY_TEST_FRAME_FINAL]
-		.rx.frame_info_flags_request = 0;
-
-	return access;
-}
-
-static struct mcps802154_region_ops ping_pong_region_resp_ops = {
-	.owner = THIS_MODULE,
-	.name = "ping-pong-resp",
-	.get_access = ping_pong_resp_get_access,
-};
-
-static void
-ping_pong_init_idle_rx_frame(struct mcps802154_access *access, int frame_idx,
-			     struct sk_buff *skb,
-			     const struct mcps802154_rx_frame_info *info,
-			     enum mcps802154_rx_error_type error)
-{
-	if (!skb)
-		return;
-	kfree_skb(skb);
-}
-
-struct mcps802154_access_ops ping_pong_init_idle_access_ops = {
-	.rx_frame = ping_pong_init_idle_rx_frame,
-};
-
-static struct mcps802154_access *
-ping_pong_init_idle_get_access(struct mcps802154_region *region,
-			       u32 next_timestamp_dtu, int next_in_region_dtu,
-			       int region_duration_dtu)
-{
-	struct ping_pong_local *local =
-		region_init_idle_to_ping_pong_local(region);
-	struct mcps802154_access *access = &local->access;
-
-	access->method = MCPS802154_ACCESS_METHOD_IMMEDIATE_RX;
-	access->ops = &ping_pong_init_idle_access_ops;
-	return &local->access;
-}
-
-static struct mcps802154_region_ops ping_pong_region_init_idle_ops = {
-	.owner = THIS_MODULE,
-	.name = "ping-pong-init-idle",
-	.get_access = ping_pong_init_idle_get_access,
-};
-
-static void
-ping_pong_init_active_rx_frame(struct mcps802154_access *access, int frame_idx,
-			       struct sk_buff *skb,
-			       const struct mcps802154_rx_frame_info *info,
-			       enum mcps802154_rx_error_type error)
-{
-	struct ping_pong_local *local = access_to_ping_pong_local(access);
-	u32 final_timestamp_dtu;
-
-	if (!skb) {
-		/* Remote peer time out: set all time markers to 0 */
-		local->initiator_time.t_0 = 0;
-		local->initiator_time.t_3 = 0;
-		local->initiator_time.t_4 = 0;
-		/* Avoid the next TX */
-		access->n_frames = frame_idx + 1;
-		return;
-	}
-	final_timestamp_dtu =
-		info->timestamp_dtu +
-		TWR_FACTORY_TEST_T_REPLY2_RCTU / local->llhw->dtu_rctu;
-	local->initiator_time.t_3 = info->timestamp_rctu;
-	local->initiator_time.t_4 = mcps802154_tx_timestamp_dtu_to_rmarker_rctu(
-		local->llhw, final_timestamp_dtu, 0);
-	/* Set the timing for the final frame */
-	access->frames[TWR_FACTORY_TEST_FRAME_FINAL]
-		.tx_frame_info.timestamp_dtu = final_timestamp_dtu;
-
-	kfree_skb(skb);
-}
-
-static struct sk_buff *
-ping_pong_init_active_tx_get_frame(struct mcps802154_access *access,
-				   int frame_idx)
-{
-	struct ping_pong_local *local = access_to_ping_pong_local(access);
-	struct mcps802154_nl_ranging_request *request =
-		&local->ping_pong_request;
-	struct sk_buff *skb = mcps802154_frame_alloc(
-		local->llhw, PING_PONG_FRAME_MAX_SIZE, GFP_KERNEL);
-
-	ping_pong_frame_header_put(skb, mcps802154_get_pan_id(local->llhw),
-				   request->peer_extended_addr,
-				   mcps802154_get_short_addr(local->llhw));
-	if (frame_idx == TWR_FACTORY_TEST_FRAME_POLL) {
-		skb_put_u8(skb, (u8)TWR_FACTORY_TEST_FUNCTION_CODE_POLL);
-		skb_put_u8(skb, (u8)(TWR_FACTORY_TEST_FUNCTION_CODE_POLL >> 8));
-	} else {
-		WARN_ON(frame_idx != TWR_FACTORY_TEST_FRAME_FINAL);
-		skb_put_u8(skb, (u8)TWR_FACTORY_TEST_FUNCTION_CODE_FINAL);
-		skb_put_u8(skb,
-			   (u8)(TWR_FACTORY_TEST_FUNCTION_CODE_FINAL >> 8));
-	}
-	return skb;
-}
-
-static void ping_pong_init_active_access_done(struct mcps802154_access *access,
-					      bool error)
-{
-	struct ping_pong_local *local = access_to_ping_pong_local(access);
-
-	ping_pong_report(local, local->initiator_time.t_0,
-			 local->initiator_time.t_3, local->initiator_time.t_4);
-}
-
-struct mcps802154_access_ops ping_pong_init_active_access_ops = {
-	.common = {
-		.access_done = ping_pong_init_active_access_done,
-	},
-	.rx_frame = ping_pong_init_active_rx_frame,
-	.tx_get_frame = ping_pong_init_active_tx_get_frame,
-	.tx_return = ping_pong_tx_return,
-};
-
-static struct mcps802154_access *
-ping_pong_init_active_get_access(struct mcps802154_region *region,
-				 u32 next_timestamp_dtu, int next_in_region_dtu,
-				 int region_duration_dtu)
-{
-	struct ping_pong_local *local =
-		region_init_active_to_ping_pong_local(region);
-	struct mcps802154_access *access = &local->access;
-	u32 start_dtu;
-
-	/* Only send frames on time per schedule */
-	if (next_in_region_dtu != 0) {
-		return NULL;
-	}
-	start_dtu = next_timestamp_dtu;
-	access->method = MCPS802154_ACCESS_METHOD_MULTI;
-	access->n_frames = ARRAY_SIZE(local->frames);
-	access->frames = local->frames;
-	/* Hard-coded! */
-	access->n_frames = ARRAY_SIZE(local->frames);
-	access->frames[TWR_FACTORY_TEST_FRAME_POLL].is_tx = true;
-	access->frames[TWR_FACTORY_TEST_FRAME_POLL].tx_frame_info.timestamp_dtu =
-		start_dtu;
-	access->frames[TWR_FACTORY_TEST_FRAME_POLL].tx_frame_info.flags =
-		MCPS802154_TX_FRAME_TIMESTAMP_DTU |
-		MCPS802154_TX_FRAME_RANGING |
-		MCPS802154_TX_FRAME_KEEP_RANGING_CLOCK;
-	access->frames[TWR_FACTORY_TEST_FRAME_POLL]
-		.tx_frame_info.rx_enable_after_tx_dtu = 0;
-	access->frames[TWR_FACTORY_TEST_FRAME_POLL]
-		.tx_frame_info.rx_enable_after_tx_timeout_dtu = 0;
-	local->initiator_time.t_0 = mcps802154_tx_timestamp_dtu_to_rmarker_rctu(
-		local->llhw, start_dtu, 0);
-
-	access->frames[TWR_FACTORY_TEST_FRAME_RESP].is_tx = false;
-	access->frames[TWR_FACTORY_TEST_FRAME_RESP].rx.info.timestamp_dtu =
-		start_dtu +
-		TWR_FACTORY_TEST_T_REPLY1_RCTU / local->llhw->dtu_rctu;
-	access->frames[TWR_FACTORY_TEST_FRAME_RESP].rx.info.timeout_dtu = 0;
-	access->frames[TWR_FACTORY_TEST_FRAME_RESP].rx.info.flags =
-		MCPS802154_RX_INFO_TIMESTAMP_DTU | MCPS802154_RX_INFO_RANGING |
-		MCPS802154_RX_INFO_KEEP_RANGING_CLOCK;
-	access->frames[TWR_FACTORY_TEST_FRAME_RESP].rx.frame_info_flags_request =
-		MCPS802154_RX_FRAME_INFO_TIMESTAMP_DTU |
-		MCPS802154_RX_FRAME_INFO_TIMESTAMP_RCTU;
-
-	access->frames[TWR_FACTORY_TEST_FRAME_FINAL].is_tx = true;
-	access->frames[TWR_FACTORY_TEST_FRAME_FINAL]
-		.tx_frame_info.timestamp_dtu =
-		start_dtu + (TWR_FACTORY_TEST_T_REPLY1_RCTU +
-			     TWR_FACTORY_TEST_T_REPLY2_RCTU) /
-				    local->llhw->dtu_rctu;
-	access->frames[TWR_FACTORY_TEST_FRAME_FINAL].tx_frame_info.flags =
-		MCPS802154_TX_FRAME_TIMESTAMP_DTU | MCPS802154_TX_FRAME_RANGING;
-	access->frames[TWR_FACTORY_TEST_FRAME_FINAL]
-		.tx_frame_info.rx_enable_after_tx_dtu = 0;
-	access->frames[TWR_FACTORY_TEST_FRAME_FINAL]
-		.tx_frame_info.rx_enable_after_tx_timeout_dtu = 0;
-	access->ops = &ping_pong_init_active_access_ops;
-	return &local->access;
-}
-
-static struct mcps802154_region_ops ping_pong_region_init_active_ops = {
-	.owner = THIS_MODULE,
-	.name = "ping-pong-init-active",
-	.get_access = ping_pong_init_active_get_access,
-};
-
-static struct mcps802154_scheduler *
-ping_pong_scheduler_open(struct mcps802154_llhw *llhw)
-{
-	struct ping_pong_local *local;
-
-	local = kzalloc(sizeof(*local), GFP_KERNEL);
-	if (!local)
-		return NULL;
-	local->llhw = llhw;
-	local->region_init_active.ops = &ping_pong_region_init_active_ops;
-	local->region_init_idle.ops = &ping_pong_region_init_idle_ops;
-	local->region_resp.ops = &ping_pong_region_resp_ops;
-	local->is_responder = false;
-	local->enable_init_tx = false;
-	local->region_init_active_duration_dtu =
-		(TWR_FACTORY_TEST_T_REPLY1_RCTU +
-		 TWR_FACTORY_TEST_T_REPLY2_RCTU) /
-		local->llhw->dtu_rctu;
-	return &local->scheduler;
-}
-
-static void ping_pong_scheduler_close(struct mcps802154_scheduler *scheduler)
-{
-	struct ping_pong_local *ping_pong_local =
-		scheduler_to_ping_pong_local(scheduler);
-	kfree(ping_pong_local);
-}
-
-static int ping_pong_scheduler_update_schedule(
-	struct mcps802154_scheduler *scheduler,
-	const struct mcps802154_schedule_update *schedule_update,
-	u32 next_timestamp_dtu)
-{
-	struct ping_pong_local *local = scheduler_to_ping_pong_local(scheduler);
-	int r;
-
-	/* Remove the region in the schedule */
-	r = mcps802154_schedule_recycle(schedule_update, 0,
-					MCPS802154_DURATION_NO_CHANGE);
-	WARN_RETURN(r);
-
-	if (local->is_responder) {
-		r = mcps802154_schedule_add_region(schedule_update,
-						   &local->region_resp, 0, 0);
-	} else {
-		if (local->enable_init_tx) {
-			r = mcps802154_schedule_add_region(
-				schedule_update, &local->region_init_active, 0,
-				local->region_init_active_duration_dtu);
-		} else {
-			r = mcps802154_schedule_add_region(
-				schedule_update, &local->region_init_idle, 0,
-				0);
-		}
-	}
-	return r;
-}
-
-static int ping_pong_scheduler_ping_pong_setup(
-	struct mcps802154_scheduler *scheduler,
-	const struct mcps802154_nl_ranging_request *requests,
-	unsigned int n_requests)
-{
-	struct ping_pong_local *local = scheduler_to_ping_pong_local(scheduler);
-
-	if (local->is_responder)
-		return -EOPNOTSUPP;
-	if (n_requests != 1)
-		return -EINVAL;
-	if (requests[0].remote_peer_extended_addr)
-		return -EOPNOTSUPP;
-	local->ping_pong_request = requests[0];
-	/* Enable TX and force schedule update to change the region */
-	local->enable_init_tx = true;
-	mcps802154_schedule_invalidate(local->llhw);
-	return 0;
-}
-
-static int
-ping_pong_scheduler_set_parameters(struct mcps802154_scheduler *scheduler,
-				   const struct nlattr *params_attr,
-				   struct netlink_ext_ack *extack)
-{
-	struct ping_pong_local *local = scheduler_to_ping_pong_local(scheduler);
-	struct nlattr *attrs[SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_MAX + 1];
-	int r;
-	static const struct nla_policy nla_policy[SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_MAX +
-						  1] = {
-		[SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_NODE_TYPE] = { .type = NLA_U32 },
-	};
-
-	r = nla_parse_nested(attrs,
-			     SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_MAX,
-			     params_attr, nla_policy, extack);
-	if (r)
-		return r;
-
-	if (attrs[SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_NODE_TYPE]) {
-		u32 type = nla_get_u32(
-			attrs[SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_NODE_TYPE]);
-
-		if (type > 1)
-			return -EINVAL;
-
-		local->is_responder = type == 1 ? true : false;
-		mcps802154_schedule_invalidate(local->llhw);
-	}
-
-	return 0;
-}
-
-static struct mcps802154_scheduler_ops ping_pong_region_scheduler = {
-	.owner = THIS_MODULE,
-	.name = "ping-pong",
-	.open = ping_pong_scheduler_open,
-	.close = ping_pong_scheduler_close,
-	.update_schedule = ping_pong_scheduler_update_schedule,
-	.ranging_setup = ping_pong_scheduler_ping_pong_setup,
-	.set_parameters = ping_pong_scheduler_set_parameters,
-};
-
-int __init ping_pong_region_init(void)
-{
-	return mcps802154_scheduler_register(&ping_pong_region_scheduler);
-}
-
-void __exit ping_pong_region_exit(void)
-{
-	mcps802154_scheduler_unregister(&ping_pong_region_scheduler);
-}
diff --git a/kernel/net/mcps802154/simple_ranging_region.c b/kernel/net/mcps802154/simple_ranging_region.c
deleted file mode 120000
index 87111a1..0000000
--- a/kernel/net/mcps802154/simple_ranging_region.c
+++ /dev/null
@@ -1 +0,0 @@
-../../../mac/simple_ranging_region.c
\ No newline at end of file
diff --git a/kernel/net/mcps802154/simple_ranging_region.h b/kernel/net/mcps802154/simple_ranging_region.h
deleted file mode 120000
index f8e6e18..0000000
--- a/kernel/net/mcps802154/simple_ranging_region.h
+++ /dev/null
@@ -1 +0,0 @@
-../../../mac/simple_ranging_region.h
\ No newline at end of file
diff --git a/mac/ca.c b/mac/ca.c
index 69fb533..6c8301f 100644
--- a/mac/ca.c
+++ b/mac/ca.c
@@ -29,7 +29,7 @@
 #include "schedulers.h"
 #include "trace.h"
 
-struct mcps802154_access_common_ops idle_access_ops = {};
+struct mcps802154_access_common_ops ca_access_ops = {};
 
 static int mcps802154_ca_trace_int(struct mcps802154_local *local, const int r)
 {
@@ -109,7 +109,7 @@ int mcps802154_ca_start(struct mcps802154_local *local)
 	}
 
 	local->start_stop_request = true;
-	local->fproc.state->schedule_change(local);
+	mcps802154_fproc_schedule_change(local);
 
 	return local->started ? 0 : -EIO;
 }
@@ -117,7 +117,7 @@ int mcps802154_ca_start(struct mcps802154_local *local)
 void mcps802154_ca_stop(struct mcps802154_local *local)
 {
 	local->start_stop_request = false;
-	local->fproc.state->schedule_change(local);
+	mcps802154_fproc_schedule_change(local);
 }
 
 void mcps802154_ca_notify_stop(struct mcps802154_local *local)
@@ -332,18 +332,28 @@ static int mcps802154_ca_next_region(struct mcps802154_local *local,
 	struct mcps802154_schedule_region *sched_region;
 	int next_dtu = next_timestamp_dtu - sched->start_timestamp_dtu;
 	bool changed = 0;
+	bool once;
 
 	sched_region = &sched->regions[sched->current_index];
+	once = sched_region->once;
 
-	/* If not an endless region, need to test if still inside. */
-	while (sched_region->duration_dtu != 0 &&
-	       next_dtu - sched_region->start_dtu >=
-		       sched_region->duration_dtu) {
+	/* If the region schedule is over, select the next region if
+	 * possible. */
+	while (once || (sched_region->duration_dtu != 0 &&
+			next_dtu - sched_region->start_dtu >=
+				sched_region->duration_dtu)) {
 		sched->current_index++;
 		changed = 1;
+		once = false;
 
 		/* No more region, need a new schedule. */
 		if (sched->current_index >= sched->n_regions) {
+			/* Reduce the schedule duration when not fully used. */
+			if (sched_region->once && sched_region->duration_dtu) {
+				sched->duration_dtu =
+					next_timestamp_dtu -
+					sched->start_timestamp_dtu;
+			}
 			return mcps802154_schedule_update(local,
 							  next_timestamp_dtu);
 		}
@@ -355,21 +365,21 @@ static int mcps802154_ca_next_region(struct mcps802154_local *local,
 }
 
 /**
- * mcps802154_ca_idle() - Fill and return an idle access.
+ * mcps802154_ca_nothing() - Fill and return a nothing access.
  * @local: MCPS private data.
- * @timestamp_dtu: Start of idle period.
- * @duration_dtu: Duration of idle period, or 0 for endless.
+ * @timestamp_dtu: Start of nothing period.
+ * @duration_dtu: Duration of nothing period, or 0 for endless.
  *
  * Return: Pointer to access allocated inside the context.
  */
-struct mcps802154_access *mcps802154_ca_idle(struct mcps802154_local *local,
-					     u32 timestamp_dtu,
-					     int duration_dtu)
+struct mcps802154_access *mcps802154_ca_nothing(struct mcps802154_local *local,
+						u32 timestamp_dtu,
+						int duration_dtu)
 {
 	struct mcps802154_access *access = &local->ca.idle_access;
 
 	access->method = MCPS802154_ACCESS_METHOD_NOTHING;
-	access->common_ops = &idle_access_ops;
+	access->common_ops = &ca_access_ops;
 	access->timestamp_dtu = timestamp_dtu;
 	access->duration_dtu = duration_dtu;
 	return access;
@@ -409,7 +419,8 @@ mcps802154_ca_get_access(struct mcps802154_local *local, u32 next_timestamp_dtu)
 
 		/* Stay in IDLE when no schedule. */
 		if (r == -ENOENT)
-			return mcps802154_ca_idle(local, false, 0);
+			return mcps802154_ca_nothing(local, next_timestamp_dtu,
+						     0);
 		else if (r < 0)
 			return NULL;
 
@@ -420,11 +431,12 @@ mcps802154_ca_get_access(struct mcps802154_local *local, u32 next_timestamp_dtu)
 			sched->start_timestamp_dtu + sched_region->start_dtu;
 		region_duration_dtu = sched_region->duration_dtu;
 
-		/* If the region has changed, access date may be postponed. */
 		if (changed) {
 			if (is_before_dtu(next_timestamp_dtu,
-					  region_start_timestamp_dtu))
+					  region_start_timestamp_dtu)) {
+				/* Access date may be postponed. */
 				next_timestamp_dtu = region_start_timestamp_dtu;
+			}
 		}
 
 		/* Get access. */
@@ -439,6 +451,7 @@ mcps802154_ca_get_access(struct mcps802154_local *local, u32 next_timestamp_dtu)
 		access = region->ops->get_access(region, next_timestamp_dtu,
 						 next_in_region_dtu,
 						 region_duration_dtu);
+
 		if (access)
 			return access;
 
@@ -450,7 +463,7 @@ mcps802154_ca_get_access(struct mcps802154_local *local, u32 next_timestamp_dtu)
 
 			if (is_before_dtu(idle_timestamp_dtu,
 					  region_end_timestamp_dtu)) {
-				return mcps802154_ca_idle(
+				return mcps802154_ca_nothing(
 					local, next_timestamp_dtu,
 					region_end_timestamp_dtu -
 						next_timestamp_dtu);
@@ -459,7 +472,8 @@ mcps802154_ca_get_access(struct mcps802154_local *local, u32 next_timestamp_dtu)
 			/* Continue after the current region. */
 			next_timestamp_dtu = region_end_timestamp_dtu;
 		} else {
-			return mcps802154_ca_idle(local, next_timestamp_dtu, 0);
+			return mcps802154_ca_nothing(local, next_timestamp_dtu,
+						     0);
 		}
 	}
 }
@@ -467,7 +481,7 @@ mcps802154_ca_get_access(struct mcps802154_local *local, u32 next_timestamp_dtu)
 void mcps802154_ca_may_reschedule(struct mcps802154_local *local)
 {
 	if (!local->ca.held)
-		local->fproc.state->schedule_change(local);
+		mcps802154_fproc_schedule_change(local);
 }
 
 void mcps802154_ca_access_hold(struct mcps802154_local *local)
@@ -479,5 +493,5 @@ void mcps802154_ca_invalidate_schedule(struct mcps802154_local *local)
 {
 	local->ca.reset = true;
 
-	local->fproc.state->schedule_change(local);
+	mcps802154_fproc_schedule_change(local);
 }
diff --git a/mac/endless_scheduler.c b/mac/endless_scheduler.c
index 32fc958..76ee7f8 100644
--- a/mac/endless_scheduler.c
+++ b/mac/endless_scheduler.c
@@ -92,7 +92,8 @@ static int mcps802154_endless_scheduler_update_schedule(
 	/* Can not fail, only possible error is invalid parameters. */
 	WARN_RETURN(r);
 
-	r = mcps802154_schedule_add_region(schedule_update, region, 0, 0);
+	r = mcps802154_schedule_add_region(schedule_update, region, 0, 0,
+					   false);
 
 	return r;
 }
diff --git a/mac/fira_access.c b/mac/fira_access.c
index 9146f78..1eab75b 100644
--- a/mac/fira_access.c
+++ b/mac/fira_access.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the UWB stack for linux.
  *
- * Copyright (c) 2020-2021 Qorvo US, Inc.
+ * 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.
@@ -21,24 +21,25 @@
  * Qorvo. Please contact Qorvo to inquire about licensing terms.
  */
 
+#include "fira_round_hopping_sequence.h"
 #include "fira_access.h"
 #include "fira_session.h"
 #include "fira_frame.h"
 #include "fira_trace.h"
+#include "fira_sts.h"
 
 #include <asm/unaligned.h>
 #include <linux/string.h>
 #include <linux/ieee802154.h>
 #include <linux/math64.h>
 #include <linux/limits.h>
+#include <linux/errno.h>
 #include <net/mcps802154_frame.h>
 
 #include "warn_return.h"
 
 #define FIRA_STS_FOM_THRESHOLD 153
-
-static struct mcps802154_access *
-fira_access_controlee(struct fira_local *local, struct fira_session *session);
+#define FIRA_RSSI_MAX 0xff
 
 /**
  * sat_fp() - Saturate the range of fixed-point
@@ -90,26 +91,27 @@ static void fira_access_setup_frame(struct fira_local *local,
 				    bool is_tx)
 {
 	const struct fira_session_params *params = &session->params;
+	struct mcps802154_access *access = &local->access;
 	struct mcps802154_sts_params *sts_params_for_access = NULL;
 	int rframe_config = session->params.rframe_config;
 
 	const struct fira_measurement_sequence_step *current_ms_step =
-		fira_session_get_current_meas_seq_step(session);
+		fira_session_get_meas_seq_step(session);
 
 	bool is_rframe = slot->message_id <= FIRA_MESSAGE_ID_RFRAME_MAX;
 	bool is_last_rframe = slot->message_id == FIRA_MESSAGE_ID_RANGING_FINAL;
 	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. */
-		sts_params->n_segs = 1;
-		sts_params->seg_len = 64;
+		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 ?
+				128 :
+				params->sts_length == FIRA_STS_LENGTH_32 ? 32 :
+									   64;
 		sts_params->sp2_tx_gap_4chips = 0;
 		sts_params->sp2_rx_gap_4chips[0] = 0;
 		sts_params->sp2_rx_gap_4chips[1] = 0;
@@ -119,7 +121,7 @@ static void fira_access_setup_frame(struct fira_local *local,
 	}
 
 	if (is_tx) {
-		u8 flags = MCPS802154_TX_FRAME_TIMESTAMP_DTU;
+		u8 flags = MCPS802154_TX_FRAME_CONFIG_TIMESTAMP_DTU;
 
 		/* Add a small margin to the Tx timestamps. */
 		if (!is_first_frame)
@@ -134,21 +136,23 @@ static void fira_access_setup_frame(struct fira_local *local,
 			ranging_info->timestamps_rctu[slot->message_id] =
 				mcps802154_tx_timestamp_dtu_to_rmarker_rctu(
 					local->llhw, frame_dtu,
+					access->hrp_uwb_params, access->channel,
 					slot->tx_ant_set);
 
-			flags |= MCPS802154_TX_FRAME_RANGING;
+			flags |= MCPS802154_TX_FRAME_CONFIG_RANGING;
 			if (rframe_config == FIRA_RFRAME_CONFIG_SP3)
-				flags |= MCPS802154_TX_FRAME_SP3;
+				flags |= MCPS802154_TX_FRAME_CONFIG_SP3;
 			else
-				flags |= MCPS802154_TX_FRAME_SP1;
+				flags |= MCPS802154_TX_FRAME_CONFIG_SP1;
 			if (!is_last_rframe)
-				flags |= MCPS802154_TX_FRAME_KEEP_RANGING_CLOCK;
+				flags |=
+					MCPS802154_TX_FRAME_CONFIG_KEEP_RANGING_CLOCK;
 		} else if (is_first_frame) {
-			flags |= MCPS802154_TX_FRAME_RANGING_ROUND;
+			flags |= MCPS802154_TX_FRAME_CONFIG_RANGING_ROUND;
 		}
 		*frame = (struct mcps802154_access_frame){
 			.is_tx = true,
-			.tx_frame_info = {
+			.tx_frame_config = {
 				.timestamp_dtu = frame_dtu,
 				.flags = flags,
 				.ant_set_id = slot->tx_ant_set,
@@ -156,22 +160,26 @@ static void fira_access_setup_frame(struct fira_local *local,
 			.sts_params = sts_params_for_access,
 		};
 	} else {
-		u8 flags = MCPS802154_RX_INFO_TIMESTAMP_DTU;
+		u8 flags = MCPS802154_RX_FRAME_CONFIG_TIMESTAMP_DTU;
 		u16 request = 0;
 
+		if (request_rssi)
+			request |= MCPS802154_RX_FRAME_INFO_RSSI;
 		if (is_rframe) {
-			flags |= MCPS802154_RX_INFO_RANGING;
+			flags |= MCPS802154_RX_FRAME_CONFIG_RANGING;
 			if (rframe_config == FIRA_RFRAME_CONFIG_SP3)
-				flags |= MCPS802154_RX_INFO_SP3;
+				flags |= MCPS802154_RX_FRAME_CONFIG_SP3;
 			else
-				flags |= MCPS802154_RX_INFO_SP1;
+				flags |= MCPS802154_RX_FRAME_CONFIG_SP1;
 			if (!is_last_rframe)
-				flags |= MCPS802154_RX_INFO_KEEP_RANGING_CLOCK;
-			request = MCPS802154_RX_FRAME_INFO_TIMESTAMP_RCTU |
-				  MCPS802154_RX_FRAME_INFO_RANGING_STS_FOM;
+				flags |=
+					MCPS802154_RX_FRAME_CONFIG_KEEP_RANGING_CLOCK;
+			request |= MCPS802154_RX_FRAME_INFO_TIMESTAMP_RCTU |
+				   MCPS802154_RX_FRAME_INFO_RANGING_STS_FOM;
 			if (current_ms_step->type !=
 			    FIRA_MEASUREMENT_TYPE_RANGE) {
-				flags |= MCPS802154_RX_INFO_RANGING_PDOA;
+				flags |=
+					MCPS802154_RX_FRAME_CONFIG_RANGING_PDOA;
 				request |=
 					MCPS802154_RX_FRAME_INFO_RANGING_PDOA |
 					MCPS802154_RX_FRAME_INFO_RANGING_PDOA_FOM;
@@ -186,7 +194,7 @@ static void fira_access_setup_frame(struct fira_local *local,
 		*frame = (struct mcps802154_access_frame){
 			.is_tx = false,
 			.rx = {
-				.info = {
+				.frame_config = {
 					.timestamp_dtu = frame_dtu,
 					.flags = flags,
 					.ant_set_id = slot->rx_ant_set,
@@ -198,14 +206,39 @@ static void fira_access_setup_frame(struct fira_local *local,
 	}
 }
 
+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;
+
+	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_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_idx;
+}
+
 static bool fira_rx_sts_good(struct fira_local *local,
 			     const struct mcps802154_rx_frame_info *info)
 {
+	int i;
 	if (!(info->flags & MCPS802154_RX_FRAME_INFO_RANGING_STS_FOM))
 		return false;
-	/* Only one segment for the moment. */
-	if (info->ranging_sts_fom[0] < FIRA_STS_FOM_THRESHOLD)
-		return false;
+	for (i = 0; i < MCPS802154_STS_N_SEGS_MAX; i++) {
+		if (info->ranging_sts_fom[i] < FIRA_STS_FOM_THRESHOLD)
+			return false;
+	}
 	return true;
 }
 
@@ -220,16 +253,166 @@ static void fira_ranging_info_set_status(struct fira_ranging_info *ranging_info,
 	ranging_info->slot_index = slot_index;
 }
 
+static void
+fira_diagnostic_rssis(const struct mcps802154_rx_measurement_info *info,
+		      struct fira_diagnostic *diagnostic)
+{
+	if (info->flags & MCPS802154_RX_MEASUREMENTS_RSSIS) {
+		int max = max(MCPS802154_RSSIS_N_MAX - 1, info->n_rssis);
+		int i;
+		for (i = 0; i < max; i++)
+			diagnostic->rssis_q1[i] = info->rssis_q1[i];
+		diagnostic->n_rssis = i;
+	}
+}
+
+static void
+fira_diagnostic_aoas(const struct mcps802154_rx_measurement_info *info,
+		     struct fira_diagnostic *diagnostic)
+{
+	int max = max(MCPS802154_RX_AOA_MEASUREMENTS_MAX - 1, info->n_aoas);
+	int i;
+
+	for (i = 0; i < max; i++)
+		diagnostic->aoas[i] = info->aoas[i];
+	diagnostic->n_aoas = info->n_aoas;
+}
+
+static struct mcps802154_rx_cir *
+fira_diagnostic_cirs_alloc(const struct mcps802154_rx_measurement_info *info)
+{
+	const struct mcps802154_rx_cir_sample_window *si;
+	struct mcps802154_rx_cir_sample_window *so;
+	struct mcps802154_rx_cir *cirs;
+	int i;
+	int j;
+
+	cirs = kmalloc(info->n_cirs * sizeof(struct mcps802154_rx_cir),
+		       GFP_KERNEL);
+	if (!cirs)
+		return NULL;
+
+	for (i = 0; i < info->n_cirs; i++) {
+		so = &cirs[i].sample_window;
+		si = &info->cirs[i].sample_window;
+		so->samples =
+			kmalloc(si->n_samples * si->sizeof_sample, GFP_KERNEL);
+
+		if (!so->samples)
+			goto failed;
+	}
+	return cirs;
+
+failed:
+	for (j = 0; j < i; j++)
+		kfree(cirs[j].sample_window.samples);
+	kfree(cirs);
+	return NULL;
+}
+
+static void
+fira_diagnostic_cirs_copy(const struct mcps802154_rx_measurement_info *info,
+			  struct fira_diagnostic *diagnostic)
+{
+	int i;
+
+	for (i = 0; i < info->n_cirs; i++) {
+		struct mcps802154_rx_cir *cir_in;
+		struct mcps802154_rx_cir *cir_out;
+		struct mcps802154_rx_cir_sample_window *si;
+		struct mcps802154_rx_cir_sample_window *so;
+
+		cir_out = &diagnostic->cirs[i];
+		cir_in = &info->cirs[i];
+		so = &cir_out->sample_window;
+		si = &cir_in->sample_window;
+
+		cir_out->fp_index = cir_in->fp_index;
+		cir_out->fp_snr = cir_in->fp_snr;
+		cir_out->fp_ns_q6 = cir_in->fp_ns_q6;
+		cir_out->pp_index = cir_in->pp_index;
+		cir_out->pp_snr = cir_in->pp_snr;
+		cir_out->pp_ns_q6 = cir_in->pp_ns_q6;
+		cir_out->fp_sample_offset = cir_in->fp_sample_offset;
+		so->n_samples = si->n_samples;
+		so->sizeof_sample = si->sizeof_sample;
+
+		memcpy(so->samples, si->samples,
+		       si->n_samples * si->sizeof_sample);
+	}
+	diagnostic->n_cirs = i;
+}
+
+static void
+fira_diagnostic_cirs(const struct mcps802154_rx_measurement_info *info,
+		     struct fira_diagnostic *diagnostic)
+{
+	if (info->flags & MCPS802154_RX_MEASUREMENTS_CIRS) {
+		diagnostic->cirs = fira_diagnostic_cirs_alloc(info);
+		if (diagnostic->cirs)
+			fira_diagnostic_cirs_copy(info, diagnostic);
+		else
+			diagnostic->n_cirs = 0;
+	}
+}
+
+static void fira_diagnostic(struct fira_local *local,
+			    struct fira_session *session, void *rx_ctx,
+			    int slot_idx)
+{
+	const struct fira_session_params *params = &session->params;
+	struct fira_diagnostic *diagnostic = &local->diagnostics[slot_idx];
+	struct mcps802154_rx_measurement_info info = {};
+	int r;
+
+	WARN_ON(diagnostic->cirs);
+
+	if (params->diagnostic_report_flags &
+	    FIRA_RANGING_DIAGNOSTICS_FRAME_REPORT_RSSIS)
+		info.flags |= MCPS802154_RX_MEASUREMENTS_RSSIS;
+	if (params->diagnostic_report_flags &
+	    FIRA_RANGING_DIAGNOSTICS_FRAME_REPORT_CIRS)
+		info.flags |= MCPS802154_RX_MEASUREMENTS_CIRS;
+
+	if (!info.flags)
+		return;
+
+	r = mcps802154_rx_get_measurement(local->llhw, rx_ctx, &info);
+
+	if (r)
+		return;
+
+	fira_diagnostic_rssis(&info, diagnostic);
+	fira_diagnostic_cirs(&info, diagnostic);
+}
+
+static void fira_diagnostic_free(struct fira_local *local)
+{
+	int i;
+
+	for (i = 0; i < local->access.n_frames; i++) {
+		struct fira_diagnostic *diagnostic = &local->diagnostics[i];
+		int j;
+
+		for (j = 0; j < diagnostic->n_cirs; j++)
+			kfree(diagnostic->cirs[j].sample_window.samples);
+
+		kfree(diagnostic->cirs);
+		diagnostic->cirs = NULL;
+		diagnostic->n_cirs = 0;
+	}
+}
+
 static void fira_rx_frame_ranging(struct fira_local *local,
 				  const struct fira_slot *slot,
 				  struct sk_buff *skb,
 				  const struct mcps802154_rx_frame_info *info)
 {
 	const struct fira_session *session = local->current_session;
+	const struct fira_session_params *params = &session->params;
 	struct fira_ranging_info *ranging_info =
 		&local->ranging_info[slot->ranging_index];
 	struct mcps802154_ie_get_context ie_get = {};
-
 	bool pdoa_info_present;
 
 	if (!fira_rx_sts_good(local, info)) {
@@ -253,10 +436,29 @@ static void fira_rx_frame_ranging(struct fira_local *local,
 		struct fira_local_aoa_info *local_aoa;
 		bool pdoa_fom_info_present =
 			info->flags & MCPS802154_RX_FRAME_INFO_RANGING_PDOA_FOM;
-		s16 local_pdoa_q11 = info->ranging_pdoa_rad_q11;
-		s16 local_aoa_q11 = info->ranging_aoa_rad_q11;
+		s16 local_pdoa_q11 = 0;
+		s16 local_aoa_q11 = 0;
 		const struct fira_measurement_sequence_step *current_step =
-			fira_session_get_current_meas_seq_step(session);
+			fira_session_get_meas_seq_step(session);
+		struct mcps802154_rx_measurement_info meas_info = {};
+		int r;
+
+		meas_info.flags |= MCPS802154_RX_MEASUREMENTS_AOAS;
+		r = mcps802154_rx_get_measurement(
+			local->llhw, ranging_info->rx_ctx, &meas_info);
+
+		if (!r && meas_info.flags & MCPS802154_RX_MEASUREMENTS_AOAS &&
+		    meas_info.n_aoas) {
+			struct fira_diagnostic *diagnostic =
+				&local->diagnostics[slot->index];
+
+			/* TODO: Find which aoas index to use. */
+			local_pdoa_q11 = meas_info.aoas[0].pdoa_rad_q11;
+			local_aoa_q11 = meas_info.aoas[0].aoa_rad_q11;
+			if (params->diagnostic_report_flags &
+			    FIRA_RANGING_DIAGNOSTICS_FRAME_REPORT_AOAS)
+				fira_diagnostic_aoas(&meas_info, diagnostic);
+		}
 
 		switch (current_step->type) {
 		case FIRA_MEASUREMENT_TYPE_AOA:
@@ -287,7 +489,7 @@ static void fira_rx_frame_ranging(struct fira_local *local,
 			local_aoa->pdoa_2pi = map_q11_to_2pi(local_pdoa_q11);
 			local_aoa->aoa_2pi = map_q11_to_2pi(local_aoa_q11);
 			/* LCOV_EXCL_START */
-			/* FoM is always expected when PDoA present */
+			/* FoM is always expected when PDoA present. */
 			if (pdoa_fom_info_present)
 				/* LCOV_EXCL_STOP */
 				local_aoa->aoa_fom = info->ranging_pdoa_fom;
@@ -302,8 +504,8 @@ static void fira_rx_frame_ranging(struct fira_local *local,
 	}
 
 	if (skb) {
-		if (fira_frame_header_check_decrypt(local, slot, skb, &ie_get,
-						    NULL, NULL) ||
+		if (fira_frame_header_check_decrypt(local, slot, skb,
+						    &ie_get) ||
 		    !fira_frame_rframe_payload_check(local, slot, skb,
 						     &ie_get)) {
 			fira_ranging_info_set_status(
@@ -319,75 +521,107 @@ static void fira_rx_frame_control(struct fira_local *local,
 				  struct sk_buff *skb,
 				  const struct mcps802154_rx_frame_info *info)
 {
-	struct fira_ranging_info *ranging_info =
+	struct mcps802154_access *access = &local->access;
+	struct fira_ranging_info *ri =
 		&local->ranging_info[slot->ranging_index];
-	struct fira_session *session = local->current_session;
-	struct fira_session *allow_resync_session = NULL;
 	struct mcps802154_ie_get_context ie_get = {};
-	u32 sts_index;
-	unsigned int n_slots;
-	int last_slot_index, block_stride_len;
-	int i;
-	bool stop_ranging;
+	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 phy_sts_index;
+	u8 *header;
+	int r;
 
 	if (!(info->flags & MCPS802154_RX_FRAME_INFO_TIMESTAMP_DTU)) {
 		fira_ranging_info_set_status(
-			ranging_info, FIRA_STATUS_RANGING_RX_PHY_DEC_FAILED,
-			slot->index);
+			ri, FIRA_STATUS_RANGING_RX_PHY_DEC_FAILED, slot->index);
 		return;
 	}
-	if (fira_frame_header_check_decrypt(
-		    local, slot, skb, &ie_get, &sts_index,
-		    session->synchronised ? NULL : &allow_resync_session))
-		goto failed;
-	if (allow_resync_session) {
-		session = local->current_session = allow_resync_session;
-		fira_session_prepare(session);
-		fira_access_controlee(local, session);
-	}
 
-	if (!fira_frame_control_payload_check(local, skb, &ie_get, &n_slots,
-					      &stop_ranging, &block_stride_len))
+	offset_in_access_duration_dtu =
+		info->timestamp_dtu - access->timestamp_dtu;
+
+	/* Read the header to capture the session context. */
+	header = skb->data;
+	session = fira_rx_frame_control_header_check(local, slot, skb, &ie_get,
+						     &phy_sts_index);
+	if (!session)
 		goto failed;
 
-	fira_session_resync(session, sts_index, info->timestamp_dtu);
+	fira_controlee_resync(session, phy_sts_index, info->timestamp_dtu);
 
-	if (stop_ranging) {
-		session->stop_inband = true;
-		return;
-	}
+	params = &session->params;
+	ri->rx_ctx = session->rx_ctx[0];
 
-	last_slot_index = 0;
-	for (i = 1; i < n_slots; i++) {
-		const struct fira_slot *slot = &local->slots[i];
-		struct mcps802154_access_frame *frame = &local->frames[i];
-		struct mcps802154_sts_params *sts_params =
-			&local->sts_params[i];
-		bool is_tx;
-		u32 frame_dtu;
-
-		is_tx = slot->tx_controlee_index != -1;
-		frame_dtu = info->timestamp_dtu +
-			    session->params.slot_duration_dtu * slot->index;
-		last_slot_index = slot->index;
-
-		fira_access_setup_frame(local, session, frame, sts_params, slot,
-					frame_dtu, is_tx);
-	}
-	local->access.timestamp_dtu = info->timestamp_dtu;
-	local->access.duration_dtu =
-		session->params.slot_duration_dtu * (last_slot_index + 1);
-	local->access.n_frames = n_slots;
+	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_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,
+					     &session->stop_inband,
+					     &session->block_stride_len);
+	if (!r)
+		goto failed;
+
+	left_duration_dtu =
+		access->duration_dtu - offset_in_access_duration_dtu;
 
-	session->next_block_stride_len = block_stride_len;
+	/*
+	 * The RCM has been received, remaining slots are: n_slots - 1.
+	 * Stop if no time left to finish the ranging or if asked to.
+	 */
+	if (left_duration_dtu < (n_slots - 1) * params->slot_duration_dtu ||
+	    session->stop_inband) {
+		n_slots = 1;
+	} else {
+		int i;
+
+		for (i = 1; i < n_slots; i++) {
+			const struct fira_slot *slot = &local->slots[i];
+			struct mcps802154_access_frame *frame =
+				&local->frames[i];
+			struct mcps802154_sts_params *sts_params =
+				&local->sts_params[i];
+			bool is_tx;
+			u32 frame_dtu;
+
+			is_tx = !slot->controller_tx;
+			frame_dtu = info->timestamp_dtu +
+				    params->slot_duration_dtu * slot->index;
+			last_slot_index = slot->index;
+
+			fira_access_setup_frame(local, session, frame,
+						sts_params, slot, frame_dtu,
+						is_tx);
+		}
+	}
 
+	/* Trace the new (or not) session context and slots received. */
+	trace_region_fira_rx_frame_control(local, session, left_duration_dtu,
+					   n_slots);
+	/* Update the access. */
+	access->duration_dtu =
+		offset_in_access_duration_dtu +
+		(last_slot_index + 1) * params->slot_duration_dtu;
+	access->n_frames = n_slots;
 	return;
+
 failed:
-	fira_ranging_info_set_status(ranging_info,
-				     FIRA_STATUS_RANGING_RX_MAC_IE_DEC_FAILED,
-				     slot->index);
-	local->access.timestamp_dtu = info->timestamp_dtu;
-	local->access.duration_dtu = session->params.slot_duration_dtu;
+	params = &local->current_session->params;
+	access->duration_dtu =
+		offset_in_access_duration_dtu + params->slot_duration_dtu;
+	fira_ranging_info_set_status(
+		ri, FIRA_STATUS_RANGING_RX_MAC_IE_DEC_FAILED, slot->index);
 }
 
 static void fira_rx_frame_measurement_report(
@@ -398,8 +632,7 @@ static void fira_rx_frame_measurement_report(
 		&local->ranging_info[slot->ranging_index];
 	struct mcps802154_ie_get_context ie_get = {};
 
-	if (fira_frame_header_check_decrypt(local, slot, skb, &ie_get, NULL,
-					    NULL))
+	if (fira_frame_header_check_decrypt(local, slot, skb, &ie_get))
 		goto failed;
 
 	if (!fira_frame_measurement_report_payload_check(local, slot, skb,
@@ -422,8 +655,7 @@ fira_rx_frame_result_report(struct fira_local *local,
 		&local->ranging_info[slot->ranging_index];
 	struct mcps802154_ie_get_context ie_get = {};
 
-	if (fira_frame_header_check_decrypt(local, slot, skb, &ie_get, NULL,
-					    NULL))
+	if (fira_frame_header_check_decrypt(local, slot, skb, &ie_get))
 		goto failed;
 
 	if (!fira_frame_result_report_payload_check(local, slot, skb, &ie_get))
@@ -456,6 +688,7 @@ static bool fira_do_process_rx_frame(enum mcps802154_rx_error_type error,
 		break;
 	case MCPS802154_RX_ERROR_UNCORRECTABLE:
 	case MCPS802154_RX_ERROR_OTHER:
+	case MCPS802154_RX_ERROR_PHR_DECODE:
 		status = FIRA_STATUS_RANGING_RX_PHY_DEC_FAILED;
 		break;
 	}
@@ -469,12 +702,25 @@ static void fira_rx_frame(struct mcps802154_access *access, int frame_idx,
 			  enum mcps802154_rx_error_type error)
 {
 	struct fira_local *local = access_to_local(access);
+	const struct fira_session_params *params;
 	const struct fira_slot *slot = &local->slots[frame_idx];
-	struct fira_session *session = local->current_session;
-	struct fira_ranging_info *ranging_info =
+	struct fira_ranging_info *ri =
 		&local->ranging_info[slot->ranging_index];
+	/* Don't initialize session before rx_frame_control. */
+	struct fira_session *session;
+
+	trace_region_fira_rx_frame(local->current_session, slot->message_id,
+				   error);
+
+	if (info && info->flags & MCPS802154_RX_FRAME_INFO_RSSI) {
+		if ((ri->n_rx_rssis + 1) > FIRA_MESSAGE_ID_MAX)
+			return;
 
-	if (fira_do_process_rx_frame(error, ranging_info, slot->index)) {
+		ri->rx_rssis[ri->n_rx_rssis++] =
+			info->rssi < FIRA_RSSI_MAX ? info->rssi : FIRA_RSSI_MAX;
+	}
+
+	if (fira_do_process_rx_frame(error, ri, slot->index)) {
 		switch (slot->message_id) {
 		case FIRA_MESSAGE_ID_RANGING_INITIATION:
 		case FIRA_MESSAGE_ID_RANGING_RESPONSE:
@@ -497,25 +743,28 @@ static void fira_rx_frame(struct mcps802154_access *access, int frame_idx,
 			WARN_UNREACHABLE_DEFAULT();
 		}
 	}
+	/* Current session can change after call of rx_frame_control function. */
+	session = local->current_session;
+	session->last_access_timestamp_dtu = access->timestamp_dtu;
+	params = &session->params;
 
-	if (skb)
-		kfree_skb(skb);
-
-	trace_region_fira_rx_message(
-		session, slot->message_id,
-		local->ranging_info[slot->ranging_index].status);
-
-	/* Controlee: Stop round on error.
-	   Controller: Stop when all ranging fails. */
-	if (local->ranging_info[slot->ranging_index].status)
-		if (session->params.device_type == FIRA_DEVICE_TYPE_CONTROLEE ||
+	kfree_skb(skb);
+	fira_diagnostic(local, session, ri->rx_ctx, slot->index);
+
+	/*
+	 * Controlee: Stop round on error.
+	 * Controller: Stop when all ranging fails.
+	 */
+	/*
+	 * TODO:
+	 * The usage of ri->status is hidden in function called.
+	 * The reason of the end of access is not limpid.
+	 */
+	if (ri->status != FIRA_STATUS_RANGING_SUCCESS) {
+		if (params->device_type == FIRA_DEVICE_TYPE_CONTROLEE ||
 		    (slot->message_id <= FIRA_MESSAGE_ID_RFRAME_MAX &&
 		     --local->n_ranging_valid == 0))
 			access->n_frames = frame_idx + 1;
-
-	if (session->params.device_type == FIRA_DEVICE_TYPE_CONTROLEE &&
-	    session->stop_inband) {
-		access->n_frames = frame_idx + 1;
 	}
 }
 
@@ -523,17 +772,20 @@ static struct sk_buff *fira_tx_get_frame(struct mcps802154_access *access,
 					 int frame_idx)
 {
 	struct fira_local *local = access_to_local(access);
+	struct fira_session *session = local->current_session;
+	const struct fira_session_params *params = &session->params;
 	const struct fira_slot *slot = &local->slots[frame_idx];
 	struct sk_buff *skb;
-	int rframe = local->current_session->params.rframe_config;
+	int header_len;
 
-	trace_region_fira_tx_message(local->current_session, slot->message_id);
-	if (rframe == FIRA_RFRAME_CONFIG_SP3 &&
+	trace_region_fira_tx_get_frame(session, slot->message_id);
+	if (params->rframe_config == FIRA_RFRAME_CONFIG_SP3 &&
 	    slot->message_id <= FIRA_MESSAGE_ID_RFRAME_MAX)
 		return NULL;
 
 	skb = mcps802154_frame_alloc(local->llhw, IEEE802154_MTU, GFP_KERNEL);
-	WARN_RETURN_ON(!skb, NULL);
+	if (!skb)
+		return NULL;
 
 	fira_frame_header_put(local, slot, skb);
 
@@ -562,7 +814,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;
 	}
@@ -575,11 +831,13 @@ static void fira_tx_return(struct mcps802154_access *access, int frame_idx,
 			   enum mcps802154_access_tx_return_reason reason)
 {
 	struct fira_local *local = access_to_local(access);
+	struct fira_session *session = local->current_session;
 	int i;
 
 	kfree_skb(skb);
 
 	/* Error on TX. */
+	trace_region_fira_tx_return(session, reason);
 	if (reason == MCPS802154_ACCESS_TX_RETURN_REASON_CANCEL) {
 		for (i = 0; i < local->n_ranging_info; i++) {
 			local->ranging_info[i].status =
@@ -592,324 +850,330 @@ static void fira_access_done(struct mcps802154_access *access, bool error)
 {
 	struct fira_local *local = access_to_local(access);
 	struct fira_session *session = local->current_session;
-	int i;
-
-	if (error)
-		for (i = 0; i < local->n_ranging_info; i++)
-			local->ranging_info[i].status =
-				FIRA_STATUS_RANGING_INTERNAL_ERROR;
-	fira_session_access_done(local, session, true);
-
-	local->current_session = NULL;
+	u32 timestamp_dtu = access->timestamp_dtu;
+
+	trace_region_fira_access_done(local, session, access->duration_dtu,
+				      error);
+
+	/* propagate llhw error to fira session */
+	session->last_error = access->error;
+	fira_session_fsm_access_done(local, session, error);
+	fira_diagnostic_free(local);
+	if (!error)
+		/* No access are infinite normally. */
+		timestamp_dtu += access->duration_dtu;
+	/*
+	 * Must be call after FSM access done, because
+	 * shared resource in local are used.
+	 */
+	fira_check_all_missed_ranging(local, session, timestamp_dtu);
 }
 
-struct mcps802154_access_ops fira_access_ops = {
-	.common = {
-		.access_done = fira_access_done,
-	},
-	.rx_frame = fira_rx_frame,
-	.tx_get_frame = fira_tx_get_frame,
-	.tx_return = fira_tx_return,
-};
-
 static __le16 fira_access_set_short_address(struct fira_local *local,
-					    struct fira_session *session,
+					    const struct fira_session *session,
 					    struct mcps802154_access *access)
 {
+	const struct fira_session_params *params = &session->params;
 	__le16 src_short_addr = mcps802154_get_short_addr(local->llhw);
 
-	if (session->params.short_addr != IEEE802154_ADDR_SHORT_BROADCAST &&
-	    src_short_addr != session->params.short_addr) {
+	if (params->short_addr != IEEE802154_ADDR_SHORT_BROADCAST &&
+	    src_short_addr != params->short_addr) {
 		access->hw_addr_filt = (struct ieee802154_hw_addr_filt){
-			.short_addr = session->params.short_addr,
+			.short_addr = params->short_addr,
 		};
 		access->hw_addr_filt_changed = IEEE802154_AFILT_SADDR_CHANGED;
-		return session->params.short_addr;
-	} else {
-		access->hw_addr_filt_changed = 0;
-		return src_short_addr;
+		return params->short_addr;
 	}
+	access->hw_addr_filt_changed = 0;
+	return src_short_addr;
 }
 
-static const struct mcps802154_channel *
-fira_access_channel(struct fira_local *local,
-		    const struct fira_session *session)
+static struct mcps802154_access_ops fira_controller_access_ops = {
+	.common = {
+		.access_done = fira_access_done,
+	},
+	.rx_frame = fira_rx_frame,
+	.tx_get_frame = fira_tx_get_frame,
+	.tx_return = fira_tx_return,
+};
+
+int fira_session_get_slot_count(const struct fira_session *session)
 {
-	if (session->params.channel_number ||
-	    session->params.preamble_code_index) {
-		const struct mcps802154_channel *channel =
-			mcps802154_get_current_channel(local->llhw);
-
-		local->channel = *channel;
-		if (session->params.channel_number)
-			local->channel.channel = session->params.channel_number;
-		if (session->params.preamble_code_index)
-			local->channel.preamble_code =
-				session->params.preamble_code_index;
-		return &local->channel;
+	const struct fira_session_params *params = &session->params;
+	int nb_controlee = fira_session_controlees_running_count(session);
+	/* Control frame. */
+	int slot_count = 1;
+
+	if (nb_controlee) {
+		/* Ranging initiation frame. */
+		slot_count++;
+		/* Ranging response frame(s). */
+		slot_count += nb_controlee;
+		/* Ranging final frame. */
+		if (params->ranging_round_usage ==
+		    FIRA_RANGING_ROUND_USAGE_DSTWR)
+			slot_count++;
+		/* Measurement report frame. */
+		slot_count++;
+		/* Result report frame(s). */
+		slot_count += nb_controlee;
 	}
-
-	return NULL;
+	return slot_count;
 }
 
-static struct mcps802154_access *
-fira_access_controller(struct fira_local *local, struct fira_session *session)
+struct mcps802154_access *
+fira_get_access_controller(struct fira_local *local,
+			   const struct fira_session_demand *fsd)
 {
-	struct mcps802154_access *access;
+	struct fira_session *session = local->current_session;
+	const struct fira_session_params *params = &session->params;
+	const struct fira_measurement_sequence_step *step =
+		fira_session_get_meas_seq_step(session);
+	struct mcps802154_access *access = &local->access;
 	struct mcps802154_access_frame *frame;
 	struct mcps802154_sts_params *sts_params;
-	struct fira_slot *s;
 	struct fira_ranging_info *ri;
-	struct fira_controlees_array *controlees_array =
-		&session->current_controlees;
-	const struct fira_measurement_sequence_data *meas_seq =
-		&session->params.meas_seq;
-	const struct fira_measurement_sequence_step *current_ms_step =
-		&meas_seq->active->steps[meas_seq->current_step];
-	int i, j;
+	struct fira_slot *s;
 	u32 frame_dtu;
 	int index = 0;
-	bool double_sided = (session->params.ranging_round_usage ==
-			     FIRA_RANGING_ROUND_USAGE_DSTWR);
+	int i;
+	struct fira_controlee *controlee;
+
+	trace_region_fira_get_access_controller(local, session, fsd);
 
-	access = &local->access;
+	/* Update local context (shared memory used by all sessions). */
 	local->src_short_addr =
 		fira_access_set_short_address(local, session, access);
-	local->dst_short_addr = (controlees_array->size == 1) ?
-					controlees_array->data[0].short_addr :
-					IEEE802154_ADDR_SHORT_BROADCAST;
-
-	access->method = MCPS802154_ACCESS_METHOD_MULTI;
-	access->ops = &fira_access_ops;
-	access->timestamp_dtu = session->block_start_dtu +
-				fira_session_get_round_slot(session) *
-					session->params.slot_duration_dtu;
-	access->frames = local->frames;
-	access->channel = fira_access_channel(local, session);
-
-	local->n_stopped_controlees_short_addr = 0;
-	for (i = 0; i < controlees_array->size; i++) {
-		const struct fira_controlee *c = &controlees_array->data[i];
-
-		if (c->state != FIRA_CONTROLEE_STATE_RUNNING) {
-			local->stopped_controlees_short_addr
-				[local->n_stopped_controlees_short_addr] =
-				c->short_addr;
-			local->n_stopped_controlees_short_addr++;
-		}
+	local->dst_short_addr =
+		session->n_current_controlees == 1 ?
+			list_first_entry(&session->current_controlees,
+					 struct fira_controlee, entry)
+				->short_addr :
+			IEEE802154_ADDR_SHORT_BROADCAST;
+
+	/* Update session. */
+	session->last_access_timestamp_dtu = fsd->timestamp_dtu;
+	session->block_start_dtu = fsd->block_start_dtu;
+	session->block_index += fsd->add_blocks;
+	session->block_stride_len = params->block_stride_len;
+	session->round_index = fsd->round_index;
+	session->controller.next_block_index =
+		session->block_index + session->block_stride_len + 1;
+	session->next_round_index =
+		params->round_hopping ?
+			fira_round_hopping_sequence_get(
+				session, session->controller.next_block_index) :
+			0;
+
+	/* Build number of controlee which are stopped. */
+	local->n_stopped_controlees = 0;
+	list_for_each_entry (controlee, &session->current_controlees, entry) {
+		if (controlee->state == FIRA_CONTROLEE_STATE_STOPPING ||
+		    controlee->state == FIRA_CONTROLEE_STATE_DELETING)
+			local->stopped_controlees[local->n_stopped_controlees++] =
+				controlee->short_addr;
 	}
+	/* Build number of controlee which are running. */
+	local->n_ranging_valid =
+		session->n_current_controlees - local->n_stopped_controlees;
 
-	local->n_ranging_valid = local->n_ranging_info =
-		controlees_array->size - local->n_stopped_controlees_short_addr;
+	/* Reset 'n' ranging info to store info related to controlees. */
+	local->n_ranging_info = local->n_ranging_valid;
 	ri = local->ranging_info;
+	memset(ri, 0,
+	       local->n_ranging_valid * sizeof(struct fira_ranging_info));
 
-	memset(ri, 0, local->n_ranging_info * sizeof(*ri));
-
+	/* Prepare control message slot for fira_rx_frame. */
 	s = local->slots;
-
 	s->index = index++;
-	s->tx_controlee_index = -1;
+	s->controller_tx = true;
 	s->ranging_index = 0;
-	s->tx_ant_set = current_ms_step->tx_ant_set_nonranging;
+	s->tx_ant_set = step->tx_ant_set_nonranging;
 	s->message_id = FIRA_MESSAGE_ID_CONTROL;
+	s->controlee = NULL;
 	s++;
-
+	/* Prepare other slots. */
 	if (local->n_ranging_info) {
 		s->index = index++;
-		s->tx_controlee_index = -1;
+		s->controller_tx = true;
 		s->ranging_index = 0;
-		s->tx_ant_set = current_ms_step->tx_ant_set_ranging;
+		s->tx_ant_set = step->tx_ant_set_ranging;
 		s->message_id = FIRA_MESSAGE_ID_RANGING_INITIATION;
+		s->controlee = NULL;
 
 		s++;
-		for (i = 0, j = 0; i < controlees_array->size; i++) {
-			if (controlees_array->data[i].state !=
-			    FIRA_CONTROLEE_STATE_RUNNING)
+		i = 0;
+		list_for_each_entry (controlee, &session->current_controlees,
+				     entry) {
+			if (!fira_session_controlee_active(controlee))
 				continue;
-			ri->short_addr = controlees_array->data[i].short_addr;
+			ri->short_addr = controlee->short_addr;
+			ri->rx_ctx = session->rx_ctx[i];
 			/* Requested in fira_report_aoa function. */
 			ri++;
 			s->index = index++;
-			s->tx_controlee_index = i;
-			s->ranging_index = j++;
+			s->controller_tx = false;
+			s->ranging_index = i++;
 			s->rx_ant_set = fira_session_get_rx_ant_set(
 				session, FIRA_MESSAGE_ID_RANGING_RESPONSE);
 			s->message_id = FIRA_MESSAGE_ID_RANGING_RESPONSE;
+			s->controlee = controlee;
 			s++;
 		}
 
-		if (double_sided) {
+		if (params->ranging_round_usage ==
+		    FIRA_RANGING_ROUND_USAGE_DSTWR) {
 			s->index = index++;
-			s->tx_controlee_index = -1;
+			s->controller_tx = true;
 			s->ranging_index = 0;
-			s->tx_ant_set = current_ms_step->tx_ant_set_ranging;
+			s->tx_ant_set = step->tx_ant_set_ranging;
 			s->message_id = FIRA_MESSAGE_ID_RANGING_FINAL;
+			s->controlee = NULL;
 			s++;
 		}
 
 		s->index = index++;
-		s->tx_controlee_index = -1;
+		s->controller_tx = true;
 		s->ranging_index = 0;
-		s->tx_ant_set = current_ms_step->tx_ant_set_nonranging;
+		s->tx_ant_set = step->tx_ant_set_nonranging;
 		s->message_id = FIRA_MESSAGE_ID_MEASUREMENT_REPORT;
+		s->controlee = NULL;
 
 		s++;
-		if (session->params.result_report_phase) {
-			for (i = 0, j = 0; i < controlees_array->size; i++) {
-				if (controlees_array->data[i].state !=
-				    FIRA_CONTROLEE_STATE_RUNNING)
+		if (params->result_report_phase) {
+			i = 0;
+			list_for_each_entry (controlee,
+					     &session->current_controlees,
+					     entry) {
+				if (!fira_session_controlee_active(controlee))
 					continue;
 				s->index = index++;
-				s->tx_controlee_index = i;
-				s->ranging_index = j++;
-				s->rx_ant_set =
-					current_ms_step->rx_ant_set_nonranging;
+				s->controller_tx = false;
+				s->ranging_index = i++;
+				s->rx_ant_set = step->rx_ant_set_nonranging;
 				s->message_id = FIRA_MESSAGE_ID_RESULT_REPORT;
+				s->controlee = controlee;
 				s++;
 			}
 		}
 	}
 
-	access->n_frames = index;
-
-	frame_dtu = access->timestamp_dtu;
-
-	for (i = 0; i < access->n_frames; i++) {
-		bool is_tx;
-
+	/* Configure frames for fproc. */
+	frame_dtu = fsd->timestamp_dtu;
+	for (i = 0; i < index; i++) {
 		s = &local->slots[i];
 		frame = &local->frames[i];
 		sts_params = &local->sts_params[i];
 
-		is_tx = s->tx_controlee_index == -1;
-
 		fira_access_setup_frame(local, session, frame, sts_params, s,
-					frame_dtu, is_tx);
+					frame_dtu, s->controller_tx);
 
-		frame_dtu += session->params.slot_duration_dtu;
+		frame_dtu += params->slot_duration_dtu;
 	}
-	access->duration_dtu = frame_dtu - access->timestamp_dtu;
 
+	/*
+	 * Configure the access.
+	 * 'duration_dtu' can be decrease on reception error.
+	 */
+	access->ops = &fira_controller_access_ops;
+	access->timestamp_dtu = fsd->timestamp_dtu;
+	access->duration_dtu = frame_dtu - fsd->timestamp_dtu;
+	access->n_frames = index;
 	return access;
 }
 
-static struct mcps802154_access *
-fira_access_controlee(struct fira_local *local, struct fira_session *session)
+static struct mcps802154_access_ops fira_controlee_access_ops = {
+	.common = {
+		.access_done = fira_access_done,
+	},
+	.rx_frame = fira_rx_frame,
+	.tx_get_frame = fira_tx_get_frame,
+	.tx_return = fira_tx_return,
+};
+
+struct mcps802154_access *
+fira_get_access_controlee(struct fira_local *local,
+			  const struct fira_session_demand *fsd)
 {
-	struct mcps802154_access *access;
+	/*
+	 * \                    Important:
+	 *  '-.__.-'            It's almost forbidden to update session
+	 *  /oo |--.--,--,--.   content for a controlee. Because, the
+	 *  \_.-'._i__i__i_.'   session can change on control frame received.
+	 *        """""""""
+	 */
+	struct fira_session *session = local->current_session;
+	const struct fira_session_params *params = &session->params;
+	struct mcps802154_access *access = &local->access;
 	struct mcps802154_access_frame *frame;
-	struct fira_slot *s;
 	struct fira_ranging_info *ri;
-	int timeout_dtu;
+	struct fira_slot *s;
+	u16 request = MCPS802154_RX_FRAME_INFO_TIMESTAMP_DTU;
+
+	trace_region_fira_get_access_controlee(local, session, fsd);
 
-	access = &local->access;
+	/* Update local context (shared memory used by all sessions). */
 	local->src_short_addr =
 		fira_access_set_short_address(local, session, access);
-	local->dst_short_addr = session->params.controller_short_addr;
-
-	access->method = MCPS802154_ACCESS_METHOD_MULTI;
-	access->ops = &fira_access_ops;
-	access->timestamp_dtu = session->last_access_timestamp_dtu;
-	access->duration_dtu = session->last_access_duration_dtu;
-	access->frames = local->frames;
-	access->n_frames = 1;
-	access->channel = fira_access_channel(local, session);
-
-	ri = local->ranging_info;
-	memset(ri, 0, sizeof(*ri));
-	ri->short_addr = session->params.controller_short_addr;
+	local->dst_short_addr = params->controller_short_addr;
+	local->n_stopped_controlees = 0;
+
+	/*
+	 * Update session.
+	 * Updated values are used in case of bad reception (timeout/error).
+	 * Otherwise on good reception, many are override.
+	 * by the controlee resync function.
+	 */
+	session->block_start_dtu = fsd->block_start_dtu;
+	session->block_index += fsd->add_blocks;
+
+	/* Reset one ranging info to store info related to the controller. */
 	local->n_ranging_info = 1;
-	local->n_stopped_controlees_short_addr = 0;
-
+	ri = local->ranging_info;
+	memset(ri, 0, sizeof(struct fira_ranging_info));
+	/*
+	 * Warning:
+	 * - short_addr is used when rx control have an error.
+	 * - Be careful to not initialize too much in ri, because
+	 *   session can change on rx control.
+	 */
+	ri->short_addr = params->controller_short_addr;
+
+	/* Prepare control message slot for fira_rx_frame. */
 	s = local->slots;
 	s->index = 0;
-	s->tx_controlee_index = -1;
+	s->controller_tx = true;
 	s->ranging_index = 0;
-	s->rx_ant_set = fira_session_get_current_meas_seq_step(session)
-				->rx_ant_set_nonranging;
+	s->rx_ant_set =
+		fira_session_get_meas_seq_step(session)->rx_ant_set_nonranging;
 	s->message_id = FIRA_MESSAGE_ID_CONTROL;
+	s->controlee = NULL;
 
-	if (session->synchronised)
-		timeout_dtu = 2 * fira_session_get_block_duration_margin(
-					  local, session);
-	else if (!access->duration_dtu)
-		timeout_dtu = -1;
-	else
-		timeout_dtu = access->duration_dtu -
-			      session->params.round_duration_slots *
-				      session->params.slot_duration_dtu;
-
+	/* Configure frames for fproc. */
+	if (params->report_rssi)
+		request |= MCPS802154_RX_FRAME_INFO_RSSI;
 	frame = local->frames;
 	*frame = (struct mcps802154_access_frame){
 		.is_tx = false,
 		.rx = {
-			.info = {
-				.timestamp_dtu = access->timestamp_dtu,
-				.timeout_dtu = timeout_dtu,
-				.flags = MCPS802154_RX_INFO_TIMESTAMP_DTU |
-					MCPS802154_RX_INFO_RANGING_ROUND,
+			.frame_config = {
+				.timestamp_dtu = fsd->timestamp_dtu,
+				.timeout_dtu = fsd->rx_timeout_dtu,
+				.flags = MCPS802154_RX_FRAME_CONFIG_RANGING_ROUND |
+					MCPS802154_RX_FRAME_CONFIG_TIMESTAMP_DTU,
 				.ant_set_id = s->rx_ant_set,
 			},
-			.frame_info_flags_request
-				= MCPS802154_RX_FRAME_INFO_TIMESTAMP_DTU,
+			.frame_info_flags_request = request,
 		},
 	};
 
+	/*
+	 * Configure the access.
+	 * 'duration_dtu' will be overridden on control frame reception.
+	 */
+	access->ops = &fira_controlee_access_ops;
+	access->timestamp_dtu = fsd->timestamp_dtu;
+	access->duration_dtu = fsd->max_duration_dtu;
+	access->n_frames = 1;
 	return access;
 }
-
-struct mcps802154_access *fira_get_access(struct mcps802154_region *region,
-					  u32 next_timestamp_dtu,
-					  int next_in_region_dtu,
-					  int region_duration_dtu)
-{
-	struct fira_local *local = region_to_local(region);
-	struct fira_session *session;
-
-	if (!local->current_session) {
-		session = fira_session_next(
-			local, next_timestamp_dtu + local->llhw->anticip_dtu,
-			region_duration_dtu - next_in_region_dtu);
-		local->current_session = session;
-	} else {
-		session = local->current_session;
-	}
-
-	if (!session)
-		return NULL;
-	return fira_compute_access(local, session);
-}
-
-struct mcps802154_access *fira_compute_access(struct fira_local *local,
-					      struct fira_session *session)
-{
-	fira_session_prepare(session);
-	if (session->params.device_type == FIRA_DEVICE_TYPE_CONTROLLER)
-		return fira_access_controller(local, session);
-	else
-		return fira_access_controlee(local, session);
-}
-
-void fira_session_get_demand(struct fira_local *local,
-			     struct fira_session *session,
-			     struct mcps802154_region_demand *demand)
-{
-	u32 base_timestamp_dtu = session->block_start_dtu +
-				 fira_session_get_round_slot(session) *
-					 session->params.slot_duration_dtu;
-	if (session->params.device_type == FIRA_DEVICE_TYPE_CONTROLLER) {
-		demand->timestamp_dtu = base_timestamp_dtu;
-		demand->max_duration_dtu =
-			(4 + 2 * session->current_controlees.size) *
-			session->params.slot_duration_dtu;
-	} else {
-		int block_duration_margin_dtu =
-			fira_session_get_block_duration_margin(local, session);
-		demand->timestamp_dtu =
-			base_timestamp_dtu - block_duration_margin_dtu;
-		demand->max_duration_dtu =
-			session->params.round_duration_slots *
-				session->params.slot_duration_dtu +
-			block_duration_margin_dtu;
-	}
-}
diff --git a/mac/fira_access.h b/mac/fira_access.h
index 43812ba..3b6ca8c 100644
--- a/mac/fira_access.h
+++ b/mac/fira_access.h
@@ -1,7 +1,7 @@
 /*
  * This file is part of the UWB stack for linux.
  *
- * Copyright (c) 2020-2021 Qorvo US, Inc.
+ * 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.
@@ -26,41 +26,39 @@
 
 #include <net/mcps802154_schedule.h>
 
+/* Forward declaration. */
 struct fira_local;
 struct fira_session;
+struct fira_session_demand;
 
 /**
- * fira_get_access() - Get access for a given region at the given timestamp.
- * @region: Region.
- * @next_timestamp_dtu: Next access opportunity.
- * @next_in_region_dtu: Unused.
- * @region_duration_dtu: Unused.
+ * fira_session_get_slot_count() - Return the number of slot for the session.
+ * @session: FiRa session context.
  *
- * Return: The access.
+ * Return: Number of slot.
  */
-struct mcps802154_access *fira_get_access(struct mcps802154_region *region,
-					  u32 next_timestamp_dtu,
-					  int next_in_region_dtu,
-					  int region_duration_dtu);
+int fira_session_get_slot_count(const struct fira_session *session);
 
 /**
- * fira_compute_access() - Get access for a given session.
- * @local: FiRa context.
- * @session: Session.
+ * fira_get_access_controller() - Build the access for controller.
+ * @local: FiRa region context.
+ * @fsd: FiRa Session Demand from the get_demand of the session fsm.
  *
- * Return: The access.
+ * Return: A valid access.
  */
-struct mcps802154_access *fira_compute_access(struct fira_local *local,
-					      struct fira_session *session);
+struct mcps802154_access *
+fira_get_access_controller(struct fira_local *local,
+			   const struct fira_session_demand *fsd);
 
 /**
- * fira_session_get_demand() - Get access information for a given session.
- * @local: FiRa context.
- * @session: Session.
- * @demand: Access information.
+ * fira_get_access_controlee() - Build the access for controlee.
+ * @local: FiRa region context.
+ * @fsd: FiRa Session Demand from the get_demand of the session fsm.
+ *
+ * Return: A valid access.
  */
-void fira_session_get_demand(struct fira_local *local,
-			     struct fira_session *session,
-			     struct mcps802154_region_demand *demand);
+struct mcps802154_access *
+fira_get_access_controlee(struct fira_local *local,
+			  const struct fira_session_demand *fsd);
 
 #endif /* FIRA_ACCESS_H */
diff --git a/mac/fira_aead.h b/mac/fira_aead.h
deleted file mode 100644
index 11baf70..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-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_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 464eb72..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-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_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 56f8299..61b6a67 100644..100755
--- a/mac/fira_crypto.c
+++ b/mac/fira_crypto.c
@@ -1,15 +1,15 @@
 /*
  * This file is part of the UWB stack for linux.
  *
- * Copyright (c) 2020-2021 Qorvo US, Inc.
+ * 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/>.
+ * 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 88b4609..8fd2f8d 100644
--- a/mac/fira_crypto.h
+++ b/mac/fira_crypto.h
@@ -1,7 +1,7 @@
 /*
  * This file is part of the UWB stack for linux.
  *
- * Copyright (c) 2020-2021 Qorvo US, Inc.
+ * 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.
@@ -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 1b4cf4c..6ebefbe 100644
--- a/mac/fira_frame.c
+++ b/mac/fira_frame.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the UWB stack for linux.
  *
- * Copyright (c) 2020-2021 Qorvo US, Inc.
+ * 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.
@@ -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,78 +36,26 @@
 
 #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(struct fira_session *session,
+bool fira_frame_check_n_controlees(const struct fira_session *session,
 				   size_t n_controlees, bool active)
 {
-	/* TODO: use more parameters (embedded mode, ranging mode, device
+	/*
+	 * TODO: use more parameters (embedded mode, ranging mode, device
 	 * type...) to calculate the size of frames.
 	 * Currently only SS-TWR vs DS-TWR mode is considered.
 	 * The computation MUST stay "pessimistic" (aka strict).
-	 * E.g.: for RCM each new controlee consumes 8 bytes so we need
-	 * AT LEAST 8 * n_controlee bytes of "free space". */
-	struct fira_session_params *params = &session->params;
+	 * E.g.: for control frame, each new controlee consumes 8 bytes so
+	 * we need AT LEAST 8 * n_controlee bytes of "free space".
+	 */
+	const struct fira_session_params *params = &session->params;
 	size_t mrm_size, rcm_size;
 	size_t n_msg_controller;
 	size_t n_msg_controlee = 2;
 
+	if (n_controlees > FIRA_CONTROLEES_MAX)
+		return false;
 	if (!active)
-		return n_controlees <= FIRA_CONTROLEES_MAX;
+		return true;
 
 	if (params->ranging_round_usage == FIRA_RANGING_ROUND_USAGE_DSTWR) {
 		mrm_size = FIRA_FRAME_WITHOUT_PAYLOAD_LEN +
@@ -138,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);
@@ -147,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);
@@ -156,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,
@@ -185,8 +137,7 @@ void fira_frame_control_payload_put(const struct fira_local *local,
 	u8 *p;
 	int i;
 
-	n_mngt = local->access.n_frames - 1 +
-		 local->n_stopped_controlees_short_addr;
+	n_mngt = local->access.n_frames - 1 + local->n_stopped_controlees;
 
 	p = fira_frame_common_payload_put(skb,
 					  FIRA_IE_PAYLOAD_CONTROL_LEN(n_mngt),
@@ -194,18 +145,15 @@ void fira_frame_control_payload_put(const struct fira_local *local,
 
 	*p++ = n_mngt;
 	*p++ = 0;
-	*p++ = session->next_block_stride_len;
+	*p++ = session->block_stride_len;
 
 	for (i = 0; i < local->access.n_frames - 1; i++) {
 		const struct fira_slot *slot = &local->slots[i + 1];
-		int initiator = slot->tx_controlee_index == -1;
+		int initiator = slot->controller_tx;
 		int slot_index = slot->index;
-		__le16 short_addr =
-			slot->tx_controlee_index == -1 ?
-				local->src_short_addr :
-				session->current_controlees
-					.data[slot->tx_controlee_index]
-					.short_addr;
+		__le16 short_addr = slot->controller_tx ?
+					    local->src_short_addr :
+					    slot->controlee->short_addr;
 		int message_id = slot->message_id;
 		u32 mngt = FIELD_PREP(FIRA_MNGT_RANGING_ROLE, initiator) |
 			   FIELD_PREP(FIRA_MNGT_SLOT_INDEX, slot_index) |
@@ -215,8 +163,8 @@ void fira_frame_control_payload_put(const struct fira_local *local,
 		p += sizeof(u32);
 	}
 
-	for (i = 0; i < local->n_stopped_controlees_short_addr; i++) {
-		__le16 short_addr = local->stopped_controlees_short_addr[i];
+	for (i = 0; i < local->n_stopped_controlees; i++) {
+		__le16 short_addr = local->stopped_controlees[i];
 		u32 mngt = FIELD_PREP(FIRA_MNGT_SHORT_ADDR, short_addr) |
 			   FIELD_PREP(FIRA_MNGT_STOP, 1);
 		put_unaligned_le32(mngt, p);
@@ -229,10 +177,11 @@ void fira_frame_measurement_report_payload_put(const struct fira_local *local,
 					       struct sk_buff *skb)
 {
 	const struct fira_session *session = local->current_session;
+	const struct fira_session_params *params = &session->params;
 	const struct fira_ranging_info *ranging_info =
 		&local->ranging_info[slot->ranging_index];
 	u8 *p;
-	int hopping_mode = session->params.round_hopping;
+	int hopping_mode = params->round_hopping;
 	int round_index_present = 1;
 	int reply_time_present = 0; /* for initiator */
 	int n_reply_time = local->n_ranging_valid;
@@ -240,8 +189,8 @@ void fira_frame_measurement_report_payload_put(const struct fira_local *local,
 	u32 first_round_trip_time;
 	u32 reply_time;
 	u64 initiation_rctu, response_rctu, final_rctu;
-	bool double_sided = (session->params.ranging_round_usage ==
-			     FIRA_RANGING_ROUND_USAGE_DSTWR);
+	bool double_sided = params->ranging_round_usage ==
+			    FIRA_RANGING_ROUND_USAGE_DSTWR;
 
 	p = fira_frame_common_payload_put(
 		skb,
@@ -267,10 +216,12 @@ void fira_frame_measurement_report_payload_put(const struct fira_local *local,
 	put_unaligned_le16(session->next_round_index, p);
 	p += sizeof(u16);
 
-	/* No handling for failed measurement, as there is only one, a failed
+	/*
+	 * No handling for failed measurement, as there is only one, a failed
 	 * measurement will cancel the ranging round.
 	 * With several measurements, make sure a later measurement can still be
-	 * done if an earlier one is failed. */
+	 * done if an earlier one is failed.
+	 */
 	initiation_rctu =
 		ranging_info
 			->timestamps_rctu[FIRA_MESSAGE_ID_RANGING_INITIATION];
@@ -322,7 +273,7 @@ void fira_frame_result_report_payload_put(const struct fira_local *local,
 	const struct fira_ranging_info *ranging_info =
 		&local->ranging_info[slot->ranging_index];
 	bool tof_present, aoa_azimuth_present, aoa_elevation_present,
-		aoa_fom_present;
+		aoa_fom_present, neg_tof_present;
 	u8 *p;
 
 	tof_present = ranging_info->tof_present && params->report_tof;
@@ -333,12 +284,13 @@ void fira_frame_result_report_payload_put(const struct fira_local *local,
 	aoa_fom_present = (ranging_info->local_aoa_azimuth.aoa_fom ||
 			   ranging_info->local_aoa_elevation.aoa_fom) &&
 			  params->report_aoa_fom;
+	neg_tof_present = tof_present && (ranging_info->tof_rctu < 0);
 
 	p = fira_frame_common_payload_put(
 		skb,
 		FIRA_IE_PAYLOAD_RESULT_REPORT_LEN(
 			tof_present, aoa_azimuth_present, aoa_elevation_present,
-			aoa_fom_present),
+			aoa_fom_present, neg_tof_present),
 		FIRA_MESSAGE_ID_RESULT_REPORT);
 
 	*p++ = FIELD_PREP(FIRA_RESULT_REPORT_CONTROL_TOF_PRESENT, tof_present) |
@@ -347,7 +299,9 @@ void fira_frame_result_report_payload_put(const struct fira_local *local,
 	       FIELD_PREP(FIRA_RESULT_REPORT_CONTROL_AOA_ELEVATION_PRESENT,
 			  aoa_elevation_present) |
 	       FIELD_PREP(FIRA_RESULT_REPORT_CONTROL_AOA_FOM_PRESENT,
-			  aoa_fom_present);
+			  aoa_fom_present) |
+		   FIELD_PREP(FIRA_RESULT_REPORT_CONTROL_NEG_TOF_PRESENT,
+			  neg_tof_present);
 
 	if (tof_present) {
 		put_unaligned_le32(
@@ -372,41 +326,48 @@ void fira_frame_result_report_payload_put(const struct fira_local *local,
 			p++;
 		}
 	}
+	if (neg_tof_present) {
+		put_unaligned_le32(-ranging_info->tof_rctu, p);
+		p += sizeof(u32);
+	}
 }
 
 void fira_frame_rframe_payload_put(struct fira_local *local,
 				   struct sk_buff *skb)
 {
 	struct fira_session *session = local->current_session;
-	struct fira_session_params *params = &local->current_session->params;
+	const struct fira_session_params *params = &session->params;
 	u8 *p;
 
-	if (params->data_payload_len == 0)
+	if (session->data_payload.seq == params->data_payload_seq)
 		return;
 
 	p = mcps802154_ie_put_payload_ie(skb, IEEE802154_IE_PAYLOAD_VENDOR_GID,
 					 FIRA_IE_VENDOR_OUI_LEN +
 						 params->data_payload_len);
 	WARN_RETURN_VOID_ON(!p);
-
 	put_unaligned_le24(params->data_vendor_oui, p);
 	p += FIRA_IE_VENDOR_OUI_LEN;
 	memcpy(p, params->data_payload, params->data_payload_len);
-	params->data_payload_len = 0;
-	session->data_payload_seq_sent = params->data_payload_seq;
+	session->data_payload.seq = params->data_payload_seq;
+	session->data_payload.sent = true;
 }
 
 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;
@@ -414,18 +375,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 &&
@@ -435,21 +393,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,
@@ -461,8 +435,8 @@ static bool fira_frame_control_read(struct fira_local *local, u8 *p,
 	int n_mngt, i;
 	u16 msg_ids = 0;
 	bool stop_found = false;
-	const struct fira_measurement_sequence_step *current_ms_step =
-		fira_session_get_current_meas_seq_step(session);
+	const struct fira_measurement_sequence_step *step =
+		fira_session_get_meas_seq_step(session);
 
 	n_mngt = *p++;
 	if (ie_len < FIRA_IE_PAYLOAD_CONTROL_LEN(n_mngt))
@@ -517,19 +491,13 @@ static bool fira_frame_control_read(struct fira_local *local, u8 *p,
 			msg_ids |= msg_id;
 			if (slot == local->slots + FIRA_CONTROLEE_FRAMES_MAX)
 				return false;
-			if (!initiator)
-				last.tx_controlee_index = 0;
-			else
-				last.tx_controlee_index = -1;
+			last.controller_tx = initiator;
 			last.ranging_index = 0;
 			last.message_id = message_id;
 			if (!initiator) {
 				last.tx_ant_set =
-					is_rframe ?
-						current_ms_step
-							->tx_ant_set_ranging :
-						current_ms_step
-							->tx_ant_set_nonranging;
+					is_rframe ? step->tx_ant_set_ranging :
+						    step->tx_ant_set_nonranging;
 			} else {
 				last.rx_ant_set = fira_session_get_rx_ant_set(
 					session, message_id);
@@ -604,13 +572,13 @@ fira_frame_measurement_report_fill_ranging_info(struct fira_local *local,
 	u64 rx_initiation_rctu, tx_response_rctu, rx_final_rctu;
 	u32 local_round_trip_rctu, local_reply_rctu;
 	int tof_rctu, i;
-	bool double_sided = (session->params.ranging_round_usage ==
-			     FIRA_RANGING_ROUND_USAGE_DSTWR);
+	bool double_sided = session->params.ranging_round_usage ==
+			    FIRA_RANGING_ROUND_USAGE_DSTWR;
 	control = *p++;
-	hopping_mode =
-		!!(control & FIRA_MEASUREMENT_REPORT_CONTROL_HOPPING_MODE);
-	round_index_present = !!(
-		control & FIRA_MEASUREMENT_REPORT_CONTROL_ROUND_INDEX_PRESENT);
+	hopping_mode = FIELD_GET(FIRA_MEASUREMENT_REPORT_CONTROL_HOPPING_MODE,
+				 control);
+	round_index_present = FIELD_GET(
+		FIRA_MEASUREMENT_REPORT_CONTROL_ROUND_INDEX_PRESENT, control);
 	n_reply_time = FIELD_GET(FIRA_MEASUREMENT_REPORT_CONTROL_N_REPLY_TIME,
 				 control);
 
@@ -634,13 +602,13 @@ fira_frame_measurement_report_fill_ranging_info(struct fira_local *local,
 				      n_reply_time)))
 		return false;
 
-	session->hopping_sequence_generation = hopping_mode &&
-					       !round_index_present;
 	if (round_index_present) {
 		int next_round_index;
 
 		next_round_index = get_unaligned_le16(p);
 		p += sizeof(u16);
+
+		session->controlee.next_round_index_valid = true;
 		session->next_round_index = next_round_index;
 	}
 
@@ -699,9 +667,10 @@ fira_frame_measurement_report_fill_ranging_info(struct fira_local *local,
 		tof_rctu =
 			((s32)remote_round_trip_rctu - adjusted_reply_rctu) / 2;
 	}
-	ranging_info->tof_rctu = tof_rctu > 0 ? tof_rctu : 0;
+	ranging_info->tof_rctu = (!slot->controller_tx) ? -tof_rctu : tof_rctu;
 	ranging_info->tof_present = true;
 
+	session->controlee.hopping_mode = hopping_mode;
 	return true;
 }
 
@@ -709,13 +678,14 @@ bool fira_frame_measurement_report_payload_check(
 	struct fira_local *local, const struct fira_slot *slot,
 	struct sk_buff *skb, struct mcps802154_ie_get_context *ie_get)
 {
+	const struct fira_session *session = local->current_session;
+	const struct fira_session_params *params = &session->params;
 	bool fira_payload_seen = false;
 	unsigned int minimum_payload_len;
 	int r;
 	u8 *p;
 
-	if (local->current_session->params.ranging_round_usage ==
-	    FIRA_RANGING_ROUND_USAGE_DSTWR)
+	if (params->ranging_round_usage == FIRA_RANGING_ROUND_USAGE_DSTWR)
 		minimum_payload_len =
 			FIRA_IE_PAYLOAD_MEASUREMENT_REPORT_TYPE1_LEN(false, 0);
 	else
@@ -766,7 +736,7 @@ fira_frame_result_report_fill_ranging_info(struct fira_local *local,
 	struct fira_ranging_info *ranging_info =
 		&local->ranging_info[slot->ranging_index];
 	u8 control;
-	bool tof_present, aoa_azimuth_present, aoa_elevation_present,
+	bool tof_present, neg_tof_present, aoa_azimuth_present, aoa_elevation_present,
 		aoa_fom_present;
 
 	control = *p++;
@@ -777,9 +747,10 @@ fira_frame_result_report_fill_ranging_info(struct fira_local *local,
 		!!(control & FIRA_RESULT_REPORT_CONTROL_AOA_ELEVATION_PRESENT);
 	aoa_fom_present =
 		!!(control & FIRA_RESULT_REPORT_CONTROL_AOA_FOM_PRESENT);
+	neg_tof_present = !!(control & FIRA_RESULT_REPORT_CONTROL_NEG_TOF_PRESENT);
 	if (ie_len < FIRA_IE_PAYLOAD_RESULT_REPORT_LEN(
 			     tof_present, aoa_azimuth_present,
-			     aoa_elevation_present, aoa_fom_present))
+			     aoa_elevation_present, aoa_fom_present, neg_tof_present))
 		return false;
 
 	if (tof_present) {
@@ -797,6 +768,13 @@ fira_frame_result_report_fill_ranging_info(struct fira_local *local,
 		ranging_info->remote_aoa_elevation_pi = get_unaligned_le16(p);
 		p += sizeof(s16);
 	}
+	if (neg_tof_present) {
+		/* When negative ToF is present at end of frame,
+		 * ToF read ahead MUST be 0, so, is safe to overwrite */
+		ranging_info->tof_rctu = -get_unaligned_le32(p);
+		p += sizeof(u32);
+	}
+
 	if (aoa_fom_present) {
 		ranging_info->remote_aoa_fom_present = true;
 		if (aoa_azimuth_present)
@@ -832,7 +810,7 @@ bool fira_frame_result_report_payload_check(
 				continue;
 
 			if (ie_get->len < FIRA_IE_PAYLOAD_RESULT_REPORT_LEN(
-						  false, false, false, false))
+						  false, false, false, false, false))
 				return false;
 			message_id = (*p++) & 0xf;
 			if (message_id != FIRA_MESSAGE_ID_RESULT_REPORT)
@@ -857,9 +835,10 @@ bool fira_frame_rframe_payload_check(struct fira_local *local,
 				     struct sk_buff *skb,
 				     struct mcps802154_ie_get_context *ie_get)
 {
+	const struct fira_session *session = local->current_session;
+	const struct fira_session_params *params = &session->params;
 	struct fira_ranging_info *ranging_info =
 		&local->ranging_info[slot->ranging_index];
-	struct fira_session_params *params = &local->current_session->params;
 	bool rframe_payload_seen = false;
 	int r;
 	u8 *p;
@@ -870,7 +849,8 @@ bool fira_frame_rframe_payload_check(struct fira_local *local,
 		skb_pull(skb, ie_get->len);
 
 		if (ie_get->id == IEEE802154_IE_PAYLOAD_VENDOR_GID &&
-		    ie_get->len >= FIRA_IE_VENDOR_OUI_LEN) {
+		    ie_get->len >= FIRA_IE_VENDOR_OUI_LEN &&
+		    ie_get->len <= FIRA_IE_VENDOR_OUI_LEN + FIRA_DATA_PAYLOAD_SIZE_MAX) {
 			u32 vendor;
 			unsigned int data_len;
 
@@ -896,76 +876,77 @@ 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)
+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 *phy_sts_index)
 {
-	__le16 src_short_addr;
-
-	if (slot->tx_controlee_index == -1)
-		src_short_addr = local->dst_short_addr;
-	else
-		src_short_addr = session->current_controlees
-					 .data[slot->tx_controlee_index]
-					 .short_addr;
+	const struct fira_session *session = local->current_session;
+	struct fira_session *session_found = NULL;
+	u32 session_id;
 
-	return fira_aead_decrypt(&session->crypto.aead, skb, header_len,
-				 src_short_addr, slot->index);
+	if (!fira_frame_header_check(local, skb, ie_get, phy_sts_index,
+				     &session_id))
+		return NULL;
+	if (session->id == session_id) {
+		session_found = local->current_session;
+	} else if (session->controlee.synchronised) {
+		return NULL;
+	} else {
+		session_found =
+			fira_get_session_by_session_id(local, session_id);
+		if (!session_found ||
+		    session_found->params.device_type !=
+			    FIRA_DEVICE_TYPE_CONTROLEE ||
+		    !fira_session_is_active(session_found))
+			return NULL;
+		/*
+		 * FIXME: The previous session will not sent a ranging round
+		 * report failure.
+		 *
+		 * The most simple is probably to remove a round ranging?
+		 * or keep somewhere, previous value.
+		 * or choice number 3.
+		 * ```
+		 * int remove_blocks = session->block_stride_len + 1;
+		 *
+		 * session->block_start_dtu -= remove_blocks *
+		 *     params->block_duration_dtu;
+		 * session->block_index -= remove_blocks;
+		 * ```
+		 */
+	}
+	/* Update current and allow content of session to be updated. */
+	local->current_session = session_found;
+	return session_found;
 }
 
 int fira_frame_header_check_decrypt(struct fira_local *local,
 				    const struct fira_slot *slot,
 				    struct sk_buff *skb,
-				    struct mcps802154_ie_get_context *ie_get,
-				    u32 *allow_resync_sts_index,
-				    struct fira_session **allow_resync_session)
+				    struct mcps802154_ie_get_context *ie_get)
 {
 	struct fira_session *session = local->current_session;
-	u32 sts_index;
+	int header_len;
+	__le16 src_short_addr;
+	u32 phy_sts_index;
 	u32 session_id;
 	u8 *header;
-	unsigned int header_len;
-	bool active;
 
 	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 (allow_resync_session && session_id != session->id) {
-		session = fira_session_get(local, session_id, &active);
-		if (!session ||
-		    session->params.device_type != FIRA_DEVICE_TYPE_CONTROLEE ||
-		    !active || session->synchronised)
-			return -EBADMSG;
-		*allow_resync_session = session;
-	} else if (session_id != session->id) {
+	if (session_id != session->id)
 		return -EBADMSG;
-	}
 
-	if (allow_resync_sts_index) {
-		*allow_resync_sts_index = sts_index - slot->index;
-	} else 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 33bf1f2..7adf39c 100644
--- a/mac/fira_frame.h
+++ b/mac/fira_frame.h
@@ -1,7 +1,7 @@
 /*
  * This file is part of the UWB stack for linux.
  *
- * Copyright (c) 2020-2021 Qorvo US, Inc.
+ * 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.
@@ -33,6 +33,64 @@ 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, neg_tof_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)) +		\
+	4 * (neg_tof_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)
+#define FIRA_RESULT_REPORT_CONTROL_NEG_TOF_PRESENT (1 << 4)
+
 /**
  * fira_frame_check_n_controlees() - Check the number of wanted
  * controlees.
@@ -47,7 +105,7 @@ struct fira_session_params;
  * For an active session, it depends on the space left in messages, which is
  * determined by the session parameters.
  */
-bool fira_frame_check_n_controlees(struct fira_session *session,
+bool fira_frame_check_n_controlees(const struct fira_session *session,
 				   size_t n_controlees, bool active);
 
 /**
@@ -106,7 +164,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 +172,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,29 +235,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.
+ * 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.
+ * @phy_sts_index: STS index received.
  *
- * 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.
+ * Return: Session context or NULL.
  */
-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);
+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 *phy_sts_index);
 
 /**
  * fira_frame_header_check_decrypt() - Check and consume header, and decrypt
@@ -208,18 +257,12 @@ int fira_frame_decrypt(struct fira_local *local, struct fira_session *session,
  * @slot: Corresponding slot.
  * @skb: Frame buffer.
  * @ie_get: Context used to read IE, must be zero initialized.
- * @allow_resync_sts_index: If not NULL, allow STS index resynchronisation and
- * store received STS index at given address, if NULL, forbid resynchronisation.
- * @allow_resync_session: If not NULL, allow session synchronisation and store received
- * session at given address, if NULL, forbid resynchronisation.
  *
  * Return: 0 or error.
  */
 int fira_frame_header_check_decrypt(struct fira_local *local,
 				    const struct fira_slot *slot,
 				    struct sk_buff *skb,
-				    struct mcps802154_ie_get_context *ie_get,
-				    u32 *allow_resync_sts_index,
-				    struct fira_session **allow_resync_session);
+				    struct mcps802154_ie_get_context *ie_get);
 
 #endif /* FIRA_FRAME_H */
diff --git a/mac/fira_region.c b/mac/fira_region.c
index 7924c6f..9e4bf53 100644
--- a/mac/fira_region.c
+++ b/mac/fira_region.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the UWB stack for linux.
  *
- * Copyright (c) 2020-2021 Qorvo US, Inc.
+ * 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.
@@ -22,10 +22,8 @@
  */
 
 #include <linux/slab.h>
-#include <linux/errno.h>
-#include <linux/math64.h>
-
 #include <linux/netdevice.h>
+#include <linux/errno.h>
 
 #include <net/mcps802154_schedule.h>
 #include <net/fira_region_nl.h>
@@ -34,10 +32,27 @@
 #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)
+{
+	struct fira_local *local =
+		container_of(work, struct fira_local, report_work);
+	struct sk_buff *skb;
+	int r;
+
+	while (!skb_queue_empty(&local->report_queue)) {
+		skb = skb_dequeue(&local->report_queue);
+		r = mcps802154_region_event(local->llhw, skb);
+		if (r == -ECONNREFUSED)
+			/* TODO stop. */
+			;
+	}
+}
 
 static struct mcps802154_region *fira_open(struct mcps802154_llhw *llhw)
 {
@@ -48,17 +63,28 @@ static struct mcps802154_region *fira_open(struct mcps802154_llhw *llhw)
 		return NULL;
 	local->llhw = llhw;
 	local->region.ops = &fira_region_ops;
-	local->current_session = NULL;
 	INIT_LIST_HEAD(&local->inactive_sessions);
 	INIT_LIST_HEAD(&local->active_sessions);
+	skb_queue_head_init(&local->report_queue);
+	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;
 }
 
 static void fira_close(struct mcps802154_region *region)
 {
 	struct fira_local *local = region_to_local(region);
+	struct fira_session *session, *s;
+
+	list_for_each_entry_safe (session, s, &local->inactive_sessions,
+				  entry) {
+		fira_session_free(local, session);
+	}
 
+	cancel_work_sync(&local->report_work);
+	skb_queue_purge(&local->report_queue);
 	kfree_sensitive(local);
 }
 
@@ -68,8 +94,8 @@ static void fira_notify_stop(struct mcps802154_region *region)
 	struct fira_session *session, *s;
 
 	list_for_each_entry_safe (session, s, &local->active_sessions, entry) {
-		session->stop_request = true;
-		fira_session_access_done(local, session, false);
+		fira_session_stop_controlees(session);
+		fira_session_fsm_stop(local, session);
 	}
 }
 
@@ -86,236 +112,300 @@ static int fira_call(struct mcps802154_region *region, u32 call_id,
 	}
 }
 
-static int fira_get_demand(struct mcps802154_region *region,
-			   u32 next_timestamp_dtu,
-			   struct mcps802154_region_demand *demand)
+/**
+ * fira_session_init_block_start_dtu() - Build the first block start dtu.
+ * @local: FiRa context.
+ * @session: Session context.
+ * @timestamp_dtu: First access opportunity.
+ */
+static void fira_session_init_block_start_dtu(struct fira_local *local,
+					      struct fira_session *session,
+					      u32 timestamp_dtu)
 {
-	struct fira_local *local = region_to_local(region);
-	struct fira_session *session;
-
-	session = fira_session_next(
-		local, next_timestamp_dtu + local->llhw->anticip_dtu, 0);
-
-	if (session) {
-		fira_session_get_demand(local, session, demand);
-		demand->max_duration_dtu = session->last_access_duration_dtu;
-		local->current_session = session;
-		return 1;
+	if (!session->block_start_valid) {
+		const struct fira_session_params *params = &session->params;
+		int dtu_freq_khz = local->llhw->dtu_freq_hz / 1000;
+
+		session->block_start_valid = true;
+		session->block_start_dtu =
+			timestamp_dtu +
+			params->initiation_time_ms * dtu_freq_khz;
+		session->next_access_timestamp_dtu = session->block_start_dtu;
 	}
-	return 0;
-}
-
-static int fira_report_local_aoa(struct sk_buff *msg,
-				 const struct fira_local_aoa_info *info)
-{
-#define A(x) FIRA_RANGING_DATA_MEASUREMENTS_AOA_ATTR_##x
-	if (nla_put_u8(msg, A(RX_ANTENNA_SET), info->rx_ant_set))
-		goto nla_put_failure;
-	if (nla_put_s16(msg, A(AOA_2PI), info->aoa_2pi))
-		goto nla_put_failure;
-	if (nla_put_s16(msg, A(PDOA_2PI), info->pdoa_2pi))
-		goto nla_put_failure;
-	if (nla_put_u8(msg, A(AOA_FOM), info->aoa_fom))
-		goto nla_put_failure;
-#undef A
-	return 0;
-nla_put_failure:
-	return -EMSGSIZE;
 }
 
-static int fira_report_measurement(struct fira_local *local,
-				   struct sk_buff *msg,
-				   const struct fira_ranging_info *ranging_info)
+/**
+ * fira_get_next_session() - Find the next session which should have the
+ * access.
+ * @local: FiRa context.
+ * @next_timestamp_dtu: Next access opportunity.
+ * @max_duration_dtu: Max duration of the next access opportunity.
+ * @adopted_demand: Output updated related to next session returned.
+ *
+ * Return: Pointer to the next session which should have the access.
+ */
+static struct fira_session *
+fira_get_next_session(struct fira_local *local, u32 next_timestamp_dtu,
+		      int max_duration_dtu,
+		      struct fira_session_demand *adopted_demand)
 {
-	const struct fira_session *session = local->current_session;
-	struct nlattr *aoa;
-#define A(x) FIRA_RANGING_DATA_MEASUREMENTS_ATTR_##x
-
-	if (nla_put_u16(msg, A(SHORT_ADDR), ranging_info->short_addr) ||
-	    nla_put_u8(msg, A(STATUS), ranging_info->status))
-		goto nla_put_failure;
-
-	if (ranging_info->status) {
-		if (nla_put_u8(msg, A(SLOT_INDEX), ranging_info->slot_index))
-			goto nla_put_failure;
-		return 0;
-	}
+	struct fira_session *adopted_session = NULL;
+	struct fira_session *session;
+	bool is_candidate_unsync, is_adopted_unsync;
+	int max_unsync_duration_dtu = max_duration_dtu;
+	int r;
 
-	if (ranging_info->tof_present) {
-		static const s64 speed_of_light_mm_per_s = 299702547000ull;
-		s32 distance_mm = div64_s64(
-			ranging_info->tof_rctu * speed_of_light_mm_per_s,
-			(s64)local->llhw->dtu_freq_hz * local->llhw->dtu_rctu);
-		if (nla_put_s32(msg, A(DISTANCE_MM), distance_mm))
-			goto nla_put_failure;
+	/*
+	 * Little cheat to start all sessions as initiation_time_ms is a
+	 * delay, and not a absolute time. This is the only function
+	 * which change the session content.
+	 */
+	list_for_each_entry (session, &local->active_sessions, entry) {
+		fira_session_init_block_start_dtu(local, session,
+						  next_timestamp_dtu);
 	}
 
-	if (ranging_info->local_aoa.present) {
-		aoa = nla_nest_start(msg, A(LOCAL_AOA));
-		if (!aoa)
-			goto nla_put_failure;
-		if (fira_report_local_aoa(msg, &ranging_info->local_aoa))
-			goto nla_put_failure;
-		nla_nest_end(msg, aoa);
-	}
+	/*
+	 * Reminder: active_sessions list is sorted by session->priority
+	 * from highest priority to lowest priority.
+	 */
+	list_for_each_entry (session, &local->active_sessions, entry) {
+		/*
+		 * Welcome in sessions election!
+		 *
+		 * First, the candidate session will provide its wish in
+		 * 'candidate_demand'.
+		 * And then the candidate will be compared with the adopted
+		 * session. The "best" will be become or stay the adopted
+		 * session.
+		 * So the session election will process candidate after
+		 * candidate, to find the most appropriate session.
+		 */
+		struct fira_session_demand candidate_demand;
+
+		/*
+		 * Sessions with lower priority are not allowed to overlap
+		 * the adopted session. But a lower priority can start and
+		 * stop before the session adopted.
+		 */
+		if (adopted_session && adopted_session->params.priority !=
+					       session->params.priority) {
+			/* Is there some time left? */
+			if (is_before_dtu(next_timestamp_dtu,
+					  adopted_demand->timestamp_dtu))
+				/*
+				 * Limit max duration for session with lower
+				 * priority to not overlap sessions which have
+				 * an higher priority.
+				 */
+				max_duration_dtu =
+					adopted_demand->timestamp_dtu -
+					next_timestamp_dtu;
+			else
+				/* No more time left. */
+				break;
+			if (!max_unsync_duration_dtu ||
+			    max_unsync_duration_dtu > max_duration_dtu)
+				max_unsync_duration_dtu = max_duration_dtu;
+		}
 
-	if (ranging_info->local_aoa_azimuth.present) {
-		aoa = nla_nest_start(msg, A(LOCAL_AOA_AZIMUTH));
-		if (!aoa)
-			goto nla_put_failure;
-		if (fira_report_local_aoa(msg,
-					  &ranging_info->local_aoa_azimuth))
-			goto nla_put_failure;
-		nla_nest_end(msg, aoa);
-	}
-	if (ranging_info->local_aoa_elevation.present) {
-		aoa = nla_nest_start(msg, A(LOCAL_AOA_ELEVATION));
-		if (!aoa)
-			goto nla_put_failure;
-		if (fira_report_local_aoa(msg,
-					  &ranging_info->local_aoa_elevation))
-			goto nla_put_failure;
-		nla_nest_end(msg, aoa);
-	}
-	if (ranging_info->remote_aoa_azimuth_present) {
-		if (nla_put_s16(msg, A(REMOTE_AOA_AZIMUTH_2PI),
-				ranging_info->remote_aoa_azimuth_2pi))
-			goto nla_put_failure;
-		if (ranging_info->remote_aoa_fom_present) {
-			if (nla_put_u8(msg, A(REMOTE_AOA_AZIMUTH_FOM),
-				       ranging_info->remote_aoa_azimuth_fom))
-				goto nla_put_failure;
+		is_candidate_unsync = session->params.device_type ==
+					      FIRA_DEVICE_TYPE_CONTROLEE &&
+				      !session->controlee.synchronised;
+		/* Retrieve the wish of the session candidate. */
+		r = fira_session_fsm_get_demand(
+			local, session, next_timestamp_dtu,
+			is_candidate_unsync ? max_unsync_duration_dtu :
+					      max_duration_dtu,
+			&candidate_demand);
+		/* When 'r' is one, the session have a demand. */
+		if (r != 1)
+			/* The session doesn't have a demand. */
+			continue;
+
+		/*
+		 * If there is no adopted session, the candidate is the
+		 * adopted session.
+		 */
+		if (!adopted_session)
+			goto candidate_adopted;
+		/*
+		 * Is session finish before the adopted session ?
+		 * adopted_demand |              [-----]
+		 * candidate      |   [------]
+		 *              --+-----------------------> Time
+		 */
+		if (is_before_dtu(candidate_demand.timestamp_dtu +
+					  candidate_demand.max_duration_dtu,
+				  adopted_demand->timestamp_dtu))
+			/*
+			 * Candidate is adopted and replace the
+			 * previous one.
+			 */
+			goto candidate_adopted;
+		/*
+		 * Is session start after the adopted session ?
+		 * adopted_demand |   [------]
+		 * candidate      |             [--------]
+		 *              --+----------------------> Time
+		 */
+		if (is_before_dtu(adopted_demand->timestamp_dtu +
+					  adopted_demand->max_duration_dtu,
+				  candidate_demand.timestamp_dtu))
+			/* Candidate is not adopted. */
+			continue;
+		/*
+		 * The candidate session have an overlap with the adopted
+		 * session. Try the negotiation first to find an agreement
+		 * about the access usage.
+		 *
+		 * But take care, synchronized session have a better
+		 * eloquence in case of negotiation failure with an
+		 * unsynchronized session.
+		 */
+		is_adopted_unsync = adopted_session->params.device_type ==
+					    FIRA_DEVICE_TYPE_CONTROLEE &&
+				    !adopted_session->controlee.synchronised;
+		/*
+		 * The candidate session have an overlap with the adopted
+		 * session.
+		 *
+		 * adopted_demand |   [------]
+		 * candidate      |       [--------]
+		 *              --+----------------------> Time
+		 *
+		 * Request a duration reduction to the adopted session.
+		 */
+		if (is_adopted_unsync &&
+		    !is_before_dtu(candidate_demand.timestamp_dtu,
+				   adopted_demand->timestamp_dtu)) {
+			int limit_duration_dtu =
+				candidate_demand.timestamp_dtu -
+				adopted_demand->timestamp_dtu;
+			struct fira_session_demand tmp;
+
+			if (limit_duration_dtu)
+				/* Ask to reduce the duration. */
+				r = fira_session_fsm_get_demand(
+					local, adopted_session,
+					next_timestamp_dtu, limit_duration_dtu,
+					&tmp);
+			else
+				/* Both sessions start at same time. */
+				r = 0;
+			if (r == 1) {
+				/*
+				 * The adopted session accept to
+				 * reduction its max duration.
+				 */
+				*adopted_demand = tmp;
+				max_unsync_duration_dtu = limit_duration_dtu;
+				continue;
+			}
+			if (!is_candidate_unsync)
+				/*
+				 * In this corrupted world, synchronized
+				 * session have better relation.
+				 */
+				goto candidate_adopted;
 		}
-	}
-	if (ranging_info->remote_aoa_elevation_present) {
-		if (nla_put_s16(msg, A(REMOTE_AOA_ELEVATION_PI),
-				ranging_info->remote_aoa_elevation_pi))
-			goto nla_put_failure;
-		if (ranging_info->remote_aoa_fom_present) {
-			if (nla_put_u8(msg, A(REMOTE_AOA_ELEVATION_FOM),
-				       ranging_info->remote_aoa_elevation_fom))
-				goto nla_put_failure;
+		/*
+		 * The candidate session have an overlap with the adopted
+		 * session.
+		 *
+		 * adopted_demand |       [-----]
+		 * candidate      |  [------]
+		 *              --+----------------------> Time
+		 *
+		 * Request a duration reduction to the candidate session.
+		 */
+		if (is_candidate_unsync &&
+		    !is_before_dtu(adopted_demand->timestamp_dtu,
+				   candidate_demand.timestamp_dtu)) {
+			int limit_duration_dtu = adopted_demand->timestamp_dtu -
+						 candidate_demand.timestamp_dtu;
+			struct fira_session_demand tmp;
+
+			if (limit_duration_dtu)
+				/* Ask to reduce the duration. */
+				r = fira_session_fsm_get_demand(
+					local, session, next_timestamp_dtu,
+					limit_duration_dtu, &tmp);
+			else
+				/* Both sessions start at same time. */
+				r = 0;
+			if (r == 1) {
+				/*
+				 * The candidate session accept to
+				 * reduction its max duration.
+				 */
+				adopted_session = session;
+				*adopted_demand = tmp;
+				max_unsync_duration_dtu = limit_duration_dtu;
+				continue;
+			}
+			if (!is_adopted_unsync)
+				/*
+				 * In this corrupted world, synchronized
+				 * session have better relation.
+				 */
+				continue;
 		}
-	}
-	if (ranging_info->data_payload_len > 0) {
-		if (nla_put(msg, A(DATA_PAYLOAD_RECV),
-			    ranging_info->data_payload_len,
-			    ranging_info->data_payload))
-			goto nla_put_failure;
-	}
-	if (session->data_payload_seq_sent > 0) {
-		if (nla_put_u32(msg, A(DATA_PAYLOAD_SEQ_SENT),
-				session->data_payload_seq_sent))
-			goto nla_put_failure;
-	}
 
-#undef A
-	return 0;
-nla_put_failure:
-	return -EMSGSIZE;
+		/*
+		 * Finally, negotiation between adopted and candidate fails.
+		 * One of the session will probably have ranging not done.
+		 * Choose the session which have the oldest access.
+		 */
+		if (is_before_dtu(session->last_access_timestamp_dtu,
+				  adopted_session->last_access_timestamp_dtu))
+			goto candidate_adopted;
+
+		/* Candidate is not adopted. */
+		continue;
+
+	candidate_adopted:
+		adopted_session = session;
+		*adopted_demand = candidate_demand;
+	}
+	return adopted_session;
 }
 
-static int fira_report_measurement_stopped_controlee(struct fira_local *local,
-						     struct sk_buff *msg,
-						     __le16 short_addr)
+static struct mcps802154_access *
+fira_get_access(struct mcps802154_region *region, u32 next_timestamp_dtu,
+		int next_in_region_dtu, int region_duration_dtu)
 {
-#define A(x) FIRA_RANGING_DATA_MEASUREMENTS_ATTR_##x
-
-	if (nla_put_u16(msg, A(SHORT_ADDR), short_addr) ||
-	    nla_put_u8(msg, A(STOPPED), 1))
-		goto nla_put_failure;
-
-#undef A
-	return 0;
-nla_put_failure:
-	return -EMSGSIZE;
+	struct fira_local *local = region_to_local(region);
+	/* 'fsd' acronyms is FiRa Session Demand. */
+	struct fira_session_demand fsd;
+	struct fira_session *session;
+	int max_duration_dtu =
+		region_duration_dtu ? region_duration_dtu - next_in_region_dtu :
+				      0;
+
+	session = fira_get_next_session(local, next_timestamp_dtu,
+					max_duration_dtu, &fsd);
+	if (session)
+		return fira_session_fsm_get_access(local, session, &fsd);
+	return NULL;
 }
 
-void fira_report(struct fira_local *local, struct fira_session *session,
-		 bool add_measurements)
+static int fira_get_demand(struct mcps802154_region *region,
+			   u32 next_timestamp_dtu,
+			   struct mcps802154_region_demand *next_demand)
 {
-	struct sk_buff *msg;
-	struct nlattr *data, *measurements, *measurement;
-	int ranging_interval_ms, i, r;
-	bool stop_completed;
-
-	msg = mcps802154_region_event_alloc_skb(local->llhw, &local->region,
-						FIRA_CALL_SESSION_NOTIFICATION,
-						session->event_portid,
-						NLMSG_DEFAULT_SIZE, GFP_KERNEL);
-	if (!msg)
-		return;
-
-	if (nla_put_u32(msg, FIRA_CALL_ATTR_SESSION_ID, session->id))
-		goto nla_put_failure;
-
-	data = nla_nest_start(msg, FIRA_CALL_ATTR_RANGING_DATA);
-	if (!data)
-		goto nla_put_failure;
-
-	ranging_interval_ms = session->params.block_duration_dtu *
-			      (session->block_stride_len + 1) /
-			      (local->llhw->dtu_freq_hz / 1000);
-	if (nla_put_u32(msg, FIRA_RANGING_DATA_ATTR_BLOCK_INDEX,
-			session->block_index) ||
-	    nla_put_u32(msg, FIRA_RANGING_DATA_ATTR_RANGING_INTERVAL_MS,
-			ranging_interval_ms))
-		goto nla_put_failure;
-
-	stop_completed = (session->max_number_of_measurements_reached ||
-			  session->stop_request) &&
-			 !(session->controlee_management_flags &
-			   FIRA_SESSION_CONTROLEE_MANAGEMENT_FLAG_STOP);
-	if (stop_completed || session->stop_inband ||
-	    session->stop_no_response) {
-		enum fira_ranging_data_attrs_stopped_values stopped_value =
-			stop_completed ?
-				FIRA_RANGING_DATA_ATTR_STOPPED_REQUEST :
-				session->stop_inband ?
-				FIRA_RANGING_DATA_ATTR_STOPPED_IN_BAND :
-				FIRA_RANGING_DATA_ATTR_STOPPED_NO_RESPONSE;
-
-		if (nla_put_u8(msg, FIRA_RANGING_DATA_ATTR_STOPPED,
-			       stopped_value))
-			goto nla_put_failure;
-	}
-
-	if (add_measurements && (local->n_ranging_info +
-				 local->n_stopped_controlees_short_addr) != 0) {
-		measurements = nla_nest_start(
-			msg, FIRA_RANGING_DATA_ATTR_MEASUREMENTS);
-		if (!measurements)
-			goto nla_put_failure;
-
-		for (i = 0; i < local->n_ranging_info; i++) {
-			measurement = nla_nest_start(msg, 1);
-			if (fira_report_measurement(local, msg,
-						    &local->ranging_info[i]))
-				goto nla_put_failure;
-			nla_nest_end(msg, measurement);
-		}
-
-		for (i = 0; i < local->n_stopped_controlees_short_addr; i++) {
-			measurement = nla_nest_start(msg, 1);
-			if (fira_report_measurement_stopped_controlee(
-				    local, msg,
-				    local->stopped_controlees_short_addr[i]))
-				goto nla_put_failure;
-			nla_nest_end(msg, measurement);
-		}
+	struct fira_local *local = region_to_local(region);
+	/* 'fsd' for FiRa Session Demand. */
+	struct fira_session_demand fsd;
+	struct fira_session *session;
 
-		nla_nest_end(msg, measurements);
+	session = fira_get_next_session(local, next_timestamp_dtu, 0, &fsd);
+	if (session) {
+		next_demand->timestamp_dtu = fsd.timestamp_dtu;
+		next_demand->max_duration_dtu = fsd.max_duration_dtu;
+		return 1;
 	}
-
-	nla_nest_end(msg, data);
-
-	r = mcps802154_region_event(local->llhw, msg);
-	if (r == -ECONNREFUSED)
-		/* TODO stop. */
-		;
-	return;
-nla_put_failure:
-	kfree_skb(msg);
+	return 0;
 }
 
 static struct mcps802154_region_ops fira_region_ops = {
@@ -329,12 +419,50 @@ static struct mcps802154_region_ops fira_region_ops = {
 	.get_demand = fira_get_demand,
 };
 
-int __init fira_region_init(void)
+struct fira_session *fira_get_session_by_session_id(struct fira_local *local,
+						    u32 session_id)
 {
-	int r;
+	struct fira_session *session;
+
+	list_for_each_entry (session, &local->active_sessions, entry) {
+		if (session->id == session_id)
+			return session;
+	}
+	list_for_each_entry (session, &local->inactive_sessions, entry) {
+		if (session->id == session_id)
+			return session;
+	}
+	return NULL;
+}
 
-	r = fira_crypto_test();
-	WARN_RETURN(r);
+void fira_check_all_missed_ranging(struct fira_local *local,
+				   const struct fira_session *recent_session,
+				   u32 timestamp_dtu)
+{
+	struct fira_session *session, *s;
+
+	/*
+	 * Process sessions with safe function, as the session FSM can leave
+	 * the active list for many stop reasons.
+	 */
+	list_for_each_entry_safe (session, s, &local->active_sessions, entry) {
+		if (recent_session == session)
+			continue;
+		/*
+		 * Does the session started during the access of
+		 * recent_session?
+		 */
+		if (!session->block_start_valid)
+			continue;
+		fira_session_fsm_check_missed_ranging(local, session,
+						      timestamp_dtu);
+	}
+}
+
+int __init fira_region_init(void)
+{
+	if (do_crypto_selftest_on_module_init)
+		WARN_RETURN(fira_crypto_test());
 
 	return mcps802154_region_register(&fira_region_ops);
 }
@@ -344,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.h b/mac/fira_region.h
index 7feaf3f..8ec9d84 100644
--- a/mac/fira_region.h
+++ b/mac/fira_region.h
@@ -1,7 +1,7 @@
 /*
  * This file is part of the UWB stack for linux.
  *
- * Copyright (c) 2020-2021 Qorvo US, Inc.
+ * 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.
@@ -25,6 +25,7 @@
 #define NET_FIRA_REGION_H
 
 #include <linux/kernel.h>
+#include <linux/workqueue.h>
 #include <net/mcps802154_schedule.h>
 
 #include "net/fira_region_params.h"
@@ -73,6 +74,41 @@ enum fira_message_id {
 };
 
 /**
+ * struct fira_diagnostic - Diagnostic result.
+ */
+struct fira_diagnostic {
+	/**
+	 * @rssis_q1: Received signal strength indication (RSSI), absolute value
+	 * in Q1 fixed point format, unit is dBm.
+	 */
+	u8 rssis_q1[MCPS802154_RSSIS_N_MAX];
+	/**
+	 * @n_rssis: The number of RSSI in the array below.
+	 */
+	size_t n_rssis;
+	/**
+	 * @aoas: Angle of arrival, ordered by increasing measurement type.
+	 */
+	struct mcps802154_rx_aoa_measurements
+		aoas[MCPS802154_RX_AOA_MEASUREMENTS_MAX];
+	/**
+	 * @n_aoas: Number of angle of arrival.
+	 */
+	size_t n_aoas;
+	/**
+	 * @cirs: CIR for different parts of the frame.
+	 *
+	 * Set by low-level driver, must be kept valid until next received
+	 * frame.
+	 */
+	struct mcps802154_rx_cir *cirs;
+	/**
+	 * @n_cirs: Number of parts of CIR.
+	 */
+	size_t n_cirs;
+};
+
+/**
  * struct fira_slot - Information on an active slot.
  */
 struct fira_slot {
@@ -83,10 +119,9 @@ struct fira_slot {
 	 */
 	int index;
 	/**
-	 * @tx_controlee_index: Index of the controlee transmitting in this
-	 * slot, or -1 for the controller.
+	 * @controller_tx: True if Tx is performed by the controller.
 	 */
-	int tx_controlee_index;
+	bool controller_tx;
 	/**
 	 * @ranging_index: Index of the ranging in the ranging information
 	 * table, -1 if none.
@@ -104,6 +139,10 @@ struct fira_slot {
 	 * @rx_ant_set: Rx antenna set.
 	 */
 	int rx_ant_set;
+	/**
+	 * @controlee: Controlee.
+	 */
+	struct fira_controlee *controlee;
 };
 
 /**
@@ -173,6 +212,14 @@ struct fira_ranging_info {
 	 */
 	u8 remote_aoa_elevation_fom;
 	/**
+	 * @rx_rssis: RSSI value measured for individual Rx frames.
+	 */
+	u8 rx_rssis[FIRA_MESSAGE_ID_MAX + 1];
+	/**
+	 * @n_rx_rssis: Number of Rx RSSI saved.
+	 */
+	int n_rx_rssis;
+	/**
 	 * @short_addr: Peer short address.
 	 */
 	__le16 short_addr;
@@ -216,6 +263,10 @@ struct fira_ranging_info {
 	 * @data_payload_len: Custom data payload length.
 	 */
 	int data_payload_len;
+	/**
+	 * @rx_ctx: Pointer to the current rx_ctx context controlee.
+	 */
+	void *rx_ctx;
 };
 
 /**
@@ -235,6 +286,14 @@ struct fira_local {
 	 */
 	struct mcps802154_access access;
 	/**
+	 * @report_queue: Queue of report frame to be processed.
+	 */
+	struct sk_buff_head report_queue;
+	/**
+	 * @report_work: Process work of report event.
+	 */
+	struct work_struct report_work;
+	/**
 	 * @frames: Access frames referenced from access.
 	 */
 	struct mcps802154_access_frame frames[FIRA_FRAMES_MAX];
@@ -247,10 +306,35 @@ struct fira_local {
 	 */
 	struct mcps802154_channel channel;
 	/**
+	 * @inactive_sessions: List of inactive sessions.
+	 */
+	struct list_head inactive_sessions;
+	/**
+	 * @active_sessions: List of active sessions.
+	 */
+	struct list_head active_sessions;
+	/**
 	 * @current_session: Pointer to the current session.
 	 */
 	struct fira_session *current_session;
 	/**
+	 * @src_short_addr: Source address for the current session (actually
+	 * never put as a source address in a frame, but used for control
+	 * message).
+	 */
+	__le16 src_short_addr;
+	/**
+	 * @dst_short_addr: Destination address for the current session. When
+	 * controller, this is broadcast or the address of the only controlee.
+	 * When controlee, this is the address of the controller.
+	 */
+	__le16 dst_short_addr;
+	/**
+	 * @block_duration_rx_margin_ppm: Block duration rx margin for
+	 * controlees.
+	 */
+	int block_duration_rx_margin_ppm;
+	/**
 	 * @slots: Descriptions of each active slots for the current session.
 	 * When controller, this is filled when the access is requested. When
 	 * controlee, the first slot is filled when the access is requested and
@@ -275,41 +359,19 @@ struct fira_local {
 	 */
 	int n_ranging_valid;
 	/**
-	 * @src_short_addr: Source address for the current session (actually
-	 * never put as a source address in a frame, but used for control
-	 * message).
-	 */
-	__le16 src_short_addr;
-	/**
-	 * @dst_short_addr: Destination address for the current session. When
-	 * controller, this is broadcast or the address of the only controlee.
-	 * When controlee, this is the address of the controller.
-	 */
-	__le16 dst_short_addr;
-	/**
-	 * @inactive_sessions: List of inactive sessions.
-	 */
-	struct list_head inactive_sessions;
-	/**
-	 * @active_sessions: List of active sessions.
-	 */
-	struct list_head active_sessions;
-	/**
-	 * @stopped_controlees_short_addr: Short addresses of the stopped
-	 * controlees for which an element must be added to the Device
-	 * Management List of the RCM message.
+	 * @diagnostics: Diagnostic collected for each slot.
 	 */
-	__le16 stopped_controlees_short_addr[FIRA_CONTROLEES_MAX];
+	struct fira_diagnostic diagnostics[FIRA_FRAMES_MAX];
 	/**
-	 * @n_stopped_controlees_short_addr: Number of elements in the stopped
-	 * controlees short addr table.
+	 * @stopped_controlees: Short addresses of the stopped controlees for
+	 * which an element must be added to the Device Management List of
+	 * the control message.
 	 */
-	int n_stopped_controlees_short_addr;
+	__le16 stopped_controlees[FIRA_CONTROLEES_MAX];
 	/**
-	 * @block_duration_rx_margin_ppm: Block duration rx margin for
-	 * controlees.
+	 * @n_stopped_controlees: Number of elements in the stopped controlees .
 	 */
-	int block_duration_rx_margin_ppm;
+	int n_stopped_controlees;
 };
 
 static inline struct fira_local *
@@ -325,12 +387,24 @@ access_to_local(struct mcps802154_access *access)
 }
 
 /**
- * fira_report() - Report state change or ranging result for a session.
+ * fira_get_session_by_session_id() - Get a session by its identifier.
+ * @local: FiRa context.
+ * @session_id: Session identifier.
+ *
+ * Return: The session or NULL if not found.
+ */
+struct fira_session *fira_get_session_by_session_id(struct fira_local *local,
+						    u32 session_id);
+
+/**
+ * fira_check_all_missed_ranging() - Check missed ranging round for all active
+ * session except the recent.
  * @local: FiRa context.
- * @session: Session to report. Report ranging result if current session.
- * @add_measurements: True to add measurements to report.
+ * @recent_session: FiRa session to not check in active list.
+ * @timestamp_dtu: Timestamp used to trig (or not) a report of ranging failure.
  */
-void fira_report(struct fira_local *local, struct fira_session *session,
-		 bool add_measurements);
+void fira_check_all_missed_ranging(struct fira_local *local,
+				   const struct fira_session *recent_session,
+				   u32 timestamp_dtu);
 
 #endif /* NET_FIRA_REGION_H */
diff --git a/mac/fira_region_call.c b/mac/fira_region_call.c
index 2a66ab1..1438d4b 100644
--- a/mac/fira_region_call.c
+++ b/mac/fira_region_call.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the UWB stack for linux.
  *
- * Copyright (c) 2020-2021 Qorvo US, Inc.
+ * 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.
@@ -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 },
@@ -43,101 +44,61 @@ static const struct nla_policy fira_call_nla_policy[FIRA_CALL_ATTR_MAX + 1] = {
 
 static const struct nla_policy fira_session_param_nla_policy[FIRA_SESSION_PARAM_ATTR_MAX +
 							     1] = {
-	[FIRA_SESSION_PARAM_ATTR_DEVICE_TYPE] = {
-		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_DEVICE_TYPE_CONTROLLER,
-	},
-	[FIRA_SESSION_PARAM_ATTR_DEVICE_ROLE] = {
-		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_DEVICE_ROLE_INITIATOR,
-	},
-	[FIRA_SESSION_PARAM_ATTR_RANGING_ROUND_USAGE] = {
-		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_RANGING_ROUND_USAGE_DSTWR,
-	},
-	[FIRA_SESSION_PARAM_ATTR_MULTI_NODE_MODE] = {
-		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_MULTI_NODE_MODE_MANY_TO_MANY,
-	},
+	[FIRA_SESSION_PARAM_ATTR_DEVICE_TYPE] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_DEVICE_TYPE_CONTROLLER),
+	[FIRA_SESSION_PARAM_ATTR_DEVICE_ROLE] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_DEVICE_ROLE_INITIATOR),
+	[FIRA_SESSION_PARAM_ATTR_RANGING_ROUND_USAGE] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_RANGING_ROUND_USAGE_DSTWR),
+	[FIRA_SESSION_PARAM_ATTR_MULTI_NODE_MODE] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_MULTI_NODE_MODE_MANY_TO_MANY),
 	[FIRA_SESSION_PARAM_ATTR_SHORT_ADDR] = { .type = NLA_U16 },
 	[FIRA_SESSION_PARAM_ATTR_DESTINATION_SHORT_ADDR] = { .type = NLA_U16 },
 	[FIRA_SESSION_PARAM_ATTR_INITIATION_TIME_MS] = { .type = NLA_U32 },
 	[FIRA_SESSION_PARAM_ATTR_SLOT_DURATION_RSTU] = { .type = NLA_U32 },
 	[FIRA_SESSION_PARAM_ATTR_BLOCK_DURATION_MS] = { .type = NLA_U32 },
 	[FIRA_SESSION_PARAM_ATTR_ROUND_DURATION_SLOTS] = { .type = NLA_U32 },
-	[FIRA_SESSION_PARAM_ATTR_BLOCK_STRIDE_LENGTH] = {
-		.type = NLA_U32, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_BLOCK_STRIDE_LEN_MAX, },
+	[FIRA_SESSION_PARAM_ATTR_BLOCK_STRIDE_LENGTH] =
+		NLA_POLICY_MAX(NLA_U32, FIRA_BLOCK_STRIDE_LEN_MAX),
 	[FIRA_SESSION_PARAM_ATTR_MAX_NUMBER_OF_MEASUREMENTS] = { .type = NLA_U32 },
 	[FIRA_SESSION_PARAM_ATTR_MAX_RR_RETRY] = { .type = NLA_U32 },
-	[FIRA_SESSION_PARAM_ATTR_ROUND_HOPPING] = {
-		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_BOOLEAN_MAX,
-	},
-	[FIRA_SESSION_PARAM_ATTR_PRIORITY] = {
-		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_PRIORITY_MAX,
-	},
-	[FIRA_SESSION_PARAM_ATTR_RESULT_REPORT_PHASE] = {
-		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_BOOLEAN_MAX,
-	},
-	[FIRA_SESSION_PARAM_ATTR_MR_AT_INITIATOR] = {
-		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_MEASUREMENT_REPORT_AT_INITIATOR,
-	},
-	[FIRA_SESSION_PARAM_ATTR_EMBEDDED_MODE] = {
-		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_EMBEDDED_MODE_NON_DEFERRED,
-	},
-	[FIRA_SESSION_PARAM_ATTR_IN_BAND_TERMINATION_ATTEMPT_COUNT] = {
-		.type = NLA_U32, .validation_type = NLA_VALIDATE_RANGE,
-		.min = FIRA_IN_BAND_TERMINATION_ATTEMPT_COUNT_MIN,
-		.max = FIRA_IN_BAND_TERMINATION_ATTEMPT_COUNT_MAX,
-	},
+	[FIRA_SESSION_PARAM_ATTR_ROUND_HOPPING] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_BOOLEAN_MAX),
+	[FIRA_SESSION_PARAM_ATTR_PRIORITY] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_PRIORITY_MAX),
+	[FIRA_SESSION_PARAM_ATTR_RESULT_REPORT_PHASE] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_BOOLEAN_MAX),
+	[FIRA_SESSION_PARAM_ATTR_MR_AT_INITIATOR] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_MEASUREMENT_REPORT_AT_INITIATOR),
+	[FIRA_SESSION_PARAM_ATTR_EMBEDDED_MODE] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_EMBEDDED_MODE_NON_DEFERRED),
+	[FIRA_SESSION_PARAM_ATTR_IN_BAND_TERMINATION_ATTEMPT_COUNT] =
+		NLA_POLICY_RANGE(NLA_U32,
+				 FIRA_IN_BAND_TERMINATION_ATTEMPT_COUNT_MIN,
+				 FIRA_IN_BAND_TERMINATION_ATTEMPT_COUNT_MAX),
 	[FIRA_SESSION_PARAM_ATTR_CHANNEL_NUMBER] = { .type = NLA_U8 },
 	[FIRA_SESSION_PARAM_ATTR_PREAMBLE_CODE_INDEX] = { .type = NLA_U8 },
-	[FIRA_SESSION_PARAM_ATTR_RFRAME_CONFIG] = {
-		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_RFRAME_CONFIG_SP3,
-	},
-	[FIRA_SESSION_PARAM_ATTR_PRF_MODE] = {
-		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_PRF_MODE_HPRF,
-	},
-	[FIRA_SESSION_PARAM_ATTR_PREAMBLE_DURATION] = {
-		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_PREAMBULE_DURATION_64,
-	},
-	[FIRA_SESSION_PARAM_ATTR_SFD_ID] = {
-		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_SFD_ID_4,
-	},
-	[FIRA_SESSION_PARAM_ATTR_NUMBER_OF_STS_SEGMENTS] = {
-		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_STS_SEGMENTS_2,
-	},
-	[FIRA_SESSION_PARAM_ATTR_PSDU_DATA_RATE] = {
-		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_PSDU_DATA_RATE_31M2,
-	},
-	[FIRA_SESSION_PARAM_ATTR_BPRF_PHR_DATA_RATE] = {
-		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_PHR_DATA_RATE_6M81,
-	},
-	[FIRA_SESSION_PARAM_ATTR_MAC_FCS_TYPE] = {
-		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_MAC_FCS_TYPE_CRC_32,
-	},
-	[FIRA_SESSION_PARAM_ATTR_TX_ADAPTIVE_PAYLOAD_POWER] = {
-		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_BOOLEAN_MAX,
-	},
+	[FIRA_SESSION_PARAM_ATTR_RFRAME_CONFIG] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_RFRAME_CONFIG_SP3),
+	[FIRA_SESSION_PARAM_ATTR_PRF_MODE] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_PRF_MODE_HPRF_HIGH_RATE),
+	[FIRA_SESSION_PARAM_ATTR_PREAMBLE_DURATION] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_PREAMBULE_DURATION_64),
+	[FIRA_SESSION_PARAM_ATTR_SFD_ID] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_SFD_ID_4),
+	[FIRA_SESSION_PARAM_ATTR_NUMBER_OF_STS_SEGMENTS] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_STS_SEGMENTS_4),
+	[FIRA_SESSION_PARAM_ATTR_PSDU_DATA_RATE] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_PSDU_DATA_RATE_31M2),
+	[FIRA_SESSION_PARAM_ATTR_BPRF_PHR_DATA_RATE] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_PHR_DATA_RATE_6M81),
+	[FIRA_SESSION_PARAM_ATTR_MAC_FCS_TYPE] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_MAC_FCS_TYPE_CRC_32),
+	[FIRA_SESSION_PARAM_ATTR_TX_ADAPTIVE_PAYLOAD_POWER] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_BOOLEAN_MAX),
 	[FIRA_SESSION_PARAM_ATTR_MEASUREMENT_SEQUENCE] = { .type = NLA_NESTED_ARRAY },
-	[FIRA_SESSION_PARAM_ATTR_STS_CONFIG] = {
-		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_STS_CONFIG_DYNAMIC_INDIVIDUAL_KEY,
-	},
+	[FIRA_SESSION_PARAM_ATTR_STS_CONFIG] =
+		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),
@@ -145,82 +106,67 @@ static const struct nla_policy fira_session_param_nla_policy[FIRA_SESSION_PARAM_
 		.type = NLA_BINARY, .len = FIRA_KEY_SIZE_MAX, },
 	[FIRA_SESSION_PARAM_ATTR_SUB_SESSION_KEY] = {
 		.type = NLA_BINARY, .len = FIRA_KEY_SIZE_MAX, },
-	[FIRA_SESSION_PARAM_ATTR_KEY_ROTATION] = {
-		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_BOOLEAN_MAX, },
+	[FIRA_SESSION_PARAM_ATTR_KEY_ROTATION] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_BOOLEAN_MAX),
 	[FIRA_SESSION_PARAM_ATTR_KEY_ROTATION_RATE] = { .type = NLA_U8 },
-	[FIRA_SESSION_PARAM_ATTR_AOA_RESULT_REQ] = {
-		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_BOOLEAN_MAX,
-	},
-	[FIRA_SESSION_PARAM_ATTR_REPORT_TOF] = {
-		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_BOOLEAN_MAX,
-	},
-	[FIRA_SESSION_PARAM_ATTR_REPORT_AOA_AZIMUTH] = {
-		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_BOOLEAN_MAX,
-	},
-	[FIRA_SESSION_PARAM_ATTR_REPORT_AOA_ELEVATION] = {
-		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_BOOLEAN_MAX,
-	},
-	[FIRA_SESSION_PARAM_ATTR_REPORT_AOA_FOM] = {
-		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = FIRA_BOOLEAN_MAX,
-	},
+	[FIRA_SESSION_PARAM_ATTR_AOA_RESULT_REQ] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_BOOLEAN_MAX),
+	[FIRA_SESSION_PARAM_ATTR_REPORT_TOF] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_BOOLEAN_MAX),
+	[FIRA_SESSION_PARAM_ATTR_REPORT_AOA_AZIMUTH] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_BOOLEAN_MAX),
+	[FIRA_SESSION_PARAM_ATTR_REPORT_AOA_ELEVATION] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_BOOLEAN_MAX),
+	[FIRA_SESSION_PARAM_ATTR_REPORT_AOA_FOM] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_BOOLEAN_MAX),
+	[FIRA_SESSION_PARAM_ATTR_REPORT_RSSI] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_RSSI_REPORT_AVERAGE),
 	[FIRA_SESSION_PARAM_ATTR_DATA_VENDOR_OUI] = { .type = NLA_U32 },
 	[FIRA_SESSION_PARAM_ATTR_DATA_PAYLOAD] = {
 		.type = NLA_BINARY, .len = FIRA_DATA_PAYLOAD_SIZE_MAX, },
+	[FIRA_SESSION_PARAM_ATTR_DIAGNOSTICS] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_BOOLEAN_MAX),
+	[FIRA_SESSION_PARAM_ATTR_DIAGNOSTICS_FRAME_REPORTS_FIELDS] = {.type = NLA_U32},
+	[FIRA_SESSION_PARAM_ATTR_STS_LENGTH] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_STS_LENGTH_128),
+	[FIRA_SESSION_PARAM_ATTR_RANGE_DATA_NTF_CONFIG] =
+		NLA_POLICY_MAX(NLA_U8, FIRA_RANGE_DATA_NTF_PROXIMITY),
+	[FIRA_SESSION_PARAM_ATTR_RANGE_DATA_NTF_PROXIMITY_NEAR] =
+		{ .type = NLA_U32 },
+	[FIRA_SESSION_PARAM_ATTR_RANGE_DATA_NTF_PROXIMITY_FAR] =
+		{ .type = NLA_U32 },
 };
 
 /**
- * get_session_state() - Get state of the session.
+ * fira_get_state_by_session_id() - Get state of the session.
  * @local: FiRa context.
- * @session_id: Fira session id.
+ * @session_id: FiRa session id.
  *
  * Return: current session state.
  */
-static enum fira_session_state get_session_state(struct fira_local *local,
-						 u32 session_id)
+static enum fira_session_state_id
+fira_get_state_by_session_id(struct fira_local *local, u32 session_id)
 {
-	struct fira_session *session;
-	bool active;
-	enum fira_session_state state;
-
-	/* Determine if session exists already. */
-	session = fira_session_get(local, session_id, &active);
-	if (!session) {
-		state = FIRA_SESSION_STATE_DEINIT;
-	} else {
-		/* Is session active? */
-		if (active) {
-			state = FIRA_SESSION_STATE_ACTIVE;
-		} else {
-			/* Is session ready? */
-			if (fira_session_is_ready(local, session))
-				state = FIRA_SESSION_STATE_IDLE;
-			else
-				state = FIRA_SESSION_STATE_INIT;
-		}
-	}
+	struct fira_session *session =
+		fira_get_session_by_session_id(local, session_id);
 
-	return state;
+	if (session)
+		return fira_session_get_state_id(session);
+	return FIRA_SESSION_STATE_ID_DEINIT;
 }
 
 /**
- * fira_session_init() - Initialize Fira session.
+ * fira_session_init() - Initialize FiRa session.
  * @local: FiRa context.
- * @session_id: Fira session id.
+ * @session_id: FiRa session id.
  *
  * Return: 0 or error.
  */
 static int fira_session_init(struct fira_local *local, u32 session_id)
 {
-	bool active;
-	struct fira_session *session;
+	struct fira_session *session =
+		fira_get_session_by_session_id(local, session_id);
 
-	session = fira_session_get(local, session_id, &active);
 	if (session)
 		return -EBUSY;
 
@@ -232,9 +178,9 @@ static int fira_session_init(struct fira_local *local, u32 session_id)
 }
 
 /**
- * fira_session_start() - Start Fira session.
+ * fira_session_start() - Start FiRa session.
  * @local: FiRa context.
- * @session_id: Fira session id.
+ * @session_id: FiRa session id.
  * @info: Request information.
  *
  * Return: 0 or error.
@@ -242,191 +188,51 @@ static int fira_session_init(struct fira_local *local, u32 session_id)
 static int fira_session_start(struct fira_local *local, u32 session_id,
 			      const struct genl_info *info)
 {
-	int n;
-	bool active;
 	struct fira_session *session;
 
-	session = fira_session_get(local, session_id, &active);
+	session = fira_get_session_by_session_id(local, session_id);
 	if (!session)
 		return -ENOENT;
 
-	trace_region_fira_session_params(session, &session->params);
-	for (n = 0; n < session->params.meas_seq.active->n_steps; n++)
-		trace_region_fira_meas_seq_step(
-			session, &(session->params.meas_seq.active->steps[n]),
-			n);
-
-	if (!fira_session_is_ready(local, session))
-		return -EINVAL;
-
-	session->event_portid = info->snd_portid;
-
-	if (!active) {
-		u32 now_dtu;
-		int initiation_time_dtu;
-		int block_stride_len;
-		int r;
-
-		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;
-
-		if (session->params.initiation_time_ms) {
-			initiation_time_dtu =
-				session->params.initiation_time_ms *
-				(local->llhw->dtu_freq_hz / 1000);
-		} else if (session->params.device_type ==
-				   FIRA_DEVICE_TYPE_CONTROLLER &&
-			   local->llhw->anticip_dtu) {
-			/* In order to be able to generate a frame for
-			   sts_index = sts_index_init, add anticip_dtu two
-			   times, for mcps802154_fproc_access_now and for
-			   fira_get_access.
-			   Also add 5 ms delay since now_dtu will change between
-			   fira_session_start and fira_get_access. */
-			initiation_time_dtu =
-				2 * local->llhw->anticip_dtu +
-				5 * (local->llhw->dtu_freq_hz / 1000);
-		} else {
-			initiation_time_dtu = 0;
-		}
-
-		if (session->params.device_type == FIRA_DEVICE_TYPE_CONTROLLER)
-			block_stride_len = session->params.block_stride_len;
-		else
-			block_stride_len = 0;
-
-		session->block_start_dtu = now_dtu + initiation_time_dtu;
-		session->block_index = 0;
-		session->sts_index = session->crypto.sts_index_init;
-		session->hopping_sequence_generation =
-			session->params.round_hopping;
-		session->round_index = 0;
-		session->next_round_index = 0;
-		session->block_stride_len = block_stride_len;
-		session->next_block_stride_len = block_stride_len;
-		session->synchronised = false;
-		session->last_access_timestamp_dtu = -1;
-		session->last_access_duration_dtu = 0;
-		session->last_block_index = -(block_stride_len + 1);
-		fira_session_update_round_index(session);
-		session->number_of_measurements = 0;
-
-		session->params.meas_seq.current_step = 0;
-		session->params.meas_seq.n_measurements_achieved = 0;
-
-		list_move(&session->entry, &local->active_sessions);
-
-		mcps802154_reschedule(local->llhw);
-	}
-
-	return 0;
+	return fira_session_fsm_start(local, session, info);
 }
 
 /**
- * fira_session_stop() - Stop Fira session.
+ * fira_session_stop() - Stop FiRa session.
  * @local: FiRa context.
- * @session_id: Fira session id.
+ * @session_id: FiRa session id.
  *
  * Return: 0 or error.
  */
 static int fira_session_stop(struct fira_local *local, u32 session_id)
 {
-	bool active;
 	struct fira_session *session;
 
-	session = fira_session_get(local, session_id, &active);
+	session = fira_get_session_by_session_id(local, session_id);
 	if (!session)
 		return -ENOENT;
 
-	if (active) {
-		session->stop_request = true;
-		if (session->params.device_type == FIRA_DEVICE_TYPE_CONTROLEE) {
-			if (local->current_session != session)
-				fira_session_access_done(local, session, false);
-			else
-				mcps802154_reschedule(local->llhw);
-		} else {
-			if (local->current_session == NULL ||
-			    (local->current_session != session &&
-			     !session->current_controlees.size)) {
-				fira_session_access_done(local, session, false);
-			} else {
-				if (session->controlee_management_flags) {
-					/* Many changes on same round or while a controlee is stopping is not supported. */
-					return -EBUSY;
-				}
-				fira_session_copy_controlees(
-					&session->new_controlees,
-					&session->current_controlees);
-				fira_session_stop_controlees(
-					session, &session->new_controlees);
-				session->controlee_management_flags =
-					FIRA_SESSION_CONTROLEE_MANAGEMENT_FLAG_UPDATE |
-					FIRA_SESSION_CONTROLEE_MANAGEMENT_FLAG_STOP;
-			}
-		}
-	}
-
-	return 0;
+	return fira_session_fsm_stop(local, session);
 }
 
 /**
- * fira_session_deinit() - Deinitialize Fira session.
+ * fira_session_deinit() - Deinitialize FiRa session.
  * @local: FiRa context.
- * @session_id: Fira session id.
+ * @session_id: FiRa session id.
  *
  * Return: 0 or error.
  */
 static int fira_session_deinit(struct fira_local *local, u32 session_id)
 {
-	bool active;
-	struct fira_session *session;
+	struct fira_session *session =
+		fira_get_session_by_session_id(local, session_id);
 
-	session = fira_session_get(local, session_id, &active);
 	if (!session)
 		return -ENOENT;
-	if (active)
+	if (fira_session_is_active(session))
 		return -EBUSY;
 
-	fira_session_free(session);
-	return 0;
-}
-
-static int is_allowed_param_active(enum fira_device_type dev_type, int param)
-{
-	/* These arrays contain attributes which can be changed in an active
-	   controller or controlee session */
-	static const int allowed_controller_param_active[] = {
-		FIRA_SESSION_PARAM_ATTR_DATA_PAYLOAD,
-		FIRA_SESSION_PARAM_ATTR_BLOCK_STRIDE_LENGTH,
-		FIRA_SESSION_PARAM_ATTR_MEASUREMENT_SEQUENCE,
-	};
-	static const int allowed_controlee_param_active[] = {
-		FIRA_SESSION_PARAM_ATTR_DATA_PAYLOAD,
-		FIRA_SESSION_PARAM_ATTR_MEASUREMENT_SEQUENCE,
-	};
-	const int *allowed_param_active =
-		dev_type == FIRA_DEVICE_TYPE_CONTROLLER ?
-			allowed_controller_param_active :
-			allowed_controlee_param_active;
-	const int num_allowed =
-		dev_type == FIRA_DEVICE_TYPE_CONTROLLER ?
-			ARRAY_SIZE(allowed_controller_param_active) :
-			ARRAY_SIZE(allowed_controlee_param_active);
-	int i;
-
-	for (i = 0; i < num_allowed; i++)
-		if (allowed_param_active[i] == param)
-			return 1;
-
+	fira_session_free(local, session);
 	return 0;
 }
 
@@ -456,13 +262,11 @@ static int fira_session_params_set_measurement_sequence_step(
 		[STEP_ATTR(TX_ANT_SET_NONRANGING)] = { .type = NLA_U8 },
 		[STEP_ATTR(TX_ANT_SET_RANGING)] = { .type = NLA_U8 },
 	};
-
 	static const struct nla_policy
 		rx_ant_sets_ranging_policy[ASR_ATTR(MAX) + 1] = {
 			[ASR_ATTR(0)] = { .type = NLA_U8 },
 			[ASR_ATTR(1)] = { .type = NLA_U8 },
 		};
-
 	struct nlattr *step_attrs[STEP_ATTR(MAX) + 1];
 	struct nlattr *rx_ant_sets_attrs[ASR_ATTR(MAX) + 1];
 	int r = 0;
@@ -473,12 +277,11 @@ static int fira_session_params_set_measurement_sequence_step(
 	r = nla_parse_nested(step_attrs, STEP_ATTR(MAX), params,
 			     meas_seq_step_policy, info->extack);
 	/* LCOV_EXCL_START */
-	/* defensive check, should not happen */
 	if (r)
 		return r;
 	/* LCOV_EXCL_STOP */
 
-	memset((void *)step, 0, sizeof(struct fira_measurement_sequence_step));
+	memset(step, 0, sizeof(struct fira_measurement_sequence_step));
 	if (!step_attrs[STEP_ATTR(MEASUREMENT_TYPE)] ||
 	    !step_attrs[STEP_ATTR(N_MEASUREMENTS)])
 		return -EINVAL;
@@ -503,11 +306,13 @@ static int fira_session_params_set_measurement_sequence_step(
 	GET_ANTENNA(step_attrs[STEP_ATTR(TX_ANT_SET_RANGING)],
 		    step->tx_ant_set_ranging);
 
+	if (!step_attrs[STEP_ATTR(RX_ANT_SETS_RANGING)])
+		return -EINVAL;
+
 	r = nla_parse_nested(rx_ant_sets_attrs, ASR_ATTR(MAX),
 			     step_attrs[STEP_ATTR(RX_ANT_SETS_RANGING)],
 			     rx_ant_sets_ranging_policy, info->extack);
 	/* LCOV_EXCL_START */
-	/* defensive check, should not happen */
 	if (r)
 		return r;
 	/* LCOV_EXCL_STOP */
@@ -548,41 +353,58 @@ static int fira_session_params_set_measurement_sequence(
 	const struct nlattr *params, const struct genl_info *info,
 	struct fira_measurement_sequence *meas_seq)
 {
-	struct nlattr *request = NULL;
+	struct nlattr *request;
 	int r, rem = 0;
 	size_t n_steps = 0;
 
-	/* LCOV_EXCL_START */
-	/* defensive check, not easy to trigger using unit test */
-	if (!params)
-		return -EINVAL;
-	if (!meas_seq)
-		return -EINVAL;
-	/* LCOV_EXCL_STOP */
-
 	nla_for_each_nested (request, params, rem) {
 		if (n_steps >= FIRA_MEASUREMENT_SEQUENCE_STEP_MAX)
 			return -EINVAL;
-
 		r = fira_session_params_set_measurement_sequence_step(
-			request, info, meas_seq->steps + n_steps);
+			request, info, &meas_seq->steps[n_steps]);
 		if (r)
 			return r;
-
-		++n_steps;
+		n_steps++;
 	}
-
-	if (0 == n_steps)
+	if (!n_steps)
 		return -EINVAL;
 	meas_seq->n_steps = n_steps;
+	return 0;
+}
 
+/**
+ * check_parameter_proximity_range() - Check proximity range concistency.
+ * @params: Current session parameters.
+ * @set_attrs: Updated session parameters.
+ *
+ * Return: 0 or error.
+ */
+static inline int
+check_parameter_proximity_range(const struct fira_session_params *params,
+				struct nlattr *const *set_attrs)
+{
+	uint32_t proximity_near = params->range_data_ntf_proximity_near_mm;
+	uint32_t proximity_far = params->range_data_ntf_proximity_far_mm;
+	const struct nlattr *near_attr =
+		set_attrs[FIRA_SESSION_PARAM_ATTR_RANGE_DATA_NTF_PROXIMITY_NEAR];
+	const struct nlattr *far_attr =
+		set_attrs[FIRA_SESSION_PARAM_ATTR_RANGE_DATA_NTF_PROXIMITY_FAR];
+	if (near_attr) {
+		proximity_near = nla_get_u32(near_attr);
+	}
+	if (far_attr) {
+		proximity_far = nla_get_u32(far_attr);
+	}
+	if (proximity_near > proximity_far) {
+		return -ERANGE;
+	}
 	return 0;
 }
 
 /**
- * fira_session_set_parameters() - Set Fira session parameters.
+ * fira_session_set_parameters() - Set FiRa session parameters.
  * @local: FiRa context.
- * @session_id: Fira session id.
+ * @session_id: FiRa session id.
  * @params: Nested attribute containing session parameters.
  * @info: Request information.
  *
@@ -593,38 +415,39 @@ static int fira_session_set_parameters(struct fira_local *local, u32 session_id,
 				       const struct genl_info *info)
 {
 	struct nlattr *attrs[FIRA_SESSION_PARAM_ATTR_MAX + 1];
-	bool active;
 	struct fira_session *session;
 	struct fira_session_params *p;
-	struct fira_measurement_sequence *meas_seq;
+	struct fira_measurement_sequence meas_seq = {};
 	int r;
 
-	session = fira_session_get(local, session_id, &active);
-	if (!session)
-		return -ENOENT;
 	if (!params)
 		return -EINVAL;
 
+	session = fira_get_session_by_session_id(local, session_id);
+	if (!session)
+		return -ENOENT;
+
 	r = nla_parse_nested(attrs, FIRA_SESSION_PARAM_ATTR_MAX, params,
 			     fira_session_param_nla_policy, info->extack);
 	if (r)
 		return r;
+	/* Check attribute validity. */
+	r = check_parameter_proximity_range(&session->params, attrs);
+	if (r)
+		return r;
 
-	p = &session->params;
-	if (p->meas_seq.active == &(p->_meas_seq_1))
-		meas_seq = &(p->_meas_seq_2);
-	else
-		meas_seq = &(p->_meas_seq_1);
-
-	if (active) {
-		int i;
-		for (i = FIRA_SESSION_PARAM_ATTR_UNSPEC + 1;
-		     i <= FIRA_SESSION_PARAM_ATTR_MAX; i++) {
-			if (attrs[i] &&
-			    !is_allowed_param_active(p->device_type, i))
-				return -EBUSY;
-		}
+	if (attrs[FIRA_SESSION_PARAM_ATTR_MEASUREMENT_SEQUENCE]) {
+		r = fira_session_params_set_measurement_sequence(
+			attrs[FIRA_SESSION_PARAM_ATTR_MEASUREMENT_SEQUENCE],
+			info, &meas_seq);
+		if (r)
+			return r;
 	}
+	r = fira_session_fsm_check_parameters(session, attrs);
+	if (r)
+		return r;
+
+	p = &session->params;
 #define P(attr, member, type, conv)                                     \
 	do {                                                            \
 		int x;                                                  \
@@ -678,59 +501,72 @@ static int fira_session_set_parameters(struct fira_local *local, u32 session_id,
 	P(RFRAME_CONFIG, rframe_config, u8, x);
 	P(PREAMBLE_DURATION, preamble_duration, u8, x);
 	P(SFD_ID, sfd_id, u8, x);
+	P(NUMBER_OF_STS_SEGMENTS, number_of_sts_segments, u8, x);
 	P(PSDU_DATA_RATE, psdu_data_rate, u8, x);
 	P(MAC_FCS_TYPE, mac_fcs_type, u8, x);
+	P(PRF_MODE, prf_mode, u8, x);
+	P(BPRF_PHR_DATA_RATE, phr_data_rate, u8, x);
 	/* Measurement Sequence */
 	if (attrs[FIRA_SESSION_PARAM_ATTR_MEASUREMENT_SEQUENCE]) {
-		if (fira_session_params_set_measurement_sequence(
-			    attrs[FIRA_SESSION_PARAM_ATTR_MEASUREMENT_SEQUENCE],
-			    info, meas_seq)) {
-			return -EINVAL;
-		} else if (!active) {
-			/* immediate update */
-			p->meas_seq.active = meas_seq;
-		} else {
-			/* deferred update when session is running */
-			p->meas_seq.update_flag = true;
-		}
+		p->meas_seq = meas_seq;
+		session->measurements.reset = true;
 	}
 	/* 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);
 	P(REPORT_AOA_AZIMUTH, report_aoa_azimuth, u8, !!x);
 	P(REPORT_AOA_ELEVATION, report_aoa_elevation, u8, !!x);
 	P(REPORT_AOA_FOM, report_aoa_fom, u8, !!x);
+	P(REPORT_RSSI, report_rssi, u8, x);
 	/* Custom data */
 	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. */
-	if (attrs[FIRA_SESSION_PARAM_ATTR_DATA_PAYLOAD]) {
+	/* Increment payload sequence number if a new data is received. */
+	if (attrs[FIRA_SESSION_PARAM_ATTR_DATA_PAYLOAD])
 		p->data_payload_seq++;
-	}
-	/* TODO: set all fira session parameters. */
-#undef P
-#undef PMEMCPY
+	/* Diagnostics */
+	P(DIAGNOSTICS, report_diagnostics, u8, x);
+	P(DIAGNOSTICS_FRAME_REPORTS_FIELDS, diagnostic_report_flags, u32, x);
+	/* Misc */
+	P(STS_LENGTH, sts_length, u8, x);
+	P(RANGE_DATA_NTF_CONFIG, range_data_ntf_config, u8, x);
+	P(RANGE_DATA_NTF_PROXIMITY_NEAR, range_data_ntf_proximity_near_mm, u32,
+	  x);
+	P(RANGE_DATA_NTF_PROXIMITY_FAR, range_data_ntf_proximity_far_mm, u32,
+	  x);
 #undef PMEMNCPY
+#undef PMEMCPY
+#undef P
 
+	fira_session_fsm_parameters_updated(local, session);
 	return 0;
 }
 
 /**
  * fira_session_get_state() - Get state of the session.
  * @local: FiRa context.
- * @session_id: Fira session id.
+ * @session_id: FiRa session id.
  *
  * Return: 0 or error.
  */
 static int fira_session_get_state(struct fira_local *local, u32 session_id)
 {
 	struct sk_buff *msg;
-	enum fira_session_state state;
+	enum fira_session_state_id state;
 
-	state = get_session_state(local, session_id);
+	state = fira_get_state_by_session_id(local, session_id);
 
 	msg = mcps802154_region_call_alloc_reply_skb(
 		local->llhw, &local->region, FIRA_CALL_SESSION_GET_STATE,
@@ -821,12 +657,6 @@ static int fira_session_params_get_measurement_sequence(
 	struct nlattr *meas_seq_params = NULL;
 	size_t i;
 
-	/* LCOV_EXCL_START */
-	/* defensive check, should not happen */
-	if (!meas_seq || !msg_buf)
-		return -EINVAL;
-	/* LCOV_EXCL_STOP */
-
 	meas_seq_params = nla_nest_start(
 		msg_buf, FIRA_SESSION_PARAM_ATTR_MEASUREMENT_SEQUENCE);
 	if (!meas_seq_params)
@@ -846,26 +676,24 @@ static int fira_session_params_get_measurement_sequence(
 }
 
 /**
- * fira_session_get_parameters() - Get Fira session parameters.
+ * fira_session_get_parameters() - Get FiRa session parameters.
  * @local: FiRa context.
- * @session_id: Fira session id.
+ * @session_id: FiRa session id.
  *
  * Return: 0 or error.
  */
 static int fira_session_get_parameters(struct fira_local *local, u32 session_id)
 {
-	bool active;
 	const struct fira_session *session;
 	const struct fira_session_params *p;
 	struct sk_buff *msg;
 	struct nlattr *params;
 
-	session = fira_session_get(local, session_id, &active);
+	session = fira_get_session_by_session_id(local, session_id);
 	if (!session)
 		return -ENOENT;
 
 	p = &session->params;
-
 	msg = mcps802154_region_call_alloc_reply_skb(
 		local->llhw, &local->region, FIRA_CALL_SESSION_GET_PARAMS,
 		NLMSG_DEFAULT_SIZE);
@@ -921,24 +749,40 @@ static int fira_session_get_parameters(struct fira_local *local, u32 session_id)
 	P(RFRAME_CONFIG, rframe_config, u8, x);
 	P(PREAMBLE_DURATION, preamble_duration, u8, x);
 	P(SFD_ID, sfd_id, u8, x);
+	P(NUMBER_OF_STS_SEGMENTS, number_of_sts_segments, u8, x);
 	P(PSDU_DATA_RATE, psdu_data_rate, u8, x);
+	P(PRF_MODE, prf_mode, u8, x);
+	P(BPRF_PHR_DATA_RATE, phr_data_rate, u8, x);
 	P(MAC_FCS_TYPE, mac_fcs_type, u8, x);
 	/* Measurement Sequence */
 	if (fira_session_params_get_measurement_sequence(
-		    session->params.meas_seq.active, msg))
+		    &session->measurements.sequence, msg))
 		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);
 	P(REPORT_AOA_AZIMUTH, report_aoa_azimuth, u8, !!x);
 	P(REPORT_AOA_ELEVATION, report_aoa_elevation, u8, !!x);
 	P(REPORT_AOA_FOM, report_aoa_fom, u8, !!x);
+	P(REPORT_RSSI, report_rssi, u8, !!x);
 	/* Custom data */
 	if (p->data_vendor_oui)
 		P(DATA_VENDOR_OUI, data_vendor_oui, u32, x);
+	/* Diagnostics */
+	P(DIAGNOSTICS, report_diagnostics, u8, x);
+	P(DIAGNOSTICS_FRAME_REPORTS_FIELDS, diagnostic_report_flags, u32, x);
+	/* Misc */
+	P(STS_LENGTH, sts_length, u8, x);
+	P(RANGE_DATA_NTF_CONFIG, range_data_ntf_config, u8, x);
+	P(RANGE_DATA_NTF_PROXIMITY_NEAR, range_data_ntf_proximity_near_mm, u32,
+	  x);
+	P(RANGE_DATA_NTF_PROXIMITY_FAR, range_data_ntf_proximity_far_mm, u32,
+	  x);
 #undef P
 #undef PMEMCPY
 
@@ -953,14 +797,15 @@ nla_put_failure:
 /**
  * fira_manage_controlees() - Manage controlees.
  * @local: FiRa context.
- * @call_id: Fira call id.
- * @session_id: Fira session id.
+ * @call_id: FiRa call id.
+ * @session_id: FiRa session id.
  * @params: Nested attribute containing controlee parameters.
  * @info: Request information.
  * Return: 0 or error.
  */
-static int fira_manage_controlees(struct fira_local *local, u32 call_id,
-				  u32 session_id, const struct nlattr *params,
+static int fira_manage_controlees(struct fira_local *local,
+				  enum fira_call call_id, u32 session_id,
+				  const struct nlattr *params,
 				  const struct genl_info *info)
 {
 	static const struct nla_policy new_controlee_nla_policy[FIRA_CALL_CONTROLEE_ATTR_MAX +
@@ -971,121 +816,148 @@ static int fira_manage_controlees(struct fira_local *local, u32 call_id,
 							       .len = FIRA_KEY_SIZE_MAX },
 	};
 	struct nlattr *request;
-	struct fira_controlee controlees[FIRA_CONTROLEES_MAX];
 	struct nlattr *attrs[FIRA_CALL_CONTROLEE_ATTR_MAX + 1];
 	int r, rem, i, n_controlees = 0;
 	struct fira_session *session;
-	struct fira_controlees_array *controlees_array;
-	bool active;
+	struct fira_controlee *controlee, *tmp_controlee;
+	bool is_active;
+	struct list_head controlees;
 
 	if (!params)
 		return -EINVAL;
 
+	session = fira_get_session_by_session_id(local, session_id);
+	if (!session)
+		return -ENOENT;
+
+	INIT_LIST_HEAD(&controlees);
+
 	nla_for_each_nested (request, params, rem) {
-		if (n_controlees >= FIRA_CONTROLEES_MAX)
-			return -EINVAL;
+		if (n_controlees >= FIRA_CONTROLEES_MAX) {
+			r = -EINVAL;
+			goto end;
+		}
 
 		r = nla_parse_nested(attrs, FIRA_CALL_CONTROLEE_ATTR_MAX,
 				     request, new_controlee_nla_policy,
 				     info->extack);
 		if (r)
-			return r;
+			goto end;
+
+		controlee = kzalloc(sizeof(struct fira_controlee), GFP_KERNEL);
+		if (!controlee) {
+			r = -ENOMEM;
+			goto end;
+		}
 
 		if (!attrs[FIRA_CALL_CONTROLEE_ATTR_SHORT_ADDR] ||
 		    (!attrs[FIRA_CALL_CONTROLEE_ATTR_SUB_SESSION_ID] ^
-		     !attrs[FIRA_CALL_CONTROLEE_ATTR_SUB_SESSION_KEY]))
-			return -EINVAL;
+		     !attrs[FIRA_CALL_CONTROLEE_ATTR_SUB_SESSION_KEY])) {
+			kfree(controlee);
+			r = -EINVAL;
+			goto end;
+		}
 
-		controlees[n_controlees].short_addr = nla_get_le16(
+		controlee->short_addr = nla_get_le16(
 			attrs[FIRA_CALL_CONTROLEE_ATTR_SHORT_ADDR]);
 		if (attrs[FIRA_CALL_CONTROLEE_ATTR_SUB_SESSION_ID]) {
-			if (call_id == FIRA_CALL_DEL_CONTROLEE)
-				return -EINVAL;
-			controlees[n_controlees].sub_session = true;
-			controlees[n_controlees].sub_session_id = nla_get_u32(
+			if (call_id == FIRA_CALL_DEL_CONTROLEE) {
+				kfree(controlee);
+				r = -EINVAL;
+				goto end;
+			}
+			controlee->sub_session = true;
+			controlee->sub_session_id = nla_get_u32(
 				attrs[FIRA_CALL_CONTROLEE_ATTR_SUB_SESSION_ID]);
-			memcpy(controlees[n_controlees].sub_session_key,
+			memcpy(controlee->sub_session_key,
 			       nla_data(
 				       attrs[FIRA_CALL_CONTROLEE_ATTR_SUB_SESSION_KEY]),
 			       nla_len(attrs[FIRA_CALL_CONTROLEE_ATTR_SUB_SESSION_KEY]));
-			controlees[n_controlees].sub_session_key_len = nla_len(
+			controlee->sub_session_key_len = nla_len(
 				attrs[FIRA_CALL_CONTROLEE_ATTR_SUB_SESSION_KEY]);
-		} else
-			controlees[n_controlees].sub_session = false;
-		controlees[n_controlees].state = FIRA_CONTROLEE_STATE_RUNNING;
-
-		for (i = 0; i < n_controlees; i++) {
-			if (controlees[n_controlees].short_addr ==
-			    controlees[i].short_addr)
-				return -EINVAL;
+		} else {
+			controlee->sub_session = false;
 		}
-
+		controlee->state = FIRA_CONTROLEE_STATE_RUNNING;
+		/* Check and reject a duplication of short_addr. */
+		list_for_each_entry (tmp_controlee, &controlees, entry) {
+			if (controlee->short_addr ==
+			    tmp_controlee->short_addr) {
+				kfree(controlee);
+				r = -EINVAL;
+				goto end;
+			}
+		}
+		list_add_tail(&controlee->entry, &controlees);
 		n_controlees++;
 	}
-	if (!n_controlees)
-		return -EINVAL;
+	if (list_empty(&controlees)) {
+		r = -EINVAL;
+		goto end;
+	}
 
-	session = fira_session_get(local, session_id, &active);
-	if (!session)
-		return -ENOENT;
+	/*
+	 * Be careful, active is not equal to
+	 * 'local->current_session == session'.
+	 */
+	is_active = fira_session_is_active(session);
 
-	if (session->controlee_management_flags) {
-		/* Many changes on same round or while a controlee is stopping is not supported. */
-		return -EBUSY;
-	} else if (active) {
+	if (is_active) {
 		/* If unicast refuse to add more than one controlee. */
-		if (call_id == FIRA_CALL_NEW_CONTROLEE &&
-		    session->params.multi_node_mode ==
-			    FIRA_MULTI_NODE_MODE_UNICAST &&
-		    (session->current_controlees.size > 0 || n_controlees > 1))
-			return -EINVAL;
-		if (call_id == FIRA_CALL_SET_CONTROLEE)
-			return -EBUSY;
-		fira_session_copy_controlees(&session->new_controlees,
-					     &session->current_controlees);
-		/* Use second array to not disturbe active session. */
-		controlees_array = &session->new_controlees;
-	} else {
-		/* No risk to disturbe this session. */
-		controlees_array = &session->current_controlees;
+		switch (call_id) {
+		case FIRA_CALL_NEW_CONTROLEE:
+			if (session->params.multi_node_mode ==
+				    FIRA_MULTI_NODE_MODE_UNICAST &&
+			    (!list_empty(&session->current_controlees) ||
+			     n_controlees > 1)) {
+				r = -EINVAL;
+				goto end;
+			}
+			break;
+		case FIRA_CALL_SET_CONTROLEE:
+			r = -EBUSY;
+			goto end;
+		default:
+			break;
+		}
 	}
 
 	switch (call_id) {
 	case FIRA_CALL_SET_CONTROLEE:
-		r = fira_session_set_controlees(local, session,
-						controlees_array, controlees,
+		r = fira_session_set_controlees(local, session, &controlees,
 						n_controlees);
 		break;
 	case FIRA_CALL_DEL_CONTROLEE:
-		if (active)
-			r = fira_session_async_del_controlees(session,
-							      controlees_array,
-							      controlees,
-							      n_controlees);
-		else
-			r = fira_session_del_controlees(
-				controlees_array, controlees, n_controlees);
+		r = fira_session_del_controlees(session, &controlees,
+						is_active);
 		break;
 	/* FIRA_CALL_NEW_CONTROLEE. */
 	default:
-		r = fira_session_new_controlees(session, active,
-						controlees_array, controlees,
-						n_controlees);
+		r = fira_session_new_controlees(session, &controlees,
+						n_controlees, is_active);
 	}
 	if (r)
-		return r;
+		goto end;
 
-	if (active)
-		session->controlee_management_flags |=
-			FIRA_SESSION_CONTROLEE_MANAGEMENT_FLAG_UPDATE;
+	if (!is_active && local->llhw->rx_ctx_size) {
+		for (i = 0; i < session->n_current_controlees; i++) {
+			memset(session->rx_ctx[i], 0, local->llhw->rx_ctx_size);
+		}
+	}
 
-	return 0;
+	fira_session_fsm_controlee_list_updated(local, session);
+end:
+	list_for_each_entry_safe (controlee, tmp_controlee, &controlees,
+				  entry) {
+		kfree(controlee);
+	}
+	return r;
 }
 
 /**
  * fira_session_get_controlees() - Get list of controlees.
  * @local: FiRa context.
- * @session_id: Fira session id.
+ * @session_id: FiRa session id.
  * @info: Request information.
  *
  * Return: 0 or error.
@@ -1093,13 +965,12 @@ static int fira_manage_controlees(struct fira_local *local, u32 call_id,
 static int fira_session_get_controlees(struct fira_local *local, u32 session_id,
 				       const struct genl_info *info)
 {
-	bool active;
 	const struct fira_session *session;
 	struct sk_buff *msg;
-	struct nlattr *controlees, *controlee;
-	int i;
+	struct nlattr *controlees, *controlee_attr;
+	struct fira_controlee *controlee;
 
-	session = fira_session_get(local, session_id, &active);
+	session = fira_get_session_by_session_id(local, session_id);
 	if (!session)
 		return -ENOENT;
 
@@ -1117,20 +988,18 @@ static int fira_session_get_controlees(struct fira_local *local, u32 session_id,
 	if (!controlees)
 		goto nla_put_failure;
 
-	for (i = 0; i < session->current_controlees.size; i++) {
-		const struct fira_controlee *c =
-			&session->current_controlees.data[i];
-		controlee = nla_nest_start(msg, 1);
-		if (!controlee)
+	list_for_each_entry (controlee, &session->current_controlees, entry) {
+		controlee_attr = nla_nest_start(msg, 1);
+		if (!controlee_attr)
 			goto nla_put_failure;
 		if (nla_put_le16(msg, FIRA_CALL_CONTROLEE_ATTR_SHORT_ADDR,
-				 c->short_addr))
+				 controlee->short_addr))
 			goto nla_put_failure;
-		if (c->sub_session &&
+		if (controlee->sub_session &&
 		    nla_put_u32(msg, FIRA_CALL_CONTROLEE_ATTR_SUB_SESSION_ID,
-				c->sub_session_id))
+				controlee->sub_session_id))
 			goto nla_put_failure;
-		nla_nest_end(msg, controlee);
+		nla_nest_end(msg, controlee_attr);
 	}
 
 	nla_nest_end(msg, controlees);
@@ -1146,6 +1015,8 @@ 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;
@@ -1174,6 +1045,16 @@ int fira_get_capabilities(struct fira_local *local,
 			goto nla_put_failure;                         \
 		}                                                     \
 	} while (0)
+#define C(name, hw_flag)                  \
+	do {                              \
+		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);
@@ -1192,25 +1073,46 @@ int fira_get_capabilities(struct fira_local *local,
 	P(CHANNEL_NUMBER, u16,
 	  local->llhw->hw->phy->supported
 		  .channels[local->llhw->hw->phy->current_page]);
-	F(RFRAME_CONFIG_SP1);
-	F(RFRAME_CONFIG_SP3);
-	F(PRF_MODE_BPRF);
-	F(PREAMBLE_DURATION_64);
-	F(SFD_ID_2);
+	C(RFRAME_CONFIG_SP1,
+	  MCPS802154_LLHW_STS_SEGMENT_1 | MCPS802154_LLHW_STS_SEGMENT_2);
+	C(RFRAME_CONFIG_SP3,
+	  MCPS802154_LLHW_STS_SEGMENT_1 | MCPS802154_LLHW_STS_SEGMENT_2);
+	C(PRF_MODE_BPRF, MCPS802154_LLHW_BPRF);
+	C(PRF_MODE_HPRF, MCPS802154_LLHW_HPRF);
+	C(PREAMBLE_DURATION_32, MCPS802154_LLHW_PSR_32);
+	C(PREAMBLE_DURATION_64, MCPS802154_LLHW_PSR_64);
+	C(SFD_ID_0, MCPS802154_LLHW_SFD_4A);
+	C(SFD_ID_1, MCPS802154_LLHW_SFD_4Z_4);
+	C(SFD_ID_2, MCPS802154_LLHW_SFD_4Z_8);
+	C(SFD_ID_3, MCPS802154_LLHW_SFD_4Z_16);
+	C(SFD_ID_4, MCPS802154_LLHW_SFD_4Z_32);
 	F(NUMBER_OF_STS_SEGMENTS_0);
-	F(NUMBER_OF_STS_SEGMENTS_1);
-	F(PSDU_DATA_RATE_6M81);
-	F(BPRF_PHR_DATA_RATE_850K);
+	C(NUMBER_OF_STS_SEGMENTS_1, MCPS802154_LLHW_STS_SEGMENT_1);
+	C(NUMBER_OF_STS_SEGMENTS_2, MCPS802154_LLHW_STS_SEGMENT_2);
+	C(NUMBER_OF_STS_SEGMENTS_3, MCPS802154_LLHW_STS_SEGMENT_3);
+	C(NUMBER_OF_STS_SEGMENTS_4, MCPS802154_LLHW_STS_SEGMENT_4);
+	C(PSDU_DATA_RATE_6M81, MCPS802154_LLHW_DATA_RATE_6M81);
+	C(PSDU_DATA_RATE_7M80, MCPS802154_LLHW_DATA_RATE_7M80);
+	C(PSDU_DATA_RATE_27M2, MCPS802154_LLHW_DATA_RATE_27M2);
+	C(PSDU_DATA_RATE_31M2, MCPS802154_LLHW_DATA_RATE_31M2);
+	C(BPRF_PHR_DATA_RATE_850K, MCPS802154_LLHW_PHR_DATA_RATE_850K);
+	C(BPRF_PHR_DATA_RATE_6M81, MCPS802154_LLHW_PHR_DATA_RATE_6M81);
 	F(TX_ADAPTIVE_PAYLOAD_POWER);
 	/* Antenna. */
 	P(RX_ANTENNA_PAIRS, u32, local->llhw->rx_antenna_pairs);
 	P(TX_ANTENNAS, u32, local->llhw->tx_antennas);
 	/* STS and crypto capabilities. */
-	F(STS_CONFIG_STATIC);
+	S(STATIC);
+	S(DYNAMIC);
+	S(DYNAMIC_INDIVIDUAL_KEY);
+	S(PROVISIONED);
+	S(PROVISIONED_INDIVIDUAL_KEY);
 	/* Report. */
-	F(AOA_AZIMUTH);
-	F(AOA_ELEVATION);
-	F(AOA_FOM);
+	C(AOA_AZIMUTH, MCPS802154_LLHW_AOA_AZIMUTH);
+	C(AOA_AZIMUTH_FULL, MCPS802154_LLHW_AOA_AZIMUTH_FULL);
+	C(AOA_ELEVATION, MCPS802154_LLHW_AOA_ELEVATION);
+	C(AOA_FOM, MCPS802154_LLHW_AOA_FOM);
+#undef C
 #undef F
 #undef P
 
@@ -1251,7 +1153,7 @@ nla_put_failure:
 	return -ENOBUFS;
 }
 
-int fira_session_control(struct fira_local *local, u32 call_id,
+int fira_session_control(struct fira_local *local, enum fira_call call_id,
 			 const struct nlattr *params,
 			 const struct genl_info *info)
 {
@@ -1275,6 +1177,7 @@ int fira_session_control(struct fira_local *local, u32 call_id,
 		return -EINVAL;
 
 	session_id = nla_get_u32(attrs[FIRA_CALL_ATTR_SESSION_ID]);
+	trace_region_fira_session_control(local, session_id, call_id);
 
 	switch (call_id) {
 	case FIRA_CALL_SESSION_INIT:
diff --git a/mac/fira_region_call.h b/mac/fira_region_call.h
index 778dcf3..670b6b3 100644
--- a/mac/fira_region_call.h
+++ b/mac/fira_region_call.h
@@ -1,7 +1,7 @@
 /*
  * This file is part of the UWB stack for linux.
  *
- * Copyright (c) 2020-2021 Qorvo US, Inc.
+ * 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.
@@ -26,7 +26,7 @@
 #include "fira_region.h"
 
 /**
- * fira_get_capabilities() - Get Fira capabilities.
+ * fira_get_capabilities() - Get FiRa capabilities.
  * @local: FiRa context.
  * @info: Request information.
  *
@@ -36,15 +36,15 @@ int fira_get_capabilities(struct fira_local *local,
 			  const struct genl_info *info);
 
 /**
- * fira_session_control() - Control Fira session.
+ * fira_session_control() - Control FiRa session.
  * @local: FiRa context.
- * @call_id: Identifier of the fira procedure.
+ * @call_id: Identifier of the FiRa procedure.
  * @params: Nested attribute containing procedure parameters.
  * @info: Request information.
  *
  * Return: 0 or error.
  */
-int fira_session_control(struct fira_local *local, u32 call_id,
+int fira_session_control(struct fira_local *local, enum fira_call call_id,
 			 const struct nlattr *params,
 			 const struct genl_info *info);
 
diff --git a/mac/fira_round_hopping_crypto.h b/mac/fira_round_hopping_crypto.h
index 3064673..0f0603a 100644
--- a/mac/fira_round_hopping_crypto.h
+++ b/mac/fira_round_hopping_crypto.h
@@ -1,7 +1,7 @@
 /*
  * This file is part of the UWB stack for linux.
  *
- * Copyright (c) 2020-2021 Qorvo US, Inc.
+ * 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.
@@ -38,7 +38,7 @@ struct fira_round_hopping_sequence;
  * Return: 0 or error.
  */
 int fira_round_hopping_crypto_encrypt(
-	struct fira_round_hopping_sequence *round_hopping_sequence,
+	const struct fira_round_hopping_sequence *round_hopping_sequence,
 	const u8 *data, u8 *out);
 
 /**
diff --git a/mac/fira_round_hopping_sequence.c b/mac/fira_round_hopping_sequence.c
index 9ce6c12..921f8cc 100644
--- a/mac/fira_round_hopping_sequence.c
+++ b/mac/fira_round_hopping_sequence.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the UWB stack for linux.
  *
- * Copyright (c) 2020-2021 Qorvo US, Inc.
+ * 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.
@@ -44,18 +44,18 @@ void fira_round_hopping_sequence_destroy(struct fira_session *session)
 	fira_round_hopping_crypto_destroy(round_hopping_sequence);
 }
 
-int fira_round_hopping_sequence_get(struct fira_session *session,
+int fira_round_hopping_sequence_get(const struct fira_session *session,
 				    int block_index)
 {
-	struct fira_round_hopping_sequence *round_hopping_sequence =
+	const struct fira_session_params *params = &session->params;
+	const struct fira_round_hopping_sequence *round_hopping_sequence =
 		&session->round_hopping_sequence;
+	int block_duration_slots =
+		params->block_duration_dtu / params->slot_duration_dtu;
+	int n_rounds = block_duration_slots / params->round_duration_slots;
 	u8 block[AES_BLOCK_SIZE];
 	u8 out[AES_BLOCK_SIZE];
 	int r;
-	int block_duration_slots = session->params.block_duration_dtu /
-				   session->params.slot_duration_dtu;
-	int n_rounds =
-		block_duration_slots / session->params.round_duration_slots;
 
 	if (!block_index)
 		return 0;
diff --git a/mac/fira_round_hopping_sequence.h b/mac/fira_round_hopping_sequence.h
index ca634b6..a89d91b 100644
--- a/mac/fira_round_hopping_sequence.h
+++ b/mac/fira_round_hopping_sequence.h
@@ -1,7 +1,7 @@
 /*
  * This file is part of the UWB stack for linux.
  *
- * Copyright (c) 2020-2021 Qorvo US, Inc.
+ * 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.
@@ -47,7 +47,7 @@ void fira_round_hopping_sequence_destroy(struct fira_session *session);
  *
  * Return: Round index.
  */
-int fira_round_hopping_sequence_get(struct fira_session *session,
+int fira_round_hopping_sequence_get(const struct fira_session *session,
 				    int block_index);
 
 #endif /* NET_MCPS802154_FIRA_ROUND_HOPPING_SEQUENCE_H */
diff --git a/mac/fira_session.c b/mac/fira_session.c
index 033641c..cca4252 100644
--- a/mac/fira_session.c
+++ b/mac/fira_session.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the UWB stack for linux.
  *
- * Copyright (c) 2020-2021 Qorvo US, Inc.
+ * 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.
@@ -21,709 +21,910 @@
  * Qorvo. Please contact Qorvo to inquire about licensing terms.
  */
 
-#include "fira_session.h"
-#include "fira_crypto.h"
-#include "fira_round_hopping_sequence.h"
-#include "fira_access.h"
-#include "fira_frame.h"
-#include "fira_trace.h"
-
 #include <linux/bitops.h>
 #include <linux/errno.h>
 #include <linux/ieee802154.h>
 #include <linux/string.h>
 #include <linux/limits.h>
+#include <linux/math64.h>
 
-#define FIRA_DRIFT_TOLERANCE_PPM 30
-
-struct fira_session *fira_session_new(struct fira_local *local, u32 session_id)
-{
-	struct fira_session *session;
+#include <net/mcps802154_frame.h>
+#include <net/fira_region_nl.h>
 
-	session = kzalloc(sizeof(*session), GFP_KERNEL);
-	if (!session)
-		return NULL;
+#include "fira_session.h"
+#include "fira_round_hopping_sequence.h"
+#include "fira_access.h"
+#include "fira_frame.h"
+#include "fira_trace.h"
 
-	session->id = session_id;
-	session->params.ranging_round_usage = FIRA_RANGING_ROUND_USAGE_DSTWR;
-	session->params.short_addr = IEEE802154_ADDR_SHORT_BROADCAST;
-	session->params.controller_short_addr = IEEE802154_ADDR_SHORT_BROADCAST;
-	session->params.slot_duration_dtu =
-		FIRA_SLOT_DURATION_RSTU_DEFAULT * local->llhw->rstu_dtu;
-	session->params.block_duration_dtu = FIRA_BLOCK_DURATION_MS_DEFAULT *
-					     (local->llhw->dtu_freq_hz / 1000);
-	session->params.round_duration_slots =
-		FIRA_ROUND_DURATION_SLOTS_DEFAULT;
-	session->params.max_rr_retry = FIRA_MAX_RR_RETRY_DEFAULT;
-	session->params.round_hopping = false;
-	session->params.priority = FIRA_PRIORITY_DEFAULT;
-	session->params.result_report_phase = true;
-	session->params.rframe_config = FIRA_RFRAME_CONFIG_SP3;
-	session->params.preamble_duration = FIRA_PREAMBULE_DURATION_64;
-	session->params.sfd_id = FIRA_SFD_ID_2;
-
-	session->params._meas_seq_1.n_steps = 1;
-	session->params._meas_seq_1.steps[0].type = FIRA_MEASUREMENT_TYPE_RANGE;
-	session->params._meas_seq_1.steps[0].n_measurements = 1;
-	session->params._meas_seq_1.steps[0].rx_ant_set_nonranging = 0;
-	session->params._meas_seq_1.steps[0].rx_ant_sets_ranging[0] = 0;
-	session->params._meas_seq_1.steps[0].rx_ant_sets_ranging[1] = 0;
-	session->params._meas_seq_1.steps[0].tx_ant_set_nonranging = 0;
-	session->params._meas_seq_1.steps[0].tx_ant_set_ranging = 0;
-	session->params._meas_seq_2.n_steps = 0;
-	session->params.meas_seq.active = &(session->params._meas_seq_1);
-	session->params.meas_seq.current_step = 0;
-	session->params.meas_seq.n_measurements_achieved = 0;
+inline static int
+fira_compute_minimum_rssi(const struct fira_ranging_info *ranging_data)
+{
+	/*
+	 * We want the WORST RSSI level.
+	 * Please Note : RSSI is actually a negative number, but encoded
+	 * as an absolute value.
+	 */
+	u8 min_rssi = 0;
+	int i;
 
-	/* Report parameters. */
-	session->params.aoa_result_req = true;
-	session->params.report_tof = true;
+	for (i = 0; i < ranging_data->n_rx_rssis; i++)
+		min_rssi = max(ranging_data->rx_rssis[i], min_rssi);
+	return min_rssi;
+}
 
-	if (fira_round_hopping_sequence_init(session))
-		goto failed;
+inline static int
+fira_compute_average_rssi(const struct fira_ranging_info *ranging_data)
+{
+	unsigned sum;
+	int i;
 
-	list_add(&session->entry, &local->inactive_sessions);
+	if (!ranging_data->n_rx_rssis)
+		return 0;
 
-	return session;
-failed:
-	kfree(session);
-	return NULL;
+	for (i = 0, sum = 0; i < ranging_data->n_rx_rssis; i++)
+		sum += ranging_data->rx_rssis[i];
+	return sum / i;
 }
 
-void fira_session_free(struct fira_session *session)
+static int fira_report_local_aoa(struct sk_buff *msg, int nest_attr_id,
+				 const struct fira_local_aoa_info *info)
 {
-	fira_round_hopping_sequence_destroy(session);
-	list_del(&session->entry);
-	fira_aead_destroy(&session->crypto.aead);
-	/* The session structure contains the Crypto context. This needs to be
-	 * cleared. */
-	kfree_sensitive(session);
+	struct nlattr *aoa;
+
+	aoa = nla_nest_start(msg, nest_attr_id);
+	if (!aoa)
+		goto nla_put_failure;
+#define A(x) FIRA_RANGING_DATA_MEASUREMENTS_AOA_ATTR_##x
+	if (nla_put_u8(msg, A(RX_ANTENNA_SET), info->rx_ant_set))
+		goto nla_put_failure;
+	if (nla_put_s16(msg, A(AOA_2PI), info->aoa_2pi))
+		goto nla_put_failure;
+	if (nla_put_s16(msg, A(PDOA_2PI), info->pdoa_2pi))
+		goto nla_put_failure;
+	if (nla_put_u8(msg, A(AOA_FOM), info->aoa_fom))
+		goto nla_put_failure;
+#undef A
+	nla_nest_end(msg, aoa);
+	return 0;
+nla_put_failure:
+	return -EMSGSIZE;
 }
 
-struct fira_session *fira_session_get(struct fira_local *local, u32 session_id,
-				      bool *active)
+inline static int fira_session_report_measurement(
+	const struct fira_session *session, struct sk_buff *msg,
+	const struct fira_ranging_info *ranging_data, s64 rctu_freq_hz)
 {
-	struct fira_session *session;
-
-	list_for_each_entry (session, &local->inactive_sessions, entry) {
-		if (session->id == session_id) {
-			*active = false;
-			return session;
+	const struct fira_session_params *params = &session->params;
+	bool report_rssi_val_present = false;
+	int report_rssi_val = 0;
+
+	if (params->report_rssi &&
+	    ranging_data->status == FIRA_STATUS_RANGING_SUCCESS) {
+		switch (params->report_rssi) {
+		case FIRA_RSSI_REPORT_MINIMUM:
+			report_rssi_val_present = true;
+			report_rssi_val =
+				fira_compute_minimum_rssi(ranging_data);
+			break;
+		case FIRA_RSSI_REPORT_AVERAGE:
+			report_rssi_val_present = true;
+			report_rssi_val =
+				fira_compute_average_rssi(ranging_data);
+			break;
+		default:
+			break;
 		}
 	}
 
-	list_for_each_entry (session, &local->active_sessions, entry) {
-		if (session->id == session_id) {
-			*active = true;
-			return session;
+#define A(x) FIRA_RANGING_DATA_MEASUREMENTS_ATTR_##x
+
+	if (nla_put_u16(msg, A(SHORT_ADDR), ranging_data->short_addr) ||
+	    nla_put_u8(msg, A(STATUS), ranging_data->status))
+		goto nla_put_failure;
+
+	if (ranging_data->status) {
+		if (nla_put_u8(msg, A(SLOT_INDEX), ranging_data->slot_index))
+			goto nla_put_failure;
+		return 0;
+	}
+	if (ranging_data->tof_present) {
+		static const s64 speed_of_light_mm_per_s = 299702547000ull;
+		s32 distance_mm = div64_s64(ranging_data->tof_rctu *
+						    speed_of_light_mm_per_s,
+					    rctu_freq_hz);
+		if (nla_put_s32(msg, A(DISTANCE_MM), distance_mm))
+			goto nla_put_failure;
+	}
+	if (ranging_data->local_aoa.present) {
+		if (fira_report_local_aoa(msg, A(LOCAL_AOA),
+					  &ranging_data->local_aoa))
+			goto nla_put_failure;
+	}
+	if (ranging_data->local_aoa_azimuth.present) {
+		if (fira_report_local_aoa(msg, A(LOCAL_AOA_AZIMUTH),
+					  &ranging_data->local_aoa_azimuth))
+			goto nla_put_failure;
+	}
+	if (ranging_data->local_aoa_elevation.present) {
+		if (fira_report_local_aoa(msg, A(LOCAL_AOA_ELEVATION),
+					  &ranging_data->local_aoa_elevation))
+			goto nla_put_failure;
+	}
+	if (ranging_data->remote_aoa_azimuth_present) {
+		if (nla_put_s16(msg, A(REMOTE_AOA_AZIMUTH_2PI),
+				ranging_data->remote_aoa_azimuth_2pi))
+			goto nla_put_failure;
+		if (ranging_data->remote_aoa_fom_present) {
+			if (nla_put_u8(msg, A(REMOTE_AOA_AZIMUTH_FOM),
+				       ranging_data->remote_aoa_azimuth_fom))
+				goto nla_put_failure;
+		}
+	}
+	if (ranging_data->remote_aoa_elevation_present) {
+		if (nla_put_s16(msg, A(REMOTE_AOA_ELEVATION_PI),
+				ranging_data->remote_aoa_elevation_pi))
+			goto nla_put_failure;
+		if (ranging_data->remote_aoa_fom_present) {
+			if (nla_put_u8(msg, A(REMOTE_AOA_ELEVATION_FOM),
+				       ranging_data->remote_aoa_elevation_fom))
+				goto nla_put_failure;
 		}
 	}
+	if (report_rssi_val_present) {
+		if (nla_put_u8(msg, A(RSSI), report_rssi_val))
+			goto nla_put_failure;
+	}
+	if (ranging_data->data_payload_len > 0) {
+		if (nla_put(msg, A(DATA_PAYLOAD_RECV),
+			    ranging_data->data_payload_len,
+			    ranging_data->data_payload))
+			goto nla_put_failure;
+	}
+	if (session->data_payload.sent) {
+		if (nla_put_u32(msg, A(DATA_PAYLOAD_SEQ_SENT),
+				session->data_payload.seq))
+			goto nla_put_failure;
+	}
 
-	return NULL;
+#undef A
+	return 0;
+nla_put_failure:
+	return -EMSGSIZE;
 }
 
-void fira_session_copy_controlees(struct fira_controlees_array *to,
-				  const struct fira_controlees_array *from)
+inline static int fira_report_measurement_stopped_controlee(struct sk_buff *msg,
+							    __le16 short_addr)
 {
-	/* Copy only valid entries. */
-	memcpy(to->data, from->data, from->size * sizeof(from->data[0]));
-	to->size = from->size;
+#define A(x) FIRA_RANGING_DATA_MEASUREMENTS_ATTR_##x
+
+	if (nla_put_u16(msg, A(SHORT_ADDR), short_addr) ||
+	    nla_put_u8(msg, A(STOPPED), 1))
+		goto nla_put_failure;
+
+#undef A
+	return 0;
+nla_put_failure:
+	return -EMSGSIZE;
 }
 
-int fira_session_set_controlees(struct fira_local *local,
-				struct fira_session *session,
-				struct fira_controlees_array *controlees_array,
-				const struct fira_controlee *controlees,
-				size_t n_controlees)
+inline static int
+fira_session_report_ranging_data(const struct fira_session *session,
+				 const struct fira_report_info *report_info,
+				 int dtu_freq_hz, int dtu_rctu,
+				 struct sk_buff *msg)
 {
+	const struct fira_session_params *params = &session->params;
+	struct nlattr *data, *measurements, *measurement;
+	int ranging_interval_ms = params->block_duration_dtu *
+				  (session->block_stride_len + 1) /
+				  (dtu_freq_hz / 1000);
+	s64 rctu_freq_hz = (s64)dtu_freq_hz * dtu_rctu;
+
 	int i;
 
-	if (!fira_frame_check_n_controlees(session, n_controlees, false))
-		return -EINVAL;
+	data = nla_nest_start(msg, FIRA_CALL_ATTR_RANGING_DATA);
+	if (!data)
+		goto nla_put_failure;
 
-	for (i = 0; i < n_controlees; i++)
-		controlees_array->data[i] = controlees[i];
+	if (nla_put_u32(msg, FIRA_RANGING_DATA_ATTR_BLOCK_INDEX,
+			session->block_index) ||
+	    nla_put_u32(msg, FIRA_RANGING_DATA_ATTR_RANGING_INTERVAL_MS,
+			ranging_interval_ms))
+		goto nla_put_failure;
 
-	controlees_array->size = n_controlees;
+	if (report_info->stopped) {
+		enum fira_ranging_data_attrs_stopped_values stopped;
 
-	return 0;
-}
+		if (session->stop_no_response)
+			stopped = FIRA_RANGING_DATA_ATTR_STOPPED_NO_RESPONSE;
+		else if (session->stop_inband)
+			stopped = FIRA_RANGING_DATA_ATTR_STOPPED_IN_BAND;
+		else
+			stopped = FIRA_RANGING_DATA_ATTR_STOPPED_REQUEST;
 
-int fira_session_new_controlees(struct fira_session *session, bool active,
-				struct fira_controlees_array *controlees_array,
-				const struct fira_controlee *controlees,
-				size_t n_controlees)
-{
-	int i, j;
+		if (nla_put_u8(msg, FIRA_RANGING_DATA_ATTR_STOPPED, stopped))
+			goto nla_put_failure;
 
-	if (!fira_frame_check_n_controlees(
-		    session, controlees_array->size + n_controlees, active))
-		return -EINVAL;
+		/*
+		  Case where measurements are not available:
+		   - A controller stop request.
+		   - A controller max measurements reached.
+		   - A controlee stop in band.
+		*/
+		if ((session->params.device_type ==
+			     FIRA_DEVICE_TYPE_CONTROLLER &&
+		     stopped == FIRA_RANGING_DATA_ATTR_STOPPED_REQUEST) ||
+		    (session->params.device_type ==
+			     FIRA_DEVICE_TYPE_CONTROLEE &&
+		     stopped == FIRA_RANGING_DATA_ATTR_STOPPED_IN_BAND))
+			goto end_report;
+	}
 
-	for (i = 0; i < n_controlees; i++) {
-		for (j = 0; j < controlees_array->size; j++) {
-			if (controlees[i].short_addr ==
-			    controlees_array->data[j].short_addr)
-				return -EINVAL;
+	if (report_info->n_ranging_data + report_info->n_stopped_controlees) {
+		measurements = nla_nest_start(
+			msg, FIRA_RANGING_DATA_ATTR_MEASUREMENTS);
+		if (!measurements)
+			goto nla_put_failure;
+
+		for (i = 0; i < report_info->n_ranging_data; i++) {
+			measurement = nla_nest_start(msg, 1);
+			if (fira_session_report_measurement(
+				    session, msg, &report_info->ranging_data[i],
+				    rctu_freq_hz))
+				goto nla_put_failure;
+			nla_nest_end(msg, measurement);
+		}
+
+		for (i = 0; i < report_info->n_stopped_controlees; i++) {
+			measurement = nla_nest_start(msg, 1);
+			if (fira_report_measurement_stopped_controlee(
+				    msg, report_info->stopped_controlees[i]))
+				goto nla_put_failure;
+			nla_nest_end(msg, measurement);
 		}
-	}
 
-	for (i = 0; i < n_controlees; i++)
-		controlees_array->data[controlees_array->size++] =
-			controlees[i];
+		nla_nest_end(msg, measurements);
+	}
 
+end_report:
+	nla_nest_end(msg, data);
 	return 0;
+
+nla_put_failure:
+	return -EMSGSIZE;
 }
 
-static void
-fira_session_update_stopping_controlees(struct fira_session *session)
+static int
+fira_session_report_diagnostic_rssi(const struct fira_diagnostic *diagnostic,
+				    struct sk_buff *msg)
 {
-	size_t ii, io;
-	struct fira_controlees_array *controlees_array =
-		&session->current_controlees;
-
-	for (ii = 0, io = 0; ii < controlees_array->size; ii++) {
-		struct fira_controlee *c = &controlees_array->data[ii];
-
-		if (c->state != FIRA_CONTROLEE_STATE_PENDING_DEL) {
-			if (io != ii)
-				controlees_array->data[io] = *c;
-			controlees_array->data[io].state =
-				FIRA_CONTROLEE_STATE_RUNNING;
-			io++;
-		}
+	struct nlattr *nest;
+	int i;
+
+	nest = nla_nest_start(
+		msg, FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_ATTR_RSSIS);
+	if (!nest)
+		goto nla_put_failure;
+	for (i = 0; i < diagnostic->n_rssis; i++) {
+		if (nla_put_u8(msg, i, diagnostic->rssis_q1[i]))
+			goto nla_put_failure;
 	}
-	controlees_array->size = io;
+	nla_nest_end(msg, nest);
+	return 0;
+
+nla_put_failure:
+	return -EMSGSIZE;
 }
 
-int fira_session_del_controlees(struct fira_controlees_array *controlees_array,
-				const struct fira_controlee *controlees,
-				size_t n_controlees)
+static int
+fira_session_report_diagnostic_aoa(const struct fira_diagnostic *diagnostic,
+				   struct sk_buff *msg)
 {
-	size_t ii, io, j;
-
-	for (ii = 0, io = 0; ii < controlees_array->size; ii++) {
-		bool remove = false;
-		struct fira_controlee *c = &controlees_array->data[ii];
-
-		for (j = 0; j < n_controlees && !remove; j++) {
-			if (c->short_addr == controlees[j].short_addr)
-				remove = true;
-		}
+	const struct mcps802154_rx_aoa_measurements *aoa;
+	struct nlattr *aoas, *aoa_report;
+	int i;
 
-		if (!remove) {
-			if (io != ii)
-				controlees_array->data[io] = *c;
-			io++;
-		}
+	aoas = nla_nest_start(msg,
+			      FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_ATTR_AOAS);
+	if (!aoas)
+		goto nla_put_failure;
+	for (i = 0; i < diagnostic->n_aoas; i++) {
+		aoa = &diagnostic->aoas[i];
+
+		aoa_report = nla_nest_start(msg, i);
+		if (!aoa_report)
+			goto nla_put_failure;
+#define A(x) FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_AOAS_ATTR_##x
+		if (nla_put_s16(msg, A(TDOA), aoa->tdoa_rctu))
+			goto nla_put_failure;
+		if (nla_put_s16(msg, A(PDOA), aoa->pdoa_rad_q11))
+			goto nla_put_failure;
+		if (nla_put_s16(msg, A(AOA), aoa->aoa_rad_q11))
+			goto nla_put_failure;
+		if (nla_put_u8(msg, A(FOM), aoa->fom))
+			goto nla_put_failure;
+		if (nla_put_u8(msg, A(TYPE), aoa->type))
+			goto nla_put_failure;
+#undef A
+		nla_nest_end(msg, aoa_report);
 	}
-	controlees_array->size = io;
-
+	nla_nest_end(msg, aoas);
 	return 0;
+
+nla_put_failure:
+	return -EMSGSIZE;
 }
 
-int fira_session_async_del_controlees(
-	struct fira_session *session,
-	struct fira_controlees_array *controlees_array,
-	const struct fira_controlee *controlees, size_t n_controlees)
+static int fira_session_report_cir_samples(const struct mcps802154_rx_cir *cir,
+					   struct sk_buff *msg)
 {
-	size_t i, j;
+	const struct mcps802154_rx_cir_sample_window *sw = &cir->sample_window;
+	const u8 *samples = sw->samples;
+	struct nlattr *sample_nest;
+	int i;
 
-	for (i = 0; i < controlees_array->size; i++) {
-		struct fira_controlee *c = &controlees_array->data[i];
-		enum fira_controlee_state state = FIRA_CONTROLEE_STATE_RUNNING;
+	sample_nest = nla_nest_start(
+		msg,
+		FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_CIRS_ATTR_FP_SAMPLE_WINDOW);
+	if (!sample_nest)
+		goto nla_put_failure;
+	for (i = 0; i < sw->n_samples; i++) {
+		const u8 *sample = &samples[i * sw->sizeof_sample];
 
-		for (j = 0; j < n_controlees; j++) {
-			if (c->short_addr == controlees[j].short_addr) {
-				state = FIRA_CONTROLEE_STATE_PENDING_DEL;
-				session->controlee_management_flags |=
-					FIRA_SESSION_CONTROLEE_MANAGEMENT_FLAG_STOP;
-				break;
-			}
-		}
-		c->state = state;
+		if (nla_put(msg, i, sw->sizeof_sample, sample))
+			goto nla_put_failure;
 	}
-
+	nla_nest_end(msg, sample_nest);
 	return 0;
+
+nla_put_failure:
+	return -EMSGSIZE;
 }
 
-void fira_session_stop_controlees(struct fira_session *session,
-				  struct fira_controlees_array *controlees_array)
+static int
+fira_session_report_diagnostic_cir(const struct fira_diagnostic *diagnostic,
+				   struct sk_buff *msg)
 {
-	size_t i;
+	struct nlattr *cirs_nest, *cir_nest;
+	int i;
 
-	for (i = 0; i < controlees_array->size; i++) {
-		controlees_array->data[i].state =
-			FIRA_CONTROLEE_STATE_PENDING_STOP;
+	cirs_nest = nla_nest_start(
+		msg, FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_ATTR_CIRS);
+	if (!cirs_nest)
+		goto nla_put_failure;
+	for (i = 0; i < diagnostic->n_cirs; i++) {
+		struct mcps802154_rx_cir *cir = &diagnostic->cirs[i];
+
+		cir_nest = nla_nest_start(msg, i);
+		if (!cir_nest)
+			goto nla_put_failure;
+#define A(x) FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_CIRS_ATTR_##x
+		if (nla_put_u16(msg, A(FP_IDX), cir->fp_index))
+			goto nla_put_failure;
+		if (nla_put_s16(msg, A(FP_SNR), cir->fp_snr))
+			goto nla_put_failure;
+		if (nla_put_u16(msg, A(FP_NS), cir->fp_ns_q6))
+			goto nla_put_failure;
+		if (nla_put_u16(msg, A(PP_IDX), cir->pp_index))
+			goto nla_put_failure;
+		if (nla_put_s16(msg, A(PP_SNR), cir->pp_snr))
+			goto nla_put_failure;
+		if (nla_put_u16(msg, A(PP_NS), cir->pp_ns_q6))
+			goto nla_put_failure;
+		if (nla_put_u16(msg, A(FP_SAMPLE_OFFSET),
+				cir->fp_sample_offset))
+			goto nla_put_failure;
+#undef A
+		if (fira_session_report_cir_samples(cir, msg))
+			goto nla_put_failure;
+		nla_nest_end(msg, cir_nest);
 	}
+	nla_nest_end(msg, cirs_nest);
+	return 0;
+
+nla_put_failure:
+	return -EMSGSIZE;
 }
 
-bool fira_session_is_ready(struct fira_local *local,
-			   struct fira_session *session)
+static int fira_session_report_frame_diagnostics(
+	const struct fira_session *session,
+	const struct fira_report_info *report_info, struct sk_buff *msg)
 {
-	int round_duration_dtu;
-	struct fira_session_params *params = &session->params;
+	const struct fira_session_params *params = &session->params;
+	struct nlattr *frame_nest, *reports_nest;
+	bool is_controller = params->device_type == FIRA_DEVICE_TYPE_CONTROLLER;
+	int i;
 
-	if (params->multi_node_mode == FIRA_MULTI_NODE_MODE_UNICAST) {
-		if (session->current_controlees.size > 1)
-			return false;
-	} else {
-		/* on success, session will become active, so assume it is */
-		if (!fira_frame_check_n_controlees(
-			    session, session->current_controlees.size, true))
-			return false;
+	frame_nest = nla_nest_start(
+		msg, FIRA_RANGING_DIAGNOSTICS_ATTR_FRAME_REPORTS);
+	if (!frame_nest)
+		goto nla_put_failure;
+
+	for (i = 0; i < report_info->n_slots; i++) {
+		const struct fira_slot *slot = &report_info->slots[i];
+		int is_tx = slot->controller_tx ? is_controller :
+						  !is_controller;
+
+		reports_nest = nla_nest_start(msg, i);
+		if (!reports_nest)
+			goto nla_put_failure;
+#define A(x) FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_ATTR_##x
+		if (nla_put_u8(msg, A(ANT_SET),
+			       is_tx ? slot->tx_ant_set : slot->rx_ant_set))
+			goto nla_put_failure;
+		if (nla_put_u8(msg, A(ACTION), is_tx))
+			goto nla_put_failure;
+		if (nla_put_u8(msg, A(MSG_ID), slot->message_id))
+			goto nla_put_failure;
+#undef A
+		/* Specific reports are done for Rx frames only. */
+		if (!is_tx) {
+			const struct fira_diagnostic *diagnostic =
+				&report_info->diagnostics[i];
+
+			if (params->diagnostic_report_flags &
+			    FIRA_RANGING_DIAGNOSTICS_FRAME_REPORT_RSSIS) {
+				if (fira_session_report_diagnostic_rssi(
+					    diagnostic, msg))
+					goto nla_put_failure;
+			}
+			if (params->diagnostic_report_flags &
+			    FIRA_RANGING_DIAGNOSTICS_FRAME_REPORT_AOAS) {
+				if (fira_session_report_diagnostic_aoa(
+					    diagnostic, msg))
+					goto nla_put_failure;
+			}
+			if (params->diagnostic_report_flags &
+			    FIRA_RANGING_DIAGNOSTICS_FRAME_REPORT_CIRS) {
+				if (fira_session_report_diagnostic_cir(
+					    diagnostic, msg))
+					goto nla_put_failure;
+			}
+		}
+		nla_nest_end(msg, reports_nest);
 	}
+	nla_nest_end(msg, frame_nest);
+	return 0;
 
-	round_duration_dtu =
-		params->slot_duration_dtu * params->round_duration_slots;
-	return params->slot_duration_dtu != 0 &&
-	       params->block_duration_dtu != 0 &&
-	       params->round_duration_slots != 0 &&
-	       round_duration_dtu < params->block_duration_dtu;
+nla_put_failure:
+	return -EMSGSIZE;
 }
 
-inline static void fira_update_meas_seq(struct fira_session *session)
+static inline int fira_session_report_ranging_diagnostics(
+	const struct fira_session *session,
+	const struct fira_report_info *report_info, struct sk_buff *msg)
 {
-	struct fira_session_params *p = &session->params;
-	if (p->meas_seq.update_flag) {
-		if (p->meas_seq.active == &p->_meas_seq_1)
-			p->meas_seq.active = &p->_meas_seq_2;
-		else
-			p->meas_seq.active = &p->_meas_seq_1;
-		p->meas_seq.current_step = 0;
-		p->meas_seq.n_measurements_achieved = 0;
-		p->meas_seq.update_flag = false;
-	} else {
-		if (p->meas_seq.n_measurements_achieved >=
-		    p->meas_seq.active->steps[p->meas_seq.current_step]
-			    .n_measurements) {
-			p->meas_seq.n_measurements_achieved = 0;
-			p->meas_seq.current_step++;
-			p->meas_seq.current_step %= p->meas_seq.active->n_steps;
-		}
-	}
-	trace_region_fira_meas_seq_step(
-		session, &(p->meas_seq.active->steps[p->meas_seq.current_step]),
-		p->meas_seq.current_step);
+	const struct fira_session_params *params = &session->params;
+	struct nlattr *diagnostics_nest;
+
+	if (!params->report_diagnostics)
+		return 0;
+
+	diagnostics_nest =
+		nla_nest_start(msg, FIRA_CALL_ATTR_RANGING_DIAGNOSTICS);
+	if (!diagnostics_nest)
+		goto nla_put_failure;
+
+	if (fira_session_report_frame_diagnostics(session, report_info, msg))
+		goto nla_put_failure;
+
+	nla_nest_end(msg, diagnostics_nest);
+	return 0;
+
+nla_put_failure:
+	return -EMSGSIZE;
 }
 
-void fira_session_prepare(struct fira_session *session)
+struct fira_session *fira_session_new(struct fira_local *local, u32 session_id)
 {
-	fira_update_meas_seq(session);
-	if (session->params.device_type == FIRA_DEVICE_TYPE_CONTROLLER) {
-		session->next_block_stride_len =
-			session->params.block_stride_len;
-		if (session->params.round_hopping) {
-			u32 next_block_index = session->block_index +
-					       session->next_block_stride_len +
-					       1;
-			session->next_round_index =
-				fira_round_hopping_sequence_get(
-					session, next_block_index);
+	struct fira_session *session;
+	struct fira_session_params *params;
+	int all_rx_ctx_size = FIRA_CONTROLEES_MAX * local->llhw->rx_ctx_size;
+	void *rx_ctx_base = NULL;
+	int i;
+
+	session = kzalloc(sizeof(*session), GFP_KERNEL);
+	if (!session)
+		return NULL;
+	if (all_rx_ctx_size) {
+		rx_ctx_base = kzalloc(all_rx_ctx_size, GFP_KERNEL);
+		if (!rx_ctx_base)
+			goto failed;
+	}
+
+	params = &session->params;
+	session->id = session_id;
+	session->measurements.reset = true;
+
+	/* Explicit default parameters as implicit is zero. */
+	params->ranging_round_usage = FIRA_RANGING_ROUND_USAGE_DSTWR;
+	params->short_addr = IEEE802154_ADDR_SHORT_BROADCAST;
+	params->controller_short_addr = IEEE802154_ADDR_SHORT_BROADCAST;
+	params->slot_duration_dtu =
+		FIRA_SLOT_DURATION_RSTU_DEFAULT * local->llhw->rstu_dtu;
+	params->block_duration_dtu = FIRA_BLOCK_DURATION_MS_DEFAULT *
+				     (local->llhw->dtu_freq_hz / 1000);
+	params->round_duration_slots = FIRA_ROUND_DURATION_SLOTS_DEFAULT;
+	params->max_rr_retry = FIRA_MAX_RR_RETRY_DEFAULT;
+	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;
+	params->number_of_sts_segments = FIRA_STS_SEGMENTS_1;
+	params->meas_seq.n_steps = 1;
+	params->meas_seq.steps[0].type = FIRA_MEASUREMENT_TYPE_RANGE;
+	params->meas_seq.steps[0].n_measurements = 1;
+	params->meas_seq.steps[0].rx_ant_set_nonranging = 0;
+	params->meas_seq.steps[0].rx_ant_sets_ranging[0] = 0;
+	params->meas_seq.steps[0].rx_ant_sets_ranging[1] = 0;
+	params->meas_seq.steps[0].tx_ant_set_nonranging = 0;
+	params->meas_seq.steps[0].tx_ant_set_ranging = 0;
+	/* Report parameters. */
+	params->aoa_result_req = true;
+	params->report_tof = true;
+	params->result_report_phase = true;
+	params->range_data_ntf_config = FIRA_RANGE_DATA_NTF_ALWAYS;
+	params->range_data_ntf_proximity_near_mm = 0;
+	params->range_data_ntf_proximity_far_mm =
+		FIRA_RANGE_DATA_NTF_PROXIMITY_FAR_DEFAULT;
+
+	if (fira_round_hopping_sequence_init(session))
+		goto failed;
+
+	if (all_rx_ctx_size) {
+		for (i = 0; i < FIRA_CONTROLEES_MAX; i++) {
+			void *rx_ctx = (char *)rx_ctx_base +
+				       i * local->llhw->rx_ctx_size;
+			session->rx_ctx[i] = rx_ctx;
 		}
 	}
+
+	INIT_LIST_HEAD(&session->current_controlees);
+
+	fira_session_fsm_initialise(local, session);
+	return session;
+
+failed:
+	kfree(rx_ctx_base);
+	kfree(session);
+	return NULL;
 }
 
-void fira_session_update_round_index(struct fira_session *session)
+void fira_session_free(struct fira_local *local, struct fira_session *session)
 {
-	if (session->hopping_sequence_generation) {
-		session->round_index = fira_round_hopping_sequence_get(
-			session, session->block_index);
-	} else {
-		session->round_index = session->next_round_index;
-		session->hopping_sequence_generation =
-			session->params.round_hopping;
+	struct fira_controlee *controlee, *tmp_controlee;
+
+	list_for_each_entry_safe (controlee, tmp_controlee,
+				  &session->current_controlees, entry) {
+		list_del(&controlee->entry);
+		kfree(controlee);
 	}
+	fira_session_fsm_uninit(local, session);
+	fira_round_hopping_sequence_destroy(session);
+	kfree(session->rx_ctx[0]);
+	kfree_sensitive(session);
 }
 
-static void fira_session_update(struct fira_local *local,
+int fira_session_set_controlees(struct fira_local *local,
 				struct fira_session *session,
-				u32 next_timestamp_dtu)
+				struct list_head *controlees, int n_controlees)
 {
-	u32 access_dtu;
-	s32 diff_dtu;
-	int block_duration_margin_dtu = 0;
-
-	if (session->params.device_type == FIRA_DEVICE_TYPE_CONTROLEE)
-		block_duration_margin_dtu =
-			fira_session_get_block_duration_margin(local, session);
-
-	/* Do we have the time to participate in the current block? */
-	access_dtu = session->block_start_dtu +
-		     fira_session_get_round_slot(session) *
-			     session->params.slot_duration_dtu -
-		     block_duration_margin_dtu;
-	diff_dtu = access_dtu - next_timestamp_dtu;
-
-	if (diff_dtu < 0) {
-		int block_duration_dtu = session->params.block_duration_dtu;
-		int block_duration_slots =
-			block_duration_dtu / session->params.slot_duration_dtu;
-		int block_stride_len = session->block_stride_len;
-		int block_stride_duration_dtu =
-			block_duration_dtu * (block_stride_len + 1);
-		int add_blocks, add_strides;
+	struct fira_controlee *controlee, *tmp_controlee;
 
-		/*
-		 * No time in current block, which block should we try? The
-		 * result of this can be 0, meaning that we are still in the
-		 * same block, but the access was earlier in this block.
-		 */
-		diff_dtu = session->block_start_dtu -
-			   block_duration_margin_dtu - next_timestamp_dtu;
-		add_strides = -diff_dtu / block_stride_duration_dtu;
-		add_blocks = add_strides * (block_stride_len + 1);
-
-		session->block_start_dtu += add_blocks * block_duration_dtu;
-		session->block_index += add_blocks;
-		session->sts_index += add_blocks * block_duration_slots;
-		if (add_blocks != 0) {
-			/*
-			 * More than one ranging round skipped, can not trust
-			 * last hopping instructions.
-			 */
-			if (add_blocks > block_stride_len + 1)
-				session->hopping_sequence_generation =
-					session->params.round_hopping;
-			fira_session_update_round_index(session);
-		}
+	if (!fira_frame_check_n_controlees(session, n_controlees, false))
+		return -EINVAL;
 
-		/* Retry in the found block. */
-		access_dtu = session->block_start_dtu +
-			     fira_session_get_round_slot(session) *
-				     session->params.slot_duration_dtu -
-			     block_duration_margin_dtu;
-		diff_dtu = access_dtu - next_timestamp_dtu;
-
-		if (diff_dtu < 0) {
-			/* Still no time, next one will be OK. */
-			add_blocks = block_stride_len + 1;
-			session->block_start_dtu +=
-				add_blocks * block_duration_dtu;
-			session->block_index += add_blocks;
-			session->sts_index += add_blocks * block_duration_slots;
-			fira_session_update_round_index(session);
-		}
+	list_for_each_entry_safe (controlee, tmp_controlee,
+				  &session->current_controlees, entry) {
+		list_del(&controlee->entry);
+		kfree(controlee);
+	}
+	list_for_each_entry_safe (controlee, tmp_controlee, controlees, entry) {
+		list_move_tail(&controlee->entry, &session->current_controlees);
 	}
+	session->n_current_controlees = n_controlees;
+	return 0;
 }
 
-static inline bool
-fira_session_has_higher_priority(const struct fira_session *session,
-				 const struct fira_session *selected_session)
+int fira_session_new_controlees(struct fira_session *session,
+				struct list_head *controlees, int n_controlees,
+				bool async)
 {
-	return session->params.priority > selected_session->params.priority ||
-	       (session->params.priority == selected_session->params.priority &&
-		is_before_dtu(session->last_access_timestamp_dtu,
-			      selected_session->last_access_timestamp_dtu));
-}
+	struct fira_controlee *controlee, *new_controlee, *tmp_new_controlee;
 
-static struct fira_session *fira_session_find_next(struct fira_local *local,
-						   u32 next_timestamp_dtu,
-						   u32 max_access_duration_dtu,
-						   u32 *timestamp_dtu,
-						   u32 *duration_dtu)
-{
-	struct fira_session *selected_session = NULL;
-	struct fira_session *session;
-	u32 selected_timestamp_dtu = 0;
-	u32 selected_duration_dtu = 0;
-	u32 access_timestamp_dtu;
-	u32 access_duration_dtu;
-	u32 unsync_access_duration_dtu;
-	u32 selected_unsync_access_duration_dtu = 0;
-	u32 max_unsync_access_duration_dtu = 0;
-	bool found_sync_session = false;
-	struct mcps802154_region_demand demand;
-
-	/* Select the next synchronised session that can be scheduled without
-	 * overlapping any other synchronised sessions or if they are
-	 * overlapping, the session with the highest priority. */
-	list_for_each_entry (session, &local->active_sessions, entry) {
-		if (session->params.device_type == FIRA_DEVICE_TYPE_CONTROLEE &&
-		    !session->synchronised)
-			continue;
-		fira_session_update(local, session, next_timestamp_dtu);
-		fira_session_get_demand(local, session, &demand);
-		access_timestamp_dtu = demand.timestamp_dtu;
-		access_duration_dtu = demand.max_duration_dtu;
-		if ((!selected_session ||
-		     is_before_dtu(access_timestamp_dtu + access_duration_dtu +
-					   local->llhw->anticip_dtu,
-				   selected_timestamp_dtu + 1) ||
-		     (is_before_dtu(access_timestamp_dtu,
-				    selected_timestamp_dtu +
-					    selected_duration_dtu +
-					    local->llhw->anticip_dtu) &&
-		      fira_session_has_higher_priority(session,
-						       selected_session))) &&
-		    (!max_access_duration_dtu ||
-		     access_duration_dtu <= max_access_duration_dtu)) {
-			found_sync_session = true;
-			selected_session = session;
-			selected_timestamp_dtu = access_timestamp_dtu;
-			selected_duration_dtu = access_duration_dtu;
+	if (!fira_frame_check_n_controlees(
+		    session, session->n_current_controlees + n_controlees,
+		    async))
+		return -EINVAL;
+
+	list_for_each_entry (new_controlee, controlees, entry) {
+		list_for_each_entry (controlee, &session->current_controlees,
+				     entry) {
+			if (new_controlee->short_addr == controlee->short_addr)
+				return -EINVAL;
 		}
 	}
 
-	if (found_sync_session)
-		max_unsync_access_duration_dtu =
-			max((s32)(selected_timestamp_dtu - next_timestamp_dtu -
-				  local->llhw->anticip_dtu),
-			    0);
-
-	/* Select a session that is not synchronised if there is enough time to
-	 * schedule it before the synchronised session currently selected. */
-	list_for_each_entry (session, &local->active_sessions, entry) {
-		if (session->params.device_type != FIRA_DEVICE_TYPE_CONTROLEE ||
-		    session->synchronised)
-			continue;
-		fira_session_update(local, session, next_timestamp_dtu);
-		fira_session_get_demand(local, session, &demand);
-		access_duration_dtu = demand.max_duration_dtu;
-		unsync_access_duration_dtu = U32_MAX;
-		if (session->params.max_rr_retry) {
-			int nb_blocks =
-				session->params.max_rr_retry *
-					(session->block_stride_len + 1) +
-				session->last_block_index -
-				session->block_index;
-
-			unsync_access_duration_dtu =
-				min((u32)session->params.block_duration_dtu *
-					    max(nb_blocks, 1),
-				    unsync_access_duration_dtu);
-		}
-		if (found_sync_session)
-			unsync_access_duration_dtu =
-				min(max_unsync_access_duration_dtu,
-				    unsync_access_duration_dtu);
-		if (max_access_duration_dtu)
-			unsync_access_duration_dtu =
-				min(max_access_duration_dtu,
-				    unsync_access_duration_dtu);
-		/* Among the sessions that are not synchronised, select the one for which the
-		 * shortest access needs to be generated. */
-		if (access_duration_dtu <= unsync_access_duration_dtu &&
-		    (!selected_unsync_access_duration_dtu ||
-		     unsync_access_duration_dtu <
-			     selected_unsync_access_duration_dtu)) {
-			selected_session = session;
-			selected_timestamp_dtu = next_timestamp_dtu;
-			if (unsync_access_duration_dtu != U32_MAX) {
-				selected_unsync_access_duration_dtu =
-					selected_duration_dtu =
-						unsync_access_duration_dtu;
-			} else {
-				selected_unsync_access_duration_dtu =
-					selected_duration_dtu = 0;
-			}
-		}
+	list_for_each_entry_safe (new_controlee, tmp_new_controlee, controlees,
+				  entry) {
+		if (async)
+			new_controlee->state = FIRA_CONTROLEE_STATE_PENDING_RUN;
+		list_move_tail(&new_controlee->entry,
+			       &session->current_controlees);
+		session->n_current_controlees++;
 	}
 
-	*timestamp_dtu = selected_timestamp_dtu;
-	*duration_dtu = selected_duration_dtu;
-	return selected_session;
+	return 0;
 }
 
-static void
-fira_session_check_max_number_of_measurements(struct fira_local *local,
-					      struct fira_session *session)
+int fira_session_del_controlees(struct fira_session *session,
+				struct list_head *controlees, bool async)
 {
-	if (!session->max_number_of_measurements_reached &&
-	    session->params.max_number_of_measurements &&
-	    ((s32)(session->params.max_number_of_measurements -
-		   session->number_of_measurements) <= 0)) {
-		session->max_number_of_measurements_reached = true;
-		session->controlee_management_flags =
-			FIRA_SESSION_CONTROLEE_MANAGEMENT_FLAG_STOP;
-		fira_session_stop_controlees(session,
-					     &session->current_controlees);
+	struct fira_controlee *controlee, *new_controlee, *tmp_controlee,
+		*tmp_new_controlee;
+
+	list_for_each_entry_safe (new_controlee, tmp_new_controlee, controlees,
+				  entry) {
+		list_for_each_entry_safe (controlee, tmp_controlee,
+					  &session->current_controlees, entry) {
+			if (new_controlee->short_addr ==
+			    controlee->short_addr) {
+				if (async) {
+					controlee->state =
+						FIRA_CONTROLEE_STATE_PENDING_DEL;
+				} else {
+					list_del(&controlee->entry);
+					kfree(controlee);
+					session->n_current_controlees--;
+				}
+				break;
+			}
+		}
+		list_del(&new_controlee->entry);
+		kfree(new_controlee);
 	}
+	return 0;
 }
 
-static bool fira_session_check_max_rr_retry(struct fira_session *session)
+void fira_session_stop_controlees(struct fira_session *session)
 {
-	if (session->params.max_rr_retry &&
-	    !((s32)((session->block_index - session->last_block_index) /
-			    (session->block_stride_len + 1) -
-		    session->params.max_rr_retry) < 0)) {
-		session->stop_no_response = true;
-		return true;
+	struct fira_controlee *controlee;
+
+	list_for_each_entry (controlee, &session->current_controlees, entry) {
+		controlee->state = FIRA_CONTROLEE_STATE_PENDING_STOP;
 	}
-	return false;
 }
 
-static void
-fira_session_send_collision_reports(struct fira_local *local,
-				    struct fira_session *selected_session,
-				    u32 selected_end_dtu)
+void fira_session_restart_controlees(struct fira_session *session)
 {
-	struct fira_session *session;
-	struct fira_session *tmp_session;
-	struct mcps802154_region_demand demand;
-	int i;
+	struct fira_controlee *controlee;
 
-	list_for_each_entry_safe (session, tmp_session, &local->active_sessions,
-				  entry) {
-		if (session == selected_session ||
-		    (session->params.device_type ==
-			     FIRA_DEVICE_TYPE_CONTROLEE &&
-		     !session->synchronised))
-			continue;
-		fira_session_get_demand(local, session, &demand);
-		if (is_before_dtu(demand.timestamp_dtu, selected_end_dtu)) {
-			fira_compute_access(local, session);
-			for (i = 0; i < local->n_ranging_info; i++) {
-				local->ranging_info[i].status =
-					FIRA_STATUS_RANGING_TX_FAILED;
-			}
-			fira_session_access_done(local, session, true);
-		}
+	list_for_each_entry (controlee, &session->current_controlees, entry) {
+		if (controlee->state != FIRA_CONTROLEE_STATE_PENDING_DEL &&
+		    controlee->state != FIRA_CONTROLEE_STATE_DELETING)
+			controlee->state = FIRA_CONTROLEE_STATE_RUNNING;
 	}
 }
 
-static void fira_session_stop_expired_sessions(struct fira_local *local)
+int fira_session_controlees_running_count(const struct fira_session *session)
 {
-	struct fira_session *session;
-	struct fira_session *tmp_session;
-	int i;
-
-	list_for_each_entry_safe (session, tmp_session, &local->active_sessions,
-				  entry) {
-		if (session == local->current_session ||
-		    !fira_session_check_max_rr_retry(session))
-			continue;
-		fira_compute_access(local, session);
-		for (i = 0; i < local->n_ranging_info; i++) {
-			local->ranging_info[i].status =
-				FIRA_STATUS_RANGING_RX_TIMEOUT;
-		}
-		fira_session_access_done(local, session, true);
+	struct fira_controlee *controlee;
+	int count = 0;
+
+	list_for_each_entry (controlee, &session->current_controlees, entry) {
+		if (controlee->state == FIRA_CONTROLEE_STATE_RUNNING ||
+		    controlee->state == FIRA_CONTROLEE_STATE_PENDING_STOP ||
+		    controlee->state == FIRA_CONTROLEE_STATE_PENDING_DEL)
+			count++;
 	}
+	return count;
 }
 
-static void fira_session_check_unsync(struct fira_local *local,
-				      struct fira_session *session)
+void fira_session_update_controlees(struct fira_local *local,
+				    struct fira_session *session)
 {
-	int nb_blocks;
-	int unsync_drift_dtu;
-	int block_margin_dtu;
+	struct fira_controlee *controlee, *tmp_controlee;
+	bool reset_rx_ctx = false;
+	int i;
 
-	if ((session->params.device_type != FIRA_DEVICE_TYPE_CONTROLEE) ||
-	    !session->synchronised)
-		return;
+	list_for_each_entry_safe (controlee, tmp_controlee,
+				  &session->current_controlees, entry) {
+		if (controlee->state == FIRA_CONTROLEE_STATE_PENDING_RUN) {
+			controlee->state = FIRA_CONTROLEE_STATE_RUNNING;
+			reset_rx_ctx = true;
+		} else if (controlee->state == FIRA_CONTROLEE_STATE_RUNNING) {
+			/* Stop raised by max number of measurements threshold. */
+			if (session->stop_request)
+				controlee->state =
+					FIRA_CONTROLEE_STATE_STOPPING;
+		} else if (controlee->state ==
+			   FIRA_CONTROLEE_STATE_PENDING_STOP) {
+			controlee->state = FIRA_CONTROLEE_STATE_STOPPING;
+		} else if (controlee->state ==
+			   FIRA_CONTROLEE_STATE_PENDING_DEL) {
+			controlee->state = FIRA_CONTROLEE_STATE_DELETING;
+		} else if (controlee->state == FIRA_CONTROLEE_STATE_DELETING) {
+			list_del(&controlee->entry);
+			kfree(controlee);
+			session->n_current_controlees--;
+			reset_rx_ctx = true;
+		}
+	}
 
-	nb_blocks = session->block_index - session->last_block_index;
-	unsync_drift_dtu = (long long)nb_blocks *
-			   session->params.block_duration_dtu *
-			   FIRA_DRIFT_TOLERANCE_PPM / 1000000;
-	block_margin_dtu =
-		fira_session_get_block_duration_margin(local, session);
-	if (unsync_drift_dtu >= block_margin_dtu)
-		session->synchronised = false;
+	if (reset_rx_ctx && local->llhw->rx_ctx_size) {
+		for (i = 0; i < session->n_current_controlees; i++) {
+			memset(session->rx_ctx[i], 0, local->llhw->rx_ctx_size);
+		}
+	}
 }
 
-struct fira_session *fira_session_next(struct fira_local *local,
-				       u32 next_timestamp_dtu,
-				       u32 max_access_duration_dtu)
+bool fira_session_is_ready(const struct fira_local *local,
+			   const struct fira_session *session)
 {
-	struct fira_session *selected_session;
-	u32 selected_timestamp_dtu = 0;
-	u32 selected_duration_dtu = 0;
-	u32 selected_end_dtu;
+	const struct fira_session_params *params = &session->params;
+	int round_duration_dtu;
 
-	if (list_empty(&local->active_sessions))
-		return NULL;
+	if (params->multi_node_mode == FIRA_MULTI_NODE_MODE_UNICAST) {
+		if (session->n_current_controlees > 1)
+			return false;
+	} else {
+		/* On success, session will become active, so assume it is. */
+		if (!fira_frame_check_n_controlees(
+			    session, session->n_current_controlees, true))
+			return false;
+	}
 
-	selected_session = fira_session_find_next(local, next_timestamp_dtu,
-						  max_access_duration_dtu,
-						  &selected_timestamp_dtu,
-						  &selected_duration_dtu);
-	if (!selected_session)
-		return NULL;
-	selected_end_dtu = selected_timestamp_dtu + selected_duration_dtu +
-			   local->llhw->anticip_dtu;
-	fira_session_send_collision_reports(local, selected_session,
-					    selected_end_dtu);
-	selected_session->last_access_timestamp_dtu = selected_timestamp_dtu;
-	selected_session->last_access_duration_dtu = selected_duration_dtu;
-	return selected_session;
-}
+	/* Check uwb parameters. */
+	if (params->prf_mode == FIRA_PRF_MODE_BPRF) {
+		/* FIXME: when preamble code index is not set, we will use
+		 * the default set one, that may be for HPRF... */
+		if (params->preamble_code_index != 0 &&
+		    (params->preamble_code_index < 9 ||
+		     params->preamble_code_index > 24))
+			return false;
+		if (params->sfd_id != FIRA_SFD_ID_0 &&
+		    params->sfd_id != FIRA_SFD_ID_2)
+			return false;
+		if (params->psdu_data_rate != FIRA_PSDU_DATA_RATE_6M81)
+			return false;
+		if (params->preamble_duration != FIRA_PREAMBULE_DURATION_64)
+			return false;
+		if (params->number_of_sts_segments > FIRA_STS_SEGMENTS_1)
+			return false;
+	} else {
+		if (params->preamble_code_index != 0 &&
+		    (params->preamble_code_index < 25 ||
+		     params->preamble_code_index > 32))
+			return false;
+		if (params->sfd_id == FIRA_SFD_ID_0)
+			return false;
+		if (params->prf_mode == FIRA_PRF_MODE_HPRF &&
+		    params->psdu_data_rate > FIRA_PSDU_DATA_RATE_7M80)
+			return false;
+		if (params->prf_mode == FIRA_PRF_MODE_HPRF_HIGH_RATE &&
+		    params->psdu_data_rate < FIRA_PSDU_DATA_RATE_27M2)
+			return false;
+	}
+	if ((params->rframe_config == FIRA_RFRAME_CONFIG_SP0) &&
+	    (params->number_of_sts_segments != FIRA_STS_SEGMENTS_0))
+		return false;
+	if ((params->rframe_config != FIRA_RFRAME_CONFIG_SP0) &&
+	    (params->number_of_sts_segments == FIRA_STS_SEGMENTS_0))
+		return false;
 
-void fira_session_resync(struct fira_session *session, u32 sts_index,
-			 u32 timestamp_dtu)
-{
-	int block_duration_slots = session->params.block_duration_dtu /
-				   session->params.slot_duration_dtu;
-	int slot_index = sts_index - session->crypto.sts_index_init;
-	int block_index = slot_index / block_duration_slots;
-	int round_slot_index = slot_index - block_index * block_duration_slots;
-
-	session->block_index = block_index;
-	session->block_start_dtu =
-		timestamp_dtu -
-		round_slot_index * session->params.slot_duration_dtu;
-	session->sts_index = sts_index - round_slot_index;
-	session->round_index =
-		round_slot_index / session->params.round_duration_slots;
-	session->synchronised = true;
-	session->last_access_timestamp_dtu = timestamp_dtu;
+	round_duration_dtu =
+		params->slot_duration_dtu * params->round_duration_slots;
+	return params->slot_duration_dtu != 0 &&
+	       params->block_duration_dtu != 0 &&
+	       params->round_duration_slots != 0 &&
+	       round_duration_dtu <= params->block_duration_dtu;
 }
 
-void fira_session_access_done(struct fira_local *local,
-			      struct fira_session *session,
-			      bool add_measurements)
+/**
+ * proximity_enable_report() - Check proximity range to enable/disable report.
+ * @report_info: report info to be enabled/disabled
+ * @min_distance_mm: minimum distance in mm, value included
+ * @max_distance_mm: maximum distance in mm, value included
+ * @dtu_freq_hz: Frequency, to be used to compute distance from report
+ * @dtu_rctu: RCTU, to be used to compute distance from report
+ *
+ * Return: true if the report should be sent
+ *
+ * Report notification is sent with all of its measurements when:
+ * - it contains a stopped condition
+ * - it contains stopped controlees
+ * - it contains a measurement error
+ * - one of its measurement is inside of the configured proximity range
+ *
+ * Report notification is not sent when all of its measurements are valid
+ * and outside of the configured proximity range.
+ */
+static bool proximity_enable_report(const struct fira_report_info *report_info,
+				    u32 min_distance_mm, u32 max_distance_mm,
+				    int dtu_freq_hz, int dtu_rctu)
 {
-	if (session->controlee_management_flags ==
-	    FIRA_SESSION_CONTROLEE_MANAGEMENT_FLAG_STOP) {
-		fira_session_update_stopping_controlees(session);
-		session->controlee_management_flags = 0;
-	}
-
-	if (session == local->current_session) {
-		if (!(session->params.device_type ==
-			      FIRA_DEVICE_TYPE_CONTROLEE &&
-		      local->ranging_info[0].status) &&
-		    !(session->params.device_type ==
-			      FIRA_DEVICE_TYPE_CONTROLLER &&
-		      local->n_ranging_valid != local->n_ranging_info)) {
-			session->last_block_index = session->block_index;
-		} else {
-			fira_session_check_unsync(local, session);
+	static const s64 speed_of_light_mm_per_s = 299702547000ull;
+	const s64 rctu_freq_hz = (s64)dtu_freq_hz * dtu_rctu;
+	s32 distance_mm;
+	const struct fira_ranging_info *ranging_data;
+	int i;
+
+	if (report_info->stopped || report_info->n_stopped_controlees) {
+		return true;
+	}
+
+	for (i = 0; i < report_info->n_ranging_data; i++) {
+		ranging_data = &report_info->ranging_data[i];
+		if (ranging_data->status != FIRA_STATUS_RANGING_SUCCESS) {
+			return true;
+		}
+		if (!ranging_data->tof_present) {
+			return true;
+		}
+		/* Computation needs to be kept in sync with fira_session_report_measurement() */
+		distance_mm = div64_s64(ranging_data->tof_rctu *
+						speed_of_light_mm_per_s,
+					rctu_freq_hz);
+		if (distance_mm >= min_distance_mm &&
+		    distance_mm <= max_distance_mm) {
+			return true;
 		}
-		session->number_of_measurements++;
-		session->params.meas_seq.n_measurements_achieved++;
 	}
 
-	fira_session_check_max_number_of_measurements(local, session);
-	fira_session_check_max_rr_retry(session);
-	fira_report(local, session, add_measurements);
+	return false;
+}
 
-	if (session->controlee_management_flags &
-	    FIRA_SESSION_CONTROLEE_MANAGEMENT_FLAG_UPDATE) {
-		fira_session_copy_controlees(&session->current_controlees,
-					     &session->new_controlees);
-		session->controlee_management_flags &=
-			~FIRA_SESSION_CONTROLEE_MANAGEMENT_FLAG_UPDATE;
-	}
+void fira_session_report(struct fira_local *local, struct fira_session *session,
+			 const struct fira_report_info *report_info)
+{
+	struct sk_buff *msg;
+	const struct fira_session_params *params = &session->params;
 
-	if (((session->stop_request ||
-	      session->max_number_of_measurements_reached) &&
-	     !(session->controlee_management_flags &
-	       FIRA_SESSION_CONTROLEE_MANAGEMENT_FLAG_STOP)) ||
-	    session->stop_inband || session->stop_no_response) {
-		list_move(&session->entry, &local->inactive_sessions);
-		session->stop_request = false;
-		session->stop_inband = false;
-		session->stop_no_response = false;
-		session->max_number_of_measurements_reached = false;
-		/* Reset data parameters. */
-		session->params.data_payload_seq = 0;
-		session->params.data_payload_len = 0;
+	if (params->range_data_ntf_config == FIRA_RANGE_DATA_NTF_DISABLED &&
+		!report_info->stopped && !report_info->n_stopped_controlees) {
+		return;
 	}
 
-	if (session == local->current_session) {
-		fira_session_stop_expired_sessions(local);
-		session->block_stride_len = session->next_block_stride_len;
+	if (params->range_data_ntf_config == FIRA_RANGE_DATA_NTF_PROXIMITY) {
+		if (!proximity_enable_report(
+			    report_info,
+			    params->range_data_ntf_proximity_near_mm,
+			    params->range_data_ntf_proximity_far_mm,
+			    local->llhw->dtu_freq_hz, local->llhw->dtu_rctu)) {
+			return;
+		}
 	}
+
+	trace_region_fira_session_report(session, report_info);
+	msg = mcps802154_region_event_alloc_skb(local->llhw, &local->region,
+						FIRA_CALL_SESSION_NOTIFICATION,
+						session->event_portid,
+						NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+	if (!msg)
+		return;
+
+	if (nla_put_u32(msg, FIRA_CALL_ATTR_SESSION_ID, session->id))
+		goto nla_put_failure;
+	if (nla_put_u32(msg, FIRA_CALL_ATTR_SEQUENCE_NUMBER,
+			session->sequence_number))
+		goto nla_put_failure;
+	if (fira_session_report_ranging_data(session, report_info,
+					     local->llhw->dtu_freq_hz,
+					     local->llhw->dtu_rctu, msg))
+		goto nla_put_failure;
+	if (fira_session_report_ranging_diagnostics(session, report_info, msg))
+		goto nla_put_failure;
+	session->sequence_number++;
+	session->data_payload.sent = false;
+
+	skb_queue_tail(&local->report_queue, msg);
+	schedule_work(&local->report_work);
+	return;
+
+nla_put_failure:
+	kfree_skb(msg);
 }
diff --git a/mac/fira_session.h b/mac/fira_session.h
index 5f1a7cd..4f02596 100644
--- a/mac/fira_session.h
+++ b/mac/fira_session.h
@@ -1,7 +1,7 @@
 /*
  * This file is part of the UWB stack for linux.
  *
- * Copyright (c) 2020-2021 Qorvo US, Inc.
+ * 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.
@@ -24,20 +24,31 @@
 #ifndef NET_MCPS802154_FIRA_SESSION_H
 #define NET_MCPS802154_FIRA_SESSION_H
 
+#include "fira_session_fsm.h"
 #include "fira_region.h"
+#include "fira_sts.h"
 #include "fira_crypto.h"
 #include "fira_round_hopping_crypto_impl.h"
 
 /**
  * enum fira_controlee_state - State of controlee.
+ * @FIRA_CONTROLEE_STATE_PENDING_RUN: The controlee will be set to running state
+ * at the end of round.
  * @FIRA_CONTROLEE_STATE_RUNNING: The controlee is running.
- * @FIRA_CONTROLEE_STATE_PENDING_STOP: The controlee is stopping.
- * @FIRA_CONTROLEE_STATE_PENDING_DEL: RFU.
+ * @FIRA_CONTROLEE_STATE_PENDING_STOP: The controlee will be set to stopping
+ * state at the end of round.
+ * @FIRA_CONTROLEE_STATE_STOPPING: The controlee is stopping.
+ * @FIRA_CONTROLEE_STATE_PENDING_DEL: The controlee will be set to deleting
+ * state at the end of round.
+ * @FIRA_CONTROLEE_STATE_DELETING: The controlee is being deleted.
  */
 enum fira_controlee_state {
+	FIRA_CONTROLEE_STATE_PENDING_RUN,
 	FIRA_CONTROLEE_STATE_RUNNING,
 	FIRA_CONTROLEE_STATE_PENDING_STOP,
+	FIRA_CONTROLEE_STATE_STOPPING,
 	FIRA_CONTROLEE_STATE_PENDING_DEL,
+	FIRA_CONTROLEE_STATE_DELETING,
 };
 
 /**
@@ -56,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.
 	 */
@@ -69,17 +80,10 @@ struct fira_controlee {
 	 * @state: Current state of the controlee.
 	 */
 	enum fira_controlee_state state;
-};
-
-enum fira_session_controlee_management_flags {
-	FIRA_SESSION_CONTROLEE_MANAGEMENT_FLAG_UPDATE = 1,
-	FIRA_SESSION_CONTROLEE_MANAGEMENT_FLAG_STOP = 2,
-};
-
-struct fira_controlees_array {
-	struct fira_controlee data[FIRA_CONTROLEES_MAX];
-	/* Number of data valid. */
-	size_t size;
+	/**
+	 * @entry: Entry in list of controlees.
+	 */
+	struct list_head entry;
 };
 
 struct fira_measurement_sequence_step {
@@ -97,13 +101,6 @@ struct fira_measurement_sequence {
 	size_t n_steps;
 };
 
-struct fira_measurement_sequence_data {
-	struct fira_measurement_sequence *active;
-	u8 current_step;
-	u8 n_measurements_achieved;
-	bool update_flag;
-};
-
 struct fira_session_params {
 	/* Main parameters. */
 	enum fira_device_type device_type;
@@ -129,23 +126,36 @@ struct fira_session_params {
 	enum fira_rframe_config rframe_config;
 	enum fira_preambule_duration preamble_duration;
 	enum fira_sfd_id sfd_id;
+	enum fira_sts_segments number_of_sts_segments;
 	enum fira_psdu_data_rate psdu_data_rate;
 	enum fira_mac_fcs_type mac_fcs_type;
+	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;
 	bool report_aoa_elevation;
 	bool report_aoa_fom;
-	struct fira_measurement_sequence_data meas_seq;
-	struct fira_measurement_sequence _meas_seq_1;
-	struct fira_measurement_sequence _meas_seq_2;
+	enum fira_rssi_report_type report_rssi;
+	struct fira_measurement_sequence meas_seq;
 	u32 data_vendor_oui;
 	u8 data_payload[FIRA_DATA_PAYLOAD_SIZE_MAX];
 	u32 data_payload_seq;
 	int data_payload_len;
+	bool report_diagnostics;
+	enum fira_ranging_diagnostics_frame_report_flags diagnostic_report_flags;
+	/* Misc */
+	enum fira_sts_length sts_length;
+	enum fira_range_data_ntf_config range_data_ntf_config;
+	u32 range_data_ntf_proximity_near_mm;
+	u32 range_data_ntf_proximity_far_mm;
 };
 
 /**
@@ -157,48 +167,74 @@ struct fira_session {
 	 */
 	u32 id;
 	/**
+	 * @sequence_number: Session notification counter.
+	 */
+	u32 sequence_number;
+	/**
 	 * @entry: Entry in list of sessions.
 	 */
 	struct list_head entry;
 	/**
+	 * @state: State of the session.
+	 */
+	const struct fira_session_fsm_state *state;
+	/**
 	 * @params: Session parameters, mostly read only while the session is
 	 * active.
 	 */
 	struct fira_session_params params;
 	/**
-	 * @block_start_dtu: Timestamp of the current or previous block. All
-	 * other fields are referring to this same block.
+	 * @hrp_uwb_params: HRP UWB parameters, read only while the session is
+	 * active.
 	 */
-	u32 block_start_dtu;
+	struct mcps802154_hrp_uwb_params hrp_uwb_params;
 	/**
-	 * @block_index: Block index of the reference block.
+	 * @event_portid: Port identifier to use for notifications.
 	 */
-	u32 block_index;
+	u32 event_portid;
 	/**
-	 * @sts_index: STS index value at reference block start.
+	 * @block_start_valid: True when block_start_dtu is valid.
+	 * It's false on the first access wo initiation delay.
 	 */
-	u32 sts_index;
+	bool block_start_valid;
 	/**
-	 * @hopping_sequence_generation: Whether to compute round index from ranging round sequence.
+	 * @block_start_dtu: Block start timestamp in dtu of the last
+	 * get_access.
 	 */
-	bool hopping_sequence_generation;
+	u32 block_start_dtu;
 	/**
-	 * @round_index: Round index of the reference block.
+	 * @next_access_timestamp_dtu: Next access timestamp in dtu.
+	 * It's equal to block_start_dtu when the hopping is disabled.
+	 * Otherwise it's beyond the block_start_dtu.
+	 * It's updated after each good or missed ranging round.
 	 */
-	int round_index;
+	u32 next_access_timestamp_dtu;
 	/**
-	 * @next_round_index: Round index of the block after the reference block.
+	 * @last_access_timestamp_dtu: Last timestamp where the session got
+	 * the access.
+	 * It's used only on session's election, when a negotiation between
+	 * two session fails.
 	 */
-	int next_round_index;
+	u32 last_access_timestamp_dtu;
 	/**
-	 * @block_stride_len: Stride length for the reference block.
+	 * @block_index: Block index used on the last access.
+	 */
+	u32 block_index;
+	/**
+	 * @block_stride_len: Stride length indicates how many ranging blocks
+	 * will be skipped.
+	 * The value is updated at the beginning of an access.
 	 */
 	int block_stride_len;
 	/**
-	 * @next_block_stride_len: Stride length for the block after the
-	 * reference block.
+	 * @round_index: Round index used on the last access.
 	 */
-	int next_block_stride_len;
+	int round_index;
+	/**
+	 * @next_round_index: Next round index a announced in measurement
+	 * report message.
+	 */
+	int next_round_index;
 	/**
 	* @stop_request: Session has been requested to stop.
 	*/
@@ -214,62 +250,197 @@ struct fira_session {
 	 */
 	bool stop_no_response;
 	/**
-	 * @max_number_of_measurements_reached: Session has been requested to stop
-	 * because max_number_of_measurements was reached.
-	 */
-	bool max_number_of_measurements_reached;
-	/**
-	 * @crypto: Crypto context.
+	 * @n_ranging_round_retry: Number of ranging round failed.
+	 * Counter reset on ranging round success.
 	 */
-	struct fira_crypto crypto;
+	int n_ranging_round_retry;
+
 	/**
 	 * @round_hopping_sequence: Round hopping sequence generation context.
 	 */
 	struct fira_round_hopping_sequence round_hopping_sequence;
 	/**
-	 * @event_portid: Port identifier to use for notifications.
+	 * @controlee: Group of persistent variable(s) used when session
+	 * is a controlee.
+	 */
+	struct {
+		/**
+		 * @synchronised: Whether a controlee session was synchronised.
+		 */
+		bool synchronised;
+		/**
+		 * @block_index_sync: Last block index received.
+		 */
+		int block_index_sync;
+		/**
+		 * @hopping_mode: True when hopping is enabled on last
+		 * measurement frame.
+		 */
+		bool hopping_mode;
+		/**
+		 * @next_round_index_valid: True when the next round index
+		 * is present in measurement report frame.
+		 */
+		bool next_round_index_valid;
+	} controlee;
+	/**
+	 * @controller: Group of persistent variable(s) used when session
+	 * is a controller.
+	 */
+	struct {
+		/**
+		 * @next_block_index: Next block index built on get access with
+		 * next round index.
+		 * It's only to avoid to rebuild the next round index on next
+		 * access, when this last occur in time as block index will
+		 * match.
+		 */
+		u32 next_block_index;
+	} controller;
+	/**
+	 * @data_payload: Local context for data_payload feature.
+	 */
+	struct {
+		/**
+		 * @seq: Sequence number of last sent data.
+		 */
+		u32 seq;
+		/**
+		 * @sent: True when data have been send during ranging round.
+		 */
+		bool sent;
+	} data_payload;
+	/**
+	 * @current_controlees: Current list of controlees.
+	 */
+	struct list_head current_controlees;
+	/**
+	 * @n_current_controlees: Number of elements in the list of current
+	 * controlees.
+	 */
+	size_t n_current_controlees;
+	/**
+	 * @measurements: Measurement configurations which influence diagnostics.
+	 */
+	struct {
+		/**
+		 * @sequence: Copy of the meas_seq parameter on get_access
+		 * event.
+		 */
+		struct fira_measurement_sequence sequence;
+		/**
+		 * @index: Index of the step in sequence array.
+		 */
+		int index;
+		/**
+		 * @n_achieved: Number of measurements done inside a step.
+		 */
+		int n_achieved;
+		/**
+		 * @n_total_achieved: Total number of measurements done.
+		 */
+		int n_total_achieved;
+		/**
+		 * @reset: True when new parameters have to be retrieved.
+		 */
+		bool reset;
+	} measurements;
+	/**
+	 * @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;
+	/*
+	 * @last_error: last error that occurred during the active session.
+	 */
+	int last_error;
+};
+
+/**
+ * struct fira_session_demand - Next access information for one FiRa session.
+ */
+struct fira_session_demand {
+	/**
+	 * @block_start_dtu: Block start in dtu.
 	 */
-	u32 event_portid;
+	u32 block_start_dtu;
 	/**
-	 * @synchronised: Whether a controlee session was synchronised. This
-	 * field is not used for controller sessions.
+	 * @timestamp_dtu: Access timestamp in dtu.
 	 */
-	bool synchronised;
+	u32 timestamp_dtu;
 	/**
-	 * @last_access_timestamp_dtu: Timestamp of the last computed access.
+	 * @max_duration_dtu: Maximum duration for the access.
 	 */
-	u32 last_access_timestamp_dtu;
+	int max_duration_dtu;
+	/**
+	 * @add_blocks: Number of block to add.
+	 */
+	int add_blocks;
+	/**
+	 * @round_index: Round index to apply for the access.
+	 */
+	int round_index;
+	/**
+	 * @rx_timeout_dtu: timeout to apply when first frame of the controlee.
+	 */
+	int rx_timeout_dtu;
+};
+
+/**
+ * struct fira_report_info - Report information for all peer.
+ */
+struct fira_report_info {
 	/**
-	 * @last_access_duration_dtu: Duration of the last computed access.
+	 * @ranging_data: Base address of ranging data per peer, or null
+	 * pointer.
 	 */
-	u32 last_access_duration_dtu;
+	const struct fira_ranging_info *ranging_data;
 	/**
-	 * @data_payload_seq_sent: Sequence number of last sent data.
+	 * @n_ranging_data: Number of entry in ranging_data above.
 	 */
-	u32 data_payload_seq_sent;
+	size_t n_ranging_data;
 	/**
-	 * @last_block_index: Block index of the last successful ranging.
+	 * @stopped_controlees: NULL, or short address of all stopped controlees.
 	 */
-	u32 last_block_index;
+	const __le16 *stopped_controlees;
 	/**
-	 * @new_controlees: List of controlees to applies on next ca.
+	 * @n_stopped_controlees: Number of controlees stopped in array above.
 	 */
-	struct fira_controlees_array new_controlees;
+	size_t n_stopped_controlees;
 	/**
-	 * @current_controlees: List of controlees currently applied.
+	 * @diagnostics: Array of diagnostic collected per slots.
 	 */
-	struct fira_controlees_array current_controlees;
+	const struct fira_diagnostic *diagnostics;
 	/**
-	 * @controlee_management_flags: Flags used to indicates if the list of
-	 * controlees must be updated and if any controlee must be stopped
-	 * before allowing updates again. See
-	 * &fira_session_controlee_management_flags.
+	 * @slots: Array of information slots.
 	 */
-	u32 controlee_management_flags;
+	const struct fira_slot *slots;
 	/**
-	 * @number_of_measurements: Number of measurements.
+	 * @n_slots: Number of slots above.
 	 */
-	u32 number_of_measurements;
+	size_t n_slots;
+	/**
+	 * @stopped: True when the session is stopped.
+	 */
+	bool stopped;
 };
 
 /**
@@ -283,224 +454,124 @@ struct fira_session *fira_session_new(struct fira_local *local, u32 session_id);
 
 /**
  * fira_session_free() - Remove a session.
- * @session: Session to remove, must be inactive.
- */
-void fira_session_free(struct fira_session *session);
-
-/**
- * fira_session_get() - Get a session by its identifier.
  * @local: FiRa context.
- * @session_id: Session identifier.
- * @active: When session is found set to true if active, false if inactive.
- *
- * Return: The session or NULL if not found.
- */
-struct fira_session *fira_session_get(struct fira_local *local, u32 session_id,
-				      bool *active);
-
-/**
- * fira_session_copy_controlees() - copy controlees array between two array.
- * @to: FiRa controlees array to write.
- * @from: FiRa controlees array to read.
+ * @session: Session to remove, must be inactive.
  */
-void fira_session_copy_controlees(struct fira_controlees_array *to,
-				  const struct fira_controlees_array *from);
+void fira_session_free(struct fira_local *local, struct fira_session *session);
 
 /**
  * fira_session_set_controlees() - Set controlees.
  * @local: FiRa context.
  * @session: Session.
- * @controlees_array: Destination array where to store the new controlees list.
- * @controlees: Controlees information.
+ * @controlees: List of controlees.
  * @n_controlees: Number of controlees.
  *
  * Return: 0 or error.
  */
 int fira_session_set_controlees(struct fira_local *local,
 				struct fira_session *session,
-				struct fira_controlees_array *controlees_array,
-				const struct fira_controlee *controlees,
-				size_t n_controlees);
+				struct list_head *controlees, int n_controlees);
 
 /**
  * fira_session_new_controlees() - Add new controlees.
  * @session: Session.
- * @active: True if session is active.
- * @controlees_array: Destination array where to store the updated
- * controlees list.
- * @controlees: Controlees information.
+ * @controlees: List of controlees to add.
  * @n_controlees: Number of controlees.
+ * @async: True is the controlees must be added asynchronously.
  *
  * Return: 0 or error.
  */
-int fira_session_new_controlees(struct fira_session *session, bool active,
-				struct fira_controlees_array *controlees_array,
-				const struct fira_controlee *controlees,
-				size_t n_controlees);
+int fira_session_new_controlees(struct fira_session *session,
+				struct list_head *controlees, int n_controlees,
+				bool async);
 
 /**
- * fira_session_del_controlees() - Delete without stopping controlees.
- * @controlees_array: Destination array where to store the updated
- * controlees list.
- * @controlees: Controlees information.
- * @n_controlees: Number of controlees.
+ * fira_session_restart_controlees() - Restart controlee and erase pending del.
+ * @session: FiRa session context.
  *
- * Return: 0 or error.
+ * Return: Number of controlee removed.
  */
-int fira_session_del_controlees(struct fira_controlees_array *controlees_array,
-				const struct fira_controlee *controlees,
-				size_t n_controlees);
+void fira_session_restart_controlees(struct fira_session *session);
 
 /**
- * fira_session_async_del_controlees() - Set flag to indicate that controlees
- * need to be stopped then deleted.
+ * fira_session_del_controlees() - Delete controlees.
  * @session: Session.
- * @controlees_array: Destination array where store new controlees list.
- * @controlees: Controlees information.
- * @n_controlees: Number of controlees.
+ * @controlees: List of controlees to delete.
+ * @async: True is the controlees must be deleted asynchronously.
  *
  * Return: 0 or error.
  */
-int fira_session_async_del_controlees(
-	struct fira_session *session,
-	struct fira_controlees_array *controlees_array,
-	const struct fira_controlee *controlees, size_t n_controlees);
+int fira_session_del_controlees(struct fira_session *session,
+				struct list_head *controlees, bool async);
 
 /**
  * fira_session_stop_controlees() - Stop controlees.
  * @session: Session.
- * @controlees_array: Destination array where store new controlees list.
  */
-void fira_session_stop_controlees(
-	struct fira_session *session,
-	struct fira_controlees_array *controlees_array);
+void fira_session_stop_controlees(struct fira_session *session);
 
 /**
- * fira_session_is_ready() - Test whether a session is ready to be started.
- * @local: FiRa context.
- * @session: Session to test.
- *
- * Return: true if the session can be started.
- */
-bool fira_session_is_ready(struct fira_local *local,
-			   struct fira_session *session);
-
-/**
- * fira_session_prepare() - Prepare a FiRa session to run.
+ * fira_session_controlees_running_count() - Get the number of running controlees.
  * @session: Session.
- */
-void fira_session_prepare(struct fira_session *session);
-
-/**
- * fira_session_next() - Find the next session to use after the given timestamp.
- * @local: FiRa context.
- * @next_timestamp_dtu: Next access opportunity.
- * @max_access_duration_dtu: Maximum access duration.
  *
- * Return: The session or NULL if none.
- */
-struct fira_session *fira_session_next(struct fira_local *local,
-				       u32 next_timestamp_dtu,
-				       u32 max_access_duration_dtu);
-
-/**
- * fira_session_update_round_index() - Update round index for round hopping.
- * @session: Session to update.
+ * Return: Number of running controlees.
  */
-void fira_session_update_round_index(struct fira_session *session);
+int fira_session_controlees_running_count(const struct fira_session *session);
 
 /**
- * fira_session_resync() - Resync session parameters on control message.
- * @session: Session to synchronize.
- * @sts_index: STS index of control message.
- * @timestamp_dtu: Timestamp of control message.
- */
-void fira_session_resync(struct fira_session *session, u32 sts_index,
-			 u32 timestamp_dtu);
-
-/**
- * fira_session_access_done() - Update session at end of access, or on event
- * when no access is active.
+ * fira_session_update_controlees() - Update controlee's states.
  * @local: FiRa context.
- * @session: Session.
- * @add_measurements: True to add measurements to report.
- */
-void fira_session_access_done(struct fira_local *local,
-			      struct fira_session *session,
-			      bool add_measurements);
-
-/**
- * fira_session_get_round_slot() - Get current round's slot.
- * @session: Session.
- *
- * Return: The first slot of the current round.
+ * @session: Session to test.
  */
-static inline u32
-fira_session_get_round_slot(const struct fira_session *session)
-{
-	return session->round_index * session->params.round_duration_slots;
-}
+void fira_session_update_controlees(struct fira_local *local,
+				    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)
-{
-	return session->sts_index + fira_session_get_round_slot(session);
-}
-
-/**
- * fira_session_get_block_duration_margin() - Get block duration margin.
+ * fira_session_is_ready() - Test whether a session is ready to be started.
  * @local: FiRa context.
- * @session: Session.
+ * @session: Session to test.
  *
- * Return: Block duration margin in dtu.
+ * Return: true if the session can be started.
  */
-static inline int
-fira_session_get_block_duration_margin(struct fira_local *local,
-				       const struct fira_session *session)
-{
-	return (long long int)session->params.block_duration_dtu *
-	       (session->block_stride_len + 1) *
-	       local->block_duration_rx_margin_ppm / 1000000;
-}
+bool fira_session_is_ready(const struct fira_local *local,
+			   const struct fira_session *session);
 
 /**
- * fira_session_get_current_meas_seq_step() - Get current measurement step.
+ * fira_session_get_meas_seq_step() - Get current measurement step.
  * @session: Session.
  *
  * Return: Current Measurement Sequence step for given session.
  */
 static inline const struct fira_measurement_sequence_step *
-fira_session_get_current_meas_seq_step(const struct fira_session *session)
+fira_session_get_meas_seq_step(const struct fira_session *session)
 {
-	return &(session->params.meas_seq.active
-			 ->steps[session->params.meas_seq.current_step]);
+	const struct fira_measurement_sequence *seq =
+		&session->measurements.sequence;
+
+	return &seq->steps[session->measurements.index];
 }
 
 /**
  * fira_session_get_rx_ant_set() - Get Rx antenna set for a given message ID.
- * @message_id: Message ID of Fira frame.
  * @session: Session.
+ * @message_id: Message ID of FiRa frame.
  *
  * Return: Adequate antenna set id for given frame and session parameters.
  */
-static inline s8 fira_session_get_rx_ant_set(const struct fira_session *session,
-					     enum fira_message_id message_id)
+static inline int
+fira_session_get_rx_ant_set(const struct fira_session *session,
+			    enum fira_message_id message_id)
 {
+	const struct fira_session_params *params = &session->params;
 	const struct fira_measurement_sequence_step *step =
-		fira_session_get_current_meas_seq_step(session);
+		fira_session_get_meas_seq_step(session);
 
 	if (message_id > FIRA_MESSAGE_ID_RFRAME_MAX)
 		return step->rx_ant_set_nonranging;
 
 	/* TODO: replace this test by device_role == FIRA_DEVICE_ROLE_INITIATOR
 	* as soon as this feature is supported */
-	if (session->params.device_type == FIRA_DEVICE_TYPE_CONTROLLER)
+	if (params->device_type == FIRA_DEVICE_TYPE_CONTROLLER)
 		return step->rx_ant_sets_ranging[0];
 	else
 		switch (step->type) {
@@ -512,13 +583,38 @@ static inline s8 fira_session_get_rx_ant_set(const struct fira_session *session,
 		case FIRA_MEASUREMENT_TYPE_AOA_AZIMUTH_ELEVATION:
 			return step->rx_ant_sets_ranging
 				[message_id == FIRA_MESSAGE_ID_RANGING_FINAL];
-		/* LCOV_EXCL_START */
 		default:
-			/* defensive check, should not happen */
 			return -1;
-			/* LCOV_EXCL_STOP */
 		}
 	return -1;
 }
 
+/**
+ * fira_session_report() - Report state change and ranging result for a session.
+ * @local: FiRa context.
+ * @session: Session to report.
+ * @report_info: report information to exploit for the reporting.
+ */
+void fira_session_report(struct fira_local *local, struct fira_session *session,
+			 const struct fira_report_info *report_info);
+
+/**
+ * fira_session_controlee_active() - Return whether the controlee is currently active.
+ * @controlee: Controlee.
+ *
+ * Return: True if the controlee is currently active.
+ */
+static inline bool
+fira_session_controlee_active(struct fira_controlee *controlee)
+{
+	switch (controlee->state) {
+	case FIRA_CONTROLEE_STATE_RUNNING:
+	case FIRA_CONTROLEE_STATE_PENDING_STOP:
+	case FIRA_CONTROLEE_STATE_PENDING_DEL:
+		return true;
+	default:
+		return false;
+	}
+}
+
 #endif /* NET_MCPS802154_FIRA_SESSION_H */
diff --git a/mac/fira_session_fsm.c b/mac/fira_session_fsm.c
new file mode 100644
index 0000000..3d1d478
--- /dev/null
+++ b/mac/fira_session_fsm.c
@@ -0,0 +1,156 @@
+/*
+ * 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/errno.h>
+
+#include "fira_session_fsm_init.h"
+#include "fira_session_fsm_idle.h"
+#include "fira_session_fsm_active.h"
+#include "fira_session.h"
+#include "fira_access.h"
+#include "fira_trace.h"
+
+void fira_session_fsm_initialise(struct fira_local *local,
+				 struct fira_session *session)
+{
+	list_add(&session->entry, &local->inactive_sessions);
+	session->state = &fira_session_fsm_init;
+	WARN_ON(!session->state->enter);
+	session->state->enter(local, session);
+}
+
+void fira_session_fsm_uninit(struct fira_local *local,
+			     struct fira_session *session)
+{
+	if (session->state->leave)
+		session->state->leave(local, session);
+
+	trace_region_fira_session_fsm_change_state(
+		session, FIRA_SESSION_STATE_ID_DEINIT);
+	list_del(&session->entry);
+}
+
+void fira_session_fsm_change_state(
+	struct fira_local *local, struct fira_session *session,
+	const struct fira_session_fsm_state *new_state)
+{
+	if (session->state->leave)
+		session->state->leave(local, session);
+	trace_region_fira_session_fsm_change_state(session, new_state->id);
+	session->state = new_state;
+	if (session->state->enter)
+		session->state->enter(local, session);
+}
+
+bool fira_session_is_active(const struct fira_session *session)
+{
+	return session->state == &fira_session_fsm_active;
+}
+
+enum fira_session_state_id
+fira_session_get_state_id(const struct fira_session *session)
+{
+	return session->state->id;
+}
+
+int fira_session_fsm_check_parameters(const struct fira_session *session,
+				      struct nlattr **attrs)
+{
+	WARN_ON(!session->state->check_parameters);
+	return session->state->check_parameters(session, attrs);
+}
+
+void fira_session_fsm_parameters_updated(struct fira_local *local,
+					 struct fira_session *session)
+{
+	/* The handler is defined for all states. */
+	WARN_ON(!session->state->parameters_updated);
+	session->state->parameters_updated(local, session);
+}
+
+void fira_session_fsm_controlee_list_updated(struct fira_local *local,
+					     struct fira_session *session)
+{
+	if (session->state->controlee_list_updated)
+		session->state->controlee_list_updated(local, session);
+}
+
+int fira_session_fsm_start(struct fira_local *local,
+			   struct fira_session *session,
+			   const struct genl_info *info)
+{
+	if (session->state->start)
+		return session->state->start(local, session, info);
+	return -EINVAL;
+}
+
+int fira_session_fsm_stop(struct fira_local *local,
+			  struct fira_session *session)
+{
+	if (session->state->stop)
+		return session->state->stop(local, session);
+	return 0;
+}
+
+int fira_session_fsm_get_demand(const struct fira_local *local,
+				const struct fira_session *session,
+				u32 next_timestamp_dtu, int max_duration_dtu,
+				struct fira_session_demand *session_demand)
+{
+	/*
+	 * fira_get_demand will not call this function without an
+	 * active session.
+	 */
+	WARN_ON(!session->state->get_demand);
+	return session->state->get_demand(local, session, next_timestamp_dtu,
+					  max_duration_dtu, session_demand);
+}
+
+struct mcps802154_access *
+fira_session_fsm_get_access(struct fira_local *local,
+			    struct fira_session *session,
+			    const struct fira_session_demand *session_demand)
+{
+	/*
+	 * fira_get_access will not call this function without an
+	 * active session.
+	 */
+	WARN_ON(!session->state->get_access);
+	return session->state->get_access(local, session, session_demand);
+}
+
+void fira_session_fsm_access_done(struct fira_local *local,
+				  struct fira_session *session, bool error)
+{
+	WARN_ON(!session->state->access_done);
+	return session->state->access_done(local, session, error);
+}
+
+void fira_session_fsm_check_missed_ranging(struct fira_local *local,
+					   struct fira_session *session,
+					   u32 timestamp_dtu)
+{
+	WARN_ON(!session->state->check_missed_ranging);
+	return session->state->check_missed_ranging(local, session,
+						    timestamp_dtu);
+}
diff --git a/mac/fira_session_fsm.h b/mac/fira_session_fsm.h
new file mode 100644
index 0000000..9d1b622
--- /dev/null
+++ b/mac/fira_session_fsm.h
@@ -0,0 +1,241 @@
+/*
+ * 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.
+ */
+
+#ifndef NET_MCPS802154_FIRA_SESSION_FSM_H
+#define NET_MCPS802154_FIRA_SESSION_FSM_H
+
+#include <linux/ieee802154.h>
+
+#include "fira_access.h"
+
+/* Forward declaration. */
+struct fira_local;
+struct fira_session;
+struct fira_session_demand;
+
+/**
+ * enum fira_session_state_id - State of the FiRa session.
+ * @FIRA_SESSION_STATE_ID_INIT:
+ *     Initial state, session is not ready yet.
+ * @FIRA_SESSION_STATE_ID_DEINIT:
+ *     Session does not exist.
+ * @FIRA_SESSION_STATE_ID_ACTIVE:
+ *     Session is currently active.
+ * @FIRA_SESSION_STATE_ID_IDLE:
+ *     Session is ready to start, but not currently active.
+ */
+enum fira_session_state_id {
+	FIRA_SESSION_STATE_ID_INIT,
+	FIRA_SESSION_STATE_ID_DEINIT,
+	FIRA_SESSION_STATE_ID_ACTIVE,
+	FIRA_SESSION_STATE_ID_IDLE,
+};
+
+/**
+ * struct fira_session_fsm_state - FiRa session FSM state.
+ *
+ * This structure contains the callbacks which are called on an event to handle
+ * the transition from the current state.
+ */
+struct fira_session_fsm_state {
+	/** @id: Name of state. */
+	enum fira_session_state_id id;
+	/** @enter: Run when the state is entered. */
+	void (*enter)(struct fira_local *local, struct fira_session *session);
+	/** @leave: Run when the state is left. */
+	void (*leave)(struct fira_local *local, struct fira_session *session);
+	/** @check_parameters: Handle a check parameters. */
+	int (*check_parameters)(const struct fira_session *session,
+				struct nlattr **attrs);
+	/** @parameters_updated: Handle parameters updated event. */
+	void (*parameters_updated)(struct fira_local *local,
+				   struct fira_session *session);
+	/** @controlee_list_updated: Handle controlee list updated event. */
+	void (*controlee_list_updated)(struct fira_local *local,
+				       struct fira_session *session);
+	/** @start: Handle start. */
+	int (*start)(struct fira_local *local, struct fira_session *session,
+		     const struct genl_info *info);
+	/** @stop: Handle stop. */
+	int (*stop)(struct fira_local *local, struct fira_session *session);
+	/** @get_demand: Handle the get demand. */
+	int (*get_demand)(const struct fira_local *local,
+			  const struct fira_session *session,
+			  u32 next_timestamp_dtu, int max_duration_dtu,
+			  struct fira_session_demand *session_demand);
+	/** @get_access: Handle the get access. */
+	struct mcps802154_access *(*get_access)(
+		struct fira_local *local, struct fira_session *session,
+		const struct fira_session_demand *session_demand);
+	/** @access_done: Handle end of access. */
+	void (*access_done)(struct fira_local *local,
+			    struct fira_session *session, bool error);
+	/** @check_missed_ranging: Handle the check of missed ranging. */
+	void (*check_missed_ranging)(struct fira_local *local,
+				     struct fira_session *session,
+				     u32 timestamp_dtu);
+};
+
+/**
+ * fira_session_fsm_change_state() - Change the state of the FSM.
+ * @local: FiRa context.
+ * @session: Session context.
+ * @new_state: New to state to use in the FSM.
+ *
+ * This function shall be called only by fira_session_fsm files.
+ */
+void fira_session_fsm_change_state(
+	struct fira_local *local, struct fira_session *session,
+	const struct fira_session_fsm_state *new_state);
+
+/**
+ * fira_session_is_active() - Return the active status of the session.
+ * @session: Session context.
+ *
+ * Return: True is the session is active, false otherwise.
+ */
+bool fira_session_is_active(const struct fira_session *session);
+
+/**
+ * fira_session_fsm_initialise() - Initialize the FSM.
+ * @local: FiRa context.
+ * @session: Session context.
+ */
+void fira_session_fsm_initialise(struct fira_local *local,
+				 struct fira_session *session);
+
+/**
+ * fira_session_fsm_uninit() - Uninitialise the FSM.
+ * @local: FiRa context.
+ * @session: Session context.
+ */
+void fira_session_fsm_uninit(struct fira_local *local,
+			     struct fira_session *session);
+
+/**
+ * fira_session_get_state_id() - Get current state id (for reporting).
+ * @session: Session context.
+ *
+ * Return: State id value.
+ */
+enum fira_session_state_id
+fira_session_get_state_id(const struct fira_session *session);
+
+/**
+ * fira_session_fsm_check_parameters() - Check parameters change ask by upper
+ * layer.
+ * @session: Session context.
+ * @attrs: Netlink attributs.
+ *
+ * Return: 0 on success, errno when change are refused.
+ */
+int fira_session_fsm_check_parameters(const struct fira_session *session,
+				      struct nlattr **attrs);
+
+/**
+ * fira_session_fsm_parameters_updated() - Parameters updated by upper layer.
+ * @local: FiRa context.
+ * @session: Session context.
+ */
+void fira_session_fsm_parameters_updated(struct fira_local *local,
+					 struct fira_session *session);
+
+/**
+ * fira_session_fsm_controlee_list_updated() - Controlee list updated by upper
+ * layer.
+ * @local: FiRa context.
+ * @session: Session context.
+ */
+void fira_session_fsm_controlee_list_updated(struct fira_local *local,
+					     struct fira_session *session);
+
+/**
+ * fira_session_fsm_start() - Start request from upper layer.
+ * @local: FiRa context.
+ * @session: Session context.
+ * @info: Netlink info used only for the portid.
+ *
+ * Return: 0 on success, errno otherwise.
+ */
+int fira_session_fsm_start(struct fira_local *local,
+			   struct fira_session *session,
+			   const struct genl_info *info);
+
+/**
+ * fira_session_fsm_stop() - Stop request from upper layer.
+ * @local: FiRa context.
+ * @session: Session context.
+ *
+ * Return: 0 on success, errno otherwise.
+ */
+int fira_session_fsm_stop(struct fira_local *local,
+			  struct fira_session *session);
+
+/**
+ * fira_session_fsm_get_demand() - Request the next ranging round of the session.
+ * @local: FiRa context.
+ * @session: Session context.
+ * @next_timestamp_dtu: Timestamp to start a demand.
+ * @max_duration_dtu: Max duration obligation to be consider by the session.
+ * @session_demand: Wish of the session when the return value is 1.
+ *
+ * Return: 1 for a session demand otherwise 0 for no demand.
+ */
+int fira_session_fsm_get_demand(const struct fira_local *local,
+				const struct fira_session *session,
+				u32 next_timestamp_dtu, int max_duration_dtu,
+				struct fira_session_demand *session_demand);
+
+/**
+ * fira_session_fsm_get_access() - Get access to process.
+ * @local: FiRa context.
+ * @session: Session context.
+ * @session_demand: Next access built by the get_demand.
+ *
+ * Return: The access for fproc, or NULL pointer.
+ */
+struct mcps802154_access *
+fira_session_fsm_get_access(struct fira_local *local,
+			    struct fira_session *session,
+			    const struct fira_session_demand *session_demand);
+
+/**
+ * fira_session_fsm_access_done() - End of the access to report.
+ * @local: FiRa context.
+ * @session: Session context.
+ * @error: True when an error happen.
+ */
+void fira_session_fsm_access_done(struct fira_local *local,
+				  struct fira_session *session, bool error);
+
+/**
+ * fira_session_fsm_check_missed_ranging() - Report a missed ranging if exist.
+ * @local: FiRa context.
+ * @session: Session context.
+ * @timestamp_dtu: Timestamp dtu where no fallback is possible.
+ */
+void fira_session_fsm_check_missed_ranging(struct fira_local *local,
+					   struct fira_session *session,
+					   u32 timestamp_dtu);
+
+#endif /* NET_MCPS802154_FIRA_SESSION_FSM_H */
diff --git a/mac/fira_session_fsm_active.c b/mac/fira_session_fsm_active.c
new file mode 100644
index 0000000..90ddc94
--- /dev/null
+++ b/mac/fira_session_fsm_active.c
@@ -0,0 +1,985 @@
+/*
+ * 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 <net/mcps802154_frame.h>
+#include <net/fira_region_nl.h>
+#include <linux/errno.h>
+#include <linux/math64.h>
+
+#include "fira_round_hopping_sequence.h"
+#include "fira_session_fsm_init.h"
+#include "fira_session_fsm_idle.h"
+#include "fira_session_fsm_active.h"
+#include "fira_session.h"
+#include "fira_trace.h"
+#include "warn_return.h"
+
+/**
+ * list_move_to_active() - Move from inactive list to active list.
+ * @local: FiRa context.
+ * @session: Session context.
+ */
+static void list_move_to_active(struct fira_local *local,
+				struct fira_session *session)
+{
+	struct list_head *position = &local->active_sessions;
+	struct fira_session *tmp;
+
+	/*
+	 * Search the position to maintain a list sorted from highest to
+	 * lowest priority. And for the same priority keep the call
+	 * order (moved to the tail).
+	 * Highest value of priority is the highest priority.
+	 * Range of priority is between: 0 to FIRA_PRIORITY_MAX.
+	 */
+	list_for_each_entry (tmp, &local->active_sessions, entry) {
+		if (session->params.priority <= tmp->params.priority)
+			position = &tmp->entry;
+		else
+			break;
+	}
+	list_move(&session->entry, position);
+}
+
+/**
+ * get_channel() - Retrieve the channel to applied on the access.
+ * @local: FiRa context.
+ * @session: Session context.
+ *
+ * Return: The channel.
+ */
+static const struct mcps802154_channel *
+get_channel(struct fira_local *local, const struct fira_session *session)
+{
+	const struct fira_session_params *params = &session->params;
+
+	if (params->channel_number || params->preamble_code_index) {
+		const struct mcps802154_channel *channel =
+			mcps802154_get_current_channel(local->llhw);
+
+		local->channel = *channel;
+		if (params->channel_number)
+			local->channel.channel = params->channel_number;
+		if (params->preamble_code_index)
+			local->channel.preamble_code =
+				params->preamble_code_index;
+		return &local->channel;
+	}
+	return NULL;
+}
+
+/**
+ * get_round_index() - Return the round index for a specific block index.
+ * @session: Session context.
+ * @block_index: Block index.
+ *
+ * Return: Round index freshly computed or the round index saved.
+ */
+static int get_round_index(const struct fira_session *session, int block_index)
+{
+	const struct fira_session_params *params = &session->params;
+	int expected_block_index;
+
+	switch (params->device_type) {
+	default:
+	case FIRA_DEVICE_TYPE_CONTROLLER:
+		if (!params->round_hopping)
+			return 0;
+		/*
+		 * Avoid to rebuild the round_index.
+		 * The condition is true on first get_access too.
+		 */
+		if (session->controller.next_block_index == block_index)
+			return session->next_round_index;
+		break;
+	case FIRA_DEVICE_TYPE_CONTROLEE:
+		if (!session->controlee.hopping_mode)
+			return 0;
+		/*
+		 * Return the round index received, only when the block index
+		 * match with expected block index.
+		 */
+		expected_block_index = session->controlee.block_index_sync +
+				       session->block_stride_len + 1;
+		if (expected_block_index == block_index &&
+		    session->controlee.next_round_index_valid)
+			return session->next_round_index;
+		break;
+	}
+	return fira_round_hopping_sequence_get(session, block_index);
+}
+
+/**
+ * get_rx_margin_duration_dtu() - Build the maximum margin tolerance for Rx.
+ * @local: FiRa context.
+ * @session: Session context.
+ *
+ * Return: Duration to apply on first and Rx frame of controlee's access.
+ */
+static int get_rx_margin_duration_dtu(const struct fira_local *local,
+				      const struct fira_session *session)
+{
+	const struct fira_session_params *params = &session->params;
+	s64 duration_dtu = (s64)(session->block_stride_len + 1) *
+			   params->block_duration_dtu;
+	/*
+	 * TODO: Unit test should be able to predic timestamp.
+	 * - Replace 'local->block_duration_rx_margin_ppm by'
+	 *   UWB_BLOCK_DURATION_MARGIN_PPM
+	 * - Remove 'local' from args.
+	 */
+	return div64_s64(duration_dtu * local->block_duration_rx_margin_ppm,
+			 1000000);
+}
+
+/**
+ * get_next_access_timestamp_dtu() - Build the next access timestamp.
+ * @local: FiRa context.
+ * @session: Session context.
+ *
+ * Return: Timestamp in dtu.
+ */
+static u32 get_next_access_timestamp_dtu(const struct fira_local *local,
+					 const struct fira_session *session)
+{
+	const struct fira_session_params *params = &session->params;
+	int add_blocks = session->block_stride_len + 1;
+	int next_block_index = session->block_index + add_blocks;
+	int next_round_index = get_round_index(session, next_block_index);
+	int round_duration_dtu =
+		params->round_duration_slots * params->slot_duration_dtu;
+	u32 next_block_start_dtu = session->block_start_dtu +
+				   add_blocks * params->block_duration_dtu +
+				   next_round_index * round_duration_dtu;
+
+	switch (params->device_type) {
+	default:
+	case FIRA_DEVICE_TYPE_CONTROLLER:
+		return next_block_start_dtu;
+	case FIRA_DEVICE_TYPE_CONTROLEE:
+		return next_block_start_dtu -
+		       get_rx_margin_duration_dtu(local, session);
+	}
+}
+
+/**
+ * is_controlee_synchronised() - Answer to the question of the synchronisation
+ * status.
+ * @local: FiRa context.
+ * @session: Session context.
+ *
+ * Return: True when the controlee session is still synchronized.
+ */
+static bool is_controlee_synchronised(const struct fira_local *local,
+				      const struct fira_session *session)
+{
+#define FIRA_DRIFT_TOLERANCE_PPM 30
+	const struct fira_session_params *params = &session->params;
+	int n_unsync_blocks;
+	s64 unsync_duration_dtu;
+	int drift_ppm, rx_margin_ppm;
+
+	if (session->controlee.synchronised) {
+		n_unsync_blocks = session->block_index -
+				  session->controlee.block_index_sync;
+		unsync_duration_dtu =
+			n_unsync_blocks * params->block_duration_dtu;
+		drift_ppm = div64_s64(unsync_duration_dtu *
+					      FIRA_DRIFT_TOLERANCE_PPM,
+				      1000000);
+		rx_margin_ppm = get_rx_margin_duration_dtu(local, session);
+
+		trace_region_fira_is_controlee_synchronised(session, drift_ppm,
+							    rx_margin_ppm);
+		if (drift_ppm <= rx_margin_ppm)
+			return true;
+	}
+	return false;
+}
+
+/**
+ * is_stopped() - Is the session stopped?
+ * @session: Session context.
+ *
+ * Return: True when the session is stopped, false otherwise.
+ */
+static bool is_stopped(struct fira_session *session)
+{
+	const struct fira_session_params *params = &session->params;
+	int nb_controlee = fira_session_controlees_running_count(session);
+
+	if (params->max_rr_retry &&
+	    session->n_ranging_round_retry >= params->max_rr_retry)
+		session->stop_no_response = true;
+
+	return (session->stop_request && !nb_controlee) ||
+	       session->stop_inband || session->stop_no_response;
+}
+
+/**
+ * forward_to_next_ranging() - Update the session to forward to next ranging.
+ * @session: Session context.
+ * @n_ranging: Number of ranging (forward).
+ */
+static void forward_to_next_ranging(struct fira_session *session, int n_ranging)
+{
+	const struct fira_session_params *params = &session->params;
+	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;
+
+	session->block_index += add_blocks;
+	session->block_start_dtu += duration_dtu;
+	session->n_ranging_round_retry += n_ranging;
+}
+
+/**
+ * ranging_round_done() - Update controlee and notify the upper layer.
+ * @local: FiRa context.
+ * @session: Session context.
+ * @report_info: Report information to forward fira_session_report.
+ */
+static void ranging_round_done(struct fira_local *local,
+			       struct fira_session *session,
+			       struct fira_report_info *report_info)
+{
+	const struct fira_session_params *params = &session->params;
+
+	session->next_access_timestamp_dtu =
+		get_next_access_timestamp_dtu(local, session);
+	report_info->stopped = is_stopped(session);
+
+	switch (params->device_type) {
+	default:
+	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) {
+		fira_session_fsm_change_state(local, session,
+					      &fira_session_fsm_idle);
+	} else {
+		forward_to_next_ranging(session, 1);
+	}
+}
+
+static void fira_session_fsm_active_enter(struct fira_local *local,
+					  struct fira_session *session)
+{
+	const struct fira_session_params *params = &session->params;
+
+	session->stop_request = false;
+	session->stop_inband = false;
+	session->stop_no_response = false;
+	session->measurements.n_total_achieved = 0;
+	session->block_stride_len = params->block_stride_len;
+	session->controlee.synchronised = false;
+	session->controlee.hopping_mode = false;
+	session->controlee.next_round_index_valid = false;
+	session->controlee.block_index_sync = 0;
+	session->round_index = 0;
+	/*
+	 * Initialize to 1 when initiation_time_ms is 0,
+	 * because first add_blocks built will be 0.
+	 */
+	session->n_ranging_round_retry = params->initiation_time_ms ? 0 : 1;
+
+	list_move_to_active(local, session);
+}
+
+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);
+}
+
+static int
+fira_session_fsm_active_check_parameters(const struct fira_session *session,
+					 struct nlattr **attrs)
+{
+	const struct fira_session_params *params = &session->params;
+	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_MEASUREMENT_SEQUENCE:
+		case FIRA_SESSION_PARAM_ATTR_DATA_PAYLOAD:
+		case FIRA_SESSION_PARAM_ATTR_RANGE_DATA_NTF_CONFIG:
+		case FIRA_SESSION_PARAM_ATTR_RANGE_DATA_NTF_PROXIMITY_NEAR:
+		case FIRA_SESSION_PARAM_ATTR_RANGE_DATA_NTF_PROXIMITY_FAR:
+			/* Allowed for all device type. */
+			break;
+		case FIRA_SESSION_PARAM_ATTR_BLOCK_STRIDE_LENGTH:
+			/* Allowed only for controller. */
+			if (params->device_type == FIRA_DEVICE_TYPE_CONTROLLER)
+				continue;
+			return -EBUSY;
+		default:
+			return -EBUSY;
+		}
+	}
+	return 0;
+}
+
+static void
+fira_session_fsm_active_parameters_updated(struct fira_local *local,
+					   struct fira_session *session)
+{
+	const struct fira_session_params *params = &session->params;
+	int i;
+
+	if (session->measurements.reset) {
+		for (i = 0; i < params->meas_seq.n_steps; i++) {
+			const struct fira_measurement_sequence_step *step;
+
+			step = &params->meas_seq.steps[i];
+			trace_region_fira_session_meas_seq_params(session, step,
+								  i);
+		}
+	}
+}
+
+static int fira_session_fsm_active_start(struct fira_local *local,
+					 struct fira_session *session,
+					 const struct genl_info *info)
+{
+	/* Already started. */
+	return 0;
+}
+
+static int fira_session_fsm_active_stop(struct fira_local *local,
+					struct fira_session *session)
+{
+	const struct fira_session_params *params = &session->params;
+	struct fira_report_info report_info = {
+		.stopped = true,
+	};
+
+	switch (params->device_type) {
+	default:
+	case FIRA_DEVICE_TYPE_CONTROLLER:
+		if (local->current_session == NULL) {
+			session->stop_request = true;
+			/*
+			 * FIXME/BUG:
+			 * In unit test, the stop_tx_frame_error (or rx),
+			 * stop the current access and trig a broken event.
+			 * Then the TearDown request a session_stop, but
+			 * there is still more than one controlee running.
+			 *
+			 * Normally the controller must do an access to
+			 * announce a stop of all controlees.
+			 * But here, there is a missing mechanism, as:
+			 *  - notify_stop is not called,
+			 *  - error is only a boolean in access_done.
+			 *    And the error boolean is True on -ETIME.
+			 *
+			 * And as the current_session equal to NULL is a
+			 * normal behavior in multi-region. We have a bug.
+			 */
+			fira_session_report(local, session, &report_info);
+			fira_session_fsm_change_state(local, session,
+						      &fira_session_fsm_idle);
+		} else if (session->n_current_controlees) {
+			/*
+			 * A ranging round to announced all controlee
+			 * stopped is required.
+			 */
+			fira_session_stop_controlees(session);
+			session->stop_request = true;
+		} else if (local->current_session != session) {
+			session->stop_request = true;
+			fira_session_report(local, session, &report_info);
+			fira_session_fsm_change_state(local, session,
+						      &fira_session_fsm_idle);
+		}
+		break;
+	case FIRA_DEVICE_TYPE_CONTROLEE:
+		session->stop_request = true;
+		if (local->current_session != session) {
+			fira_session_report(local, session, &report_info);
+			fira_session_fsm_change_state(local, session,
+						      &fira_session_fsm_idle);
+		}
+		break;
+	}
+	mcps802154_reschedule(local->llhw);
+	return 0;
+}
+
+static int
+fira_session_fsm_active_get_demand(const struct fira_local *local,
+				   const struct fira_session *session,
+				   u32 next_timestamp_dtu, int max_duration_dtu,
+				   struct fira_session_demand *session_demand)
+{
+	const struct fira_session_params *params = &session->params;
+	int round_duration_dtu =
+		params->round_duration_slots * params->slot_duration_dtu;
+	u32 block_start_dtu;
+	u32 timestamp_dtu;
+	u32 duration_dtu;
+	int round_index = 0;
+	u32 block_index;
+	int add_blocks = 0;
+	int rx_timeout_dtu = 0;
+	int slot_count;
+
+	/* First, determine two dates: block_start_dtu and timestamp_dtu. */
+	if (!is_before_dtu(session->next_access_timestamp_dtu,
+			   next_timestamp_dtu)) {
+		/*
+		 * block_start_dtu is set in the future or present.
+		 * It's happen mainly when initiation_time_ms is not zero.
+		 */
+		timestamp_dtu = block_start_dtu = session->block_start_dtu;
+	} else {
+		/*
+		 * block start is in the past, we have to evaluate the
+		 * new block start dtu.
+		 * It's could be the same with a controlee not synchronized.
+		 *
+		 * Example of time graph of what's could happen:
+		 *
+		 * -------x----------------x----------------x-------
+		 * #x - 1 | Block Index #x |    #x + 1      | #x + 2
+		 * -------x----------------x------x---------x-------> Time
+		 *        |<--------------------->|
+		 *     Block         |            |
+		 *     start         |            next_timestamp_dtu
+		 *                   |
+		 * duration_from_block_start_dtu
+		 *
+		 * In the graph example, one block is missed, but it's could be
+		 * more or less(controlee).
+		 */
+		int duration_from_block_start_dtu =
+			next_timestamp_dtu - session->block_start_dtu;
+
+		if (params->device_type == FIRA_DEVICE_TYPE_CONTROLEE &&
+		    !session->controlee.synchronised) {
+			/*
+			 * With a controlee not synchronized, consider the
+			 * block as missed when there is no more left duration
+			 * in the current block.
+			 *
+			 *      block
+			 *      start       next_timestamp_dtu
+			 *        |                 |
+			 * -------x-----------------x---x------
+			 * #x - 1 |       #x        :   | #x + 1
+			 * -------x-----------------x---x-----> Time
+			 *        |                 |
+			 *        |<--------------->|
+			 *                  |
+			 *                  |
+			 *   add_blocks = Time / block_duration
+			 */
+			add_blocks = duration_from_block_start_dtu /
+				     params->block_duration_dtu;
+		} else {
+			int blocks_per_ranging = session->block_stride_len + 1;
+
+			/*
+			 * With a controller or a controlee synchronized,
+			 * consider a block started as a missed block.
+			 */
+			add_blocks = (duration_from_block_start_dtu +
+				      params->block_duration_dtu - 1) /
+				     params->block_duration_dtu;
+			/*
+			 * Block stride feature announced/received in last
+			 * access.
+			 */
+			if (session->block_stride_len) {
+				int n = add_blocks % blocks_per_ranging;
+
+				/*
+				 * Add more block(s) to reach block stride
+				 * modulo.
+				 */
+				if (n)
+					add_blocks += blocks_per_ranging - n;
+			}
+		}
+
+		/* Compute block start dtu. 'add_blocks' can be zero. */
+		block_start_dtu = session->block_start_dtu +
+				  add_blocks * params->block_duration_dtu;
+		/* Determine the access timestamp. */
+		if (is_before_dtu(block_start_dtu, next_timestamp_dtu))
+			/*
+			 * Only the controlee not synchronized can have its
+			 * next access timestamp_dtu in the future of the
+			 * block start.
+			 *
+			 * block_start_dtu
+			 *        |
+			 * -------x-----------------x----------
+			 * #x - 1 | Block index #x  | #x + 1
+			 * -------x------x----------x----------> Time
+			 *               |
+			 *      next_timestamp_dtu
+			 */
+			timestamp_dtu = next_timestamp_dtu;
+		else
+			timestamp_dtu = block_start_dtu;
+	}
+
+	/*
+	 * As block_start_dtu is updated with new timestamp in the future,
+	 * or still in the past (controlee), then other variables will be
+	 * build to fill the session_demand output.
+	 *
+	 * In other words, locale variables can have a new values which
+	 * represent the next(future) block/access/index/...
+	 * Or keep +/- the same values.
+	 */
+	block_index = session->block_index + add_blocks;
+	switch (params->device_type) {
+	default:
+	case FIRA_DEVICE_TYPE_CONTROLLER:
+		slot_count = fira_session_get_slot_count(session);
+		round_index = get_round_index(session, block_index);
+		timestamp_dtu =
+			block_start_dtu + round_index * round_duration_dtu;
+		duration_dtu = slot_count * params->slot_duration_dtu;
+		if (max_duration_dtu &&
+		    is_before_dtu(next_timestamp_dtu + max_duration_dtu,
+				  timestamp_dtu + duration_dtu))
+			/* No way to start an access. */
+			return 0;
+		break;
+	case FIRA_DEVICE_TYPE_CONTROLEE:
+		if (session->controlee.synchronised) {
+			int margin_less, margin_more;
+
+			/*
+			 * Time graph to illustrate the controlee access
+			 * and its synchronization.
+			 *
+			 *    Next          Timestamp
+			 *  timestamp     without margin
+			 *      |             |
+			 *  ----x--------x----x----x-----> Time
+			 *               |<---|--->|
+			 *      Rx enabled         Rx timeout
+			 *    @timestamp_dtu
+			 *
+			 * rx_margin is the maximum margin accepted.
+			 */
+			round_index = get_round_index(session, block_index);
+			timestamp_dtu += round_index * round_duration_dtu;
+			margin_less = margin_more =
+				get_rx_margin_duration_dtu(local, session);
+			if (timestamp_dtu - next_timestamp_dtu < margin_less)
+				/*
+				 * Avoid to build a timestamp_dtu which is in
+				 * the past of next_timestamp_dtu.
+				 */
+				margin_less =
+					timestamp_dtu - next_timestamp_dtu;
+			timestamp_dtu -= margin_less;
+			rx_timeout_dtu = margin_less + margin_more;
+			duration_dtu = round_duration_dtu + margin_less;
+			if (max_duration_dtu &&
+			    is_before_dtu(next_timestamp_dtu + max_duration_dtu,
+					  timestamp_dtu + duration_dtu))
+				/* No way to start an access. */
+				return 0;
+		} else {
+			int unsync_max_duration_dtu =
+				params->block_duration_dtu +
+				params->slot_duration_dtu;
+
+			/*
+			 * A controlee not synchronized is allowed to start/end
+			 * anywhere in the block to find the controller.
+			 * But the session continue to work with block duration
+			 * to provide:
+			 *  - Regular reporting.
+			 *  - Time-sharing in multi-session/multi-region.
+			 *
+			 * Time graph:
+			 *
+			 *           unsync_max_duration_dtu
+			 *       |<----------------------------->|
+			 *       |                               |
+			 * --+---x-----------------------|-------x------>
+			 *   |        Block #x           |  Block #x + 1
+			 * --+---x-----------------------|---x---x------> Time
+			 *       |<------------------------->|<->|
+			 *             block duration         slot duration
+			 *
+			 * The unsync duration is bigger than the block, to
+			 * listen the medium for one block min. But to avoid
+			 * to be in late on the next access, we must add one
+			 * slot.
+			 */
+			if (max_duration_dtu &&
+			    is_before_dtu(next_timestamp_dtu + max_duration_dtu,
+					  timestamp_dtu +
+						  params->slot_duration_dtu))
+				/* No way to start an access. */
+				return 0;
+			else if (!max_duration_dtu ||
+				 is_before_dtu(timestamp_dtu +
+						       unsync_max_duration_dtu,
+					       next_timestamp_dtu +
+						       max_duration_dtu))
+				/* Maximum access granted. */
+				duration_dtu = unsync_max_duration_dtu;
+			else
+				/* Adjusted access duration. */
+				duration_dtu = next_timestamp_dtu +
+					       max_duration_dtu - timestamp_dtu;
+
+			/*
+			 * 'rx_timeout_dtu' is set to allow the reception
+			 * of the control frame close to the end of the
+			 * access, and so be synchronized for next block.
+			 *
+			 * But if the control message is received
+			 * at the end of access, the other frames
+			 * will be dropped to respect the duration_dtu.
+			 * See: rx control frame.
+			 */
+			rx_timeout_dtu =
+				duration_dtu - params->slot_duration_dtu;
+		}
+		break;
+	}
+
+	/*
+	 * Update the session demand (output):
+	 * - rx_timeout_dtu: Used only by the controlee.
+	 *
+	 * On the get_access, the session_demand will be applied
+	 * to the session. Otherwise the session_demand is dropped.
+	 *
+	 * In a way, session_demand represent the next access.
+	 */
+	*session_demand = (struct fira_session_demand){
+		.block_start_dtu = block_start_dtu,
+		.timestamp_dtu = timestamp_dtu,
+		.max_duration_dtu = duration_dtu,
+		.add_blocks = add_blocks,
+		.rx_timeout_dtu = rx_timeout_dtu,
+		.round_index = round_index,
+	};
+	trace_region_fira_session_fsm_active_get_demand_return(local, session,
+							       session_demand);
+	return 1;
+}
+
+static struct mcps802154_access *
+fira_session_fsm_active_get_access(struct fira_local *local,
+				   struct fira_session *session,
+				   const struct fira_session_demand *fsd)
+{
+	const struct fira_session_params *params = &session->params;
+	const struct mcps802154_hrp_uwb_params *hrp = &session->hrp_uwb_params;
+	struct mcps802154_access *access = &local->access;
+	int blocks_per_ranging;
+
+	/*
+	 *      ,     ,
+	 *     (\____/)              Important:
+	 *      (_oo_)
+	 *        (O)        It's almost forbidden to update session
+	 *      __||__    \) content for a controlee.
+	 *   []/______\[] /
+	 *   / \______/ \/   Because, the session can change on control
+	 *  /    /__\        frame reception (static STS only).
+	 * (\   /____\
+	 */
+	local->current_session = session;
+
+	/*
+	 * Update common access fields for controlee and controller.
+	 * hrp must stay const, see 'Important' above.
+	 */
+	access->method = MCPS802154_ACCESS_METHOD_MULTI;
+	access->frames = local->frames;
+	access->n_frames = 0;
+	access->channel = get_channel(local, session);
+	access->hrp_uwb_params = hrp;
+
+	/*
+	 * For the ranging round failure counter, consider these rounds as
+	 * failed. And reset the counter in the access_done if success.
+	 */
+	blocks_per_ranging = session->block_stride_len + 1;
+	session->n_ranging_round_retry += fsd->add_blocks / blocks_per_ranging;
+
+	/* Continue to 'device type' access. */
+	if (params->device_type == FIRA_DEVICE_TYPE_CONTROLLER)
+		return fira_get_access_controller(local, fsd);
+	return fira_get_access_controlee(local, fsd);
+}
+
+static void fira_session_fsm_active_access_done(struct fira_local *local,
+						struct fira_session *session,
+						bool error)
+{
+	const struct fira_session_params *params = &session->params;
+	const struct fira_measurement_sequence_step *step;
+	struct fira_report_info report_info = {
+		.ranging_data = local->ranging_info,
+		.n_ranging_data = local->n_ranging_info,
+		.stopped_controlees = local->stopped_controlees,
+		.n_stopped_controlees = local->n_stopped_controlees,
+		.diagnostics = local->diagnostics,
+		.slots = local->slots,
+		.n_slots = local->access.n_frames,
+	};
+	struct fira_ranging_info *ri;
+	int i;
+
+	/* Update local. */
+	local->current_session = NULL;
+
+	if (error) {
+		/*
+		 * FIXME:
+		 * Why corrupt all status, the last slot_index is not
+		 * enough?
+		 * TODO: Proposal:
+		 *  - Set INTERNAL_ERROR on status during the get_access.
+		 *  - Update status on tx_return/rx_frame.
+		 *  - Update testu which expect the wrong status.
+		 */
+		for (i = 0; i < local->n_ranging_info; i++) {
+			ri = &local->ranging_info[i];
+			ri->status = FIRA_STATUS_RANGING_INTERNAL_ERROR;
+		}
+	} else {
+		for (i = 0; i < local->n_ranging_info; i++) {
+			ri = &local->ranging_info[i];
+			if (ri->status != FIRA_STATUS_RANGING_SUCCESS)
+				break;
+		}
+		/* Reset ranging round failure counter. */
+		if (i == local->n_ranging_info)
+			session->n_ranging_round_retry = 0;
+	}
+
+	session->measurements.n_achieved++;
+	session->measurements.n_total_achieved++;
+	step = fira_session_get_meas_seq_step(session);
+	if (session->measurements.reset) {
+		/* Copy new measurement sequence. */
+		session->measurements.sequence = params->meas_seq;
+		session->measurements.index = 0;
+		session->measurements.n_achieved = 0;
+		session->measurements.reset = false;
+	} else if (session->measurements.n_achieved >= step->n_measurements) {
+		struct fira_measurement_sequence *seq =
+			&session->measurements.sequence;
+
+		session->measurements.n_achieved = 0;
+		session->measurements.index++;
+		session->measurements.index %= seq->n_steps;
+	}
+	/* Check max number of measurements. */
+	if (params->max_number_of_measurements &&
+	    session->measurements.n_total_achieved >=
+		    params->max_number_of_measurements) {
+		session->stop_request = true;
+	}
+
+	ranging_round_done(local, session, &report_info);
+}
+
+static void
+fira_session_fsm_active_check_missed_ranging(struct fira_local *local,
+					     struct fira_session *session,
+					     u32 timestamp_dtu)
+{
+	const struct fira_session_params *params = &session->params;
+	int next_block_start_dtu =
+		session->block_start_dtu + params->block_duration_dtu;
+	bool is_missed_ranging_round = false;
+
+	/*
+	 * Example of possible timings (without hopping):
+	 *
+	 *                          check(timestamp_dtu)
+	 *          Ok        Miss      Miss  |
+	 * Session: [--]      [--]      [--]  |   [--]
+	 *    ------x---------x---------------x--------> Time
+	 *          |         |
+	 * block_start_dtu    next_access_timestamp_dtu
+	 *
+	 * Tips:
+	 * - 'session->block_start_dtu' is the block start of the last access.
+	 * - 'session->next_access_timestamp_dtu' value can be:
+	 *   - Next block start when hopping is disabled.
+	 *   - Beyond the next block start when hopping is enabled.
+	 * - When the session haven't been check since a long time,
+	 *   many blocks could be missed.
+	 */
+
+	/* First, determine if there is missed ranging round. */
+	if (params->device_type == FIRA_DEVICE_TYPE_CONTROLEE &&
+	    !session->controlee.synchronised) {
+		/* Consider the block as missed when next block is reached. */
+		if (!is_before_dtu(timestamp_dtu, next_block_start_dtu))
+			is_missed_ranging_round = true;
+	} else if (is_before_dtu(session->next_access_timestamp_dtu,
+				 timestamp_dtu)) {
+		/* A late is not accepted here. */
+		is_missed_ranging_round = true;
+	}
+
+	/* Compute the number of missed ranging. */
+	if (is_missed_ranging_round) {
+		int blocks_per_ranging = session->block_stride_len + 1;
+		int add_blocks = 0;
+
+		/* Drift probably due to multi-session or multi-region. */
+		if (is_before_dtu(next_block_start_dtu, timestamp_dtu))
+			add_blocks = (timestamp_dtu - next_block_start_dtu) /
+				     params->block_duration_dtu;
+		if (add_blocks >= blocks_per_ranging) {
+			int n_ranging_failed = add_blocks / blocks_per_ranging;
+
+			if (params->max_rr_retry &&
+			    session->n_ranging_round_retry + n_ranging_failed >
+				    params->max_rr_retry) {
+				/*
+				 * Avoid to set a block index bigger than the
+				 * max ranging round retry in the report.
+				 */
+				n_ranging_failed =
+					params->max_rr_retry -
+					session->n_ranging_round_retry;
+			}
+			forward_to_next_ranging(session, n_ranging_failed);
+		}
+	}
+
+	/* Finally, do the missed ranging round report. */
+	if (is_missed_ranging_round) {
+		struct fira_report_info report_info = {};
+		__le16 *pend_del;
+		struct fira_ranging_info *ri;
+		int j, k;
+		struct fira_controlee *controlee;
+
+		/*
+		 *           \\\||||||////
+		 *            \\  ~ ~  //
+		 *             (  @ @  )
+		 * _________ oOOo-(_)-oOOo________________________________
+		 * WARN_RETURN_VOID_ON: Because the 'local' information will
+		 * be used until the end of this bloc.
+		 * So this function must not be called during an access,
+		 * to avoid to use a shared memory already used by current
+		 * session.
+		 * ________________Oooo.__________________________________
+		 *       .oooO     (   )
+		 *        (   )     ) /
+		 *         \ (     (_/
+		 *          \_)
+		 */
+		WARN_RETURN_VOID_ON(local->current_session);
+		/* Build a missed ranging round report. */
+		report_info.ranging_data = local->ranging_info;
+		switch (params->device_type) {
+		default:
+		case FIRA_DEVICE_TYPE_CONTROLLER:
+			pend_del = local->stopped_controlees;
+			j = k = 0;
+			list_for_each_entry (controlee,
+					     &session->current_controlees,
+					     entry) {
+				switch (controlee->state) {
+				case FIRA_CONTROLEE_STATE_RUNNING:
+				case FIRA_CONTROLEE_STATE_PENDING_STOP:
+				case FIRA_CONTROLEE_STATE_PENDING_DEL:
+					ri = &local->ranging_info[j];
+					*ri = (struct fira_ranging_info){
+						.short_addr =
+							controlee->short_addr,
+						.status =
+							FIRA_STATUS_RANGING_TX_FAILED,
+					};
+					j++;
+					break;
+				default:
+					pend_del[k++] = controlee->short_addr;
+					break;
+				}
+			}
+			report_info.stopped_controlees = pend_del;
+			report_info.n_stopped_controlees = k,
+			report_info.n_ranging_data = j;
+			break;
+		case FIRA_DEVICE_TYPE_CONTROLEE:
+			ri = &local->ranging_info[0];
+			*ri = (struct fira_ranging_info){
+				.short_addr = params->controller_short_addr,
+				.status = FIRA_STATUS_RANGING_RX_TIMEOUT,
+			};
+			report_info.n_ranging_data = 1;
+			break;
+		}
+		ranging_round_done(local, session, &report_info);
+	}
+}
+
+const struct fira_session_fsm_state fira_session_fsm_active = {
+	.id = FIRA_SESSION_STATE_ID_ACTIVE,
+	.enter = fira_session_fsm_active_enter,
+	.leave = fira_session_fsm_active_leave,
+	.check_parameters = fira_session_fsm_active_check_parameters,
+	.parameters_updated = fira_session_fsm_active_parameters_updated,
+	.start = fira_session_fsm_active_start,
+	.stop = fira_session_fsm_active_stop,
+	.get_demand = fira_session_fsm_active_get_demand,
+	.get_access = fira_session_fsm_active_get_access,
+	.access_done = fira_session_fsm_active_access_done,
+	.check_missed_ranging = fira_session_fsm_active_check_missed_ranging,
+};
diff --git a/mac/fira_session_fsm_active.h b/mac/fira_session_fsm_active.h
new file mode 100644
index 0000000..f6ad54a
--- /dev/null
+++ b/mac/fira_session_fsm_active.h
@@ -0,0 +1,31 @@
+/*
+ * 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.
+ */
+
+#ifndef NET_MCPS802154_FIRA_SESSION_FSM_ACTIVE_H
+#define NET_MCPS802154_FIRA_SESSION_FSM_ACTIVE_H
+
+#include "fira_session_fsm.h"
+
+extern const struct fira_session_fsm_state fira_session_fsm_active;
+
+#endif /* NET_MCPS802154_FIRA_SESSION_FSM_ACTIVE_H */
diff --git a/mac/fira_session_fsm_idle.c b/mac/fira_session_fsm_idle.c
new file mode 100644
index 0000000..fcbcf58
--- /dev/null
+++ b/mac/fira_session_fsm_idle.c
@@ -0,0 +1,169 @@
+/*
+ * 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 <net/mcps802154_frame.h>
+
+#include "fira_session_fsm_init.h"
+#include "fira_session_fsm_idle.h"
+#include "fira_session_fsm_active.h"
+#include "fira_session.h"
+#include "fira_trace.h"
+
+static void
+fira_session_fsm_idle_parameters_updated(struct fira_local *local,
+					 struct fira_session *session)
+{
+	const struct fira_session_params *params = &session->params;
+
+	if (session->measurements.reset) {
+		session->measurements.reset = false;
+		session->measurements.sequence = params->meas_seq;
+	}
+	if (!fira_session_is_ready(local, session)) {
+		fira_session_fsm_change_state(local, session,
+					      &fira_session_fsm_init);
+	}
+}
+
+static void
+fira_session_fsm_idle_controlee_list_updated(struct fira_local *local,
+					     struct fira_session *session)
+{
+	if (!fira_session_is_ready(local, session)) {
+		fira_session_fsm_change_state(local, session,
+					      &fira_session_fsm_init);
+	}
+}
+
+static int fira_session_fsm_idle_start(struct fira_local *local,
+				       struct fira_session *session,
+				       const struct genl_info *info)
+{
+	const struct fira_session_params *params = &session->params;
+	struct mcps802154_hrp_uwb_params *hrp = &session->hrp_uwb_params;
+	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++) {
+		const struct fira_measurement_sequence_step *step;
+
+		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 = mcps802154_get_current_timestamp_dtu(local->llhw, &now_dtu);
+	if (r)
+		return r;
+
+	/* Update session. */
+	session->event_portid = info->snd_portid;
+	session->block_start_valid = false;
+	session->block_index = 0;
+	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:
+		hrp->prf = MCPS802154_PRF_64;
+		break;
+	case FIRA_PRF_MODE_HPRF:
+		hrp->prf = MCPS802154_PRF_125;
+		break;
+	default:
+		hrp->prf = MCPS802154_PRF_250;
+		break;
+	}
+	hrp->psr = params->preamble_duration == FIRA_PREAMBULE_DURATION_64 ?
+			   MCPS802154_PSR_64 :
+			   MCPS802154_PSR_32;
+	hrp->sfd_selector = (enum mcps802154_sfd)params->sfd_id;
+	hrp->phr_hi_rate = params->phr_data_rate == FIRA_PHR_DATA_RATE_6M81;
+	switch (params->psdu_data_rate) {
+	default:
+	case FIRA_PSDU_DATA_RATE_6M81:
+		hrp->data_rate = MCPS802154_DATA_RATE_6M81;
+		break;
+	case FIRA_PSDU_DATA_RATE_7M80:
+		hrp->data_rate = MCPS802154_DATA_RATE_7M80;
+		break;
+	case FIRA_PSDU_DATA_RATE_27M2:
+		hrp->data_rate = MCPS802154_DATA_RATE_27M2;
+		break;
+	case FIRA_PSDU_DATA_RATE_31M2:
+		hrp->data_rate = MCPS802154_DATA_RATE_31M2;
+		break;
+	}
+	fira_session_fsm_change_state(local, session, &fira_session_fsm_active);
+
+	mcps802154_reschedule(local->llhw);
+	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_idle.h b/mac/fira_session_fsm_idle.h
new file mode 100644
index 0000000..a8dd0aa
--- /dev/null
+++ b/mac/fira_session_fsm_idle.h
@@ -0,0 +1,31 @@
+/*
+ * 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.
+ */
+
+#ifndef NET_MCPS802154_FIRA_SESSION_FSM_IDLE_H
+#define NET_MCPS802154_FIRA_SESSION_FSM_IDLE_H
+
+#include "fira_session_fsm.h"
+
+extern const struct fira_session_fsm_state fira_session_fsm_idle;
+
+#endif /* NET_MCPS802154_FIRA_SESSION_FSM_IDLE_H */
diff --git a/mac/fira_session_fsm_init.c b/mac/fira_session_fsm_init.c
new file mode 100644
index 0000000..5c12e62
--- /dev/null
+++ b/mac/fira_session_fsm_init.c
@@ -0,0 +1,77 @@
+/*
+ * 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 <net/mcps802154_frame.h>
+
+#include "fira_session_fsm_init.h"
+#include "fira_session_fsm_idle.h"
+#include "fira_session_fsm_active.h"
+#include "fira_session.h"
+
+static void fira_session_fsm_init_enter(struct fira_local *local,
+					struct fira_session *session)
+{
+	const struct fira_session_params *params = &session->params;
+
+	session->measurements.sequence = params->meas_seq;
+
+	if (fira_session_is_ready(local, session)) {
+		fira_session_fsm_change_state(local, session,
+					      &fira_session_fsm_idle);
+	}
+}
+
+void fira_session_fsm_init_parameters_updated(struct fira_local *local,
+					      struct fira_session *session)
+{
+	const struct fira_session_params *params = &session->params;
+
+	if (session->measurements.reset) {
+		session->measurements.reset = false;
+		session->measurements.sequence = params->meas_seq;
+	}
+	if (fira_session_is_ready(local, session)) {
+		fira_session_fsm_change_state(local, session,
+					      &fira_session_fsm_idle);
+	}
+}
+
+static void
+fira_session_fsm_init_controlee_list_updated(struct fira_local *local,
+					     struct fira_session *session)
+{
+	if (fira_session_is_ready(local, session)) {
+		fira_session_fsm_change_state(local, session,
+					      &fira_session_fsm_idle);
+	}
+}
+
+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_session_fsm_init.h b/mac/fira_session_fsm_init.h
new file mode 100644
index 0000000..5191a2e
--- /dev/null
+++ b/mac/fira_session_fsm_init.h
@@ -0,0 +1,31 @@
+/*
+ * 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.
+ */
+
+#ifndef NET_MCPS802154_FIRA_SESSION_FSM_INIT_H
+#define NET_MCPS802154_FIRA_SESSION_FSM_INIT_H
+
+#include "fira_session_fsm.h"
+
+extern const struct fira_session_fsm_state fira_session_fsm_init;
+
+#endif /* NET_MCPS802154_FIRA_SESSION_FSM_INIT_H */
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 8e89247..58410a4 100644
--- a/mac/fira_trace.h
+++ b/mac/fira_trace.h
@@ -1,7 +1,7 @@
 /*
  * This file is part of the UWB stack for linux.
  *
- * Copyright (c) 2020-2021 Qorvo US, Inc.
+ * 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.
@@ -30,6 +30,7 @@
 #include <linux/tracepoint.h>
 #include "fira_session.h"
 #include "net/fira_region_params.h"
+#include <net/fira_region_nl.h>
 
 /* clang-format off */
 
@@ -82,6 +83,18 @@ TRACE_DEFINE_ENUM(FIRA_RFRAME_CONFIG_SP3);
 TRACE_DEFINE_ENUM(FIRA_PREAMBULE_DURATION_32);
 TRACE_DEFINE_ENUM(FIRA_PREAMBULE_DURATION_64);
 
+#define FIRA_STS_SEGMENTS_SYMBOLS    		\
+	{ FIRA_STS_SEGMENTS_0, "0" },	\
+	{ FIRA_STS_SEGMENTS_1, "1" },  \
+	{ FIRA_STS_SEGMENTS_2, "2" },  \
+	{ FIRA_STS_SEGMENTS_3, "3" },  \
+	{ FIRA_STS_SEGMENTS_4, "4" }
+TRACE_DEFINE_ENUM(FIRA_STS_SEGMENTS_0);
+TRACE_DEFINE_ENUM(FIRA_STS_SEGMENTS_1);
+TRACE_DEFINE_ENUM(FIRA_STS_SEGMENTS_2);
+TRACE_DEFINE_ENUM(FIRA_STS_SEGMENTS_3);
+TRACE_DEFINE_ENUM(FIRA_STS_SEGMENTS_4);
+
 #define FIRA_PSDU_DATA_RATE_SYMBOLS            \
 	{ FIRA_PSDU_DATA_RATE_6M81, "6M81" },  \
 	{ FIRA_PSDU_DATA_RATE_7M80, "7M80" },  \
@@ -93,9 +106,9 @@ TRACE_DEFINE_ENUM(FIRA_PSDU_DATA_RATE_27M2);
 TRACE_DEFINE_ENUM(FIRA_PSDU_DATA_RATE_31M2);
 
 #define FIRA_PHR_DATA_RATE_SYMBOLS            \
-	{ FIRA_PHR_DATA_RATE_850k, "850k" },  \
+	{ FIRA_PHR_DATA_RATE_850K, "850k" },  \
 	{ FIRA_PHR_DATA_RATE_6M81, "6M81" }
-TRACE_DEFINE_ENUM(FIRA_PHR_DATA_RATE_850k);
+TRACE_DEFINE_ENUM(FIRA_PHR_DATA_RATE_850K);
 TRACE_DEFINE_ENUM(FIRA_PHR_DATA_RATE_6M81);
 
 #define FIRA_MAC_FCS_TYPE_CRC_SYMBOLS        \
@@ -104,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" }, \
@@ -128,25 +145,39 @@ TRACE_DEFINE_ENUM(FIRA_MESSAGE_ID_MEASUREMENT_REPORT);
 TRACE_DEFINE_ENUM(FIRA_MESSAGE_ID_RESULT_REPORT);
 TRACE_DEFINE_ENUM(FIRA_MESSAGE_ID_CONTROL_UPDATE);
 
-#define FIRA_RANGING_STATUS                                                   \
-	{ FIRA_STATUS_RANGING_SUCCESS, "success" },                           \
-	{ FIRA_STATUS_RANGING_TX_FAILED, "tx_failed" },                       \
-	{ FIRA_STATUS_RANGING_RX_TIMEOUT, "rx_timeout" },                     \
-	{ FIRA_STATUS_RANGING_RX_PHY_DEC_FAILED, "rx_phy_dec_failed" },       \
-	{ FIRA_STATUS_RANGING_RX_PHY_TOA_FAILED, "rx_phy_toa_failed" },       \
-	{ FIRA_STATUS_RANGING_RX_PHY_STS_FAILED, "rx_phy_sts_failed" },       \
-	{ FIRA_STATUS_RANGING_RX_MAC_DEC_FAILED, "rx_mac_dec_failed" },       \
-	{ FIRA_STATUS_RANGING_RX_MAC_IE_DEC_FAILED, "rx_mac_ie_dec_failed" }, \
-	{ FIRA_STATUS_RANGING_RX_MAC_IE_MISSING, "rx_mac_ie_missing" }
-TRACE_DEFINE_ENUM(FIRA_STATUS_RANGING_SUCCESS);
-TRACE_DEFINE_ENUM(FIRA_STATUS_RANGING_TX_FAILED);
-TRACE_DEFINE_ENUM(FIRA_STATUS_RANGING_RX_TIMEOUT);
-TRACE_DEFINE_ENUM(FIRA_STATUS_RANGING_RX_PHY_DEC_FAILED);
-TRACE_DEFINE_ENUM(FIRA_STATUS_RANGING_RX_PHY_TOA_FAILED);
-TRACE_DEFINE_ENUM(FIRA_STATUS_RANGING_RX_PHY_STS_FAILED);
-TRACE_DEFINE_ENUM(FIRA_STATUS_RANGING_RX_MAC_DEC_FAILED);
-TRACE_DEFINE_ENUM(FIRA_STATUS_RANGING_RX_MAC_IE_DEC_FAILED);
-TRACE_DEFINE_ENUM(FIRA_STATUS_RANGING_RX_MAC_IE_MISSING);
+#define mcps802154_rx_error_name(name)                               \
+	{                                                            \
+		MCPS802154_RX_ERROR_##name, #name                    \
+	}
+#define MCPS802154_RX_ERROR_SYMBOLS                                  \
+	mcps802154_rx_error_name(NONE),                              \
+	mcps802154_rx_error_name(TIMEOUT),                           \
+	mcps802154_rx_error_name(BAD_CKSUM),                         \
+	mcps802154_rx_error_name(UNCORRECTABLE),                     \
+	mcps802154_rx_error_name(FILTERED),                          \
+	mcps802154_rx_error_name(SFD_TIMEOUT),                       \
+	mcps802154_rx_error_name(PHR_DECODE),                        \
+	mcps802154_rx_error_name(OTHER)
+TRACE_DEFINE_ENUM(MCPS802154_RX_ERROR_NONE);
+TRACE_DEFINE_ENUM(MCPS802154_RX_ERROR_TIMEOUT);
+TRACE_DEFINE_ENUM(MCPS802154_RX_ERROR_BAD_CKSUM);
+TRACE_DEFINE_ENUM(MCPS802154_RX_ERROR_UNCORRECTABLE);
+TRACE_DEFINE_ENUM(MCPS802154_RX_ERROR_FILTERED);
+TRACE_DEFINE_ENUM(MCPS802154_RX_ERROR_SFD_TIMEOUT);
+TRACE_DEFINE_ENUM(MCPS802154_RX_ERROR_PHR_DECODE);
+TRACE_DEFINE_ENUM(MCPS802154_RX_ERROR_OTHER);
+
+#define mcps802154_tx_reason_name(name)                              \
+	{                                                            \
+		MCPS802154_ACCESS_TX_RETURN_REASON_##name, #name     \
+	}
+#define MCPS802154_TX_REASON_SYMBOLS                                 \
+	mcps802154_tx_reason_name(CONSUMED),                         \
+	mcps802154_tx_reason_name(FAILURE),                          \
+	mcps802154_tx_reason_name(CANCEL)
+TRACE_DEFINE_ENUM(MCPS802154_ACCESS_TX_RETURN_REASON_CONSUMED);
+TRACE_DEFINE_ENUM(MCPS802154_ACCESS_TX_RETURN_REASON_FAILURE);
+TRACE_DEFINE_ENUM(MCPS802154_ACCESS_TX_RETURN_REASON_CANCEL);
 
 #define FIRA_MEAS_SEQ_STEP_TYPE                                   \
 	{ FIRA_MEASUREMENT_TYPE_RANGE, "range" },                   \
@@ -161,6 +192,56 @@ TRACE_DEFINE_ENUM(FIRA_MEASUREMENT_TYPE_AOA_AZIMUTH);
 TRACE_DEFINE_ENUM(FIRA_MEASUREMENT_TYPE_AOA_ELEVATION);
 TRACE_DEFINE_ENUM(FIRA_MEASUREMENT_TYPE_AOA_AZIMUTH_ELEVATION);
 
+#define fira_session_state_id_name(name)                             \
+	{                                                            \
+		FIRA_SESSION_STATE_ID_##name, #name                  \
+	}
+#define FIRA_SESSION_STATE_ID_SYMBOLS                                \
+	fira_session_state_id_name(DEINIT),                          \
+	fira_session_state_id_name(INIT),                            \
+	fira_session_state_id_name(ACTIVE),                          \
+	fira_session_state_id_name(IDLE)
+TRACE_DEFINE_ENUM(FIRA_SESSION_STATE_ID_DEINIT);
+TRACE_DEFINE_ENUM(FIRA_SESSION_STATE_ID_INIT);
+TRACE_DEFINE_ENUM(FIRA_SESSION_STATE_ID_ACTIVE);
+TRACE_DEFINE_ENUM(FIRA_SESSION_STATE_ID_IDLE);
+
+
+#define fira_call_name(name)                                         \
+	{                                                            \
+		FIRA_CALL_##name, #name                              \
+	}
+#define FIRA_CALL_SYMBOLS                                            \
+	fira_call_name(GET_CAPABILITIES),                            \
+	fira_call_name(SESSION_INIT),                                \
+	fira_call_name(SESSION_START),                               \
+	fira_call_name(SESSION_STOP),                                \
+	fira_call_name(SESSION_DEINIT),                              \
+	fira_call_name(SESSION_SET_PARAMS),                          \
+	fira_call_name(NEW_CONTROLEE),                               \
+	fira_call_name(DEL_CONTROLEE),                               \
+	fira_call_name(SESSION_NOTIFICATION),                        \
+	fira_call_name(SESSION_GET_PARAMS),                          \
+	fira_call_name(SESSION_GET_STATE),                           \
+	fira_call_name(SESSION_GET_COUNT),                           \
+	fira_call_name(SET_CONTROLEE),                               \
+	fira_call_name(GET_CONTROLEES)
+TRACE_DEFINE_ENUM(FIRA_CALL_GET_CAPABILITIES);
+TRACE_DEFINE_ENUM(FIRA_CALL_SESSION_INIT);
+TRACE_DEFINE_ENUM(FIRA_CALL_SESSION_START);
+TRACE_DEFINE_ENUM(FIRA_CALL_SESSION_STOP);
+TRACE_DEFINE_ENUM(FIRA_CALL_SESSION_DEINIT);
+TRACE_DEFINE_ENUM(FIRA_CALL_SESSION_SET_PARAMS);
+TRACE_DEFINE_ENUM(FIRA_CALL_NEW_CONTROLEE);
+TRACE_DEFINE_ENUM(FIRA_CALL_DEL_CONTROLEE);
+TRACE_DEFINE_ENUM(FIRA_CALL_SESSION_NOTIFICATION);
+TRACE_DEFINE_ENUM(FIRA_CALL_SESSION_GET_PARAMS);
+TRACE_DEFINE_ENUM(FIRA_CALL_SESSION_GET_STATE);
+TRACE_DEFINE_ENUM(FIRA_CALL_SESSION_GET_COUNT);
+TRACE_DEFINE_ENUM(FIRA_CALL_SET_CONTROLEE);
+TRACE_DEFINE_ENUM(FIRA_CALL_GET_CONTROLEES);
+
+
 TRACE_EVENT(region_fira_session_params,
 	TP_PROTO(const struct fira_session *session,
 		 const struct fira_session_params *params),
@@ -185,15 +266,21 @@ TRACE_EVENT(region_fira_session_params,
 		__field(enum fira_rframe_config, rframe_config)
 		__field(enum fira_preambule_duration, preamble_duration)
 		__field(enum fira_sfd_id, sfd_id)
+		__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)
 		__field(bool, report_aoa_elevation)
 		__field(bool, report_aoa_fom)
+		__field(bool, report_diagnostics)
 		),
 	TP_fast_assign(
 		FIRA_SESSION_ASSIGN;
@@ -215,24 +302,32 @@ TRACE_EVENT(region_fira_session_params,
 		__entry->rframe_config = params->rframe_config;
 		__entry->preamble_duration = params->preamble_duration;
 		__entry->sfd_id = params->sfd_id;
+		__entry->number_of_sts_segments = params->number_of_sts_segments;
 		__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;
 		__entry->report_aoa_elevation = params->report_aoa_elevation;
 		__entry->report_aoa_fom = params->report_aoa_fom;
+		__entry->report_diagnostics = params->report_diagnostics;
 		),
 	TP_printk(FIRA_SESSION_PR_FMT " device_type=%s ranging_round_usage=%s multi_node_mode=%s "
 		  "controller_short_addr=0x%x initiation_time_ms=%d slot_duration_dtu=%d "
 		  "block_duration_dtu=%d block_stride_len=%d max_nb_of_measurements=%d "
 		  "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 psdu_data_rate=%s mac_fcs_type=%s "
-		  "sts_config=%s vupper64=%s aoa_result_req=%d report_tof=%d report_aoa_azimuth=%d "
-		  "report_aoa_elevation=%d report_aoa_fom=%d",
+		  "preamble_duration=%s sfd_id=%d number_of_sts_segments=%s psdu_data_rate=%s mac_fcs_type=%s "
+		  "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),
 		  __print_symbolic(__entry->ranging_round_usage, FIRA_RANGING_ROUND_SYMBOLS),
@@ -252,41 +347,250 @@ TRACE_EVENT(region_fira_session_params,
 		  __print_symbolic(__entry->rframe_config, FIRA_RFRAME_CONFIG_SYMBOLS),
 		  __print_symbolic(__entry->preamble_duration, FIRA_PREAMBULE_DURATION_SYMBOLS),
 		  __entry->sfd_id,
+		  __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,
 		  __entry->report_aoa_elevation,
-		  __entry->report_aoa_fom
+		  __entry->report_aoa_fom,
+		  __entry->report_diagnostics
+		)
+	);
+
+TRACE_EVENT(region_fira_session_meas_seq_params,
+	TP_PROTO(const struct fira_session *session,
+		 const struct fira_measurement_sequence_step *step,
+		 int index),
+	TP_ARGS(session, step, index),
+	TP_STRUCT__entry(
+		FIRA_SESSION_ENTRY
+		__field(int, index)
+		__field(enum fira_measurement_type, type)
+		__field(u8, n_measurements)
+		__field(s8, rx_ant_set_nonranging)
+		__field(s8, rx_ant_sets_ranging_0)
+		__field(s8, rx_ant_sets_ranging_1)
+		__field(s8, tx_ant_set_nonranging)
+		__field(s8, tx_ant_set_ranging)
+	),
+	TP_fast_assign(
+		FIRA_SESSION_ASSIGN;
+		__entry->index = index;
+		__entry->type = step->type;
+		__entry->n_measurements = step->n_measurements;
+		__entry->rx_ant_set_nonranging = step->rx_ant_set_nonranging;
+		__entry->rx_ant_sets_ranging_0 = step->rx_ant_sets_ranging[0];
+		__entry->rx_ant_sets_ranging_1 = step->rx_ant_sets_ranging[1];
+		__entry->tx_ant_set_nonranging = step->tx_ant_set_nonranging;
+		__entry->tx_ant_set_ranging = step->tx_ant_set_ranging;
+		),
+	TP_printk(FIRA_SESSION_PR_FMT " index=%d type=%s n_measurements=%d "
+		  "rx_ant_set_nonranging=%d rx_ant_sets_ranging_0=%d "
+		  "rx_ant_sets_ranging_1=%d tx_ant_set_nonranging=%d "
+		  "tx_ant_set_ranging=%d",
+		  FIRA_SESSION_PR_ARG,
+		__entry->index,
+		__print_symbolic(__entry->type, FIRA_MEAS_SEQ_STEP_TYPE),
+		__entry->n_measurements,
+		__entry->rx_ant_set_nonranging,
+		__entry->rx_ant_sets_ranging_0,
+		__entry->rx_ant_sets_ranging_1,
+		__entry->tx_ant_set_nonranging,
+		__entry->tx_ant_set_ranging)
+	);
+
+TRACE_EVENT(region_fira_session_control,
+	TP_PROTO(const struct fira_local *local,
+		 int session_id, enum fira_call call_id),
+	TP_ARGS(local, session_id, call_id),
+	TP_STRUCT__entry(
+		FIRA_SESSION_ENTRY
+		__field(enum fira_call, call_id)
+		),
+	TP_fast_assign(
+		__entry->session_id = session_id;
+		__entry->call_id = call_id;
+		),
+	TP_printk(FIRA_SESSION_PR_FMT " call_id=%s",
+		FIRA_SESSION_PR_ARG,
+		__print_symbolic(__entry->call_id, FIRA_CALL_SYMBOLS)
+		)
+	);
+
+TRACE_EVENT(
+	region_fira_session_fsm_active_get_demand_return,
+	TP_PROTO(const struct fira_local *local,
+		 const struct fira_session *session,
+		 const struct fira_session_demand *fsd),
+	TP_ARGS(local, session, fsd),
+	TP_STRUCT__entry(
+		FIRA_SESSION_ENTRY
+		__field(u32, block_start_dtu)
+		__field(u32, timestamp_dtu)
+		__field(int, max_duration_dtu)
+		),
+	TP_fast_assign(
+		FIRA_SESSION_ASSIGN;
+		__entry->block_start_dtu = fsd->block_start_dtu;
+		__entry->timestamp_dtu = fsd->timestamp_dtu;
+		__entry->max_duration_dtu = fsd->max_duration_dtu;
+		),
+	TP_printk(FIRA_SESSION_PR_FMT " block_start_dtu=%#x "
+		  "timestamp_dtu=%#x max_duration_dtu=%d",
+		  FIRA_SESSION_PR_ARG,
+		  __entry->block_start_dtu,
+		  __entry->timestamp_dtu,
+		  __entry->max_duration_dtu
+		)
+	);
+
+TRACE_EVENT(
+	region_fira_get_access_controller,
+	TP_PROTO(const struct fira_local *local,
+		 const struct fira_session *session,
+		 const struct fira_session_demand *fsd),
+	TP_ARGS(local, session, fsd),
+	TP_STRUCT__entry(
+		FIRA_SESSION_ENTRY
+		__field(int, block_index)
+		__field(int, add_blocks)
+		__field(int, round_index)
+		__field(int, block_stride_len)
+		__field(u32, block_start_dtu)
+		__field(u32, timestamp_dtu)
+		__field(int, max_duration_dtu)
+		),
+	TP_fast_assign(
+		FIRA_SESSION_ASSIGN;
+		__entry->block_index = session->block_index;
+		__entry->add_blocks = fsd->add_blocks;
+		__entry->round_index = fsd->round_index;
+		__entry->block_stride_len = session->block_stride_len;
+		__entry->block_start_dtu = fsd->block_start_dtu;
+		__entry->timestamp_dtu = fsd->timestamp_dtu;
+		__entry->max_duration_dtu = fsd->max_duration_dtu;
+		),
+	TP_printk(FIRA_SESSION_PR_FMT " block_index=%d add_blocks=%d "
+		  "round_index=%d block_stride_len=%d block_start_dtu=%#x "
+		  "timestamp_dtu=%#x max_duration_dtu=%d",
+		  FIRA_SESSION_PR_ARG,
+		  __entry->block_index,
+		  __entry->add_blocks,
+		  __entry->round_index,
+		  __entry->block_stride_len,
+		  __entry->block_start_dtu,
+		  __entry->timestamp_dtu,
+		  __entry->max_duration_dtu
 		)
 	);
 
-TRACE_EVENT(region_fira_rx_message,
+TRACE_EVENT(
+	region_fira_get_access_controlee,
+	TP_PROTO(const struct fira_local *local,
+		 const struct fira_session *session,
+		 const struct fira_session_demand *fsd),
+	TP_ARGS(local, session, fsd),
+	TP_STRUCT__entry(
+		FIRA_SESSION_ENTRY
+		__field(int, block_index)
+		__field(int, add_blocks)
+		__field(int, round_index)
+		__field(u32, block_start_dtu)
+		__field(u32, timestamp_dtu)
+		__field(int, max_duration_dtu)
+		),
+	TP_fast_assign(
+		FIRA_SESSION_ASSIGN;
+		__entry->block_index = session->block_index;
+		__entry->add_blocks = fsd->add_blocks;
+		__entry->round_index = fsd->round_index;
+		__entry->block_start_dtu = fsd->block_start_dtu;
+		__entry->timestamp_dtu = fsd->timestamp_dtu;
+		__entry->max_duration_dtu = fsd->max_duration_dtu;
+		),
+	TP_printk(FIRA_SESSION_PR_FMT " block_index=%d add_blocks=%d "
+		  "round_index=%d block_start_dtu=%#x timestamp_dtu=%#x "
+		  "max_duration_dtu=%d",
+		  FIRA_SESSION_PR_ARG,
+		  __entry->block_index,
+		  __entry->add_blocks,
+		  __entry->round_index,
+		  __entry->block_start_dtu,
+		  __entry->timestamp_dtu,
+		  __entry->max_duration_dtu
+		)
+	);
+
+TRACE_EVENT(region_fira_rx_frame,
 	TP_PROTO(const struct fira_session *session,
 		 enum fira_message_id message_id,
-		 enum fira_ranging_status status),
-	TP_ARGS(session, message_id, status),
+		 enum mcps802154_rx_error_type error),
+	TP_ARGS(session, message_id, error),
 	TP_STRUCT__entry(
 		FIRA_SESSION_ENTRY
 		__field(enum fira_message_id, message_id)
-		__field(enum fira_ranging_status, status)
+		__field(enum mcps802154_rx_error_type, error)
 		),
 	TP_fast_assign(
 		FIRA_SESSION_ASSIGN;
 		__entry->message_id = message_id;
-		__entry->status = status;
+		__entry->error = error;
 		),
-	TP_printk(FIRA_SESSION_PR_FMT " message_id=%s status=%s",
+	TP_printk(FIRA_SESSION_PR_FMT " message_id=%s error=%s",
 		FIRA_SESSION_PR_ARG,
 		__print_symbolic(__entry->message_id, FIRA_MESSAGE_TYPE),
-		__print_symbolic(__entry->status, FIRA_RANGING_STATUS)
+		__print_symbolic(__entry->error, MCPS802154_RX_ERROR_SYMBOLS)
+		)
+	);
+
+TRACE_EVENT(region_fira_rx_frame_control,
+	TP_PROTO(const struct fira_local *local,
+		 const struct fira_session *session,
+		 int left_duration_dtu, int n_slots),
+	TP_ARGS(local, session, left_duration_dtu, n_slots),
+	TP_STRUCT__entry(
+		FIRA_SESSION_ENTRY
+		__field(u32, block_start_dtu)
+		__field(int, block_index)
+		__field(int, round_index)
+		__field(bool, stop_inband)
+		__field(int, left_duration_dtu)
+		__field(int, n_slots)
+		),
+	TP_fast_assign(
+		FIRA_SESSION_ASSIGN;
+		__entry->block_start_dtu = session->block_start_dtu;
+		__entry->block_index = session->block_index;
+		__entry->round_index = session->round_index;
+		__entry->stop_inband = session->stop_inband;
+		__entry->left_duration_dtu = left_duration_dtu;
+		__entry->n_slots = n_slots;
+		),
+	TP_printk(FIRA_SESSION_PR_FMT " block_start_dtu=%#x block_index=%d "
+		  "round_index=%d stop_inband=%s left_duration_dtu=%d n_slots=%d",
+		FIRA_SESSION_PR_ARG,
+		__entry->block_start_dtu,
+		__entry->block_index,
+		__entry->round_index,
+		__entry->stop_inband ? "true": "false",
+		__entry->left_duration_dtu,
+		__entry->n_slots
 		)
 	);
 
-TRACE_EVENT(region_fira_tx_message,
+TRACE_EVENT(region_fira_tx_get_frame,
 	TP_PROTO(const struct fira_session *session,
 		 enum fira_message_id message_id),
 	TP_ARGS(session, message_id),
@@ -304,56 +608,140 @@ TRACE_EVENT(region_fira_tx_message,
 		)
 	);
 
-TRACE_EVENT(fira_nondeferred_not_supported,
-	TP_PROTO(const struct fira_session *session),
-	TP_ARGS(session),
-	TP_STRUCT__entry(FIRA_SESSION_ENTRY),
-	TP_fast_assign(FIRA_SESSION_ASSIGN;),
-	TP_printk(FIRA_SESSION_PR_FMT "FiRa non-deferred mode ranging not supported yet",
-		  FIRA_SESSION_PR_ARG
+TRACE_EVENT(region_fira_tx_return,
+	TP_PROTO(const struct fira_session *session,
+		 enum mcps802154_access_tx_return_reason reason),
+	TP_ARGS(session, reason),
+	TP_STRUCT__entry(
+		FIRA_SESSION_ENTRY
+		__field(enum mcps802154_access_tx_return_reason, reason)
+		),
+	TP_fast_assign(
+		FIRA_SESSION_ASSIGN;
+		__entry->reason = reason;
+		),
+	TP_printk(FIRA_SESSION_PR_FMT " reason=%s",
+		FIRA_SESSION_PR_ARG,
+		  __print_symbolic(__entry->reason,
+				   MCPS802154_TX_REASON_SYMBOLS)
+		)
+	);
+
+TRACE_EVENT(
+	region_fira_session_fsm_change_state,
+	TP_PROTO(const struct fira_session *session, enum fira_session_state_id new_state_id),
+	TP_ARGS(session, new_state_id),
+	TP_STRUCT__entry(
+		FIRA_SESSION_ENTRY
+		__field(enum fira_session_state_id, new_state_id)
+		),
+	TP_fast_assign(
+		FIRA_SESSION_ASSIGN;
+		__entry->new_state_id = new_state_id;
+		),
+	TP_printk(FIRA_SESSION_PR_FMT " new_state_id=%s",
+		  FIRA_SESSION_PR_ARG,
+		  __print_symbolic(__entry->new_state_id,
+				   FIRA_SESSION_STATE_ID_SYMBOLS)
 		)
 	);
 
-TRACE_EVENT(region_fira_meas_seq_step,
+TRACE_EVENT(
+	region_fira_access_done,
+	TP_PROTO(const struct fira_local *local,
+		 const struct fira_session *session,
+		 int access_duration_dtu, bool error),
+	TP_ARGS(local, session, access_duration_dtu, error),
+	TP_STRUCT__entry(
+		FIRA_SESSION_ENTRY
+		__field(int, access_duration_dtu)
+		__field(bool, error)
+		),
+	TP_fast_assign(
+		FIRA_SESSION_ASSIGN;
+		__entry->access_duration_dtu = access_duration_dtu;
+		__entry->error = error;
+		),
+	TP_printk(FIRA_SESSION_PR_FMT " access_duration_dtu=%d error=%s",
+		  FIRA_SESSION_PR_ARG,
+		  __entry->access_duration_dtu,
+		  __entry->error ? "true": "false"
+		)
+	);
+
+TRACE_EVENT(
+	region_fira_is_controlee_synchronised,
 	TP_PROTO(const struct fira_session *session,
-		 const struct fira_measurement_sequence_step *step,
-		 u8 current_step),
-	TP_ARGS(session, step, current_step),
+		 int drift_ppm, int rx_margin_ppm),
+	TP_ARGS(session, drift_ppm, rx_margin_ppm),
 	TP_STRUCT__entry(
 		FIRA_SESSION_ENTRY
-		__field(u8, step_nb)
-		__field(enum fira_measurement_type, type)
-		__field(u8, n_measurements)
-		__field(s8, rx_ant_set_nonranging)
-		__field(s8, rx_ant_sets_ranging_0)
-		__field(s8, rx_ant_sets_ranging_1)
-		__field(s8, tx_ant_set_nonranging)
-		__field(s8, tx_ant_set_ranging)
-	),
+		__field(int, block_index_sync)
+		__field(int, drift_ppm)
+		__field(int, rx_margin_ppm)
+		),
 	TP_fast_assign(
 		FIRA_SESSION_ASSIGN;
-		__entry->step_nb = current_step;
-		__entry->type = step->type;
-		__entry->n_measurements = step->n_measurements;
-		__entry->rx_ant_set_nonranging = step->rx_ant_set_nonranging;
-		__entry->rx_ant_sets_ranging_0 = step->rx_ant_sets_ranging[0];
-		__entry->rx_ant_sets_ranging_1 = step->rx_ant_sets_ranging[1];
-		__entry->tx_ant_set_nonranging = step->tx_ant_set_nonranging;
-		__entry->tx_ant_set_ranging = step->tx_ant_set_ranging;
+		__entry->block_index_sync = session->controlee.block_index_sync;
+		__entry->drift_ppm = drift_ppm;
+		__entry->rx_margin_ppm = rx_margin_ppm;
 		),
-	TP_printk(FIRA_SESSION_PR_FMT " step #%d : type=%s, n_measurements=%d, "
-		  "ant_sets : RxNR=%d, RxR[0]=%d, RxR[1]=%d, TxNR=%d, TxR=%d",
+	TP_printk(FIRA_SESSION_PR_FMT " block_index_sync=%d drift_ppm=%d rx_margin_ppm=%d",
 		  FIRA_SESSION_PR_ARG,
-		__entry->step_nb,
-		__print_symbolic(__entry->type, FIRA_MEAS_SEQ_STEP_TYPE),
-		__entry->n_measurements,
-		__entry->rx_ant_set_nonranging,
-		__entry->rx_ant_sets_ranging_0,
-		__entry->rx_ant_sets_ranging_1,
-		__entry->tx_ant_set_nonranging,
-		__entry->tx_ant_set_ranging)
+		  __entry->block_index_sync,
+		  __entry->drift_ppm,
+		  __entry->rx_margin_ppm
+		)
+	);
+
+TRACE_EVENT(
+	region_fira_session_report,
+	TP_PROTO(const struct fira_session *session,
+		 const struct fira_report_info *report_info),
+	TP_ARGS(session, report_info),
+	TP_STRUCT__entry(
+		FIRA_SESSION_ENTRY
+		__field(int, sequence_number)
+		__field(int, block_index)
+		__field(int, n_ranging_data)
+		__field(int, n_stopped_controlees)
+		__field(int, n_slots)
+		__field(bool, stopped)
+		),
+	TP_fast_assign(
+		FIRA_SESSION_ASSIGN;
+		__entry->sequence_number = session->sequence_number;
+		__entry->block_index = session->block_index;
+		__entry->n_ranging_data = report_info->n_ranging_data;
+		__entry->n_stopped_controlees = report_info->n_stopped_controlees;
+		__entry->n_slots = report_info->n_slots;
+		__entry->stopped = report_info->stopped;
+		),
+	TP_printk(FIRA_SESSION_PR_FMT " sequence_number=%d block_index=%d "
+		  "n_ranging_data=%d n_stopped_controlees=%d n_slots=%d stopped=%s",
+		  FIRA_SESSION_PR_ARG,
+		  __entry->sequence_number,
+		  __entry->block_index,
+		  __entry->n_ranging_data,
+		  __entry->n_stopped_controlees,
+		  __entry->n_slots,
+		  __entry->stopped ? "true": "false"
+		)
 	);
 
+TRACE_EVENT(fira_nondeferred_not_supported,
+	TP_PROTO(const struct fira_session *session),
+	TP_ARGS(session),
+	TP_STRUCT__entry(
+		FIRA_SESSION_ENTRY
+		),
+	TP_fast_assign(
+		FIRA_SESSION_ASSIGN;
+		),
+	TP_printk(FIRA_SESSION_PR_FMT,
+		  FIRA_SESSION_PR_ARG
+		)
+	);
 
 #endif /* !FIRA_TRACE_H || TRACE_HEADER_MULTI_READ */
 
diff --git a/mac/fproc.c b/mac/fproc.c
index 0026f97..4e1274c 100644
--- a/mac/fproc.c
+++ b/mac/fproc.c
@@ -25,6 +25,25 @@
 #include "mcps802154_i.h"
 #include "llhw-ops.h"
 
+bool mcps802154_fproc_is_non_recoverable_error(struct mcps802154_access *access)
+{
+	bool rc = false;
+	if (access->error < 0) {
+		switch (access->error) {
+			case -ETIME:
+			case -EIO:
+			case -EAGAIN:
+				rc = false;
+				break;
+			default:
+				pr_err("mcps: error %d is not recoverable", access->error);
+				rc = true;
+				break;
+		}
+	}
+	return rc;
+}
+
 void mcps802154_fproc_init(struct mcps802154_local *local)
 {
 	local->fproc.state = &mcps802154_fproc_stopped;
@@ -37,6 +56,7 @@ void mcps802154_fproc_uninit(struct mcps802154_local *local)
 	WARN_ON(local->fproc.access);
 	WARN_ON(local->fproc.tx_skb);
 	WARN_ON(local->started);
+	WARN_ON(local->fproc.deferred);
 }
 
 void mcps802154_fproc_change_state(
@@ -54,7 +74,6 @@ void mcps802154_fproc_access(struct mcps802154_local *local,
 			     u32 next_timestamp_dtu)
 {
 	struct mcps802154_access *access;
-	int r;
 
 	if (!local->start_stop_request) {
 		mcps802154_fproc_stopped_handle(local);
@@ -72,30 +91,35 @@ void mcps802154_fproc_access(struct mcps802154_local *local,
 
 	switch (access->method) {
 	case MCPS802154_ACCESS_METHOD_NOTHING:
-		r = mcps802154_fproc_nothing_handle(local, access);
+		access->error = mcps802154_fproc_nothing_handle(local, access);
+		break;
+	case MCPS802154_ACCESS_METHOD_IDLE:
+		access->error = mcps802154_fproc_idle_handle(local, access);
 		break;
 	case MCPS802154_ACCESS_METHOD_IMMEDIATE_RX:
-		r = mcps802154_fproc_rx_handle(local, access);
+		access->error = mcps802154_fproc_rx_handle(local, access);
 		break;
 	case MCPS802154_ACCESS_METHOD_IMMEDIATE_TX:
-		r = mcps802154_fproc_tx_handle(local, access);
+		access->error = mcps802154_fproc_tx_handle(local, access);
 		break;
 	case MCPS802154_ACCESS_METHOD_MULTI:
-		r = mcps802154_fproc_multi_handle(local, access);
+		access->error = mcps802154_fproc_multi_handle(local, access);
 		break;
 	case MCPS802154_ACCESS_METHOD_VENDOR:
-		r = mcps802154_fproc_vendor_handle(local, access);
+		access->error = mcps802154_fproc_vendor_handle(local, access);
 		break;
 	default:
-		r = -1;
+		access->error = -1;
 	}
 
-	if (r) {
-		mcps802154_fproc_access_done(local, true);
-		if (r == -ETIME)
-			mcps802154_fproc_access_now(local);
-		else
+	if (access->error) {
+		if (mcps802154_fproc_is_non_recoverable_error(access)) {
+			mcps802154_fproc_access_done(local, true);
 			mcps802154_fproc_broken_handle(local);
+		} else {
+			mcps802154_fproc_access_done(local, false);
+			mcps802154_fproc_access_now(local);
+		}
 	}
 }
 
@@ -150,6 +174,22 @@ static void mcps802154_broken_safe(struct mcps802154_local *local)
 		mcps802154_fproc_broken_handle(local);
 }
 
+static void mcps802154_fproc_call_deferred(struct mcps802154_local *local)
+{
+	struct mcps802154_region *region = local->fproc.deferred;
+
+	if (region) {
+		local->fproc.deferred = NULL;
+		region->ops->deferred(region);
+	}
+}
+
+void mcps802154_fproc_schedule_change(struct mcps802154_local *local)
+{
+	local->fproc.state->schedule_change(local);
+	mcps802154_fproc_call_deferred(local);
+}
+
 void mcps802154_rx_frame(struct mcps802154_llhw *llhw)
 {
 	struct mcps802154_local *local = llhw_to_local(llhw);
@@ -160,6 +200,7 @@ void mcps802154_rx_frame(struct mcps802154_llhw *llhw)
 		local->fproc.state->rx_frame(local);
 	else
 		mcps802154_broken_safe(local);
+	mcps802154_fproc_call_deferred(local);
 	trace_llhw_event_done(local);
 	mutex_unlock(&local->fsm_lock);
 }
@@ -175,6 +216,7 @@ void mcps802154_rx_timeout(struct mcps802154_llhw *llhw)
 		local->fproc.state->rx_timeout(local);
 	else
 		mcps802154_broken_safe(local);
+	mcps802154_fproc_call_deferred(local);
 	trace_llhw_event_done(local);
 	mutex_unlock(&local->fsm_lock);
 }
@@ -191,6 +233,7 @@ void mcps802154_rx_error(struct mcps802154_llhw *llhw,
 		local->fproc.state->rx_error(local, error);
 	else
 		mcps802154_broken_safe(local);
+	mcps802154_fproc_call_deferred(local);
 	trace_llhw_event_done(local);
 	mutex_unlock(&local->fsm_lock);
 }
@@ -206,6 +249,7 @@ void mcps802154_tx_done(struct mcps802154_llhw *llhw)
 		local->fproc.state->tx_done(local);
 	else
 		mcps802154_broken_safe(local);
+	mcps802154_fproc_call_deferred(local);
 	trace_llhw_event_done(local);
 	mutex_unlock(&local->fsm_lock);
 }
@@ -223,7 +267,7 @@ void mcps802154_tx_too_late(struct mcps802154_llhw *llhw)
 	trace_llhw_event_done(local);
 	mutex_unlock(&local->fsm_lock);
 }
-EXPORT_SYMBOL_GPL(mcps802154_tx_too_late);
+EXPORT_SYMBOL(mcps802154_tx_too_late);
 
 void mcps802154_rx_too_late(struct mcps802154_llhw *llhw)
 {
@@ -237,7 +281,7 @@ void mcps802154_rx_too_late(struct mcps802154_llhw *llhw)
 	trace_llhw_event_done(local);
 	mutex_unlock(&local->fsm_lock);
 }
-EXPORT_SYMBOL_GPL(mcps802154_rx_too_late);
+EXPORT_SYMBOL(mcps802154_rx_too_late);
 
 void mcps802154_broken(struct mcps802154_llhw *llhw)
 {
@@ -246,6 +290,7 @@ void mcps802154_broken(struct mcps802154_llhw *llhw)
 	mutex_lock(&local->fsm_lock);
 	trace_llhw_event_broken(local);
 	mcps802154_broken_safe(local);
+	mcps802154_fproc_call_deferred(local);
 	trace_llhw_event_done(local);
 	mutex_unlock(&local->fsm_lock);
 }
@@ -259,6 +304,7 @@ void mcps802154_timer_expired(struct mcps802154_llhw *llhw)
 	trace_llhw_event_timer_expired(local);
 	if (local->fproc.state->timer_expired)
 		local->fproc.state->timer_expired(local);
+	mcps802154_fproc_call_deferred(local);
 	trace_llhw_event_done(local);
 	mutex_unlock(&local->fsm_lock);
 }
diff --git a/mac/fproc.h b/mac/fproc.h
index c65e1a0..f26f55d 100644
--- a/mac/fproc.h
+++ b/mac/fproc.h
@@ -68,6 +68,8 @@ struct mcps802154_fproc {
 	struct sk_buff *tx_skb;
 	/** @frame_idx: Frame index for multiple frames method. */
 	size_t frame_idx;
+	/** @deferred: Pointer to region context requesting deferred call. */
+	struct mcps802154_region *deferred;
 };
 
 extern const struct mcps802154_fproc_state mcps802154_fproc_stopped;
@@ -126,6 +128,15 @@ void mcps802154_fproc_access_done(struct mcps802154_local *local, bool error);
 void mcps802154_fproc_access_reset(struct mcps802154_local *local);
 
 /**
+ * mcps802154_fproc_schedule_change() - Try a schedule change.
+ * @local: MCPS private data.
+ *
+ * Inform the current state that the schedule has changed. To be called
+ * exclusively from CA.
+ */
+void mcps802154_fproc_schedule_change(struct mcps802154_local *local);
+
+/**
  * mcps802154_fproc_stopped_handle() - Go to stopped.
  * @local: MCPS private data.
  */
@@ -148,6 +159,17 @@ int mcps802154_fproc_nothing_handle(struct mcps802154_local *local,
 				    struct mcps802154_access *access);
 
 /**
+ * mcps802154_fproc_idle_handle() - Handle inactivity with trust in
+ * access->duration.
+ * @local: MCPS private data.
+ * @access: Current access to handle.
+ *
+ * Return: 0 or error.
+ */
+int mcps802154_fproc_idle_handle(struct mcps802154_local *local,
+				 struct mcps802154_access *access);
+
+/**
  * mcps802154_fproc_rx_handle() - Handle an RX access and change state.
  * @local: MCPS private data.
  * @access: Current access to handle.
diff --git a/mac/fproc_broken.c b/mac/fproc_broken.c
index 36692f2..86bed74 100644
--- a/mac/fproc_broken.c
+++ b/mac/fproc_broken.c
@@ -23,9 +23,11 @@
 #include <linux/printk.h>
 
 #include "mcps802154_i.h"
+#include "trace.h"
 
 static void mcps802154_fproc_broken_enter(struct mcps802154_local *local)
 {
+	trace_fproc_broken_enter(local);
 	pr_err_ratelimited("mcps802154: entering broken state for %s\n",
 			   wpan_phy_name(local->hw->phy));
 	local->broken = true;
diff --git a/mac/fproc_idle.c b/mac/fproc_idle.c
new file mode 100644
index 0000000..300ddb0
--- /dev/null
+++ b/mac/fproc_idle.c
@@ -0,0 +1,68 @@
+/*
+ * 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 "mcps802154_i.h"
+#include "llhw-ops.h"
+
+static void mcps802154_fproc_idle_timer_expired(struct mcps802154_local *local)
+{
+	struct mcps802154_access *access = local->fproc.access;
+
+	mcps802154_fproc_access_done(local, false);
+	if (access->duration_dtu) {
+		u32 next_access_dtu =
+			access->timestamp_dtu + access->duration_dtu;
+
+		mcps802154_fproc_access(local, next_access_dtu);
+	} else {
+		mcps802154_fproc_access_now(local);
+	}
+}
+
+static void
+mcps802154_fproc_idle_schedule_change(struct mcps802154_local *local)
+{
+	mcps802154_fproc_access_done(local, false);
+	mcps802154_fproc_access_now(local);
+}
+
+static const struct mcps802154_fproc_state mcps802154_fproc_idle = {
+	.name = "idle",
+	.timer_expired = mcps802154_fproc_idle_timer_expired,
+	.schedule_change = mcps802154_fproc_idle_schedule_change,
+};
+
+int mcps802154_fproc_idle_handle(struct mcps802154_local *local,
+				 struct mcps802154_access *access)
+{
+	int r;
+
+	r = llhw_idle(local, access->duration_dtu != 0,
+		      access->timestamp_dtu + access->duration_dtu);
+	if (r)
+		return r;
+
+	mcps802154_fproc_change_state(local, &mcps802154_fproc_idle);
+
+	return 0;
+}
diff --git a/mac/fproc_multi.c b/mac/fproc_multi.c
index 9ca05ad..2c7b2fa 100644
--- a/mac/fproc_multi.c
+++ b/mac/fproc_multi.c
@@ -23,6 +23,7 @@
 
 #include <linux/errno.h>
 
+#include "mcps802154_fproc.h"
 #include "mcps802154_i.h"
 #include "llhw-ops.h"
 
@@ -89,7 +90,7 @@ mcps802154_fproc_multi_check_frames(struct mcps802154_local *local,
 		const struct mcps802154_access_frame *frame =
 			&access->frames[frame_idx];
 		/* Only first Rx can be without timeout. */
-		if (!frame->is_tx && frame->rx.info.timeout_dtu == -1)
+		if (!frame->is_tx && frame->rx.frame_config.timeout_dtu == -1)
 			return -EINVAL;
 	}
 	return 0;
@@ -107,37 +108,41 @@ static void mcps802154_fproc_multi_next(struct mcps802154_local *local,
 					struct mcps802154_access *access,
 					size_t frame_idx)
 {
-	int r;
-
 	frame_idx++;
 	if (access->ops->access_extend && frame_idx == access->n_frames) {
 		frame_idx = 0;
 		access->n_frames = 0;
 		access->ops->access_extend(access);
-		r = mcps802154_fproc_multi_check_frames(local, access, 0);
-		if (r) {
-			mcps802154_fproc_access_done(local, true);
-			mcps802154_fproc_broken_handle(local);
-			return;
+		access->error = mcps802154_fproc_multi_check_frames(local, access, 0);
+		if (access->error) {
+			if (mcps802154_fproc_is_non_recoverable_error(access)) {
+				mcps802154_fproc_access_done(local, true);
+				mcps802154_fproc_broken_handle(local);
+				return;
+			}
 		}
 	}
 	if (frame_idx < access->n_frames) {
 		/* Next frame. */
-		r = mcps802154_fproc_multi_handle_frame(local, access,
+		access->error = mcps802154_fproc_multi_handle_frame(local, access,
 							frame_idx);
-		if (r) {
-			mcps802154_fproc_access_done(local, true);
-			if (r == -ETIME)
-				mcps802154_fproc_access_now(local);
-			else
+		if (access->error) {
+			if (mcps802154_fproc_is_non_recoverable_error(access)) {
+				mcps802154_fproc_access_done(local, true);
 				mcps802154_fproc_broken_handle(local);
+			} else {
+				mcps802154_fproc_access_done(local, false);
+				mcps802154_fproc_access_now(local);
+			}
 		}
 	} else {
-		r = mcps802154_fproc_multi_restore(local, access);
-		mcps802154_fproc_access_done(local, !!r);
-		if (r) {
-			mcps802154_fproc_broken_handle(local);
-			return;
+		access->error = mcps802154_fproc_multi_restore(local, access);
+		mcps802154_fproc_access_done(local, !!access->error);
+		if (access->error) {
+			if (mcps802154_fproc_is_non_recoverable_error(access)) {
+				mcps802154_fproc_broken_handle(local);
+				return;
+			}
 		}
 		/* Next access. */
 		if (access->duration_dtu) {
@@ -155,25 +160,25 @@ static void mcps802154_fproc_multi_rx_rx_frame(struct mcps802154_local *local)
 	struct mcps802154_access *access = local->fproc.access;
 	size_t frame_idx = local->fproc.frame_idx;
 	struct mcps802154_access_frame *frame = &access->frames[frame_idx];
-	int r;
 
 	/* Read frame. */
 	struct sk_buff *skb = NULL;
 	struct mcps802154_rx_frame_info info = {
 		.flags = frame->rx.frame_info_flags_request,
 	};
-	r = llhw_rx_get_frame(local, &skb, &info);
-	if (!r)
+	access->error =  llhw_rx_get_frame(local, &skb, &info);
+	if (!access->error)
 		access->ops->rx_frame(access, frame_idx, skb, &info,
 				      MCPS802154_RX_ERROR_NONE);
 
-	if (r && r != -EBUSY) {
-		mcps802154_fproc_access_done(local, true);
-		mcps802154_fproc_broken_handle(local);
-	} else {
-		/* Next. */
-		mcps802154_fproc_multi_next(local, access, frame_idx);
+	if (access->error) {
+		if (mcps802154_fproc_is_non_recoverable_error(access)) {
+			mcps802154_fproc_access_done(local, true);
+			mcps802154_fproc_broken_handle(local);
+			return;
+		}
 	}
+	mcps802154_fproc_multi_next(local, access, frame_idx);
 }
 
 static void mcps802154_fproc_multi_rx_rx_timeout(struct mcps802154_local *local)
@@ -195,7 +200,7 @@ mcps802154_fproc_multi_rx_rx_error(struct mcps802154_local *local,
 	struct mcps802154_access *access = local->fproc.access;
 	size_t frame_idx = local->fproc.frame_idx;
 	struct mcps802154_rx_frame_info info = {
-		.flags = MCPS802154_RX_INFO_TIMESTAMP_DTU,
+		.flags = MCPS802154_RX_FRAME_INFO_TIMESTAMP_DTU,
 	};
 
 	llhw_rx_get_error_frame(local, &info);
@@ -213,23 +218,24 @@ mcps802154_fproc_multi_rx_schedule_change(struct mcps802154_local *local)
 	int frame_idx = local->fproc.frame_idx;
 	struct mcps802154_access_frame *frame = &access->frames[frame_idx];
 
-	if (frame->rx.info.timeout_dtu == -1) {
+	if (frame->rx.frame_config.timeout_dtu == -1) {
 		/* Disable RX. */
-		int r = llhw_rx_disable(local);
-
-		if (r == -EBUSY)
+		access->error = llhw_rx_disable(local);
+		if (access->error == -EBUSY)
 			/* Wait for RX result. */
 			return;
 
 		access->ops->rx_frame(access, frame_idx, NULL, NULL,
 				      MCPS802154_RX_ERROR_TIMEOUT);
-		if (r) {
-			mcps802154_fproc_access_done(local, r);
-			mcps802154_fproc_broken_handle(local);
-		} else {
-			/* Next. */
-			mcps802154_fproc_multi_next(local, access, frame_idx);
+		if (access->error) {
+			if (mcps802154_fproc_is_non_recoverable_error(access)) {
+				mcps802154_fproc_access_done(local, true);
+				mcps802154_fproc_broken_handle(local);
+				return;
+			}
 		}
+		/* Next. */
+		mcps802154_fproc_multi_next(local, access, frame_idx);
 	}
 }
 
@@ -314,7 +320,8 @@ static int mcps802154_fproc_multi_handle_frame(struct mcps802154_local *local,
 
 	frame = &access->frames[frame_idx];
 	if (!frame->is_tx) {
-		if (frame->rx.info.flags & MCPS802154_RX_INFO_AACK)
+		if (frame->rx.frame_config.flags &
+		    MCPS802154_RX_FRAME_CONFIG_AACK)
 			return -EINVAL;
 
 		if (frame->sts_params) {
@@ -323,14 +330,15 @@ static int mcps802154_fproc_multi_handle_frame(struct mcps802154_local *local,
 				return r;
 		}
 
-		r = llhw_rx_enable(local, &frame->rx.info, frame_idx, 0);
+		r = llhw_rx_enable(local, &frame->rx.frame_config, frame_idx,
+				   0);
 		if (r)
 			return r;
 
 		mcps802154_fproc_change_state(local,
 					      &mcps802154_fproc_multi_rx);
 	} else {
-		if (frame->tx_frame_info.rx_enable_after_tx_dtu)
+		if (frame->tx_frame_config.rx_enable_after_tx_dtu)
 			return -EINVAL;
 
 		skb = access->ops->tx_get_frame(access, frame_idx);
@@ -347,8 +355,8 @@ static int mcps802154_fproc_multi_handle_frame(struct mcps802154_local *local,
 			}
 		}
 
-		r = llhw_tx_frame(local, skb, &frame->tx_frame_info, frame_idx,
-				  0);
+		r = llhw_tx_frame(local, skb, &frame->tx_frame_config,
+				  frame_idx, 0);
 		if (r) {
 			access->ops->tx_return(
 				access, frame_idx, skb,
@@ -396,5 +404,12 @@ int mcps802154_fproc_multi_handle(struct mcps802154_local *local,
 			return r;
 		}
 	}
+
+	if (access->hrp_uwb_params) {
+		r = llhw_set_hrp_uwb_params(local, access->hrp_uwb_params);
+		if (r)
+			return r;
+	}
+
 	return mcps802154_fproc_multi_handle_frame(local, access, 0);
 }
diff --git a/mac/fproc_rx.c b/mac/fproc_rx.c
index cfa2b7b..b7cc171 100644
--- a/mac/fproc_rx.c
+++ b/mac/fproc_rx.c
@@ -22,6 +22,7 @@
  */
 #include <linux/errno.h>
 
+#include "mcps802154_fproc.h"
 #include "mcps802154_i.h"
 #include "llhw-ops.h"
 
@@ -48,15 +49,14 @@ static const struct mcps802154_fproc_state mcps802154_fproc_rx_wait_tx_done = {
 static void mcps802154_fproc_rx_rx_frame(struct mcps802154_local *local)
 {
 	struct mcps802154_access *access = local->fproc.access;
-	int r;
 
 	/* Read frame. */
 	struct sk_buff *skb = NULL;
 	struct mcps802154_rx_frame_info info = {
 		.flags = MCPS802154_RX_FRAME_INFO_LQI,
 	};
-	r = llhw_rx_get_frame(local, &skb, &info);
-	if (!r) {
+	access->error = llhw_rx_get_frame(local, &skb, &info);
+	if (!access->error) {
 		access->ops->rx_frame(access, 0, skb, &info,
 				      MCPS802154_RX_ERROR_NONE);
 		/* If auto-ack was sent, wait for tx_done. */
@@ -66,39 +66,44 @@ static void mcps802154_fproc_rx_rx_frame(struct mcps802154_local *local)
 			return;
 		}
 	}
-	mcps802154_fproc_access_done(local, !!r);
-
-	if (r && r != -EBUSY)
-		mcps802154_fproc_broken_handle(local);
-	else
-		/* Next access. */
-		mcps802154_fproc_access_now(local);
+	if (access->error) {
+		if (mcps802154_fproc_is_non_recoverable_error(access)) {
+			mcps802154_fproc_access_done(local, true);
+			mcps802154_fproc_broken_handle(local);
+			return;
+		}
+	}
+	/* Next access. */
+	mcps802154_fproc_access_done(local, false);
+	mcps802154_fproc_access_now(local);
 }
 
 static void mcps802154_fproc_rx_rx_error(struct mcps802154_local *local,
 					 enum mcps802154_rx_error_type error)
 {
 	mcps802154_fproc_access_done(local, true);
-
 	/* Next access. */
 	mcps802154_fproc_access_now(local);
 }
 
 static void mcps802154_fproc_rx_schedule_change(struct mcps802154_local *local)
 {
-	int r;
-
+	struct mcps802154_access *access = local->fproc.access;
 	/* Disable RX. */
-	r = llhw_rx_disable(local);
-	if (r == -EBUSY)
+	access->error = llhw_rx_disable(local);
+	if (access->error == -EBUSY)
 		/* Wait for RX result. */
 		return;
 
-	mcps802154_fproc_access_done(local, !!r);
-	if (r)
-		mcps802154_fproc_broken_handle(local);
-	else
+	if (access->error) {
+		if (mcps802154_fproc_is_non_recoverable_error(access)) {
+			mcps802154_fproc_access_done(local, true);
+			mcps802154_fproc_broken_handle(local);
+			return;
+		}
+	}
 		/* Next access. */
+		mcps802154_fproc_access_done(local, false);
 		mcps802154_fproc_access_now(local);
 }
 
@@ -112,14 +117,13 @@ static const struct mcps802154_fproc_state mcps802154_fproc_rx = {
 int mcps802154_fproc_rx_handle(struct mcps802154_local *local,
 			       struct mcps802154_access *access)
 {
-	int r;
-	struct mcps802154_rx_info rx_info = {
-		.flags = MCPS802154_RX_INFO_AACK,
+	struct mcps802154_rx_frame_config rx_config = {
+		.flags = MCPS802154_RX_FRAME_CONFIG_AACK,
 		.timeout_dtu = -1,
 	};
-	r = llhw_rx_enable(local, &rx_info, 0, 0);
-	if (r)
-		return r;
+	access->error = llhw_rx_enable(local, &rx_config, 0, 0);
+	if (access->error)
+		return access->error;
 
 	mcps802154_fproc_change_state(local, &mcps802154_fproc_rx);
 
diff --git a/mac/fproc_tx.c b/mac/fproc_tx.c
index 53f1281..f96a431 100644
--- a/mac/fproc_tx.c
+++ b/mac/fproc_tx.c
@@ -24,6 +24,7 @@
 #include <linux/ieee802154.h>
 #include <linux/netdevice.h>
 
+#include "mcps802154_fproc.h"
 #include "mcps802154_i.h"
 #include "llhw-ops.h"
 
@@ -55,36 +56,38 @@ static const struct mcps802154_fproc_state mcps802154_fproc_tx = {
 static void mcps802154_fproc_tx_wack_rx_frame(struct mcps802154_local *local)
 {
 	struct mcps802154_access *access = local->fproc.access;
-	int r;
 
 	/* Read frame. */
 	struct sk_buff *skb = NULL;
 	struct mcps802154_rx_frame_info info = {
 		.flags = MCPS802154_RX_FRAME_INFO_LQI,
 	};
-	r = llhw_rx_get_frame(local, &skb, &info);
-	if (!r) {
+	access->error = llhw_rx_get_frame(local, &skb, &info);
+	if (!access->error) {
 		/* Is it an ack frame? With same seq number? */
 		if (IEEE802154_FC_TYPE(skb->data[0]) ==
-			    IEEE802154_FC_TYPE_ACK &&
-		    skb->data[IEEE802154_FC_LEN] ==
-			    local->fproc.tx_skb->data[IEEE802154_FC_LEN]) {
+				IEEE802154_FC_TYPE_ACK &&
+				skb->data[IEEE802154_FC_LEN] ==
+				local->fproc.tx_skb->data[IEEE802154_FC_LEN]) {
 			/* Ack frame. */
 			access->ops->tx_return(
-				access, 0, local->fproc.tx_skb,
-				MCPS802154_ACCESS_TX_RETURN_REASON_CONSUMED);
+					access, 0, local->fproc.tx_skb,
+					MCPS802154_ACCESS_TX_RETURN_REASON_CONSUMED);
 		} else {
 			/* Not an ack or read failure or a bad sequence number. */
 			access->ops->tx_return(
-				access, 0, local->fproc.tx_skb,
-				MCPS802154_ACCESS_TX_RETURN_REASON_FAILURE);
+					access, 0, local->fproc.tx_skb,
+					MCPS802154_ACCESS_TX_RETURN_REASON_FAILURE);
 		}
 		dev_kfree_skb_any(skb);
 		local->fproc.tx_skb = NULL;
 		mcps802154_fproc_access_done(local, false);
 		mcps802154_fproc_access_now(local);
-	} else {
+	} else if (access->error && mcps802154_fproc_is_non_recoverable_error(access)) {
 		mcps802154_fproc_broken_handle(local);
+	} else {
+		mcps802154_fproc_access_done(local, false);
+		mcps802154_fproc_access_now(local);
 	}
 }
 
@@ -136,16 +139,21 @@ int mcps802154_fproc_tx_handle(struct mcps802154_local *local,
 			       struct mcps802154_access *access)
 {
 	int r;
+	u8 ack_req;
+	struct mcps802154_tx_frame_config tx_config = {};
 	struct sk_buff *skb = access->ops->tx_get_frame(access, 0);
-	u8 ack_req = skb->data[0] & IEEE802154_FC_ACK_REQ;
-	struct mcps802154_tx_frame_info tx_info = {
-		.flags = 0,
-		.rx_enable_after_tx_dtu =
-			ack_req ? IEEE802154_AIFS_DURATION_SYMBOLS *
-					  local->llhw.symbol_dtu :
-				  0,
-	};
-	r = llhw_tx_frame(local, skb, &tx_info, 0, 0);
+
+	if (!skb)
+		return -ENOMEM;
+
+	ack_req = skb->data[0] & IEEE802154_FC_ACK_REQ;
+	if (ack_req) {
+		tx_config.rx_enable_after_tx_dtu =
+			IEEE802154_AIFS_DURATION_SYMBOLS *
+			local->llhw.symbol_dtu;
+	}
+
+	r = llhw_tx_frame(local, skb, &tx_config, 0, 0);
 	if (r) {
 		access->ops->tx_return(
 			access, 0, skb,
diff --git a/mac/fproc_vendor.c b/mac/fproc_vendor.c
index 8723a0b..f067e16 100644
--- a/mac/fproc_vendor.c
+++ b/mac/fproc_vendor.c
@@ -40,6 +40,7 @@ mcps802154_fproc_vendor_handle_callback_return(struct mcps802154_local *local,
 	if (!r)
 		return;
 
+	access->error = r;
 	mcps802154_fproc_access_done(local, error);
 	if (error) {
 		mcps802154_fproc_broken_handle(local);
diff --git a/mac/idle_region.c b/mac/idle_region.c
new file mode 100644
index 0000000..e4f62c3
--- /dev/null
+++ b/mac/idle_region.c
@@ -0,0 +1,166 @@
+/*
+ * 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 "idle_region.h"
+#include "trace.h"
+#include <net/idle_region_nl.h>
+#include <linux/errno.h>
+#include <linux/limits.h>
+
+static struct mcps802154_region_ops idle_region_ops;
+
+struct idle_local {
+	/**
+	 * @region: Region instance returned to MCPS.
+	 */
+	struct mcps802154_region region;
+	/**
+	 * @llhw: Low-level device pointer.
+	 */
+	struct mcps802154_llhw *llhw;
+	/**
+	 * @params: Parameters.
+	 */
+	struct idle_params params;
+	/**
+	 * @access: Access returned to ca.
+	 */
+	struct mcps802154_access access;
+};
+
+static inline struct idle_local *
+region_to_local(struct mcps802154_region *region)
+{
+	return container_of(region, struct idle_local, region);
+}
+
+static struct mcps802154_region *idle_open(struct mcps802154_llhw *llhw)
+{
+	struct idle_local *local;
+
+	local = kzalloc(sizeof(*local), GFP_KERNEL);
+	if (!local)
+		return NULL;
+	local->llhw = llhw;
+	local->region.ops = &idle_region_ops;
+
+	/* Default value of parameters. */
+	local->params.min_duration_dtu = llhw->anticip_dtu * 2;
+	local->params.max_duration_dtu = 0;
+
+	return &local->region;
+}
+
+static void idle_close(struct mcps802154_region *region)
+{
+	kfree(region_to_local(region));
+}
+
+static const struct nla_policy idle_param_nla_policy[IDLE_PARAM_ATTR_MAX + 1] = {
+	[IDLE_PARAM_ATTR_MIN_DURATION_DTU] = NLA_POLICY_MIN(NLA_S32, 0),
+	[IDLE_PARAM_ATTR_MAX_DURATION_DTU] = NLA_POLICY_MIN(NLA_S32, 0),
+};
+
+static int idle_set_parameters(struct mcps802154_region *region,
+			       const struct nlattr *params,
+			       struct netlink_ext_ack *extack)
+{
+	struct idle_local *local = region_to_local(region);
+	struct nlattr *attrs[IDLE_PARAM_ATTR_MAX + 1];
+	struct idle_params *p = &local->params;
+	int min_duration_dtu, max_duration_dtu;
+	int r;
+
+	r = nla_parse_nested(attrs, IDLE_PARAM_ATTR_MAX, params,
+			     idle_param_nla_policy, extack);
+	if (r)
+		return r;
+
+	min_duration_dtu =
+		attrs[IDLE_PARAM_ATTR_MIN_DURATION_DTU] ?
+			nla_get_s32(attrs[IDLE_PARAM_ATTR_MIN_DURATION_DTU]) :
+			p->min_duration_dtu;
+	max_duration_dtu =
+		attrs[IDLE_PARAM_ATTR_MAX_DURATION_DTU] ?
+			nla_get_s32(attrs[IDLE_PARAM_ATTR_MAX_DURATION_DTU]) :
+			p->max_duration_dtu;
+	if (max_duration_dtu && min_duration_dtu &&
+	    min_duration_dtu > max_duration_dtu)
+		return -EINVAL;
+
+	p->min_duration_dtu = min_duration_dtu;
+	p->max_duration_dtu = max_duration_dtu;
+	trace_region_idle_params(p);
+	return 0;
+}
+
+static struct mcps802154_access_ops idle_access_ops = {};
+
+static struct mcps802154_access *
+idle_get_access(struct mcps802154_region *region, u32 next_timestamp_dtu,
+		int next_in_region_dtu, int region_duration_dtu)
+{
+	struct idle_local *local = region_to_local(region);
+	struct mcps802154_access *access = &local->access;
+	const struct idle_params *p = &local->params;
+	int left_region_duration_dtu = region_duration_dtu - next_in_region_dtu;
+	int duration_dtu;
+
+	if (!left_region_duration_dtu) {
+		/* Region used with endless scheduler. */
+		duration_dtu = p->max_duration_dtu;
+	} else {
+		/* Region used directly in on_demand scheduler. */
+		if (left_region_duration_dtu < p->min_duration_dtu)
+			return NULL;
+		duration_dtu = left_region_duration_dtu;
+	}
+
+	trace_region_idle_get_access(next_timestamp_dtu, duration_dtu);
+	access->method = MCPS802154_ACCESS_METHOD_IDLE;
+	access->ops = &idle_access_ops;
+	access->timestamp_dtu = next_timestamp_dtu;
+	access->duration_dtu = duration_dtu;
+
+	return access;
+}
+
+static struct mcps802154_region_ops idle_region_ops = {
+	.owner = THIS_MODULE,
+	.name = "idle",
+	.open = idle_open,
+	.close = idle_close,
+	.set_parameters = idle_set_parameters,
+	.get_demand = NULL, /* Not wanted. */
+	.get_access = idle_get_access,
+};
+
+int mcps802154_idle_region_init(void)
+{
+	return mcps802154_region_register(&idle_region_ops);
+}
+
+void mcps802154_idle_region_exit(void)
+{
+	mcps802154_region_unregister(&idle_region_ops);
+}
diff --git a/mac/idle_region.h b/mac/idle_region.h
new file mode 100644
index 0000000..f3082d1
--- /dev/null
+++ b/mac/idle_region.h
@@ -0,0 +1,42 @@
+/*
+ * This file is part of the UWB stack for linux.
+ *
+ * Copyright (c) 2021-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 IDLE_REGION_H
+#define IDLE_REGION_H
+
+struct idle_params {
+	/**
+	 * @min_duration_dtu: Minimum duration of an access.
+	 * If min is 0, no minimum is required on get_access.
+	 */
+	int min_duration_dtu;
+	/**
+	 * @max_duration_dtu: Maximum duration of an access.
+	 */
+	int max_duration_dtu;
+};
+
+int mcps802154_idle_region_init(void);
+void mcps802154_idle_region_exit(void);
+
+#endif /* IDLE_REGION_H */
diff --git a/mac/include/net/fira_region_nl.h b/mac/include/net/fira_region_nl.h
index 789610b..a78788c 100644
--- a/mac/include/net/fira_region_nl.h
+++ b/mac/include/net/fira_region_nl.h
@@ -25,18 +25,18 @@
 #define FIRA_REGION_NL_H
 
 /**
- * enum fira_call - Fira calls identifiers.
+ * enum fira_call - FiRa calls identifiers.
  *
  * @FIRA_CALL_GET_CAPABILITIES:
- *	Request Fira capabilities.
+ *	Request FiRa capabilities.
  * @FIRA_CALL_SESSION_INIT:
- *	Initialize Fira session.
+ *	Initialize FiRa session.
  * @FIRA_CALL_SESSION_START:
- *	Start Fira session.
+ *	Start FiRa session.
  * @FIRA_CALL_SESSION_STOP:
- *	Stop Fira session.
+ *	Stop FiRa session.
  * @FIRA_CALL_SESSION_DEINIT:
- *	Deinit Fira session.
+ *	Deinit FiRa session.
  * @FIRA_CALL_SESSION_SET_PARAMS:
  *	Set session parameters.
  * @FIRA_CALL_NEW_CONTROLEE:
@@ -76,7 +76,7 @@ enum fira_call {
 };
 
 /**
- * enum fira_capability_attrs - Fira capabilities.
+ * enum fira_capability_attrs - FiRa capabilities.
  *
  * @FIRA_CAPABILITY_ATTR_FIRA_PHY_VERSION_RANGE:
  *	FiRa PHY version range supported, ex: 0x01010202 -> support from 1.1 to 2.2.
@@ -142,6 +142,10 @@ enum fira_call {
  *	1 segment for STS supported.
  * @FIRA_CAPABILITY_ATTR_NUMBER_OF_STS_SEGMENTS_2:
  *	2 segments for STS supported.
+ * @FIRA_CAPABILITY_ATTR_NUMBER_OF_STS_SEGMENTS_3:
+ *	3 segments for STS supported.
+ * @FIRA_CAPABILITY_ATTR_NUMBER_OF_STS_SEGMENTS_4:
+ *	4 segments for STS supported.
  * @FIRA_CAPABILITY_ATTR_PSDU_DATA_RATE_6M81:
  *	6.81 Mbps support.
  * @FIRA_CAPABILITY_ATTR_PSDU_DATA_RATE_7M80:
@@ -162,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:
@@ -217,6 +225,8 @@ enum fira_capability_attrs {
 	FIRA_CAPABILITY_ATTR_NUMBER_OF_STS_SEGMENTS_0,
 	FIRA_CAPABILITY_ATTR_NUMBER_OF_STS_SEGMENTS_1,
 	FIRA_CAPABILITY_ATTR_NUMBER_OF_STS_SEGMENTS_2,
+	FIRA_CAPABILITY_ATTR_NUMBER_OF_STS_SEGMENTS_3,
+	FIRA_CAPABILITY_ATTR_NUMBER_OF_STS_SEGMENTS_4,
 	FIRA_CAPABILITY_ATTR_PSDU_DATA_RATE_6M81,
 	FIRA_CAPABILITY_ATTR_PSDU_DATA_RATE_7M80,
 	FIRA_CAPABILITY_ATTR_PSDU_DATA_RATE_27M2,
@@ -229,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,
@@ -243,7 +255,7 @@ enum fira_capability_attrs {
 };
 
 /**
- * enum fira_call_attrs - Fira call attributes.
+ * enum fira_call_attrs - FiRa call attributes.
  *
  * @FIRA_CALL_ATTR_SESSION_ID:
  *	Session identifier.
@@ -259,6 +271,10 @@ enum fira_capability_attrs {
  *	Session state.
  * @FIRA_CALL_ATTR_SESSION_COUNT:
  *	Sessions count.
+ * @FIRA_CALL_ATTR_SEQUENCE_NUMBER:
+ *	Session notification counter.
+ * @FIRA_CALL_ATTR_RANGING_DIAGNOSTICS:
+ * 	Diagnostic information.
  *
  * @FIRA_CALL_ATTR_UNSPEC: Invalid command.
  * @__FIRA_CALL_ATTR_AFTER_LAST: Internal use.
@@ -273,13 +289,15 @@ enum fira_call_attrs {
 	FIRA_CALL_ATTR_CAPABILITIES,
 	FIRA_CALL_ATTR_SESSION_STATE,
 	FIRA_CALL_ATTR_SESSION_COUNT,
+	FIRA_CALL_ATTR_SEQUENCE_NUMBER,
+	FIRA_CALL_ATTR_RANGING_DIAGNOSTICS,
 
 	__FIRA_CALL_ATTR_AFTER_LAST,
 	FIRA_CALL_ATTR_MAX = __FIRA_CALL_ATTR_AFTER_LAST - 1
 };
 
 /**
- * enum fira_session_param_attrs - Fira session parameters attributes.
+ * enum fira_session_param_attrs - FiRa session parameters attributes.
  *
  * @FIRA_SESSION_PARAM_ATTR_DEVICE_TYPE:
  *	Controlee (0) or controller (1)
@@ -325,21 +343,19 @@ enum fira_call_attrs {
  * @FIRA_SESSION_PARAM_ATTR_CHANNEL_NUMBER:
  *	Override channel for this session: 5, 6, 8, 9, 10, 12, 13 or 14
  * @FIRA_SESSION_PARAM_ATTR_PREAMBLE_CODE_INDEX:
- *	Override preamble code for this session, BPRF (9-24),
- *	HPRF (25-32, not supported)
+ *	Override preamble code for this session, BPRF (9-24), HPRF (25-32)
  * @FIRA_SESSION_PARAM_ATTR_RFRAME_CONFIG:
  *	SP0 (0), SP1 (1), SP2 (2, unused, not in FiRa 1.1) or SP3 (3, default)
  * @FIRA_SESSION_PARAM_ATTR_PRF_MODE:
- *	BPRF (0, default) or HPRF (1, not supported)
+ *	BPRF (0, default), HPRF (1) or HPRF with high data rate (2)
  * @FIRA_SESSION_PARAM_ATTR_PREAMBLE_DURATION:
  *	64 (1, default) or 32 (0, only for HPRF)
  * @FIRA_SESSION_PARAM_ATTR_SFD_ID:
- *	BPRF (0 or 2), HPRF (1-4, not supported), default 2
+ *	BPRF (0 or 2), HPRF (1-4), default 2
  * @FIRA_SESSION_PARAM_ATTR_NUMBER_OF_STS_SEGMENTS:
  *	0-2, default to 0 for SP0, default to 1 for SP1 & SP3, 2 not supported
  * @FIRA_SESSION_PARAM_ATTR_PSDU_DATA_RATE:
- *	6.81 Mbps (0, default), 7.80 Mbps (1, not supported),
- *	27.2 Mbps (2, not supported), 31.2 Mbps (3, not supported)
+ *	6.81 Mbps (0, default), 7.80 Mbps (1), 27.2 Mbps (2), 31.2 Mbps (3)
  * @FIRA_SESSION_PARAM_ATTR_BPRF_PHR_DATA_RATE:
  *	850 kbps (0, default) or 6.81 Mbps (1)
  * @FIRA_SESSION_PARAM_ATTR_MAC_FCS_TYPE:
@@ -349,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:
@@ -373,10 +390,32 @@ enum fira_call_attrs {
  *	Report AoA elevation in result message, disabled (0, default) or enabled (1)
  * @FIRA_SESSION_PARAM_ATTR_REPORT_AOA_FOM:
  *	Report AoA FOM in result message, disabled (0, default) or enabled (1)
+ * @FIRA_SESSION_PARAM_ATTR_REPORT_RSSI:
+ *	Report average RSSI of the round in result message, disabled (0, default) or enabled (1)
  * @FIRA_SESSION_PARAM_ATTR_DATA_VENDOR_OUI:
- *	Set the vendor OUI for custom data exchanges
- * @FIRA_SESSION_PARAM_ATTR_DATA_PAYLOAD:
- *	Set the data payload to send in next ranging packet
+*	Set the vendor OUI for custom data exchanges
+* @FIRA_SESSION_PARAM_ATTR_DATA_PAYLOAD:
+*	Set the data payload to send in next ranging packet
+ * @FIRA_SESSION_PARAM_ATTR_DIAGNOSTICS:
+ *	Report diagnostic information on each round, disabled (0, default) or enabled (1)
+ * @FIRA_SESSION_PARAM_ATTR_DIAGNOSTICS_FRAME_REPORTS_FIELDS:
+ *	Bitfield activating various frame diagnostics in the report (0: no frame diagnostic report, default).
+ *	see &enum fira_ranging_diagnostics_frame_report_flags
+ * @FIRA_SESSION_PARAM_ATTR_STS_LENGTH:
+ *	Number of symbols in a STS segment. 32 (0x00), 64 (0x01, default) or 128
+ *	symbols (0x02)
+ * @FIRA_SESSION_PARAM_ATTR_CAP_SIZE_MAX:
+ *      Maximum for contention access period size
+ * @FIRA_SESSION_PARAM_ATTR_CAP_SIZE_MIN:
+ *      Minimum for contention access period size
+ * @FIRA_SESSION_PARAM_ATTR_RANGE_DATA_NTF_CONFIG:
+ *      Configure range data notification
+ * @FIRA_SESSION_PARAM_ATTR_RANGE_DATA_NTF_PROXIMITY_NEAR:
+ *       Lower bound in cm above which the ranging notifications
+ *       should be enabled when RANGE_DATA_NTF_CONFIG is set to "proximity"
+ * @FIRA_SESSION_PARAM_ATTR_RANGE_DATA_NTF_PROXIMITY_FAR:
+ *       Upper bound in cm above which the ranging notifications
+ *       should be disabled when RANGE_DATA_NTF_CONFIG is set to "proximity"
  *
  * @FIRA_SESSION_PARAM_ATTR_UNSPEC: Invalid command.
  * @__FIRA_SESSION_PARAM_ATTR_AFTER_LAST: Internal use.
@@ -434,16 +473,28 @@ enum fira_session_param_attrs {
 	FIRA_SESSION_PARAM_ATTR_REPORT_AOA_AZIMUTH,
 	FIRA_SESSION_PARAM_ATTR_REPORT_AOA_ELEVATION,
 	FIRA_SESSION_PARAM_ATTR_REPORT_AOA_FOM,
+	FIRA_SESSION_PARAM_ATTR_REPORT_RSSI,
 	/* Custom Data */
 	FIRA_SESSION_PARAM_ATTR_DATA_VENDOR_OUI,
 	FIRA_SESSION_PARAM_ATTR_DATA_PAYLOAD,
-
+	/* Diagnostics */
+	FIRA_SESSION_PARAM_ATTR_DIAGNOSTICS,
+	FIRA_SESSION_PARAM_ATTR_DIAGNOSTICS_FRAME_REPORTS_FIELDS,
+	/* Misc */
+	FIRA_SESSION_PARAM_ATTR_STS_LENGTH,
+	/* Contention-based ranging */
+	FIRA_SESSION_PARAM_ATTR_CAP_SIZE_MAX,
+	FIRA_SESSION_PARAM_ATTR_CAP_SIZE_MIN,
+	/* Range data notification enable */
+	FIRA_SESSION_PARAM_ATTR_RANGE_DATA_NTF_CONFIG,
+	FIRA_SESSION_PARAM_ATTR_RANGE_DATA_NTF_PROXIMITY_NEAR,
+	FIRA_SESSION_PARAM_ATTR_RANGE_DATA_NTF_PROXIMITY_FAR,
 	__FIRA_SESSION_PARAM_ATTR_AFTER_LAST,
 	FIRA_SESSION_PARAM_ATTR_MAX = __FIRA_SESSION_PARAM_ATTR_AFTER_LAST - 1
 };
 
 /**
- * enum fira_call_controlee_attrs - Fira controlee parameters attributes.
+ * enum fira_call_controlee_attrs - FiRa controlee parameters attributes.
  *
  * @FIRA_CALL_CONTROLEE_ATTR_SHORT_ADDR:
  *	Controlee short address.
@@ -485,7 +536,7 @@ enum fira_ranging_data_attrs_stopped_values {
 };
 
 /**
- * enum fira_ranging_data_attrs - Fira ranging data attributes.
+ * enum fira_ranging_data_attrs - FiRa ranging data attributes.
  *
  * @FIRA_RANGING_DATA_ATTR_STOPPED:
  *	If present, session was stopped, see
@@ -517,7 +568,7 @@ enum fira_ranging_data_attrs {
 };
 
 /**
- * enum fira_ranging_data_measurements_attrs - Fira ranging data measurements
+ * enum fira_ranging_data_measurements_attrs - FiRa ranging data measurements
  * attributes.
  *
  * @FIRA_RANGING_DATA_MEASUREMENTS_ATTR_SHORT_ADDR:
@@ -553,6 +604,10 @@ enum fira_ranging_data_attrs {
  *	Estimation of azimuth reliability of the participing device.
  * @FIRA_RANGING_DATA_MEASUREMENTS_ATTR_REMOTE_AOA_ELEVATION_FOM:
  *	Estimation of elevation reliability of the participing device.
+ * @FIRA_RANGING_DATA_MEASUREMENTS_ATTR_RSSI:
+ *	RSSI "summary" for received frames during the ranging round,
+ *	reported as Q7.1. Summary method depends on session params
+ *	(average, minimum, etc).
  * @FIRA_RANGING_DATA_MEASUREMENTS_ATTR_DATA_PAYLOAD_SEQ_SENT:
  *	Sequence number of last data sent
  * @FIRA_RANGING_DATA_MEASUREMENTS_ATTR_DATA_PAYLOAD_RECV:
@@ -578,6 +633,7 @@ enum fira_ranging_data_measurements_attrs {
 	FIRA_RANGING_DATA_MEASUREMENTS_ATTR_REMOTE_AOA_ELEVATION_PI,
 	FIRA_RANGING_DATA_MEASUREMENTS_ATTR_REMOTE_AOA_AZIMUTH_FOM,
 	FIRA_RANGING_DATA_MEASUREMENTS_ATTR_REMOTE_AOA_ELEVATION_FOM,
+	FIRA_RANGING_DATA_MEASUREMENTS_ATTR_RSSI,
 	FIRA_RANGING_DATA_MEASUREMENTS_ATTR_DATA_PAYLOAD_SEQ_SENT,
 	FIRA_RANGING_DATA_MEASUREMENTS_ATTR_DATA_PAYLOAD_RECV,
 
@@ -587,7 +643,7 @@ enum fira_ranging_data_measurements_attrs {
 };
 
 /**
- * enum fira_ranging_data_measurements_aoa_attrs - Fira ranging AoA measurements
+ * enum fira_ranging_data_measurements_aoa_attrs - FiRa ranging AoA measurements
  * attributes.
  *
  * @FIRA_RANGING_DATA_MEASUREMENTS_AOA_ATTR_RX_ANTENNA_SET:
@@ -616,14 +672,14 @@ enum fira_ranging_data_measurements_aoa_attrs {
 };
 
 /**
- * enum fira_session_param_meas_seq_step_attrs - Fira measurement sequence
+ * enum fira_session_param_meas_seq_step_attrs - FiRa measurement sequence
  * step attributes.
  *
  * @FIRA_SESSION_PARAM_MEAS_SEQ_STEP_ATTR_MEASUREMENT_TYPE:
  *	The type of measurement to perform during the step.
  * @FIRA_SESSION_PARAM_MEAS_SEQ_STEP_ATTR_N_MEASUREMENTS:
  *	The number of times this type of measurement shall be performed
- * 	during the step.
+ *	during the step.
  * @FIRA_SESSION_PARAM_MEAS_SEQ_STEP_ATTR_RX_ANT_SET_NONRANGING:
  *	The antenna set to use to receive the non-rfames during the step.
  * @FIRA_SESSION_PARAM_MEAS_SEQ_STEP_ATTR_RX_ANT_SETS_RANGING:
@@ -654,11 +710,11 @@ enum fira_session_param_meas_seq_step_attrs {
 
 /**
  * enum fira_session_params_meas_seq_step_sets_attrs - Attributes of the
- * Fira RX antenna sets to use during a step.
+ * FiRa RX antenna sets to use during a step.
  *
  * @FIRA_SESSION_PARAM_MEAS_SEQ_STEP_RX_ANT_SETS_RANGING_ATTR_0:
  *	Antenna set used to receive all rframes for range, azimuth and elevation
- * 	steps or initial rframe for azimuth_elevation step.
+ *	steps or initial rframe for azimuth_elevation step.
  * @FIRA_SESSION_PARAM_MEAS_SEQ_STEP_RX_ANT_SETS_RANGING_ATTR_1:
  *	Antenna set used to receive final rframes for azimuth_elevation step.
  *
@@ -680,4 +736,140 @@ enum fira_session_params_meas_seq_step_sets_attrs {
 		1
 };
 
+/**
+ * enum fira_ranging_diagnostics_attrs - FiRa ranging diagnostic attributes.
+ *
+ * @FIRA_RANGING_DIAGNOSTICS_ATTR_FRAME_REPORTS:
+ *	Diagnostics for individual frames of the round.
+ *
+ * @FIRA_RANGING_DIAGNOSTICS_ATTR_UNSPEC: Invalid command.
+ * @__FIRA_RANGING_DIAGNOSTICS_ATTR_AFTER_LAST : Internal use.
+ * @FIRA_RANGING_DIAGNOSTICS_ATTR_MAX : Internal use.
+ */
+enum fira_ranging_diagnostics_attrs {
+	FIRA_RANGING_DIAGNOSTICS_ATTR_UNSPEC,
+	FIRA_RANGING_DIAGNOSTICS_ATTR_FRAME_REPORTS,
+
+	__FIRA_RANGING_DIAGNOSTICS_ATTR_AFTER_LAST,
+	FIRA_RANGING_DIAGNOSTICS_ATTR_MAX =
+		__FIRA_RANGING_DIAGNOSTICS_ATTR_AFTER_LAST - 1
+};
+
+/**
+ * enum fira_ranging_diagnostics_frame_reports_attrs - FiRa ranging
+ * diagnostic info for individual frames.
+ *
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_ATTR_ANT_SET:
+ *	Antenna set ID, used for the frame transmission.
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_ATTR_ACTION:
+ *	Action type of the frame (0: TX or 1: RX).
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_ATTR_MSG_ID:
+ *	FiRa message ID (0: RIM, 1: RRM, 2: RFM, 3: CM,
+ *	4: MRM, 5: RRRM, 6: CU).
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_ATTR_RSSIS:
+ *	RSSI for the current (Rx) frame, reported as a Q7.1.
+ *	As many values as receivers.
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_ATTR_AOAS:
+ *	Nested attribute reporting different AoA related information.
+ *	As many as AoA types.
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_ATTR_CIRS:
+ *	Nested attribute reporting CIR sample window information.
+ *	As many array elements as receivers.
+ *
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_ATTR_UNSPEC: Invalid command.
+ * @__FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_ATTR_AFTER_LAST: Internal use.
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_ATTR_MAX: Internal use.
+ */
+enum fira_ranging_diagnostics_frame_reports_attrs {
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_ATTR_UNSPEC,
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_ATTR_ANT_SET,
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_ATTR_ACTION,
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_ATTR_MSG_ID,
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_ATTR_RSSIS,
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_ATTR_AOAS,
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_ATTR_CIRS,
+
+	__FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_ATTR_AFTER_LAST,
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_ATTR_MAX =
+		__FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_ATTR_AFTER_LAST - 1
+};
+
+/**
+ * enum fira_ranging_diagnostics_frame_reports_aoa_attrs - AoA diagnostic
+ * information per frame
+ *
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_AOAS_ATTR_TDOA:
+ *      TDoA in rctu, reported as s16.
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_AOAS_ATTR_PDOA:
+ *      PDoA in radians, reported as Q5.11.
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_AOAS_ATTR_AOA:
+ *      AoA in radians, reported as Q5.11.
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_AOAS_ATTR_FOM:
+ *      AoA FoM between 0 and 255 (0 being an invalid measure and 255 being
+ *      a 100% confidence measure), reported as u8.
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_AOAS_ATTR_TYPE:
+ *      AoA Measurement type (azimuth, elevation...), reported as u8.
+ *
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_AOAS_ATTR_UNSPEC: Invalid command.
+ * @__FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_AOAS_ATTR_AFTER_LAST: Internal use.
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_AOAS_ATTR_MAX: Internal use.
+ */
+enum fira_ranging_diagnostics_frame_reports_aoa_attrs {
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_AOAS_ATTR_UNSPEC,
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_AOAS_ATTR_TDOA,
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_AOAS_ATTR_PDOA,
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_AOAS_ATTR_AOA,
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_AOAS_ATTR_FOM,
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_AOAS_ATTR_TYPE,
+
+	__FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_AOAS_ATTR_AFTER_LAST,
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_AOAS_ATTR_MAX =
+		__FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_AOAS_ATTR_AFTER_LAST -
+		1
+};
+
+/**
+ * enum fira_ranging_diagnostics_frame_reports_cir_attrs - CIR diagnostic
+ * information per frame
+ *
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_CIRS_ATTR_FP_IDX:
+ *      Absolute index of the sample considered as first path, reported as u16.
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_CIRS_ATTR_FP_SNR:
+ *      SNR of the sample considered as first path, reported as s16.
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_CIRS_ATTR_FP_NS:
+ *      Timestamp of the sample considered as first path, reported as u16.
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_CIRS_ATTR_PP_IDX:
+ *      Absolute index of the sample considered as peak path, reported as u16.
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_CIRS_ATTR_PP_SNR:
+ *      SNR of the sample considered as peak path, reported as s16.
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_CIRS_ATTR_PP_NS:
+ *      Timestamp of the sample considered as peak path, reported as u16.
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_CIRS_ATTR_FP_SAMPLE_OFFSET:
+ *      Offset of the first path in the sample window, reported as u16.
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_CIRS_ATTR_FP_SAMPLE_WINDOW:
+ *      Sliding window containing CIR samples, each sample is considered as
+ *      a byte sequence depending on sample size.
+ *      As many samples as the window size.
+ *
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_CIRS_ATTR_UNSPEC: Invalid command.
+ * @__FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_CIRS_ATTR_AFTER_LAST: Internal use.
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_CIRS_ATTR_MAX: Internal use.
+ */
+enum fira_ranging_diagnostics_frame_reports_cir_attrs {
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_CIRS_ATTR_UNSPEC,
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_CIRS_ATTR_FP_IDX,
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_CIRS_ATTR_FP_SNR,
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_CIRS_ATTR_FP_NS,
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_CIRS_ATTR_PP_IDX,
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_CIRS_ATTR_PP_SNR,
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_CIRS_ATTR_PP_NS,
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_CIRS_ATTR_FP_SAMPLE_OFFSET,
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_CIRS_ATTR_FP_SAMPLE_WINDOW,
+
+	__FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_CIRS_ATTR_AFTER_LAST,
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_CIRS_ATTR_MAX =
+		__FIRA_RANGING_DIAGNOSTICS_FRAME_REPORTS_CIRS_ATTR_AFTER_LAST -
+		1
+};
+
 #endif /* FIRA_REGION_NL_H */
diff --git a/mac/include/net/fira_region_params.h b/mac/include/net/fira_region_params.h
index e6fe8ad..bd4f650 100644
--- a/mac/include/net/fira_region_params.h
+++ b/mac/include/net/fira_region_params.h
@@ -27,9 +27,10 @@
 #include <linux/types.h>
 
 #define FIRA_VUPPER64_SIZE 8
-#define FIRA_KEY_SIZE_MAX 32
+#define FIRA_STS_VUPPER64_OFFSET 8
+#define FIRA_KEY_SIZE_MAX 16
 #define FIRA_KEY_SIZE_MIN 16
-#define FIRA_CONTROLEES_MAX 16
+#define FIRA_CONTROLEES_MAX 8
 #define FIRA_RX_ANTENNA_PAIR_INVALID 0xff
 /*
  * In BPRF, frame is at most 127
@@ -37,6 +38,9 @@
  */
 #define FIRA_DATA_PAYLOAD_SIZE_MAX 84
 
+/* From UCI spec v1.1.0 (converted to mm) */
+#define FIRA_RANGE_DATA_NTF_PROXIMITY_FAR_DEFAULT 200000
+
 /**
  * enum fira_device_type - Type of a device.
  * @FIRA_DEVICE_TYPE_CONTROLEE: The device is a controlee.
@@ -138,16 +142,18 @@ enum fira_rframe_config {
 };
 
 /**
- * enum fira_prf_mode - **[NOT IMPLEMENTED]** Pulse Repetition Frequency
- * mode.
+ * enum fira_prf_mode - Pulse Repetition Frequency mode
  * @FIRA_PRF_MODE_BPRF: Base Pulse Repetition Frequency.
  * @FIRA_PRF_MODE_HPRF: Higher Pulse Repetition Frequency.
+ * @FIRA_PRF_MODE_HPRF_HIGH_RATE: Higher Pulse Repetition Frequency allows
+ * high data rate (27.2 Mbps and 31.2 Mbps).
  *
  * This enum is not used in the current implementation.
  */
 enum fira_prf_mode {
 	FIRA_PRF_MODE_BPRF,
 	FIRA_PRF_MODE_HPRF,
+	FIRA_PRF_MODE_HPRF_HIGH_RATE,
 };
 
 /**
@@ -180,17 +186,19 @@ enum fira_sfd_id {
 };
 
 /**
- * enum fira_sts_segments - **[NOT IMPLEMENTED]** RFU.
- * @FIRA_STS_SEGMENTS_0: RFU.
- * @FIRA_STS_SEGMENTS_1: RFU.
- * @FIRA_STS_SEGMENTS_2: RFU.
- *
- * This enum is not used in the current implementation.
+ * enum fira_sts_segments - Number of STS segments.
+ * @FIRA_STS_SEGMENTS_0: No STS Segment (Rframe config SP0).
+ * @FIRA_STS_SEGMENTS_1: 1 STS Segment.
+ * @FIRA_STS_SEGMENTS_2: 2 STS Segments.
+ * @FIRA_STS_SEGMENTS_3: 3 STS Segments.
+ * @FIRA_STS_SEGMENTS_4: 4 STS Segments.
  */
 enum fira_sts_segments {
 	FIRA_STS_SEGMENTS_0,
 	FIRA_STS_SEGMENTS_1,
 	FIRA_STS_SEGMENTS_2,
+	FIRA_STS_SEGMENTS_3,
+	FIRA_STS_SEGMENTS_4,
 };
 
 /**
@@ -208,15 +216,14 @@ enum fira_psdu_data_rate {
 };
 
 /**
- * enum fira_phr_data_rate - **[NOT IMPLEMENTED]**
- * Data rate used to exchange PHR.
- * @FIRA_PHR_DATA_RATE_850k: 850kb/s rate.
+ * enum fira_phr_data_rate - Data rate used to exchange PHR.
+ * @FIRA_PHR_DATA_RATE_850K: 850kb/s rate.
  * @FIRA_PHR_DATA_RATE_6M81: 6.8Mb/s rate.
  *
  * This enum is not used in the current implementation.
  */
 enum fira_phr_data_rate {
-	FIRA_PHR_DATA_RATE_850k,
+	FIRA_PHR_DATA_RATE_850K,
 	FIRA_PHR_DATA_RATE_6M81,
 };
 
@@ -231,18 +238,42 @@ enum fira_mac_fcs_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_rssi_report_type - Mode used to sum up individual frames RSSI
+ * in report.
+ * @FIRA_RSSI_REPORT_NONE: No RSSI value in report.
+ * @FIRA_RSSI_REPORT_MINIMUM: Report minimum RSSI
+ * @FIRA_RSSI_REPORT_AVERAGE: Report average RSSI
+ */
+enum fira_rssi_report_type {
+	FIRA_RSSI_REPORT_NONE,
+	FIRA_RSSI_REPORT_MINIMUM,
+	FIRA_RSSI_REPORT_AVERAGE,
+};
+
+/**
+ * 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.
@@ -308,4 +339,45 @@ enum fira_measurement_type {
 	__FIRA_MEASUREMENT_TYPE_AFTER_LAST,
 };
 
+/**
+ * enum fira_ranging_diagnostics_frame_report_flags - Activation flags for different frame diagnostics information.
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORT_NONE: No specific frame diagnostic report requested.
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORT_RSSIS: Report RSSI in frame diagnostics.
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORT_AOAS: Report AOA in frame diagnostics.
+ * @FIRA_RANGING_DIAGNOSTICS_FRAME_REPORT_CIRS: Report CIR in frame diagnostics.
+ * @__FIRA_RANGING_DIAGNOSTICS_FRAME_REPORT_AFTER_LAST: Internal use.
+ */
+enum fira_ranging_diagnostics_frame_report_flags {
+	FIRA_RANGING_DIAGNOSTICS_FRAME_REPORT_NONE = 0,
+	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 = 1 << 31,
+};
+
+/**
+ * enum fira_sts_length - Number of symbols in a STS segment.
+ * @FIRA_STS_LENGTH_32: The STS length is 32 symbols.
+ * @FIRA_STS_LENGTH_64: The STS length is 64 symbols.
+ * @FIRA_STS_LENGTH_128: The STS length is 128 symbols.
+ */
+enum fira_sts_length {
+	FIRA_STS_LENGTH_32 = 0,
+	FIRA_STS_LENGTH_64 = 1,
+	FIRA_STS_LENGTH_128 = 2,
+};
+
+/**
+ * enum fira_range_data_ntf_config - Configure range data notification.
+ * @FIRA_RANGE_DATA_NTF_DISABLED: Do not report range data.
+ * @FIRA_RANGE_DATA_NTF_ALWAYS: Report range data.
+ * @FIRA_RANGE_DATA_NTF_PROXIMITY: Report range data if it is within range
+ * defined by proximity parameters (RANGE_DATA_NTF_PROXIMITY_NEAR/FAR).
+ */
+enum fira_range_data_ntf_config {
+	FIRA_RANGE_DATA_NTF_DISABLED = 0,
+	FIRA_RANGE_DATA_NTF_ALWAYS = 1,
+	FIRA_RANGE_DATA_NTF_PROXIMITY = 2
+};
+
 #endif /* NET_FIRA_REGION_PARAMS_H */
diff --git a/mac/include/net/idle_region_nl.h b/mac/include/net/idle_region_nl.h
new file mode 100644
index 0000000..03a7e05
--- /dev/null
+++ b/mac/include/net/idle_region_nl.h
@@ -0,0 +1,49 @@
+/*
+ * 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.
+ */
+
+#ifndef IDLE_REGION_NL_H
+#define IDLE_REGION_NL_H
+
+/**
+ * enum idle_param_attrs - Idle parameters attributes.
+ *
+ * @IDLE_PARAM_ATTR_MIN_DURATION_DTU:
+ *	Minimum duration of an access.
+ * @IDLE_PARAM_ATTR_MAX_DURATION_DTU:
+ *	Maximum duration of an access.
+ *
+ * @IDLE_PARAM_ATTR_UNSPEC: Invalid command.
+ * @__IDLE_PARAM_ATTR_AFTER_LAST: Internal use.
+ * @IDLE_PARAM_ATTR_MAX: Internal use.
+ */
+enum idle_param_attrs {
+	IDLE_PARAM_ATTR_UNSPEC,
+
+	IDLE_PARAM_ATTR_MIN_DURATION_DTU,
+	IDLE_PARAM_ATTR_MAX_DURATION_DTU,
+
+	__IDLE_PARAM_ATTR_AFTER_LAST,
+	IDLE_PARAM_ATTR_MAX = __IDLE_PARAM_ATTR_AFTER_LAST - 1
+};
+
+#endif /* IDLE_REGION_NL_H */
diff --git a/mac/include/net/mcps802154.h b/mac/include/net/mcps802154.h
index dd9c8dc..77ef002 100644
--- a/mac/include/net/mcps802154.h
+++ b/mac/include/net/mcps802154.h
@@ -35,6 +35,12 @@
 /** Maximum number of STS segments. */
 #define MCPS802154_STS_N_SEGS_MAX 4
 
+/** Maximum number of RSSI values. */
+#define MCPS802154_RSSIS_N_MAX 2
+
+/** Maximum number of angle of arrival measurements. */
+#define MCPS802154_RX_AOA_MEASUREMENTS_MAX 3
+
 /**
  * struct mcps802154_channel - Channel parameters.
  */
@@ -61,10 +67,125 @@ struct mcps802154_channel {
  *	Support for ranging (RDEV). TODO: move to &ieee802154_hw.
  * @MCPS802154_LLHW_ERDEV:
  *	Support for enhanced ranging (ERDEV). TODO: move to &ieee802154_hw.
+ * @MCPS802154_LLHW_BPRF:
+ *	Support for BPRF.
+ * @MCPS802154_LLHW_HPRF:
+ *	Support for HPRF.
+ * @MCPS802154_LLHW_DATA_RATE_850K:
+ *	Support for data rate 110 kpbs.
+ * @MCPS802154_LLHW_DATA_RATE_6M81:
+ *	Support for data rate 6.81 Mpbs.
+ * @MCPS802154_LLHW_DATA_RATE_7M80:
+ *	Support for data rate 7.8 Mpbs.
+ * @MCPS802154_LLHW_DATA_RATE_27M2:
+ *	Support for data rate 27.2 Mpbs.
+ * @MCPS802154_LLHW_DATA_RATE_31M2:
+ *	Support for data rate 31.2 Mpbs.
+ * @MCPS802154_LLHW_DATA_RATE_CUSTOM:
+ *	Support for custom data rate, When presents extra data rate are
+ *	possible to set.
+ * @MCPS802154_LLHW_PHR_DATA_RATE_850K:
+ *	Support PHR data rate 850 kpbs.
+ * @MCPS802154_LLHW_PHR_DATA_RATE_6M81:
+ *	Support PHR data rate 6.81 Mpbs.
+ * @MCPS802154_LLHW_PRF_4:
+ *	Support Pulse Repetition Frequency 4 MHz.
+ * @MCPS802154_LLHW_PRF_16:
+ *	Support Pulse Repetition Frequency 16 MHz.
+ * @MCPS802154_LLHW_PRF_64:
+ *	Support Pulse Repetition Frequency 64 MHz.
+ * @MCPS802154_LLHW_PRF_125:
+ *	Support Pulse Repetition Frequency 125 MHz.
+ * @MCPS802154_LLHW_PRF_250:
+ *	Support Pulse Repetition Frequency 250 MHz.
+ * @MCPS802154_LLHW_PSR_16:
+ *	Support 16 symbols in preamble symbol repetitions.
+ * @MCPS802154_LLHW_PSR_24:
+ *	Support 24 symbols in preamble symbol repetitions.
+ * @MCPS802154_LLHW_PSR_32:
+ *	Support 32 symbols in preamble symbol repetitions.
+ * @MCPS802154_LLHW_PSR_48:
+ *	Support 48 symbols in preamble symbol repetitions.
+ * @MCPS802154_LLHW_PSR_64:
+ *	Support 64 symbols in preamble symbol repetitions.
+ * @MCPS802154_LLHW_PSR_96:
+ *	Support 96 symbols in preamble symbol repetitions.
+ * @MCPS802154_LLHW_PSR_128:
+ *	Support 128 symbols in preamble symbol repetitions.
+ * @MCPS802154_LLHW_PSR_256:
+ *	Support 256 symbols in preamble symbol repetitions.
+ * @MCPS802154_LLHW_PSR_1024:
+ *	Support 1024 symbols in preamble symbol repetitions.
+ * @MCPS802154_LLHW_PSR_4096:
+ *	Support 4096 symbols in preamble symbol repetitions.
+ * @MCPS802154_LLHW_SFD_4A:
+ *	Support SFD defined in 4a.
+ * @MCPS802154_LLHW_SFD_4Z_4:
+ *	Support SFD defined in 4z with length of 4 symbols.
+ * @MCPS802154_LLHW_SFD_4Z_8:
+ *	Support SFD defined in 4z with length of 8 symbols.
+ * @MCPS802154_LLHW_SFD_4Z_16:
+ *	Support SFD defined in 4z with length of 16 symbols.
+ * @MCPS802154_LLHW_SFD_4Z_32:
+ *	Support SFD defined in 4z with length of 32 symbols.
+ * @MCPS802154_LLHW_STS_SEGMENT_1:
+ *	Support one STS segment.
+ * @MCPS802154_LLHW_STS_SEGMENT_2:
+ *	Support two STS segments.
+ * @MCPS802154_LLHW_STS_SEGMENT_3:
+ *	Support three STS segments.
+ * @MCPS802154_LLHW_STS_SEGMENT_4:
+ *	Support four STS segments.
+ * @MCPS802154_LLHW_AOA_AZIMUTH:
+ *	Support AOA azimuth [-90°,+90°].
+ * @MCPS802154_LLHW_AOA_AZIMUTH_FULL:
+ *	Support AOA full azimuth [-180°,+180°].
+ * @MCPS802154_LLHW_AOA_ELEVATION:
+ *	Support AOA elevation [-90°,+90°].
+ * @MCPS802154_LLHW_AOA_FOM:
+ *	Support AOA figure of merit.
  */
 enum mcps802154_llhw_flags {
 	MCPS802154_LLHW_RDEV = BIT(0),
 	MCPS802154_LLHW_ERDEV = BIT(1),
+	MCPS802154_LLHW_BPRF = BIT(2),
+	MCPS802154_LLHW_HPRF = BIT(3),
+	MCPS802154_LLHW_DATA_RATE_850K = BIT(4),
+	MCPS802154_LLHW_DATA_RATE_6M81 = BIT(5),
+	MCPS802154_LLHW_DATA_RATE_7M80 = BIT(6),
+	MCPS802154_LLHW_DATA_RATE_27M2 = BIT(7),
+	MCPS802154_LLHW_DATA_RATE_31M2 = BIT(8),
+	MCPS802154_LLHW_DATA_RATE_CUSTOM = BIT(9),
+	MCPS802154_LLHW_PHR_DATA_RATE_850K = BIT(10),
+	MCPS802154_LLHW_PHR_DATA_RATE_6M81 = BIT(11),
+	MCPS802154_LLHW_PRF_4 = BIT(12),
+	MCPS802154_LLHW_PRF_16 = BIT(13),
+	MCPS802154_LLHW_PRF_64 = BIT(14),
+	MCPS802154_LLHW_PRF_125 = BIT(15),
+	MCPS802154_LLHW_PRF_250 = BIT(16),
+	MCPS802154_LLHW_PSR_16 = BIT(17),
+	MCPS802154_LLHW_PSR_24 = BIT(18),
+	MCPS802154_LLHW_PSR_32 = BIT(19),
+	MCPS802154_LLHW_PSR_48 = BIT(20),
+	MCPS802154_LLHW_PSR_64 = BIT(21),
+	MCPS802154_LLHW_PSR_96 = BIT(22),
+	MCPS802154_LLHW_PSR_128 = BIT(23),
+	MCPS802154_LLHW_PSR_256 = BIT(24),
+	MCPS802154_LLHW_PSR_1024 = BIT(25),
+	MCPS802154_LLHW_PSR_4096 = BIT(26),
+	MCPS802154_LLHW_SFD_4A = BIT(27),
+	MCPS802154_LLHW_SFD_4Z_4 = BIT(28),
+	MCPS802154_LLHW_SFD_4Z_8 = BIT(29),
+	MCPS802154_LLHW_SFD_4Z_16 = BIT(30),
+	MCPS802154_LLHW_SFD_4Z_32 = BIT(31),
+	MCPS802154_LLHW_STS_SEGMENT_1 = BIT_ULL(32),
+	MCPS802154_LLHW_STS_SEGMENT_2 = BIT_ULL(33),
+	MCPS802154_LLHW_STS_SEGMENT_3 = BIT_ULL(34),
+	MCPS802154_LLHW_STS_SEGMENT_4 = BIT_ULL(35),
+	MCPS802154_LLHW_AOA_AZIMUTH = BIT_ULL(36),
+	MCPS802154_LLHW_AOA_AZIMUTH_FULL = BIT_ULL(37),
+	MCPS802154_LLHW_AOA_ELEVATION = BIT_ULL(38),
+	MCPS802154_LLHW_AOA_FOM = BIT_ULL(39),
 };
 
 /**
@@ -131,7 +252,7 @@ struct mcps802154_llhw {
 	/**
 	 * @flags: Low-level hardware flags, see &enum mcps802154_llhw_flags.
 	 */
-	u32 flags;
+	u64 flags;
 	/**
 	 * @hw: Pointer to IEEE802154 hardware exposed by MCPS. The low-level
 	 * driver needs to update this and hw->phy according to supported
@@ -153,57 +274,61 @@ struct mcps802154_llhw {
 	 * @priv: Driver private data.
 	 */
 	void *priv;
+	/**
+	 * @rx_ctx_size: size of the context.
+	 */
+	u32 rx_ctx_size;
 };
 
 /**
- * enum mcps802154_tx_frame_info_flags - Flags for transmitting a frame.
- * @MCPS802154_TX_FRAME_TIMESTAMP_DTU:
+ * enum mcps802154_tx_frame_config_flags - Flags for transmitting a frame.
+ * @MCPS802154_TX_FRAME_CONFIG_TIMESTAMP_DTU:
  *	Start transmission at given timestamp in device time unit.
- * @MCPS802154_TX_FRAME_CCA:
+ * @MCPS802154_TX_FRAME_CONFIG_CCA:
  *	Use CCA before transmission using the programmed mode.
- * @MCPS802154_TX_FRAME_RANGING:
+ * @MCPS802154_TX_FRAME_CONFIG_RANGING:
  *	Enable precise timestamping for the transmitted frame and its response
  *	(RDEV only).
- * @MCPS802154_TX_FRAME_KEEP_RANGING_CLOCK:
+ * @MCPS802154_TX_FRAME_CONFIG_KEEP_RANGING_CLOCK:
  *      Request that the ranging clock be kept valid after the transmission of
  *      this frame (RDEV only).
- * @MCPS802154_TX_FRAME_RANGING_PDOA:
+ * @MCPS802154_TX_FRAME_CONFIG_RANGING_PDOA:
  *	Enable phase difference of arrival measurement for the response frame
  *	(RDEV only).
- * @MCPS802154_TX_FRAME_SP1:
+ * @MCPS802154_TX_FRAME_CONFIG_SP1:
  *	Enable STS for the transmitted frame and its response, mode 1 (STS after
  *	SFD and before PHR, ERDEV only).
- * @MCPS802154_TX_FRAME_SP2:
+ * @MCPS802154_TX_FRAME_CONFIG_SP2:
  *	Enable STS for the transmitted frame and its response, mode 2 (STS after
  *	the payload, ERDEV only).
- * @MCPS802154_TX_FRAME_SP3:
+ * @MCPS802154_TX_FRAME_CONFIG_SP3:
  *	Enable STS for the transmitted frame and its response, mode 3 (STS after
  *	SFD, no PHR, no payload, ERDEV only).
- * @MCPS802154_TX_FRAME_STS_MODE_MASK:
+ * @MCPS802154_TX_FRAME_CONFIG_STS_MODE_MASK:
  *      Mask covering all the STS mode configuration values.
- * @MCPS802154_TX_FRAME_RANGING_ROUND:
+ * @MCPS802154_TX_FRAME_CONFIG_RANGING_ROUND:
  *	Inform low-level driver the transmitted frame is the start of a ranging
  *	round (RDEV only).
  *
  * If no timestamp flag is given, transmit as soon as possible.
  */
-enum mcps802154_tx_frame_info_flags {
-	MCPS802154_TX_FRAME_TIMESTAMP_DTU = BIT(0),
-	MCPS802154_TX_FRAME_CCA = BIT(1),
-	MCPS802154_TX_FRAME_RANGING = BIT(2),
-	MCPS802154_TX_FRAME_KEEP_RANGING_CLOCK = BIT(3),
-	MCPS802154_TX_FRAME_RANGING_PDOA = BIT(4),
-	MCPS802154_TX_FRAME_SP1 = BIT(5),
-	MCPS802154_TX_FRAME_SP2 = BIT(6),
-	MCPS802154_TX_FRAME_SP3 = BIT(5) | BIT(6),
-	MCPS802154_TX_FRAME_STS_MODE_MASK = BIT(5) | BIT(6),
-	MCPS802154_TX_FRAME_RANGING_ROUND = BIT(7),
+enum mcps802154_tx_frame_config_flags {
+	MCPS802154_TX_FRAME_CONFIG_TIMESTAMP_DTU = BIT(0),
+	MCPS802154_TX_FRAME_CONFIG_CCA = BIT(1),
+	MCPS802154_TX_FRAME_CONFIG_RANGING = BIT(2),
+	MCPS802154_TX_FRAME_CONFIG_KEEP_RANGING_CLOCK = BIT(3),
+	MCPS802154_TX_FRAME_CONFIG_RANGING_PDOA = BIT(4),
+	MCPS802154_TX_FRAME_CONFIG_SP1 = BIT(5),
+	MCPS802154_TX_FRAME_CONFIG_SP2 = BIT(6),
+	MCPS802154_TX_FRAME_CONFIG_SP3 = BIT(5) | BIT(6),
+	MCPS802154_TX_FRAME_CONFIG_STS_MODE_MASK = BIT(5) | BIT(6),
+	MCPS802154_TX_FRAME_CONFIG_RANGING_ROUND = BIT(7),
 };
 
 /**
- * struct mcps802154_tx_frame_info - Information for transmitting a frame.
+ * struct mcps802154_tx_frame_config - Information for transmitting a frame.
  */
-struct mcps802154_tx_frame_info {
+struct mcps802154_tx_frame_config {
 	/**
 	 * @timestamp_dtu: If timestamped, date of transmission start.
 	 */
@@ -221,7 +346,7 @@ struct mcps802154_tx_frame_info {
 	 */
 	int rx_enable_after_tx_timeout_dtu;
 	/**
-	 * @flags: See &enum mcps802154_tx_frame_info_flags.
+	 * @flags: See &enum mcps802154_tx_frame_config_flags.
 	 */
 	u8 flags;
 	/**
@@ -231,52 +356,52 @@ struct mcps802154_tx_frame_info {
 };
 
 /**
- * enum mcps802154_rx_info_flags - Flags for enabling the receiver.
- * @MCPS802154_RX_INFO_TIMESTAMP_DTU:
+ * enum mcps802154_rx_frame_config_flags - Flags for enabling the receiver.
+ * @MCPS802154_RX_FRAME_CONFIG_TIMESTAMP_DTU:
  *	Enable receiver at given timestamp in device time unit.
- * @MCPS802154_RX_INFO_AACK:
+ * @MCPS802154_RX_FRAME_CONFIG_AACK:
  *	Enable automatic acknowledgment.
- * @MCPS802154_RX_INFO_RANGING:
+ * @MCPS802154_RX_FRAME_CONFIG_RANGING:
  *	Enable precise timestamping for the received frame (RDEV only).
- * @MCPS802154_RX_INFO_KEEP_RANGING_CLOCK:
+ * @MCPS802154_RX_FRAME_CONFIG_KEEP_RANGING_CLOCK:
  *      Request that the ranging clock be kept valid after the reception of the
  *      frame (RDEV only).
- * @MCPS802154_RX_INFO_RANGING_PDOA:
+ * @MCPS802154_RX_FRAME_CONFIG_RANGING_PDOA:
  *	Enable phase difference of arrival measurement (RDEV only).
- * @MCPS802154_RX_INFO_SP1:
+ * @MCPS802154_RX_FRAME_CONFIG_SP1:
  *	Enable STS for the received frame, mode 1 (STS after SFD and before PHR,
  *	ERDEV only).
- * @MCPS802154_RX_INFO_SP2:
+ * @MCPS802154_RX_FRAME_CONFIG_SP2:
  *	Enable STS for the received frame, mode 2 (STS after the payload, ERDEV
  *	only).
- * @MCPS802154_RX_INFO_SP3:
+ * @MCPS802154_RX_FRAME_CONFIG_SP3:
  *	Enable STS for the received frame, mode 3 (STS after SFD, no PHR, no
  *	payload, ERDEV only).
- * @MCPS802154_RX_INFO_STS_MODE_MASK:
+ * @MCPS802154_RX_FRAME_CONFIG_STS_MODE_MASK:
  *      Mask covering all the STS mode configuration values.
- * @MCPS802154_RX_INFO_RANGING_ROUND:
+ * @MCPS802154_RX_FRAME_CONFIG_RANGING_ROUND:
  *	Inform low-level driver the expected received frame is the start of a
  *	ranging round (RDEV only).
  *
  * If no timestamp flag is given, enable receiver as soon as possible.
  */
-enum mcps802154_rx_info_flags {
-	MCPS802154_RX_INFO_TIMESTAMP_DTU = BIT(0),
-	MCPS802154_RX_INFO_AACK = BIT(1),
-	MCPS802154_RX_INFO_RANGING = BIT(2),
-	MCPS802154_RX_INFO_KEEP_RANGING_CLOCK = BIT(3),
-	MCPS802154_RX_INFO_RANGING_PDOA = BIT(4),
-	MCPS802154_RX_INFO_SP1 = BIT(5),
-	MCPS802154_RX_INFO_SP2 = BIT(6),
-	MCPS802154_RX_INFO_SP3 = BIT(5) | BIT(6),
-	MCPS802154_RX_INFO_STS_MODE_MASK = BIT(5) | BIT(6),
-	MCPS802154_RX_INFO_RANGING_ROUND = BIT(7),
+enum mcps802154_rx_frame_config_flags {
+	MCPS802154_RX_FRAME_CONFIG_TIMESTAMP_DTU = BIT(0),
+	MCPS802154_RX_FRAME_CONFIG_AACK = BIT(1),
+	MCPS802154_RX_FRAME_CONFIG_RANGING = BIT(2),
+	MCPS802154_RX_FRAME_CONFIG_KEEP_RANGING_CLOCK = BIT(3),
+	MCPS802154_RX_FRAME_CONFIG_RANGING_PDOA = BIT(4),
+	MCPS802154_RX_FRAME_CONFIG_SP1 = BIT(5),
+	MCPS802154_RX_FRAME_CONFIG_SP2 = BIT(6),
+	MCPS802154_RX_FRAME_CONFIG_SP3 = BIT(5) | BIT(6),
+	MCPS802154_RX_FRAME_CONFIG_STS_MODE_MASK = BIT(5) | BIT(6),
+	MCPS802154_RX_FRAME_CONFIG_RANGING_ROUND = BIT(7),
 };
 
 /**
- * struct mcps802154_rx_info - Information for enabling the receiver.
+ * struct mcps802154_rx_frame_config - Information for enabling the receiver.
  */
-struct mcps802154_rx_info {
+struct mcps802154_rx_frame_config {
 	/**
 	 * @timestamp_dtu: If timestamped, date to enable the receiver.
 	 */
@@ -287,7 +412,13 @@ struct mcps802154_rx_info {
 	 */
 	int timeout_dtu;
 	/**
-	 * @flags: See &enum mcps802154_rx_info_flags.
+	 * @frame_timeout_dtu: If no zero, timeout value for the full frame
+	 * reception. This allow limiting the length of accepted frame. The
+	 * timeout starts after &mcps802154_rx_frame_config.timeout_dtu value.
+	 */
+	int frame_timeout_dtu;
+	/**
+	 * @flags: See &enum mcps802154_rx_frame_config_flags.
 	 */
 	u8 flags;
 	/**
@@ -363,7 +494,8 @@ struct mcps802154_rx_frame_info {
 	 */
 	int frame_duration_dtu;
 	/**
-	 * @rssi: Received signal strength indication (RSSI).
+	 * @rssi: Received signal strength indication (RSSI),
+	 * absolute value in Q1 fixed point format.
 	 */
 	int rssi;
 	/**
@@ -378,16 +510,6 @@ struct mcps802154_rx_frame_info {
 	 */
 	int ranging_offset_rctu;
 	/**
-	 * @ranging_pdoa_rad_q11: Phase difference of arrival, unit is radian
-	 * multiplied by 2048 (RDEV only).
-	 */
-	int ranging_pdoa_rad_q11;
-	/**
-	 * @ranging_aoa_rad_q11: AoA interpolated by the driver from its
-	 * calibration LUT. unit is rad multiplied by 2048 (RDEV only).
-	 */
-	int ranging_aoa_rad_q11;
-	/**
 	 * @ranging_sts_timestamp_diffs_rctu: For each SRMARKERx, difference
 	 * between the measured timestamp and the expected timestamp relative to
 	 * RMARKER in ranging count time unit (ERDEV only). When STS mode is
@@ -423,6 +545,190 @@ struct mcps802154_rx_frame_info {
 };
 
 /**
+ * enum mcps802154_rx_measurement_info_flags - Flags for measurements on a received
+ * frame.
+ * @MCPS802154_RX_MEASUREMENTS_TIMESTAMP:
+ *	Set by MCPS to request time of arrival measurement and associated figure
+ *	of merit (RDEV only).
+ * @MCPS802154_RX_MEASUREMENTS_CLOCK_OFFSET:
+ *	Set by MCPS to request clock characterization data (RDEV only).
+ * @MCPS802154_RX_MEASUREMENTS_STS_SEGS_TIMESTAMPS:
+ *	Set by MCPS to request time of arrival measurement on STS segments and
+ *	associated figure of merit (ERDEV only).
+ * @MCPS802154_RX_MEASUREMENTS_RSSIS:
+ *	Set by MCPS to request RSSI values.
+ * @MCPS802154_RX_MEASUREMENTS_AOAS:
+ *	Set by MCPS to request angle of arrival measurements, time difference of
+ *	arrival, phase difference of arrival and associated figure of merit
+ *	(RDEV only).
+ * @MCPS802154_RX_MEASUREMENTS_CIRS:
+ *	Set by MCPS to request CIR samples (RDEV only).
+ * @MCPS802154_RX_MEASUREMENTS_VENDOR0:
+ *	Set by MCPS to request first set of vendor specific measurements.
+ * @MCPS802154_RX_MEASUREMENTS_VENDOR1:
+ *	Set by MCPS to request second set of vendor specific measurements.
+ * @MCPS802154_RX_MEASUREMENTS_VENDOR2:
+ *	Set by MCPS to request third set of vendor specific measurements.
+ * @MCPS802154_RX_MEASUREMENTS_VENDOR3:
+ *	Set by MCPS to request fourth set of vendor specific measurements.
+ *
+ * The low-level driver must clear the corresponding flag if the information is
+ * not available.
+ */
+enum mcps802154_rx_measurement_info_flags {
+	MCPS802154_RX_MEASUREMENTS_TIMESTAMP = BIT(0),
+	MCPS802154_RX_MEASUREMENTS_CLOCK_OFFSET = BIT(1),
+	MCPS802154_RX_MEASUREMENTS_STS_SEGS_TIMESTAMPS = BIT(2),
+	MCPS802154_RX_MEASUREMENTS_RSSIS = BIT(3),
+	MCPS802154_RX_MEASUREMENTS_AOAS = BIT(4),
+	MCPS802154_RX_MEASUREMENTS_CIRS = BIT(5),
+	MCPS802154_RX_MEASUREMENTS_VENDOR0 = BIT(12),
+	MCPS802154_RX_MEASUREMENTS_VENDOR1 = BIT(13),
+	MCPS802154_RX_MEASUREMENTS_VENDOR2 = BIT(14),
+	MCPS802154_RX_MEASUREMENTS_VENDOR3 = BIT(15),
+};
+
+/**
+ * struct mcps802154_rx_aoa_measurements - Angle of arrival measurements on a
+ * received frame (RDEV only).
+ */
+struct mcps802154_rx_aoa_measurements {
+	/**
+	 * @tdoa_rctu: Time difference of arrival, in ranging count time unit.
+	 */
+	s16 tdoa_rctu;
+	/**
+	 * @pdoa_rad_q11: Phase difference of arrival, unit is radian multiplied
+	 * by 2048.
+	 */
+	s16 pdoa_rad_q11;
+	/**
+	 * @aoa_rad_q11: Angle of arrival, unit is radian multiplied by 2048.
+	 */
+	s16 aoa_rad_q11;
+	/**
+	 * @fom: Measurements figure of merit (FoM). Range is 0 to 255, with 0
+	 * being an invalid measure and 255 being a 100% confidence.
+	 */
+	u8 fom;
+	/**
+	 * @type: Measurement type (azimuth, elevation...). Actual value is
+	 * driver dependant.
+	 */
+	u8 type;
+};
+
+/**
+ * struct mcps802154_rx_cir_sample_window - Window of CIR samples.
+ */
+struct mcps802154_rx_cir_sample_window {
+	/**
+	 * @n_samples: The number of samples contained in the window.
+	 */
+	u16 n_samples;
+	/**
+	 * @sizeof_sample: The size of a single sample.
+	 */
+	u16 sizeof_sample;
+	/**
+	 * @samples: CIR samples values.
+	 *
+	 * Each sample is composed of the real and imaginary part which are
+	 * signed numbers. Each sample is encoded using the platform endianness
+	 * with @mcps802154_rx_cir_sample_window.sizeof_sample bytes, first half
+	 * is the real part, second half is the imaginary part.
+	 *
+	 * Must be kept valid until next received frame
+	 */
+	void *samples;
+};
+
+/**
+ * struct mcps802154_rx_cir - CIR measurements.
+ */
+struct mcps802154_rx_cir {
+	/**
+	 * @fp_index: The absolute index of the sample considered as first path.
+	 */
+	u16 fp_index;
+	/**
+	 * @fp_snr: The SNR of the sample considered as first path.
+	 */
+	s16 fp_snr;
+	/**
+	 * @fp_ns_q6: (Q10.6) Time in nanosecond of the first path index
+	 */
+	u16 fp_ns_q6;
+	/**
+	 * @pp_index: The absolute index of the sample considered as peak path.
+	 */
+	u16 pp_index;
+	/**
+	 * @pp_snr: The SNR of the sample considered as peak path.
+	 */
+	s16 pp_snr;
+	/**
+	 * @pp_ns_q6: (Q10.6) Time in nanosecond of the peak path index
+	 */
+	u16 pp_ns_q6;
+	/**
+	 * @fp_sample_offset: The offset of the first path in the sample window.
+	 */
+	u16 fp_sample_offset;
+	/**
+	 * @sample_window: CIR samples.
+	 */
+	struct mcps802154_rx_cir_sample_window sample_window;
+};
+
+/**
+ * struct mcps802154_rx_measurement_info - Measurements on a received frame.
+ */
+struct mcps802154_rx_measurement_info {
+	/**
+	 * @n_rssis: The number of RSSI computed for this frame. Depends on the
+	 * antenna set used to receive.
+	 *
+	 * Set by low-level driver.
+	 */
+	int n_rssis;
+	/**
+	 * @rssis_q1: Received signal strength indication (RSSI), array of
+	 * absolute values in Q7.1 fixed point format, unit is dBm.
+	 */
+	u8 rssis_q1[MCPS802154_RSSIS_N_MAX];
+	/**
+	 * @n_aoas: Number of angle of arrival measurements.
+	 *
+	 * Set by low-level driver.
+	 */
+	int n_aoas;
+	/**
+	 * @aoas: Angle of arrival measurements, ordered by increasing
+	 * measurement type.
+	 */
+	struct mcps802154_rx_aoa_measurements
+		aoas[MCPS802154_RX_AOA_MEASUREMENTS_MAX];
+	/**
+	 * @n_cirs: Number of parts of CIR measurements.
+	 *
+	 * Set by low-level driver.
+	 */
+	int n_cirs;
+	/**
+	 * @cirs: CIR measurements for different parts of the frame.
+	 *
+	 * Set by low-level driver, must be kept valid until next received
+	 * frame.
+	 */
+	struct mcps802154_rx_cir *cirs;
+	/**
+	 * @flags: See &enum mcps802154_rx_measurement_info_flags.
+	 */
+	int flags;
+};
+
+/**
  * struct mcps802154_sts_params - STS parameters for HRP UWB.
  */
 struct mcps802154_sts_params {
@@ -458,6 +764,246 @@ struct mcps802154_sts_params {
 };
 
 /**
+ * enum mcps802154_prf - Pulse repetition frequency.
+ * @MCPS802154_PRF_16:
+ *	16 MHz, only used in 4a.
+ * @MCPS802154_PRF_64:
+ *	64 MHz, used for 4a and 4z BPRF.
+ * @MCPS802154_PRF_125:
+ *	125 MHz, used for 4z HPRF.
+ * @MCPS802154_PRF_250:
+ *	250 MHz, used for 4z HPRF.
+ */
+enum mcps802154_prf {
+	MCPS802154_PRF_16 = 16,
+	MCPS802154_PRF_64 = 64,
+	MCPS802154_PRF_125 = 125,
+	MCPS802154_PRF_250 = 250,
+};
+
+/**
+ * enum mcps802154_psr - Number of preamble symbol repetitions in the SYNC
+ * sequence.
+ * @MCPS802154_PSR_16:
+ *	16 symbols, used in 4a and 4z HPRF.
+ * @MCPS802154_PSR_24:
+ *	24 symbols, used only in 4z HPRF.
+ * @MCPS802154_PSR_32:
+ *	32 symbols, used only in 4z HPRF.
+ * @MCPS802154_PSR_48:
+ *	48 symbols, used only in 4z HPRF.
+ * @MCPS802154_PSR_64:
+ *	64 symbols, used 4a and 4z BPRF and HPRF.
+ * @MCPS802154_PSR_96:
+ *	96 symbols, used only in 4z HPRF.
+ * @MCPS802154_PSR_128:
+ *	128 symbols, used only in 4z HPRF.
+ * @MCPS802154_PSR_256:
+ *	256 symbols, used only in 4z HPRF.
+ * @MCPS802154_PSR_1024:
+ *	1024 symbols, used only in 4a.
+ * @MCPS802154_PSR_4096:
+ *	4096 symbols, used only in 4a.
+ */
+enum mcps802154_psr {
+	MCPS802154_PSR_16 = 16,
+	MCPS802154_PSR_24 = 24,
+	MCPS802154_PSR_32 = 32,
+	MCPS802154_PSR_48 = 48,
+	MCPS802154_PSR_64 = 64,
+	MCPS802154_PSR_96 = 96,
+	MCPS802154_PSR_128 = 128,
+	MCPS802154_PSR_256 = 256,
+	MCPS802154_PSR_1024 = 1024,
+	MCPS802154_PSR_4096 = 4096,
+};
+
+/**
+ * enum mcps802154_sfd - sfd type selector.
+ * @MCPS802154_SFD_4A:
+ *	SFD defined in 4a, length of 8 symbols.
+ * @MCPS802154_SFD_4Z_4:
+ *	SFD defined in 4z, length of 4 symbols.
+ * @MCPS802154_SFD_4Z_8:
+ *	SFD defined in 4z, length of 8 symbols.
+ * @MCPS802154_SFD_4Z_16:
+ *	SFD defined in 4z, length of 16 symbols.
+ * @MCPS802154_SFD_4Z_32:
+ *	SFD defined in 4z, length of 32 symbols.
+ */
+enum mcps802154_sfd {
+	MCPS802154_SFD_4A,
+	MCPS802154_SFD_4Z_4,
+	MCPS802154_SFD_4Z_8,
+	MCPS802154_SFD_4Z_16,
+	MCPS802154_SFD_4Z_32,
+};
+
+/**
+ * enum mcps802154_data_rate - Data rate.
+ * @MCPS802154_DATA_RATE_850K:
+ *	850 kbps, used only for 4a.
+ * @MCPS802154_DATA_RATE_6M81:
+ *	6.81 Mbps, used for 4a and 4z (PRF must be 125MHz).
+ * @MCPS802154_DATA_RATE_7M80:
+ *	7.80 Mbps, only used for 4z (PRF must be 125MHz).
+ * @MCPS802154_DATA_RATE_27M2:
+ *	27.2 Mbps, used for 4a and 4z (PRF must be 250MHz).
+ * @MCPS802154_DATA_RATE_31M2:
+ *	31.2 Mbps, used for 4z (PRF must be 250MHz).
+ * NOTE: device specific values can be set to use a custom data rate.
+ */
+enum mcps802154_data_rate {
+	MCPS802154_DATA_RATE_850K = 0,
+	MCPS802154_DATA_RATE_6M81 = 6,
+	MCPS802154_DATA_RATE_7M80 = 7,
+	MCPS802154_DATA_RATE_27M2 = 27,
+	MCPS802154_DATA_RATE_31M2 = 31,
+};
+
+/**
+ * enum mcps802154_hrp_uwb_psdu_size - PSDU size in HPRF.
+ * @MCPS802154_HRP_UWB_PSDU_SIZE_1023:
+ *	1023-bytes PSDU.
+ * @MCPS802154_HRP_UWB_PSDU_SIZE_2047:
+ *	2047-bytes PSDU.
+ * @MCPS802154_HRP_UWB_PSDU_SIZE_4095:
+ *	4095-bytes PSDU.
+ */
+enum mcps802154_hrp_uwb_psdu_size {
+	MCPS802154_HRP_UWB_PSDU_SIZE_1023 = 0,
+	MCPS802154_HRP_UWB_PSDU_SIZE_2047 = 1,
+	MCPS802154_HRP_UWB_PSDU_SIZE_4095 = 2,
+};
+
+/**
+ * struct mcps802154_hrp_uwb_params - Parameters for HRP UWB.
+ *
+ * Parameters are given directly to driver without checking. The driver needs to
+ * check the parameters for supported values, but it can accept non-standard
+ * values.
+ */
+struct mcps802154_hrp_uwb_params {
+	/**
+	 * @prf: Nominal mean Pulse Repetition Frequency.
+	 *
+	 * For 4a, one of MCPS802154_PRF_16 or MCPS802154_PRF_64.
+	 *
+	 * For 4z BPRF, must be MCPS802154_PRF_64.
+	 *
+	 * For 4z HPRF, one of MCPS802154_PRF_125 or MCPS802154_PRF_250.
+	 */
+	enum mcps802154_prf prf;
+	/**
+	 * @psr: Number of preamble symbol repetitions in the SYNC sequence, or
+	 * preamble length.
+	 *
+	 * For 4a, one of 16, 64, 1024 or 4096.
+	 *
+	 * For 4z BPRF, must be 64.
+	 *
+	 * For 4z HPRF, one of 16, 24, 32, 48, 64, 96, 128 or 256.
+	 */
+	enum mcps802154_psr psr;
+	/**
+	 * @sfd_selector: SFD type selector.
+	 *
+	 * When MCPS802154_SFD_4A, use short SFD defined in 802.15.4a.
+	 *
+	 * When MCPS802154_SFD_4Z_*, use SFD defined in 802.15.4z, with length
+	 * 4, 8, 16 or 32.
+	 *
+	 * For 4a, must be MCPS802154_SFD_4A.
+	 *
+	 * For 4z BPRF, one of MCPS802154_SFD_4A or MCPS802154_SFD_4Z_8.
+	 *
+	 * For 4z HPRF, one of MCPS802154_SFD_4Z_{4,8,16,32}.
+	 */
+	enum mcps802154_sfd sfd_selector;
+	/**
+	 * @data_rate: Data rate.
+	 *
+	 * For 4a, one of 850 kbps, 6.81 Mbps or 27.2 Mbps.
+	 *
+	 * For 4z BPRF, must be 6.81 Mbps.
+	 *
+	 * For 4z HPRF at 125 MHz, use 6.81 Mbps or 7.8 Mbps.
+	 *
+	 * For 4z HPRF at 250 MHz, use 27.2 Mbps or 31.2 Mbps.
+	 */
+	int data_rate;
+	/**
+	 * @phr_hi_rate: Use high PHR data rate, for 4z BPRF only.
+	 *
+	 * For 4a and 4z HPRF, this parameter is ignored.
+	 *
+	 * For 4z BPRF, when enabled use 6.81 Mbps, otherwise use 850 kbps.
+	 */
+	bool phr_hi_rate;
+	/**
+	 * @psdu_size: PSDU size in HPRF.
+	 */
+	enum mcps802154_hrp_uwb_psdu_size psdu_size;
+};
+
+/**
+ * enum mcps802154_antenna_caps - Antenna set capabilities
+ * @MCPS802154_AOA_X_AXIS:
+ *   Antenna can report azimuth
+ * @MCPS802154_AOA_Y_AXIS:
+ *   Antenna can report elevation
+ */
+enum mcps802154_antenna_caps {
+	MCPS802154_AOA_X_AXIS = BIT(0),
+	MCPS802154_AOA_Y_AXIS = BIT(1),
+};
+
+/**
+ * enum mcps802154_power_state - Power states
+ * @MCPS802154_PWR_STATE_OFF:
+ *	Power off state.
+ * @MCPS802154_PWR_STATE_SLEEP:
+ * 	Deep sleep state.
+ * @MCPS802154_PWR_STATE_IDLE:
+ * 	Idle state, ready to transmit or receive.
+ * @MCPS802154_PWR_STATE_RX:
+ * 	Receive state.
+ * @MCPS802154_PWR_STATE_TX:
+ * 	Transmit state.
+ * @MCPS802154_PWR_STATE_MAX:
+ * 	Total power states count.
+ */
+enum mcps802154_power_state {
+	MCPS802154_PWR_STATE_OFF,
+	MCPS802154_PWR_STATE_SLEEP,
+	MCPS802154_PWR_STATE_IDLE,
+	MCPS802154_PWR_STATE_RX,
+	MCPS802154_PWR_STATE_TX,
+	MCPS802154_PWR_STATE_MAX
+};
+
+/**
+ * struct mcps802154_power_state_stats - Statistics for a power state.
+ * @dur: Duration in this power state in ns.
+ * @count: Count of transitions in this power state.
+ */
+struct mcps802154_power_state_stats {
+	u64 dur;
+	u64 count;
+};
+
+/**
+ * struct mcps802154_power_stats - Global power statistics.
+ * @power_state_stats: Array of power statistics for each power state.
+ * @interrupts: Hardware interrupts count on the device.
+ */
+struct mcps802154_power_stats {
+	struct mcps802154_power_state_stats
+		power_state_stats[MCPS802154_PWR_STATE_MAX];
+	u64 interrupts;
+};
+
+/**
  * struct mcps802154_ops - Callback from MCPS to the driver.
  */
 struct mcps802154_ops {
@@ -475,7 +1021,7 @@ struct mcps802154_ops {
 	/**
 	 * @tx_frame: Transmit a frame. skb contains the buffer starting from
 	 * the IEEE 802.15.4 header. The low-level driver should send the frame
-	 * as specified in info. Receiver should be disabled automatically
+	 * as specified in config. Receiver should be disabled automatically
 	 * unless a frame is being received.
 	 *
 	 * The &frame_idx parameter gives the index of the frame in a "block".
@@ -489,7 +1035,7 @@ struct mcps802154_ops {
 	 * -EBUSY if a reception is happening right now, or any other error.
 	 */
 	int (*tx_frame)(struct mcps802154_llhw *llhw, struct sk_buff *skb,
-			const struct mcps802154_tx_frame_info *info,
+			const struct mcps802154_tx_frame_config *config,
 			int frame_idx, int next_delay_dtu);
 	/**
 	 * @rx_enable: Enable receiver.
@@ -505,8 +1051,8 @@ struct mcps802154_ops {
 	 * timestamp, or any other error.
 	 */
 	int (*rx_enable)(struct mcps802154_llhw *llhw,
-			 const struct mcps802154_rx_info *info, int frame_idx,
-			 int next_delay_dtu);
+			 const struct mcps802154_rx_frame_config *config,
+			 int frame_idx, int next_delay_dtu);
 	/**
 	 * @rx_disable: Disable receiver, or a programmed receiver enabling,
 	 * unless a frame reception is happening right now.
@@ -538,6 +1084,14 @@ struct mcps802154_ops {
 	int (*rx_get_error_frame)(struct mcps802154_llhw *llhw,
 				  struct mcps802154_rx_frame_info *info);
 	/**
+	 * @rx_get_measurement: Get measurement associated with a received
+	 * frame.
+	 *
+	 * Return: 0, -EBUSY if no longer available, or any other error.
+	 */
+	int (*rx_get_measurement)(struct mcps802154_llhw *llhw, void *rx_ctx,
+				  struct mcps802154_rx_measurement_info *info);
+	/**
 	 * @idle: Put the device into idle mode without time limit or until the
 	 * given timestamp.  The driver should call &mcps802154_timer_expired()
 	 * before the given timestamp so that an action can be programmed at the
@@ -581,9 +1135,11 @@ struct mcps802154_ops {
 	 *
 	 * Return: The RMARKER timestamp.
 	 */
-	u64 (*tx_timestamp_dtu_to_rmarker_rctu)(struct mcps802154_llhw *llhw,
-						u32 tx_timestamp_dtu,
-						int ant_set_id);
+	u64 (*tx_timestamp_dtu_to_rmarker_rctu)(
+		struct mcps802154_llhw *llhw, u32 tx_timestamp_dtu,
+		const struct mcps802154_hrp_uwb_params *hrp_uwb_params,
+		const struct mcps802154_channel *channel_params,
+		int ant_set_id);
 	/**
 	 * @difference_timestamp_rctu: Compute the difference between two
 	 * timestamp values.
@@ -617,9 +1173,18 @@ struct mcps802154_ops {
 	 *
 	 * Return: 0 or error.
 	 */
-	int (*set_hrp_uwb_params)(struct mcps802154_llhw *llhw, int prf,
-				  int psr, int sfd_selector, int phr_rate,
-				  int data_rate);
+	int (*set_hrp_uwb_params)(
+		struct mcps802154_llhw *llhw,
+		const struct mcps802154_hrp_uwb_params *params);
+	/**
+	 * @check_hrp_uwb_params: Check that the HRP parameters are compatible
+	 * with the hardware capabilities.
+	 *
+	 * Return: 0 or error.
+	 */
+	int (*check_hrp_uwb_params)(
+		struct mcps802154_llhw *llhw,
+		const struct mcps802154_hrp_uwb_params *params);
 	/**
 	 * @set_sts_params: Set STS parameters (ERDEV only).
 	 *
@@ -706,6 +1271,20 @@ struct mcps802154_ops {
 	 */
 	int (*vendor_cmd)(struct mcps802154_llhw *llhw, u32 vendor_id,
 			  u32 subcmd, void *data, size_t data_len);
+	/**
+	 * @get_antenna_caps: Return antenna set capabilites.
+	 *
+	 * Return: 0 or error.
+	 */
+	int (*get_antenna_caps)(struct mcps802154_llhw *llhw, int ant_idx,
+				u32 *caps);
+	/**
+	 * @get_power_stats: Get the power statistics.
+	 *
+	 * Return: 0 or error.
+	 */
+	int (*get_power_stats)(struct mcps802154_llhw *llhw,
+			       struct mcps802154_power_stats *pwr_stats);
 #ifdef CONFIG_MCPS802154_TESTMODE
 	/**
 	 * @testmode_cmd: Run a testmode command.
@@ -736,9 +1315,11 @@ struct mcps802154_ops {
  * @MCPS802154_RX_ERROR_FILTERED:
  *	A received frame was rejected due to frame filter.
  * @MCPS802154_RX_ERROR_SFD_TIMEOUT:
- *	A preamble has been detected but no SFD.
+ *	A preamble has been detected but without SFD.
  * @MCPS802154_RX_ERROR_OTHER:
  *	Other error, frame reception is aborted.
+ * @MCPS802154_RX_ERROR_PHR_DECODE:
+ *	the preamble and SFD have been detected but without PHR.
  * @MCPS802154_RX_ERROR_HPDWARN:
  *   Too late to program RX operation.
  */
@@ -750,7 +1331,8 @@ enum mcps802154_rx_error_type {
 	MCPS802154_RX_ERROR_FILTERED = 4,
 	MCPS802154_RX_ERROR_SFD_TIMEOUT = 5,
 	MCPS802154_RX_ERROR_OTHER = 6,
-	MCPS802154_RX_ERROR_HPDWARN = 7,
+	MCPS802154_RX_ERROR_PHR_DECODE = 7,
+	MCPS802154_RX_ERROR_HPDWARN = 8,
 };
 
 /**
diff --git a/mac/include/net/mcps802154_frame.h b/mac/include/net/mcps802154_frame.h
index 6052f5d..2bc8105 100644
--- a/mac/include/net/mcps802154_frame.h
+++ b/mac/include/net/mcps802154_frame.h
@@ -25,6 +25,7 @@
 #define NET_MCPS802154_FRAME_H
 
 #include <linux/skbuff.h>
+#include "mcps802154.h"
 
 #define IEEE802154_FC_NO_SEQ_SHIFT 8
 #define IEEE802154_FC_NO_SEQ (1 << IEEE802154_FC_NO_SEQ_SHIFT)
@@ -270,13 +271,16 @@ int mcps802154_get_current_timestamp_dtu(struct mcps802154_llhw *llhw,
  * device time unit (RDEV only).
  * @llhw: Low-level device pointer.
  * @tx_timestamp_dtu: TX timestamp in device time unit.
+ * @hrp_uwb_params: HRP UWB parameters.
+ * @channel_params: Channel parameters.
  * @ant_set_id: Antennas set id used to transmit.
  *
  * Return: RMARKER timestamp in ranging count time unit.
  */
-u64 mcps802154_tx_timestamp_dtu_to_rmarker_rctu(struct mcps802154_llhw *llhw,
-						u32 tx_timestamp_dtu,
-						int ant_set_id);
+u64 mcps802154_tx_timestamp_dtu_to_rmarker_rctu(
+	struct mcps802154_llhw *llhw, u32 tx_timestamp_dtu,
+	const struct mcps802154_hrp_uwb_params *hrp_uwb_params,
+	const struct mcps802154_channel *channel_params, int ant_set_id);
 
 /**
  * mcps802154_difference_timestamp_rctu() - Compute the difference between two
@@ -316,4 +320,15 @@ int mcps802154_compute_frame_duration_dtu(struct mcps802154_llhw *llhw,
 int mcps802154_vendor_cmd(struct mcps802154_llhw *llhw, u32 vendor_id,
 			  u32 subcmd, void *data, size_t data_len);
 
+/**
+ * mcps802154_rx_get_measurement() - Get measurement.
+ * @llhw: Low-level device pointer.
+ * @rx_ctx: Rx context (can be NULL).
+ * @info: Measurements updated by the llhw.
+ *
+ * Return: 0 or error.
+ */
+int mcps802154_rx_get_measurement(struct mcps802154_llhw *llhw, void *rx_ctx,
+				  struct mcps802154_rx_measurement_info *info);
+
 #endif /* NET_MCPS802154_FRAME_H */
diff --git a/mac/include/net/mcps802154_nl.h b/mac/include/net/mcps802154_nl.h
index 8423a9d..9aa5a9b 100644
--- a/mac/include/net/mcps802154_nl.h
+++ b/mac/include/net/mcps802154_nl.h
@@ -55,12 +55,10 @@
  * @MCPS802154_CMD_TESTMODE:
  *	Run a testmode command with TESTDATA blob attribute to pass through
  *	to the driver.
- * @MCPS802154_CMD_SET_RANGING_REQUESTS:
- *	Set the list of ranging requests.
- * @MCPS802154_CMD_RANGING_REPORT:
- *	Result of ranging.
- * @MCPS802154_CMD_PING_PONG_REPORT:
- *	Result of a ping pong request.
+ * @MCPS802154_CMD_CLOSE_SCHEDULER:
+ *	Close current scheduler and its regions.
+ * @MCPS802154_CMD_GET_PWR_STATS:
+ *	Get the power statistics.
  *
  * @MCPS802154_CMD_UNSPEC: Invalid command.
  * @__MCPS802154_CMD_AFTER_LAST: Internal use.
@@ -71,26 +69,18 @@ enum mcps802154_commands {
 
 	MCPS802154_CMD_GET_HW, /* can dump */
 	MCPS802154_CMD_NEW_HW,
-
 	MCPS802154_CMD_SET_SCHEDULER,
 	MCPS802154_CMD_SET_SCHEDULER_PARAMS,
 	MCPS802154_CMD_CALL_SCHEDULER,
-
 	MCPS802154_CMD_SET_SCHEDULER_REGIONS,
 	MCPS802154_CMD_SET_REGIONS_PARAMS,
 	MCPS802154_CMD_CALL_REGION,
-
 	MCPS802154_CMD_SET_CALIBRATIONS,
 	MCPS802154_CMD_GET_CALIBRATIONS,
 	MCPS802154_CMD_LIST_CALIBRATIONS,
-
 	MCPS802154_CMD_TESTMODE,
-
-	/* Temporary ranging interface. */
-	MCPS802154_CMD_SET_RANGING_REQUESTS,
-	MCPS802154_CMD_RANGING_REPORT,
-	MCPS802154_CMD_PING_PONG_REPORT,
-
+	MCPS802154_CMD_CLOSE_SCHEDULER,
+	MCPS802154_CMD_GET_PWR_STATS,
 	__MCPS802154_CMD_AFTER_LAST,
 	MCPS802154_CMD_MAX = __MCPS802154_CMD_AFTER_LAST - 1
 };
@@ -119,13 +109,8 @@ enum mcps802154_commands {
  *	driver-specific attributes.
  * @MCPS802154_ATTR_CALIBRATIONS:
  *	Nested array of calibrations.
- * @MCPS802154_ATTR_RANGING_REQUESTS:
- *	List of ranging requests. This is a nested attribute containing an array
- *	of nested attributes.
- * @MCPS802154_ATTR_RANGING_RESULT:
- *	Ranging result, this is a nested attribute.
- * @MCPS802154_ATTR_PING_PONG_RESULT:
- *	Ping pong result, this is a nested attribute.
+ * @MCPS802154_ATTR_PWR_STATS:
+ * 	Nested power statistics data.
  *
  * @MCPS802154_ATTR_UNSPEC: Invalid command.
  * @__MCPS802154_ATTR_AFTER_LAST: Internal use.
@@ -150,10 +135,7 @@ enum mcps802154_attrs {
 
 	MCPS802154_ATTR_CALIBRATIONS,
 
-	/* Temporary ranging interface. */
-	MCPS802154_ATTR_RANGING_REQUESTS,
-	MCPS802154_ATTR_RANGING_RESULT,
-	MCPS802154_ATTR_PING_PONG_RESULT,
+	MCPS802154_ATTR_PWR_STATS,
 
 	__MCPS802154_ATTR_AFTER_LAST,
 	MCPS802154_ATTR_MAX = __MCPS802154_ATTR_AFTER_LAST - 1
@@ -191,35 +173,6 @@ enum mcps802154_region_attrs {
 };
 
 /**
- * enum mcps802154_ranging_request_attrs - Ranging request.
- *
- * @MCPS802154_RANGING_REQUEST_ATTR_ID:
- *	Request identifier, returned in report.
- * @MCPS802154_RANGING_REQUEST_ATTR_FREQUENCY_HZ:
- *	Ranging frequency in Hz.
- * @MCPS802154_RANGING_REQUEST_ATTR_PEER:
- *	Ranging peer extended address.
- * @MCPS802154_RANGING_REQUEST_ATTR_REMOTE_PEER:
- *	Ranging remote peer extended address.
- *
- * @MCPS802154_RANGING_REQUEST_ATTR_UNSPEC: Invalid command.
- * @__MCPS802154_RANGING_REQUEST_ATTR_AFTER_LAST: Internal use.
- * @MCPS802154_RANGING_REQUEST_ATTR_MAX: Internal use.
- */
-enum mcps802154_ranging_request_attrs {
-	MCPS802154_RANGING_REQUEST_ATTR_UNSPEC,
-
-	MCPS802154_RANGING_REQUEST_ATTR_ID,
-	MCPS802154_RANGING_REQUEST_ATTR_FREQUENCY_HZ,
-	MCPS802154_RANGING_REQUEST_ATTR_PEER,
-	MCPS802154_RANGING_REQUEST_ATTR_REMOTE_PEER,
-
-	__MCPS802154_RANGING_REQUEST_ATTR_AFTER_LAST,
-	MCPS802154_RANGING_REQUEST_ATTR_MAX =
-		__MCPS802154_RANGING_REQUEST_ATTR_AFTER_LAST - 1
-};
-
-/**
  * enum mcps802154_calibrations_attrs - Calibration item.
  *
  * @MCPS802154_CALIBRATIONS_ATTR_KEY:
@@ -246,67 +199,56 @@ enum mcps802154_calibrations_attrs {
 };
 
 /**
- * enum mcps802154_ranging_result_attrs - Ranging result.
- *
- * @MCPS802154_RANGING_RESULT_ATTR_ID:
- *	Identifier of request.
- * @MCPS802154_RANGING_RESULT_ATTR_TOF_RCTU:
- *	Time of flight in RCTU.
- * @MCPS802154_RANGING_RESULT_ATTR_LOCAL_PDOA_RAD_Q11:
- *	Local Phase Difference Of Arrival, unit is multiple of 2048.
- * @MCPS802154_RANGING_RESULT_ATTR_REMOTE_PDOA_RAD_Q11:
- *	Remote Phase Difference Of Arrival, unit is multiple of 2048.
- * @MCPS802154_RANGING_RESULT_ATTR_LOCAL_PDOA_ELEVATION_RAD_Q11:
- *	Local Phase Difference Of Arrival with second pair antenna, unit is multiple of 2048.
- * @MCPS802154_RANGING_RESULT_ATTR_REMOTE_PDOA_ELEVATION_RAD_Q11:
- *	Remote Phase Difference Of Arrival with second pair antenna, unit is multiple of 2048.
- *
- * @MCPS802154_RANGING_RESULT_ATTR_UNSPEC: Invalid command.
- * @__MCPS802154_RANGING_RESULT_ATTR_AFTER_LAST: Internal use.
- * @MCPS802154_RANGING_RESULT_ATTR_MAX: Internal use.
+ * enum mcps802154_nl_pwr_stats_state_attrs - Power state item.
+ *
+ * @MCPS802154_PWR_STATS_STATE_ATTR_TIME:
+ * 	Time spent in this state.
+ * @MCPS802154_PWR_STATS_STATE_ATTR_COUNT:
+ * 	Number of transitions to this state.
+ * @MCPS802154_PWR_STATS_STATE_ATTR_UNSPEC: Invalid command.
+ * @__MCPS802154_PWR_STATS_STATE_ATTR_AFTER_LAST: Internal use.
+ * @MCPS802154_PWR_STATS_STATE_ATTR_MAX: Internal use.
  */
-enum mcps802154_ranging_result_attrs {
-	MCPS802154_RANGING_RESULT_ATTR_UNSPEC,
+enum mcps802154_nl_pwr_stats_state_attrs {
+	MCPS802154_PWR_STATS_STATE_ATTR_UNSPEC,
 
-	MCPS802154_RANGING_RESULT_ATTR_ID,
-	MCPS802154_RANGING_RESULT_ATTR_TOF_RCTU,
-	MCPS802154_RANGING_RESULT_ATTR_LOCAL_PDOA_RAD_Q11,
-	MCPS802154_RANGING_RESULT_ATTR_REMOTE_PDOA_RAD_Q11,
-	MCPS802154_RANGING_RESULT_ATTR_LOCAL_PDOA_ELEVATION_RAD_Q11,
-	MCPS802154_RANGING_RESULT_ATTR_REMOTE_PDOA_ELEVATION_RAD_Q11,
+	MCPS802154_PWR_STATS_STATE_ATTR_TIME,
+	MCPS802154_PWR_STATS_STATE_ATTR_COUNT,
 
-	__MCPS802154_RANGING_RESULT_ATTR_AFTER_LAST,
-	MCPS802154_RANGING_RESULT_ATTR_MAX =
-		__MCPS802154_RANGING_RESULT_ATTR_AFTER_LAST - 1
+	__MCPS802154_PWR_STATS_STATE_ATTR_AFTER_LAST,
+	MCPS802154_PWR_STATS_STATE_ATTR_MAX =
+		__MCPS802154_PWR_STATS_STATE_ATTR_AFTER_LAST - 1
 };
 
 /**
- * enum mcps802154_ping_pong_result_attrs - Ping pong result.
- *
- * @MCPS802154_PING_PONG_RESULT_ATTR_ID:
- *	Identifier of request.
- * @MCPS802154_PING_PONG_RESULT_ATTR_T_0:
- *	t_0 of ping pong
- * @MCPS802154_PING_PONG_RESULT_ATTR_T_3:
- *	t_3 of ping pong.
- * @MCPS802154_PING_PONG_RESULT_ATTR_T_4:
- *	t_4 of ping pong.
- *
- * @MCPS802154_PING_PONG_RESULT_ATTR_UNSPEC: Invalid command.
- * @__MCPS802154_PING_PONG_RESULT_ATTR_AFTER_LAST: Internal use.
- * @MCPS802154_PING_PONG_RESULT_ATTR_MAX: Internal use.
+ * enum mcps802154_pwr_stats_attrs - Power statistics item.
+ *
+ * @MCPS802154_PWR_STATS_ATTR_SLEEP:
+ * 	Sleep state nested attribute.
+ * @MCPS802154_PWR_STATS_ATTR_IDLE:
+ * 	Idle state nested attribute.
+ * @MCPS802154_PWR_STATS_ATTR_RX:
+ * 	Rx state nested attribute.
+ * @MCPS802154_PWR_STATS_ATTR_TX:
+ * 	Tx state nested attribute.
+ * @MCPS802154_PWR_STATS_ATTR_INTERRUPTS:
+ * 	Interrupts count attribute.
+ * @MCPS802154_PWR_STATS_ATTR_UNSPEC: Invalid command.
+ * @__MCPS802154_PWR_STATS_ATTR_AFTER_LAST: Internal use.
+ * @MCPS802154_PWR_STATS_ATTR_MAX: Internal use.
  */
-enum mcps802154_ping_pong_result_attrs {
-	MCPS802154_PING_PONG_RESULT_ATTR_UNSPEC,
-
-	MCPS802154_PING_PONG_RESULT_ATTR_ID,
-	MCPS802154_PING_PONG_RESULT_ATTR_T_0,
-	MCPS802154_PING_PONG_RESULT_ATTR_T_3,
-	MCPS802154_PING_PONG_RESULT_ATTR_T_4,
-
-	__MCPS802154_PING_PONG_RESULT_ATTR_AFTER_LAST,
-	MCPS802154_PING_PONG_RESULT_ATTR_MAX =
-		__MCPS802154_PING_PONG_RESULT_ATTR_AFTER_LAST - 1
+enum mcps802154_pwr_stats_attrs {
+	MCPS802154_PWR_STATS_ATTR_UNSPEC,
+
+	MCPS802154_PWR_STATS_ATTR_SLEEP,
+	MCPS802154_PWR_STATS_ATTR_IDLE,
+	MCPS802154_PWR_STATS_ATTR_RX,
+	MCPS802154_PWR_STATS_ATTR_TX,
+	MCPS802154_PWR_STATS_ATTR_INTERRUPTS,
+
+	__MCPS802154_PWR_STATS_ATTR_AFTER_LAST,
+	MCPS802154_PWR_STATS_ATTR_MAX =
+		__MCPS802154_PWR_STATS_ATTR_AFTER_LAST - 1
 };
 
 #endif /* NET_MCPS802154_NL_H */
diff --git a/mac/include/net/mcps802154_schedule.h b/mac/include/net/mcps802154_schedule.h
index 5c0416b..7355742 100644
--- a/mac/include/net/mcps802154_schedule.h
+++ b/mac/include/net/mcps802154_schedule.h
@@ -44,6 +44,9 @@ struct mcps802154_nl_ranging_request;
  * @MCPS802154_ACCESS_METHOD_NOTHING:
  *      Nothing to do, wait for end of region, or a schedule change. Internal,
  *      region handlers must return a NULL access if no access is possible.
+ * @MCPS802154_ACCESS_METHOD_IDLE:
+ * 	Nothing to do, wait for end of region, or a schedule change.
+ * 	Trust the access duration to not get the current time.
  * @MCPS802154_ACCESS_METHOD_IMMEDIATE_RX:
  *	RX as soon as possible, without timeout, with auto-ack.
  * @MCPS802154_ACCESS_METHOD_IMMEDIATE_TX:
@@ -55,6 +58,7 @@ struct mcps802154_nl_ranging_request;
  */
 enum mcps802154_access_method {
 	MCPS802154_ACCESS_METHOD_NOTHING,
+	MCPS802154_ACCESS_METHOD_IDLE,
 	MCPS802154_ACCESS_METHOD_IMMEDIATE_RX,
 	MCPS802154_ACCESS_METHOD_IMMEDIATE_TX,
 	MCPS802154_ACCESS_METHOD_MULTI,
@@ -89,18 +93,19 @@ struct mcps802154_access_frame {
 	bool is_tx;
 	union {
 		/**
-		 * @tx_frame_info: Information for transmitting a frame. Should
+		 * @tx_frame_config: Information for transmitting a frame. Should
 		 * have rx_enable_after_tx_dtu == 0.
 		 */
-		struct mcps802154_tx_frame_info tx_frame_info;
+		struct mcps802154_tx_frame_config tx_frame_config;
 		/**
 		 * @rx: Information for receiving a frame.
 		 */
 		struct {
 			/**
-			 * @rx.info: Information for enabling the receiver.
+			 * @rx.frame_config: Information for enabling the
+			 * receiver.
 			 */
-			struct mcps802154_rx_info info;
+			struct mcps802154_rx_frame_config frame_config;
 			/**
 			 * @rx.frame_info_flags_request: Information to request
 			 * when a frame is received, see
@@ -208,6 +213,15 @@ struct mcps802154_access {
 	 * ieee802154 interface.
 	 */
 	const struct mcps802154_channel *channel;
+	/**
+	 * @hrp_uwb_params: If not NULL, parameters for a HRP UWB Phy set at the
+	 * start of a multiple frames access.
+	 */
+	const struct mcps802154_hrp_uwb_params *hrp_uwb_params;
+	/**
+	 * @error: contain the error from the llhw in order to propagate it to upper regions.
+	 */
+	int error;
 };
 
 /**
@@ -252,6 +266,10 @@ struct mcps802154_access_ops {
 	/**
 	 * @tx_get_frame: Return a frame to send, the buffer is lend to caller
 	 * and should be returned with &mcps802154_access_ops.tx_return().
+	 *
+	 * The return value can be NULL for frames without data. In this case,
+	 * &mcps802154_access_ops.tx_return() will be called anyway, with a NULL
+	 * pointer.
 	 */
 	struct sk_buff *(*tx_get_frame)(struct mcps802154_access *access,
 					int frame_idx);
@@ -409,6 +427,11 @@ struct mcps802154_region_ops {
 	 * Return 1 if the region accepted to transmit the buffer, 0 otherwise.
 	 */
 	int (*xmit_skb)(struct mcps802154_region *region, struct sk_buff *skb);
+	/**
+	 * @deferred: Called at the end of event processing on request. See
+	 * mcps802154_region_deferred.
+	 */
+	void (*deferred)(struct mcps802154_region *region);
 };
 
 /**
@@ -511,6 +534,15 @@ struct mcps802154_scheduler_ops {
 		struct mcps802154_scheduler *scheduler,
 		const struct mcps802154_nl_ranging_request *requests,
 		unsigned int n_requests);
+	/**
+	 * @get_next_demands: Called to get an aggregated demand for the specified
+	 * region.
+	 */
+	int (*get_next_demands)(struct mcps802154_scheduler *scheduler,
+				const struct mcps802154_region *region,
+				u32 timestamp_dtu, int duration_dtu,
+				int delta_dtu,
+				struct mcps802154_region_demand *demands);
 };
 
 /**
@@ -693,6 +725,22 @@ void mcps802154_region_rx_skb(struct mcps802154_llhw *llhw,
 			      struct sk_buff *skb, u8 lqi);
 
 /**
+ * mcps802154_region_deferred() - Request to call the deferred callback at the
+ * end of event processing.
+ * @llhw: Low-level device pointer.
+ * @region: Pointer to the open region.
+ *
+ * Event is coming from the low-level device. The region must be the one which
+ * triggered the event (region must not call this after a get_access). If this
+ * is not respected, this call can return -EINVAL in case two regions request
+ * the deferred callback at the same time.
+ *
+ * Return: 0 or -EINVAL.
+ */
+int mcps802154_region_deferred(struct mcps802154_llhw *llhw,
+			       struct mcps802154_region *region);
+
+/**
  * mcps802154_scheduler_register() - Register a scheduler, to be called when
  * your module is loaded.
  * @scheduler_ops: Scheduler to register.
@@ -743,12 +791,14 @@ int mcps802154_schedule_recycle(
  * @region: Region to add.
  * @start_dtu: Region start from the start of the schedule.
  * @duration_dtu: Region duration, or 0 for endless region.
+ * @once: Schedule the region once, ignoring the remaining region duration.
  *
  * Return: 0 or error.
  */
 int mcps802154_schedule_add_region(
 	const struct mcps802154_schedule_update *schedule_update,
-	struct mcps802154_region *region, int start_dtu, int duration_dtu);
+	struct mcps802154_region *region, int start_dtu, int duration_dtu,
+	bool once);
 
 /**
  * mcps802154_reschedule() - Request to change access as possible.
@@ -789,4 +839,21 @@ void mcps802154_schedule_invalidate(struct mcps802154_llhw *llhw);
 int mcps802154_schedule_get_regions(struct mcps802154_llhw *llhw,
 				    struct list_head **regions);
 
+/**
+ * mcps802154_schedule_get_next_demands() - Get an aggregated demand for the
+ * specified region.
+ * @llhw: Low-level device pointer.
+ * @region: Region.
+ * @timestamp_dtu: Timestamp from which demands must be computed.
+ * @duration_dtu: Duration for which demands are considered.
+ * @delta_dtu: Maximum gap between two demands.
+ * @demands: Aggregated demand.
+ *
+ * Return: 1 if demand is returned, 0 if no demand or error.
+ */
+int mcps802154_schedule_get_next_demands(
+	struct mcps802154_llhw *llhw, const struct mcps802154_region *region,
+	u32 timestamp_dtu, int duration_dtu, int delta_dtu,
+	struct mcps802154_region_demand *demands);
+
 #endif /* NET_MCPS802154_SCHEDULE_H */
diff --git a/kernel/net/mcps802154/fira_aead_impl.h b/mac/include/net/mcps_skb_frag.h
index 8a564de..78418f0 100644
--- a/kernel/net/mcps802154/fira_aead_impl.h
+++ b/mac/include/net/mcps_skb_frag.h
@@ -1,7 +1,7 @@
 /*
  * This file is part of the UWB stack for linux.
  *
- * Copyright (c) 2020-2021 Qorvo US, Inc.
+ * 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.
@@ -21,21 +21,14 @@
  * 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>
+#include <linux/skbuff.h>
 
 /**
- * struct fira_aead - Context for payload encryption/decryption.
+ * mcps_skb_frags_len() - Return the total length of the fragments attached to this buffer.
+ * @skb: Pointer to buffer.
+ *
+ * Return: Attached fragments length.
+ *
+ * NOTE: The parent length is NOT included in the computed value
  */
-struct fira_aead {
-	/**
-	 * @tfm: Transformation context for payload.
-	 */
-	struct crypto_aead *tfm;
-};
-
-#endif /* FIRA_AEAD_IMPL_H */
+int mcps_skb_frags_len(struct sk_buff *skb);
diff --git a/mac/include/net/nfcc_coex_region_nl.h b/mac/include/net/nfcc_coex_region_nl.h
index 2082e48..169691a 100644
--- a/mac/include/net/nfcc_coex_region_nl.h
+++ b/mac/include/net/nfcc_coex_region_nl.h
@@ -73,7 +73,9 @@ enum nfcc_coex_call_attrs {
  * @NFCC_COEX_CCC_SESSION_PARAM_ATTR_TIME0_NS:
  *	Initiation time in unit of ns, default 0.
  * @NFCC_COEX_CCC_SESSION_PARAM_ATTR_CHANNEL_NUMBER:
- *	Override channel for this session: 5, 6, 8, 9, 10, 12, 13 or 14
+ *	Override channel for this session: 5, 6, 8, 9, 10, 12, 13 or 14.
+  * @NFCC_COEX_CCC_SESSION_PARAM_ATTR_VERSION:
+ *	Protocol version to be used.
  *
  * @NFCC_COEX_CCC_SESSION_PARAM_ATTR_UNSPEC: Invalid command.
  * @__NFCC_COEX_CCC_SESSION_PARAM_ATTR_AFTER_LAST: Internal use.
@@ -84,6 +86,7 @@ enum nfcc_coex_ccc_session_param_attrs {
 
 	NFCC_COEX_CCC_SESSION_PARAM_ATTR_TIME0_NS,
 	NFCC_COEX_CCC_SESSION_PARAM_ATTR_CHANNEL_NUMBER,
+	NFCC_COEX_CCC_SESSION_PARAM_ATTR_VERSION,
 
 	__NFCC_COEX_CCC_SESSION_PARAM_ATTR_AFTER_LAST,
 	NFCC_COEX_CCC_SESSION_PARAM_ATTR_MAX =
diff --git a/mac/include/net/pctt_region_nl.h b/mac/include/net/pctt_region_nl.h
index 1402252..f9a674a 100644
--- a/mac/include/net/pctt_region_nl.h
+++ b/mac/include/net/pctt_region_nl.h
@@ -26,12 +26,7 @@
 
 /**
  * enum pctt_call - PCTT calls identifiers.
- * FIXME: Must be rework, 3 netlink requests to set parameters is too complex.
  *
- * @PCTT_CALL_SET_PARAMS:
- *	First set parameters.
- *	TODO: Move all in "start test" parameters (like NFCC_COEX) or,
- *	      SET_PARAMS call_id (like FiRa).
  * @PCTT_CALL_SESSION_INIT:
  *	Initialize PCTT session.
  * @PCTT_CALL_SESSION_CMD:
@@ -50,7 +45,6 @@
  * @PCTT_CALL_MAX: Internal use.
  */
 enum pctt_call {
-	PCTT_CALL_SET_PARAMS,
 	PCTT_CALL_SESSION_INIT,
 	PCTT_CALL_SESSION_CMD,
 	PCTT_CALL_SESSION_DEINIT,
@@ -63,9 +57,7 @@ enum pctt_call {
 
 enum pctt_call_attrs {
 	PCTT_CALL_ATTR_UNSPEC,
-	PCTT_CALL_ATTR_PARAMS,
 	PCTT_CALL_ATTR_CMD_ID,
-	PCTT_CALL_ATTR_CMD_PARAMS,
 	PCTT_CALL_ATTR_RESULT_DATA,
 	PCTT_CALL_ATTR_SESSION_ID,
 	PCTT_CALL_ATTR_SESSION_STATE,
@@ -108,13 +100,41 @@ enum pctt_call_attrs {
  * @PCTT_SESSION_PARAM_ATTR_PSDU_DATA_RATE:
  *	6.81 Mbps (0, default), 7.80 Mbps (1, not supported),
  *	27.2 Mbps (2, not supported), 31.2 Mbps (3, not supported)
+ * @PCTT_SESSION_PARAM_ATTR_BPRF_PHR_DATA_RATE:
+ *	850 kbps (0, default) or 6.81 Mbps (1)
  * @PCTT_SESSION_PARAM_ATTR_MAC_FCS_TYPE:
  *	CRC16 (0, default) or CRC32 (1, not supported)
  * @PCTT_SESSION_PARAM_ATTR_TX_ADAPTIVE_PAYLOAD_POWER:
  *	Disable adaptive payload power for TX (0, default) or enable (1)
  * @PCTT_SESSION_PARAM_ATTR_STS_INDEX:
  *	STS index initialization value
- *
+ * @PCTT_SESSION_PARAM_ATTR_STS_LENGTH:
+ *  	Number of symbols in a STS segment. 32 (0x00), 64 (0x01, default) or 128
+ *  	symbols (0x02)
+ * @PCTT_SESSION_PARAM_ATTR_NUM_PACKETS:
+ * 	Number of packets (default 1000).
+ * @PCTT_SESSION_PARAM_ATTR_T_GAP:
+ *  	Gap between start of one packet to the next in µs (default 2000).
+ * @PCTT_SESSION_PARAM_ATTR_T_START:
+ *  	Max. time from the start of T_GAP to SFD found state in µs (default
+ *	450us).
+ * @PCTT_SESSION_PARAM_ATTR_T_WIN:
+ *  	Max. time for which RX is looking for a packet from the start of T_GAP
+ * 	in µs (default 750).
+ * @PCTT_SESSION_PARAM_ATTR_RANDOMIZE_PSDU:
+ *  	Disable (0, default) or enable (1) PSDU randomization.
+ * @PCTT_SESSION_PARAM_ATTR_PHR_RANGING_BIT:
+ *  	Disable (0, default) or enable (1) ranging bit field of PHR in both BPRF
+ * 	and HPRF.
+ * @PCTT_SESSION_PARAM_ATTR_RMARKER_TX_START:
+ *  Start time of TX in 1/(128*499.2MHz) units.
+ * @PCTT_SESSION_PARAM_ATTR_RMARKER_RX_START:
+ *  Start time of RX in 1/(128*499.2MHz) units.
+ * @PCTT_SESSION_PARAM_ATTR_STS_INDEX_AUTO_INCR:
+ *  	Disable (0, default) or enable (1) incrementation of STS_INDEX config
+ * 	value for every frame in PER Rx/Periodic TX test.
+ * @PCTT_SESSION_PARAM_ATTR_DATA_PAYLOAD:
+ *	PSDU Data.
  * @PCTT_SESSION_PARAM_ATTR_UNSPEC: Invalid command.
  * @__PCTT_SESSION_PARAM_ATTR_AFTER_LAST: Internal use.
  * @PCTT_SESSION_PARAM_ATTR_MAX: Internal use.
@@ -138,32 +158,29 @@ enum pctt_session_param_attrs {
 	PCTT_SESSION_PARAM_ATTR_SFD_ID,
 	PCTT_SESSION_PARAM_ATTR_NUMBER_OF_STS_SEGMENTS,
 	PCTT_SESSION_PARAM_ATTR_PSDU_DATA_RATE,
+	PCTT_SESSION_PARAM_ATTR_BPRF_PHR_DATA_RATE,
 	PCTT_SESSION_PARAM_ATTR_MAC_FCS_TYPE,
 	PCTT_SESSION_PARAM_ATTR_TX_ADAPTIVE_PAYLOAD_POWER,
 	/* STS and crypto */
 	PCTT_SESSION_PARAM_ATTR_STS_INDEX,
+	PCTT_SESSION_PARAM_ATTR_STS_LENGTH,
+	/* Test configuration parameters */
+	PCTT_SESSION_PARAM_ATTR_NUM_PACKETS,
+	PCTT_SESSION_PARAM_ATTR_T_GAP,
+	PCTT_SESSION_PARAM_ATTR_T_START,
+	PCTT_SESSION_PARAM_ATTR_T_WIN,
+	PCTT_SESSION_PARAM_ATTR_RANDOMIZE_PSDU,
+	PCTT_SESSION_PARAM_ATTR_PHR_RANGING_BIT,
+	PCTT_SESSION_PARAM_ATTR_RMARKER_TX_START,
+	PCTT_SESSION_PARAM_ATTR_RMARKER_RX_START,
+	PCTT_SESSION_PARAM_ATTR_STS_INDEX_AUTO_INCR,
+	/* Payload */
+	PCTT_SESSION_PARAM_ATTR_DATA_PAYLOAD,
 
 	__PCTT_SESSION_PARAM_ATTR_AFTER_LAST,
 	PCTT_SESSION_PARAM_ATTR_MAX = __PCTT_SESSION_PARAM_ATTR_AFTER_LAST - 1
 };
 
-enum pctt_param_attrs {
-	PCTT_PARAM_ATTR_UNSPEC,
-
-	PCTT_PARAM_ATTR_NUM_PACKETS,
-	PCTT_PARAM_ATTR_T_GAP,
-	PCTT_PARAM_ATTR_T_START,
-	PCTT_PARAM_ATTR_T_WIN,
-	PCTT_PARAM_ATTR_RANDOMIZE_PSDU,
-	PCTT_PARAM_ATTR_PHR_RANGING_BIT,
-	PCTT_PARAM_ATTR_RMARKER_TX_START,
-	PCTT_PARAM_ATTR_RMARKER_RX_START,
-	PCTT_PARAM_ATTR_STS_INDEX_AUTO_INCR,
-
-	__PCTT_PARAM_ATTR_AFTER_LAST,
-	PCTT_PARAM_ATTR_MAX = __PCTT_PARAM_ATTR_AFTER_LAST - 1
-};
-
 enum pctt_id_attrs {
 	PCTT_ID_ATTR_UNSPEC,
 
@@ -178,15 +195,6 @@ enum pctt_id_attrs {
 	PCTT_ID_ATTR_MAX = __PCTT_ID_ATTR_AFTER_LAST - 1
 };
 
-enum pctt_test_param_attrs {
-	PCTT_TEST_PARAM_ATTR_UNSPEC,
-
-	PCTT_TEST_PARAM_ATTR_PAYLOAD,
-
-	__PCTT_TEST_PARAM_ATTR_AFTER_LAST,
-	PCTT_TEST_PARAM_ATTR_MAX = __PCTT_TEST_PARAM_ATTR_AFTER_LAST - 1,
-};
-
 enum pctt_result_data_attrs {
 	PCTT_RESULT_DATA_ATTR_UNSPEC,
 
@@ -213,7 +221,6 @@ enum pctt_result_data_attrs {
 	PCTT_RESULT_DATA_ATTR_PHR,
 	PCTT_RESULT_DATA_ATTR_PSDU_DATA_LEN,
 	PCTT_RESULT_DATA_ATTR_PSDU_DATA,
-
 	PCTT_RESULT_DATA_ATTR_TX_TS_INT,
 	PCTT_RESULT_DATA_ATTR_TX_TS_FRAC,
 	PCTT_RESULT_DATA_ATTR_RX_TS_INT,
@@ -221,8 +228,14 @@ enum pctt_result_data_attrs {
 
 	PCTT_RESULT_DATA_ATTR_MEASUREMENT,
 
+	PCTT_RESULT_DATA_ATTR_PDOA_AZIMUTH_DEG_Q7,
+	PCTT_RESULT_DATA_ATTR_PDOA_ELEVATION_DEG_Q7,
+	PCTT_RESULT_DATA_ATTR_RSSI,
+	PCTT_RESULT_DATA_ATTR_AOA_AZIMUTH_DEG_Q7,
+	PCTT_RESULT_DATA_ATTR_AOA_ELEVATION_DEG_Q7,
+
 	__PCTT_RESULT_DATA_ATTR_AFTER_LAST,
-	PCTT_RESULT_DATA_ATTR_MAX = __PCTT_RESULT_DATA_ATTR_AFTER_LAST - 1
+	PCTT_RESULT_DATA_ATTR_MAX = __PCTT_RESULT_DATA_ATTR_AFTER_LAST - 1,
 };
 
 #endif /* NET_PCTT_REGION_NL_H */
diff --git a/mac/include/net/pctt_region_params.h b/mac/include/net/pctt_region_params.h
index 4d51060..0a0f745 100644
--- a/mac/include/net/pctt_region_params.h
+++ b/mac/include/net/pctt_region_params.h
@@ -41,11 +41,30 @@
  */
 #define PCTT_DATA_PAYLOAD_SIZE_MAX 84
 
+/**
+ * enum pctt_device_role - **[NOT IMPLEMENTED]**  Role played by a device.
+ * @PCTT_DEVICE_ROLE_RESPONDER: The device acts as a responder.
+ * @PCTT_DEVICE_ROLE_INITIATOR: The device acts as an initiator.
+ *
+ * Current implementation does not support decorrelation between the
+ * device's role and the device's type. The controller is always
+ * the initiator and the controlee is always the responder.
+ *
+ * This enum is not used in the current implementation.
+ */
 enum pctt_device_role {
 	PCTT_DEVICE_ROLE_RESPONDER,
 	PCTT_DEVICE_ROLE_INITIATOR,
 };
 
+/**
+ * enum pctt_rframe_config - Rframe configuration used to transmit/receive
+ * ranging messages.
+ * @PCTT_RFRAME_CONFIG_SP0: Use SP0 mode.
+ * @PCTT_RFRAME_CONFIG_SP1: Use SP1 mode.
+ * @PCTT_RFRAME_CONFIG_SP2: RFU
+ * @PCTT_RFRAME_CONFIG_SP3: Use SP3 mode.
+ */
 enum pctt_rframe_config {
 	PCTT_RFRAME_CONFIG_SP0,
 	PCTT_RFRAME_CONFIG_SP1,
@@ -53,16 +72,42 @@ enum pctt_rframe_config {
 	PCTT_RFRAME_CONFIG_SP3,
 };
 
+/**
+ * enum pctt_prf_mode - Pulse Repetition Frequency mode.
+ * @PCTT_PRF_MODE_BPRF: Base Pulse Repetition Frequency.
+ * @PCTT_PRF_MODE_HPRF: Higher Pulse Repetition Frequency.
+ * @PCTT_PRF_MODE_HPRF_HIGH_RATE: Higher Pulse Repetition Frequency allowing
+ * higher data rates (27M2 and 31M2).
+ *
+ * This enum is not used in the current implementation.
+ */
 enum pctt_prf_mode {
 	PCTT_PRF_MODE_BPRF,
 	PCTT_PRF_MODE_HPRF,
+	PCTT_PRF_MODE_HPRF_HIGH_RATE,
 };
 
+/**
+ * enum pctt_preamble_duration - Duration of preamble in symbols.
+ * @PCTT_PREAMBLE_DURATION_32: 32 symbols duration.
+ * @PCTT_PREAMBLE_DURATION_64: 64 symbols duration.
+ */
 enum pctt_preamble_duration {
 	PCTT_PREAMBLE_DURATION_32,
 	PCTT_PREAMBLE_DURATION_64,
 };
 
+/**
+ * enum pctt_sfd_id - Start-of-frame delimiter.
+ * @PCTT_SFD_ID_0: Delimiter is [0 +1 0 –1 +1 0 0 –1]
+ * @PCTT_SFD_ID_1: Delimiter is [ –1 –1 +1 –1 ]
+ * @PCTT_SFD_ID_2: Delimiter is [ –1 –1 –1 +1 –1 –1 +1 –1 ]
+ * @PCTT_SFD_ID_3: Delimiter is
+ * [ –1 –1 –1 –1 –1 +1 +1 –1 –1 +1 –1 +1 –1 –1 +1 –1 ]
+ * @PCTT_SFD_ID_4: Delimiter is
+ * [ –1 –1 –1 –1 –1 –1 –1 +1 –1 –1 +1 –1 –1 +1 –1 +1 –1 +1
+ * –1 –1 –1 +1 +1 –1 –1 –1 +1 –1 +1 +1 –1 –1 ]
+ */
 enum pctt_sfd_id {
 	PCTT_SFD_ID_0,
 	PCTT_SFD_ID_1,
@@ -71,12 +116,29 @@ enum pctt_sfd_id {
 	PCTT_SFD_ID_4,
 };
 
+/**
+ * enum pctt_number_of_sts_segments - Number of STS segments.
+ * @PCTT_NUMBER_OF_STS_SEGMENTS_NONE: No STS Segment (Rframe config SP0).
+ * @PCTT_NUMBER_OF_STS_SEGMENTS_1_SEGMENT: 1 STS Segment.
+ * @PCTT_NUMBER_OF_STS_SEGMENTS_2_SEGMENTS: 2 STS Segments.
+ * @PCTT_NUMBER_OF_STS_SEGMENTS_3_SEGMENTS: 3 STS Segments.
+ * @PCTT_NUMBER_OF_STS_SEGMENTS_4_SEGMENTS: 4 STS Segments.
+ */
 enum pctt_number_of_sts_segments {
 	PCTT_NUMBER_OF_STS_SEGMENTS_NONE,
 	PCTT_NUMBER_OF_STS_SEGMENTS_1_SEGMENT,
 	PCTT_NUMBER_OF_STS_SEGMENTS_2_SEGMENTS,
+	PCTT_NUMBER_OF_STS_SEGMENTS_3_SEGMENTS,
+	PCTT_NUMBER_OF_STS_SEGMENTS_4_SEGMENTS,
 };
 
+/**
+ * enum pctt_psdu_data_rate - Data rate used to exchange PSDUs.
+ * @PCTT_PSDU_DATA_RATE_6M81: 6.8Mb/s rate.
+ * @PCTT_PSDU_DATA_RATE_7M80: 7.8Mb/s rate.
+ * @PCTT_PSDU_DATA_RATE_27M2: 27.2Mb/s rate.
+ * @PCTT_PSDU_DATA_RATE_31M2: 31.2Mb/s rate.
+ */
 enum pctt_psdu_data_rate {
 	PCTT_PSDU_DATA_RATE_6M81,
 	PCTT_PSDU_DATA_RATE_7M80,
@@ -84,6 +146,18 @@ enum pctt_psdu_data_rate {
 	PCTT_PSDU_DATA_RATE_31M2,
 };
 
+/**
+ * enum pctt_phr_data_rate - Data rate used to exchange PHR.
+ * @PCTT_PHR_DATA_RATE_850K: 850kb/s rate.
+ * @PCTT_PHR_DATA_RATE_6M81: 6.8Mb/s rate.
+ *
+ * This enum is not used in the current implementation.
+ */
+enum pctt_phr_data_rate {
+	PCTT_PHR_DATA_RATE_850K,
+	PCTT_PHR_DATA_RATE_6M81,
+};
+
 enum pctt_mac_fcs_type {
 	PCTT_MAC_FCS_TYPE_CRC_16,
 	PCTT_MAC_FCS_TYPE_CRC_32,
@@ -131,4 +205,16 @@ enum pctt_session_state {
 	PCTT_SESSION_STATE_IDLE,
 };
 
+/**
+ * enum pctt_sts_length - Number of symbols in a STS segment.
+ * @PCTT_STS_LENGTH_32: The STS length is 32 symbols.
+ * @PCTT_STS_LENGTH_64: The STS length is 64 symbols.
+ * @PCTT_STS_LENGTH_128: The STS length is 128 symbols.
+ */
+enum pctt_sts_length {
+	PCTT_STS_LENGTH_32 = 0,
+	PCTT_STS_LENGTH_64 = 1,
+	PCTT_STS_LENGTH_128 = 2,
+};
+
 #endif /* NET_PCTT_REGION_PARAMS_H */
diff --git a/mac/include/net/simple_ranging_region_nl.h b/mac/include/net/simple_ranging_region_nl.h
deleted file mode 100644
index 7dc62ee..0000000
--- a/mac/include/net/simple_ranging_region_nl.h
+++ /dev/null
@@ -1,59 +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 SIMPLE_RANGING_REGION_NL_H
-#define SIMPLE_RANGING_REGION_NL_H
-
-/**
- * enum simple_ranging_region_set_parameters_attrs - Simple ranging params.
- *
- * @SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_SLOT_DURATION_MS:
- *	Slot duration in milliseconds.
- * @SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_NODE_TYPE:
- *	The node type, either 0 for initiator, or 1 for responder.
- * @SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_TX_ANTENNA:
- *	The antenna index for transmit.
- * @SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_RX_ANTENNA_PAIR_AZIMUTH:
- *	The antenna pair index for receive with azimuth AoA.
- * @SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_RX_ANTENNA_PAIR_ELEVATION:
- *	The antenna pair index for receive with elevation AoA.
- *
- * @SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_UNSPEC: Invalid command.
- * @__SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_AFTER_LAST: Internal use.
- * @SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_MAX: Internal use.
- */
-enum simple_ranging_region_set_parameters_attrs {
-	SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_UNSPEC,
-
-	SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_SLOT_DURATION_MS,
-	SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_NODE_TYPE,
-	SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_TX_ANTENNA,
-	SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_RX_ANTENNA_PAIR_AZIMUTH,
-	SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_RX_ANTENNA_PAIR_ELEVATION,
-
-	__SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_AFTER_LAST,
-	SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_MAX =
-		__SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_AFTER_LAST - 1
-};
-
-#endif /* SIMPLE_RANGING_REGION_NL_H */
diff --git a/mac/include/net/vendor_cmd.h b/mac/include/net/vendor_cmd.h
index bb64452..38a6224 100644
--- a/mac/include/net/vendor_cmd.h
+++ b/mac/include/net/vendor_cmd.h
@@ -25,30 +25,40 @@
 #define NET_VENDOR_CMD_H
 
 #include <linux/types.h>
+#include <net/mcps802154.h>
 
 /**
- * enum dw3000_vendor_cmd - Vendor command identifiers.
- * @DW3000_VENDOR_CMD_NFCC_COEX_HANDLE_ACCESS:
- *     NFCC Coex: handle access.
- * @DW3000_VENDOR_CMD_NFCC_COEX_GET_ACCESS_INFORMATION:
- *     NFCC Coex: get access information.
- * @DW3000_VENDOR_CMD_NFCC_COEX_STOP:
- *     NFCC Coex: stop.
- * @DW3000_VENDOR_CMD_PCTT_SETUP_HW:
+ * enum llhw_vendor_cmd - Vendor command identifiers.
+ * @LLHW_VENDOR_CMD_NFCC_COEX_HANDLE_ACCESS:
+ *     NFCC Coex: Handle access.
+ * @LLHW_VENDOR_CMD_NFCC_COEX_GET_ACCESS_INFORMATION:
+ *     NFCC Coex: Get access information.
+ * @LLHW_VENDOR_CMD_NFCC_COEX_STOP:
+ *     NFCC Coex: Stop.
+ * @LLHW_VENDOR_CMD_PCTT_SETUP_HW:
  *     PCTT: Setup hardware access.
+ * @LLHW_VENDOR_CMD_PCTT_HANDLE_LOOPBACK:
+ *     PCTT: Handle loop-back test.
+ * @LLHW_VENDOR_CMD_PCTT_GET_LOOPBACK_INFO:
+ *     PCTT: Get loop-back information.
+ * @LLHW_VENDOR_CMD_PCTT_GET_FRAME_INFO:
+ *     PCTT: Get the last received frame information.
  */
-enum dw3000_vendor_cmd {
-	DW3000_VENDOR_CMD_NFCC_COEX_HANDLE_ACCESS,
-	DW3000_VENDOR_CMD_NFCC_COEX_GET_ACCESS_INFORMATION,
-	DW3000_VENDOR_CMD_NFCC_COEX_STOP,
-	DW3000_VENDOR_CMD_PCTT_SETUP_HW,
+enum llhw_vendor_cmd {
+	LLHW_VENDOR_CMD_NFCC_COEX_HANDLE_ACCESS,
+	LLHW_VENDOR_CMD_NFCC_COEX_GET_ACCESS_INFORMATION,
+	LLHW_VENDOR_CMD_NFCC_COEX_STOP,
+	LLHW_VENDOR_CMD_PCTT_SETUP_HW,
+	LLHW_VENDOR_CMD_PCTT_HANDLE_LOOPBACK,
+	LLHW_VENDOR_CMD_PCTT_GET_LOOPBACK_INFO,
+	LLHW_VENDOR_CMD_PCTT_GET_FRAME_INFO,
 };
 
 /**
- * struct dw3000_vendor_cmd_nfcc_coex_handle_access - NFCC Coex: handle access
+ * struct llhw_vendor_cmd_nfcc_coex_handle_access - NFCC Coex: handle access
  * vendor command.
  */
-struct dw3000_vendor_cmd_nfcc_coex_handle_access {
+struct llhw_vendor_cmd_nfcc_coex_handle_access {
 	/**
 	 * @start: True to start a new session.
 	 */
@@ -69,13 +79,17 @@ struct dw3000_vendor_cmd_nfcc_coex_handle_access {
 	 * @chan: Channel number, 5 or 9.
 	 */
 	int chan;
+	/**
+	 * @version: Protocol version.
+	 */
+	int version;
 };
 
 /**
- * struct dw3000_vendor_cmd_nfcc_coex_get_access_info - NFCC Coex: get access
+ * struct llhw_vendor_cmd_nfcc_coex_get_access_info - NFCC Coex: get access
  * info vendor command.
  */
-struct dw3000_vendor_cmd_nfcc_coex_get_access_info {
+struct llhw_vendor_cmd_nfcc_coex_get_access_info {
 	/**
 	 * @stop: If true, the NFCC did not give a next access.
 	 */
@@ -102,10 +116,31 @@ struct dw3000_vendor_cmd_nfcc_coex_get_access_info {
 };
 
 /**
- * struct dw3000_vendor_cmd_pctt_setup_hw - PCTT: direct HW access
+ * struct llhw_vendor_cmd_nfcc_coex_stop - NFCC Coex: stop
  * vendor command.
  */
-struct dw3000_vendor_cmd_pctt_setup_hw {
+struct llhw_vendor_cmd_nfcc_coex_stop {
+	/**
+	 * @timestamp_dtu:
+	 *     Access date when the stop must be sent.
+	 */
+	u32 timestamp_dtu;
+	/**
+	 * @duration_dtu:
+	 *     Duration of the access.
+	 */
+	int duration_dtu;
+	/**
+	 * @version: Protocol version.
+	 */
+	int version;
+};
+
+/**
+ * struct llhw_vendor_cmd_pctt_setup_hw - PCTT: direct HW access
+ * vendor command.
+ */
+struct llhw_vendor_cmd_pctt_setup_hw {
 	/**
 	 * @chan: Channel number, 5 or 9.
 	 */
@@ -133,4 +168,76 @@ struct dw3000_vendor_cmd_pctt_setup_hw {
 	u8 preamble_duration;
 };
 
+/**
+ * struct llhw_vendor_cmd_pctt_handle_loopback - PCTT: handle loopback access.
+ */
+struct llhw_vendor_cmd_pctt_handle_loopback {
+	/**
+	 * @ant_set_id : antenna set index to use for transmit/receive.
+	 */
+	int ant_set_id;
+	/**
+	 * @rx_timeout_dtu: If negative, no timeout, if zero, use a default timeout
+	 * value, else this is the timeout value in device time unit.
+	 */
+	int rx_timeout_dtu;
+	/**
+	 * @rx_frame_timeout_dtu: If no zero, timeout value for the full frame
+	 * reception. This allow limiting the length of accepted frame. The
+	 * timeout starts after rx_timeout_dtu value.
+	 */
+	int rx_frame_timeout_dtu;
+	/**
+	 * @data_payload: Array of payload to send during loopback test.
+	 */
+	const u8 *data_payload;
+	/**
+	 * @data_payload_len: Length of the payload array in byte.
+	 */
+	size_t data_payload_len;
+};
+
+/**
+ * struct llhw_vendor_cmd_pctt_get_loopback_info - PCTT: get access
+ * info vendor command.
+ */
+struct llhw_vendor_cmd_pctt_get_loopback_info {
+	/**
+	 * @skb: sk buffer containing received data.
+	 */
+	struct sk_buff *skb;
+	/**
+	 * @success: True when data sent match with received.
+	 */
+	bool success;
+	/**
+	 * @rssi: Received signal strength indication (RSSI),
+	 * absolute value in Q1 fixed point format.
+	 */
+	int rssi;
+	/**
+	 * @rx_timestamp_rctu: RX timestamp in RCTU units.
+	 */
+	u64 rx_timestamp_rctu;
+	/**
+	 * @tx_timestamp_rctu: TX timestamp in RCTU units.
+	 */
+	u64 tx_timestamp_rctu;
+};
+
+/**
+ * struct llhw_vendor_cmd_pctt_get_frame_info - PCTT: last received frame
+ * information.
+ */
+struct llhw_vendor_cmd_pctt_get_frame_info {
+	/**
+	 * @skb: sk buffer containing received data.
+	 */
+	struct sk_buff *skb;
+	/**
+	 * @info: frame information.
+	 */
+	struct mcps802154_rx_frame_info info;
+};
+
 #endif /* NET_VENDOR_CMD_H */
diff --git a/mac/llhw-ops.h b/mac/llhw-ops.h
index 69aa986..1dfcf54 100644
--- a/mac/llhw-ops.h
+++ b/mac/llhw-ops.h
@@ -48,20 +48,20 @@ static inline void llhw_stop(struct mcps802154_local *local)
 
 static inline int llhw_tx_frame(struct mcps802154_local *local,
 				struct sk_buff *skb,
-				const struct mcps802154_tx_frame_info *info,
+				const struct mcps802154_tx_frame_config *config,
 				int frame_idx, int next_delay_dtu)
 {
 	int r;
 
-	trace_llhw_tx_frame(local, info, frame_idx, next_delay_dtu);
-	r = local->ops->tx_frame(&local->llhw, skb, info, frame_idx,
+	trace_llhw_tx_frame(local, config, frame_idx, next_delay_dtu);
+	r = local->ops->tx_frame(&local->llhw, skb, config, frame_idx,
 				 next_delay_dtu);
 	trace_llhw_return_int(local, r);
 	return r;
 }
 
 static inline int llhw_rx_enable(struct mcps802154_local *local,
-				 const struct mcps802154_rx_info *info,
+				 const struct mcps802154_rx_frame_config *info,
 				 int frame_idx, int next_delay_dtu)
 {
 	int r;
@@ -141,12 +141,14 @@ static inline int llhw_get_current_timestamp_dtu(struct mcps802154_local *local,
 	return r;
 }
 
-static inline u64
-llhw_tx_timestamp_dtu_to_rmarker_rctu(struct mcps802154_local *local,
-				      u32 tx_timestamp_dtu, int ant_set_id)
+static inline u64 llhw_tx_timestamp_dtu_to_rmarker_rctu(
+	struct mcps802154_local *local, u32 tx_timestamp_dtu,
+	const struct mcps802154_hrp_uwb_params *hrp_uwb_params,
+	const struct mcps802154_channel *channel_params, int ant_set_id)
 {
 	return local->ops->tx_timestamp_dtu_to_rmarker_rctu(
-		&local->llhw, tx_timestamp_dtu, ant_set_id);
+		&local->llhw, tx_timestamp_dtu, hrp_uwb_params, channel_params,
+		ant_set_id);
 }
 
 static inline s64 llhw_difference_timestamp_rctu(struct mcps802154_local *local,
@@ -176,16 +178,14 @@ static inline int llhw_set_channel(struct mcps802154_local *local, u8 page,
 	return r;
 }
 
-static inline int llhw_set_hrp_uwb_params(struct mcps802154_local *local,
-					  int prf, int psr, int sfd_selector,
-					  int phr_rate, int data_rate)
+static inline int __nocfi
+llhw_set_hrp_uwb_params(struct mcps802154_local *local,
+			const struct mcps802154_hrp_uwb_params *params)
 {
 	int r;
 
-	trace_llhw_set_hrp_uwb_params(local, prf, psr, sfd_selector, phr_rate,
-				      data_rate);
-	r = local->ops->set_hrp_uwb_params(&local->llhw, prf, psr, sfd_selector,
-					   phr_rate, data_rate);
+	trace_llhw_set_hrp_uwb_params(local, params);
+	r = local->ops->set_hrp_uwb_params(&local->llhw, params);
 	trace_llhw_return_int(local, r);
 	return r;
 }
@@ -289,7 +289,11 @@ llhw_list_calibration(struct mcps802154_local *local)
 	const char *const *r;
 
 	trace_llhw_list_calibration(local);
-	r = local->ops->list_calibration(&local->llhw);
+	if (local->ops->list_calibration) {
+		r = local->ops->list_calibration(&local->llhw);
+	} else {
+		r = NULL;
+	}
 	trace_llhw_return_void(local);
 	return r;
 }
@@ -309,6 +313,36 @@ static inline int llhw_vendor_cmd(struct mcps802154_local *local, u32 vendor_id,
 	return r;
 }
 
+static inline int llhw_check_hrp_uwb_params(
+	struct mcps802154_local *local,
+	const struct mcps802154_hrp_uwb_params *hrp_uwb_params)
+{
+	int r;
+
+	trace_llhw_check_hrp_uwb_params(local, hrp_uwb_params);
+	if (local->ops->check_hrp_uwb_params)
+		r = local->ops->check_hrp_uwb_params(&local->llhw,
+						     hrp_uwb_params);
+	else
+		r = -EOPNOTSUPP;
+	trace_llhw_return_int(local, r);
+	return r;
+}
+
+static inline int
+llhw_rx_get_measurement(struct mcps802154_local *local, void *rx_ctx,
+			struct mcps802154_rx_measurement_info *info)
+{
+	int r;
+	trace_llhw_rx_get_measurement(local, rx_ctx);
+	if (local->ops->rx_get_measurement)
+		r = local->ops->rx_get_measurement(&local->llhw, rx_ctx, info);
+	else
+		r = -EOPNOTSUPP;
+	trace_llhw_return_measurement(local, r, info);
+	return r;
+}
+
 #ifdef CONFIG_MCPS802154_TESTMODE
 static inline int llhw_testmode_cmd(struct mcps802154_local *local, void *data,
 				    int len)
diff --git a/kernel/net/mcps802154/ping_pong_region.h b/mac/mcps802154_fproc.h
index c793aa5..fc6e871 100644
--- a/kernel/net/mcps802154/ping_pong_region.h
+++ b/mac/mcps802154_fproc.h
@@ -21,10 +21,12 @@
  * Qorvo. Please contact Qorvo to inquire about licensing terms.
  */
 
-#ifndef NET_MCPS802154_PING_PONG_REGION_H
-#define NET_MCPS802154_PING_PONG_REGION_H
+#ifndef __MCPS802154_FPROC_H__
+#define __MCPS802154_FPROC_H__
 
-int ping_pong_region_init(void);
-void ping_pong_region_exit(void);
+#include <net/mcps802154_schedule.h>
+
+bool mcps802154_fproc_is_non_recoverable_error(struct mcps802154_access *access);
+
+#endif /* MCPS802154_FPROC_H  */
 
-#endif /* NET_MCPS802154_PING_PONG_REGION_H */
diff --git a/mac/mcps_main.c b/mac/mcps_main.c
index cde51a0..f3c2590 100644
--- a/mac/mcps_main.c
+++ b/mac/mcps_main.c
@@ -32,12 +32,9 @@
 #include "mcps802154_i.h"
 #include "llhw-ops.h"
 #include "default_region.h"
-#include "simple_ranging_region.h"
+#include "idle_region.h"
 #include "endless_scheduler.h"
 #include "on_demand_scheduler.h"
-#ifdef CONFIG_MCPS802154_TESTMODE
-#include "ping_pong_region.h"
-#endif
 #include "nl.h"
 #include "warn_return.h"
 
@@ -207,13 +204,16 @@ int mcps802154_get_current_timestamp_dtu(struct mcps802154_llhw *llhw,
 }
 EXPORT_SYMBOL(mcps802154_get_current_timestamp_dtu);
 
-u64 mcps802154_tx_timestamp_dtu_to_rmarker_rctu(struct mcps802154_llhw *llhw,
-						u32 tx_timestamp_dtu,
-						int ant_set_id)
+u64 mcps802154_tx_timestamp_dtu_to_rmarker_rctu(
+	struct mcps802154_llhw *llhw, u32 tx_timestamp_dtu,
+	const struct mcps802154_hrp_uwb_params *hrp_uwb_params,
+	const struct mcps802154_channel *channel_params, int ant_set_id)
 {
 	struct mcps802154_local *local = llhw_to_local(llhw);
 
 	return llhw_tx_timestamp_dtu_to_rmarker_rctu(local, tx_timestamp_dtu,
+						     hrp_uwb_params,
+						     channel_params,
 						     ant_set_id);
 }
 EXPORT_SYMBOL(mcps802154_tx_timestamp_dtu_to_rmarker_rctu);
@@ -229,6 +229,15 @@ s64 mcps802154_difference_timestamp_rctu(struct mcps802154_llhw *llhw,
 }
 EXPORT_SYMBOL(mcps802154_difference_timestamp_rctu);
 
+int mcps802154_rx_get_measurement(struct mcps802154_llhw *llhw, void *rx_ctx,
+				  struct mcps802154_rx_measurement_info *info)
+{
+	struct mcps802154_local *local = llhw_to_local(llhw);
+
+	return llhw_rx_get_measurement(local, rx_ctx, info);
+}
+EXPORT_SYMBOL(mcps802154_rx_get_measurement);
+
 int mcps802154_compute_frame_duration_dtu(struct mcps802154_llhw *llhw,
 					  int payload_bytes)
 {
@@ -247,6 +256,16 @@ int mcps802154_vendor_cmd(struct mcps802154_llhw *llhw, u32 vendor_id,
 }
 EXPORT_SYMBOL(mcps802154_vendor_cmd);
 
+int mcps802154_check_hrp_uwb_params(
+	struct mcps802154_llhw *llhw,
+	const struct mcps802154_hrp_uwb_params *hrp_uwb_params)
+{
+	struct mcps802154_local *local = llhw_to_local(llhw);
+
+	return llhw_check_hrp_uwb_params(local, hrp_uwb_params);
+}
+EXPORT_SYMBOL(mcps802154_check_hrp_uwb_params);
+
 struct mcps802154_local *mcps802154_get_first_by_idx(int hw_idx)
 {
 	struct mcps802154_local *result = NULL, *local;
@@ -274,30 +293,24 @@ int __init mcps802154_init(void)
 		return r;
 	r = mcps802154_default_region_init();
 	WARN_RETURN(r);
-	r = simple_ranging_region_init();
-	WARN_ON(r);
+	r = mcps802154_idle_region_init();
+	WARN_RETURN(r);
 	r = mcps802154_endless_scheduler_init();
 	WARN_ON(r);
 	r = mcps802154_default_scheduler_init();
 	WARN_ON(r);
 	r = mcps802154_on_demand_scheduler_init();
 	WARN_ON(r);
-#ifdef CONFIG_MCPS802154_TESTMODE
-	r = ping_pong_region_init();
-	WARN_ON(r);
-#endif
+
 	return r;
 }
 
 void __exit mcps802154_exit(void)
 {
-#ifdef CONFIG_MCPS802154_TESTMODE
-	ping_pong_region_exit();
-#endif
 	mcps802154_on_demand_scheduler_exit();
 	mcps802154_default_scheduler_exit();
 	mcps802154_endless_scheduler_exit();
-	simple_ranging_region_exit();
+	mcps802154_idle_region_exit();
 	mcps802154_default_region_exit();
 	mcps802154_nl_exit();
 }
diff --git a/mac/simple_ranging_region.h b/mac/mcps_skb_frag.c
index 676863e..6657612 100644
--- a/mac/simple_ranging_region.h
+++ b/mac/mcps_skb_frag.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the UWB stack for linux.
  *
- * Copyright (c) 2020-2021 Qorvo US, Inc.
+ * 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.
@@ -21,10 +21,13 @@
  * Qorvo. Please contact Qorvo to inquire about licensing terms.
  */
 
-#ifndef NET_MCPS802154_SIMPLE_RANGING_REGION_H
-#define NET_MCPS802154_SIMPLE_RANGING_REGION_H
+#include <linux/skbuff.h>
+#include <linux/module.h>
+#include <linux/errno.h>
 
-int simple_ranging_region_init(void);
-void simple_ranging_region_exit(void);
-
-#endif /* NET_MCPS802154_SIMPLE_RANGING_REGION_H */
+int mcps_skb_frags_len(struct sk_buff *skb)
+{
+	/* No fragmentation on Linux. */
+	return 0;
+}
+EXPORT_SYMBOL(mcps_skb_frags_len);
diff --git a/mac/nfcc_coex_access.c b/mac/nfcc_coex_access.c
index 873cc01..60d6bae 100644
--- a/mac/nfcc_coex_access.c
+++ b/mac/nfcc_coex_access.c
@@ -40,38 +40,60 @@ static void nfcc_coex_access_done(struct mcps802154_access *access, bool error)
 	struct nfcc_coex_local *local = access_to_local(access);
 	struct nfcc_coex_session *session = &local->session;
 
-	if (error) {
-		const struct dw3000_vendor_cmd_nfcc_coex_get_access_info stop = {
+	/* Stop on error because the next timestamps is unknown.
+	 * Stop in V2, because the vendor stop is not supported by NFC. */
+	if ((error || (session->state == NFCC_COEX_STATE_STOPPING &&
+		       session->params.version == 2)) &&
+	    !session->get_access_info.watchdog_timeout) {
+		const struct llhw_vendor_cmd_nfcc_coex_get_access_info stop = {
 			.stop = true,
 		};
 
 		local->session.get_access_info = stop;
 	}
 
-	if (session->state != NFCC_COEX_STATE_ACCESSING ||
-	    session->get_access_info.stop ||
+	if (session->get_access_info.stop ||
 	    session->get_access_info.watchdog_timeout)
-		session->started = false;
+		nfcc_coex_set_state(local, NFCC_COEX_STATE_IDLE);
+
 	nfcc_coex_report(local);
-	nfcc_coex_set_state(local, NFCC_COEX_STATE_IDLE);
 }
 
 static int nfcc_coex_handle(struct mcps802154_access *access)
 {
 	struct nfcc_coex_local *local = access_to_local(access);
 	struct nfcc_coex_session *session = &local->session;
-	struct dw3000_vendor_cmd_nfcc_coex_handle_access handle_access = {};
+	struct llhw_vendor_cmd_nfcc_coex_handle_access handle_access = {};
 
 	handle_access.start = session->first_access;
 	handle_access.timestamp_dtu = access->timestamp_dtu;
 	handle_access.duration_dtu = access->duration_dtu;
 	handle_access.chan = session->params.channel_number;
-
-	nfcc_coex_set_state(local, NFCC_COEX_STATE_ACCESSING);
-	session->first_access = false;
+	handle_access.version = session->params.version;
+
+	if (session->state == NFCC_COEX_STATE_STOPPING &&
+	    session->params.version == 3) {
+		/* Stop processing : stop the nfcc coex */
+		if (local->session.first_access) {
+			struct mcps802154_region_demand *rd =
+				&session->region_demand;
+			struct llhw_vendor_cmd_nfcc_coex_stop stop = {
+				.timestamp_dtu = rd->timestamp_dtu,
+				.duration_dtu = rd->max_duration_dtu,
+				.version = session->params.version,
+			};
+			return mcps802154_vendor_cmd(
+				local->llhw, VENDOR_QORVO_OUI,
+				LLHW_VENDOR_CMD_NFCC_COEX_STOP, &stop,
+				sizeof(stop));
+		} else
+			return mcps802154_vendor_cmd(
+				local->llhw, VENDOR_QORVO_OUI,
+				LLHW_VENDOR_CMD_NFCC_COEX_STOP, NULL, 0);
+	}
 
 	return mcps802154_vendor_cmd(local->llhw, VENDOR_QORVO_OUI,
-				     DW3000_VENDOR_CMD_NFCC_COEX_HANDLE_ACCESS,
+				     LLHW_VENDOR_CMD_NFCC_COEX_HANDLE_ACCESS,
 				     &handle_access, sizeof(handle_access));
 }
 
@@ -79,14 +101,16 @@ static int nfcc_coex_tx_done(struct mcps802154_access *access)
 {
 	struct nfcc_coex_local *local = access_to_local(access);
 	struct nfcc_coex_session *session = &local->session;
-	struct dw3000_vendor_cmd_nfcc_coex_get_access_info *get_access_info =
+	struct llhw_vendor_cmd_nfcc_coex_get_access_info *get_access_info =
 		&session->get_access_info;
 	struct mcps802154_region_demand *rd = &session->region_demand;
 	int r;
 
+	session->first_access = false;
+
 	r = mcps802154_vendor_cmd(
 		local->llhw, VENDOR_QORVO_OUI,
-		DW3000_VENDOR_CMD_NFCC_COEX_GET_ACCESS_INFORMATION,
+		LLHW_VENDOR_CMD_NFCC_COEX_GET_ACCESS_INFORMATION,
 		get_access_info, sizeof(*get_access_info));
 	if (r)
 		return r;
@@ -98,12 +122,17 @@ static int nfcc_coex_tx_done(struct mcps802154_access *access)
 	return 1;
 }
 
-static int nfcc_coex_schedule_change(struct mcps802154_access *access)
+static int nfcc_coex_broken(struct mcps802154_access *access)
 {
 	struct nfcc_coex_local *local = access_to_local(access);
-	struct nfcc_coex_session *session = &local->session;
+	const struct llhw_vendor_cmd_nfcc_coex_get_access_info
+		watchdog_timeout = {
+			.watchdog_timeout = true,
+		};
 
-	return session->state == NFCC_COEX_STATE_STOPPING ? 1 : 0;
+	local->session.get_access_info = watchdog_timeout;
+	/* Request end of current access. */
+	return -ETIME;
 }
 
 struct mcps802154_access_vendor_ops nfcc_coex_ops = {
@@ -112,7 +141,7 @@ struct mcps802154_access_vendor_ops nfcc_coex_ops = {
 	},
 	.handle = nfcc_coex_handle,
 	.tx_done = nfcc_coex_tx_done,
-	.schedule_change = nfcc_coex_schedule_change,
+	.broken = nfcc_coex_broken,
 };
 
 static struct mcps802154_access *
@@ -140,7 +169,8 @@ struct mcps802154_access *nfcc_coex_get_access(struct mcps802154_region *region,
 	struct nfcc_coex_local *local = region_to_local(region);
 	struct nfcc_coex_session *session = &local->session;
 
-	if (session->started) {
+	if (session->state == NFCC_COEX_STATE_STARTED ||
+	    session->state == NFCC_COEX_STATE_STOPPING) {
 		nfcc_coex_session_update(local, session, next_timestamp_dtu,
 					 region_duration_dtu);
 		return nfcc_coex_access_controller(local, session);
diff --git a/mac/nfcc_coex_region.c b/mac/nfcc_coex_region.c
index bbd12df..1e26b64 100644
--- a/mac/nfcc_coex_region.c
+++ b/mac/nfcc_coex_region.c
@@ -64,16 +64,8 @@ static void nfcc_coex_close(struct mcps802154_region *region)
 static void nfcc_coex_notify_stop(struct mcps802154_region *region)
 {
 	struct nfcc_coex_local *local = region_to_local(region);
-	struct nfcc_coex_session *session = &local->session;
 
 	trace_region_nfcc_coex_notify_stop(local);
-	nfcc_coex_session_control(local, NFCC_COEX_CALL_CCC_SESSION_STOP, NULL,
-				  NULL);
-	if (session->started) {
-		pr_err("device stopped while nfcc coex not stopped state=%d",
-		       local->session.state);
-		session->started = false;
-	}
 }
 
 static int nfcc_coex_call(struct mcps802154_region *region, u32 call_id,
@@ -100,44 +92,30 @@ static int nfcc_coex_get_demand(struct mcps802154_region *region,
 	const struct nfcc_coex_session *session = &local->session;
 	const struct mcps802154_region_demand *rd = &session->region_demand;
 
-	trace_region_nfcc_coex_get_demand(local, next_timestamp_dtu, rd);
-	if (!session->started)
-		return 0;
+	demand->max_duration_dtu = 0;
+
+	switch (session->state) {
+	case NFCC_COEX_STATE_STARTED:
+		if (is_before_dtu(rd->timestamp_dtu, next_timestamp_dtu))
+			demand->timestamp_dtu = next_timestamp_dtu;
+		else
+			demand->timestamp_dtu = rd->timestamp_dtu;
+		return 1;
+
+	case NFCC_COEX_STATE_STOPPING:
+		if (session->first_access) {
+			if (is_before_dtu(rd->timestamp_dtu,
+					  next_timestamp_dtu))
+				demand->timestamp_dtu = next_timestamp_dtu;
+			else
+				demand->timestamp_dtu = rd->timestamp_dtu;
+		} else
+			demand->timestamp_dtu = next_timestamp_dtu;
+		return 1;
 
-	if (is_before_dtu(rd->timestamp_dtu, next_timestamp_dtu)) {
-		/* Date is late. */
-		int shift_dtu = next_timestamp_dtu - rd->timestamp_dtu;
-		int new_duration_dtu = rd->max_duration_dtu - shift_dtu;
-
-		new_duration_dtu =
-			new_duration_dtu <= 0 ? 1 : new_duration_dtu;
-		/* Keep 'rd' unchanged, because the update will be done
-		 * during the get_access.
-		 * See nfcc_coex_session_update function. */
-		demand->timestamp_dtu = next_timestamp_dtu;
-		demand->max_duration_dtu = new_duration_dtu;
-	} else if (!rd->max_duration_dtu) {
-		/* Infinite duration will lock the region
-		 * interleaving.
-		 * Duration value can be 0 when the region is started
-		 * when an another region have been started.
-		 * In other words, the get_demand will be call
-		 * before the get_access/access_done.
-		 *
-		 * Remarks:
-		 * - The duration_dtu must stay at 0, which is
-		 *   forward to nfcc_coex_access_controller and
-		 *   nfcc_coex_handle functions.
-		 * - 12ms is an default value returned which sess_dbg done
-		 *   on nfcc initiator board (it's a workaround).
-		 **/
-		demand->timestamp_dtu = rd->timestamp_dtu;
-		demand->max_duration_dtu =
-			12 * (local->llhw->dtu_freq_hz / 1000);
-	} else {
-		memcpy(demand, rd, sizeof(*demand));
+	default:
+		return 0;
 	}
-	return 1;
 }
 
 void nfcc_coex_set_state(struct nfcc_coex_local *local,
@@ -152,8 +130,8 @@ void nfcc_coex_set_state(struct nfcc_coex_local *local,
 void nfcc_coex_report(struct nfcc_coex_local *local)
 {
 	struct nfcc_coex_session *session = &local->session;
-	const struct dw3000_vendor_cmd_nfcc_coex_get_access_info
-		*get_access_info = &session->get_access_info;
+	const struct llhw_vendor_cmd_nfcc_coex_get_access_info *get_access_info =
+		&session->get_access_info;
 	struct sk_buff *msg;
 	int r;
 
diff --git a/mac/nfcc_coex_region_call.c b/mac/nfcc_coex_region_call.c
index bf58da1..a7e63cd 100644
--- a/mac/nfcc_coex_region_call.c
+++ b/mac/nfcc_coex_region_call.c
@@ -42,6 +42,8 @@ static const struct nla_policy nfcc_coex_session_param_nla_policy
 	[NFCC_COEX_CCC_SESSION_PARAM_ATTR_MAX + 1] = {
 		[NFCC_COEX_CCC_SESSION_PARAM_ATTR_TIME0_NS] = { .type = NLA_U64 },
 		[NFCC_COEX_CCC_SESSION_PARAM_ATTR_CHANNEL_NUMBER] = { .type = NLA_U8 },
+		[NFCC_COEX_CCC_SESSION_PARAM_ATTR_VERSION] =
+			NLA_POLICY_RANGE(NLA_U8, 2, 3),
 	};
 
 /**
@@ -66,6 +68,9 @@ static int nfcc_coex_session_set_parameters(struct nfcc_coex_local *local,
 		(S32_MAX * NS_PER_SECOND) / local->llhw->dtu_freq_hz;
 	int r;
 
+	if (!params)
+		return -EINVAL;
+
 	r = nla_parse_nested(attrs, NFCC_COEX_CCC_SESSION_PARAM_ATTR_MAX,
 			     params, nfcc_coex_session_param_nla_policy,
 			     info->extack);
@@ -84,6 +89,7 @@ static int nfcc_coex_session_set_parameters(struct nfcc_coex_local *local,
 
 	P(TIME0_NS, time0_ns, u64, x);
 	P(CHANNEL_NUMBER, channel_number, u8, x);
+	P(VERSION, version, u8, x);
 
 #undef P
 
@@ -94,7 +100,6 @@ static int nfcc_coex_session_set_parameters(struct nfcc_coex_local *local,
 
 	if (p->time0_ns - now_ns > max_time0_ns)
 		return -ERANGE;
-
 	return 0;
 }
 
@@ -116,7 +121,7 @@ static int nfcc_coex_session_start(struct nfcc_coex_local *local,
 	s64 diff_dtu;
 	int r;
 
-	WARN_ON(session->started);
+	WARN_ON(session->state == NFCC_COEX_STATE_STARTED);
 
 	trace_region_nfcc_coex_session_start(local, p);
 	r = mcps802154_get_current_timestamp_dtu(local->llhw, &now_dtu);
@@ -132,7 +137,7 @@ static int nfcc_coex_session_start(struct nfcc_coex_local *local,
 	session->region_demand.max_duration_dtu = 0;
 	session->event_portid = info->snd_portid;
 	session->first_access = true;
-	session->started = true;
+	nfcc_coex_set_state(local, NFCC_COEX_STATE_STARTED);
 
 	mcps802154_reschedule(local->llhw);
 	return 0;
@@ -162,7 +167,7 @@ static int nfcc_coex_session_start_all(struct nfcc_coex_local *local,
 	if (r)
 		return r;
 
-	if (local->session.started)
+	if (local->session.state == NFCC_COEX_STATE_STARTED)
 		return -EBUSY;
 
 	nfcc_coex_session_init(local);
@@ -189,21 +194,13 @@ static int nfcc_coex_session_start_all(struct nfcc_coex_local *local,
 static int nfcc_coex_session_stop(struct nfcc_coex_local *local)
 {
 	struct nfcc_coex_session *session = &local->session;
-	int r = 0;
 
 	trace_region_nfcc_coex_session_stop(local);
-	if (session->started) {
-		if (session->state == NFCC_COEX_STATE_ACCESSING) {
-			nfcc_coex_set_state(local, NFCC_COEX_STATE_STOPPING);
-			r = mcps802154_vendor_cmd(
-				local->llhw, VENDOR_QORVO_OUI,
-				DW3000_VENDOR_CMD_NFCC_COEX_STOP, NULL, 0);
-			if (!r)
-				/* Access is stopped. */
-				mcps802154_reschedule(local->llhw);
-		}
+	if (session->state == NFCC_COEX_STATE_STARTED) {
+		nfcc_coex_set_state(local, NFCC_COEX_STATE_STOPPING);
+		mcps802154_schedule_invalidate(local->llhw);
 	}
-	return r;
+	return 0;
 }
 
 int nfcc_coex_session_control(struct nfcc_coex_local *local, u32 call_id,
diff --git a/mac/nfcc_coex_session.c b/mac/nfcc_coex_session.c
index 712b470..6247365 100644
--- a/mac/nfcc_coex_session.c
+++ b/mac/nfcc_coex_session.c
@@ -30,6 +30,9 @@ void nfcc_coex_session_init(struct nfcc_coex_local *local)
 	struct nfcc_coex_session_params *p = &local->session.params;
 
 	memset(p, 0, sizeof(*p));
+
+	/* Default protocol version is V2 */
+	p->version = 3;
 }
 
 void nfcc_coex_session_update(struct nfcc_coex_local *local,
@@ -40,13 +43,10 @@ void nfcc_coex_session_update(struct nfcc_coex_local *local,
 
 	if (is_before_dtu(rd->timestamp_dtu, next_timestamp_dtu)) {
 		int shift_dtu = next_timestamp_dtu - rd->timestamp_dtu;
-		int new_duration_dtu = rd->max_duration_dtu - shift_dtu;
 
 		/* Date is late. */
-		new_duration_dtu = new_duration_dtu < 0 ? 0 : new_duration_dtu;
-		trace_region_nfcc_coex_session_update_late(local, shift_dtu,
-							   new_duration_dtu);
+		trace_region_nfcc_coex_session_update_late(local, shift_dtu, 0);
 		rd->timestamp_dtu = next_timestamp_dtu;
-		rd->max_duration_dtu = new_duration_dtu;
+		rd->max_duration_dtu = 0;
 	}
 }
diff --git a/mac/nfcc_coex_session.h b/mac/nfcc_coex_session.h
index a3cad45..49de0b3 100644
--- a/mac/nfcc_coex_session.h
+++ b/mac/nfcc_coex_session.h
@@ -42,20 +42,24 @@ struct nfcc_coex_session_params {
 	 * @channel_number: Channel to use for the session, 5 or 9.
 	 */
 	u8 channel_number;
+	/**
+	 * @version: Protocol version to use.
+	 */
+	u8 version;
 };
 
 /**
  * enum nfcc_coex_state - State of the unique session.
  * @NFCC_COEX_STATE_IDLE:
  *     Session is not used by access right now.
- * @NFCC_COEX_STATE_ACCESSING:
- *     Session is currently used on an access.
+ * @NFCC_COEX_STATE_STARTED:
+ *     Session is started.
  * @NFCC_COEX_STATE_STOPPING:
  *     Session is currently used for the last access.
  */
 enum nfcc_coex_state {
 	NFCC_COEX_STATE_IDLE,
-	NFCC_COEX_STATE_ACCESSING,
+	NFCC_COEX_STATE_STARTED,
 	NFCC_COEX_STATE_STOPPING,
 };
 
@@ -75,7 +79,7 @@ struct nfcc_coex_session {
 	/**
 	 * @get_access_info: Next access feedback get through a vendor command.
 	 */
-	struct dw3000_vendor_cmd_nfcc_coex_get_access_info get_access_info;
+	struct llhw_vendor_cmd_nfcc_coex_get_access_info get_access_info;
 	/**
 	 * @region_demand: Region access demand which contains start and duration.
 	 */
@@ -88,10 +92,6 @@ struct nfcc_coex_session {
 	 * @state: State of the unique session.
 	 */
 	enum nfcc_coex_state state;
-	/**
-	 * @started: Session is currently started.
-	 */
-	bool started;
 };
 
 /* Forward declaration. */
diff --git a/mac/nfcc_coex_trace.h b/mac/nfcc_coex_trace.h
index 4753e7a..3d24f81 100644
--- a/mac/nfcc_coex_trace.h
+++ b/mac/nfcc_coex_trace.h
@@ -52,10 +52,10 @@ TRACE_DEFINE_ENUM(NFCC_COEX_CALL_CCC_SESSION_NOTIFICATION);
 	}
 #define NFCC_COEX_STATE_SYMBOLS                          \
 	nfcc_coex_state_name(IDLE),                      \
-	nfcc_coex_state_name(ACCESSING),                 \
+	nfcc_coex_state_name(STARTED),                   \
 	nfcc_coex_state_name(STOPPING)
 TRACE_DEFINE_ENUM(NFCC_COEX_STATE_IDLE);
-TRACE_DEFINE_ENUM(NFCC_COEX_STATE_ACCESSING);
+TRACE_DEFINE_ENUM(NFCC_COEX_STATE_STARTED);
 TRACE_DEFINE_ENUM(NFCC_COEX_STATE_STOPPING);
 
 #define NFCC_COEX_LOCAL_ENTRY __field(enum nfcc_coex_state, state)
@@ -86,15 +86,17 @@ TRACE_EVENT(
 		NFCC_COEX_LOCAL_ENTRY
 		__field(u64, time0_ns)
 		__field(u8, channel_number)
+		__field(u8, version)
 	),
 	TP_fast_assign(
 		NFCC_COEX_LOCAL_ASSIGN;
 		__entry->time0_ns = p->time0_ns;
 		__entry->channel_number = p->channel_number;
+		__entry->version = p->version;
 	),
-	TP_printk(NFCC_COEX_LOCAL_PR_FMT " time0_ns=%llu channel_number=%d",
+	TP_printk(NFCC_COEX_LOCAL_PR_FMT " time0_ns=%llu channel_number=%d version=%d",
 		  NFCC_COEX_LOCAL_PR_ARG, __entry->time0_ns,
-		  __entry->channel_number)
+		  __entry->channel_number, __entry->version)
 );
 
 DEFINE_EVENT(
@@ -128,32 +130,6 @@ TRACE_EVENT(
 );
 
 TRACE_EVENT(
-	region_nfcc_coex_get_demand,
-	TP_PROTO(const struct nfcc_coex_local *local,
-		 u32 next_timestamp_dtu,
-		 const struct mcps802154_region_demand *rd),
-	TP_ARGS(local, next_timestamp_dtu, rd),
-	TP_STRUCT__entry(
-		NFCC_COEX_LOCAL_ENTRY
-		__field(u32, next_timestamp_dtu)
-		__field(u32, timestamp_dtu)
-		__field(int, duration_dtu)
-		),
-	TP_fast_assign(
-		NFCC_COEX_LOCAL_ASSIGN;
-		__entry->next_timestamp_dtu = next_timestamp_dtu;
-		__entry->timestamp_dtu = rd->timestamp_dtu;
-		__entry->duration_dtu = rd->max_duration_dtu;
-		),
-	TP_printk(NFCC_COEX_LOCAL_PR_FMT " next_timestamp_dtu=0x%08x "
-		  "rd.timestamp_dtu=0x%08x rd.duration_dtu=0x%08x",
-		  NFCC_COEX_LOCAL_PR_ARG,
-		  __entry->next_timestamp_dtu,
-		  __entry->timestamp_dtu,
-		  __entry->duration_dtu)
-);
-
-TRACE_EVENT(
 	region_nfcc_coex_session_update_late,
 	TP_PROTO(const struct nfcc_coex_local *local,
 		 int shift_dtu, int new_duration_dtu),
@@ -197,7 +173,7 @@ TRACE_EVENT(
 TRACE_EVENT(
 	region_nfcc_coex_report,
 	TP_PROTO(const struct nfcc_coex_local *local,
-		 const struct dw3000_vendor_cmd_nfcc_coex_get_access_info *info),
+		 const struct llhw_vendor_cmd_nfcc_coex_get_access_info *info),
 	TP_ARGS(local, info),
 	TP_STRUCT__entry(
 		NFCC_COEX_LOCAL_ENTRY
diff --git a/mac/on_demand_scheduler.c b/mac/on_demand_scheduler.c
index 4cf2b24..852901e 100644
--- a/mac/on_demand_scheduler.c
+++ b/mac/on_demand_scheduler.c
@@ -47,6 +47,10 @@ struct mcps802154_on_demand_local {
 	 * @llhw: Low layer hardware attached.
 	 */
 	struct mcps802154_llhw *llhw;
+	/**
+	 * @idle_region: Idle region to delay start of region selected.
+	 */
+	struct mcps802154_region *idle_region;
 };
 
 static inline struct mcps802154_on_demand_local *
@@ -63,10 +67,20 @@ mcps802154_on_demand_scheduler_open(struct mcps802154_llhw *llhw)
 
 	plocal = kmalloc(sizeof(*plocal), GFP_KERNEL);
 	if (!plocal)
-		return NULL;
+		goto open_failure;
+
+	plocal->idle_region = mcps802154_region_open(llhw, "idle", NULL, NULL);
+	if (!plocal->idle_region) {
+		goto open_failure;
+	}
+
 	plocal->llhw = llhw;
 	plocal->scheduler.n_regions = 0;
 	return &plocal->scheduler;
+
+open_failure:
+	kfree(plocal);
+	return NULL;
 }
 
 static void
@@ -75,28 +89,27 @@ mcps802154_on_demand_scheduler_close(struct mcps802154_scheduler *scheduler)
 	struct mcps802154_on_demand_local *plocal =
 		scheduler_to_plocal(scheduler);
 
+	kfree(plocal->idle_region);
 	kfree(plocal);
 }
 
-static int mcps802154_on_demand_scheduler_update_schedule(
-	struct mcps802154_scheduler *scheduler,
-	const struct mcps802154_schedule_update *schedule_update,
-	u32 next_timestamp_dtu)
+static int mcps802154_on_demand_scheduler_get_next_region(
+	struct mcps802154_on_demand_local *plocal, struct list_head *regions,
+	const struct mcps802154_region *first_region, u32 next_timestamp_dtu,
+	struct mcps802154_region_demand *next_demand,
+	struct mcps802154_region **next_region)
 {
-	struct mcps802154_on_demand_local *plocal =
-		scheduler_to_plocal(scheduler);
-	struct mcps802154_region_demand demand;
-	struct mcps802154_region *region, *next_region = NULL;
-	struct list_head *regions;
+	struct mcps802154_region *region;
 	int max_duration_dtu = 0;
-	u32 start_dtu;
 	int r;
 
-	mcps802154_schedule_get_regions(plocal->llhw, &regions);
-
+	*next_region = NULL;
 	list_for_each_entry (region, regions, ca_entry) {
 		struct mcps802154_region_demand candidate = {};
 
+		if (first_region && region == first_region)
+			continue;
+
 		r = mcps802154_region_get_demand(
 			plocal->llhw, region, next_timestamp_dtu, &candidate);
 		switch (r) {
@@ -121,30 +134,51 @@ static int mcps802154_on_demand_scheduler_update_schedule(
 						     next_timestamp_dtu;
 
 		/* Arbitrate between regions. */
-		if (!next_region || is_before_dtu(candidate.timestamp_dtu,
-						  demand.timestamp_dtu)) {
-			next_region = region;
-			demand = candidate;
+		if (!*next_region ||
+		    is_before_dtu(candidate.timestamp_dtu,
+				  next_demand->timestamp_dtu)) {
+			*next_region = region;
+			*next_demand = candidate;
 			/* Is there some time remaining for a region with
 			 * less priority? */
 			if (!is_before_dtu(next_timestamp_dtu,
-					   demand.timestamp_dtu))
+					   next_demand->timestamp_dtu))
 				break;
 			else
-				max_duration_dtu = demand.timestamp_dtu -
+				max_duration_dtu = next_demand->timestamp_dtu -
 						   next_timestamp_dtu;
 		}
 	}
 
+	return *next_region ? 1 : 0;
+}
+
+static int mcps802154_on_demand_scheduler_update_schedule(
+	struct mcps802154_scheduler *scheduler,
+	const struct mcps802154_schedule_update *schedule_update,
+	u32 next_timestamp_dtu)
+{
+	struct mcps802154_on_demand_local *plocal =
+		scheduler_to_plocal(scheduler);
+	struct list_head *regions;
+	struct mcps802154_region_demand next_demand;
+	struct mcps802154_region *next_region = NULL;
+	u32 start_in_schedule_dtu;
+	int r;
+
+	mcps802154_schedule_get_regions(plocal->llhw, &regions);
+	r = mcps802154_on_demand_scheduler_get_next_region(
+		plocal, regions, NULL, next_timestamp_dtu, &next_demand,
+		&next_region);
+	if (r < 0)
+		return r;
+
 	if (!next_region)
 		return -ENOENT;
 
-	start_dtu = demand.timestamp_dtu -
-		    schedule_update->expected_start_timestamp_dtu;
+	start_in_schedule_dtu = next_demand.timestamp_dtu - next_timestamp_dtu;
 
-	r = mcps802154_schedule_set_start(
-		schedule_update, schedule_update->expected_start_timestamp_dtu);
-	/* Can not fail, only possible error is invalid parameters. */
+	r = mcps802154_schedule_set_start(schedule_update, next_timestamp_dtu);
 	WARN_RETURN(r);
 
 	r = mcps802154_schedule_recycle(schedule_update, 0,
@@ -152,12 +186,81 @@ static int mcps802154_on_demand_scheduler_update_schedule(
 	/* Can not fail, only possible error is invalid parameters. */
 	WARN_RETURN(r);
 
+	if (next_demand.max_duration_dtu)
+		next_demand.max_duration_dtu += start_in_schedule_dtu;
+ 	start_in_schedule_dtu = 0;
+
+	if (start_in_schedule_dtu)
+		/* Don't give the access to the region too early.
+		 * And provide advantages:
+		 *  - to have a region inserted with a CA invalidate schedule.
+		 *  - Reduce latency with TX frame prepared close to region
+		 *    start date. */
+		r = mcps802154_schedule_add_region(schedule_update,
+						   plocal->idle_region, 0,
+						   start_in_schedule_dtu,
+						   false);
 	r = mcps802154_schedule_add_region(schedule_update, next_region,
-					   start_dtu, demand.max_duration_dtu);
+					   start_in_schedule_dtu,
+					   next_demand.max_duration_dtu, true);
 
 	return r;
 }
 
+static int mcps802154_on_demand_scheduler_get_next_demands(
+	struct mcps802154_scheduler *scheduler,
+	const struct mcps802154_region *region, u32 timestamp_dtu,
+	int duration_dtu, int delta_dtu,
+	struct mcps802154_region_demand *demands)
+{
+	struct mcps802154_on_demand_local *plocal =
+		scheduler_to_plocal(scheduler);
+	struct list_head *regions;
+	bool is_demands_set = false;
+	u32 next_timestamp_dtu = timestamp_dtu;
+	int r;
+
+	mcps802154_schedule_get_regions(plocal->llhw, &regions);
+
+	while (true) {
+		struct mcps802154_region_demand next_demand;
+		struct mcps802154_region *next_region = NULL;
+
+		r = mcps802154_on_demand_scheduler_get_next_region(
+			plocal, regions, region, next_timestamp_dtu,
+			&next_demand, &next_region);
+		if (r < 0)
+			return r;
+		if (!r || !next_demand.max_duration_dtu ||
+		    !is_before_dtu(next_demand.timestamp_dtu,
+				   timestamp_dtu + duration_dtu))
+			break;
+		if (!is_demands_set) {
+			*demands = next_demand;
+			is_demands_set = true;
+		} else if (!is_before_dtu(demands->timestamp_dtu +
+						  demands->max_duration_dtu +
+						  delta_dtu,
+					  next_demand.timestamp_dtu)) {
+			demands->max_duration_dtu =
+				next_demand.timestamp_dtu +
+				next_demand.max_duration_dtu -
+				demands->timestamp_dtu;
+		} else {
+			break;
+		}
+
+		if (!is_before_dtu(demands->timestamp_dtu +
+					   demands->max_duration_dtu,
+				   timestamp_dtu + duration_dtu))
+			break;
+
+		next_timestamp_dtu =
+			demands->timestamp_dtu + demands->max_duration_dtu;
+	}
+	return is_demands_set ? 1 : 0;
+}
+
 static struct mcps802154_scheduler_ops
 	mcps802154_on_demand_scheduler_scheduler = {
 		.owner = THIS_MODULE,
@@ -167,6 +270,8 @@ static struct mcps802154_scheduler_ops
 		.set_parameters = NULL, /* No scheduler parameters for now. */
 		.update_schedule =
 			mcps802154_on_demand_scheduler_update_schedule,
+		.get_next_demands =
+			mcps802154_on_demand_scheduler_get_next_demands,
 	};
 
 int __init mcps802154_on_demand_scheduler_init(void)
diff --git a/mac/pctt_access.c b/mac/pctt_access.c
index 69747ec..ae7b994 100644
--- a/mac/pctt_access.c
+++ b/mac/pctt_access.c
@@ -21,6 +21,7 @@
  * Qorvo. Please contact Qorvo to inquire about licensing terms.
  */
 
+#include <linux/math64.h>
 #include "pctt_access.h"
 #include "pctt_region.h"
 #include "pctt_region_call.h"
@@ -32,18 +33,34 @@
 #include <net/pctt_region_params.h>
 #include <asm/unaligned.h>
 
+#include "warn_return.h"
+
 #define PCTT_STS_FOM_THRESHOLD 153
+/* The FC-PHY shall have a block timing tolerance of +/-100 ppm as
+   specified in IEEE Std 802.15.4z-2020, subclause 6.9.7.2. */
+#define PCTT_MARGIN_PPM 200
+
+static inline int pctt_rx_margin(int duration)
+{
+	return duration / (1000000 / PCTT_MARGIN_PPM);
+}
 
-static void pctt_set_sts_params(struct mcps802154_sts_params *sts_params,
-				u32 sts_index)
+static void
+pctt_set_sts_params(struct mcps802154_sts_params *sts_params,
+		    const struct pctt_session_params *session_params)
 {
 	const u8 key[AES_KEYSIZE_128] = { 0x14, 0x14, 0x86, 0x74, 0xd1, 0xd3,
 					  0x36, 0xaa, 0xf8, 0x60, 0x50, 0xa8,
 					  0x14, 0xeb, 0x22, 0xf };
 	u8 *iv = sts_params->v;
-
-	sts_params->n_segs = 1;
-	sts_params->seg_len = 64;
+	u8 seg_len = session_params->sts_length == PCTT_STS_LENGTH_128 ?
+			     128 :
+			     session_params->sts_length == PCTT_STS_LENGTH_32 ?
+			     32 :
+			     64;
+
+	sts_params->n_segs = session_params->number_of_sts_segments;
+	sts_params->seg_len = seg_len;
 	sts_params->sp2_tx_gap_4chips = 0;
 	sts_params->sp2_rx_gap_4chips[0] = 0;
 	sts_params->sp2_rx_gap_4chips[1] = 0;
@@ -52,11 +69,31 @@ static void pctt_set_sts_params(struct mcps802154_sts_params *sts_params,
 
 	/* Overflow is not propagated to the next IV */
 	put_unaligned_be32(0x362eeb34u, &iv[0]);
-	put_unaligned_be32(0xc44fa8fbu + sts_index, &iv[sizeof(u32)]);
+	put_unaligned_be32(0xc44fa8fbu + session_params->sts_index,
+			   &iv[sizeof(u32)]);
 	put_unaligned_be64(0xd37ec3ca1f9a3de4ull, &iv[sizeof(u64)]);
 	memcpy(sts_params->key, key, AES_KEYSIZE_128);
 }
 
+static void pctt_randomize_psdu(struct pctt_local *local)
+{
+	struct pctt_session *session = &local->session;
+	struct pctt_session_params *p = &session->params;
+
+	if (p->randomize_psdu && session->first_access) {
+		const int A = 1664525, B = 1013904223;
+		/* First byte of data is used as seed. */
+		u32 state = p->data_payload[0];
+		u8 *buf = p->data_payload;
+		int size = p->data_payload_len;
+		int i;
+		for (i = 0; i < size; i++) {
+			state = A * state + B;
+			buf[i] = state >> 8;
+		}
+	}
+}
+
 /**
  * pctt_access_setup_frame() - Fill an access frame from a PCTT slot.
  * @local: PCTT context.
@@ -77,26 +114,26 @@ static void pctt_access_setup_frame(struct pctt_local *local,
 	bool is_rframe = p->rframe_config != PCTT_RFRAME_CONFIG_SP0;
 
 	if (is_rframe) {
-		pctt_set_sts_params(sts_params, p->sts_index);
+		pctt_set_sts_params(sts_params, p);
 		sts_params_for_access = sts_params;
 	}
 
 	if (slot->is_tx) {
 		u8 flags = slot->is_immediate ?
 				   0 :
-				   MCPS802154_TX_FRAME_TIMESTAMP_DTU;
+				   MCPS802154_TX_FRAME_CONFIG_TIMESTAMP_DTU;
 
 		if (is_rframe) {
 			if (p->rframe_config == PCTT_RFRAME_CONFIG_SP3)
-				flags |= MCPS802154_TX_FRAME_SP3;
+				flags |= MCPS802154_TX_FRAME_CONFIG_SP3;
 			else if (p->rframe_config == PCTT_RFRAME_CONFIG_SP2)
-				flags |= MCPS802154_TX_FRAME_SP2;
+				flags |= MCPS802154_TX_FRAME_CONFIG_SP2;
 			else
-				flags |= MCPS802154_TX_FRAME_SP1;
+				flags |= MCPS802154_TX_FRAME_CONFIG_SP1;
 		}
 		*frame = (struct mcps802154_access_frame){
 			.is_tx = true,
-			.tx_frame_info = {
+			.tx_frame_config = {
 				.timestamp_dtu = frame_dtu,
 				.flags = flags,
 				.ant_set_id = p->tx_antenna_selection,
@@ -106,24 +143,31 @@ static void pctt_access_setup_frame(struct pctt_local *local,
 	} else {
 		u8 flags = slot->is_immediate ?
 				   0 :
-				   MCPS802154_RX_INFO_TIMESTAMP_DTU;
-		u16 request = MCPS802154_RX_FRAME_INFO_TIMESTAMP_RCTU;
+				   MCPS802154_RX_FRAME_CONFIG_TIMESTAMP_DTU;
+		u16 request = MCPS802154_RX_FRAME_INFO_TIMESTAMP_RCTU |
+			      MCPS802154_RX_FRAME_INFO_TIMESTAMP_DTU |
+			      MCPS802154_RX_FRAME_INFO_RSSI;
 
 		if (is_rframe) {
-			flags |= MCPS802154_RX_INFO_RANGING;
 			request |= MCPS802154_RX_FRAME_INFO_RANGING_STS_FOM;
-
+			flags |= MCPS802154_RX_FRAME_CONFIG_RANGING;
+			if (session->cmd_id == PCTT_ID_ATTR_SS_TWR) {
+				flags |=
+					MCPS802154_RX_FRAME_CONFIG_RANGING_PDOA;
+				request |=
+					MCPS802154_RX_FRAME_INFO_RANGING_PDOA;
+			}
 			if (p->rframe_config == PCTT_RFRAME_CONFIG_SP3)
-				flags |= MCPS802154_RX_INFO_SP3;
+				flags |= MCPS802154_RX_FRAME_CONFIG_SP3;
 			else if (p->rframe_config == PCTT_RFRAME_CONFIG_SP2)
-				flags |= MCPS802154_RX_INFO_SP2;
+				flags |= MCPS802154_RX_FRAME_CONFIG_SP2;
 			else
-				flags |= MCPS802154_RX_INFO_SP1;
+				flags |= MCPS802154_RX_FRAME_CONFIG_SP1;
 		}
 		*frame = (struct mcps802154_access_frame){
 			.is_tx = false,
 			.rx = {
-				.info = {
+				.frame_config = {
 					.timestamp_dtu = frame_dtu,
 					.flags = flags,
 					.timeout_dtu = slot->timeout_dtu,
@@ -140,15 +184,15 @@ static struct sk_buff *pctt_tx_get_frame(struct mcps802154_access *access,
 					 int frame_idx)
 {
 	struct pctt_local *local = access_to_local(access);
+	struct pctt_session *session = &local->session;
+	const struct pctt_session_params *p = &session->params;
 	struct sk_buff *skb = NULL;
 
-	if (local->data_payload_len) {
-		/* FIXME: Which size is the good one?
-		 *  - 1024,
-		 *  - local->data_payload_len,
-		 *  - PCTT_PAYLOAD_MAX_LEN(4096). */
-		skb = mcps802154_frame_alloc(local->llhw, 1024, GFP_KERNEL);
-		skb_put_data(skb, local->data_payload, local->data_payload_len);
+	if (p->data_payload_len) {
+		skb = mcps802154_frame_alloc(local->llhw, p->data_payload_len,
+					     GFP_KERNEL);
+		if (skb)
+			skb_put_data(skb, p->data_payload, p->data_payload_len);
 	}
 
 	return skb;
@@ -171,11 +215,13 @@ static void pctt_tx_return(struct mcps802154_access *access, int frame_idx,
 
 static bool pctt_rx_sts_good(const struct mcps802154_rx_frame_info *i)
 {
+	int idx;
 	if (!(i->flags & MCPS802154_RX_FRAME_INFO_RANGING_STS_FOM))
 		return false;
-	/* Only one segment for the moment. */
-	if (i->ranging_sts_fom[0] < PCTT_STS_FOM_THRESHOLD)
-		return false;
+	for (idx = 0; idx < MCPS802154_STS_N_SEGS_MAX; idx++) {
+		if (i->ranging_sts_fom[idx] < PCTT_STS_FOM_THRESHOLD)
+			return false;
+	}
 	return true;
 }
 
@@ -193,6 +239,34 @@ static void pctt_rx_frame_ss_twr(struct pctt_local *local,
 				PCTT_STATUS_RANGING_RX_PHY_TOA_FAILED;
 			return;
 		}
+		if (!pctt_rx_sts_good(info)) {
+			local->results.status =
+				PCTT_STATUS_RANGING_RX_PHY_STS_FAILED;
+			return;
+		}
+		if (info->flags & MCPS802154_RX_FRAME_INFO_RANGING_PDOA) {
+			struct mcps802154_rx_measurement_info info = {};
+			int r;
+
+			info.flags |= MCPS802154_RX_MEASUREMENTS_AOAS;
+			r = mcps802154_rx_get_measurement(local->llhw, NULL,
+							  &info);
+			if (!r &&
+			    info.flags & MCPS802154_RX_MEASUREMENTS_AOAS &&
+			    info.n_aoas) {
+				/* TODO: Find which aoas index to use */
+				ss_twr->pdoa_azimuth_deg_q7 =
+					map_rad_q11_to_deg_q7(
+						info.aoas[0].pdoa_rad_q11);
+				ss_twr->aoa_azimuth_deg_q7 =
+					map_rad_q11_to_deg_q7(
+						info.aoas[0].aoa_rad_q11);
+			}
+		}
+
+		if (info->flags & MCPS802154_RX_FRAME_INFO_RSSI) {
+			ss_twr->rssi = info->rssi;
+		}
 
 		ss_twr->rx_timestamps_rctu = info->timestamp_rctu;
 
@@ -218,6 +292,7 @@ static void pctt_rx_frame_ss_twr(struct pctt_local *local,
 			ss_twr->tx_timestamps_rctu =
 				mcps802154_tx_timestamp_dtu_to_rmarker_rctu(
 					local->llhw, frame_dtu,
+					access->hrp_uwb_params, access->channel,
 					p->tx_antenna_selection);
 
 			pctt_access_setup_frame(local, s, frame_dtu, frame,
@@ -240,58 +315,65 @@ static void pctt_rx_frame_ss_twr(struct pctt_local *local,
 	}
 }
 
-static void pctt_rx_frame_per_rx(struct pctt_local *local,
+static void pctt_rx_frame_per_rx(struct pctt_local *local, struct sk_buff *skb,
 				 const struct mcps802154_rx_frame_info *info,
 				 enum mcps802154_rx_error_type error)
 {
 	struct pctt_test_per_rx_results *per_rx = &local->results.tests.per_rx;
 
-	if (info) {
-		const struct pctt_session_params *p = &local->session.params;
-		bool is_rframe = p->rframe_config != PCTT_RFRAME_CONFIG_SP0;
+	struct pctt_session *session = &local->session;
+	const struct pctt_session_params *p = &session->params;
+	bool has_sts = p->rframe_config != PCTT_RFRAME_CONFIG_SP0;
 
-		if (is_rframe) {
-			if (pctt_rx_sts_good(info))
-				per_rx->sts_found++;
-			else
-				local->results.status =
-					PCTT_STATUS_RANGING_RX_PHY_STS_FAILED;
+	if (info) {
+		if (info->flags & MCPS802154_RX_FRAME_INFO_TIMESTAMP_DTU) {
+			session->next_timestamp_dtu = info->timestamp_dtu;
+			session->first_rx_synchronized = true;
+		}
+		if (info->flags & MCPS802154_RX_FRAME_INFO_RSSI) {
+			if (!per_rx->rssi || per_rx->rssi > info->rssi)
+				per_rx->rssi = info->rssi;
 		}
 	}
+	session->next_timestamp_dtu +=
+		p->gap_duration_dtu - pctt_rx_margin(p->gap_duration_dtu);
 
 	switch (error) {
 	case MCPS802154_RX_ERROR_NONE:
+	case MCPS802154_RX_ERROR_BAD_CKSUM:
 		per_rx->acq_detect++;
 		per_rx->sync_cir_ready++;
 		per_rx->sfd_found++;
 		per_rx->eof++;
+		if (has_sts && pctt_rx_sts_good(info))
+			per_rx->sts_found++;
+		if (skb && (skb->len != p->data_payload_len ||
+			    (!p->randomize_psdu &&
+			     memcmp(skb->data, p->data_payload, skb->len))))
+			per_rx->psdu_bit_error++;
+		if (error == MCPS802154_RX_ERROR_BAD_CKSUM)
+			per_rx->psdu_dec_error++;
 		break;
 	case MCPS802154_RX_ERROR_SFD_TIMEOUT:
-		per_rx->acq_reject++;
-		per_rx->sfd_fail++;
-		break;
-	case MCPS802154_RX_ERROR_BAD_CKSUM:
-		per_rx->psdu_bit_error++;
-		per_rx->eof++;
-		per_rx->rx_fail++;
 		per_rx->acq_detect++;
-		per_rx->sync_cir_ready++;
-		per_rx->sfd_found++;
+		per_rx->sfd_fail++;
 		break;
 	case MCPS802154_RX_ERROR_UNCORRECTABLE:
 	case MCPS802154_RX_ERROR_FILTERED:
 	case MCPS802154_RX_ERROR_HPDWARN:
 	case MCPS802154_RX_ERROR_OTHER:
-		per_rx->rx_fail++;
+	case MCPS802154_RX_ERROR_PHR_DECODE:
 		per_rx->acq_detect++;
 		per_rx->sync_cir_ready++;
 		per_rx->sfd_found++;
 		if (error == MCPS802154_RX_ERROR_OTHER) {
-			per_rx->phr_dec_error++;
 			per_rx->psdu_dec_error++;
+		} else if (error == MCPS802154_RX_ERROR_PHR_DECODE) {
+			per_rx->phr_dec_error++;
 		}
 		break;
 	case MCPS802154_RX_ERROR_TIMEOUT:
+		per_rx->rx_fail++;
 		break;
 	}
 }
@@ -314,10 +396,14 @@ static void pctt_rx_frame_rx(struct pctt_local *local, struct sk_buff *skb,
 			rx->rx_done_ts_int = (info->timestamp_rctu >> 32) &
 					     0xfffffffe;
 			rx->rx_done_ts_frac = info->timestamp_rctu & 0xffff;
-		} else
+		} else {
 			local->results.status =
 				PCTT_STATUS_RANGING_RX_PHY_TOA_FAILED;
-	}
+		}
+		if (info->flags & MCPS802154_RX_FRAME_INFO_RSSI)
+			rx->rssi = info->rssi;
+	} else
+		local->results.status = PCTT_STATUS_RANGING_RX_TIMEOUT;
 }
 
 static void pctt_rx_frame(struct mcps802154_access *access, int frame_idx,
@@ -327,13 +413,29 @@ static void pctt_rx_frame(struct mcps802154_access *access, int frame_idx,
 {
 	struct pctt_local *local = access_to_local(access);
 	struct pctt_session *session = &local->session;
+	struct llhw_vendor_cmd_pctt_get_frame_info frame_info = {};
 
 	local->frames_remaining_nb--;
 
+	if (error == MCPS802154_RX_ERROR_BAD_CKSUM) {
+		struct mcps802154_access_frame *frame =
+			&access->frames[frame_idx];
+		int r;
+
+		frame_info.info.flags = frame->rx.frame_info_flags_request;
+		r = mcps802154_vendor_cmd(local->llhw, VENDOR_QORVO_OUI,
+					  LLHW_VENDOR_CMD_PCTT_GET_FRAME_INFO,
+					  &frame_info, sizeof(frame_info));
+		if (!r) {
+			skb = frame_info.skb;
+			info = &frame_info.info;
+		}
+	}
+
 	if (session->cmd_id == PCTT_ID_ATTR_SS_TWR)
 		pctt_rx_frame_ss_twr(local, info);
 	else if (session->cmd_id == PCTT_ID_ATTR_PER_RX)
-		pctt_rx_frame_per_rx(local, info, error);
+		pctt_rx_frame_per_rx(local, skb, info, error);
 	else
 		pctt_rx_frame_rx(local, skb, info);
 
@@ -351,6 +453,7 @@ static void pctt_rx_frame(struct mcps802154_access *access, int frame_idx,
 	case MCPS802154_RX_ERROR_UNCORRECTABLE:
 	case MCPS802154_RX_ERROR_HPDWARN:
 	case MCPS802154_RX_ERROR_OTHER:
+	case MCPS802154_RX_ERROR_PHR_DECODE:
 		local->results.status = PCTT_STATUS_RANGING_RX_PHY_DEC_FAILED;
 		break;
 	}
@@ -371,6 +474,7 @@ static void pctt_access_done(struct mcps802154_access *access, bool error)
 		local->results.status = PCTT_STATUS_RANGING_INTERNAL_ERROR;
 
 	switch (session->cmd_id) {
+	case PCTT_ID_ATTR_LOOPBACK:
 	case PCTT_ID_ATTR_SS_TWR:
 	case PCTT_ID_ATTR_RX:
 		end_of_test = true;
@@ -389,7 +493,7 @@ static void pctt_access_done(struct mcps802154_access *access, bool error)
 		int r;
 
 		r = mcps802154_vendor_cmd(local->llhw, VENDOR_QORVO_OUI,
-					  DW3000_VENDOR_CMD_PCTT_SETUP_HW, NULL,
+					  LLHW_VENDOR_CMD_PCTT_SETUP_HW, NULL,
 					  0);
 
 		if (r)
@@ -415,10 +519,11 @@ static struct mcps802154_access *
 pctt_get_access_periodic_tx(struct pctt_local *local, u32 next_timestamp_dtu)
 {
 	struct pctt_session *session = &local->session;
-	const struct pctt_test_params *tp = &session->test_params;
+	const struct pctt_session_params *p = &session->params;
 	struct mcps802154_access *access = &local->access;
 	struct pctt_slot *s = local->slots;
 	u32 frame_dtu;
+	access->hrp_uwb_params = &session->hrp_uwb_params;
 
 	/* Unique frame in this access. */
 	*s = (struct pctt_slot){
@@ -437,7 +542,9 @@ pctt_get_access_periodic_tx(struct pctt_local *local, u32 next_timestamp_dtu)
 	access->frames = local->frames;
 	access->timestamp_dtu = frame_dtu;
 	/* Compute next transmit date. */
-	session->next_timestamp_dtu = frame_dtu + tp->gap_duration_dtu;
+	session->next_timestamp_dtu = frame_dtu + p->gap_duration_dtu;
+
+	pctt_randomize_psdu(local);
 
 	return access;
 }
@@ -445,24 +552,123 @@ pctt_get_access_periodic_tx(struct pctt_local *local, u32 next_timestamp_dtu)
 static struct mcps802154_access *
 pctt_get_access_per_rx(struct pctt_local *local, u32 next_timestamp_dtu)
 {
+	struct pctt_session *session = &local->session;
+	const struct pctt_session_params *p = &session->params;
 	struct mcps802154_access *access = &local->access;
 	struct pctt_slot *s = local->slots;
+	u32 frame_timestamp_dtu;
+	access->hrp_uwb_params = &session->hrp_uwb_params;
 
 	/* Unique frame in this access. */
 	*s = (struct pctt_slot){
-		.is_immediate = true,
-		.timeout_dtu = -1,
+		.is_immediate = !session->first_rx_synchronized,
+		.timeout_dtu = session->first_rx_synchronized ?
+				       2 * pctt_rx_margin(p->gap_duration_dtu) :
+				       -1,
 	};
 
-	pctt_access_setup_frame(local, s, next_timestamp_dtu, &local->frames[0],
-				&local->sts_params[0]);
+	frame_timestamp_dtu = session->first_rx_synchronized ?
+				      session->next_timestamp_dtu :
+				      next_timestamp_dtu;
+
+	pctt_access_setup_frame(local, s, frame_timestamp_dtu,
+				&local->frames[0], &local->sts_params[0]);
 
 	access->ops = &pctt_access_ops;
 	access->method = MCPS802154_ACCESS_METHOD_MULTI;
-	access->timestamp_dtu = next_timestamp_dtu;
-	access->duration_dtu = 0;
+	access->timestamp_dtu = frame_timestamp_dtu;
+	access->duration_dtu =
+		session->first_rx_synchronized ? p->gap_duration_dtu : 0;
 	access->n_frames = 1;
 	access->frames = local->frames;
+
+	return access;
+}
+
+static int pctt_handle_loopback(struct mcps802154_access *access)
+{
+	struct pctt_local *local = access_to_local(access);
+	struct pctt_session *session = &local->session;
+	const struct pctt_session_params *p = &session->params;
+	struct llhw_vendor_cmd_pctt_handle_loopback handle_loopback = {};
+
+	handle_loopback.ant_set_id = p->tx_antenna_selection;
+	handle_loopback.data_payload = p->data_payload;
+	handle_loopback.data_payload_len = p->data_payload_len;
+
+	return mcps802154_vendor_cmd(local->llhw, VENDOR_QORVO_OUI,
+				     LLHW_VENDOR_CMD_PCTT_HANDLE_LOOPBACK,
+				     &handle_loopback, sizeof(handle_loopback));
+}
+
+static int pctt_tx_done_loopback(struct mcps802154_access *access)
+{
+	struct pctt_local *local = access_to_local(access);
+	struct pctt_session *session = &local->session;
+	const struct pctt_session_params *p = &session->params;
+	struct llhw_vendor_cmd_pctt_get_loopback_info loopback_info = {};
+	int r;
+
+	r = mcps802154_vendor_cmd(local->llhw, VENDOR_QORVO_OUI,
+				  LLHW_VENDOR_CMD_PCTT_GET_LOOPBACK_INFO,
+				  &loopback_info, sizeof(loopback_info));
+	if (r)
+		return r;
+
+	local->results.status = loopback_info.success ?
+					PCTT_STATUS_RANGING_SUCCESS :
+					PCTT_STATUS_RANGING_TX_FAILED;
+
+	local->results.tests.loopback.rssi = loopback_info.rssi;
+
+	if (loopback_info.success) {
+		/* Compare data received with the one sent. */
+		struct sk_buff *rx_skb = loopback_info.skb;
+		WARN_RETURN_ON(!rx_skb, -EFAULT);
+
+		if ((rx_skb->len != p->data_payload_len) ||
+		    memcmp(rx_skb->data, p->data_payload, rx_skb->len)) {
+			local->results.status = PCTT_STATUS_RANGING_TX_FAILED;
+		}
+
+		/* Free rx_frame skb. */
+		kfree_skb(rx_skb);
+	}
+
+	local->results.tests.loopback.rx_ts_int =
+		(u32)(loopback_info.rx_timestamp_rctu >> PCTT_TIMESTAMP_SHIFT);
+	local->results.tests.loopback.rx_ts_frac =
+		(u16)(loopback_info.rx_timestamp_rctu &
+		      (((unsigned long)1 << PCTT_TIMESTAMP_SHIFT) - 1));
+	local->results.tests.loopback.tx_ts_int =
+		(u32)(loopback_info.tx_timestamp_rctu >> PCTT_TIMESTAMP_SHIFT);
+	local->results.tests.loopback.tx_ts_frac =
+		(u16)(loopback_info.tx_timestamp_rctu &
+		      (((unsigned long)1 << PCTT_TIMESTAMP_SHIFT) - 1));
+
+	/* Request end of current access. */
+	return 1;
+}
+
+struct mcps802154_access_vendor_ops pctt_access_ops_loopback = {
+	.common = {
+		.access_done = pctt_access_done,
+	},
+	.handle = pctt_handle_loopback,
+	.tx_done = pctt_tx_done_loopback,
+};
+
+static struct mcps802154_access *
+pctt_get_access_loopback(struct pctt_local *local, u32 next_timestamp_dtu)
+{
+	struct mcps802154_access *access = &local->access;
+
+	access->method = MCPS802154_ACCESS_METHOD_VENDOR;
+	access->vendor_ops = &pctt_access_ops_loopback;
+	access->duration_dtu = 0;
+	access->timestamp_dtu = next_timestamp_dtu;
+	access->n_frames = 0;
+	access->frames = NULL;
 	return access;
 }
 
@@ -477,6 +683,7 @@ pctt_get_access_ss_twr(struct pctt_local *local, u32 next_timestamp_dtu)
 	int nb_frames;
 	u32 frame_dtu;
 	int i;
+	access->hrp_uwb_params = &session->hrp_uwb_params;
 
 	/* First frames. */
 	*s = (struct pctt_slot){
@@ -497,6 +704,7 @@ pctt_get_access_ss_twr(struct pctt_local *local, u32 next_timestamp_dtu)
 		ss_twr->tx_timestamps_rctu =
 			mcps802154_tx_timestamp_dtu_to_rmarker_rctu(
 				local->llhw, next_timestamp_dtu,
+				access->hrp_uwb_params, access->channel,
 				p->tx_antenna_selection);
 	}
 
@@ -513,10 +721,6 @@ pctt_get_access_ss_twr(struct pctt_local *local, u32 next_timestamp_dtu)
 		frame_dtu += p->slot_duration_dtu;
 	}
 
-	if (!local->frames[0].is_tx)
-		local->frames[0].rx.frame_info_flags_request |=
-			MCPS802154_RX_FRAME_INFO_TIMESTAMP_DTU;
-
 	access->method = MCPS802154_ACCESS_METHOD_MULTI;
 	access->ops = &pctt_access_ops;
 	access->timestamp_dtu = next_timestamp_dtu;
@@ -547,6 +751,9 @@ struct mcps802154_access *pctt_get_access(struct mcps802154_region *region,
 	case PCTT_ID_ATTR_RX:
 		access = pctt_get_access_per_rx(local, next_timestamp_dtu);
 		break;
+	case PCTT_ID_ATTR_LOOPBACK:
+		access = pctt_get_access_loopback(local, next_timestamp_dtu);
+		break;
 	case PCTT_ID_ATTR_SS_TWR:
 		access = pctt_get_access_ss_twr(local, next_timestamp_dtu);
 		break;
@@ -562,7 +769,7 @@ struct mcps802154_access *pctt_get_access(struct mcps802154_region *region,
 		int r;
 
 		r = mcps802154_vendor_cmd(local->llhw, VENDOR_QORVO_OUI,
-					  DW3000_VENDOR_CMD_PCTT_SETUP_HW,
+					  LLHW_VENDOR_CMD_PCTT_SETUP_HW,
 					  &session->setup_hw,
 					  sizeof(session->setup_hw));
 		if (r) {
diff --git a/mac/pctt_region.c b/mac/pctt_region.c
index b6f9699..4c11f5b 100644
--- a/mac/pctt_region.c
+++ b/mac/pctt_region.c
@@ -70,7 +70,6 @@ static int pctt_call(struct mcps802154_region *region, u32 call_id,
 	case PCTT_CALL_SESSION_GET_PARAMS:
 		return pctt_call_session_get_params(local);
 	case PCTT_CALL_SESSION_SET_PARAMS:
-	case PCTT_CALL_SET_PARAMS:
 	case PCTT_CALL_SESSION_CMD:
 		return pctt_call_session_control(local, call_id, attrs, info);
 	default:
@@ -117,6 +116,7 @@ static int pctt_report_per_rx(struct pctt_local *local, struct sk_buff *msg)
 	P(PSDU_BIT_ERROR, u32, per_rx->psdu_bit_error);
 	P(STS_FOUND, u32, per_rx->sts_found);
 	P(EOF, u32, per_rx->eof);
+	P(RSSI, u8, per_rx->rssi);
 #undef P
 	return 0;
 
@@ -144,6 +144,7 @@ static int pctt_report_rx(struct pctt_local *local, struct sk_buff *msg)
 	P(AOA_ELEVATION, s16, rx->aoa_elevation);
 	P(TOA_GAP, u8, rx->toa_gap);
 	P(PHR, u16, rx->phr);
+	P(RSSI, u8, rx->rssi);
 	P(PSDU_DATA_LEN, u16, rx->psdu_data_len);
 	if (rx->psdu_data_len > 0 &&
 	    nla_put(msg, PCTT_RESULT_DATA_ATTR_PSDU_DATA, rx->psdu_data_len,
@@ -156,6 +157,45 @@ nla_put_failure:
 	return -EMSGSIZE;
 }
 
+static int pctt_report_loopback(struct pctt_local *local, struct sk_buff *msg)
+{
+	struct pctt_session *session = &local->session;
+	const struct pctt_session_params *p = &session->params;
+	trace_region_pctt_report_loopback(local->results.status);
+
+#define P(attr, type, value)                                          \
+	do {                                                          \
+		if (nla_put_##type(msg, PCTT_RESULT_DATA_ATTR_##attr, \
+				   value)) {                          \
+			goto nla_put_failure;                         \
+		}                                                     \
+	} while (0)
+
+	P(STATUS, u8, local->results.status);
+	P(RSSI, u8, local->results.tests.loopback.rssi);
+	P(RX_TS_INT, u32, local->results.tests.loopback.rx_ts_int);
+	P(RX_TS_FRAC, u16, local->results.tests.loopback.rx_ts_frac);
+	P(TX_TS_INT, u32, local->results.tests.loopback.tx_ts_int);
+	P(TX_TS_FRAC, u16, local->results.tests.loopback.tx_ts_frac);
+
+	/* If test succeeded, return data that was sent (and received) as
+	* PSDU payload. */
+	if (!local->results.status) {
+		P(PSDU_DATA_LEN, u16, p->data_payload_len);
+		if (nla_put(msg, PCTT_RESULT_DATA_ATTR_PSDU_DATA,
+			    p->data_payload_len, p->data_payload)) {
+			goto nla_put_failure;
+		}
+	} else {
+		P(PSDU_DATA_LEN, u16, 0);
+	}
+#undef P
+	return 0;
+
+nla_put_failure:
+	return -EMSGSIZE;
+}
+
 static int pctt_report_ss_twr(struct pctt_local *local, struct sk_buff *msg)
 {
 	const struct pctt_test_ss_twr_results *ss_twr =
@@ -171,6 +211,11 @@ static int pctt_report_ss_twr(struct pctt_local *local, struct sk_buff *msg)
 	} while (0)
 	P(STATUS, u8, local->results.status);
 	P(MEASUREMENT, u32, ss_twr->measurement_rctu);
+	P(PDOA_AZIMUTH_DEG_Q7, s16, ss_twr->pdoa_azimuth_deg_q7);
+	P(PDOA_ELEVATION_DEG_Q7, s16, ss_twr->pdoa_elevation_deg_q7);
+	P(AOA_AZIMUTH_DEG_Q7, s16, ss_twr->aoa_azimuth_deg_q7);
+	P(AOA_ELEVATION_DEG_Q7, s16, ss_twr->aoa_elevation_deg_q7);
+	P(RSSI, u8, ss_twr->rssi);
 #undef P
 	return 0;
 
@@ -211,6 +256,10 @@ void pctt_report(struct pctt_local *local)
 		if (pctt_report_rx(local, msg))
 			goto nla_put_failure;
 		break;
+	case PCTT_ID_ATTR_LOOPBACK:
+		if (pctt_report_loopback(local, msg))
+			goto nla_put_failure;
+		break;
 	case PCTT_ID_ATTR_SS_TWR:
 		if (pctt_report_ss_twr(local, msg))
 			goto nla_put_failure;
diff --git a/mac/pctt_region.h b/mac/pctt_region.h
index 66ce610..8e7830c 100644
--- a/mac/pctt_region.h
+++ b/mac/pctt_region.h
@@ -34,10 +34,22 @@
 #include "pctt_session.h"
 
 #define PCTT_SESSION_ID 0
-#define PCTT_PAYLOAD_MAX_LEN 4096
 #define PCTT_BOOLEAN_MAX 1
 #define PCTT_FRAMES_MAX 2
 
+#define PCTT_TIMESTAMP_SHIFT 9
+/**
+ * map_rad_q11_to_deg_q7() - Map a Fixed Point angle to a signed 16-bit integer
+ * @ang_rad_q11: angle as Q11 fixed_point value in range [-PI, PI]
+ *
+ * Return: the angle mapped to deg q7
+ */
+static inline s16 map_rad_q11_to_deg_q7(int ang_rad_q11)
+{
+	/* 180 / (pi * (1 << 4)) => ~3,581. */
+	return ang_rad_q11 * 3581 / 1000;
+}
+
 /**
  * struct pctt_test_per_rx_results - PER_RX result for report.
  */
@@ -94,6 +106,10 @@ struct pctt_test_per_rx_results {
 	 * @eof: No. of times end of frame event was triggered.
 	 */
 	u32 eof;
+	/**
+	 * @rssi: Received signal strength indication (RSSI).
+	 */
+	u8 rssi;
 };
 
 /**
@@ -117,10 +133,6 @@ struct pctt_test_rx_results {
 	 */
 	s16 aoa_elevation;
 	/**
-	 * @toa_gap: ToA of main path minus ToA of first path in nanosecond.
-	 */
-	u8 toa_gap;
-	/**
 	 * @phr: Received PHR (bits 0-12 as per IEEE spec).
 	 */
 	u16 phr;
@@ -129,6 +141,14 @@ struct pctt_test_rx_results {
 	 */
 	u16 psdu_data_len;
 	/**
+	 * @toa_gap: ToA of main path minus ToA of first path in nanosecond.
+	 */
+	u8 toa_gap;
+	/**
+	 * @rssi: Received signal strength indication (RSSI).
+	 */
+	u8 rssi;
+	/**
 	 * @psdu_data: Received PSDU Data[0:N] bytes.
 	 */
 	u8 psdu_data[PCTT_PAYLOAD_MAX_LEN];
@@ -151,6 +171,52 @@ struct pctt_test_ss_twr_results {
 	 * Treply time of Responder depending on DEVICE_ROLE option.
 	 */
 	u32 measurement_rctu;
+	/**
+	 * @pdoa_azimuth_deg_q7: Phase Difference of Arrival Azimuth in deg Q7
+	 */
+	s16 pdoa_azimuth_deg_q7;
+	/**
+	 * @aoa_azimuth_deg_q7: AoA Azimuth in deg Q7
+	 */
+	s16 aoa_azimuth_deg_q7;
+	/**
+	 * @pdoa_elevation_deg_q7: Phase Difference of Arrival Elevation in deg Q7
+	 */
+	s16 pdoa_elevation_deg_q7;
+	/**
+	 * @aoa_elevation_deg_q7: AoA Elevation in deg Q7
+	 */
+	s16 aoa_elevation_deg_q7;
+	/**
+	 * @rssi: Received signal strength indication (RSSI).
+	 */
+	u8 rssi;
+};
+
+/**
+ * struct pctt_test_loopback_results - LOOPBACK result for report.
+ */
+struct pctt_test_loopback_results {
+	/**
+	 * @rssi: Received signal strength indication (RSSI).
+	 */
+	u8 rssi;
+	/**
+	 * @tx_ts_int: Integer part of TX timestamp in 1/124.8 us. resolution.
+	 */
+	u32 tx_ts_int;
+	/**
+	 * @tx_ts_frac: Fractional part of TX timestamp in 1/124.8/512 us. resolution.
+	 */
+	u16 tx_ts_frac;
+	/**
+	 * @rx_ts_int: Integer part of Rx timestamp in 1/124.8 us. resolution.
+	 */
+	u32 rx_ts_int;
+	/**
+	 * @rx_ts_frac: Fractional part of RX timestamp in 1/124.8/512 us. resolution.
+	 */
+	u16 rx_ts_frac;
 };
 
 /**
@@ -169,6 +235,10 @@ union pctt_tests_results {
 	 * @ss_twr: Result of the SS_TWR command.
 	 */
 	struct pctt_test_ss_twr_results ss_twr;
+	/**
+	 * @loopback: Result of the LOOPBACK command.
+	 */
+	struct pctt_test_loopback_results loopback;
 };
 
 /**
@@ -202,7 +272,7 @@ struct pctt_slot {
 	 */
 	bool is_immediate;
 	/**
-	 * @timeout_dtu: see (mcps802154_rx_info).timeout_dtu.
+	 * @timeout_dtu: see (mcps802154_rx_frame_config).timeout_dtu.
 	 */
 	int timeout_dtu;
 };
@@ -244,14 +314,6 @@ struct pctt_local {
 	 * @frames_remaining_nb: Number of frame remaining to do for the current test.
 	 */
 	int frames_remaining_nb;
-	/**
-	 * @data_payload: Data to put in TX test frame.
-	 */
-	u8 data_payload[PCTT_PAYLOAD_MAX_LEN];
-	/**
-	 * @data_payload_len: Length of data to put in TX test frame.
-	 */
-	int data_payload_len;
 };
 
 static inline struct pctt_local *
diff --git a/mac/pctt_region_call.c b/mac/pctt_region_call.c
index 252e03c..65b60f3 100644
--- a/mac/pctt_region_call.c
+++ b/mac/pctt_region_call.c
@@ -34,27 +34,13 @@
 #include "pctt_trace.h"
 
 static const struct nla_policy pctt_call_nla_policy[PCTT_CALL_ATTR_MAX + 1] = {
-	[PCTT_CALL_ATTR_PARAMS] = { .type = NLA_NESTED },
 	[PCTT_CALL_ATTR_CMD_ID] = { .type = NLA_U8 },
-	[PCTT_CALL_ATTR_CMD_PARAMS] = { .type = NLA_NESTED },
 	[PCTT_CALL_ATTR_RESULT_DATA] = { .type = NLA_NESTED },
 	[PCTT_CALL_ATTR_SESSION_ID] = { .type = NLA_U32 },
 	[PCTT_CALL_ATTR_SESSION_STATE] = { .type = NLA_U8 },
 	[PCTT_CALL_ATTR_SESSION_PARAMS] = { .type = NLA_NESTED },
 };
 
-static const struct nla_policy pctt_param_nla_policy[PCTT_PARAM_ATTR_MAX + 1] = {
-	[PCTT_PARAM_ATTR_NUM_PACKETS] = { .type = NLA_U32 },
-	[PCTT_PARAM_ATTR_T_GAP] = { .type = NLA_U32 },
-	[PCTT_PARAM_ATTR_T_START] = { .type = NLA_U32 },
-	[PCTT_PARAM_ATTR_T_WIN] = { .type = NLA_U32 },
-	[PCTT_PARAM_ATTR_RANDOMIZE_PSDU] = { .type = NLA_U8 },
-	[PCTT_PARAM_ATTR_PHR_RANGING_BIT] = { .type = NLA_U8 },
-	[PCTT_PARAM_ATTR_RMARKER_TX_START] = { .type = NLA_U32 },
-	[PCTT_PARAM_ATTR_RMARKER_RX_START] = { .type = NLA_U32 },
-	[PCTT_PARAM_ATTR_STS_INDEX_AUTO_INCR] = { .type = NLA_U8 },
-};
-
 static const struct nla_policy pctt_session_param_nla_policy[PCTT_SESSION_PARAM_ATTR_MAX +
 							     1] = {
 	[PCTT_SESSION_PARAM_ATTR_DEVICE_ROLE] = {
@@ -74,7 +60,7 @@ static const struct nla_policy pctt_session_param_nla_policy[PCTT_SESSION_PARAM_
 	},
 	[PCTT_SESSION_PARAM_ATTR_PRF_MODE] = {
 		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = PCTT_PRF_MODE_HPRF,
+		.max = PCTT_PRF_MODE_HPRF_HIGH_RATE,
 	},
 	[PCTT_SESSION_PARAM_ATTR_PREAMBLE_DURATION] = {
 		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
@@ -86,12 +72,16 @@ static const struct nla_policy pctt_session_param_nla_policy[PCTT_SESSION_PARAM_
 	},
 	[PCTT_SESSION_PARAM_ATTR_NUMBER_OF_STS_SEGMENTS] = {
 		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
-		.max = PCTT_NUMBER_OF_STS_SEGMENTS_2_SEGMENTS,
+		.max = PCTT_NUMBER_OF_STS_SEGMENTS_4_SEGMENTS,
 	},
 	[PCTT_SESSION_PARAM_ATTR_PSDU_DATA_RATE] = {
 		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
 		.max = PCTT_PSDU_DATA_RATE_31M2,
 	},
+	[PCTT_SESSION_PARAM_ATTR_BPRF_PHR_DATA_RATE] = {
+		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
+		.max = PCTT_PHR_DATA_RATE_6M81,
+	},
 	[PCTT_SESSION_PARAM_ATTR_MAC_FCS_TYPE] = {
 		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
 		.max = PCTT_MAC_FCS_TYPE_CRC_32,
@@ -101,57 +91,25 @@ static const struct nla_policy pctt_session_param_nla_policy[PCTT_SESSION_PARAM_
 		.max = PCTT_BOOLEAN_MAX,
 	},
 	[PCTT_SESSION_PARAM_ATTR_STS_INDEX] = { .type = NLA_U32 },
+	[PCTT_SESSION_PARAM_ATTR_STS_LENGTH] = {
+		.type = NLA_U8, .validation_type = NLA_VALIDATE_MAX,
+		.max = PCTT_STS_LENGTH_128,
+	},
+	[PCTT_SESSION_PARAM_ATTR_NUM_PACKETS] = { .type = NLA_U32 },
+	[PCTT_SESSION_PARAM_ATTR_T_GAP] = { .type = NLA_U32 },
+	[PCTT_SESSION_PARAM_ATTR_T_START] = { .type = NLA_U32 },
+	[PCTT_SESSION_PARAM_ATTR_T_WIN] = { .type = NLA_U32 },
+	[PCTT_SESSION_PARAM_ATTR_RANDOMIZE_PSDU] = { .type = NLA_U8 },
+	[PCTT_SESSION_PARAM_ATTR_PHR_RANGING_BIT] = { .type = NLA_U8 },
+	[PCTT_SESSION_PARAM_ATTR_RMARKER_TX_START] = { .type = NLA_U32 },
+	[PCTT_SESSION_PARAM_ATTR_RMARKER_RX_START] = { .type = NLA_U32 },
+	[PCTT_SESSION_PARAM_ATTR_STS_INDEX_AUTO_INCR] = { .type = NLA_U8 },
+	[PCTT_SESSION_PARAM_ATTR_DATA_PAYLOAD] = {
+		.type = NLA_BINARY,
+		.len = PCTT_PAYLOAD_MAX_LEN
+	},
 };
 
-static const struct nla_policy
-	pctt_test_param_nla_policy[PCTT_TEST_PARAM_ATTR_MAX + 1] = {
-		[PCTT_TEST_PARAM_ATTR_PAYLOAD] = { .type = NLA_BINARY,
-						   .len = PCTT_PAYLOAD_MAX_LEN },
-	};
-
-static int pctt_call_set_params(struct pctt_local *local,
-				const struct nlattr *params,
-				const struct genl_info *info)
-{
-	struct pctt_session *session = &local->session;
-	struct nlattr *attrs[PCTT_PARAM_ATTR_MAX + 1];
-	struct pctt_test_params *tp = &session->test_params;
-	int r;
-
-	if (!params)
-		return -EINVAL;
-	if (session->test_on_going)
-		return -EBUSY;
-
-	r = nla_parse_nested(attrs, PCTT_PARAM_ATTR_MAX, params,
-			     pctt_param_nla_policy, info->extack);
-	if (r)
-		return r;
-
-#define P(attr, member, type, conv)                                        \
-	do {                                                               \
-		int x;                                                     \
-		if (attrs[PCTT_PARAM_ATTR_##attr]) {                       \
-			x = nla_get_##type(attrs[PCTT_PARAM_ATTR_##attr]); \
-			tp->member = conv;                                 \
-		}                                                          \
-	} while (0)
-
-	P(NUM_PACKETS, num_packets, u32, x);
-	P(T_GAP, gap_duration_dtu, u32,
-	  ((u64)x * (local->llhw->dtu_freq_hz / 1000)) / 1000);
-	P(T_START, t_start, u32, x);
-	P(T_WIN, t_win, u32, x);
-	P(RANDOMIZE_PSDU, randomize_psdu, u8, x);
-	P(PHR_RANGING_BIT, phr_ranging_bit, u8, x);
-	P(RMARKER_TX_START, rmarker_tx_start, u32, x);
-	P(RMARKER_RX_START, rmarker_rx_start, u32, x);
-	P(STS_INDEX_AUTO_INCR, sts_index_auto_incr, u8, x);
-#undef P
-
-	return 0;
-}
-
 int pctt_call_session_get_state(struct pctt_local *local)
 {
 	struct pctt_session *session = &local->session;
@@ -212,14 +170,26 @@ int pctt_call_session_get_params(struct pctt_local *local)
 	P(CHANNEL_NUMBER, channel_number, u8, x);
 	P(PREAMBLE_CODE_INDEX, preamble_code_index, u8, x);
 	P(RFRAME_CONFIG, rframe_config, u8, x);
-	P(PRF_MODE, prf_mode, u8, x);
 	P(PREAMBLE_DURATION, preamble_duration, u8, x);
 	P(SFD_ID, sfd_id, u8, x);
 	P(NUMBER_OF_STS_SEGMENTS, number_of_sts_segments, u8, x);
 	P(PSDU_DATA_RATE, psdu_data_rate, u8, x);
 	P(MAC_FCS_TYPE, mac_fcs_type, u8, x);
+	P(PRF_MODE, prf_mode, u8, x);
+	P(BPRF_PHR_DATA_RATE, phr_data_rate, u8, x);
 	P(TX_ADAPTIVE_PAYLOAD_POWER, tx_adaptive_payload_power, u8, x);
 	P(STS_INDEX, sts_index, u32, x);
+	P(STS_LENGTH, sts_length, u8, x);
+	P(NUM_PACKETS, num_packets, u32, x);
+	P(T_GAP, gap_duration_dtu, u32,
+	  (((u64)x * 1000) / (local->llhw->dtu_freq_hz / 1000)));
+	P(T_START, t_start, u32, x);
+	P(T_WIN, t_win, u32, x);
+	P(RANDOMIZE_PSDU, randomize_psdu, u8, x);
+	P(PHR_RANGING_BIT, phr_ranging_bit, u8, x);
+	P(RMARKER_TX_START, rmarker_tx_start, u32, x);
+	P(RMARKER_RX_START, rmarker_rx_start, u32, x);
+	P(STS_INDEX_AUTO_INCR, sts_index_auto_incr, u8, x);
 #undef P
 	nla_nest_end(msg, params);
 
@@ -257,6 +227,17 @@ static int pctt_call_session_set_params(struct pctt_local *local,
 			p->member = conv;                               \
 		}                                                       \
 	} while (0)
+#define PMEMNCPY(attr, member, size)                                   \
+	do {                                                           \
+		if (attrs[PCTT_SESSION_PARAM_ATTR_##attr]) {           \
+			struct nlattr *attr =                          \
+				attrs[PCTT_SESSION_PARAM_ATTR_##attr]; \
+			int len = nla_len(attr);                       \
+			memcpy(p->member, nla_data(attr), len);        \
+			p->size = len;                                 \
+		}                                                      \
+	} while (0)
+
 	P(DEVICE_ROLE, device_role, u8, x);
 	P(SHORT_ADDR, short_addr, u16, x);
 	P(DESTINATION_SHORT_ADDR, dst_short_addr, u16, x);
@@ -267,62 +248,35 @@ static int pctt_call_session_set_params(struct pctt_local *local,
 	P(CHANNEL_NUMBER, channel_number, u8, x);
 	P(PREAMBLE_CODE_INDEX, preamble_code_index, u8, x);
 	P(RFRAME_CONFIG, rframe_config, u8, x);
-	P(PRF_MODE, prf_mode, u8, x);
 	P(PREAMBLE_DURATION, preamble_duration, u8, x);
 	P(SFD_ID, sfd_id, u8, x);
 	P(NUMBER_OF_STS_SEGMENTS, number_of_sts_segments, u8, x);
 	P(PSDU_DATA_RATE, psdu_data_rate, u8, x);
 	P(MAC_FCS_TYPE, mac_fcs_type, u8, x);
+	P(PRF_MODE, prf_mode, u8, x);
+	P(BPRF_PHR_DATA_RATE, phr_data_rate, u8, x);
 	P(TX_ADAPTIVE_PAYLOAD_POWER, tx_adaptive_payload_power, u8, x);
 	P(STS_INDEX, sts_index, u32, x);
+	P(STS_LENGTH, sts_length, u8, x);
+	P(NUM_PACKETS, num_packets, u32, x);
+	P(T_GAP, gap_duration_dtu, u32,
+	  ((u64)x * (local->llhw->dtu_freq_hz / 1000)) / 1000);
+	P(T_START, t_start, u32, x);
+	P(T_WIN, t_win, u32, x);
+	P(RANDOMIZE_PSDU, randomize_psdu, u8, x);
+	P(PHR_RANGING_BIT, phr_ranging_bit, u8, x);
+	P(RMARKER_TX_START, rmarker_tx_start, u32, x);
+	P(RMARKER_RX_START, rmarker_rx_start, u32, x);
+	P(STS_INDEX_AUTO_INCR, sts_index_auto_incr, u8, x);
+	PMEMNCPY(DATA_PAYLOAD, data_payload, data_payload_len);
+#undef PMEMNCPY
 #undef P
 
 	return 0;
 }
 
-static int pctt_session_set_test_params(struct pctt_local *local,
-					const struct nlattr *cmd_params_attr,
-					const struct genl_info *info)
-{
-	struct pctt_session *session = &local->session;
-	const struct pctt_session_params *p = &session->params;
-	struct nlattr *attrs[PCTT_TEST_PARAM_ATTR_MAX + 1];
-	struct nlattr *attr;
-	int r;
-
-	/* Test parameters are not mandatory. */
-	if (!cmd_params_attr)
-		return 0;
-
-	r = nla_parse_nested(attrs, PCTT_TEST_PARAM_ATTR_MAX, cmd_params_attr,
-			     pctt_test_param_nla_policy, info->extack);
-	if (r)
-		return r;
-
-	local->data_payload_len = 0;
-
-	attr = attrs[PCTT_TEST_PARAM_ATTR_PAYLOAD];
-	if (attr) {
-		int len = nla_len(attr);
-		const char *data = nla_data(attr);
-
-		if (local->llhw->hw->flags & IEEE802154_HW_TX_OMIT_CKSUM)
-			len -= IEEE802154_FCS_LEN;
-
-		if (len > 0) {
-			if (p->rframe_config == PCTT_RFRAME_CONFIG_SP3)
-				return -EINVAL;
-
-			memcpy(local->data_payload, data, len);
-			local->data_payload_len = len;
-		}
-	}
-	return 0;
-}
-
 static int pctt_call_cmd(struct pctt_local *local,
 			 const struct nlattr *cmd_id_attr,
-			 const struct nlattr *cmd_params_attr,
 			 const struct genl_info *info)
 {
 	struct pctt_session *session = &local->session;
@@ -333,8 +287,10 @@ static int pctt_call_cmd(struct pctt_local *local,
 	cmd_id = nla_get_u8(cmd_id_attr);
 
 	if (session->test_on_going) {
-		if (cmd_id == PCTT_ID_ATTR_STOP_TEST) {
+		if (cmd_id == PCTT_ID_ATTR_STOP_TEST &&
+		    !session->stop_request) {
 			session->stop_request = true;
+			mcps802154_reschedule(local->llhw);
 			return 0;
 		}
 		return -EBUSY;
@@ -344,9 +300,7 @@ static int pctt_call_cmd(struct pctt_local *local,
 
 		if (cmd_id == PCTT_ID_ATTR_STOP_TEST)
 			return 0;
-		r = pctt_session_set_test_params(local, cmd_params_attr, info);
-		if (r)
-			return r;
+
 		/* FIXME: Used only to detect dw3000_is_active. */
 		r = mcps802154_get_current_timestamp_dtu(local->llhw, &now_dtu);
 		if (r)
@@ -377,13 +331,9 @@ int pctt_call_session_control(struct pctt_local *local, enum pctt_call call_id,
 	if (r)
 		return r;
 
-	if (call_id == PCTT_CALL_SET_PARAMS)
-		return pctt_call_set_params(local, attrs[PCTT_CALL_ATTR_PARAMS],
-					    info);
-	else if (call_id == PCTT_CALL_SESSION_SET_PARAMS)
+	if (call_id == PCTT_CALL_SESSION_SET_PARAMS)
 		return pctt_call_session_set_params(
 			local, attrs[PCTT_CALL_ATTR_SESSION_PARAMS], info);
 	else
-		return pctt_call_cmd(local, attrs[PCTT_CALL_ATTR_CMD_ID],
-				     attrs[PCTT_CALL_ATTR_CMD_PARAMS], info);
+		return pctt_call_cmd(local, attrs[PCTT_CALL_ATTR_CMD_ID], info);
 }
diff --git a/mac/pctt_session.c b/mac/pctt_session.c
index 77bb53a..70beaf1 100644
--- a/mac/pctt_session.c
+++ b/mac/pctt_session.c
@@ -34,8 +34,8 @@ int pctt_session_init(struct pctt_local *local)
 	struct pctt_session *session = &local->session;
 	struct pctt_session_params *p = &session->params;
 
-	/* Do the same behavior as get_session_state in fira.
-	 * INIT state means kzalloc in fira once by session_id.
+	/* Do the same behavior as get_session_state in FiRa.
+	 * INIT state means kzalloc in FiRa once by session_id.
 	 * But as pctt have only one static region. Simulate
 	 * kzalloc to do or already done with the local state. */
 	if (session->state != PCTT_SESSION_STATE_DEINIT)
@@ -44,6 +44,9 @@ int pctt_session_init(struct pctt_local *local)
 	memset(p, 0, sizeof(*p));
 	p->rx_antenna_selection = RX_ANT_SET_ID_DEFAULT;
 	p->tx_antenna_selection = TX_ANT_SET_ID_DEFAULT;
+	p->preamble_duration = PCTT_PREAMBLE_DURATION_64;
+	p->preamble_code_index = 9;
+	p->sts_length = PCTT_STS_LENGTH_64;
 	pctt_session_set_state(local, PCTT_SESSION_STATE_INIT);
 	return 0;
 }
@@ -52,8 +55,8 @@ int pctt_session_deinit(struct pctt_local *local)
 {
 	struct pctt_session *session = &local->session;
 
-	/* Do the same behavior as get_session_state in fira.
-	 * DEINIT state means kfree in fira.
+	/* Do the same behavior as get_session_state in FiRa.
+	 * DEINIT state means kfree in FiRa.
 	 * But as pctt have only one static region. Simulate
 	 * kfree to do or already done with the local state. */
 	if (session->state == PCTT_SESSION_STATE_DEINIT)
@@ -81,6 +84,12 @@ int pctt_session_start_test(struct pctt_local *local, enum pctt_id_attrs cmd_id,
 {
 	struct pctt_session *session = &local->session;
 	const struct pctt_session_params *p = &session->params;
+	static const enum mcps802154_data_rate pctt_rate_to_mcps_rate[] = {
+		MCPS802154_DATA_RATE_6M81,
+		MCPS802154_DATA_RATE_7M80,
+		MCPS802154_DATA_RATE_27M2,
+		MCPS802154_DATA_RATE_31M2,
+	};
 
 	trace_region_pctt_session_start_test(cmd_id, p);
 
@@ -88,8 +97,7 @@ int pctt_session_start_test(struct pctt_local *local, enum pctt_id_attrs cmd_id,
 	case PCTT_ID_ATTR_PERIODIC_TX:
 		break;
 	case PCTT_ID_ATTR_LOOPBACK:
-		/* Not implemented. */
-		return -EINVAL;
+		break;
 	case PCTT_ID_ATTR_SS_TWR:
 		if (p->rframe_config != PCTT_RFRAME_CONFIG_SP3)
 			return -EINVAL;
@@ -104,14 +112,70 @@ int pctt_session_start_test(struct pctt_local *local, enum pctt_id_attrs cmd_id,
 		return -EINVAL;
 	}
 
+	/* check uwb parameters. */
+	if (p->prf_mode == PCTT_PRF_MODE_BPRF) {
+		if (p->preamble_code_index < 9 || p->preamble_code_index > 24)
+			return -EINVAL;
+		if (p->sfd_id != PCTT_SFD_ID_0 && p->sfd_id != PCTT_SFD_ID_2)
+			return -EINVAL;
+		if (p->psdu_data_rate != PCTT_PSDU_DATA_RATE_6M81)
+			return -EINVAL;
+		if (p->preamble_duration != PCTT_PREAMBLE_DURATION_64)
+			return -EINVAL;
+		if (p->number_of_sts_segments >
+		    PCTT_NUMBER_OF_STS_SEGMENTS_1_SEGMENT)
+			return -EINVAL;
+	} else {
+		if (p->preamble_code_index < 25 || p->preamble_code_index > 32)
+			return -EINVAL;
+		if (p->sfd_id == PCTT_SFD_ID_0)
+			return -EINVAL;
+		if (p->prf_mode == PCTT_PRF_MODE_HPRF &&
+		    p->psdu_data_rate > PCTT_PSDU_DATA_RATE_7M80)
+			return -EINVAL;
+		if (p->prf_mode == PCTT_PRF_MODE_HPRF_HIGH_RATE &&
+		    p->psdu_data_rate < PCTT_PSDU_DATA_RATE_27M2)
+			return -EINVAL;
+	}
+	if ((p->rframe_config == PCTT_RFRAME_CONFIG_SP0) &&
+	    (p->number_of_sts_segments != PCTT_NUMBER_OF_STS_SEGMENTS_NONE))
+		return -EINVAL;
+	if ((p->rframe_config != PCTT_RFRAME_CONFIG_SP0) &&
+	    (p->number_of_sts_segments == PCTT_NUMBER_OF_STS_SEGMENTS_NONE))
+		return -EINVAL;
+	if ((p->rframe_config == PCTT_RFRAME_CONFIG_SP3) &&
+	    (p->data_payload_len))
+		return -EINVAL;
+
+	/* Set radio parameters. */
+	switch (p->prf_mode) {
+	case PCTT_PRF_MODE_BPRF:
+		session->hrp_uwb_params.prf = MCPS802154_PRF_64;
+		break;
+	case PCTT_PRF_MODE_HPRF:
+		session->hrp_uwb_params.prf = MCPS802154_PRF_125;
+		break;
+	default:
+		session->hrp_uwb_params.prf = MCPS802154_PRF_250;
+	}
+	session->hrp_uwb_params.psr =
+		p->preamble_duration == PCTT_PREAMBLE_DURATION_64 ?
+			MCPS802154_PSR_64 :
+			MCPS802154_PSR_32;
+	session->hrp_uwb_params.sfd_selector = (enum mcps802154_sfd)(p->sfd_id);
+	session->hrp_uwb_params.phr_hi_rate = !!p->phr_data_rate;
+	session->hrp_uwb_params.data_rate =
+		pctt_rate_to_mcps_rate[p->psdu_data_rate];
+
 	/* Update unique session context. */
 	session->first_access = true;
+	session->first_rx_synchronized = false;
 	/* FIXME: Delete portid_set_once.
 	 * See: UWB-2057. */
 	session->portid_set_once = true;
 	session->event_portid = info->snd_portid;
 	/* Set parameters used by the PCTT vendor command. */
-	session->setup_hw = (struct dw3000_vendor_cmd_pctt_setup_hw){
+	session->setup_hw = (struct llhw_vendor_cmd_pctt_setup_hw){
 		.chan = p->channel_number,
 		.rframe_config = p->rframe_config,
 		.preamble_code_index = p->preamble_code_index,
@@ -121,7 +185,7 @@ int pctt_session_start_test(struct pctt_local *local, enum pctt_id_attrs cmd_id,
 	};
 	/* Update region context. */
 	memset(&local->results, 0, sizeof(local->results));
-	local->frames_remaining_nb = session->test_params.num_packets;
+	local->frames_remaining_nb = session->params.num_packets;
 	session->cmd_id = cmd_id;
 	session->test_on_going = true;
 	/* At the end, update the state. */
diff --git a/mac/pctt_session.h b/mac/pctt_session.h
index 4297df5..40058e4 100644
--- a/mac/pctt_session.h
+++ b/mac/pctt_session.h
@@ -30,6 +30,8 @@
 #include <net/pctt_region_params.h>
 #include <net/pctt_region_nl.h>
 
+#define PCTT_PAYLOAD_MAX_LEN 4096
+
 struct pctt_session_params {
 	enum pctt_device_role device_role;
 	__le16 short_addr;
@@ -40,17 +42,17 @@ struct pctt_session_params {
 	int channel_number;
 	int preamble_code_index;
 	enum pctt_rframe_config rframe_config;
-	enum pctt_prf_mode prf_mode;
 	enum pctt_preamble_duration preamble_duration;
 	enum pctt_sfd_id sfd_id;
 	enum pctt_number_of_sts_segments number_of_sts_segments;
 	enum pctt_psdu_data_rate psdu_data_rate;
 	enum pctt_mac_fcs_type mac_fcs_type;
+	enum pctt_prf_mode prf_mode;
+	enum pctt_phr_data_rate phr_data_rate;
 	u8 tx_adaptive_payload_power;
 	u32 sts_index;
-};
-
-struct pctt_test_params {
+	enum pctt_sts_length sts_length;
+	/* Test specific parameters */
 	u32 num_packets;
 	int gap_duration_dtu;
 	u32 t_start;
@@ -60,6 +62,9 @@ struct pctt_test_params {
 	u32 rmarker_tx_start;
 	u32 rmarker_rx_start;
 	u8 sts_index_auto_incr;
+	/* Data payload to put in TX test frame */
+	u8 data_payload[PCTT_PAYLOAD_MAX_LEN];
+	int data_payload_len;
 };
 
 /**
@@ -72,9 +77,10 @@ struct pctt_session {
 	 */
 	struct pctt_session_params params;
 	/**
-	 * @test_params: test parameters provided with the test id.
+	 * @hrp_uwb_params: HRP UWB parameters, read only while the session is
+	 * active.
 	 */
-	struct pctt_test_params test_params;
+	struct mcps802154_hrp_uwb_params hrp_uwb_params;
 	/**
 	 * @event_portid: Port identifier to use for notifications.
 	 */
@@ -90,17 +96,21 @@ struct pctt_session {
 	 */
 	bool first_access;
 	/**
+	 * @first_rx_synchronized: True after the first successful reception.
+	 */
+	bool first_rx_synchronized;
+	/**
 	 * @stop_request: True to not start an another access.
 	 */
 	bool stop_request;
 	/**
-	 * @next_timestamp_dtu: next date for PERIODIC_TX test.
+	 * @next_timestamp_dtu: next date for next frame.
 	 */
 	u32 next_timestamp_dtu;
 	/**
 	 * @setup_hw: setup hardware through a vendor command.
 	 */
-	struct dw3000_vendor_cmd_pctt_setup_hw setup_hw;
+	struct llhw_vendor_cmd_pctt_setup_hw setup_hw;
 	/**
 	 * @state: UWB session state.
 	 */
diff --git a/mac/pctt_trace.h b/mac/pctt_trace.h
index a2fe1bf..3cba8e7 100644
--- a/mac/pctt_trace.h
+++ b/mac/pctt_trace.h
@@ -66,11 +66,13 @@ TRACE_DEFINE_ENUM(PCTT_RFRAME_CONFIG_SP3);
 
 #define pctt_prf_mode_name(name)         \
 	{ PCTT_PRF_MODE_##name, #name }
-#define PCTT_PRF_MODE_SYMBOLS            \
-	pctt_prf_mode_name(BPRF),        \
-	pctt_prf_mode_name(HPRF)
+#define PCTT_PRF_MODE_SYMBOLS              \
+	pctt_prf_mode_name(BPRF),          \
+	pctt_prf_mode_name(HPRF),          \
+	pctt_prf_mode_name(HPRF_HIGH_RATE)
 TRACE_DEFINE_ENUM(PCTT_PRF_MODE_BPRF);
 TRACE_DEFINE_ENUM(PCTT_PRF_MODE_HPRF);
+TRACE_DEFINE_ENUM(PCTT_PRF_MODE_HPRF_HIGH_RATE);
 #define PCTT_PRF_MODE_ENTRY __field(enum pctt_prf_mode, prf_mode)
 #define PCTT_PRF_MODE_ASSIGN __entry->prf_mode = params->prf_mode
 #define PCTT_PRF_MODE_PR_FMT "prf_mode=%s"
@@ -117,10 +119,14 @@ TRACE_DEFINE_ENUM(PCTT_SFD_ID_4);
 #define PCTT_NUMBER_OF_STS_SEGMENTS_SYMBOLS            \
 	pctt_number_of_sts_segments_name(NONE),        \
 	pctt_number_of_sts_segments_name(1_SEGMENT),   \
-	pctt_number_of_sts_segments_name(2_SEGMENTS)
+	pctt_number_of_sts_segments_name(2_SEGMENTS),  \
+	pctt_number_of_sts_segments_name(3_SEGMENTS),  \
+	pctt_number_of_sts_segments_name(4_SEGMENTS)
 TRACE_DEFINE_ENUM(PCTT_NUMBER_OF_STS_SEGMENTS_NONE);
 TRACE_DEFINE_ENUM(PCTT_NUMBER_OF_STS_SEGMENTS_1_SEGMENT);
 TRACE_DEFINE_ENUM(PCTT_NUMBER_OF_STS_SEGMENTS_2_SEGMENTS);
+TRACE_DEFINE_ENUM(PCTT_NUMBER_OF_STS_SEGMENTS_3_SEGMENTS);
+TRACE_DEFINE_ENUM(PCTT_NUMBER_OF_STS_SEGMENTS_4_SEGMENTS);
 #define PCTT_NUMBER_OF_STS_SEGMENTS_ENTRY \
 	__field(enum pctt_number_of_sts_segments, number_of_sts_segments)
 #define PCTT_NUMBER_OF_STS_SEGMENTS_ASSIGN \
@@ -149,6 +155,21 @@ TRACE_DEFINE_ENUM(PCTT_PSDU_DATA_RATE_31M2);
 #define PCTT_PSDU_DATA_RATE_PR_ARG \
 	__print_symbolic(__entry->psdu_data_rate, PCTT_PSDU_DATA_RATE_SYMBOLS)
 
+#define pctt_phr_data_rate_name(name)         \
+	{ PCTT_PHR_DATA_RATE##name, #name }
+#define PCTT_PHR_DATA_RATE_SYMBOLS            \
+	pctt_phr_data_rate_name(850k),        \
+	pctt_phr_data_rate_name(6M81),        \
+TRACE_DEFINE_ENUM(PCTT_PHR_DATA_RATE_850k);
+TRACE_DEFINE_ENUM(PCTT_PHR_DATA_RATE_6M81);
+#define PCTT_PHR_DATA_RATE_ENTRY \
+	__field(enum pctt_phr_data_rate, phr_data_rate)
+#define PCTT_PHR_DATA_RATE_ASSIGN \
+	__entry->phr_data_rate = params->phr_data_rate
+#define PCTT_PHR_DATA_RATE_PR_FMT "phr_data_rate=%s"
+#define PCTT_PHR_DATA_RATE_PR_ARG \
+	__print_symbolic(__entry->phr_data_rate, PCTT_PHR_DATA_RATE_SYMBOLS)
+
 #define pctt_mac_fcs_type_name(name)         \
 	{ PCTT_MAC_FCS_TYPE_##name, #name }
 #define PCTT_MAC_FCS_TYPE_SYMBOLS            \
@@ -356,6 +377,7 @@ TRACE_EVENT(region_pctt_report_per_rx,
 		__field(u32, psdu_bit_error)
 		__field(u32, sts_found)
 		__field(u32, eof)
+		__field(u8, rssi)
 	    ),
 	    TP_fast_assign(
 		PCTT_STATUS_RANGING_ASSIGN;
@@ -372,20 +394,21 @@ TRACE_EVENT(region_pctt_report_per_rx,
 		__entry->psdu_bit_error = per_rx->psdu_bit_error;
 		__entry->sts_found = per_rx->sts_found;
 		__entry->eof = per_rx->eof;
+		__entry->rssi = per_rx->rssi;
 	    ),
 	    TP_printk(PCTT_STATUS_RANGING_PR_FMT " "
 		      "attempts=%u acq_detect=%u acq_reject=%u "
 		      "rx_fail=%u sync_cir_ready=%u sfd_fail=%u "
 		      "sfd_found=%u phr_dec_error=%u phr_bit_error=%u "
 		      "psdu_dec_error=%u psdu_bit_error=%u sts_found=%u "
-		      "eof=%u",
+		      "eof=%u rssi=%u ",
 		      PCTT_STATUS_RANGING_PR_ARG, __entry->attempts,
 		      __entry->acq_detect, __entry->acq_reject,
 		      __entry->rx_fail, __entry->sync_cir_ready,
 		      __entry->sfd_fail, __entry->sfd_found,
 		      __entry->phr_dec_error, __entry->phr_bit_error,
 		      __entry->psdu_dec_error, __entry->psdu_bit_error,
-		      __entry->sts_found, __entry->eof)
+		      __entry->sts_found, __entry->eof, __entry->rssi)
 );
 
 TRACE_EVENT(region_pctt_report_rx,
@@ -400,6 +423,7 @@ TRACE_EVENT(region_pctt_report_rx,
 		__field(s16, aoa_elevation)
 		__field(u8, toa_gap)
 		__field(u16, phr)
+		__field(u8, rssi)
 		__field(u16, psdu_data_len)
 		__dynamic_array(u8, psdu_data, rx->psdu_data_len)
 	    ),
@@ -411,6 +435,7 @@ TRACE_EVENT(region_pctt_report_rx,
 		__entry->aoa_elevation = rx->aoa_elevation;
 		__entry->toa_gap = rx->toa_gap;
 		__entry->phr = rx->phr;
+		__entry->rssi = rx->rssi;
 		__entry->psdu_data_len = rx->psdu_data_len;
 		memcpy(__get_dynamic_array(psdu_data), rx->psdu_data,
 		       __get_dynamic_array_len(psdu_data));
@@ -419,15 +444,28 @@ TRACE_EVENT(region_pctt_report_rx,
 	    TP_printk(PCTT_STATUS_RANGING_PR_FMT " "
 		      "rx_done_ts_int=%u rx_done_ts_frac=%u "
 		      "aoa_azimuth=%d aoa_elevation=%d toa_gap=%u "
-		      "phr=%u psdu_data_len=%u psdu_data=%s",
+		      "phr=%u rssi=%u  psdu_data_len=%u psdu_data=%s",
 		      PCTT_STATUS_RANGING_PR_ARG, __entry->rx_done_ts_int,
 		      __entry->rx_done_ts_frac, __entry->aoa_azimuth,
 		      __entry->aoa_elevation, __entry->toa_gap, __entry->phr,
-		      __entry->psdu_data_len,
+		      __entry->rssi, __entry->psdu_data_len,
 		      __print_hex(__get_dynamic_array(psdu_data),
 				  __get_dynamic_array_len(psdu_data)))
 );
 
+TRACE_EVENT(region_pctt_report_loopback,
+	    TP_PROTO(enum pctt_status_ranging status_ranging),
+	    TP_ARGS(status_ranging),
+	    TP_STRUCT__entry(
+		PCTT_STATUS_RANGING_ENTRY
+	    ),
+	    TP_fast_assign(
+		PCTT_STATUS_RANGING_ASSIGN;
+	    ),
+	    TP_printk(PCTT_STATUS_RANGING_PR_FMT,
+		      PCTT_STATUS_RANGING_PR_ARG)
+);
+
 TRACE_EVENT(region_pctt_report_ss_twr,
 	    TP_PROTO(enum pctt_status_ranging status_ranging,
 		     const struct pctt_test_ss_twr_results *ss_twr),
@@ -435,13 +473,27 @@ TRACE_EVENT(region_pctt_report_ss_twr,
 	    TP_STRUCT__entry(
 		PCTT_STATUS_RANGING_ENTRY
 		__field(u32, measurement_rctu)
+		__field(s16, pdoa_azimuth_deg_q7)
+		__field(s16, pdoa_elevation_deg_q7)
+		__field(u8, rssi)
+		__field(s16, aoa_azimuth_deg_q7)
+		__field(s16, aoa_elevation_deg_q7)
 	    ),
 	    TP_fast_assign(
 		PCTT_STATUS_RANGING_ASSIGN;
 		__entry->measurement_rctu = ss_twr->measurement_rctu;
+		__entry->pdoa_azimuth_deg_q7 = ss_twr->pdoa_azimuth_deg_q7;
+		__entry->pdoa_elevation_deg_q7 = ss_twr->pdoa_elevation_deg_q7;
+		__entry->rssi = ss_twr->rssi;
+		__entry->aoa_azimuth_deg_q7 = ss_twr->aoa_azimuth_deg_q7;
+		__entry->aoa_elevation_deg_q7 = ss_twr->aoa_elevation_deg_q7;
 	    ),
-	    TP_printk(PCTT_STATUS_RANGING_PR_FMT " measurement_rctu=%u",
-		      PCTT_STATUS_RANGING_PR_ARG, __entry->measurement_rctu)
+	    TP_printk(PCTT_STATUS_RANGING_PR_FMT " measurement_rctu=%u "
+				  "pdoa_azimuth_deg_q7=%u  pdoa_elevation_deg_q7=%u "
+				  "rssi=%u aoa_azimuth_deg_q7=%u aoa_elevation_deg_q7=%u",
+		      PCTT_STATUS_RANGING_PR_ARG, __entry->measurement_rctu,
+			  __entry->pdoa_azimuth_deg_q7, __entry->pdoa_elevation_deg_q7,
+			  __entry->rssi, __entry->aoa_azimuth_deg_q7, __entry->aoa_elevation_deg_q7)
 );
 
 TRACE_EVENT(region_pctt_report_nla_put_failure,
diff --git a/mac/regions.c b/mac/regions.c
index 98e3963..26ef8d8 100644
--- a/mac/regions.c
+++ b/mac/regions.c
@@ -30,6 +30,7 @@
 #include <linux/netdevice.h>
 
 #include "mcps802154_i.h"
+#include "trace.h"
 
 static LIST_HEAD(registered_regions);
 static DEFINE_MUTEX(registered_regions_lock);
@@ -125,11 +126,11 @@ EXPORT_SYMBOL_GPL(mcps802154_region_close);
 void mcps802154_region_notify_stop(struct mcps802154_llhw *llhw,
 				   struct mcps802154_region *region)
 {
-	const struct mcps802154_region_ops *ops;
+	if (!region->ops->notify_stop)
+		return;
 
-	ops = region->ops;
-	if (ops->notify_stop)
-		ops->notify_stop(region);
+	trace_region_notify_stop(region);
+	region->ops->notify_stop(region);
 }
 EXPORT_SYMBOL_GPL(mcps802154_region_notify_stop);
 
@@ -165,10 +166,16 @@ int mcps802154_region_get_demand(struct mcps802154_llhw *llhw,
 				 u32 next_timestamp_dtu,
 				 struct mcps802154_region_demand *demand)
 {
+	int r;
+
 	if (!region->ops->get_demand)
 		return -EOPNOTSUPP;
 
-	return region->ops->get_demand(region, next_timestamp_dtu, demand);
+	trace_region_get_demand(region, next_timestamp_dtu);
+	r = region->ops->get_demand(region, next_timestamp_dtu, demand);
+	trace_region_get_demand_return(region, demand, r);
+
+	return r;
 }
 EXPORT_SYMBOL_GPL(mcps802154_region_get_demand);
 
@@ -206,3 +213,17 @@ void mcps802154_region_rx_skb(struct mcps802154_llhw *llhw,
 	ieee802154_rx_irqsafe(local->hw, skb, lqi);
 }
 EXPORT_SYMBOL_GPL(mcps802154_region_rx_skb);
+
+int mcps802154_region_deferred(struct mcps802154_llhw *llhw,
+			       struct mcps802154_region *region)
+{
+	struct mcps802154_local *local = llhw_to_local(llhw);
+
+	if (local->fproc.deferred && local->fproc.deferred != region)
+		return -EINVAL;
+
+	local->fproc.deferred = region;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mcps802154_region_deferred);
diff --git a/mac/schedule.c b/mac/schedule.c
index 2d8fa8a..1ca0bfa 100644
--- a/mac/schedule.c
+++ b/mac/schedule.c
@@ -53,6 +53,8 @@ int mcps802154_schedule_update(struct mcps802154_local *local,
 		sched->start_timestamp_dtu = next_timestamp_dtu;
 		sched->duration_dtu = 0;
 		expected_start_timestamp_dtu = next_timestamp_dtu;
+	} else if (sched->duration_dtu == 0) {
+		expected_start_timestamp_dtu = next_timestamp_dtu;
 	} else {
 		expected_start_timestamp_dtu =
 			sched->start_timestamp_dtu + sched->duration_dtu;
@@ -68,6 +70,9 @@ int mcps802154_schedule_update(struct mcps802154_local *local,
 	scheduler = local->ca.scheduler;
 	r = scheduler->ops->update_schedule(scheduler, su, next_timestamp_dtu);
 	if (r) {
+		if (r != -ENOENT)
+			trace_update_schedule_error(local, su,
+						    next_timestamp_dtu);
 		mcps802154_schedule_clear(local);
 		return r;
 	}
@@ -146,7 +151,8 @@ EXPORT_SYMBOL(mcps802154_schedule_recycle);
 
 int mcps802154_schedule_add_region(
 	const struct mcps802154_schedule_update *schedule_update,
-	struct mcps802154_region *region, int start_dtu, int duration_dtu)
+	struct mcps802154_region *region, int start_dtu, int duration_dtu,
+	bool once)
 {
 	struct mcps802154_schedule_update_local *sulocal =
 		schedule_update_to_local(schedule_update);
@@ -184,6 +190,7 @@ int mcps802154_schedule_add_region(
 	sched_region->start_dtu = start_dtu;
 	sched_region->duration_dtu = duration_dtu;
 	sched_region->region = region;
+	sched_region->once = once;
 
 	sched->regions = new_sched_regions;
 	su->n_regions = sched->n_regions = sched->n_regions + 1;
@@ -227,3 +234,19 @@ int mcps802154_schedule_get_regions(struct mcps802154_llhw *llhw,
 	return local->ca.n_regions;
 }
 EXPORT_SYMBOL(mcps802154_schedule_get_regions);
+
+int mcps802154_schedule_get_next_demands(
+	struct mcps802154_llhw *llhw, const struct mcps802154_region *region,
+	u32 timestamp_dtu, int duration_dtu, int delta_dtu,
+	struct mcps802154_region_demand *demands)
+{
+	struct mcps802154_local *local = llhw_to_local(llhw);
+	struct mcps802154_scheduler *scheduler = local->ca.scheduler;
+
+	if (!scheduler->ops->get_next_demands)
+		return -EOPNOTSUPP;
+	return scheduler->ops->get_next_demands(scheduler, region,
+						timestamp_dtu, duration_dtu,
+						delta_dtu, demands);
+}
+EXPORT_SYMBOL(mcps802154_schedule_get_next_demands);
diff --git a/mac/schedule.h b/mac/schedule.h
index 53bd51f..f17feae 100644
--- a/mac/schedule.h
+++ b/mac/schedule.h
@@ -45,6 +45,10 @@ struct mcps802154_schedule_region {
 	 * @duration_dtu: Region duration or 0 for endless region.
 	 */
 	int duration_dtu;
+	/**
+	 * @once: Schedule the region once, ignoring the remaining region duration.
+	 */
+	bool once;
 };
 
 /**
diff --git a/mac/simple_ranging_region.c b/mac/simple_ranging_region.c
deleted file mode 100644
index 20b5f6b..0000000
--- a/mac/simple_ranging_region.c
+++ /dev/null
@@ -1,941 +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 <asm/unaligned.h>
-#include <linux/errno.h>
-#include <linux/ieee802154.h>
-#include <linux/kernel.h>
-#include <linux/string.h>
-#include <linux/limits.h>
-#include <net/af_ieee802154.h>
-
-#include <net/mcps802154_schedule.h>
-#include <net/mcps802154_frame.h>
-#include <net/simple_ranging_region_nl.h>
-
-#include "nl.h"
-#include "warn_return.h"
-
-#define TWR_SLOT_MS_TO_RCTU 67108864ull
-#define TWR_SLOT_MS_MAX 64
-#define TWR_SLOT_DEFAULT_RCTU (16 * TWR_SLOT_MS_TO_RCTU)
-
-#define TWR_FUNCTION_CODE_POLL 0x40
-#define TWR_FUNCTION_CODE_RESP 0x41
-#define TWR_FUNCTION_CODE_FINAL 0x42
-#define TWR_FUNCTION_CODE_REPORT 0x43
-
-enum twr_frames {
-	TWR_FRAME_POLL,
-	TWR_FRAME_RESP,
-	TWR_FRAME_FINAL,
-	TWR_FRAME_REPORT,
-	N_TWR_FRAMES,
-};
-
-struct simple_ranging_initiator {
-	u64 poll_tx_timestamp_rctu;
-	u64 final_tx_timestamp_rctu;
-	int tof_half_tag_rctu;
-	int local_pdoa_rad_q11;
-	s16 remote_pdoa_rad_q11;
-	int local_pdoa_elevation_rad_q11;
-	s16 remote_pdoa_elevation_rad_q11;
-};
-
-struct simple_ranging_responder {
-	u64 poll_rx_timestamp_rctu;
-	u64 resp_tx_timestamp_rctu;
-	s16 local_pdoa_rad_q11;
-	s16 local_pdoa_elevation_rad_q11;
-	int tof_x4_rctu;
-};
-
-struct simple_ranging_local {
-	struct mcps802154_scheduler scheduler;
-	struct mcps802154_region region_init;
-	struct mcps802154_region region_resp;
-	struct mcps802154_llhw *llhw;
-	struct mcps802154_access access;
-	struct mcps802154_access_frame frames[N_TWR_FRAMES];
-	struct mcps802154_sts_params sts_params;
-	struct mcps802154_nl_ranging_request
-		requests[MCPS802154_NL_RANGING_REQUESTS_MAX];
-	unsigned int n_requests;
-	unsigned int request_idx;
-	int frequency_hz;
-	struct mcps802154_nl_ranging_request current_request;
-	int slot_duration_dtu;
-	bool is_responder;
-	unsigned int tx_ant_set_id;
-	unsigned int rx_ant_set_id_azimuth;
-	unsigned int rx_ant_set_id_elevation;
-	bool is_same_rx_ant_set_id;
-	union {
-		struct simple_ranging_initiator initiator;
-		struct simple_ranging_responder responder;
-	};
-};
-
-static inline struct simple_ranging_local *
-scheduler_to_local(struct mcps802154_scheduler *scheduler)
-{
-	return container_of(scheduler, struct simple_ranging_local, scheduler);
-}
-
-static inline struct simple_ranging_local *
-access_to_local(struct mcps802154_access *access)
-{
-	return container_of(access, struct simple_ranging_local, access);
-}
-
-static inline struct simple_ranging_local *
-region_init_to_local(struct mcps802154_region *region_init)
-{
-	return container_of(region_init, struct simple_ranging_local,
-			    region_init);
-}
-
-static inline struct simple_ranging_local *
-region_resp_to_local(struct mcps802154_region *region_resp)
-{
-	return container_of(region_resp, struct simple_ranging_local,
-			    region_resp);
-}
-
-/* Requests and reports. */
-
-static void twr_requests_clear(struct simple_ranging_local *local)
-{
-	local->n_requests = 0;
-	local->request_idx = 0;
-	local->frequency_hz = 1;
-}
-
-static void twr_request_start(struct simple_ranging_local *local)
-{
-	if (local->request_idx >= local->n_requests)
-		local->request_idx = 0;
-	local->current_request = local->requests[local->request_idx];
-}
-
-static void twr_report(struct simple_ranging_local *local,
-		       const struct mcps802154_nl_ranging_report *report)
-{
-	struct mcps802154_nl_ranging_request *request = &local->current_request;
-	struct mcps802154_nl_ranging_report invalid = {
-		.tof_rctu = INT_MIN,
-		.local_pdoa_rad_q11 = INT_MIN,
-		.remote_pdoa_rad_q11 = INT_MIN,
-		.local_pdoa_elevation_rad_q11 = INT_MIN,
-		.remote_pdoa_elevation_rad_q11 = INT_MIN,
-		.is_same_rx_ant_set_id = local->is_same_rx_ant_set_id,
-	};
-	int r;
-
-	if (!report)
-		report = &invalid;
-
-	r = mcps802154_nl_ranging_report(local->llhw, request->id, report);
-
-	if (r == -ECONNREFUSED)
-		twr_requests_clear(local);
-	else
-		local->request_idx++;
-}
-
-/* Frames. */
-
-#define TWR_FRAME_HEADER_SIZE                                             \
-	(IEEE802154_FC_LEN + IEEE802154_SEQ_LEN + IEEE802154_PAN_ID_LEN + \
-	 IEEE802154_EXTENDED_ADDR_LEN * 2)
-#define TWR_FRAME_POLL_SIZE 1
-#define TWR_FRAME_RESP_SIZE 3
-#define TWR_FRAME_FINAL_SIZE 5
-#define TWR_FRAME_REPORT_SIZE 7
-#define TWR_FRAME_MAX_SIZE (TWR_FRAME_HEADER_SIZE + TWR_FRAME_REPORT_SIZE)
-
-void twr_frame_header_fill_buf(u8 *buf, __le16 pan_id, __le64 dst, __le64 src)
-{
-	u16 fc = (IEEE802154_FC_TYPE_DATA | IEEE802154_FC_INTRA_PAN |
-		  (IEEE802154_ADDR_LONG << IEEE802154_FC_DAMODE_SHIFT) |
-		  (1 << IEEE802154_FC_VERSION_SHIFT) |
-		  (IEEE802154_ADDR_LONG << IEEE802154_FC_SAMODE_SHIFT));
-	u8 seq = 0;
-	size_t pos = 0;
-
-	put_unaligned_le16(fc, buf + pos);
-	pos += IEEE802154_FC_LEN;
-	buf[pos] = seq;
-	pos += IEEE802154_SEQ_LEN;
-	memcpy(buf + pos, &pan_id, sizeof(pan_id));
-	pos += IEEE802154_PAN_ID_LEN;
-	memcpy(buf + pos, &dst, sizeof(dst));
-	pos += IEEE802154_EXTENDED_ADDR_LEN;
-	memcpy(buf + pos, &src, sizeof(src));
-}
-
-void twr_frame_header_put(struct sk_buff *skb, __le16 pan_id, __le64 dst,
-			  __le64 src)
-{
-	twr_frame_header_fill_buf(skb_put(skb, TWR_FRAME_HEADER_SIZE), pan_id,
-				  dst, src);
-}
-
-bool twr_frame_header_check(struct sk_buff *skb, __le16 pan_id, __le64 dst,
-			    __le64 src)
-{
-	u8 buf[TWR_FRAME_HEADER_SIZE];
-
-	if (skb->len < TWR_FRAME_HEADER_SIZE)
-		return false;
-	twr_frame_header_fill_buf(buf, pan_id, dst, src);
-	if (memcmp(skb->data, buf, TWR_FRAME_HEADER_SIZE) != 0)
-		return false;
-	return true;
-}
-
-bool twr_frame_header_check_no_src(struct sk_buff *skb, __le16 pan_id,
-				   __le64 dst)
-{
-	/* Ignore source. */
-	const int check_size =
-		TWR_FRAME_HEADER_SIZE - IEEE802154_EXTENDED_ADDR_LEN;
-	u8 buf[TWR_FRAME_HEADER_SIZE];
-
-	if (skb->len < TWR_FRAME_HEADER_SIZE)
-		return false;
-	twr_frame_header_fill_buf(buf, pan_id, dst, 0);
-	if (memcmp(skb->data, buf, check_size) != 0)
-		return false;
-	return true;
-}
-
-void twr_frame_poll_put(struct sk_buff *skb)
-{
-	u8 function_code = TWR_FUNCTION_CODE_POLL;
-
-	skb_put_u8(skb, function_code);
-}
-
-bool twr_frame_poll_check(struct sk_buff *skb)
-{
-	u8 function_code;
-
-	if (skb->len < TWR_FRAME_POLL_SIZE)
-		return false;
-	function_code = skb->data[0];
-	if (function_code != TWR_FUNCTION_CODE_POLL)
-		return false;
-	return true;
-}
-
-void twr_frame_resp_put(struct sk_buff *skb, s16 local_pdoa_rad_q11)
-{
-	u8 function_code = TWR_FUNCTION_CODE_RESP;
-
-	skb_put_u8(skb, function_code);
-	put_unaligned_le16(local_pdoa_rad_q11, skb_put(skb, 2));
-}
-
-bool twr_frame_resp_check(struct sk_buff *skb, s16 *remote_pdoa_rad_q11)
-{
-	u8 function_code;
-
-	if (skb->len < TWR_FRAME_RESP_SIZE)
-		return false;
-	function_code = skb->data[0];
-	if (function_code != TWR_FUNCTION_CODE_RESP)
-		return false;
-	*remote_pdoa_rad_q11 = get_unaligned_le16(skb->data + 1);
-	return true;
-}
-
-void twr_frame_final_put(struct sk_buff *skb, s32 tof_half_tag_rctu)
-{
-	u8 function_code = TWR_FUNCTION_CODE_FINAL;
-
-	skb_put_u8(skb, function_code);
-	put_unaligned_le32(tof_half_tag_rctu, skb_put(skb, 4));
-}
-
-bool twr_frame_final_check(struct sk_buff *skb, s32 *tof_half_tag_rctu)
-{
-	u8 function_code;
-
-	if (skb->len < TWR_FRAME_FINAL_SIZE)
-		return false;
-	function_code = skb->data[0];
-	if (function_code != TWR_FUNCTION_CODE_FINAL)
-		return false;
-	*tof_half_tag_rctu = get_unaligned_le32(skb->data + 1);
-	return true;
-}
-
-void twr_frame_report_put(struct sk_buff *skb, s32 tof_x4_rctu,
-			  s16 local_pdoa_rad_q11)
-{
-	u8 function_code = TWR_FUNCTION_CODE_REPORT;
-
-	skb_put_u8(skb, function_code);
-	put_unaligned_le32(tof_x4_rctu, skb_put(skb, 4));
-	put_unaligned_le16(local_pdoa_rad_q11, skb_put(skb, 2));
-}
-
-bool twr_frame_report_check(struct sk_buff *skb, s32 *tof_x4_rctu,
-			    s16 *remote_pdoa_rad_q11)
-{
-	u8 function_code;
-
-	if (skb->len < TWR_FRAME_REPORT_SIZE)
-		return false;
-	function_code = skb->data[0];
-	if (function_code != TWR_FUNCTION_CODE_REPORT)
-		return false;
-	*tof_x4_rctu = get_unaligned_le32(skb->data + 1);
-	*remote_pdoa_rad_q11 = get_unaligned_le16(skb->data + 5);
-	return true;
-}
-
-/* Access responder. */
-
-static void twr_responder_rx_frame(struct mcps802154_access *access,
-				   int frame_idx, struct sk_buff *skb,
-				   const struct mcps802154_rx_frame_info *info,
-				   enum mcps802154_rx_error_type error)
-{
-	struct simple_ranging_local *local = access_to_local(access);
-	struct mcps802154_nl_ranging_request *request = &local->current_request;
-
-	if (!skb)
-		goto fail;
-
-	if (frame_idx == TWR_FRAME_POLL) {
-		u32 resp_tx_dtu;
-
-		if (!twr_frame_header_check_no_src(
-			    skb, mcps802154_get_pan_id(local->llhw),
-			    mcps802154_get_extended_addr(local->llhw)))
-			goto fail_free_skb;
-		request->peer_extended_addr =
-			get_unaligned_le64(skb->data + TWR_FRAME_HEADER_SIZE -
-					   IEEE802154_EXTENDED_ADDR_LEN);
-		skb_pull(skb, TWR_FRAME_HEADER_SIZE);
-
-		if (!twr_frame_poll_check(skb))
-			goto fail_free_skb;
-		if (!(info->flags & MCPS802154_RX_FRAME_INFO_TIMESTAMP_DTU))
-			goto fail_free_skb;
-		if (!(info->flags & MCPS802154_RX_FRAME_INFO_TIMESTAMP_RCTU))
-			goto fail_free_skb;
-		if (!(info->flags & MCPS802154_RX_FRAME_INFO_RANGING_PDOA))
-			local->responder.local_pdoa_rad_q11 = S16_MIN;
-		else
-			local->responder.local_pdoa_rad_q11 =
-				info->ranging_pdoa_rad_q11;
-		local->responder.poll_rx_timestamp_rctu = info->timestamp_rctu;
-		resp_tx_dtu = info->timestamp_dtu + local->slot_duration_dtu;
-		local->responder.resp_tx_timestamp_rctu =
-			mcps802154_tx_timestamp_dtu_to_rmarker_rctu(
-				local->llhw, resp_tx_dtu, local->tx_ant_set_id);
-		/* Set the timings for the next frames. */
-		access->frames[TWR_FRAME_RESP].tx_frame_info.timestamp_dtu =
-			resp_tx_dtu;
-		access->frames[TWR_FRAME_FINAL].rx.info.timestamp_dtu =
-			resp_tx_dtu + local->slot_duration_dtu;
-		access->frames[TWR_FRAME_REPORT].tx_frame_info.timestamp_dtu =
-			resp_tx_dtu + 2 * local->slot_duration_dtu;
-	} else {
-		s32 tof_half_rctu;
-		u64 final_rx_timestamp_rctu;
-
-		WARN_ON(frame_idx != TWR_FRAME_FINAL);
-		if (!twr_frame_header_check(
-			    skb, mcps802154_get_pan_id(local->llhw),
-			    mcps802154_get_extended_addr(local->llhw),
-			    request->peer_extended_addr))
-			goto fail_free_skb;
-		skb_pull(skb, TWR_FRAME_HEADER_SIZE);
-		if (!twr_frame_final_check(skb, &tof_half_rctu))
-			goto fail_free_skb;
-		if (!(info->flags & MCPS802154_RX_FRAME_INFO_TIMESTAMP_RCTU))
-			goto fail_free_skb;
-		if (!(info->flags & MCPS802154_RX_FRAME_INFO_RANGING_PDOA))
-			local->responder.local_pdoa_elevation_rad_q11 = S16_MIN;
-		else
-			local->responder.local_pdoa_elevation_rad_q11 =
-				info->ranging_pdoa_rad_q11;
-		final_rx_timestamp_rctu = info->timestamp_rctu;
-		local->responder.tof_x4_rctu =
-			tof_half_rctu -
-			mcps802154_difference_timestamp_rctu(
-				local->llhw,
-				local->responder.resp_tx_timestamp_rctu,
-				local->responder.poll_rx_timestamp_rctu) +
-			mcps802154_difference_timestamp_rctu(
-				local->llhw, final_rx_timestamp_rctu,
-				local->responder.resp_tx_timestamp_rctu);
-	}
-
-	kfree_skb(skb);
-	return;
-fail_free_skb:
-	kfree_skb(skb);
-fail:
-	access->n_frames = frame_idx + 1;
-}
-
-static struct sk_buff *
-twr_responder_tx_get_frame(struct mcps802154_access *access, int frame_idx)
-{
-	struct simple_ranging_local *local = access_to_local(access);
-	struct mcps802154_nl_ranging_request *request = &local->current_request;
-	struct sk_buff *skb = mcps802154_frame_alloc(
-		local->llhw, TWR_FRAME_MAX_SIZE, GFP_KERNEL);
-
-	twr_frame_header_put(skb, mcps802154_get_pan_id(local->llhw),
-			     request->peer_extended_addr,
-			     mcps802154_get_extended_addr(local->llhw));
-	if (frame_idx == TWR_FRAME_RESP) {
-		twr_frame_resp_put(skb, local->responder.local_pdoa_rad_q11);
-	} else {
-		WARN_ON(frame_idx != TWR_FRAME_REPORT);
-		twr_frame_report_put(
-			skb, local->responder.tof_x4_rctu,
-			local->responder.local_pdoa_elevation_rad_q11);
-	}
-	return skb;
-}
-
-static void
-twr_responder_tx_return(struct mcps802154_access *access, int frame_idx,
-			struct sk_buff *skb,
-			enum mcps802154_access_tx_return_reason reason)
-{
-	kfree_skb(skb);
-}
-
-struct mcps802154_access_ops twr_responder_access_ops = {
-	.rx_frame = twr_responder_rx_frame,
-	.tx_get_frame = twr_responder_tx_get_frame,
-	.tx_return = twr_responder_tx_return,
-};
-
-/* Access initiator. */
-
-static void twr_rx_frame(struct mcps802154_access *access, int frame_idx,
-			 struct sk_buff *skb,
-			 const struct mcps802154_rx_frame_info *info,
-			 enum mcps802154_rx_error_type error)
-{
-	struct simple_ranging_local *local = access_to_local(access);
-	struct mcps802154_nl_ranging_request *request = &local->current_request;
-	u64 resp_rx_timestamp_rctu;
-	struct mcps802154_nl_ranging_report report;
-
-	if (!skb)
-		goto fail;
-
-	if (!twr_frame_header_check(skb, mcps802154_get_pan_id(local->llhw),
-				    mcps802154_get_extended_addr(local->llhw),
-				    request->peer_extended_addr))
-		goto fail_free_skb;
-
-	skb_pull(skb, TWR_FRAME_HEADER_SIZE);
-
-	if (frame_idx == TWR_FRAME_RESP) {
-		if (!twr_frame_resp_check(
-			    skb, &local->initiator.remote_pdoa_rad_q11))
-			goto fail_free_skb;
-		if (!(info->flags & MCPS802154_RX_FRAME_INFO_TIMESTAMP_RCTU))
-			goto fail_free_skb;
-		if (!(info->flags & MCPS802154_RX_FRAME_INFO_RANGING_PDOA))
-			local->initiator.local_pdoa_rad_q11 = INT_MIN;
-		else
-			local->initiator.local_pdoa_rad_q11 =
-				info->ranging_pdoa_rad_q11;
-
-		resp_rx_timestamp_rctu = info->timestamp_rctu;
-		local->initiator.tof_half_tag_rctu =
-			mcps802154_difference_timestamp_rctu(
-				local->llhw, resp_rx_timestamp_rctu,
-				local->initiator.poll_tx_timestamp_rctu) -
-			mcps802154_difference_timestamp_rctu(
-				local->llhw,
-				local->initiator.final_tx_timestamp_rctu,
-				resp_rx_timestamp_rctu);
-	} else {
-		s32 report_tof_x4_rctu;
-
-		WARN_ON(frame_idx != TWR_FRAME_REPORT);
-		if (!twr_frame_report_check(
-			    skb, &report_tof_x4_rctu,
-			    &local->initiator.remote_pdoa_elevation_rad_q11))
-			goto fail_free_skb;
-
-		if (!(info->flags & MCPS802154_RX_FRAME_INFO_RANGING_PDOA))
-			local->initiator.local_pdoa_elevation_rad_q11 = INT_MIN;
-		else
-			local->initiator.local_pdoa_elevation_rad_q11 =
-				info->ranging_pdoa_rad_q11;
-
-		report.tof_rctu = report_tof_x4_rctu / 4;
-		report.local_pdoa_rad_q11 = local->initiator.local_pdoa_rad_q11;
-		report.remote_pdoa_rad_q11 =
-			local->initiator.remote_pdoa_rad_q11;
-		report.local_pdoa_elevation_rad_q11 =
-			local->initiator.local_pdoa_elevation_rad_q11;
-		report.remote_pdoa_elevation_rad_q11 =
-			local->initiator.remote_pdoa_elevation_rad_q11;
-		report.is_same_rx_ant_set_id = local->is_same_rx_ant_set_id;
-
-		twr_report(local, &report);
-	}
-
-	kfree_skb(skb);
-	return;
-fail_free_skb:
-	kfree_skb(skb);
-fail:
-	twr_report(local, NULL);
-	access->n_frames = frame_idx + 1;
-}
-
-static struct sk_buff *twr_tx_get_frame(struct mcps802154_access *access,
-					int frame_idx)
-{
-	struct simple_ranging_local *local = access_to_local(access);
-	struct mcps802154_nl_ranging_request *request = &local->current_request;
-	struct sk_buff *skb = mcps802154_frame_alloc(
-		local->llhw, TWR_FRAME_MAX_SIZE, GFP_KERNEL);
-
-	twr_frame_header_put(skb, mcps802154_get_pan_id(local->llhw),
-			     request->peer_extended_addr,
-			     mcps802154_get_extended_addr(local->llhw));
-	if (frame_idx == TWR_FRAME_POLL) {
-		twr_frame_poll_put(skb);
-	} else {
-		WARN_ON(frame_idx != TWR_FRAME_FINAL);
-		twr_frame_final_put(skb, local->initiator.tof_half_tag_rctu);
-	}
-	return skb;
-}
-
-static void twr_tx_return(struct mcps802154_access *access, int frame_idx,
-			  struct sk_buff *skb,
-			  enum mcps802154_access_tx_return_reason reason)
-{
-	kfree_skb(skb);
-}
-
-struct mcps802154_access_ops twr_access_ops = {
-	.rx_frame = twr_rx_frame,
-	.tx_get_frame = twr_tx_get_frame,
-	.tx_return = twr_tx_return,
-};
-
-/* Region responder. */
-
-static struct mcps802154_access *
-twr_responder_get_access(struct mcps802154_region *region,
-			 u32 next_timestamp_dtu, int next_in_region_dtu,
-			 int region_duration_dtu)
-{
-	struct simple_ranging_local *local = region_resp_to_local(region);
-	struct mcps802154_access *access = &local->access;
-	u32 start_dtu = next_timestamp_dtu;
-
-	access->method = MCPS802154_ACCESS_METHOD_MULTI;
-	access->ops = &twr_responder_access_ops;
-	access->n_frames = ARRAY_SIZE(local->frames);
-	access->frames = local->frames;
-
-	access->frames[TWR_FRAME_POLL] = (struct mcps802154_access_frame){
-		.is_tx = false,
-		.rx = {
-			.info = {
-				.timestamp_dtu = start_dtu,
-				.timeout_dtu = -1,
-				.flags = MCPS802154_RX_INFO_TIMESTAMP_DTU |
-					MCPS802154_RX_INFO_RANGING |
-					MCPS802154_RX_INFO_KEEP_RANGING_CLOCK |
-					MCPS802154_RX_INFO_RANGING_PDOA |
-					MCPS802154_RX_INFO_SP1 |
-					MCPS802154_RX_INFO_RANGING_ROUND,
-				.ant_set_id = local->rx_ant_set_id_azimuth,
-			},
-			.frame_info_flags_request =
-				MCPS802154_RX_FRAME_INFO_TIMESTAMP_DTU |
-				MCPS802154_RX_FRAME_INFO_TIMESTAMP_RCTU |
-				MCPS802154_RX_FRAME_INFO_RANGING_PDOA,
-		},
-		.sts_params = &local->sts_params,
-	};
-
-	access->frames[TWR_FRAME_RESP] = (struct mcps802154_access_frame){
-		.is_tx = true,
-		.tx_frame_info = {
-			.flags = MCPS802154_TX_FRAME_TIMESTAMP_DTU |
-				MCPS802154_TX_FRAME_RANGING |
-				MCPS802154_TX_FRAME_KEEP_RANGING_CLOCK |
-				MCPS802154_TX_FRAME_SP1,
-			.ant_set_id = local->tx_ant_set_id,
-		},
-	};
-
-	access->frames[TWR_FRAME_FINAL] = (struct mcps802154_access_frame){
-		.is_tx = false,
-		.rx = {
-			.info = {
-				.flags = MCPS802154_RX_INFO_TIMESTAMP_DTU |
-					MCPS802154_RX_INFO_RANGING |
-					MCPS802154_RX_INFO_RANGING_PDOA |
-					MCPS802154_RX_INFO_SP1,
-				.ant_set_id = local->rx_ant_set_id_elevation,
-			},
-			.frame_info_flags_request =
-				MCPS802154_RX_FRAME_INFO_TIMESTAMP_RCTU |
-				MCPS802154_RX_FRAME_INFO_RANGING_PDOA,
-		},
-	};
-
-	access->frames[TWR_FRAME_REPORT] = (struct mcps802154_access_frame){
-		.is_tx = true,
-		.tx_frame_info = {
-			.flags = MCPS802154_TX_FRAME_TIMESTAMP_DTU |
-				MCPS802154_TX_FRAME_RANGING |
-				MCPS802154_TX_FRAME_SP1,
-			.ant_set_id = local->tx_ant_set_id,
-		},
-	};
-
-	return access;
-}
-
-static const struct mcps802154_region_ops
-	simple_ranging_twr_responder_region_ops = {
-		.name = "twr_responder",
-		.get_access = twr_responder_get_access,
-	};
-
-/* Region initiator. */
-
-static struct mcps802154_access *
-twr_get_access(struct mcps802154_region *region, u32 next_timestamp_dtu,
-	       int next_in_region_dtu, int region_duration_dtu)
-{
-	struct simple_ranging_local *local = region_init_to_local(region);
-	struct mcps802154_access *access = &local->access;
-	const int slots_dtu = local->slot_duration_dtu * N_TWR_FRAMES;
-
-	/* Do nothing if nothing to do. */
-	if (local->n_requests == 0)
-		return NULL;
-
-	/* Only start a ranging request if we have enough time to end it. */
-	if (next_in_region_dtu + slots_dtu > region_duration_dtu)
-		return NULL;
-
-	twr_request_start(local);
-	access->method = MCPS802154_ACCESS_METHOD_MULTI;
-	access->ops = &twr_access_ops;
-	access->n_frames = ARRAY_SIZE(local->frames);
-	access->frames = local->frames;
-
-	access->frames[TWR_FRAME_POLL] = (struct mcps802154_access_frame){
-		.is_tx = true,
-		.tx_frame_info = {
-			.timestamp_dtu = next_timestamp_dtu,
-			.flags = MCPS802154_TX_FRAME_TIMESTAMP_DTU |
-				MCPS802154_TX_FRAME_RANGING |
-				MCPS802154_TX_FRAME_KEEP_RANGING_CLOCK |
-				MCPS802154_TX_FRAME_SP1 |
-				MCPS802154_TX_FRAME_RANGING_ROUND,
-			.ant_set_id = local->tx_ant_set_id,
-		},
-		.sts_params = &local->sts_params,
-	};
-	local->initiator.poll_tx_timestamp_rctu =
-		mcps802154_tx_timestamp_dtu_to_rmarker_rctu(
-			local->llhw, next_timestamp_dtu, local->tx_ant_set_id);
-
-	access->frames[TWR_FRAME_RESP] = (struct mcps802154_access_frame){
-		.is_tx = false,
-		.rx = {
-			.info = {
-				.timestamp_dtu = next_timestamp_dtu +
-					local->slot_duration_dtu,
-				.flags = MCPS802154_RX_INFO_TIMESTAMP_DTU |
-					MCPS802154_RX_INFO_RANGING |
-					MCPS802154_RX_INFO_KEEP_RANGING_CLOCK |
-					MCPS802154_RX_INFO_RANGING_PDOA |
-					MCPS802154_RX_INFO_SP1,
-				.ant_set_id = local->rx_ant_set_id_azimuth,
-			},
-			.frame_info_flags_request =
-				MCPS802154_RX_FRAME_INFO_TIMESTAMP_RCTU |
-				MCPS802154_RX_FRAME_INFO_RANGING_PDOA,
-		},
-	};
-
-	access->frames[TWR_FRAME_FINAL] = (struct mcps802154_access_frame){
-		.is_tx = true,
-		.tx_frame_info = {
-			.timestamp_dtu = next_timestamp_dtu +
-				2 * local->slot_duration_dtu,
-			.flags = MCPS802154_TX_FRAME_TIMESTAMP_DTU |
-				MCPS802154_TX_FRAME_RANGING |
-				MCPS802154_TX_FRAME_SP1,
-			.ant_set_id = local->tx_ant_set_id,
-		},
-	};
-	local->initiator.final_tx_timestamp_rctu =
-		mcps802154_tx_timestamp_dtu_to_rmarker_rctu(
-			local->llhw,
-			next_timestamp_dtu + 2 * local->slot_duration_dtu,
-			local->tx_ant_set_id);
-
-	access->frames[TWR_FRAME_REPORT] = (struct mcps802154_access_frame){
-		.is_tx = false,
-		.rx = {
-			.info = {
-				.timestamp_dtu = next_timestamp_dtu +
-					3 * local->slot_duration_dtu,
-				.flags = MCPS802154_RX_INFO_TIMESTAMP_DTU |
-					MCPS802154_RX_INFO_RANGING |
-					MCPS802154_RX_INFO_RANGING_PDOA |
-					MCPS802154_RX_INFO_SP1,
-				.ant_set_id = local->rx_ant_set_id_elevation,
-			},
-			.frame_info_flags_request =
-				MCPS802154_RX_FRAME_INFO_RANGING_PDOA,
-		},
-	};
-
-	return access;
-}
-
-static const struct mcps802154_region_ops simple_ranging_twr_region_ops = {
-	.name = "twr_initiator",
-	.get_access = twr_get_access,
-};
-
-/* Scheduler. */
-
-static struct mcps802154_scheduler *
-simple_ranging_scheduler_open(struct mcps802154_llhw *llhw)
-{
-	struct simple_ranging_local *local;
-
-	local = kzalloc(sizeof(*local), GFP_KERNEL);
-	if (!local)
-		return NULL;
-	local->region_resp.ops = &simple_ranging_twr_responder_region_ops;
-	local->region_init.ops = &simple_ranging_twr_region_ops;
-	local->llhw = llhw;
-	local->sts_params.n_segs = 1;
-	local->sts_params.seg_len = 256;
-	local->slot_duration_dtu = TWR_SLOT_DEFAULT_RCTU / llhw->dtu_rctu;
-	local->is_responder = false;
-	local->tx_ant_set_id = TX_ANT_SET_ID_DEFAULT;
-	local->rx_ant_set_id_azimuth = RX_ANT_SET_ID_DEFAULT;
-	local->rx_ant_set_id_elevation = RX_ANT_SET_ID_DEFAULT;
-	local->is_same_rx_ant_set_id = true;
-
-	twr_requests_clear(local);
-	return &local->scheduler;
-}
-
-static void
-simple_ranging_scheduler_close(struct mcps802154_scheduler *scheduler)
-{
-	struct simple_ranging_local *local = scheduler_to_local(scheduler);
-
-	kfree(local);
-}
-
-static int simple_ranging_scheduler_update_schedule(
-	struct mcps802154_scheduler *scheduler,
-	const struct mcps802154_schedule_update *schedule_update,
-	u32 next_timestamp_dtu)
-{
-	struct simple_ranging_local *local = scheduler_to_local(scheduler);
-	const int slot_dtu = local->slot_duration_dtu;
-	int twr_slots = local->n_requests * N_TWR_FRAMES;
-	int r;
-
-	if (schedule_update->n_regions) {
-		int schedule_duration_slots = local->llhw->dtu_freq_hz /
-					      slot_dtu / local->frequency_hz;
-		/* This treatment is done only for initiator.
-		 * Responder region never enters here. As it is an infinite
-		 * region. */
-		WARN_ON(local->is_responder);
-		if (schedule_duration_slots < twr_slots)
-			schedule_duration_slots = twr_slots;
-
-		r = mcps802154_schedule_set_start(
-			schedule_update,
-			schedule_update->expected_start_timestamp_dtu +
-				(schedule_duration_slots - twr_slots) *
-					slot_dtu);
-		WARN_RETURN(r);
-	}
-
-	r = mcps802154_schedule_recycle(schedule_update, 0,
-					MCPS802154_DURATION_NO_CHANGE);
-	WARN_RETURN(r);
-
-	if (local->is_responder) {
-		r = mcps802154_schedule_add_region(schedule_update,
-						   &local->region_resp, 0, 0);
-	} else {
-		r = mcps802154_schedule_add_region(schedule_update,
-						   &local->region_init, 0,
-						   twr_slots * slot_dtu);
-	}
-	return r;
-}
-
-static int simple_ranging_scheduler_ranging_setup(
-	struct mcps802154_scheduler *scheduler,
-	const struct mcps802154_nl_ranging_request *requests,
-	unsigned int n_requests)
-{
-	struct simple_ranging_local *local = scheduler_to_local(scheduler);
-	bool need_invalidate = local->n_requests == 0;
-	int max_frequency_hz = 1;
-	int i;
-
-	if (local->is_responder)
-		return -EOPNOTSUPP;
-
-	if (n_requests > MCPS802154_NL_RANGING_REQUESTS_MAX)
-		return -EINVAL;
-
-	for (i = 0; i < n_requests; i++) {
-		if (requests[i].remote_peer_extended_addr)
-			return -EOPNOTSUPP;
-		local->requests[i] = requests[i];
-		if (requests[i].frequency_hz > max_frequency_hz)
-			max_frequency_hz = requests[i].frequency_hz;
-	}
-	local->n_requests = n_requests;
-	local->frequency_hz = max_frequency_hz;
-
-	if (need_invalidate)
-		mcps802154_schedule_invalidate(local->llhw);
-
-	return 0;
-}
-
-static int
-simple_ranging_scheduler_set_parameters(struct mcps802154_scheduler *scheduler,
-					const struct nlattr *params_attr,
-					struct netlink_ext_ack *extack)
-{
-	struct simple_ranging_local *local = scheduler_to_local(scheduler);
-	struct nlattr *attrs[SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_MAX + 1];
-	int r;
-	static const struct nla_policy nla_policy[SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_MAX +
-						  1] = {
-		[SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_SLOT_DURATION_MS] = { .type = NLA_U32 },
-		[SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_NODE_TYPE] = { .type = NLA_U32,
-									  .validation_type =
-										  NLA_VALIDATE_MAX,
-									  .max = 1, },
-		[SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_TX_ANTENNA] = { .type = NLA_U8 },
-		[SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_RX_ANTENNA_PAIR_AZIMUTH] = { .type = NLA_U8 },
-		[SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_RX_ANTENNA_PAIR_ELEVATION] = { .type = NLA_U8 },
-	};
-
-	r = nla_parse_nested(attrs,
-			     SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_MAX,
-			     params_attr, nla_policy, extack);
-	if (r)
-		return r;
-
-	if (attrs[SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_SLOT_DURATION_MS]) {
-		int slot_duration_ms = nla_get_u32(
-			attrs[SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_SLOT_DURATION_MS]);
-
-		if (!slot_duration_ms ||
-		    (slot_duration_ms & (slot_duration_ms - 1)) ||
-		    slot_duration_ms > TWR_SLOT_MS_MAX)
-			return -EINVAL;
-
-		local->slot_duration_dtu = slot_duration_ms *
-					   TWR_SLOT_MS_TO_RCTU /
-					   local->llhw->dtu_rctu;
-	}
-	if (attrs[SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_NODE_TYPE]) {
-		u32 type = nla_get_u32(
-			attrs[SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_NODE_TYPE]);
-
-		local->is_responder = type == 1 ? true : false;
-		mcps802154_schedule_invalidate(local->llhw);
-	}
-
-	if (attrs[SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_TX_ANTENNA]) {
-		u8 id = nla_get_u8(
-			attrs[SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_TX_ANTENNA]);
-		local->tx_ant_set_id = id;
-	}
-	if (attrs[SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_RX_ANTENNA_PAIR_AZIMUTH]) {
-		u8 id = nla_get_u8(
-			attrs[SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_RX_ANTENNA_PAIR_AZIMUTH]);
-		local->rx_ant_set_id_azimuth = id;
-	}
-	if (attrs[SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_RX_ANTENNA_PAIR_ELEVATION]) {
-		u8 id = nla_get_u8(
-			attrs[SIMPLE_RANGING_REGION_SET_PARAMETERS_ATTR_RX_ANTENNA_PAIR_ELEVATION]);
-		local->rx_ant_set_id_elevation = id;
-	}
-
-	local->is_same_rx_ant_set_id = local->rx_ant_set_id_azimuth ==
-				       local->rx_ant_set_id_elevation;
-
-	return 0;
-}
-
-static struct mcps802154_scheduler_ops simple_ranging_scheduler_ops = {
-	.owner = THIS_MODULE,
-	.name = "simple-ranging",
-	.open = simple_ranging_scheduler_open,
-	.close = simple_ranging_scheduler_close,
-	.update_schedule = simple_ranging_scheduler_update_schedule,
-	.ranging_setup = simple_ranging_scheduler_ranging_setup,
-	.set_parameters = simple_ranging_scheduler_set_parameters,
-};
-
-int __init simple_ranging_region_init(void)
-{
-	int r = mcps802154_scheduler_register(&simple_ranging_scheduler_ops);
-	/* TODO: register regions when they can be used from another scheduler. */
-	return r;
-}
-
-void __exit simple_ranging_region_exit(void)
-{
-	mcps802154_scheduler_unregister(&simple_ranging_scheduler_ops);
-}
diff --git a/mac/trace.h b/mac/trace.h
index 7ba4d88..3808554 100644
--- a/mac/trace.h
+++ b/mac/trace.h
@@ -29,6 +29,7 @@
 
 #include <linux/tracepoint.h>
 #include "mcps802154_i.h"
+#include "idle_region.h"
 
 /* clang-format off */
 
@@ -37,72 +38,78 @@
 #define LOCAL_PR_FMT "hw%d"
 #define LOCAL_PR_ARG __entry->hw_idx
 
-#define mcps802154_tx_frame_name(name)                     \
+#define mcps802154_tx_frame_config_name(name)                     \
 	{                                                  \
-		MCPS802154_TX_FRAME_##name, #name          \
+		MCPS802154_TX_FRAME_CONFIG_##name, #name          \
 	}
-#define MCPS802154_TX_FRAME_NAMES                          \
-	mcps802154_tx_frame_name(TIMESTAMP_DTU),           \
-	mcps802154_tx_frame_name(CCA),                     \
-	mcps802154_tx_frame_name(RANGING),                 \
-	mcps802154_tx_frame_name(KEEP_RANGING_CLOCK),      \
-	mcps802154_tx_frame_name(RANGING_PDOA),            \
-	mcps802154_tx_frame_name(SP1),                     \
-	mcps802154_tx_frame_name(SP2),                     \
-	mcps802154_tx_frame_name(SP3),                     \
-	mcps802154_tx_frame_name(STS_MODE_MASK),           \
-	mcps802154_tx_frame_name(RANGING_ROUND)
-TRACE_DEFINE_ENUM(MCPS802154_TX_FRAME_TIMESTAMP_DTU);
-TRACE_DEFINE_ENUM(MCPS802154_TX_FRAME_CCA);
-TRACE_DEFINE_ENUM(MCPS802154_TX_FRAME_RANGING);
-TRACE_DEFINE_ENUM(MCPS802154_TX_FRAME_KEEP_RANGING_CLOCK);
-TRACE_DEFINE_ENUM(MCPS802154_TX_FRAME_RANGING_PDOA);
-TRACE_DEFINE_ENUM(MCPS802154_TX_FRAME_SP1);
-TRACE_DEFINE_ENUM(MCPS802154_TX_FRAME_SP2);
-TRACE_DEFINE_ENUM(MCPS802154_TX_FRAME_SP3);
-TRACE_DEFINE_ENUM(MCPS802154_TX_FRAME_STS_MODE_MASK);
-TRACE_DEFINE_ENUM(MCPS802154_TX_FRAME_RANGING_ROUND);
-
-#define mcps802154_rx_info_name(name)                      \
+#define MCPS802154_TX_FRAME_CONFIG_NAMES                          \
+	mcps802154_tx_frame_config_name(TIMESTAMP_DTU),           \
+	mcps802154_tx_frame_config_name(CCA),                     \
+	mcps802154_tx_frame_config_name(RANGING),                 \
+	mcps802154_tx_frame_config_name(KEEP_RANGING_CLOCK),      \
+	mcps802154_tx_frame_config_name(RANGING_PDOA),            \
+	mcps802154_tx_frame_config_name(SP1),                     \
+	mcps802154_tx_frame_config_name(SP2),                     \
+	mcps802154_tx_frame_config_name(SP3),                     \
+	mcps802154_tx_frame_config_name(STS_MODE_MASK),           \
+	mcps802154_tx_frame_config_name(RANGING_ROUND)
+TRACE_DEFINE_ENUM(MCPS802154_TX_FRAME_CONFIG_TIMESTAMP_DTU);
+TRACE_DEFINE_ENUM(MCPS802154_TX_FRAME_CONFIG_CCA);
+TRACE_DEFINE_ENUM(MCPS802154_TX_FRAME_CONFIG_RANGING);
+TRACE_DEFINE_ENUM(MCPS802154_TX_FRAME_CONFIG_KEEP_RANGING_CLOCK);
+TRACE_DEFINE_ENUM(MCPS802154_TX_FRAME_CONFIG_RANGING_PDOA);
+TRACE_DEFINE_ENUM(MCPS802154_TX_FRAME_CONFIG_SP1);
+TRACE_DEFINE_ENUM(MCPS802154_TX_FRAME_CONFIG_SP2);
+TRACE_DEFINE_ENUM(MCPS802154_TX_FRAME_CONFIG_SP3);
+TRACE_DEFINE_ENUM(MCPS802154_TX_FRAME_CONFIG_STS_MODE_MASK);
+TRACE_DEFINE_ENUM(MCPS802154_TX_FRAME_CONFIG_RANGING_ROUND);
+
+#define mcps802154_rx_frame_config_name(name)                      \
 	{                                                  \
-		MCPS802154_RX_INFO_##name, #name           \
+		MCPS802154_RX_FRAME_CONFIG_##name, #name   \
 	}
-#define MCPS802154_RX_INFO_NAMES                           \
-	mcps802154_rx_info_name(TIMESTAMP_DTU),            \
-	mcps802154_rx_info_name(AACK),                     \
-	mcps802154_rx_info_name(RANGING),                  \
-	mcps802154_rx_info_name(KEEP_RANGING_CLOCK),       \
-	mcps802154_rx_info_name(RANGING_PDOA),             \
-	mcps802154_rx_info_name(SP1),                      \
-	mcps802154_rx_info_name(SP2),                      \
-	mcps802154_rx_info_name(SP3),                      \
-	mcps802154_rx_info_name(STS_MODE_MASK),            \
-	mcps802154_rx_info_name(RANGING_ROUND)
-TRACE_DEFINE_ENUM(MCPS802154_RX_INFO_TIMESTAMP_DTU);
-TRACE_DEFINE_ENUM(MCPS802154_RX_INFO_AACK);
-TRACE_DEFINE_ENUM(MCPS802154_RX_INFO_RANGING);
-TRACE_DEFINE_ENUM(MCPS802154_RX_INFO_KEEP_RANGING_CLOCK);
-TRACE_DEFINE_ENUM(MCPS802154_RX_INFO_RANGING_PDOA);
-TRACE_DEFINE_ENUM(MCPS802154_RX_INFO_SP1);
-TRACE_DEFINE_ENUM(MCPS802154_RX_INFO_SP2);
-TRACE_DEFINE_ENUM(MCPS802154_RX_INFO_SP3);
-TRACE_DEFINE_ENUM(MCPS802154_RX_INFO_STS_MODE_MASK);
-TRACE_DEFINE_ENUM(MCPS802154_RX_INFO_RANGING_ROUND);
+#define MCPS802154_RX_FRAME_CONFIG_NAMES                   \
+	mcps802154_rx_frame_config_name(TIMESTAMP_DTU),            \
+	mcps802154_rx_frame_config_name(AACK),                     \
+	mcps802154_rx_frame_config_name(RANGING),                  \
+	mcps802154_rx_frame_config_name(KEEP_RANGING_CLOCK),       \
+	mcps802154_rx_frame_config_name(RANGING_PDOA),             \
+	mcps802154_rx_frame_config_name(SP1),                      \
+	mcps802154_rx_frame_config_name(SP2),                      \
+	mcps802154_rx_frame_config_name(SP3),                      \
+	mcps802154_rx_frame_config_name(STS_MODE_MASK),            \
+	mcps802154_rx_frame_config_name(RANGING_ROUND)
+TRACE_DEFINE_ENUM(MCPS802154_RX_FRAME_CONFIG_TIMESTAMP_DTU);
+TRACE_DEFINE_ENUM(MCPS802154_RX_FRAME_CONFIG_AACK);
+TRACE_DEFINE_ENUM(MCPS802154_RX_FRAME_CONFIG_RANGING);
+TRACE_DEFINE_ENUM(MCPS802154_RX_FRAME_CONFIG_KEEP_RANGING_CLOCK);
+TRACE_DEFINE_ENUM(MCPS802154_RX_FRAME_CONFIG_RANGING_PDOA);
+TRACE_DEFINE_ENUM(MCPS802154_RX_FRAME_CONFIG_SP1);
+TRACE_DEFINE_ENUM(MCPS802154_RX_FRAME_CONFIG_SP2);
+TRACE_DEFINE_ENUM(MCPS802154_RX_FRAME_CONFIG_SP3);
+TRACE_DEFINE_ENUM(MCPS802154_RX_FRAME_CONFIG_STS_MODE_MASK);
+TRACE_DEFINE_ENUM(MCPS802154_RX_FRAME_CONFIG_RANGING_ROUND);
 
 #define mcps802154_rx_error_name(name)                     \
 	{                                                  \
 		MCPS802154_RX_ERROR_##name, #name          \
 	}
 #define MCPS802154_RX_ERROR_NAMES                          \
-	mcps802154_rx_error_name(BAD_CKSUM),               \
-	mcps802154_rx_error_name(UNCORRECTABLE),           \
-	mcps802154_rx_error_name(FILTERED),                \
-	mcps802154_rx_error_name(SFD_TIMEOUT),             \
+	mcps802154_rx_error_name(NONE),            \
+	mcps802154_rx_error_name(TIMEOUT),         \
+	mcps802154_rx_error_name(BAD_CKSUM),       \
+	mcps802154_rx_error_name(UNCORRECTABLE),   \
+	mcps802154_rx_error_name(FILTERED),        \
+	mcps802154_rx_error_name(SFD_TIMEOUT),     \
+	mcps802154_rx_error_name(PHR_DECODE),      \
 	mcps802154_rx_error_name(OTHER)
+TRACE_DEFINE_ENUM(MCPS802154_RX_ERROR_NONE);
+TRACE_DEFINE_ENUM(MCPS802154_RX_ERROR_TIMEOUT);
 TRACE_DEFINE_ENUM(MCPS802154_RX_ERROR_BAD_CKSUM);
 TRACE_DEFINE_ENUM(MCPS802154_RX_ERROR_UNCORRECTABLE);
 TRACE_DEFINE_ENUM(MCPS802154_RX_ERROR_FILTERED);
 TRACE_DEFINE_ENUM(MCPS802154_RX_ERROR_SFD_TIMEOUT);
+TRACE_DEFINE_ENUM(MCPS802154_RX_ERROR_PHR_DECODE);
 TRACE_DEFINE_ENUM(MCPS802154_RX_ERROR_OTHER);
 
 #define ieee802154_afilt_name(name)                        \
@@ -277,9 +284,9 @@ DEFINE_EVENT(local_only_evt, llhw_stop,
 
 TRACE_EVENT(llhw_tx_frame,
 	TP_PROTO(const struct mcps802154_local *local,
-		 const struct mcps802154_tx_frame_info *info,
+		 const struct mcps802154_tx_frame_config *config,
 		 int frame_idx, int next_delay_dtu),
-	TP_ARGS(local, info, frame_idx, next_delay_dtu),
+	TP_ARGS(local, config, frame_idx, next_delay_dtu),
 	TP_STRUCT__entry(
 		LOCAL_ENTRY
 		__field(u32, timestamp_dtu)
@@ -292,11 +299,11 @@ TRACE_EVENT(llhw_tx_frame,
 		),
 	TP_fast_assign(
 		LOCAL_ASSIGN;
-		__entry->timestamp_dtu = info->timestamp_dtu;
-		__entry->rx_enable_after_tx_dtu = info->rx_enable_after_tx_dtu;
-		__entry->rx_enable_after_tx_timeout_dtu = info->rx_enable_after_tx_timeout_dtu;
-		__entry->ant_set_id = info->ant_set_id;
-		__entry->flags = info->flags;
+		__entry->timestamp_dtu = config->timestamp_dtu;
+		__entry->rx_enable_after_tx_dtu = config->rx_enable_after_tx_dtu;
+		__entry->rx_enable_after_tx_timeout_dtu = config->rx_enable_after_tx_timeout_dtu;
+		__entry->ant_set_id = config->ant_set_id;
+		__entry->flags = config->flags;
 		__entry->frame_idx = frame_idx;
 		__entry->next_delay_dtu = next_delay_dtu;
 		),
@@ -306,7 +313,7 @@ TRACE_EVENT(llhw_tx_frame,
 		  LOCAL_PR_ARG,
 		  __entry->timestamp_dtu, __entry->rx_enable_after_tx_dtu,
 		  __entry->rx_enable_after_tx_timeout_dtu, __entry->ant_set_id,
-		  __print_flags(__entry->flags, "|", MCPS802154_TX_FRAME_NAMES),
+		  __print_flags(__entry->flags, "|", MCPS802154_TX_FRAME_CONFIG_NAMES),
 		  __entry->frame_idx,
 		  __entry->next_delay_dtu
 		  )
@@ -314,13 +321,14 @@ TRACE_EVENT(llhw_tx_frame,
 
 TRACE_EVENT(llhw_rx_enable,
 	TP_PROTO(const struct mcps802154_local *local,
-		 const struct mcps802154_rx_info *info,
+		 const struct mcps802154_rx_frame_config *config,
 		 int frame_idx, int next_delay_dtu),
-	TP_ARGS(local, info, frame_idx, next_delay_dtu),
+	TP_ARGS(local, config, frame_idx, next_delay_dtu),
 	TP_STRUCT__entry(
 		LOCAL_ENTRY
 		__field(u32, timestamp_dtu)
 		__field(int, timeout_dtu)
+		__field(int, frame_timeout_dtu)
 		__field(u8, flags)
 		__field(int, ant_set_id)
 		__field(int, frame_idx)
@@ -328,19 +336,21 @@ TRACE_EVENT(llhw_rx_enable,
 		),
 	TP_fast_assign(
 		LOCAL_ASSIGN;
-		__entry->timestamp_dtu = info->timestamp_dtu;
-		__entry->timeout_dtu = info->timeout_dtu;
-		__entry->flags = info->flags;
-		__entry->ant_set_id = info->ant_set_id;
+		__entry->timestamp_dtu = config->timestamp_dtu;
+		__entry->timeout_dtu = config->timeout_dtu;
+		__entry->frame_timeout_dtu = config->frame_timeout_dtu;
+		__entry->flags = config->flags;
+		__entry->ant_set_id = config->ant_set_id;
 		__entry->frame_idx = frame_idx;
 		__entry->next_delay_dtu = next_delay_dtu;
 		),
 	TP_printk(LOCAL_PR_FMT " timestamp_dtu=0x%08x timeout_dtu=%d"
-		  " ant_set_id=%d flags=%s frame_idx=%d next_delay_dtu=%d",
+		  " frame_timeout_dtu=%d ant_set_id=%d flags=%s frame_idx=%d"
+		  " next_delay_dtu=%d",
 		  LOCAL_PR_ARG,
 		  __entry->timestamp_dtu, __entry->timeout_dtu,
-		  __entry->ant_set_id,
-		  __print_flags(__entry->flags, "|", MCPS802154_RX_INFO_NAMES),
+		  __entry->frame_timeout_dtu, __entry->ant_set_id,
+		  __print_flags(__entry->flags, "|", MCPS802154_RX_FRAME_CONFIG_NAMES),
 		  __entry->frame_idx,
 		  __entry->next_delay_dtu
 		  )
@@ -434,28 +444,53 @@ TRACE_EVENT(llhw_set_channel,
 	);
 
 TRACE_EVENT(llhw_set_hrp_uwb_params,
-	TP_PROTO(const struct mcps802154_local *local, int prf, int psr,
-		 int sfd_selector, int phr_rate, int data_rate),
-	TP_ARGS(local, prf, psr, sfd_selector, phr_rate, data_rate),
+	TP_PROTO(const struct mcps802154_local *local,
+		 const struct mcps802154_hrp_uwb_params *params),
+	TP_ARGS(local, params),
+	TP_STRUCT__entry(
+		LOCAL_ENTRY
+		__field(int, prf)
+		__field(int, psr)
+		__field(int, sfd_selector)
+		__field(int, phr_hi_rate)
+		__field(int, data_rate)
+		),
+	TP_fast_assign(
+		LOCAL_ASSIGN;
+		__entry->prf = params->prf;
+		__entry->psr = params->psr;
+		__entry->sfd_selector = params->sfd_selector;
+		__entry->phr_hi_rate = params->phr_hi_rate;
+		__entry->data_rate = params->data_rate;
+		),
+	TP_printk(LOCAL_PR_FMT " prf=%d psr=%d sfd_selector=%d phr_hi_rate=%d data_rate=%d",
+		  LOCAL_PR_ARG, __entry->prf, __entry->psr,
+		  __entry->sfd_selector, __entry->phr_hi_rate, __entry->data_rate)
+	);
+
+TRACE_EVENT(llhw_check_hrp_uwb_params,
+	TP_PROTO(const struct mcps802154_local *local,
+		 const struct mcps802154_hrp_uwb_params *params),
+	TP_ARGS(local, params),
 	TP_STRUCT__entry(
 		LOCAL_ENTRY
 		__field(int, prf)
 		__field(int, psr)
 		__field(int, sfd_selector)
-		__field(int, phr_rate)
+		__field(int, phr_hi_rate)
 		__field(int, data_rate)
 		),
 	TP_fast_assign(
 		LOCAL_ASSIGN;
-		__entry->prf = prf;
-		__entry->psr = psr;
-		__entry->sfd_selector = sfd_selector;
-		__entry->phr_rate = phr_rate;
-		__entry->data_rate = data_rate;
+		__entry->prf = params->prf;
+		__entry->psr = params->psr;
+		__entry->sfd_selector = params->sfd_selector;
+		__entry->phr_hi_rate = params->phr_hi_rate;
+		__entry->data_rate = params->data_rate;
 		),
-	TP_printk(LOCAL_PR_FMT " prf=%d psr=%d sfd_selector=%d phr_rate=%d data_rate=%d",
+	TP_printk(LOCAL_PR_FMT " prf=%d psr=%d sfd_selector=%d phr_hi_rate=%d data_rate=%d",
 		  LOCAL_PR_ARG, __entry->prf, __entry->psr,
-		  __entry->sfd_selector, __entry->phr_rate, __entry->data_rate)
+		  __entry->sfd_selector, __entry->phr_hi_rate, __entry->data_rate)
 	);
 
 TRACE_EVENT(llhw_set_sts_params,
@@ -468,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;
@@ -476,13 +513,67 @@ 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,
+	TP_PROTO(const struct mcps802154_local *local, const void *rx_ctx),
+	TP_ARGS(local, rx_ctx),
+	TP_STRUCT__entry(
+		LOCAL_ENTRY
+		__field(const void *, rx_ctx)
+		),
+	TP_fast_assign(
+		LOCAL_ASSIGN;
+		__entry->rx_ctx = rx_ctx;
+		),
+	TP_printk(LOCAL_PR_FMT " rx_ctx=%p",
+		  LOCAL_PR_ARG,
+		  __entry->rx_ctx)
+	);
+
+TRACE_EVENT(llhw_return_measurement,
+	TP_PROTO(const struct mcps802154_local *local, int r,
+		 const struct mcps802154_rx_measurement_info *info),
+	TP_ARGS(local, r, info),
+	TP_STRUCT__entry(
+		LOCAL_ENTRY
+		__field(int, r)
+		__field(int, n_rssis)
+		__field(int, n_aoas)
+		__field(int, n_cirs)
+		),
+	TP_fast_assign(
+		LOCAL_ASSIGN;
+		__entry->r = r;
+		__entry->n_rssis = info->n_rssis;
+		__entry->n_aoas = info->n_aoas;
+		__entry->n_cirs = info->n_cirs;
+		),
+	TP_printk(LOCAL_PR_FMT " r=%d n_rssis=%d n_aoas=%d n_cirs=%d",
+		  LOCAL_PR_ARG,
+		  __entry->r,
+		  __entry->n_rssis,
+		  __entry->n_aoas,
+		  __entry->n_cirs)
 	);
 
 TRACE_EVENT(llhw_set_hw_addr_filt,
@@ -810,6 +901,18 @@ TRACE_EVENT(ca_return_int,
 	TP_printk(LOCAL_PR_FMT " r=%d", LOCAL_PR_ARG, __entry->r)
 	);
 
+TRACE_EVENT(fproc_broken_enter,
+	TP_PROTO(const struct mcps802154_local *local),
+	TP_ARGS(local),
+	TP_STRUCT__entry(
+		LOCAL_ENTRY
+		),
+	TP_fast_assign(
+		LOCAL_ASSIGN;
+		),
+	TP_printk(LOCAL_PR_FMT, LOCAL_PR_ARG)
+	);
+
 TRACE_EVENT(schedule_update,
 	TP_PROTO(const struct mcps802154_local *local, u32 next_timestamp_dtu),
 	TP_ARGS(local, next_timestamp_dtu),
@@ -825,6 +928,38 @@ TRACE_EVENT(schedule_update,
 		  __entry->next_timestamp_dtu)
 	);
 
+TRACE_EVENT(update_schedule_error,
+	TP_PROTO(const struct mcps802154_local *local,
+		 const struct mcps802154_schedule_update *su,
+		 u32 next_timestamp_dtu),
+	TP_ARGS(local, su, next_timestamp_dtu),
+	TP_STRUCT__entry(
+		LOCAL_ENTRY
+		__field(u32, expected_start_timestamp_dtu)
+		__field(u32, start_timestamp_dtu)
+		__field(int, duration_dtu)
+		__field(size_t, n_regions)
+		__field(u32, next_timestamp_dtu)
+		),
+	TP_fast_assign(
+		LOCAL_ASSIGN;
+		__entry->expected_start_timestamp_dtu = su->expected_start_timestamp_dtu;
+		__entry->start_timestamp_dtu = su->start_timestamp_dtu;
+		__entry->duration_dtu = su->duration_dtu;
+		__entry->n_regions = su->n_regions;
+		__entry->next_timestamp_dtu = next_timestamp_dtu;
+		),
+	TP_printk(LOCAL_PR_FMT " expected_start_timestamp_dtu=0x%08x "
+		  "start_timestamp_dtu=0x%08x duration_dtu=%d "
+		  "n_regions=%lu next_timestamp_dtu=0x%08x",
+		  LOCAL_PR_ARG,
+		  __entry->expected_start_timestamp_dtu,
+		  __entry->start_timestamp_dtu,
+		  __entry->duration_dtu,
+		  __entry->n_regions,
+		  __entry->next_timestamp_dtu)
+	);
+
 TRACE_EVENT(schedule_update_done,
 	TP_PROTO(const struct mcps802154_local *local,
 		 const struct mcps802154_schedule *sched),
@@ -846,6 +981,63 @@ TRACE_EVENT(schedule_update_done,
 		  __entry->duration_dtu, __entry->n_regions)
 	);
 
+TRACE_EVENT(
+	region_notify_stop,
+	TP_PROTO(const struct mcps802154_region *region),
+	TP_ARGS(region),
+	TP_STRUCT__entry(
+		__string(region_name, region->ops->name)
+		),
+	TP_fast_assign(
+		__assign_str(region_name, region->ops->name);
+		),
+	TP_printk("region=%s",
+		  __get_str(region_name)
+		)
+	);
+
+TRACE_EVENT(
+	region_get_demand,
+	TP_PROTO(const struct mcps802154_region *region,
+		 u32 next_timestamp_dtu),
+	TP_ARGS(region, next_timestamp_dtu),
+	TP_STRUCT__entry(
+		__string(region_name, region->ops->name)
+		__field(u32, next_timestamp_dtu)
+		),
+	TP_fast_assign(
+		__assign_str(region_name, region->ops->name);
+		__entry->next_timestamp_dtu = next_timestamp_dtu;
+		),
+	TP_printk("region=%s next_timestamp_dtu=%#x",
+		  __get_str(region_name),
+		  __entry->next_timestamp_dtu
+		)
+	);
+
+TRACE_EVENT(
+	region_get_demand_return,
+	TP_PROTO(const struct mcps802154_region *region,
+		 const struct mcps802154_region_demand *demand,
+		 int r),
+	TP_ARGS(region, demand, r),
+	TP_STRUCT__entry(
+		__field(int, r)
+		__field(u32, timestamp_dtu)
+		__field(u32, max_duration_dtu)
+		),
+	TP_fast_assign(
+		__entry->r = r;
+		__entry->timestamp_dtu = demand->timestamp_dtu;
+		__entry->max_duration_dtu = demand->max_duration_dtu;
+		),
+	TP_printk("r=%d timestamp_dtu=%#x max_duration_dtu=%d",
+		  __entry->r,
+		  __entry->timestamp_dtu,
+		  __entry->max_duration_dtu
+		)
+	);
+
 TRACE_EVENT(region_get_access,
 	TP_PROTO(const struct mcps802154_local *local,
 		 const struct mcps802154_region *region,
@@ -873,6 +1065,40 @@ TRACE_EVENT(region_get_access,
 		  __entry->next_in_region_dtu, __entry->region_duration_dtu)
 	);
 
+TRACE_EVENT(
+	region_idle_params,
+	TP_PROTO(const struct idle_params *params),
+	TP_ARGS(params),
+	TP_STRUCT__entry(
+		__field(s32, min_duration_dtu)
+		__field(s32, max_duration_dtu)
+		),
+	TP_fast_assign(
+		__entry->min_duration_dtu = params->min_duration_dtu;
+		__entry->max_duration_dtu = params->max_duration_dtu;
+		),
+	TP_printk("min_duration_dtu=%d max_duration_dtu=%d",
+		  __entry->min_duration_dtu,
+		  __entry->max_duration_dtu)
+);
+
+TRACE_EVENT(
+	region_idle_get_access,
+	TP_PROTO(u32 timestamp_dtu, int duration_dtu),
+	TP_ARGS(timestamp_dtu, duration_dtu),
+	TP_STRUCT__entry(
+		__field(u32, timestamp_dtu)
+		__field(int, duration_dtu)
+		),
+	TP_fast_assign(
+		__entry->timestamp_dtu = timestamp_dtu;
+		__entry->duration_dtu = duration_dtu;
+		),
+	TP_printk("timestamp_dtu=%#x duration_dtu=%d",
+		  __entry->timestamp_dtu,
+		  __entry->duration_dtu)
+);
+
 #endif /* !TRACE_H || TRACE_HEADER_MULTI_READ */
 
 #undef TRACE_INCLUDE_PATH