1 // SPDX-License-Identifier: BSD-3-Clause-Clear 2 /* 3 * Copyright (c) 2018-2019 The Linux Foundation. All rights reserved. 4 */ 5 6 #include "testmode.h" 7 #include <net/netlink.h> 8 #include "debug.h" 9 #include "wmi.h" 10 #include "hw.h" 11 #include "core.h" 12 #include "testmode_i.h" 13 14 static const struct nla_policy ath11k_tm_policy[ATH11K_TM_ATTR_MAX + 1] = { 15 [ATH11K_TM_ATTR_CMD] = { .type = NLA_U32 }, 16 [ATH11K_TM_ATTR_DATA] = { .type = NLA_BINARY, 17 .len = ATH11K_TM_DATA_MAX_LEN }, 18 [ATH11K_TM_ATTR_WMI_CMDID] = { .type = NLA_U32 }, 19 [ATH11K_TM_ATTR_VERSION_MAJOR] = { .type = NLA_U32 }, 20 [ATH11K_TM_ATTR_VERSION_MINOR] = { .type = NLA_U32 }, 21 }; 22 23 /* Returns true if callee consumes the skb and the skb should be discarded. 24 * Returns false if skb is not used. Does not sleep. 25 */ 26 bool ath11k_tm_event_wmi(struct ath11k *ar, u32 cmd_id, struct sk_buff *skb) 27 { 28 struct sk_buff *nl_skb; 29 bool consumed; 30 int ret; 31 32 ath11k_dbg(ar->ab, ATH11K_DBG_TESTMODE, 33 "testmode event wmi cmd_id %d skb %pK skb->len %d\n", 34 cmd_id, skb, skb->len); 35 36 ath11k_dbg_dump(ar->ab, ATH11K_DBG_TESTMODE, NULL, "", skb->data, skb->len); 37 38 spin_lock_bh(&ar->data_lock); 39 40 consumed = true; 41 42 nl_skb = cfg80211_testmode_alloc_event_skb(ar->hw->wiphy, 43 2 * sizeof(u32) + skb->len, 44 GFP_ATOMIC); 45 if (!nl_skb) { 46 ath11k_warn(ar->ab, 47 "failed to allocate skb for testmode wmi event\n"); 48 goto out; 49 } 50 51 ret = nla_put_u32(nl_skb, ATH11K_TM_ATTR_CMD, ATH11K_TM_CMD_WMI); 52 if (ret) { 53 ath11k_warn(ar->ab, 54 "failed to to put testmode wmi event cmd attribute: %d\n", 55 ret); 56 kfree_skb(nl_skb); 57 goto out; 58 } 59 60 ret = nla_put_u32(nl_skb, ATH11K_TM_ATTR_WMI_CMDID, cmd_id); 61 if (ret) { 62 ath11k_warn(ar->ab, 63 "failed to to put testmode wmi even cmd_id: %d\n", 64 ret); 65 kfree_skb(nl_skb); 66 goto out; 67 } 68 69 ret = nla_put(nl_skb, ATH11K_TM_ATTR_DATA, skb->len, skb->data); 70 if (ret) { 71 ath11k_warn(ar->ab, 72 "failed to copy skb to testmode wmi event: %d\n", 73 ret); 74 kfree_skb(nl_skb); 75 goto out; 76 } 77 78 cfg80211_testmode_event(nl_skb, GFP_ATOMIC); 79 80 out: 81 spin_unlock_bh(&ar->data_lock); 82 83 return consumed; 84 } 85 86 static int ath11k_tm_cmd_get_version(struct ath11k *ar, struct nlattr *tb[]) 87 { 88 struct sk_buff *skb; 89 int ret; 90 91 ath11k_dbg(ar->ab, ATH11K_DBG_TESTMODE, 92 "testmode cmd get version_major %d version_minor %d\n", 93 ATH11K_TESTMODE_VERSION_MAJOR, 94 ATH11K_TESTMODE_VERSION_MINOR); 95 96 skb = cfg80211_testmode_alloc_reply_skb(ar->hw->wiphy, 97 nla_total_size(sizeof(u32))); 98 if (!skb) 99 return -ENOMEM; 100 101 ret = nla_put_u32(skb, ATH11K_TM_ATTR_VERSION_MAJOR, 102 ATH11K_TESTMODE_VERSION_MAJOR); 103 if (ret) { 104 kfree_skb(skb); 105 return ret; 106 } 107 108 ret = nla_put_u32(skb, ATH11K_TM_ATTR_VERSION_MINOR, 109 ATH11K_TESTMODE_VERSION_MINOR); 110 if (ret) { 111 kfree_skb(skb); 112 return ret; 113 } 114 115 return cfg80211_testmode_reply(skb); 116 } 117 118 static int ath11k_tm_cmd_wmi(struct ath11k *ar, struct nlattr *tb[]) 119 { 120 struct ath11k_pdev_wmi *wmi = ar->wmi; 121 struct sk_buff *skb; 122 u32 cmd_id, buf_len; 123 int ret; 124 void *buf; 125 126 mutex_lock(&ar->conf_mutex); 127 128 if (ar->state != ATH11K_STATE_ON) { 129 ret = -ENETDOWN; 130 goto out; 131 } 132 133 if (!tb[ATH11K_TM_ATTR_DATA]) { 134 ret = -EINVAL; 135 goto out; 136 } 137 138 if (!tb[ATH11K_TM_ATTR_WMI_CMDID]) { 139 ret = -EINVAL; 140 goto out; 141 } 142 143 buf = nla_data(tb[ATH11K_TM_ATTR_DATA]); 144 buf_len = nla_len(tb[ATH11K_TM_ATTR_DATA]); 145 cmd_id = nla_get_u32(tb[ATH11K_TM_ATTR_WMI_CMDID]); 146 147 ath11k_dbg(ar->ab, ATH11K_DBG_TESTMODE, 148 "testmode cmd wmi cmd_id %d buf %pK buf_len %d\n", 149 cmd_id, buf, buf_len); 150 151 ath11k_dbg_dump(ar->ab, ATH11K_DBG_TESTMODE, NULL, "", buf, buf_len); 152 153 skb = ath11k_wmi_alloc_skb(wmi->wmi_ab, buf_len); 154 if (!skb) { 155 ret = -ENOMEM; 156 goto out; 157 } 158 159 memcpy(skb->data, buf, buf_len); 160 161 ret = ath11k_wmi_cmd_send(wmi, skb, cmd_id); 162 if (ret) { 163 dev_kfree_skb(skb); 164 ath11k_warn(ar->ab, "failed to transmit wmi command (testmode): %d\n", 165 ret); 166 goto out; 167 } 168 169 ret = 0; 170 171 out: 172 mutex_unlock(&ar->conf_mutex); 173 return ret; 174 } 175 176 int ath11k_tm_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif, 177 void *data, int len) 178 { 179 struct ath11k *ar = hw->priv; 180 struct nlattr *tb[ATH11K_TM_ATTR_MAX + 1]; 181 int ret; 182 183 ret = nla_parse(tb, ATH11K_TM_ATTR_MAX, data, len, ath11k_tm_policy, 184 NULL); 185 if (ret) 186 return ret; 187 188 if (!tb[ATH11K_TM_ATTR_CMD]) 189 return -EINVAL; 190 191 switch (nla_get_u32(tb[ATH11K_TM_ATTR_CMD])) { 192 case ATH11K_TM_CMD_GET_VERSION: 193 return ath11k_tm_cmd_get_version(ar, tb); 194 case ATH11K_TM_CMD_WMI: 195 return ath11k_tm_cmd_wmi(ar, tb); 196 default: 197 return -EOPNOTSUPP; 198 } 199 } 200