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 phy->test.state = MT76_TM_STATE_ON; 63 } 64 65 if (!mt76_testmode_enabled(phy)) 66 goto out; 67 68 ret = mt76_mcu_send_msg(&dev->mt76, MCU_CE_CMD(TEST_CTRL), &cmd, 69 sizeof(cmd), false); 70 if (ret) 71 goto out; 72 73 if (normal) { 74 /* Switch back to the normal world */ 75 phy->test.state = MT76_TM_STATE_OFF; 76 pm->enable = true; 77 } 78 out: 79 mutex_unlock(&dev->mt76.mutex); 80 81 return ret; 82 } 83 84 static int 85 mt7921_tm_query(struct mt7921_dev *dev, struct mt7921_tm_cmd *req, 86 struct mt7921_tm_evt *evt_resp) 87 { 88 struct mt7921_rftest_cmd cmd = { 89 .action = req->action, 90 .param0 = cpu_to_le32(req->param0), 91 .param1 = cpu_to_le32(req->param1), 92 }; 93 struct mt7921_rftest_evt *evt; 94 struct sk_buff *skb; 95 int ret; 96 97 ret = mt76_mcu_send_and_get_msg(&dev->mt76, MCU_CE_CMD(TEST_CTRL), 98 &cmd, sizeof(cmd), true, &skb); 99 if (ret) 100 goto out; 101 102 evt = (struct mt7921_rftest_evt *)skb->data; 103 evt_resp->param0 = le32_to_cpu(evt->param0); 104 evt_resp->param1 = le32_to_cpu(evt->param1); 105 out: 106 dev_kfree_skb(skb); 107 108 return ret; 109 } 110 111 int mt7921_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif, 112 void *data, int len) 113 { 114 struct nlattr *tb[NUM_MT76_TM_ATTRS]; 115 struct mt76_phy *mphy = hw->priv; 116 struct mt7921_phy *phy = mphy->priv; 117 int err; 118 119 if (!test_bit(MT76_STATE_RUNNING, &mphy->state) || 120 !(hw->conf.flags & IEEE80211_CONF_MONITOR)) 121 return -ENOTCONN; 122 123 err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len, 124 mt76_tm_policy, NULL); 125 if (err) 126 return err; 127 128 if (tb[MT76_TM_ATTR_DRV_DATA]) { 129 struct nlattr *drv_tb[NUM_MT7921_TM_ATTRS], *data; 130 int ret; 131 132 data = tb[MT76_TM_ATTR_DRV_DATA]; 133 ret = nla_parse_nested_deprecated(drv_tb, 134 MT7921_TM_ATTR_MAX, 135 data, mt7921_tm_policy, 136 NULL); 137 if (ret) 138 return ret; 139 140 data = drv_tb[MT7921_TM_ATTR_SET]; 141 if (data) 142 return mt7921_tm_set(phy->dev, nla_data(data)); 143 } 144 145 return -EINVAL; 146 } 147 148 int mt7921_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg, 149 struct netlink_callback *cb, void *data, int len) 150 { 151 struct nlattr *tb[NUM_MT76_TM_ATTRS]; 152 struct mt76_phy *mphy = hw->priv; 153 struct mt7921_phy *phy = mphy->priv; 154 int err; 155 156 if (!test_bit(MT76_STATE_RUNNING, &mphy->state) || 157 !(hw->conf.flags & IEEE80211_CONF_MONITOR) || 158 !mt76_testmode_enabled(mphy)) 159 return -ENOTCONN; 160 161 if (cb->args[2]++ > 0) 162 return -ENOENT; 163 164 err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len, 165 mt76_tm_policy, NULL); 166 if (err) 167 return err; 168 169 if (tb[MT76_TM_ATTR_DRV_DATA]) { 170 struct nlattr *drv_tb[NUM_MT7921_TM_ATTRS], *data; 171 int ret; 172 173 data = tb[MT76_TM_ATTR_DRV_DATA]; 174 ret = nla_parse_nested_deprecated(drv_tb, 175 MT7921_TM_ATTR_MAX, 176 data, mt7921_tm_policy, 177 NULL); 178 if (ret) 179 return ret; 180 181 data = drv_tb[MT7921_TM_ATTR_QUERY]; 182 if (data) { 183 struct mt7921_tm_evt evt_resp; 184 185 err = mt7921_tm_query(phy->dev, nla_data(data), 186 &evt_resp); 187 if (err) 188 return err; 189 190 return nla_put(msg, MT7921_TM_ATTR_RSP, 191 sizeof(evt_resp), &evt_resp); 192 } 193 } 194 195 return -EINVAL; 196 } 197