1 // SPDX-License-Identifier: ISC 2 3 #include "mt7921.h" 4 #include "mcu.h" 5 6 enum 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 17 struct mt7921_tm_cmd { 18 u8 action; 19 u32 param0; 20 u32 param1; 21 }; 22 23 struct mt7921_tm_evt { 24 u32 param0; 25 u32 param1; 26 }; 27 28 static 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 33 static int 34 mt7921_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 } 79 out: 80 mutex_unlock(&dev->mt76.mutex); 81 82 return ret; 83 } 84 85 static int 86 mt7921_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); 106 out: 107 dev_kfree_skb(skb); 108 109 return ret; 110 } 111 112 int 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 149 int 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 } 198