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 (4418B)

      1// SPDX-License-Identifier: ISC
      2
      3#include "mt7921.h"
      4#include "mcu.h"
      5
      6enum mt7921_testmode_attr {
      7	MT7921_TM_ATTR_UNSPEC,
      8	MT7921_TM_ATTR_SET,
      9	MT7921_TM_ATTR_QUERY,
     10	MT7921_TM_ATTR_RSP,
     11
     12	/* keep last */
     13	NUM_MT7921_TM_ATTRS,
     14	MT7921_TM_ATTR_MAX = NUM_MT7921_TM_ATTRS - 1,
     15};
     16
     17struct mt7921_tm_cmd {
     18	u8 action;
     19	u32 param0;
     20	u32 param1;
     21};
     22
     23struct mt7921_tm_evt {
     24	u32 param0;
     25	u32 param1;
     26};
     27
     28static const struct nla_policy mt7921_tm_policy[NUM_MT7921_TM_ATTRS] = {
     29	[MT7921_TM_ATTR_SET] = NLA_POLICY_EXACT_LEN(sizeof(struct mt7921_tm_cmd)),
     30	[MT7921_TM_ATTR_QUERY] = NLA_POLICY_EXACT_LEN(sizeof(struct mt7921_tm_cmd)),
     31};
     32
     33static int
     34mt7921_tm_set(struct mt7921_dev *dev, struct mt7921_tm_cmd *req)
     35{
     36	struct mt7921_rftest_cmd cmd = {
     37		.action = req->action,
     38		.param0 = cpu_to_le32(req->param0),
     39		.param1 = cpu_to_le32(req->param1),
     40	};
     41	bool testmode = false, normal = false;
     42	struct mt76_connac_pm *pm = &dev->pm;
     43	struct mt76_phy *phy = &dev->mphy;
     44	int ret = -ENOTCONN;
     45
     46	mutex_lock(&dev->mt76.mutex);
     47
     48	if (req->action == TM_SWITCH_MODE) {
     49		if (req->param0 == MT7921_TM_NORMAL)
     50			normal = true;
     51		else
     52			testmode = true;
     53	}
     54
     55	if (testmode) {
     56		/* Make sure testmode running on full power mode */
     57		pm->enable = false;
     58		cancel_delayed_work_sync(&pm->ps_work);
     59		cancel_work_sync(&pm->wake_work);
     60		__mt7921_mcu_drv_pmctrl(dev);
     61
     62		mt76_wr(dev, MT_WF_RFCR(0), dev->mt76.rxfilter);
     63		phy->test.state = MT76_TM_STATE_ON;
     64	}
     65
     66	if (!mt76_testmode_enabled(phy))
     67		goto out;
     68
     69	ret = mt76_mcu_send_msg(&dev->mt76, MCU_CE_CMD(TEST_CTRL), &cmd,
     70				sizeof(cmd), false);
     71	if (ret)
     72		goto out;
     73
     74	if (normal) {
     75		/* Switch back to the normal world */
     76		phy->test.state = MT76_TM_STATE_OFF;
     77		pm->enable = true;
     78	}
     79out:
     80	mutex_unlock(&dev->mt76.mutex);
     81
     82	return ret;
     83}
     84
     85static int
     86mt7921_tm_query(struct mt7921_dev *dev, struct mt7921_tm_cmd *req,
     87		struct mt7921_tm_evt *evt_resp)
     88{
     89	struct mt7921_rftest_cmd cmd = {
     90		.action = req->action,
     91		.param0 = cpu_to_le32(req->param0),
     92		.param1 = cpu_to_le32(req->param1),
     93	};
     94	struct mt7921_rftest_evt *evt;
     95	struct sk_buff *skb;
     96	int ret;
     97
     98	ret = mt76_mcu_send_and_get_msg(&dev->mt76, MCU_CE_CMD(TEST_CTRL),
     99					&cmd, sizeof(cmd), true, &skb);
    100	if (ret)
    101		goto out;
    102
    103	evt = (struct mt7921_rftest_evt *)skb->data;
    104	evt_resp->param0 = le32_to_cpu(evt->param0);
    105	evt_resp->param1 = le32_to_cpu(evt->param1);
    106out:
    107	dev_kfree_skb(skb);
    108
    109	return ret;
    110}
    111
    112int mt7921_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
    113			void *data, int len)
    114{
    115	struct nlattr *tb[NUM_MT76_TM_ATTRS];
    116	struct mt76_phy *mphy = hw->priv;
    117	struct mt7921_phy *phy = mphy->priv;
    118	int err;
    119
    120	if (!test_bit(MT76_STATE_RUNNING, &mphy->state) ||
    121	    !(hw->conf.flags & IEEE80211_CONF_MONITOR))
    122		return -ENOTCONN;
    123
    124	err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len,
    125				   mt76_tm_policy, NULL);
    126	if (err)
    127		return err;
    128
    129	if (tb[MT76_TM_ATTR_DRV_DATA]) {
    130		struct nlattr *drv_tb[NUM_MT7921_TM_ATTRS], *data;
    131		int ret;
    132
    133		data = tb[MT76_TM_ATTR_DRV_DATA];
    134		ret = nla_parse_nested_deprecated(drv_tb,
    135						  MT7921_TM_ATTR_MAX,
    136						  data, mt7921_tm_policy,
    137						  NULL);
    138		if (ret)
    139			return ret;
    140
    141		data = drv_tb[MT7921_TM_ATTR_SET];
    142		if (data)
    143			return mt7921_tm_set(phy->dev, nla_data(data));
    144	}
    145
    146	return -EINVAL;
    147}
    148
    149int mt7921_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg,
    150			 struct netlink_callback *cb, void *data, int len)
    151{
    152	struct nlattr *tb[NUM_MT76_TM_ATTRS];
    153	struct mt76_phy *mphy = hw->priv;
    154	struct mt7921_phy *phy = mphy->priv;
    155	int err;
    156
    157	if (!test_bit(MT76_STATE_RUNNING, &mphy->state) ||
    158	    !(hw->conf.flags & IEEE80211_CONF_MONITOR) ||
    159	    !mt76_testmode_enabled(mphy))
    160		return -ENOTCONN;
    161
    162	if (cb->args[2]++ > 0)
    163		return -ENOENT;
    164
    165	err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len,
    166				   mt76_tm_policy, NULL);
    167	if (err)
    168		return err;
    169
    170	if (tb[MT76_TM_ATTR_DRV_DATA]) {
    171		struct nlattr *drv_tb[NUM_MT7921_TM_ATTRS], *data;
    172		int ret;
    173
    174		data = tb[MT76_TM_ATTR_DRV_DATA];
    175		ret = nla_parse_nested_deprecated(drv_tb,
    176						  MT7921_TM_ATTR_MAX,
    177						  data, mt7921_tm_policy,
    178						  NULL);
    179		if (ret)
    180			return ret;
    181
    182		data = drv_tb[MT7921_TM_ATTR_QUERY];
    183		if (data) {
    184			struct mt7921_tm_evt evt_resp;
    185
    186			err = mt7921_tm_query(phy->dev, nla_data(data),
    187					      &evt_resp);
    188			if (err)
    189				return err;
    190
    191			return nla_put(msg, MT7921_TM_ATTR_RSP,
    192				       sizeof(evt_resp), &evt_resp);
    193		}
    194	}
    195
    196	return -EINVAL;
    197}