cachepc-linux

Fork of AMDESE/linux with modifications for CachePC side-channel attack
git clone https://git.sinitax.com/sinitax/cachepc-linux
Log | Files | Refs | README | LICENSE | sfeed.txt

testmode.c (19465B)


      1// SPDX-License-Identifier: ISC
      2/* Copyright (C) 2020 MediaTek Inc. */
      3
      4#include "mt7915.h"
      5#include "mac.h"
      6#include "mcu.h"
      7#include "testmode.h"
      8
      9enum {
     10	TM_CHANGED_TXPOWER,
     11	TM_CHANGED_FREQ_OFFSET,
     12
     13	/* must be last */
     14	NUM_TM_CHANGED
     15};
     16
     17static const u8 tm_change_map[] = {
     18	[TM_CHANGED_TXPOWER] = MT76_TM_ATTR_TX_POWER,
     19	[TM_CHANGED_FREQ_OFFSET] = MT76_TM_ATTR_FREQ_OFFSET,
     20};
     21
     22struct reg_band {
     23	u32 band[2];
     24};
     25
     26#define REG_BAND(_list, _reg) \
     27		{ _list.band[0] = MT_##_reg(0);	\
     28		  _list.band[1] = MT_##_reg(1); }
     29#define REG_BAND_IDX(_list, _reg, _idx) \
     30		{ _list.band[0] = MT_##_reg(0, _idx);	\
     31		  _list.band[1] = MT_##_reg(1, _idx); }
     32
     33#define TM_REG_MAX_ID	17
     34static struct reg_band reg_backup_list[TM_REG_MAX_ID];
     35
     36
     37static int
     38mt7915_tm_set_tx_power(struct mt7915_phy *phy)
     39{
     40	struct mt7915_dev *dev = phy->dev;
     41	struct mt76_phy *mphy = phy->mt76;
     42	struct cfg80211_chan_def *chandef = &mphy->chandef;
     43	int freq = chandef->center_freq1;
     44	int ret;
     45	struct {
     46		u8 format_id;
     47		u8 dbdc_idx;
     48		s8 tx_power;
     49		u8 ant_idx;	/* Only 0 is valid */
     50		u8 center_chan;
     51		u8 rsv[3];
     52	} __packed req = {
     53		.format_id = 0xf,
     54		.dbdc_idx = phy != &dev->phy,
     55		.center_chan = ieee80211_frequency_to_channel(freq),
     56	};
     57	u8 *tx_power = NULL;
     58
     59	if (phy->mt76->test.state != MT76_TM_STATE_OFF)
     60		tx_power = phy->mt76->test.tx_power;
     61
     62	/* Tx power of the other antennas are the same as antenna 0 */
     63	if (tx_power && tx_power[0])
     64		req.tx_power = tx_power[0];
     65
     66	ret = mt76_mcu_send_msg(&dev->mt76,
     67				MCU_EXT_CMD(TX_POWER_FEATURE_CTRL),
     68				&req, sizeof(req), false);
     69
     70	return ret;
     71}
     72
     73static int
     74mt7915_tm_set_freq_offset(struct mt7915_phy *phy, bool en, u32 val)
     75{
     76	struct mt7915_dev *dev = phy->dev;
     77	struct mt7915_tm_cmd req = {
     78		.testmode_en = en,
     79		.param_idx = MCU_ATE_SET_FREQ_OFFSET,
     80		.param.freq.band = phy != &dev->phy,
     81		.param.freq.freq_offset = cpu_to_le32(val),
     82	};
     83
     84	return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(ATE_CTRL), &req,
     85				 sizeof(req), false);
     86}
     87
     88static int
     89mt7915_tm_mode_ctrl(struct mt7915_dev *dev, bool enable)
     90{
     91	struct {
     92		u8 format_id;
     93		bool enable;
     94		u8 rsv[2];
     95	} __packed req = {
     96		.format_id = 0x6,
     97		.enable = enable,
     98	};
     99
    100	return mt76_mcu_send_msg(&dev->mt76,
    101				 MCU_EXT_CMD(TX_POWER_FEATURE_CTRL),
    102				 &req, sizeof(req), false);
    103}
    104
    105static int
    106mt7915_tm_set_trx(struct mt7915_phy *phy, int type, bool en)
    107{
    108	struct mt7915_dev *dev = phy->dev;
    109	struct mt7915_tm_cmd req = {
    110		.testmode_en = 1,
    111		.param_idx = MCU_ATE_SET_TRX,
    112		.param.trx.type = type,
    113		.param.trx.enable = en,
    114		.param.trx.band = phy != &dev->phy,
    115	};
    116
    117	return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(ATE_CTRL), &req,
    118				 sizeof(req), false);
    119}
    120
    121static int
    122mt7915_tm_clean_hwq(struct mt7915_phy *phy, u8 wcid)
    123{
    124	struct mt7915_dev *dev = phy->dev;
    125	struct mt7915_tm_cmd req = {
    126		.testmode_en = 1,
    127		.param_idx = MCU_ATE_CLEAN_TXQUEUE,
    128		.param.clean.wcid = wcid,
    129		.param.clean.band = phy != &dev->phy,
    130	};
    131
    132	return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(ATE_CTRL), &req,
    133				 sizeof(req), false);
    134}
    135
    136static int
    137mt7915_tm_set_slot_time(struct mt7915_phy *phy, u8 slot_time, u8 sifs)
    138{
    139	struct mt7915_dev *dev = phy->dev;
    140	struct mt7915_tm_cmd req = {
    141		.testmode_en = !(phy->mt76->test.state == MT76_TM_STATE_OFF),
    142		.param_idx = MCU_ATE_SET_SLOT_TIME,
    143		.param.slot.slot_time = slot_time,
    144		.param.slot.sifs = sifs,
    145		.param.slot.rifs = 2,
    146		.param.slot.eifs = cpu_to_le16(60),
    147		.param.slot.band = phy != &dev->phy,
    148	};
    149
    150	return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(ATE_CTRL), &req,
    151				 sizeof(req), false);
    152}
    153
    154static int
    155mt7915_tm_set_tam_arb(struct mt7915_phy *phy, bool enable, bool mu)
    156{
    157	struct mt7915_dev *dev = phy->dev;
    158	u32 op_mode;
    159
    160	if (!enable)
    161		op_mode = TAM_ARB_OP_MODE_NORMAL;
    162	else if (mu)
    163		op_mode = TAM_ARB_OP_MODE_TEST;
    164	else
    165		op_mode = TAM_ARB_OP_MODE_FORCE_SU;
    166
    167	return mt7915_mcu_set_muru_ctrl(dev, MURU_SET_ARB_OP_MODE, op_mode);
    168}
    169
    170static int
    171mt7915_tm_set_wmm_qid(struct mt7915_dev *dev, u8 qid, u8 aifs, u8 cw_min,
    172		      u16 cw_max, u16 txop)
    173{
    174	struct mt7915_mcu_tx req = { .total = 1 };
    175	struct edca *e = &req.edca[0];
    176
    177	e->queue = qid;
    178	e->set = WMM_PARAM_SET;
    179
    180	e->aifs = aifs;
    181	e->cw_min = cw_min;
    182	e->cw_max = cpu_to_le16(cw_max);
    183	e->txop = cpu_to_le16(txop);
    184
    185	return mt7915_mcu_update_edca(dev, &req);
    186}
    187
    188static int
    189mt7915_tm_set_ipg_params(struct mt7915_phy *phy, u32 ipg, u8 mode)
    190{
    191#define TM_DEFAULT_SIFS	10
    192#define TM_MAX_SIFS	127
    193#define TM_MAX_AIFSN	0xf
    194#define TM_MIN_AIFSN	0x1
    195#define BBP_PROC_TIME	1500
    196	struct mt7915_dev *dev = phy->dev;
    197	u8 sig_ext = (mode == MT76_TM_TX_MODE_CCK) ? 0 : 6;
    198	u8 slot_time = 9, sifs = TM_DEFAULT_SIFS;
    199	u8 aifsn = TM_MIN_AIFSN;
    200	u32 i2t_time, tr2t_time, txv_time;
    201	u16 cw = 0;
    202
    203	if (ipg < sig_ext + slot_time + sifs)
    204		ipg = 0;
    205
    206	if (!ipg)
    207		goto done;
    208
    209	ipg -= sig_ext;
    210
    211	if (ipg <= (TM_MAX_SIFS + slot_time)) {
    212		sifs = ipg - slot_time;
    213	} else {
    214		u32 val = (ipg + slot_time) / slot_time;
    215
    216		while (val >>= 1)
    217			cw++;
    218
    219		if (cw > 16)
    220			cw = 16;
    221
    222		ipg -= ((1 << cw) - 1) * slot_time;
    223
    224		aifsn = ipg / slot_time;
    225		if (aifsn > TM_MAX_AIFSN)
    226			aifsn = TM_MAX_AIFSN;
    227
    228		ipg -= aifsn * slot_time;
    229
    230		if (ipg > TM_DEFAULT_SIFS)
    231			sifs = min_t(u32, ipg, TM_MAX_SIFS);
    232	}
    233done:
    234	txv_time = mt76_get_field(dev, MT_TMAC_ATCR(phy->band_idx),
    235				  MT_TMAC_ATCR_TXV_TOUT);
    236	txv_time *= 50;	/* normal clock time */
    237
    238	i2t_time = (slot_time * 1000 - txv_time - BBP_PROC_TIME) / 50;
    239	tr2t_time = (sifs * 1000 - txv_time - BBP_PROC_TIME) / 50;
    240
    241	mt76_set(dev, MT_TMAC_TRCR0(phy->band_idx),
    242		 FIELD_PREP(MT_TMAC_TRCR0_TR2T_CHK, tr2t_time) |
    243		 FIELD_PREP(MT_TMAC_TRCR0_I2T_CHK, i2t_time));
    244
    245	mt7915_tm_set_slot_time(phy, slot_time, sifs);
    246
    247	return mt7915_tm_set_wmm_qid(dev,
    248				     mt76_connac_lmac_mapping(IEEE80211_AC_BE),
    249				     aifsn, cw, cw, 0);
    250}
    251
    252static int
    253mt7915_tm_set_tx_len(struct mt7915_phy *phy, u32 tx_time)
    254{
    255	struct mt76_phy *mphy = phy->mt76;
    256	struct mt76_testmode_data *td = &mphy->test;
    257	struct ieee80211_supported_band *sband;
    258	struct rate_info rate = {};
    259	u16 flags = 0, tx_len;
    260	u32 bitrate;
    261	int ret;
    262
    263	if (!tx_time)
    264		return 0;
    265
    266	rate.mcs = td->tx_rate_idx;
    267	rate.nss = td->tx_rate_nss;
    268
    269	switch (td->tx_rate_mode) {
    270	case MT76_TM_TX_MODE_CCK:
    271	case MT76_TM_TX_MODE_OFDM:
    272		if (mphy->chandef.chan->band == NL80211_BAND_5GHZ)
    273			sband = &mphy->sband_5g.sband;
    274		else if (mphy->chandef.chan->band == NL80211_BAND_6GHZ)
    275			sband = &mphy->sband_6g.sband;
    276		else
    277			sband = &mphy->sband_2g.sband;
    278
    279		rate.legacy = sband->bitrates[rate.mcs].bitrate;
    280		break;
    281	case MT76_TM_TX_MODE_HT:
    282		rate.mcs += rate.nss * 8;
    283		flags |= RATE_INFO_FLAGS_MCS;
    284
    285		if (td->tx_rate_sgi)
    286			flags |= RATE_INFO_FLAGS_SHORT_GI;
    287		break;
    288	case MT76_TM_TX_MODE_VHT:
    289		flags |= RATE_INFO_FLAGS_VHT_MCS;
    290
    291		if (td->tx_rate_sgi)
    292			flags |= RATE_INFO_FLAGS_SHORT_GI;
    293		break;
    294	case MT76_TM_TX_MODE_HE_SU:
    295	case MT76_TM_TX_MODE_HE_EXT_SU:
    296	case MT76_TM_TX_MODE_HE_TB:
    297	case MT76_TM_TX_MODE_HE_MU:
    298		rate.he_gi = td->tx_rate_sgi;
    299		flags |= RATE_INFO_FLAGS_HE_MCS;
    300		break;
    301	default:
    302		break;
    303	}
    304	rate.flags = flags;
    305
    306	switch (mphy->chandef.width) {
    307	case NL80211_CHAN_WIDTH_160:
    308	case NL80211_CHAN_WIDTH_80P80:
    309		rate.bw = RATE_INFO_BW_160;
    310		break;
    311	case NL80211_CHAN_WIDTH_80:
    312		rate.bw = RATE_INFO_BW_80;
    313		break;
    314	case NL80211_CHAN_WIDTH_40:
    315		rate.bw = RATE_INFO_BW_40;
    316		break;
    317	default:
    318		rate.bw = RATE_INFO_BW_20;
    319		break;
    320	}
    321
    322	bitrate = cfg80211_calculate_bitrate(&rate);
    323	tx_len = bitrate * tx_time / 10 / 8;
    324
    325	ret = mt76_testmode_alloc_skb(phy->mt76, tx_len);
    326	if (ret)
    327		return ret;
    328
    329	return 0;
    330}
    331
    332static void
    333mt7915_tm_reg_backup_restore(struct mt7915_phy *phy)
    334{
    335	int n_regs = ARRAY_SIZE(reg_backup_list);
    336	struct mt7915_dev *dev = phy->dev;
    337	u32 *b = phy->test.reg_backup;
    338	int i;
    339
    340	REG_BAND_IDX(reg_backup_list[0], AGG_PCR0, 0);
    341	REG_BAND_IDX(reg_backup_list[1], AGG_PCR0, 1);
    342	REG_BAND_IDX(reg_backup_list[2], AGG_AWSCR0, 0);
    343	REG_BAND_IDX(reg_backup_list[3], AGG_AWSCR0, 1);
    344	REG_BAND_IDX(reg_backup_list[4], AGG_AWSCR0, 2);
    345	REG_BAND_IDX(reg_backup_list[5], AGG_AWSCR0, 3);
    346	REG_BAND(reg_backup_list[6], AGG_MRCR);
    347	REG_BAND(reg_backup_list[7], TMAC_TFCR0);
    348	REG_BAND(reg_backup_list[8], TMAC_TCR0);
    349	REG_BAND(reg_backup_list[9], AGG_ATCR1);
    350	REG_BAND(reg_backup_list[10], AGG_ATCR3);
    351	REG_BAND(reg_backup_list[11], TMAC_TRCR0);
    352	REG_BAND(reg_backup_list[12], TMAC_ICR0);
    353	REG_BAND_IDX(reg_backup_list[13], ARB_DRNGR0, 0);
    354	REG_BAND_IDX(reg_backup_list[14], ARB_DRNGR0, 1);
    355	REG_BAND(reg_backup_list[15], WF_RFCR);
    356	REG_BAND(reg_backup_list[16], WF_RFCR1);
    357
    358	if (phy->mt76->test.state == MT76_TM_STATE_OFF) {
    359		for (i = 0; i < n_regs; i++)
    360			mt76_wr(dev, reg_backup_list[i].band[phy->band_idx], b[i]);
    361		return;
    362	}
    363
    364	if (!b) {
    365		b = devm_kzalloc(dev->mt76.dev, 4 * n_regs, GFP_KERNEL);
    366		if (!b)
    367			return;
    368
    369		phy->test.reg_backup = b;
    370		for (i = 0; i < n_regs; i++)
    371			b[i] = mt76_rr(dev, reg_backup_list[i].band[phy->band_idx]);
    372	}
    373
    374	mt76_clear(dev, MT_AGG_PCR0(phy->band_idx, 0), MT_AGG_PCR0_MM_PROT |
    375		   MT_AGG_PCR0_GF_PROT | MT_AGG_PCR0_ERP_PROT |
    376		   MT_AGG_PCR0_VHT_PROT | MT_AGG_PCR0_BW20_PROT |
    377		   MT_AGG_PCR0_BW40_PROT | MT_AGG_PCR0_BW80_PROT);
    378	mt76_set(dev, MT_AGG_PCR0(phy->band_idx, 0), MT_AGG_PCR0_PTA_WIN_DIS);
    379
    380	mt76_wr(dev, MT_AGG_PCR0(phy->band_idx, 1), MT_AGG_PCR1_RTS0_NUM_THRES |
    381		MT_AGG_PCR1_RTS0_LEN_THRES);
    382
    383	mt76_clear(dev, MT_AGG_MRCR(phy->band_idx), MT_AGG_MRCR_BAR_CNT_LIMIT |
    384		   MT_AGG_MRCR_LAST_RTS_CTS_RN | MT_AGG_MRCR_RTS_FAIL_LIMIT |
    385		   MT_AGG_MRCR_TXCMD_RTS_FAIL_LIMIT);
    386
    387	mt76_rmw(dev, MT_AGG_MRCR(phy->band_idx), MT_AGG_MRCR_RTS_FAIL_LIMIT |
    388		 MT_AGG_MRCR_TXCMD_RTS_FAIL_LIMIT,
    389		 FIELD_PREP(MT_AGG_MRCR_RTS_FAIL_LIMIT, 1) |
    390		 FIELD_PREP(MT_AGG_MRCR_TXCMD_RTS_FAIL_LIMIT, 1));
    391
    392	mt76_wr(dev, MT_TMAC_TFCR0(phy->band_idx), 0);
    393	mt76_clear(dev, MT_TMAC_TCR0(phy->band_idx), MT_TMAC_TCR0_TBTT_STOP_CTRL);
    394
    395	/* config rx filter for testmode rx */
    396	mt76_wr(dev, MT_WF_RFCR(phy->band_idx), 0xcf70a);
    397	mt76_wr(dev, MT_WF_RFCR1(phy->band_idx), 0);
    398}
    399
    400static void
    401mt7915_tm_init(struct mt7915_phy *phy, bool en)
    402{
    403	struct mt7915_dev *dev = phy->dev;
    404
    405	if (!test_bit(MT76_STATE_RUNNING, &phy->mt76->state))
    406		return;
    407
    408	mt7915_mcu_set_sku_en(phy, !en);
    409
    410	mt7915_tm_mode_ctrl(dev, en);
    411	mt7915_tm_reg_backup_restore(phy);
    412	mt7915_tm_set_trx(phy, TM_MAC_TXRX, !en);
    413
    414	mt7915_mcu_add_bss_info(phy, phy->monitor_vif, en);
    415	mt7915_mcu_add_sta(dev, phy->monitor_vif, NULL, en);
    416
    417	if (!en)
    418		mt7915_tm_set_tam_arb(phy, en, 0);
    419}
    420
    421static void
    422mt7915_tm_update_channel(struct mt7915_phy *phy)
    423{
    424	mutex_unlock(&phy->dev->mt76.mutex);
    425	mt7915_set_channel(phy);
    426	mutex_lock(&phy->dev->mt76.mutex);
    427
    428	mt7915_mcu_set_chan_info(phy, MCU_EXT_CMD(SET_RX_PATH));
    429}
    430
    431static void
    432mt7915_tm_set_tx_frames(struct mt7915_phy *phy, bool en)
    433{
    434	static const u8 spe_idx_map[] = {0, 0, 1, 0, 3, 2, 4, 0,
    435					 9, 8, 6, 10, 16, 12, 18, 0};
    436	struct mt76_testmode_data *td = &phy->mt76->test;
    437	struct mt7915_dev *dev = phy->dev;
    438	struct ieee80211_tx_info *info;
    439	u8 duty_cycle = td->tx_duty_cycle;
    440	u32 tx_time = td->tx_time;
    441	u32 ipg = td->tx_ipg;
    442
    443	mt7915_tm_set_trx(phy, TM_MAC_RX_RXV, false);
    444	mt7915_tm_clean_hwq(phy, dev->mt76.global_wcid.idx);
    445
    446	if (en) {
    447		mt7915_tm_update_channel(phy);
    448
    449		if (td->tx_spe_idx) {
    450			phy->test.spe_idx = td->tx_spe_idx;
    451		} else {
    452			u8 tx_ant = td->tx_antenna_mask;
    453
    454			if (phy != &dev->phy)
    455				tx_ant >>= dev->chainshift;
    456			phy->test.spe_idx = spe_idx_map[tx_ant];
    457		}
    458	}
    459
    460	mt7915_tm_set_tam_arb(phy, en,
    461			      td->tx_rate_mode == MT76_TM_TX_MODE_HE_MU);
    462
    463	/* if all three params are set, duty_cycle will be ignored */
    464	if (duty_cycle && tx_time && !ipg) {
    465		ipg = tx_time * 100 / duty_cycle - tx_time;
    466	} else if (duty_cycle && !tx_time && ipg) {
    467		if (duty_cycle < 100)
    468			tx_time = duty_cycle * ipg / (100 - duty_cycle);
    469	}
    470
    471	mt7915_tm_set_ipg_params(phy, ipg, td->tx_rate_mode);
    472	mt7915_tm_set_tx_len(phy, tx_time);
    473
    474	if (ipg)
    475		td->tx_queued_limit = MT76_TM_TIMEOUT * 1000000 / ipg / 2;
    476
    477	if (!en || !td->tx_skb)
    478		return;
    479
    480	info = IEEE80211_SKB_CB(td->tx_skb);
    481	info->control.vif = phy->monitor_vif;
    482
    483	mt7915_tm_set_trx(phy, TM_MAC_TX, en);
    484}
    485
    486static void
    487mt7915_tm_set_rx_frames(struct mt7915_phy *phy, bool en)
    488{
    489	mt7915_tm_set_trx(phy, TM_MAC_RX_RXV, false);
    490
    491	if (en) {
    492		struct mt7915_dev *dev = phy->dev;
    493
    494		mt7915_tm_update_channel(phy);
    495
    496		/* read-clear */
    497		mt76_rr(dev, MT_MIB_SDR3(phy != &dev->phy));
    498		mt7915_tm_set_trx(phy, TM_MAC_RX_RXV, en);
    499	}
    500}
    501
    502static int
    503mt7915_tm_rf_switch_mode(struct mt7915_dev *dev, u32 oper)
    504{
    505	struct mt7915_tm_rf_test req = {
    506		.op.op_mode = cpu_to_le32(oper),
    507	};
    508
    509	return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(RF_TEST), &req,
    510				 sizeof(req), true);
    511}
    512
    513static int
    514mt7915_tm_set_tx_cont(struct mt7915_phy *phy, bool en)
    515{
    516#define TX_CONT_START	0x05
    517#define TX_CONT_STOP	0x06
    518	struct mt7915_dev *dev = phy->dev;
    519	struct cfg80211_chan_def *chandef = &phy->mt76->chandef;
    520	int freq1 = ieee80211_frequency_to_channel(chandef->center_freq1);
    521	struct mt76_testmode_data *td = &phy->mt76->test;
    522	u32 func_idx = en ? TX_CONT_START : TX_CONT_STOP;
    523	u8 rate_idx = td->tx_rate_idx, mode;
    524	u16 rateval;
    525	struct mt7915_tm_rf_test req = {
    526		.action = 1,
    527		.icap_len = 120,
    528		.op.rf.func_idx = cpu_to_le32(func_idx),
    529	};
    530	struct tm_tx_cont *tx_cont = &req.op.rf.param.tx_cont;
    531
    532	tx_cont->control_ch = chandef->chan->hw_value;
    533	tx_cont->center_ch = freq1;
    534	tx_cont->tx_ant = td->tx_antenna_mask;
    535	tx_cont->band = phy != &dev->phy;
    536
    537	switch (chandef->width) {
    538	case NL80211_CHAN_WIDTH_40:
    539		tx_cont->bw = CMD_CBW_40MHZ;
    540		break;
    541	case NL80211_CHAN_WIDTH_80:
    542		tx_cont->bw = CMD_CBW_80MHZ;
    543		break;
    544	case NL80211_CHAN_WIDTH_80P80:
    545		tx_cont->bw = CMD_CBW_8080MHZ;
    546		break;
    547	case NL80211_CHAN_WIDTH_160:
    548		tx_cont->bw = CMD_CBW_160MHZ;
    549		break;
    550	case NL80211_CHAN_WIDTH_5:
    551		tx_cont->bw = CMD_CBW_5MHZ;
    552		break;
    553	case NL80211_CHAN_WIDTH_10:
    554		tx_cont->bw = CMD_CBW_10MHZ;
    555		break;
    556	case NL80211_CHAN_WIDTH_20:
    557		tx_cont->bw = CMD_CBW_20MHZ;
    558		break;
    559	case NL80211_CHAN_WIDTH_20_NOHT:
    560		tx_cont->bw = CMD_CBW_20MHZ;
    561		break;
    562	default:
    563		return -EINVAL;
    564	}
    565
    566	if (!en) {
    567		req.op.rf.param.func_data = cpu_to_le32(phy != &dev->phy);
    568		goto out;
    569	}
    570
    571	if (td->tx_rate_mode <= MT76_TM_TX_MODE_OFDM) {
    572		struct ieee80211_supported_band *sband;
    573		u8 idx = rate_idx;
    574
    575		if (chandef->chan->band == NL80211_BAND_5GHZ)
    576			sband = &phy->mt76->sband_5g.sband;
    577		else if (chandef->chan->band == NL80211_BAND_6GHZ)
    578			sband = &phy->mt76->sband_6g.sband;
    579		else
    580			sband = &phy->mt76->sband_2g.sband;
    581
    582		if (td->tx_rate_mode == MT76_TM_TX_MODE_OFDM)
    583			idx += 4;
    584		rate_idx = sband->bitrates[idx].hw_value & 0xff;
    585	}
    586
    587	switch (td->tx_rate_mode) {
    588	case MT76_TM_TX_MODE_CCK:
    589		mode = MT_PHY_TYPE_CCK;
    590		break;
    591	case MT76_TM_TX_MODE_OFDM:
    592		mode = MT_PHY_TYPE_OFDM;
    593		break;
    594	case MT76_TM_TX_MODE_HT:
    595		mode = MT_PHY_TYPE_HT;
    596		break;
    597	case MT76_TM_TX_MODE_VHT:
    598		mode = MT_PHY_TYPE_VHT;
    599		break;
    600	case MT76_TM_TX_MODE_HE_SU:
    601		mode = MT_PHY_TYPE_HE_SU;
    602		break;
    603	case MT76_TM_TX_MODE_HE_EXT_SU:
    604		mode = MT_PHY_TYPE_HE_EXT_SU;
    605		break;
    606	case MT76_TM_TX_MODE_HE_TB:
    607		mode = MT_PHY_TYPE_HE_TB;
    608		break;
    609	case MT76_TM_TX_MODE_HE_MU:
    610		mode = MT_PHY_TYPE_HE_MU;
    611		break;
    612	default:
    613		return -EINVAL;
    614	}
    615
    616	rateval =  mode << 6 | rate_idx;
    617	tx_cont->rateval = cpu_to_le16(rateval);
    618
    619out:
    620	if (!en) {
    621		int ret;
    622
    623		ret = mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(RF_TEST), &req,
    624					sizeof(req), true);
    625		if (ret)
    626			return ret;
    627
    628		return mt7915_tm_rf_switch_mode(dev, RF_OPER_NORMAL);
    629	}
    630
    631	mt7915_tm_rf_switch_mode(dev, RF_OPER_RF_TEST);
    632	mt7915_tm_update_channel(phy);
    633
    634	return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(RF_TEST), &req,
    635				 sizeof(req), true);
    636}
    637
    638static void
    639mt7915_tm_update_params(struct mt7915_phy *phy, u32 changed)
    640{
    641	struct mt76_testmode_data *td = &phy->mt76->test;
    642	bool en = phy->mt76->test.state != MT76_TM_STATE_OFF;
    643
    644	if (changed & BIT(TM_CHANGED_FREQ_OFFSET))
    645		mt7915_tm_set_freq_offset(phy, en, en ? td->freq_offset : 0);
    646	if (changed & BIT(TM_CHANGED_TXPOWER))
    647		mt7915_tm_set_tx_power(phy);
    648}
    649
    650static int
    651mt7915_tm_set_state(struct mt76_phy *mphy, enum mt76_testmode_state state)
    652{
    653	struct mt76_testmode_data *td = &mphy->test;
    654	struct mt7915_phy *phy = mphy->priv;
    655	enum mt76_testmode_state prev_state = td->state;
    656
    657	mphy->test.state = state;
    658
    659	if (prev_state == MT76_TM_STATE_TX_FRAMES ||
    660	    state == MT76_TM_STATE_TX_FRAMES)
    661		mt7915_tm_set_tx_frames(phy, state == MT76_TM_STATE_TX_FRAMES);
    662	else if (prev_state == MT76_TM_STATE_RX_FRAMES ||
    663		 state == MT76_TM_STATE_RX_FRAMES)
    664		mt7915_tm_set_rx_frames(phy, state == MT76_TM_STATE_RX_FRAMES);
    665	else if (prev_state == MT76_TM_STATE_TX_CONT ||
    666		 state == MT76_TM_STATE_TX_CONT)
    667		mt7915_tm_set_tx_cont(phy, state == MT76_TM_STATE_TX_CONT);
    668	else if (prev_state == MT76_TM_STATE_OFF ||
    669		 state == MT76_TM_STATE_OFF)
    670		mt7915_tm_init(phy, !(state == MT76_TM_STATE_OFF));
    671
    672	if ((state == MT76_TM_STATE_IDLE &&
    673	     prev_state == MT76_TM_STATE_OFF) ||
    674	    (state == MT76_TM_STATE_OFF &&
    675	     prev_state == MT76_TM_STATE_IDLE)) {
    676		u32 changed = 0;
    677		int i;
    678
    679		for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) {
    680			u16 cur = tm_change_map[i];
    681
    682			if (td->param_set[cur / 32] & BIT(cur % 32))
    683				changed |= BIT(i);
    684		}
    685
    686		mt7915_tm_update_params(phy, changed);
    687	}
    688
    689	return 0;
    690}
    691
    692static int
    693mt7915_tm_set_params(struct mt76_phy *mphy, struct nlattr **tb,
    694		     enum mt76_testmode_state new_state)
    695{
    696	struct mt76_testmode_data *td = &mphy->test;
    697	struct mt7915_phy *phy = mphy->priv;
    698	u32 changed = 0;
    699	int i;
    700
    701	BUILD_BUG_ON(NUM_TM_CHANGED >= 32);
    702
    703	if (new_state == MT76_TM_STATE_OFF ||
    704	    td->state == MT76_TM_STATE_OFF)
    705		return 0;
    706
    707	if (td->tx_antenna_mask & ~mphy->chainmask)
    708		return -EINVAL;
    709
    710	for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) {
    711		if (tb[tm_change_map[i]])
    712			changed |= BIT(i);
    713	}
    714
    715	mt7915_tm_update_params(phy, changed);
    716
    717	return 0;
    718}
    719
    720static int
    721mt7915_tm_dump_stats(struct mt76_phy *mphy, struct sk_buff *msg)
    722{
    723	struct mt7915_phy *phy = mphy->priv;
    724	struct mt7915_dev *dev = phy->dev;
    725	enum mt76_rxq_id q;
    726	void *rx, *rssi;
    727	u16 fcs_err;
    728	int i;
    729	u32 cnt;
    730
    731	rx = nla_nest_start(msg, MT76_TM_STATS_ATTR_LAST_RX);
    732	if (!rx)
    733		return -ENOMEM;
    734
    735	if (nla_put_s32(msg, MT76_TM_RX_ATTR_FREQ_OFFSET, phy->test.last_freq_offset))
    736		return -ENOMEM;
    737
    738	rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_RCPI);
    739	if (!rssi)
    740		return -ENOMEM;
    741
    742	for (i = 0; i < ARRAY_SIZE(phy->test.last_rcpi); i++)
    743		if (nla_put_u8(msg, i, phy->test.last_rcpi[i]))
    744			return -ENOMEM;
    745
    746	nla_nest_end(msg, rssi);
    747
    748	rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_IB_RSSI);
    749	if (!rssi)
    750		return -ENOMEM;
    751
    752	for (i = 0; i < ARRAY_SIZE(phy->test.last_ib_rssi); i++)
    753		if (nla_put_s8(msg, i, phy->test.last_ib_rssi[i]))
    754			return -ENOMEM;
    755
    756	nla_nest_end(msg, rssi);
    757
    758	rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_WB_RSSI);
    759	if (!rssi)
    760		return -ENOMEM;
    761
    762	for (i = 0; i < ARRAY_SIZE(phy->test.last_wb_rssi); i++)
    763		if (nla_put_s8(msg, i, phy->test.last_wb_rssi[i]))
    764			return -ENOMEM;
    765
    766	nla_nest_end(msg, rssi);
    767
    768	if (nla_put_u8(msg, MT76_TM_RX_ATTR_SNR, phy->test.last_snr))
    769		return -ENOMEM;
    770
    771	nla_nest_end(msg, rx);
    772
    773	cnt = mt76_rr(dev, MT_MIB_SDR3(phy->band_idx));
    774	fcs_err = is_mt7915(&dev->mt76) ? FIELD_GET(MT_MIB_SDR3_FCS_ERR_MASK, cnt) :
    775		FIELD_GET(MT_MIB_SDR3_FCS_ERR_MASK_MT7916, cnt);
    776
    777	q = phy->band_idx ? MT_RXQ_EXT : MT_RXQ_MAIN;
    778	mphy->test.rx_stats.packets[q] += fcs_err;
    779	mphy->test.rx_stats.fcs_error[q] += fcs_err;
    780
    781	return 0;
    782}
    783
    784const struct mt76_testmode_ops mt7915_testmode_ops = {
    785	.set_state = mt7915_tm_set_state,
    786	.set_params = mt7915_tm_set_params,
    787	.dump_stats = mt7915_tm_dump_stats,
    788};