1 // SPDX-License-Identifier: ISC 2 3 #include "mt7925.h" 4 #include "mcu.h" 5 6 #define MT7925_EVT_RSP_LEN 512 7 8 enum mt7925_testmode_attr { 9 MT7925_TM_ATTR_UNSPEC, 10 MT7925_TM_ATTR_SET, 11 MT7925_TM_ATTR_QUERY, 12 MT7925_TM_ATTR_RSP, 13 14 /* keep last */ 15 NUM_MT7925_TM_ATTRS, 16 MT7925_TM_ATTR_MAX = NUM_MT7925_TM_ATTRS - 1, 17 }; 18 19 struct mt7925_tm_cmd { 20 u8 padding[4]; 21 struct uni_cmd_testmode_ctrl c; 22 } __packed; 23 24 struct mt7925_tm_evt { 25 u32 param0; 26 u32 param1; 27 } __packed; 28 29 static const struct nla_policy mt7925_tm_policy[NUM_MT7925_TM_ATTRS] = { 30 [MT7925_TM_ATTR_SET] = NLA_POLICY_EXACT_LEN(sizeof(struct mt7925_tm_cmd)), 31 [MT7925_TM_ATTR_QUERY] = NLA_POLICY_EXACT_LEN(sizeof(struct mt7925_tm_cmd)), 32 }; 33 34 static int 35 mt7925_tm_set(struct mt792x_dev *dev, struct mt7925_tm_cmd *req) 36 { 37 struct mt7925_rftest_cmd cmd; 38 struct mt7925_rftest_cmd *pcmd = &cmd; 39 bool testmode = false, normal = false; 40 struct mt76_connac_pm *pm = &dev->pm; 41 struct mt76_phy *phy = &dev->mphy; 42 int ret = -ENOTCONN; 43 44 memset(pcmd, 0, sizeof(*pcmd)); 45 memcpy(pcmd, req, sizeof(struct mt7925_tm_cmd)); 46 47 mutex_lock(&dev->mt76.mutex); 48 49 if (pcmd->ctrl.action == CMD_TEST_CTRL_ACT_SWITCH_MODE) { 50 if (pcmd->ctrl.data.op_mode == CMD_TEST_CTRL_ACT_SWITCH_MODE_NORMAL) 51 normal = true; 52 else 53 testmode = true; 54 } 55 56 if (testmode) { 57 /* Make sure testmode running on full power mode */ 58 pm->enable = false; 59 cancel_delayed_work_sync(&pm->ps_work); 60 cancel_work_sync(&pm->wake_work); 61 __mt792x_mcu_drv_pmctrl(dev); 62 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_UNI_CMD(TESTMODE_CTRL), &cmd, 70 sizeof(cmd), false); 71 72 if (ret) 73 goto out; 74 75 if (normal) { 76 /* Switch back to the normal world */ 77 phy->test.state = MT76_TM_STATE_OFF; 78 pm->enable = true; 79 } 80 out: 81 mutex_unlock(&dev->mt76.mutex); 82 83 return ret; 84 } 85 86 static int 87 mt7925_tm_query(struct mt792x_dev *dev, struct mt7925_tm_cmd *req, 88 char *evt_resp) 89 { 90 struct mt7925_rftest_cmd cmd; 91 char *pcmd = (char *)&cmd; 92 struct sk_buff *skb = NULL; 93 int ret = 1; 94 95 memset(pcmd, 0, sizeof(*pcmd)); 96 memcpy(pcmd + 4, (char *)&req->c, sizeof(struct uni_cmd_testmode_ctrl)); 97 98 if (*((uint16_t *)req->padding) == MCU_UNI_CMD_TESTMODE_CTRL) 99 ret = mt76_mcu_send_and_get_msg(&dev->mt76, MCU_UNI_QUERY(TESTMODE_CTRL), 100 &cmd, sizeof(cmd), true, &skb); 101 else if (*((uint16_t *)req->padding) == MCU_UNI_CMD_TESTMODE_RX_STAT) 102 ret = mt76_mcu_send_and_get_msg(&dev->mt76, MCU_UNI_QUERY(TESTMODE_RX_STAT), 103 &cmd, sizeof(cmd), true, &skb); 104 105 if (ret) 106 goto out; 107 108 memcpy((char *)evt_resp, (char *)skb->data + 8, MT7925_EVT_RSP_LEN); 109 110 out: 111 dev_kfree_skb(skb); 112 113 return ret; 114 } 115 116 int mt7925_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif, 117 void *data, int len) 118 { 119 struct nlattr *tb[NUM_MT76_TM_ATTRS]; 120 struct mt76_phy *mphy = hw->priv; 121 struct mt792x_phy *phy = mphy->priv; 122 int err; 123 124 if (!test_bit(MT76_STATE_RUNNING, &mphy->state) || 125 !(hw->conf.flags & IEEE80211_CONF_MONITOR)) 126 return -ENOTCONN; 127 128 err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len, 129 mt76_tm_policy, NULL); 130 if (err) 131 return err; 132 133 if (tb[MT76_TM_ATTR_DRV_DATA]) { 134 struct nlattr *drv_tb[NUM_MT7925_TM_ATTRS], *data; 135 int ret; 136 137 data = tb[MT76_TM_ATTR_DRV_DATA]; 138 ret = nla_parse_nested_deprecated(drv_tb, 139 MT7925_TM_ATTR_MAX, 140 data, mt7925_tm_policy, 141 NULL); 142 if (ret) 143 return ret; 144 145 data = drv_tb[MT7925_TM_ATTR_SET]; 146 if (data) 147 return mt7925_tm_set(phy->dev, nla_data(data)); 148 } 149 150 return -EINVAL; 151 } 152 153 int mt7925_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg, 154 struct netlink_callback *cb, void *data, int len) 155 { 156 struct nlattr *tb[NUM_MT76_TM_ATTRS]; 157 struct mt76_phy *mphy = hw->priv; 158 struct mt792x_phy *phy = mphy->priv; 159 int err; 160 161 if (!test_bit(MT76_STATE_RUNNING, &mphy->state) || 162 !(hw->conf.flags & IEEE80211_CONF_MONITOR) || 163 !mt76_testmode_enabled(mphy)) 164 return -ENOTCONN; 165 166 if (cb->args[2]++ > 0) 167 return -ENOENT; 168 169 err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len, 170 mt76_tm_policy, NULL); 171 if (err) 172 return err; 173 174 if (tb[MT76_TM_ATTR_DRV_DATA]) { 175 struct nlattr *drv_tb[NUM_MT7925_TM_ATTRS], *data; 176 int ret; 177 178 data = tb[MT76_TM_ATTR_DRV_DATA]; 179 ret = nla_parse_nested_deprecated(drv_tb, 180 MT7925_TM_ATTR_MAX, 181 data, mt7925_tm_policy, 182 NULL); 183 if (ret) 184 return ret; 185 186 data = drv_tb[MT7925_TM_ATTR_QUERY]; 187 if (data) { 188 char evt_resp[MT7925_EVT_RSP_LEN]; 189 190 err = mt7925_tm_query(phy->dev, nla_data(data), 191 evt_resp); 192 if (err) 193 return err; 194 195 return nla_put(msg, MT7925_TM_ATTR_RSP, 196 sizeof(evt_resp), evt_resp); 197 } 198 } 199 200 return -EINVAL; 201 } 202