minimal-linux-script/linux-5.11.6/drivers/net/wireless/mediatek/mt76/mt7915/testmode.c

// SPDX-License-Identifier: ISC
/* Copyright (C) 2020 MediaTek Inc. */

#include "mt7915.h"
#include "mac.h"
#include "mcu.h"
#include "testmode.h"

enum {
	TM_CHANGED_TXPOWER,
	TM_CHANGED_FREQ_OFFSET,

	/* must be last */
	NUM_TM_CHANGED
};

static const u8 tm_change_map[] = {
	[TM_CHANGED_TXPOWER] = MT76_TM_ATTR_TX_POWER,
	[TM_CHANGED_FREQ_OFFSET] = MT76_TM_ATTR_FREQ_OFFSET,
};

struct reg_band {
	u32 band[2];
};

#define REG_BAND(_reg) \
	{ .band[0] = MT_##_reg(0), .band[1] = MT_##_reg(1) }
#define REG_BAND_IDX(_reg, _idx) \
	{ .band[0] = MT_##_reg(0, _idx), .band[1] = MT_##_reg(1, _idx) }

static const struct reg_band reg_backup_list[] = {
	REG_BAND_IDX(AGG_PCR0, 0),
	REG_BAND_IDX(AGG_PCR0, 1),
	REG_BAND_IDX(AGG_AWSCR0, 0),
	REG_BAND_IDX(AGG_AWSCR0, 1),
	REG_BAND_IDX(AGG_AWSCR0, 2),
	REG_BAND_IDX(AGG_AWSCR0, 3),
	REG_BAND(AGG_MRCR),
	REG_BAND(TMAC_TFCR0),
	REG_BAND(TMAC_TCR0),
	REG_BAND(AGG_ATCR1),
	REG_BAND(AGG_ATCR3),
	REG_BAND(TMAC_TRCR0),
	REG_BAND(TMAC_ICR0),
	REG_BAND_IDX(ARB_DRNGR0, 0),
	REG_BAND_IDX(ARB_DRNGR0, 1),
	REG_BAND(WF_RFCR),
	REG_BAND(WF_RFCR1),
};

static int
mt7915_tm_set_tx_power(struct mt7915_phy *phy)
{
	struct mt7915_dev *dev = phy->dev;
	struct mt76_phy *mphy = phy->mt76;
	struct cfg80211_chan_def *chandef = &mphy->chandef;
	int freq = chandef->center_freq1;
	int ret;
	struct {
		u8 format_id;
		u8 dbdc_idx;
		s8 tx_power;
		u8 ant_idx;	/* Only 0 is valid */
		u8 center_chan;
		u8 rsv[3];
	} __packed req = {
		.format_id = 0xf,
		.dbdc_idx = phy != &dev->phy,
		.center_chan = ieee80211_frequency_to_channel(freq),
	};
	u8 *tx_power = NULL;

	if (dev->mt76.test.state != MT76_TM_STATE_OFF)
		tx_power = dev->mt76.test.tx_power;

	/* Tx power of the other antennas are the same as antenna 0 */
	if (tx_power && tx_power[0])
		req.tx_power = tx_power[0];

	ret = mt76_mcu_send_msg(&dev->mt76,
				MCU_EXT_CMD_TX_POWER_FEATURE_CTRL,
				&req, sizeof(req), false);

	return ret;
}

static int
mt7915_tm_set_freq_offset(struct mt7915_dev *dev, bool en, u32 val)
{
	struct mt7915_tm_cmd req = {
		.testmode_en = en,
		.param_idx = MCU_ATE_SET_FREQ_OFFSET,
		.param.freq.freq_offset = cpu_to_le32(val),
	};

	return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD_ATE_CTRL, &req,
				 sizeof(req), false);
}

static int
mt7915_tm_mode_ctrl(struct mt7915_dev *dev, bool enable)
{
	struct {
		u8 format_id;
		bool enable;
		u8 rsv[2];
	} __packed req = {
		.format_id = 0x6,
		.enable = enable,
	};

	return mt76_mcu_send_msg(&dev->mt76,
				 MCU_EXT_CMD_TX_POWER_FEATURE_CTRL,
				 &req, sizeof(req), false);
}

static int
mt7915_tm_set_trx(struct mt7915_dev *dev, struct mt7915_phy *phy,
		  int type, bool en)
{
	struct mt7915_tm_cmd req = {
		.testmode_en = 1,
		.param_idx = MCU_ATE_SET_TRX,
		.param.trx.type = type,
		.param.trx.enable = en,
		.param.trx.band = phy != &dev->phy,
	};

	return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD_ATE_CTRL, &req,
				 sizeof(req), false);
}

static void
mt7915_tm_reg_backup_restore(struct mt7915_dev *dev, struct mt7915_phy *phy)
{
	int n_regs = ARRAY_SIZE(reg_backup_list);
	bool ext_phy = phy != &dev->phy;
	u32 *b = dev->test.reg_backup;
	int i;

	if (dev->mt76.test.state == MT76_TM_STATE_OFF) {
		for (i = 0; i < n_regs; i++)
			mt76_wr(dev, reg_backup_list[i].band[ext_phy], b[i]);
		return;
	}

	if (b)
		return;

	b = devm_kzalloc(dev->mt76.dev, 4 * n_regs, GFP_KERNEL);
	if (!b)
		return;

	dev->test.reg_backup = b;
	for (i = 0; i < n_regs; i++)
		b[i] = mt76_rr(dev, reg_backup_list[i].band[ext_phy]);

	mt76_clear(dev, MT_AGG_PCR0(ext_phy, 0), MT_AGG_PCR0_MM_PROT |
		   MT_AGG_PCR0_GF_PROT | MT_AGG_PCR0_ERP_PROT |
		   MT_AGG_PCR0_VHT_PROT | MT_AGG_PCR0_BW20_PROT |
		   MT_AGG_PCR0_BW40_PROT | MT_AGG_PCR0_BW80_PROT);
	mt76_set(dev, MT_AGG_PCR0(ext_phy, 0), MT_AGG_PCR0_PTA_WIN_DIS);

	mt76_wr(dev, MT_AGG_PCR0(ext_phy, 1), MT_AGG_PCR1_RTS0_NUM_THRES |
		MT_AGG_PCR1_RTS0_LEN_THRES);

	mt76_clear(dev, MT_AGG_MRCR(ext_phy), MT_AGG_MRCR_BAR_CNT_LIMIT |
		   MT_AGG_MRCR_LAST_RTS_CTS_RN | MT_AGG_MRCR_RTS_FAIL_LIMIT |
		   MT_AGG_MRCR_TXCMD_RTS_FAIL_LIMIT);

	mt76_rmw(dev, MT_AGG_MRCR(ext_phy), MT_AGG_MRCR_RTS_FAIL_LIMIT |
		 MT_AGG_MRCR_TXCMD_RTS_FAIL_LIMIT,
		 FIELD_PREP(MT_AGG_MRCR_RTS_FAIL_LIMIT, 1) |
		 FIELD_PREP(MT_AGG_MRCR_TXCMD_RTS_FAIL_LIMIT, 1));

	mt76_wr(dev, MT_TMAC_TFCR0(ext_phy), 0);
	mt76_clear(dev, MT_TMAC_TCR0(ext_phy), MT_TMAC_TCR0_TBTT_STOP_CTRL);

	/* config rx filter for testmode rx */
	mt76_wr(dev, MT_WF_RFCR(ext_phy), 0xcf70a);
	mt76_wr(dev, MT_WF_RFCR1(ext_phy), 0);
}

static void
mt7915_tm_init(struct mt7915_dev *dev)
{
	bool en = !(dev->mt76.test.state == MT76_TM_STATE_OFF);

	if (!test_bit(MT76_STATE_RUNNING, &dev->phy.mt76->state))
		return;

	mt7915_tm_mode_ctrl(dev, en);
	mt7915_tm_reg_backup_restore(dev, &dev->phy);
	mt7915_tm_set_trx(dev, &dev->phy, TM_MAC_TXRX, !en);
}

static void
mt7915_tm_set_tx_frames(struct mt7915_dev *dev, bool en)
{
	static const u8 spe_idx_map[] = {0, 0, 1, 0, 3, 2, 4, 0,
					 9, 8, 6, 10, 16, 12, 18, 0};
	struct sk_buff *skb = dev->mt76.test.tx_skb;
	struct ieee80211_tx_info *info;

	mt7915_tm_set_trx(dev, &dev->phy, TM_MAC_RX_RXV, false);

	if (en) {
		u8 tx_ant = dev->mt76.test.tx_antenna_mask;

		mutex_unlock(&dev->mt76.mutex);
		mt7915_set_channel(&dev->phy);
		mutex_lock(&dev->mt76.mutex);

		mt7915_mcu_set_chan_info(&dev->phy, MCU_EXT_CMD_SET_RX_PATH);
		dev->test.spe_idx = spe_idx_map[tx_ant];
	}

	mt7915_tm_set_trx(dev, &dev->phy, TM_MAC_TX, en);

	if (!en || !skb)
		return;

	info = IEEE80211_SKB_CB(skb);
	info->control.vif = dev->phy.monitor_vif;
}

static void
mt7915_tm_set_rx_frames(struct mt7915_dev *dev, bool en)
{
	if (en) {
		mutex_unlock(&dev->mt76.mutex);
		mt7915_set_channel(&dev->phy);
		mutex_lock(&dev->mt76.mutex);

		mt7915_mcu_set_chan_info(&dev->phy, MCU_EXT_CMD_SET_RX_PATH);
	}

	mt7915_tm_set_trx(dev, &dev->phy, TM_MAC_RX_RXV, en);
}

static void
mt7915_tm_update_params(struct mt7915_dev *dev, u32 changed)
{
	struct mt76_testmode_data *td = &dev->mt76.test;
	bool en = dev->mt76.test.state != MT76_TM_STATE_OFF;

	if (changed & BIT(TM_CHANGED_FREQ_OFFSET))
		mt7915_tm_set_freq_offset(dev, en, en ? td->freq_offset : 0);
	if (changed & BIT(TM_CHANGED_TXPOWER))
		mt7915_tm_set_tx_power(&dev->phy);
}

static int
mt7915_tm_set_state(struct mt76_dev *mdev, enum mt76_testmode_state state)
{
	struct mt7915_dev *dev = container_of(mdev, struct mt7915_dev, mt76);
	struct mt76_testmode_data *td = &mdev->test;
	enum mt76_testmode_state prev_state = td->state;

	mdev->test.state = state;

	if (prev_state == MT76_TM_STATE_TX_FRAMES)
		mt7915_tm_set_tx_frames(dev, false);
	else if (state == MT76_TM_STATE_TX_FRAMES)
		mt7915_tm_set_tx_frames(dev, true);
	else if (prev_state == MT76_TM_STATE_RX_FRAMES)
		mt7915_tm_set_rx_frames(dev, false);
	else if (state == MT76_TM_STATE_RX_FRAMES)
		mt7915_tm_set_rx_frames(dev, true);
	else if (prev_state == MT76_TM_STATE_OFF || state == MT76_TM_STATE_OFF)
		mt7915_tm_init(dev);

	if ((state == MT76_TM_STATE_IDLE &&
	     prev_state == MT76_TM_STATE_OFF) ||
	    (state == MT76_TM_STATE_OFF &&
	     prev_state == MT76_TM_STATE_IDLE)) {
		u32 changed = 0;
		int i;

		for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) {
			u16 cur = tm_change_map[i];

			if (td->param_set[cur / 32] & BIT(cur % 32))
				changed |= BIT(i);
		}

		mt7915_tm_update_params(dev, changed);
	}

	return 0;
}

static int
mt7915_tm_set_params(struct mt76_dev *mdev, struct nlattr **tb,
		     enum mt76_testmode_state new_state)
{
	struct mt7915_dev *dev = container_of(mdev, struct mt7915_dev, mt76);
	struct mt76_testmode_data *td = &dev->mt76.test;
	u32 changed = 0;
	int i;

	BUILD_BUG_ON(NUM_TM_CHANGED >= 32);

	if (new_state == MT76_TM_STATE_OFF ||
	    td->state == MT76_TM_STATE_OFF)
		return 0;

	if (td->tx_antenna_mask & ~dev->phy.chainmask)
		return -EINVAL;

	for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) {
		if (tb[tm_change_map[i]])
			changed |= BIT(i);
	}

	mt7915_tm_update_params(dev, changed);

	return 0;
}

static int
mt7915_tm_dump_stats(struct mt76_dev *mdev, struct sk_buff *msg)
{
	struct mt7915_dev *dev = container_of(mdev, struct mt7915_dev, mt76);
	void *rx, *rssi;
	int i;

	rx = nla_nest_start(msg, MT76_TM_STATS_ATTR_LAST_RX);
	if (!rx)
		return -ENOMEM;

	if (nla_put_s32(msg, MT76_TM_RX_ATTR_FREQ_OFFSET, dev->test.last_freq_offset))
		return -ENOMEM;

	rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_RCPI);
	if (!rssi)
		return -ENOMEM;

	for (i = 0; i < ARRAY_SIZE(dev->test.last_rcpi); i++)
		if (nla_put_u8(msg, i, dev->test.last_rcpi[i]))
			return -ENOMEM;

	nla_nest_end(msg, rssi);

	rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_IB_RSSI);
	if (!rssi)
		return -ENOMEM;

	for (i = 0; i < ARRAY_SIZE(dev->test.last_ib_rssi); i++)
		if (nla_put_s8(msg, i, dev->test.last_ib_rssi[i]))
			return -ENOMEM;

	nla_nest_end(msg, rssi);

	rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_WB_RSSI);
	if (!rssi)
		return -ENOMEM;

	for (i = 0; i < ARRAY_SIZE(dev->test.last_wb_rssi); i++)
		if (nla_put_s8(msg, i, dev->test.last_wb_rssi[i]))
			return -ENOMEM;

	nla_nest_end(msg, rssi);

	if (nla_put_u8(msg, MT76_TM_RX_ATTR_SNR, dev->test.last_snr))
		return -ENOMEM;

	nla_nest_end(msg, rx);

	return 0;
}

const struct mt76_testmode_ops mt7915_testmode_ops = {
	.set_state = mt7915_tm_set_state,
	.set_params = mt7915_tm_set_params,
	.dump_stats = mt7915_tm_dump_stats,
};