mt76/mt7921/testmode.c

0001 // SPDX-License-Identifier: ISC
0002
0003 #include "mt7921.h"
0004 #include "mcu.h"
0005
0006 enum mt7921_testmode_attr {
0007     MT7921_TM_ATTR_UNSPEC,
0008     MT7921_TM_ATTR_SET,
0009     MT7921_TM_ATTR_QUERY,
0010     MT7921_TM_ATTR_RSP,
0011
0012     /* keep last */
0013     NUM_MT7921_TM_ATTRS,
0014     MT7921_TM_ATTR_MAX = NUM_MT7921_TM_ATTRS - 1,
0015 };
0016
0017 struct mt7921_tm_cmd {
0018     u8 action;
0019     u32 param0;
0020     u32 param1;
0021 };
0022
0023 struct mt7921_tm_evt {
0024     u32 param0;
0025     u32 param1;
0026 };
0027
0028 static const struct nla_policy mt7921_tm_policy[NUM_MT7921_TM_ATTRS] = {
0029     [MT7921_TM_ATTR_SET] = NLA_POLICY_EXACT_LEN(sizeof(struct mt7921_tm_cmd)),
0030     [MT7921_TM_ATTR_QUERY] = NLA_POLICY_EXACT_LEN(sizeof(struct mt7921_tm_cmd)),
0031 };
0032
0033 static int
0034 mt7921_tm_set(struct mt7921_dev *dev, struct mt7921_tm_cmd *req)
0035 {
0036     struct mt7921_rftest_cmd cmd = {
0037         .action = req->action,
0038         .param0 = cpu_to_le32(req->param0),
0039         .param1 = cpu_to_le32(req->param1),
0040     };
0041     bool testmode = false, normal = false;
0042     struct mt76_connac_pm *pm = &dev->pm;
0043     struct mt76_phy *phy = &dev->mphy;
0044     int ret = -ENOTCONN;
0045
0046     mutex_lock(&dev->mt76.mutex);
0047
0048     if (req->action == TM_SWITCH_MODE) {
0049         if (req->param0 == MT7921_TM_NORMAL)
0050             normal = true;
0051         else
0052             testmode = true;
0053     }
0054
0055     if (testmode) {
0056         /* Make sure testmode running on full power mode */
0057         pm->enable = false;
0058         cancel_delayed_work_sync(&pm->ps_work);
0059         cancel_work_sync(&pm->wake_work);
0060         __mt7921_mcu_drv_pmctrl(dev);
0061
0062         mt76_wr(dev, MT_WF_RFCR(0), dev->mt76.rxfilter);
0063         phy->test.state = MT76_TM_STATE_ON;
0064     }
0065
0066     if (!mt76_testmode_enabled(phy))
0067         goto out;
0068
0069     ret = mt76_mcu_send_msg(&dev->mt76, MCU_CE_CMD(TEST_CTRL), &cmd,
0070                 sizeof(cmd), false);
0071     if (ret)
0072         goto out;
0073
0074     if (normal) {
0075         /* Switch back to the normal world */
0076         phy->test.state = MT76_TM_STATE_OFF;
0077         pm->enable = true;
0078     }
0079 out:
0080     mutex_unlock(&dev->mt76.mutex);
0081
0082     return ret;
0083 }
0084
0085 static int
0086 mt7921_tm_query(struct mt7921_dev *dev, struct mt7921_tm_cmd *req,
0087         struct mt7921_tm_evt *evt_resp)
0088 {
0089     struct mt7921_rftest_cmd cmd = {
0090         .action = req->action,
0091         .param0 = cpu_to_le32(req->param0),
0092         .param1 = cpu_to_le32(req->param1),
0093     };
0094     struct mt7921_rftest_evt *evt;
0095     struct sk_buff *skb;
0096     int ret;
0097
0098     ret = mt76_mcu_send_and_get_msg(&dev->mt76, MCU_CE_CMD(TEST_CTRL),
0099                     &cmd, sizeof(cmd), true, &skb);
0100     if (ret)
0101         goto out;
0102
0103     evt = (struct mt7921_rftest_evt *)skb->data;
0104     evt_resp->param0 = le32_to_cpu(evt->param0);
0105     evt_resp->param1 = le32_to_cpu(evt->param1);
0106 out:
0107     dev_kfree_skb(skb);
0108
0109     return ret;
0110 }
0111
0112 int mt7921_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
0113             void *data, int len)
0114 {
0115     struct nlattr *tb[NUM_MT76_TM_ATTRS];
0116     struct mt76_phy *mphy = hw->priv;
0117     struct mt7921_phy *phy = mphy->priv;
0118     int err;
0119
0120     if (!test_bit(MT76_STATE_RUNNING, &mphy->state) ||
0121         !(hw->conf.flags & IEEE80211_CONF_MONITOR))
0122         return -ENOTCONN;
0123
0124     err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len,
0125                    mt76_tm_policy, NULL);
0126     if (err)
0127         return err;
0128
0129     if (tb[MT76_TM_ATTR_DRV_DATA]) {
0130         struct nlattr *drv_tb[NUM_MT7921_TM_ATTRS], *data;
0131         int ret;
0132
0133         data = tb[MT76_TM_ATTR_DRV_DATA];
0134         ret = nla_parse_nested_deprecated(drv_tb,
0135                           MT7921_TM_ATTR_MAX,
0136                           data, mt7921_tm_policy,
0137                           NULL);
0138         if (ret)
0139             return ret;
0140
0141         data = drv_tb[MT7921_TM_ATTR_SET];
0142         if (data)
0143             return mt7921_tm_set(phy->dev, nla_data(data));
0144     }
0145
0146     return -EINVAL;
0147 }
0148
0149 int mt7921_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg,
0150              struct netlink_callback *cb, void *data, int len)
0151 {
0152     struct nlattr *tb[NUM_MT76_TM_ATTRS];
0153     struct mt76_phy *mphy = hw->priv;
0154     struct mt7921_phy *phy = mphy->priv;
0155     int err;
0156
0157     if (!test_bit(MT76_STATE_RUNNING, &mphy->state) ||
0158         !(hw->conf.flags & IEEE80211_CONF_MONITOR) ||
0159         !mt76_testmode_enabled(mphy))
0160         return -ENOTCONN;
0161
0162     if (cb->args[2]++ > 0)
0163         return -ENOENT;
0164
0165     err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len,
0166                    mt76_tm_policy, NULL);
0167     if (err)
0168         return err;
0169
0170     if (tb[MT76_TM_ATTR_DRV_DATA]) {
0171         struct nlattr *drv_tb[NUM_MT7921_TM_ATTRS], *data;
0172         int ret;
0173
0174         data = tb[MT76_TM_ATTR_DRV_DATA];
0175         ret = nla_parse_nested_deprecated(drv_tb,
0176                           MT7921_TM_ATTR_MAX,
0177                           data, mt7921_tm_policy,
0178                           NULL);
0179         if (ret)
0180             return ret;
0181
0182         data = drv_tb[MT7921_TM_ATTR_QUERY];
0183         if (data) {
0184             struct mt7921_tm_evt evt_resp;
0185
0186             err = mt7921_tm_query(phy->dev, nla_data(data),
0187                           &evt_resp);
0188             if (err)
0189                 return err;
0190
0191             return nla_put(msg, MT7921_TM_ATTR_RSP,
0192                        sizeof(evt_resp), &evt_resp);
0193         }
0194     }
0195
0196     return -EINVAL;
0197 }