diff --git a/drivers/net/wireless/renesas/ra6w/testmode.c b/drivers/net/wireless/renesas/ra6w/testmode.c
new file mode 100644
index 000000000000..ea67183b3c41
--- /dev/null
+++ b/drivers/net/wireless/renesas/ra6w/testmode.c
@@ -0,0 +1,649 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * This file contains cfg80211 tetsmode routine.
+ *
+ * Copyright (C) [2022-2025] Renesas Electronics Corporation and/or its affiliates.
+ */
+
+#include <linux/version.h>
+#include <net/netlink.h>
+
+#include "core.h"
+#include "params.h"
+#include "dbg.h"
+#include "testmode.h"
+#include "cfg80211.h"
+
+#define TESTMODE_SIZE	1024
+
+static int ra6w_testmode_reg(struct ra6w_ctrl *ctrl, struct nlattr **tb)
+{
+	struct ra6w_core *core = container_of(ctrl, struct ra6w_core, ctrl);
+	struct ra6w_cfg80211_priv *priv = core->priv;
+	struct sk_buff *skb;
+	u32 memaddr;
+	u32 val32;
+	int ret = 0;
+
+	if (!tb[RA6W_TESTMODE_ATTR_REG_OFFSET]) {
+		ra6w_err("Error finding register offset\n");
+		return -ENOMSG;
+	}
+
+	memaddr = nla_get_u32(tb[RA6W_TESTMODE_ATTR_REG_OFFSET]);
+
+	switch (nla_get_u32(tb[RA6W_TESTMODE_ATTR_CMD])) {
+	case RA6W_TESTMODE_CMD_READ_REG: {
+		struct ra6w_cmd_mem_read_rsp mem_read_rsp;
+
+		ret = ra6w_ctrl_mem_read_req(ctrl, memaddr, &mem_read_rsp);
+		if (ret)
+			return ret;
+
+		skb = cfg80211_testmode_alloc_reply_skb(priv->wiphy, TESTMODE_SIZE);
+		if (!skb) {
+			ra6w_err("Error allocating memory\n");
+			return -ENOMEM;
+		}
+
+		val32 = le32_to_cpu(mem_read_rsp.memdata);
+		if (nla_put_u32(skb, RA6W_TESTMODE_ATTR_REG_VALUE32, val32))
+			goto nla_put_failure;
+
+		ret = cfg80211_testmode_reply(skb);
+	}
+		break;
+	case RA6W_TESTMODE_CMD_WRITE_REG:
+		if (!tb[RA6W_TESTMODE_ATTR_REG_VALUE32]) {
+			ra6w_err("Error finding value to write\n");
+			return -ENOMSG;
+		}
+
+		val32 = nla_get_u32(tb[RA6W_TESTMODE_ATTR_REG_VALUE32]);
+		ret = ra6w_ctrl_mem_write_req(ctrl, memaddr, val32);
+		break;
+	default:
+		ra6w_err("Unknown TM reg cmd ID\n");
+		return -EINVAL;
+	}
+
+	return ret;
+
+nla_put_failure:
+	kfree_skb(skb);
+
+	return -EMSGSIZE;
+}
+
+static int ra6w_testmode_dbg_filter(struct ra6w_ctrl *ctrl, struct nlattr **tb)
+{
+	u32 filter;
+	int ret = 0;
+
+	if (!tb[RA6W_TESTMODE_ATTR_REG_FILTER]) {
+		ra6w_err("Error finding filter value\n");
+		return -ENOMSG;
+	}
+
+	filter = nla_get_u32(tb[RA6W_TESTMODE_ATTR_REG_FILTER]);
+	ra6w_dbg("TM DBG filter, setting: 0x%x\n", filter);
+
+	switch (nla_get_u32(tb[RA6W_TESTMODE_ATTR_CMD])) {
+	case RA6W_TESTMODE_CMD_LOGMODEFILTER_SET:
+		ret = ra6w_ctrl_dbg_mode_filter_req(ctrl, filter);
+		break;
+	case RA6W_TESTMODE_CMD_DBGLEVELFILTER_SET:
+		ret = ra6w_ctrl_dbg_level_filter_req(ctrl, filter);
+		break;
+	default:
+		ra6w_err("Unknown testmode register command ID\n");
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
+static int ra6w_testmode_rf_tx(struct ra6w_ctrl *ctrl, struct nlattr **tb)
+{
+	struct ra6w_cmd_rf_tx_data data;
+
+	if (!tb[RA6W_TESTMODE_ATTR_START]) {
+		ra6w_err("Error finding RF TX parameters\n");
+		return -ENOMSG;
+	}
+
+	data.start = nla_get_u8(tb[RA6W_TESTMODE_ATTR_START]);
+	if (data.start == RA6W_TESTMODE_VALUE_START) {
+		if (!tb[RA6W_TESTMODE_ATTR_RATE] ||
+		    !tb[RA6W_TESTMODE_ATTR_POWER] ||
+		    !tb[RA6W_TESTMODE_ATTR_GI] ||
+		    !tb[RA6W_TESTMODE_ATTR_GREEN] ||
+		    !tb[RA6W_TESTMODE_ATTR_PREAMBLE] ||
+		    !tb[RA6W_TESTMODE_ATTR_QOS] ||
+		    !tb[RA6W_TESTMODE_ATTR_ACK] ||
+		    !tb[RA6W_TESTMODE_ATTR_AIFSN] ||
+		    !tb[RA6W_TESTMODE_ATTR_CH] ||
+		    !tb[RA6W_TESTMODE_ATTR_FRAMES_NUM] ||
+		    !tb[RA6W_TESTMODE_ATTR_ADDR_DEST] ||
+		    !tb[RA6W_TESTMODE_ATTR_BSSID])
+			return -EINVAL;
+
+		data.tx_rate = cpu_to_le32(nla_get_u32(tb[RA6W_TESTMODE_ATTR_RATE]));
+		data.tx_power = cpu_to_le32(nla_get_u32(tb[RA6W_TESTMODE_ATTR_POWER]));
+		data.gi = nla_get_u8(tb[RA6W_TESTMODE_ATTR_GI]);
+		data.green_field = nla_get_u8(tb[RA6W_TESTMODE_ATTR_GREEN]);
+		data.preamble_type = nla_get_u8(tb[RA6W_TESTMODE_ATTR_PREAMBLE]);
+		data.qos_enable = nla_get_u8(tb[RA6W_TESTMODE_ATTR_QOS]);
+		data.ack_policy = nla_get_u8(tb[RA6W_TESTMODE_ATTR_ACK]);
+		data.aifsn_val = nla_get_u8(tb[RA6W_TESTMODE_ATTR_AIFSN]);
+		data.frequency = cpu_to_le16(nla_get_u16(tb[RA6W_TESTMODE_ATTR_CH]));
+		data.num_frames = cpu_to_le16(nla_get_u16(tb[RA6W_TESTMODE_ATTR_FRAMES_NUM]));
+		data.frame_len = cpu_to_le16(nla_get_u16(tb[RA6W_TESTMODE_ATTR_FRAMES_LEN]));
+		data.dest_addr = cpu_to_le64(nla_get_u64(tb[RA6W_TESTMODE_ATTR_ADDR_DEST]));
+		data.bssid = cpu_to_le64(nla_get_u64(tb[RA6W_TESTMODE_ATTR_BSSID]));
+	}
+
+	return ra6w_ctrl_rf_tx_req(ctrl, &data);
+}
+
+static int ra6w_testmode_rf_cw(struct ra6w_ctrl *ctrl, struct nlattr **tb)
+{
+	struct ra6w_cmd_rf_cw_data data = { 0 };
+
+	if (!tb[RA6W_TESTMODE_ATTR_START]) {
+		ra6w_err("Error finding RF CW parameters\n");
+		return -ENOMSG;
+	}
+
+	data.start = nla_get_u8(tb[RA6W_TESTMODE_ATTR_START]);
+	if (data.start == RA6W_TESTMODE_VALUE_START) {
+		if (!tb[RA6W_TESTMODE_ATTR_POWER])
+			return -EINVAL;
+
+		data.tx_power = cpu_to_le32(nla_get_u32(tb[RA6W_TESTMODE_ATTR_POWER]));
+		if (!tb[RA6W_TESTMODE_ATTR_CH])
+			return -EINVAL;
+
+		data.frequency = cpu_to_le16(nla_get_u16(tb[RA6W_TESTMODE_ATTR_CH]));
+	}
+
+	return ra6w_ctrl_rf_cw_req(ctrl, &data);
+}
+
+static int ra6w_testmode_rf_cont(struct ra6w_ctrl *ctrl, struct nlattr **tb)
+{
+	struct ra6w_cmd_rf_cont_data data = { 0 };
+
+	if (!tb[RA6W_TESTMODE_ATTR_START]) {
+		ra6w_err("Error finding RF CONT parameters\n");
+		return -ENOMSG;
+	}
+
+	data.start = nla_get_u8(tb[RA6W_TESTMODE_ATTR_START]);
+	if (data.start == RA6W_TESTMODE_VALUE_START) {
+		if (!tb[RA6W_TESTMODE_ATTR_POWER])
+			return -EINVAL;
+
+		data.tx_power = cpu_to_le32(nla_get_u32(tb[RA6W_TESTMODE_ATTR_POWER]));
+		if (!tb[RA6W_TESTMODE_ATTR_CH])
+			return -EINVAL;
+
+		data.frequency = cpu_to_le16(nla_get_u16(tb[RA6W_TESTMODE_ATTR_CH]));
+		if (!tb[RA6W_TESTMODE_ATTR_RATE])
+			return -EINVAL;
+
+		data.tx_rate = cpu_to_le32(nla_get_u32(tb[RA6W_TESTMODE_ATTR_RATE]));
+	}
+
+	return ra6w_ctrl_rf_cont_req(ctrl, &data);
+}
+
+static int ra6w_testmode_rf_ch(struct ra6w_ctrl *ctrl, struct nlattr **tb)
+{
+	u16 frequency = 0;
+
+	if (!tb[RA6W_TESTMODE_ATTR_CH]) {
+		ra6w_err("Error finding RF CH parameters\n");
+		return -ENOMSG;
+	}
+
+	frequency = nla_get_u16(tb[RA6W_TESTMODE_ATTR_CH]);
+
+	return ra6w_ctrl_rf_ch_req(ctrl, frequency);
+}
+
+static int ra6w_testmode_rf_per(struct ra6w_ctrl *ctrl, struct nlattr **tb)
+{
+	struct ra6w_core *core = container_of(ctrl, struct ra6w_core, ctrl);
+	struct ra6w_cfg80211_priv *priv = core->priv;
+	struct ra6w_cmd_rf_per_rsp rsp = { 0 };
+	struct sk_buff *skb = NULL;
+	int ret = 0;
+	u8 start = 0;
+
+	if (!tb[RA6W_TESTMODE_ATTR_START]) {
+		ra6w_err("Error finding RF PER parameters\n");
+		return -ENOMSG;
+	}
+
+	start = nla_get_u8(tb[RA6W_TESTMODE_ATTR_START]);
+	if (start != RA6W_TESTMODE_VALUE_PER_GET)
+		return 0;
+
+	ret = ra6w_ctrl_rf_per_req(ctrl, start, &rsp);
+	if (ret)
+		return ret;
+
+	skb = cfg80211_testmode_alloc_reply_skb(priv->wiphy, TESTMODE_SIZE);
+	if (!skb) {
+		ra6w_err("Error allocating memory\n");
+		return -ENOMEM;
+	}
+
+	if (nla_put_u32(skb, RA6W_TESTMODE_ATTR_PER_PASS, le32_to_cpu(rsp.pass)))
+		goto nla_put_failure;
+
+	if (nla_put_u32(skb, RA6W_TESTMODE_ATTR_PER_FCS, le32_to_cpu(rsp.fcs)))
+		goto nla_put_failure;
+
+	if (nla_put_u32(skb, RA6W_TESTMODE_ATTR_PER_PHY, le32_to_cpu(rsp.phy)))
+		goto nla_put_failure;
+
+	if (nla_put_u32(skb, RA6W_TESTMODE_ATTR_PER_OVERFLOW, le32_to_cpu(rsp.overflow)))
+		goto nla_put_failure;
+
+	return cfg80211_testmode_reply(skb);
+
+nla_put_failure:
+	if (skb)
+		kfree_skb(skb);
+
+	return -EMSGSIZE;
+}
+
+static int ra6w_testmode_host_log_level(struct nlattr **tb)
+{
+	u8 level = 0;
+
+	if (!tb[RA6W_TESTMODE_ATTR_HOST_LOG_LEVEL]) {
+		ra6w_err("Error finding level attribute\n");
+		return -ENOMSG;
+	}
+
+	level = nla_get_u8(tb[RA6W_TESTMODE_ATTR_HOST_LOG_LEVEL]);
+
+	RA6W_SET_DBG_LEVEL(level);
+
+	return 0;
+}
+
+static int ra6w_testmode_tx_power(struct ra6w_ctrl *ctrl, struct nlattr **tb,
+				  const struct ra6w_cfg80211_vif *vif)
+{
+	u32 tx_power = 0;
+
+	if (!tb[RA6W_TESTMODE_ATTR_POWER]) {
+		ra6w_err("Error finding tx power attribute\n");
+		return -ENOMSG;
+	}
+
+	tx_power = nla_get_u32(tb[RA6W_TESTMODE_ATTR_POWER]);
+
+	return ra6w_ctrl_set_tx_power_req(ctrl, vif->vif_idx, tx_power);
+}
+
+static struct ra6w_cfg80211_sta *ra6w_testmode_sta_get(struct ra6w_cfg80211_vif *vif)
+{
+	struct ra6w_cfg80211_sta *sta = NULL;
+
+	if (vif->type == NL80211_IFTYPE_STATION ||
+	    vif->type == NL80211_IFTYPE_P2P_CLIENT)
+		return vif->sta.ap;
+
+	if (vif->type == NL80211_IFTYPE_AP ||
+	    vif->type == NL80211_IFTYPE_P2P_GO) {
+		list_for_each_entry(sta, &vif->ap.sta_list, list) {
+			if (sta->valid)
+				return sta;
+		}
+	}
+
+	return NULL;
+}
+
+static int ra6w_testmode_stats_start(struct ra6w_ctrl *ctrl, struct nlattr **tb,
+				     struct ra6w_cfg80211_vif *vif)
+{
+	int ret = 0;
+
+	ret = ra6w_stats_init(&vif->stats);
+	if (ret)
+		return ret;
+
+	return ra6w_ctrl_stats_tx_start_req(ctrl, RA6W_STATS_TX_START_BIT);
+}
+
+static int ra6w_testmode_stats_stop(struct ra6w_cfg80211_vif *vif)
+{
+	ra6w_stats_deinit(&vif->stats);
+
+	return 0;
+}
+
+static int ra6w_testmode_stats_tx(struct ra6w_ctrl *ctrl, struct nlattr **tb,
+				  struct ra6w_cfg80211_vif *vif)
+{
+	int ret = 0;
+	struct ra6w_cmd_stats_tx_rsp rsp = { 0 };
+	struct ra6w_core *core = container_of(ctrl, struct ra6w_core, ctrl);
+	struct ra6w_cfg80211_priv *priv = core->priv;
+	struct ra6w_cfg80211_sta *sta = NULL;
+	struct wiphy *wiphy = priv->wiphy;
+	struct sk_buff *skb = NULL;
+	char bssid[RA6W_MAC_ADDR_STR_LEN] = { 0 };
+	char mac_address[RA6W_MAC_ADDR_STR_LEN] = { 0 };
+
+	if (!vif->stats.stats_enabled)
+		return -EINVAL;
+
+	sta = ra6w_testmode_sta_get(vif);
+	if (!sta)
+		return -EINVAL;
+
+	ret = ra6w_ctrl_stats_tx_req(ctrl, RA6W_STATS_TX_REQ_BIT, &rsp);
+	if (ret)
+		return ret;
+
+	skb = cfg80211_testmode_alloc_reply_skb(wiphy, TESTMODE_SIZE);
+	if (!skb)
+		return -ENOMEM;
+
+	snprintf(bssid, sizeof(bssid), "%pM", sta->mac_addr);
+	snprintf(mac_address, sizeof(mac_address), "%pM", wiphy->perm_addr);
+
+	if (nla_put_u8(skb, RA6W_TESTMODE_ATTR_STATS_FLAGS, rsp.format_mod) ||
+	    nla_put(skb, RA6W_TESTMODE_ATTR_STATS_TX_EPR, sizeof(rsp.epr), rsp.epr) ||
+	    nla_put_string(skb, RA6W_TESTMODE_ATTR_STATS_BSSID, bssid) ||
+	    nla_put_string(skb, RA6W_TESTMODE_ATTR_STATS_OWN_MAC, mac_address) ||
+	    nla_put_u8(skb, RA6W_TESTMODE_ATTR_STATS_STAIDX, sta->sta_idx))
+		goto nla_put_failure;
+
+	switch (rsp.format_mod) {
+	case RA6W_CFG80211_FORMATMOD_NON_HT:
+	case RA6W_CFG80211_FORMATMOD_NON_HT_DUP_OFDM:
+		if (nla_put(skb, RA6W_TESTMODE_ATTR_STATS_TX_CCK, sizeof(rsp.non_ht.success),
+			    rsp.non_ht.success) ||
+		    nla_put(skb, RA6W_TESTMODE_ATTR_STATS_TX_CCK_FAIL, sizeof(rsp.non_ht.fail),
+			    rsp.non_ht.fail))
+			goto nla_put_failure;
+		break;
+	case RA6W_CFG80211_FORMATMOD_HT_MF:
+	case RA6W_CFG80211_FORMATMOD_HT_GF:
+		if (nla_put(skb, RA6W_TESTMODE_ATTR_STATS_TX_HT, sizeof(rsp.ht.success),
+			    rsp.ht.success) ||
+		    nla_put(skb, RA6W_TESTMODE_ATTR_STATS_TX_HT_FAIL, sizeof(rsp.ht.fail),
+			    rsp.ht.fail))
+			goto nla_put_failure;
+		break;
+	case RA6W_CFG80211_FORMATMOD_VHT:
+		if (nla_put(skb, RA6W_TESTMODE_ATTR_STATS_TX_VHT, sizeof(rsp.vht.success),
+			    rsp.vht.success) ||
+		    nla_put(skb, RA6W_TESTMODE_ATTR_STATS_TX_VHT_FAIL, sizeof(rsp.vht.fail),
+			    rsp.vht.fail))
+			goto nla_put_failure;
+		break;
+	case RA6W_CFG80211_FORMATMOD_HE_SU:
+	case RA6W_CFG80211_FORMATMOD_HE_ER:
+	case RA6W_CFG80211_FORMATMOD_HE_MU:
+	case RA6W_CFG80211_FORMATMOD_HE_TB:
+		if (nla_put(skb, RA6W_TESTMODE_ATTR_STATS_TX_HE, sizeof(rsp.he.success),
+			    rsp.he.success) ||
+		    nla_put(skb, RA6W_TESTMODE_ATTR_STATS_TX_HE_FAIL, sizeof(rsp.he.fail),
+			    rsp.he.fail))
+			goto nla_put_failure;
+		break;
+	};
+
+	return cfg80211_testmode_reply(skb);
+
+nla_put_failure:
+	kfree_skb(skb);
+
+	return -EMSGSIZE;
+}
+
+static int ra6w_testmode_stats_rx(struct ra6w_ctrl *ctrl, struct nlattr **tb,
+				  struct ra6w_cfg80211_vif *vif)
+{
+	struct ra6w_core *core = container_of(ctrl, struct ra6w_core, ctrl);
+	struct ra6w_cfg80211_priv *priv = core->priv;
+	struct ra6w_cfg80211_sta *sta = NULL;
+	struct wiphy *wiphy = priv->wiphy;
+	struct sk_buff *skb = NULL;
+	struct ra6w_stats *stats = &vif->stats;
+	struct ra6w_stats_rx *rx_stats = stats->rx_stats;
+	char bssid[RA6W_MAC_ADDR_STR_LEN] = { 0 };
+	char mac_address[RA6W_MAC_ADDR_STR_LEN] = { 0 };
+
+	if (!stats->stats_enabled || !rx_stats)
+		return -EINVAL;
+
+	sta = ra6w_testmode_sta_get(vif);
+	if (!sta)
+		return -EINVAL;
+
+	skb = cfg80211_testmode_alloc_reply_skb(wiphy, TESTMODE_SIZE);
+	if (!skb)
+		return -ENOMEM;
+
+	if (nla_put_u8(skb, RA6W_TESTMODE_ATTR_STATS_READY, vif->stats.stats_enabled))
+		goto nla_put_failure;
+
+	snprintf(bssid, sizeof(bssid), "%pM", sta->mac_addr);
+	snprintf(mac_address, sizeof(mac_address), "%pM", wiphy->perm_addr);
+
+	if (nla_put_u8(skb, RA6W_TESTMODE_ATTR_STATS_FLAGS, rx_stats->flags) ||
+	    nla_put_string(skb, RA6W_TESTMODE_ATTR_STATS_BSSID, bssid) ||
+	    nla_put_string(skb, RA6W_TESTMODE_ATTR_STATS_OWN_MAC, mac_address) ||
+	    nla_put_u8(skb, RA6W_TESTMODE_ATTR_STATS_STAIDX, sta->sta_idx))
+		goto nla_put_failure;
+
+	if (rx_stats->flags & RA6W_STATS_RX_OFDM_BIT ||
+	    rx_stats->flags & RA6W_STATS_RX_CCK_BIT) {
+		if (nla_put_u8(skb, RA6W_TESTMODE_ATTR_STATS_RX_BW, rx_stats->non_ht.bw) ||
+		    nla_put_u64_64bit(skb, RA6W_TESTMODE_ATTR_STATS_RX_CCK, rx_stats->non_ht.cck,
+				      NL80211_ATTR_PAD) ||
+		    nla_put_u64_64bit(skb, RA6W_TESTMODE_ATTR_STATS_RX_OFDM,
+				      rx_stats->non_ht.ofdm, NL80211_ATTR_PAD))
+			goto nla_put_failure;
+	}
+
+	if (rx_stats->flags & RA6W_STATS_RX_HT_BIT) {
+		if (nla_put_u8(skb, RA6W_TESTMODE_ATTR_STATS_RX_BW, rx_stats->ht.bw) ||
+		    nla_put_u8(skb, RA6W_TESTMODE_ATTR_STATS_RX_NSS, rx_stats->ht.nss) ||
+		    nla_put_u8(skb, RA6W_TESTMODE_ATTR_STATS_RX_GI, rx_stats->ht.gi) ||
+		    nla_put(skb, RA6W_TESTMODE_ATTR_STATS_RX_HT, sizeof(rx_stats->ht.ht),
+			    rx_stats->ht.ht))
+			goto nla_put_failure;
+	}
+
+	if (rx_stats->flags & RA6W_STATS_RX_VHT_BIT) {
+		if (nla_put_u8(skb, RA6W_TESTMODE_ATTR_STATS_RX_BW, rx_stats->vht.bw) ||
+		    nla_put_u8(skb, RA6W_TESTMODE_ATTR_STATS_RX_NSS, rx_stats->vht.nss) ||
+		    nla_put_u8(skb, RA6W_TESTMODE_ATTR_STATS_RX_GI, rx_stats->vht.gi) ||
+		    nla_put(skb, RA6W_TESTMODE_ATTR_STATS_RX_VHT, sizeof(rx_stats->vht.vht),
+			    rx_stats->vht.vht))
+			goto nla_put_failure;
+	}
+
+	if (rx_stats->flags & RA6W_STATS_RX_HE_SU_BIT) {
+		if (nla_put_u8(skb, RA6W_TESTMODE_ATTR_STATS_RX_BW, rx_stats->he_su.bw) ||
+		    nla_put_u8(skb, RA6W_TESTMODE_ATTR_STATS_RX_NSS, rx_stats->he_su.nss) ||
+		    nla_put_u8(skb, RA6W_TESTMODE_ATTR_STATS_RX_GI, rx_stats->he_su.gi) ||
+		    nla_put(skb, RA6W_TESTMODE_ATTR_STATS_RX_HE_SU, sizeof(rx_stats->he_su.he),
+			    rx_stats->he_su.he))
+			goto nla_put_failure;
+	}
+
+	return cfg80211_testmode_reply(skb);
+
+nla_put_failure:
+	kfree_skb(skb);
+
+	return -EMSGSIZE;
+}
+
+static int ra6w_testmode_stats_rssi(struct ra6w_ctrl *ctrl, struct nlattr **tb,
+				    struct ra6w_cfg80211_vif *vif)
+{
+	struct ra6w_core *core = container_of(ctrl, struct ra6w_core, ctrl);
+	struct ra6w_cfg80211_priv *priv = core->priv;
+	struct ra6w_cfg80211_sta *sta = vif->sta.ap;
+	struct wiphy *wiphy = priv->wiphy;
+	struct sk_buff *skb = NULL;
+	struct ra6w_stats *stats = &vif->stats;
+	char bssid[RA6W_MAC_ADDR_STR_LEN] = { 0 };
+	char mac_address[RA6W_MAC_ADDR_STR_LEN] = { 0 };
+
+	if (!stats->stats_enabled)
+		return -EINVAL;
+
+	if (vif->type == NL80211_IFTYPE_AP ||
+	    vif->type == NL80211_IFTYPE_P2P_GO)
+		return -EINVAL;
+
+	if (!sta)
+		return -EINVAL;
+
+	skb = cfg80211_testmode_alloc_reply_skb(wiphy, TESTMODE_SIZE);
+	if (!skb)
+		return -ENOMEM;
+
+	snprintf(bssid, sizeof(bssid), "%pM", sta->mac_addr);
+	snprintf(mac_address, sizeof(mac_address), "%pM", wiphy->perm_addr);
+
+	if (nla_put_u8(skb, RA6W_TESTMODE_ATTR_STATS_READY, stats->stats_enabled) ||
+	    nla_put_u32(skb, RA6W_TESTMODE_ATTR_STATS_RSSI, sta->stats.last_rx_data_ext.rssi1) ||
+	    nla_put_string(skb, RA6W_TESTMODE_ATTR_STATS_BSSID, bssid) ||
+	    nla_put_string(skb, RA6W_TESTMODE_ATTR_STATS_OWN_MAC, mac_address) ||
+	    nla_put_u8(skb, RA6W_TESTMODE_ATTR_STATS_STAIDX, sta->sta_idx))
+		goto nla_put_failure;
+
+	return cfg80211_testmode_reply(skb);
+
+nla_put_failure:
+	kfree_skb(skb);
+
+	return -EMSGSIZE;
+}
+
+static const struct nla_policy ra6w_testmode_attr_policy[RA6W_TESTMODE_ATTR_MAX] = {
+	[RA6W_TESTMODE_ATTR_CMD] = { .type = NLA_U32 },
+	[RA6W_TESTMODE_ATTR_REG_OFFSET]  = { .type = NLA_U32 },
+	[RA6W_TESTMODE_ATTR_REG_VALUE32] = { .type = NLA_U32 },
+	[RA6W_TESTMODE_ATTR_REG_FILTER]  = { .type = NLA_U32 },
+
+	/* RF commands */
+	[RA6W_TESTMODE_ATTR_START]       = { .type = NLA_U8 },
+	[RA6W_TESTMODE_ATTR_CH]          = { .type = NLA_U16 },
+	[RA6W_TESTMODE_ATTR_FRAMES_NUM]  = { .type = NLA_U16 },
+	[RA6W_TESTMODE_ATTR_FRAMES_LEN]  = { .type = NLA_U16 },
+	[RA6W_TESTMODE_ATTR_RATE]        = { .type = NLA_U32 },
+	[RA6W_TESTMODE_ATTR_POWER]       = { .type = NLA_U32 },
+	[RA6W_TESTMODE_ATTR_ADDR_DEST]   = { .type = NLA_U64 },
+	[RA6W_TESTMODE_ATTR_BSSID]       = { .type = NLA_U64 },
+	[RA6W_TESTMODE_ATTR_GI]          = { .type = NLA_U8 },
+	[RA6W_TESTMODE_ATTR_GREEN]       = { .type = NLA_U8 },
+	[RA6W_TESTMODE_ATTR_PREAMBLE]    = { .type = NLA_U8 },
+	[RA6W_TESTMODE_ATTR_QOS]         = { .type = NLA_U8 },
+	[RA6W_TESTMODE_ATTR_ACK]         = { .type = NLA_U8 },
+	[RA6W_TESTMODE_ATTR_AIFSN]       = { .type = NLA_U8 },
+	[RA6W_TESTMODE_ATTR_PER_PASS]    = { .type = NLA_U32 },
+	[RA6W_TESTMODE_ATTR_PER_FCS]     = { .type = NLA_U32 },
+	[RA6W_TESTMODE_ATTR_PER_PHY]     = { .type = NLA_U32 },
+	[RA6W_TESTMODE_ATTR_PER_OVERFLOW] = { .type = NLA_U32 },
+	[RA6W_TESTMODE_ATTR_HOST_LOG_LEVEL] = { .type = NLA_U8 },
+	[RA6W_TESTMODE_ATTR_STATS_RSSI] = { .type = NLA_U32 },
+	[RA6W_TESTMODE_ATTR_STATS_BSSID] = { .type = NLA_STRING },
+	[RA6W_TESTMODE_ATTR_STATS_STAIDX] = { .type = NLA_U8 },
+	[RA6W_TESTMODE_ATTR_STATS_OWN_MAC] = { .type = NLA_STRING },
+	[RA6W_TESTMODE_ATTR_STATS_FLAGS] = { .type = NLA_U8 },
+	[RA6W_TESTMODE_ATTR_STATS_RX_OFDM] = { .type = NLA_U32, .len = 8 },
+	[RA6W_TESTMODE_ATTR_STATS_READY] = { .type = NLA_U8 },
+};
+
+int ra6w_testmode_cmd(struct wiphy *wiphy, struct wireless_dev *wdev, void *data, int len)
+{
+	struct ra6w_cfg80211_priv *priv = wiphy_priv(wiphy);
+	struct nlattr *tb[RA6W_TESTMODE_ATTR_MAX];
+	int ret = -ENOENT;
+	const struct net_device *ndev = wdev->netdev;
+	struct ra6w_cfg80211_vif *vif = netdev_priv(ndev);
+	struct ra6w_ctrl *ctrl = &priv->core->ctrl;
+	u32 cmd;
+
+	ret = nla_parse_deprecated(tb, RA6W_TESTMODE_ATTR_MAX, data, len,
+				   ra6w_testmode_attr_policy, NULL);
+	if (ret)
+		return ret;
+
+	if (!tb[RA6W_TESTMODE_ATTR_CMD]) {
+		ra6w_err("Error finding testmode command type\n");
+		return -ENOMSG;
+	}
+
+	cmd = nla_get_u32(tb[RA6W_TESTMODE_ATTR_CMD]);
+
+	switch (cmd) {
+	case RA6W_TESTMODE_CMD_READ_REG:
+	case RA6W_TESTMODE_CMD_WRITE_REG:
+		ret = ra6w_testmode_reg(ctrl, tb);
+		break;
+	case RA6W_TESTMODE_CMD_LOGMODEFILTER_SET:
+	case RA6W_TESTMODE_CMD_DBGLEVELFILTER_SET:
+		ret = ra6w_testmode_dbg_filter(ctrl, tb);
+		break;
+	case RA6W_TESTMODE_CMD_TX:
+		ret = ra6w_testmode_rf_tx(ctrl, tb);
+		break;
+	case RA6W_TESTMODE_CMD_CW:
+		ret = ra6w_testmode_rf_cw(ctrl, tb);
+		break;
+	case RA6W_TESTMODE_CMD_CONT:
+		ret = ra6w_testmode_rf_cont(ctrl, tb);
+		break;
+	case RA6W_TESTMODE_CMD_CHANNEL:
+		ret = ra6w_testmode_rf_ch(ctrl, tb);
+		break;
+	case RA6W_TESTMODE_CMD_PER:
+		ret = ra6w_testmode_rf_per(ctrl, tb);
+		break;
+	case RA6W_TESTMODE_CMD_HOST_LOG_LEVEL:
+		ret = ra6w_testmode_host_log_level(tb);
+		break;
+	case RA6W_TESTMODE_CMD_TX_POWER:
+		ret = ra6w_testmode_tx_power(ctrl, tb, vif);
+		break;
+	case RA6W_TESTMODE_CMD_STATS_START:
+		ret = ra6w_testmode_stats_start(ctrl, tb, vif);
+		break;
+	case RA6W_TESTMODE_CMD_STATS_STOP:
+		ret = ra6w_testmode_stats_stop(vif);
+		break;
+	case RA6W_TESTMODE_CMD_STATS_TX:
+		ret = ra6w_testmode_stats_tx(ctrl, tb, vif);
+		break;
+	case RA6W_TESTMODE_CMD_STATS_RX:
+		ret = ra6w_testmode_stats_rx(ctrl, tb, vif);
+		break;
+	case RA6W_TESTMODE_CMD_STATS_RSSI:
+		ret = ra6w_testmode_stats_rssi(ctrl, tb, vif);
+		break;
+	default:
+		ra6w_err("Unknown testmode command: %u\n", cmd);
+		ret = -EOPNOTSUPP;
+		break;
+	}
+
+	return ret;
+}
