1 /*
2 * Driver interaction with extended Linux CFG8021
3 *
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License version 2 as
6 * published by the Free Software Foundation.
7 *
8 * Alternatively, this software may be distributed under the terms of BSD
9 * license.
10 *
11 * Changes from Qualcomm Innovation Center are provided under the following license:
12 *
13 * Copyright (c) 2022 Qualcomm Innovation Center, Inc. All rights reserved.
14 *
15 * Redistribution and use in source and binary forms, with or without
16 * modification, are permitted (subject to the limitations in the
17 * disclaimer below) provided that the following conditions are met:
18 *
19 * * Redistributions of source code must retain the above copyright
20 * notice, this list of conditions and the following disclaimer.
21 *
22 * * Redistributions in binary form must reproduce the above
23 * copyright notice, this list of conditions and the following
24 * disclaimer in the documentation and/or other materials provided
25 * with the distribution.
26 *
27 * * Neither the name of Qualcomm Innovation Center, Inc. nor the names of its
28 * contributors may be used to endorse or promote products derived
29 * from this software without specific prior written permission.
30 *
31 * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
32 * GRANTED BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT
33 * HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
34 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
35 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
36 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
37 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
38 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
39 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
40 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
41 * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
42 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
43 * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
44 */
45
46 #include "includes.h"
47 #include <sys/types.h>
48 #include <fcntl.h>
49 #include <net/if.h>
50 #include <netlink/genl/genl.h>
51 #include <netlink/genl/ctrl.h>
52 #include <netlink/object-api.h>
53 #include <netlink/genl/genl.h>
54 #include <netlink/genl/ctrl.h>
55 #include <linux/pkt_sched.h>
56
57 #include "common.h"
58 #include "linux_ioctl.h"
59 #include "driver_nl80211.h"
60 #include "wpa_supplicant_i.h"
61 #include "config.h"
62 #include "wpa_driver_common_lib.h"
63 #include "ap/hostapd.h"
64 #include "ap/sta_info.h"
65 #ifdef LINUX_EMBEDDED
66 #include <sys/ioctl.h>
67 #endif
68 #if defined(ANDROID) || defined(LINUX_EMBEDDED)
69 #include "android_drv.h"
70 #endif
71 #include "driver_cmd_nl80211_extn.h"
72
73 #define WPA_PS_ENABLED 0
74 #define WPA_PS_DISABLED 1
75 #define UNUSED(x) (void)(x)
76 #define NL80211_ATTR_MAX_INTERNAL 256
77 #define CSI_STATUS_REJECTED -1
78 #define CSI_STATUS_SUCCESS 0
79 #define ENHANCED_CFR_VER 2
80 #define CSI_GROUP_BITMAP 1
81 #define CSI_DEFAULT_GROUP_ID 0
82 #define CSI_FC_STYPE_BEACON 8
83 #define CSI_MGMT_BEACON (1<<WLAN_FC_STYPE_BEACON)
84
85 #define TWT_SETUP_WAKE_INTVL_MANTISSA_MAX 0xFFFF
86 #define TWT_SETUP_WAKE_DURATION_MAX 0xFF
87 #define TWT_SETUP_WAKE_INTVL_EXP_MAX 31
88 #define TWT_WAKE_INTERVAL_TU_FACTOR 1024
89
90 #define TWT_SETUP_STR "twt_session_setup"
91 #define TWT_TERMINATE_STR "twt_session_terminate"
92 #define TWT_PAUSE_STR "twt_session_pause"
93 #define TWT_RESUME_STR "twt_session_resume"
94 #define TWT_NUDGE_STR "twt_session_nudge"
95 #define TWT_GET_PARAMS_STR "twt_session_get_params"
96 #define TWT_GET_STATS_STR "twt_session_get_stats"
97 #define TWT_CLEAR_STATS_STR "twt_session_clear_stats"
98 #define TWT_GET_CAP_STR "twt_get_capability"
99 #define TWT_SET_PARAM_STR "twt_set_param"
100
101 #define TWT_SETUP_STRLEN strlen(TWT_SETUP_STR)
102 #define TWT_TERMINATE_STR_LEN strlen(TWT_TERMINATE_STR)
103 #define TWT_PAUSE_STR_LEN strlen(TWT_PAUSE_STR)
104 #define TWT_RESUME_STR_LEN strlen(TWT_RESUME_STR)
105 #define TWT_NUDGE_STR_LEN strlen(TWT_NUDGE_STR)
106 #define TWT_GET_PARAMS_STR_LEN strlen(TWT_GET_PARAMS_STR)
107 #define TWT_GET_STATS_STR_LEN strlen(TWT_GET_STATS_STR)
108 #define TWT_CLEAR_STATS_STR_LEN strlen(TWT_CLEAR_STATS_STR)
109 #define TWT_GET_CAP_STR_LEN strlen(TWT_GET_CAP_STR)
110 #define TWT_SET_PARAM_STR_LEN strlen(TWT_SET_PARAM_STR)
111
112 #define TWT_CMD_NOT_EXIST -EINVAL
113 #define DEFAULT_IFNAME "wlan0"
114 #define TWT_RESP_BUF_LEN 512
115
116 #define SINGLE_SPACE_LEN 1
117 #define SINGLE_DIGIT_LEN 1
118
119 #define DIALOG_ID_STR "dialog_id"
120 #define REQ_TYPE_STR "req_type"
121 #define TRIG_TYPE_STR "trig_type"
122 #define FLOW_TYPE_STR "flow_type"
123 #define WAKE_INTR_EXP_STR "wake_intr_exp"
124 #define PROTECTION_STR "protection"
125 #define WAKE_TIME_STR "wake_time"
126 #define WAKE_DUR_STR "wake_dur"
127 #define WAKE_INTR_MANTISSA_STR "wake_intr_mantissa"
128 #define BROADCAST_STR "broadcast"
129 #define MIN_WAKE_INTVL_STR "min_wake_intvl"
130 #define MAX_WAKE_INTVL_STR "max_wake_intvl"
131 #define MIN_WAKE_DUR_STR "min_wake_duration"
132 #define MAX_WAKE_DUR_STR "max_wake_duration"
133 #define NEXT_TWT_STR "next_twt"
134 #define NEXT2_TWT_STR "next2_twt"
135 #define NEXT_TWT_SIZE_STR "next_twt_size"
136 #define PAUSE_DURATION_STR "pause_duration"
137 #define WAKE_TSF_STR "wake_tsf"
138 #define ANNOUNCE_TIMEOUT_STR "announce_timeout"
139 #define AP_AC_VALUE_STR "ap_ac_value"
140 #define MAC_ADDRESS_STR "mac_addr"
141
142 #define DIALOG_ID_STR_LEN strlen(DIALOG_ID_STR)
143 #define REQ_TYPE_STR_LEN strlen(REQ_TYPE_STR)
144 #define TRIG_TYPE_STR_LEN strlen(TRIG_TYPE_STR)
145 #define FLOW_TYPE_STR_LEN strlen(FLOW_TYPE_STR)
146 #define WAKE_INTR_EXP_STR_LEN strlen(WAKE_INTR_EXP_STR)
147 #define PROTECTION_STR_LEN strlen(PROTECTION_STR)
148 #define WAKE_TIME_STR_LEN strlen(WAKE_TIME_STR)
149 #define WAKE_DUR_STR_LEN strlen(WAKE_DUR_STR)
150 #define WAKE_INTR_MANTISSA_STR_LEN strlen(WAKE_INTR_MANTISSA_STR)
151 #define BROADCAST_STR_LEN strlen(BROADCAST_STR)
152 #define MIN_WAKE_INTVL_STR_LEN strlen(MIN_WAKE_INTVL_STR)
153 #define MAX_WAKE_INTVL_STR_LEN strlen(MAX_WAKE_INTVL_STR)
154 #define MIN_WAKE_DUR_STR_LEN strlen(MIN_WAKE_DUR_STR)
155 #define MAX_WAKE_DUR_STR_LEN strlen(MAX_WAKE_DUR_STR)
156 #define NEXT_TWT_STR_LEN strlen(NEXT_TWT_STR)
157 #define NEXT2_TWT_STR_LEN strlen(NEXT2_TWT_STR)
158 #define NEXT_TWT_SIZE_STR_LEN strlen(NEXT_TWT_SIZE_STR)
159 #define PAUSE_DURATION_STR_LEN strlen(PAUSE_DURATION_STR)
160 #define WAKE_TSF_STR_LEN strlen(WAKE_TSF_STR)
161 #define ANNOUNCE_TIMEOUT_STR_LEN strlen(ANNOUNCE_TIMEOUT_STR)
162 #define AP_AC_VALUE_STR_LEN strlen(AP_AC_VALUE_STR)
163 #define MAC_ADDR_STR_LEN strlen(MAC_ADDRESS_STR)
164
165 #define MAC_ADDR_STR "%02x:%02x:%02x:%02x:%02x:%02x"
166 #define MAC_ADDR_ARRAY(a) (a)[0], (a)[1], (a)[2], (a)[3], (a)[4], (a)[5]
167
168 #define TWT_CTRL_EVENT "CTRL-EVENT-TWT"
169 #define TWT_SETUP_RESP "CTRL-EVENT-TWT SETUP"
170 #define TWT_TEARDOWN_RESP "CTRL-EVENT-TWT TERMINATE"
171 #define TWT_PAUSE_RESP "CTRL-EVENT-TWT PAUSE"
172 #define TWT_RESUME_RESP "CTRL-EVENT-TWT RESUME"
173 #define TWT_NOTIFY_RESP "CTRL-EVENT-TWT NOTIFY"
174 #define TWT_SETUP_RESP_LEN strlen(TWT_SETUP_RESP)
175 #define TWT_TEARDOWN_RESP_LEN strlen(TWT_TEARDOWN_RESP)
176 #define TWT_PAUSE_RESP_LEN strlen(TWT_PAUSE_RESP)
177 #define TWT_RESUME_RESP_LEN strlen(TWT_RESUME_RESP)
178 #define TWT_NOTIFY_RESP_LEN strlen(TWT_NOTIFY_RESP)
179
180 static int twt_async_support = -1;
181
182 struct twt_setup_parameters {
183 u8 dialog_id;
184 u8 req_type;
185 u8 trig_type;
186 u8 flow_type;
187 u8 wake_intr_exp;
188 u8 protection;
189 u32 wake_time;
190 u32 wake_dur;
191 u32 wake_intr_mantissa;
192 u8 bcast;
193 u32 min_wake_intvl;
194 u32 max_wake_intvl;
195 u32 min_wake_duration;
196 u32 max_wake_duration;
197 u64 wake_tsf;
198 u32 announce_timeout_us;
199 };
200
201 struct twt_resume_parameters {
202 u8 dialog_id;
203 u8 next_twt;
204 u32 next2_twt;
205 u32 next_twt_size;
206 };
207
208 struct twt_nudge_parameters {
209 u8 dialog_id;
210 u32 wake_time;
211 u32 next_twt_size;
212 };
213
214 struct twt_set_parameters {
215 u8 ap_ac_value;
216 };
217
218 struct twt_resp_info {
219 char *reply_buf;
220 int reply_buf_len;
221 enum qca_wlan_twt_operation twt_oper;
222 struct wpa_driver_nl80211_data *drv;
223 };
224
225
226 static int wpa_driver_twt_async_resp_event(struct wpa_driver_nl80211_data *drv,
227 u32 vendor_id, u32 subcmd, u8 *data, size_t len);
228
229 /* ============ nl80211 driver extensions =========== */
230 enum csi_state {
231 CSI_STATE_STOP = 0,
232 CSI_STATE_START,
233 };
234
235 struct csi_global_params {
236 struct i802_bss *bss;
237 enum csi_state current_state;
238 char connected_bssid[MAC_ADDR_LEN];
239 int transport_mode;
240 };
241
242 static struct csi_global_params g_csi_param = {0};
243
244 static wpa_driver_oem_cb_table_t *oem_cb_table = NULL;
245
246 #define MCC_QUOTA_MIN 10
247 #define MCC_QUOTA_MAX 90
248 /* Only one quota entry for now */
249 #define MCC_QUOTA_ENTRIES_MAX 1
250
251 struct mcc_quota {
252 uint32_t if_idx;
253 uint32_t quota;
254 };
255
256
get_next_arg(char * cmd)257 static char *get_next_arg(char *cmd)
258 {
259 char *pos = cmd;
260
261 while (*pos != ' ' && *pos != '\0')
262 pos++;
263
264 return pos;
265 }
266
wpa_driver_cmd_set_ani_level(struct i802_bss * bss,int mode,int ofdmlvl)267 static int wpa_driver_cmd_set_ani_level(struct i802_bss *bss, int mode, int ofdmlvl)
268 {
269 struct wpa_driver_nl80211_data *drv = bss->drv;
270 struct nl_msg *msg = NULL;
271 struct nlattr *params = NULL;
272 int ret = 0;
273
274 if (!(msg = nl80211_drv_msg(drv, 0, NL80211_CMD_VENDOR)) ||
275 nla_put_u32(msg, NL80211_ATTR_VENDOR_ID, OUI_QCA) ||
276 nla_put_u32(msg, NL80211_ATTR_VENDOR_SUBCMD,
277 QCA_NL80211_VENDOR_SUBCMD_SET_WIFI_CONFIGURATION) ||
278 !(params = nla_nest_start(msg, NL80211_ATTR_VENDOR_DATA)) ||
279 nla_put_u8(msg, QCA_WLAN_VENDOR_ATTR_CONFIG_ANI_SETTING, mode)) {
280 nlmsg_free(msg);
281 return -1;
282 }
283 if(mode == QCA_WLAN_ANI_SETTING_FIXED) {
284 if(nla_put(msg, QCA_WLAN_VENDOR_ATTR_CONFIG_ANI_LEVEL, sizeof(int32_t), &ofdmlvl)){
285 nlmsg_free(msg);
286 return -1;
287 }
288 }
289 nla_nest_end(msg, params);
290 ret = send_and_recv_msgs(drv, msg, NULL, NULL, NULL, NULL);
291 if (!ret)
292 return 0;
293 wpa_printf(MSG_ERROR, "%s: Failed set_ani_level, ofdmlvl=%d, ret=%d",
294 __FUNCTION__, ofdmlvl, ret);
295 return ret;
296 }
297
wpa_driver_cmd_set_congestion_report(struct i802_bss * bss,char * cmd)298 static int wpa_driver_cmd_set_congestion_report(struct i802_bss *bss, char *cmd)
299 {
300 struct wpa_driver_nl80211_data *drv = bss->drv;
301 struct nl_msg *msg;
302 struct nlattr *params = NULL;
303 char *endptr = NULL;
304 int ret;
305 int enable = -1, threshold = -1, interval = -1;
306
307 wpa_printf(MSG_INFO, "%s enter", __FUNCTION__);
308
309 enable = strtol(cmd, &endptr, 10);
310 if (enable != 0 && enable != 1) {
311 wpa_printf(MSG_ERROR, "%s: invalid enable arg %d", __FUNCTION__, enable);
312 return -EINVAL;
313 }
314
315 if (!(msg = nl80211_drv_msg(drv, 0, NL80211_CMD_VENDOR)) ||
316 nla_put_u32(msg, NL80211_ATTR_VENDOR_ID, OUI_QCA) ||
317 nla_put_u32(msg, NL80211_ATTR_VENDOR_SUBCMD,
318 QCA_NL80211_VENDOR_SUBCMD_MEDIUM_ASSESS) ||
319 !(params = nla_nest_start(msg, NL80211_ATTR_VENDOR_DATA)) ||
320 nla_put_u8(msg, QCA_WLAN_VENDOR_ATTR_MEDIUM_ASSESS_TYPE,
321 QCA_WLAN_MEDIUM_ASSESS_CONGESTION_REPORT) ||
322 nla_put_u8(msg,
323 QCA_WLAN_VENDOR_ATTR_MEDIUM_ASSESS_CONGESTION_REPORT_ENABLE,
324 enable)) {
325 nlmsg_free(msg);
326 return -1;
327 }
328 if (enable == 1) {
329 if (!(*endptr) ||
330 ((threshold = strtol(endptr, &endptr, 10)) < 0 || threshold > 100) ||
331 !(*endptr) ||
332 ((interval = strtol(endptr, &endptr, 10)) < 1 || interval > 30)) {
333 wpa_printf(MSG_ERROR, "%s: args less or invalid", __FUNCTION__);
334 nlmsg_free(msg);
335 return -EINVAL;
336 }
337 if (nla_put_u8(msg,
338 QCA_WLAN_VENDOR_ATTR_MEDIUM_ASSESS_CONGESTION_REPORT_THRESHOLD,
339 threshold) || nla_put_u8(msg,
340 QCA_WLAN_VENDOR_ATTR_MEDIUM_ASSESS_CONGESTION_REPORT_INTERVAL,
341 interval)) {
342 nlmsg_free(msg);
343 return -1;
344 }
345 }
346 nla_nest_end(msg, params);
347 ret = send_and_recv_msgs(drv, msg, NULL, NULL, NULL, NULL);
348 wpa_printf(MSG_INFO, "%s: set congestion report: enable=%d, threshold=%d,"
349 "interval=%d", __FUNCTION__, enable, threshold, interval);
350 if (!ret)
351 return 0;
352 wpa_printf(MSG_ERROR, "%s: Failed set congestion report, ret=%d", __FUNCTION__, ret);
353 return ret;
354 }
355
wpa_driver_cmd_set_tx_power(struct i802_bss * bss,char * cmd)356 static int wpa_driver_cmd_set_tx_power(struct i802_bss *bss, char *cmd)
357 {
358 struct wpa_driver_nl80211_data *drv = bss->drv;
359 struct nl_msg *msg;
360 char *endptr = NULL;
361 int ret;
362 int dbm, mbm;
363
364 wpa_printf(MSG_INFO, "%s enter: dbm=%s", __FUNCTION__, cmd);
365
366 dbm = strtol(cmd, &endptr, 10);
367 if (*endptr || dbm < 0) {
368 wpa_printf(MSG_ERROR, "%s: invalid dbm %d", __FUNCTION__, dbm);
369 return -EINVAL;
370 }
371 mbm = dbm * 100;
372 if (mbm < 0) { // integer overflow
373 wpa_printf(MSG_ERROR, "%s: invalid mbm %d", __FUNCTION__, mbm);
374 return -EINVAL;
375 }
376
377 if (!(msg = nl80211_drv_msg(drv, 0, NL80211_CMD_SET_WIPHY)) ||
378 nla_put_u32(msg, NL80211_ATTR_WIPHY_TX_POWER_SETTING,
379 NL80211_TX_POWER_LIMITED) ||
380 nla_put_u32(msg, NL80211_ATTR_WIPHY_TX_POWER_LEVEL, mbm)) {
381 nlmsg_free(msg);
382 return -ENOBUFS;
383 }
384
385 ret = send_and_recv_msgs(drv, msg, NULL, NULL, NULL, NULL);
386 if (!ret)
387 return 0;
388
389 wpa_printf(MSG_ERROR, "%s: Failed set_tx_power dbm=%d, ret=%d",
390 __FUNCTION__, dbm, ret);
391 return ret;
392 }
393
394 /* Return type for setBand*/
395 enum {
396 SEND_CHANNEL_CHANGE_EVENT = 0,
397 DO_NOT_SEND_CHANNEL_CHANGE_EVENT,
398 };
399
400 typedef struct android_wifi_priv_cmd {
401 char *buf;
402 int used_len;
403 int total_len;
404 } android_wifi_priv_cmd;
405
406 static int drv_errors = 0;
407
wpa_driver_notify_country_change(void * ctx,char * cmd)408 static void wpa_driver_notify_country_change(void *ctx, char *cmd)
409 {
410 if ((os_strncasecmp(cmd, "COUNTRY", 7) == 0) ||
411 (os_strncasecmp(cmd, "SETBAND", 7) == 0)) {
412 union wpa_event_data event;
413
414 os_memset(&event, 0, sizeof(event));
415 event.channel_list_changed.initiator = REGDOM_SET_BY_USER;
416 if (os_strncasecmp(cmd, "COUNTRY", 7) == 0) {
417 event.channel_list_changed.type = REGDOM_TYPE_COUNTRY;
418 if (os_strlen(cmd) > 9) {
419 event.channel_list_changed.alpha2[0] = cmd[8];
420 event.channel_list_changed.alpha2[1] = cmd[9];
421 }
422 } else {
423 event.channel_list_changed.type = REGDOM_TYPE_UNKNOWN;
424 }
425 wpa_supplicant_event(ctx, EVENT_CHANNEL_LIST_CHANGED, &event);
426 }
427 }
428
429 static struct remote_sta_info g_sta_info = {0};
430 static struct bss_info g_bss_info = {0};
431
prepare_nlmsg(struct wpa_driver_nl80211_data * drv,char * ifname,int cmdid,int subcmd,int flag)432 static struct nl_msg *prepare_nlmsg(struct wpa_driver_nl80211_data *drv,
433 char *ifname, int cmdid, int subcmd,
434 int flag)
435 {
436 int res;
437 struct nl_msg *nlmsg = nlmsg_alloc();
438 int ifindex;
439
440 if (nlmsg == NULL) {
441 wpa_printf(MSG_ERROR,"Out of memory");
442 return NULL;
443 }
444
445 genlmsg_put(nlmsg, /* pid = */ 0, /* seq = */ 0,
446 drv->global->nl80211_id, 0, flag, cmdid, 0);
447
448 if (cmdid == NL80211_CMD_VENDOR) {
449 res = nla_put_u32(nlmsg, NL80211_ATTR_VENDOR_ID, OUI_QCA);
450 if (res < 0) {
451 wpa_printf(MSG_ERROR,"Failed to put vendor id");
452 goto cleanup;
453 }
454
455 res = nla_put_u32(nlmsg, NL80211_ATTR_VENDOR_SUBCMD, subcmd);
456 if (res < 0) {
457 wpa_printf(MSG_ERROR,"Failed to put vendor sub command");
458 goto cleanup;
459 }
460 }
461
462 if (ifname && (strlen(ifname) > 0))
463 ifindex = if_nametoindex(ifname);
464 else
465 ifindex = if_nametoindex(DEFAULT_IFNAME);
466
467 if (nla_put_u32(nlmsg, NL80211_ATTR_IFINDEX, ifindex) != 0) {
468 wpa_printf(MSG_ERROR,"Failed to get iface index for iface: %s", ifname);
469 goto cleanup;
470 }
471
472 return nlmsg;
473
474 cleanup:
475 if (nlmsg)
476 nlmsg_free(nlmsg);
477 return NULL;
478 }
479
prepare_vendor_nlmsg(struct wpa_driver_nl80211_data * drv,char * ifname,int subcmd)480 static struct nl_msg *prepare_vendor_nlmsg(struct wpa_driver_nl80211_data *drv,
481 char *ifname, int subcmd)
482 {
483 return prepare_nlmsg(drv, ifname, NL80211_CMD_VENDOR, subcmd, 0);
484 }
485
parse_station_info(struct resp_info * info,struct nlattr * vendata,int datalen)486 static int parse_station_info(struct resp_info *info, struct nlattr *vendata,
487 int datalen)
488 {
489 struct nlattr *tb_vendor[GET_STATION_INFO_MAX + 1];
490 struct nlattr *attr, *attr1, *attr2;
491 u8 *beacon_ies = NULL;
492 size_t beacon_ies_len = 0;
493 u8 seg1;
494
495 g_bss_info.oui[0] = (OUI_QCA) & 0xFF;
496 g_bss_info.oui[1] = ((OUI_QCA)>>8) & 0xFF;
497 g_bss_info.oui[2] = ((OUI_QCA)>>16) & 0xFF;
498
499 nla_parse(tb_vendor, GET_STATION_INFO_MAX,
500 vendata, datalen, NULL);
501
502 attr = tb_vendor[QCA_WLAN_VENDOR_ATTR_GET_STATION_LINK_INFO_ATTR];
503 if (attr) {
504 struct nlattr *tb1[NL80211_ATTR_MAX + 1];
505
506 nla_parse(tb1, NL80211_ATTR_MAX, nla_data(attr),
507 nla_len(attr), NULL);
508 if (tb1[NL80211_ATTR_SSID] &&
509 (nla_len(tb1[NL80211_ATTR_SSID]) <= MAX_SSID_LEN)) {
510 os_memcpy(g_bss_info.ssid, nla_data(tb1[NL80211_ATTR_SSID]),
511 nla_len(tb1[NL80211_ATTR_SSID]));
512 g_bss_info.ssid[nla_len(tb1[NL80211_ATTR_SSID])] = '\0';
513 } else {
514 wpa_printf(MSG_ERROR,"NL80211_ATTR_SSID not found");
515 }
516 if (tb1[NL80211_ATTR_MAC]) {
517 os_memcpy(g_bss_info.oui, nla_data(tb1[NL80211_ATTR_MAC]), OUI_LEN);
518 } else {
519 wpa_printf(MSG_ERROR,"NL80211_ATTR_MAC not found");
520 }
521 if (tb1[NL80211_ATTR_SURVEY_INFO]) {
522 struct nlattr *tb2[NL80211_SURVEY_INFO_MAX + 1];
523
524 nla_parse(tb2, NL80211_SURVEY_INFO_MAX,
525 nla_data(tb1[NL80211_ATTR_SURVEY_INFO]),
526 nla_len(tb1[NL80211_ATTR_SURVEY_INFO]), NULL);
527 if (tb2[NL80211_SURVEY_INFO_FREQUENCY]) {
528 g_bss_info.channel =
529 nla_get_u32(tb2[NL80211_SURVEY_INFO_FREQUENCY]);
530 } else {
531 wpa_printf(MSG_ERROR,
532 "NL80211_SURVEY_INFO_FREQUENCY not found");
533 }
534 if (tb2[NL80211_SURVEY_INFO_NOISE]) {
535 g_bss_info.noise =
536 nla_get_u8(tb2[NL80211_SURVEY_INFO_NOISE]);
537 g_bss_info.noise -= 100;
538 } else {
539 wpa_printf(MSG_ERROR,"NL80211_SURVEY_INFO_NOISE not found");
540 }
541 } else {
542 wpa_printf(MSG_ERROR,"NL80211_ATTR_SURVEY_INFO not found");
543 }
544
545 if (tb1[NL80211_ATTR_STA_INFO]) {
546 struct nlattr *tb2[NL80211_STA_INFO_MAX + 1];
547
548 nla_parse(tb2, NL80211_STA_INFO_MAX,
549 nla_data(tb1[NL80211_ATTR_STA_INFO]),
550 nla_len(tb1[NL80211_ATTR_STA_INFO]), NULL);
551 if (tb2[NL80211_STA_INFO_SIGNAL]) {
552 g_bss_info.rssi =
553 nla_get_u8(tb2[NL80211_STA_INFO_SIGNAL]);
554 g_bss_info.rssi -= 100;
555 } else {
556 wpa_printf(MSG_ERROR,"NL80211_STA_INFO_SIGNAL not found");
557 }
558 g_bss_info.snr = g_bss_info.rssi - g_bss_info.noise;
559
560 attr1 = tb2[NL80211_STA_INFO_TX_BITRATE];
561 if (attr1) {
562 struct nlattr *tb3[NL80211_RATE_INFO_MAX + 1];
563
564 nla_parse(tb3, NL80211_RATE_INFO_MAX,
565 nla_data(attr1), nla_len(attr1),
566 NULL);
567 if (tb3[NL80211_RATE_INFO_BITRATE32]) {
568 g_bss_info.data_rate = nla_get_u32(
569 tb3[NL80211_RATE_INFO_BITRATE32])/10;
570 } else if (tb3[NL80211_RATE_INFO_BITRATE]) {
571 g_bss_info.data_rate = nla_get_u16(
572 tb3[NL80211_RATE_INFO_BITRATE])/10;
573 }
574
575 } else {
576 wpa_printf(MSG_ERROR,"NL80211_STA_INFO_TX_BITRATE not found");
577 }
578 } else {
579 wpa_printf(MSG_ERROR,"NL80211_ATTR_STA_INFO not found");
580 }
581 } else {
582 wpa_printf(MSG_ERROR,
583 "QCA_WLAN_VENDOR_ATTR_GET_STATION_LINK_INFO_ATTR not found");
584 }
585
586 if (tb_vendor[GET_STATION_INFO_AKM]) {
587 g_bss_info.akm = nla_get_u32(
588 tb_vendor[GET_STATION_INFO_AKM]);
589 }
590
591 if (tb_vendor[QCA_WLAN_VENDOR_ATTR_802_11_MODE])
592 g_bss_info.mode_80211 = nla_get_u32(
593 tb_vendor[QCA_WLAN_VENDOR_ATTR_802_11_MODE]);
594
595 attr = tb_vendor[GET_STATION_INFO_VHT_OPERATION];
596 attr1 = tb_vendor[GET_STATION_INFO_HT_OPERATION];
597 attr2 = tb_vendor[GET_STATION_INFO_HE_OPERATION];
598 if (attr) {
599 struct ieee80211_vht_operation *info = nla_data(attr);
600
601 switch (info->vht_op_info_chwidth) {
602 case CHANWIDTH_USE_HT:
603 if (attr1) {
604 struct ieee80211_ht_operation *info;
605
606 info = nla_data(attr1);
607 g_bss_info.bw = info->ht_param ? 40:20;
608 }
609 break;
610 case CHANWIDTH_80MHZ:
611 seg1 = info->vht_op_info_chan_center_freq_seg1_idx;
612 if (seg1)
613 /* Notifying 80P80 also as bandwidth = 160 */
614 g_bss_info.bw = 160;
615 else
616 g_bss_info.bw = 80;
617 break;
618 case CHANWIDTH_160MHZ:
619 g_bss_info.bw = 160;
620 break;
621 case CHANWIDTH_80P80MHZ:
622 g_bss_info.bw = 160;
623 break;
624 default:
625 wpa_printf(MSG_ERROR,"Invalid channel width received : %u",
626 info->vht_op_info_chwidth);
627 }
628 } else if (attr1) {
629 struct ieee80211_ht_operation *info = nla_data(attr1);
630
631 g_bss_info.bw = info->ht_param ? 40:20;
632 } else
633 g_bss_info.bw = 20;
634
635 if (attr2) {
636 struct ieee80211_he_operation *he_info = nla_data(attr2);
637 uint8_t *opr, ch_bw = CHANNEL_BW_INVALID;
638
639 /* Check optional field in he_info is present of not */
640 if (!he_info || (nla_len(attr2) <=
641 (sizeof(he_info->he_oper_params) +
642 sizeof(he_info->he_mcs_nss_set)))) {
643 he_info ? wpa_printf(MSG_ERROR,"Invalid he operation len: %d", nla_len(attr2)):
644 wpa_printf(MSG_ERROR,"Invalid he_info: NULL");
645 goto parse_beacon_ies;
646 }
647
648 opr = (uint8_t *)he_info;
649 /* Point to operational field */
650 opr += (sizeof(he_info->he_oper_params) +
651 sizeof(he_info->he_mcs_nss_set));
652 if (he_info->he_oper_params &
653 IEEE80211_HE_OPERATION_VHT_OPER_MASK) {
654 ch_bw = opr[HE_OPER_VHT_CH_WIDTH_OFFSET];
655 switch (ch_bw) {
656 case CHANWIDTH_USE_HT:
657 /* TO DO */
658 break;
659 case CHANWIDTH_80MHZ:
660 seg1 = opr[HE_OPER_VHT_CENTER_FRQ_SEG1_OFFSET];
661 if (seg1)
662 /* Notifying 80P80 also as bandwidth = 160 */
663 g_bss_info.bw = 160;
664 else
665 g_bss_info.bw = 80;
666 break;
667 case CHANWIDTH_160MHZ:
668 g_bss_info.bw = 160;
669 break;
670 case CHANWIDTH_80P80MHZ:
671 g_bss_info.bw = 160;
672 break;
673 default:
674 break;
675 }
676 opr += (HE_OPER_VHT_MAX_OFFSET + 1);
677 }
678
679 if (he_info->he_oper_params &
680 IEEE80211_HE_OPERATION_CO_LOC_BSS_MASK) {
681 opr += (HE_OPER_CO_LOCATED_MAX_OFFSET + 1);
682 }
683
684 if (he_info->he_oper_params &
685 IEEE80211_HE_OPERATION_6G_OPER_MASK) {
686 ch_bw = (opr[HE_OPER_6G_PARAMS_OFFSET] &
687 HE_OPER_6G_PARAMS_SUB_CH_BW_MASK);
688 switch (ch_bw) {
689 case HE_CHANWIDTH_20MHZ:
690 g_bss_info.bw = 20;
691 break;
692 case HE_CHANWIDTH_40MHZ:
693 g_bss_info.bw = 40;
694 break;
695 case HE_CHANWIDTH_80MHZ:
696 g_bss_info.bw = 80;
697 break;
698 case HE_CHANWIDTH_160MHZ:
699 /* Notifying 80P80 also as bandwidth = 160 */
700 g_bss_info.bw = 160;
701 break;
702 default:
703 wpa_printf(MSG_ERROR,"Invalid channel width received : %u", ch_bw);
704 }
705 }
706
707 }
708
709 parse_beacon_ies:
710 attr = tb_vendor[GET_STATION_INFO_BEACON_IES];
711 if (attr) {
712 beacon_ies = nla_data(attr);
713
714 beacon_ies_len = nla_len(attr);
715 if (beacon_ies && beacon_ies_len > 12) {
716 beacon_ies += 12;
717 beacon_ies_len -= 12;
718 }
719 }
720
721 if (tb_vendor[GET_STATION_INFO_DRIVER_DISCONNECT_REASON]) {
722 g_bss_info.disc_reasn_code = nla_get_u32(tb_vendor[
723 GET_STATION_INFO_DRIVER_DISCONNECT_REASON]);
724 }
725 snprintf(info->reply_buf, info->reply_buf_len,
726 "%02x%02x%02x %s %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %u %s %d %d %d %d %d %d %d %d %d %d %d %d %d",
727 g_bss_info.oui[0],
728 g_bss_info.oui[1],
729 g_bss_info.oui[2],
730 g_bss_info.ssid,
731 g_bss_info.channel,
732 g_bss_info.bw,
733 g_bss_info.rssi,
734 g_bss_info.data_rate,
735 g_bss_info.mode_80211,
736 -1,
737 -1,
738 -1,
739 g_bss_info.snr,
740 g_bss_info.noise,
741 g_bss_info.akm,
742 g_bss_info.roaming_count,
743 -1,
744 -1,
745 -1,
746 -1,
747 g_bss_info.disc_reasn_code,
748 info->country,
749 g_bss_info.ani_level,
750 -1,
751 -1,
752 -1,
753 g_bss_info.roam_trigger_reason,
754 g_bss_info.roam_fail_reason,
755 g_bss_info.roam_invoke_fail_reason,
756 g_bss_info.tsf_out_of_sync_count,
757 g_bss_info.latest_tx_power,
758 g_bss_info.latest_tx_rate,
759 g_bss_info.target_power_24g_1mbps,
760 g_bss_info.target_power_24g_6mbps,
761 g_bss_info.target_power_5g_6mbps);
762
763 return 0;
764 }
765
parse_get_feature_info(struct resp_info * info,struct nlattr * vendata,int datalen)766 static int parse_get_feature_info(struct resp_info *info, struct nlattr *vendata,
767 int datalen)
768 {
769 struct nlattr *tb_vendor[NUM_QCA_WLAN_VENDOR_FEATURES + 1];
770 struct nlattr *attr;
771 nla_parse(tb_vendor, NUM_QCA_WLAN_VENDOR_FEATURES,
772 vendata, datalen, NULL);
773 attr = tb_vendor[QCA_WLAN_VENDOR_ATTR_FEATURE_FLAGS];
774 if (attr) {
775 snprintf(info->reply_buf, info->reply_buf_len, "%u",
776 nla_get_u32(attr));
777 wpa_printf(MSG_DEBUG, "%s: driver supported feature info = %s",
778 __func__, info->reply_buf);
779 } else {
780 snprintf(info->reply_buf, info->reply_buf_len, "FAIL");
781 return -1;
782 }
783 return 0;
784 }
handle_response(struct resp_info * info,struct nlattr * vendata,int datalen)785 static int handle_response(struct resp_info *info, struct nlattr *vendata,
786 int datalen)
787 {
788 switch (info->subcmd) {
789 case QCA_NL80211_VENDOR_SUBCMD_GET_STATION:
790 os_memset(info->reply_buf, 0, info->reply_buf_len);
791 if (info->cmd_type == GETSTATSBSSINFO)
792 parse_station_info(info, vendata, datalen);
793
794 wpa_printf(MSG_INFO,"STAINFO: %s", info->reply_buf);
795 break;
796 case QCA_NL80211_VENDOR_SUBCMD_GET_FEATURES:
797 os_memset(info->reply_buf, 0, info->reply_buf_len);
798 parse_get_feature_info(info, vendata, datalen);
799 break;
800 default:
801 wpa_printf(MSG_ERROR,"Unsupported response type: %d", info->subcmd);
802 break;
803 }
804 return 0;
805 }
806
response_handler(struct nl_msg * msg,void * arg)807 static int response_handler(struct nl_msg *msg, void *arg)
808 {
809 struct genlmsghdr *mHeader;
810 struct nlattr *mAttributes[NL80211_ATTR_MAX_INTERNAL + 1];
811 struct nlattr *vendata;
812 int datalen;
813 struct resp_info *info = (struct resp_info *) arg;
814 int status;
815
816 mHeader = (struct genlmsghdr *)nlmsg_data(nlmsg_hdr(msg));
817 nla_parse(mAttributes, NL80211_ATTR_MAX_INTERNAL,
818 genlmsg_attrdata(mHeader, 0),
819 genlmsg_attrlen(mHeader, 0), NULL);
820
821 if (mAttributes[NL80211_ATTR_VENDOR_DATA]) {
822 vendata = nla_data(mAttributes[NL80211_ATTR_VENDOR_DATA]);
823 datalen = nla_len(mAttributes[NL80211_ATTR_VENDOR_DATA]);
824 if (!vendata) {
825 wpa_printf(MSG_ERROR,"Vendor data not found");
826 return -1;
827 }
828 status = handle_response(info, vendata, datalen);
829 } else {
830 wpa_printf(MSG_ERROR,"NL80211_ATTR_VENDOR_DATA not found");
831 status = -1;
832 }
833
834 return status;
835 }
836
ack_handler(struct nl_msg * msg,void * arg)837 static int ack_handler(struct nl_msg *msg, void *arg)
838 {
839 int *err = (int *)arg;
840
841 *err = 0;
842 return NL_STOP;
843 }
844
wpa_driver_nl80211_oem_event(struct wpa_driver_nl80211_data * drv,u32 vendor_id,u32 subcmd,u8 * data,size_t len)845 int wpa_driver_nl80211_oem_event(struct wpa_driver_nl80211_data *drv,
846 u32 vendor_id, u32 subcmd,
847 u8 *data, size_t len)
848 {
849 int ret = -1, lib_n;
850 if (wpa_driver_oem_initialize(&oem_cb_table) != WPA_DRIVER_OEM_STATUS_FAILURE &&
851 oem_cb_table) {
852 for (lib_n = 0;
853 oem_cb_table[lib_n].wpa_driver_driver_cmd_oem_cb != NULL;
854 lib_n++) {
855 if(oem_cb_table[lib_n].wpa_driver_nl80211_driver_oem_event) {
856 ret = oem_cb_table[lib_n].wpa_driver_nl80211_driver_oem_event(
857 drv, vendor_id,subcmd, data, len);
858 if (ret == WPA_DRIVER_OEM_STATUS_SUCCESS ) {
859 break;
860 } else if (ret == WPA_DRIVER_OEM_STATUS_ENOSUPP) {
861 continue;
862 } else if (ret == WPA_DRIVER_OEM_STATUS_FAILURE) {
863 wpa_printf(MSG_DEBUG, "%s: Received error: %d",
864 __func__, ret);
865 break;
866 }
867 }
868 }
869 }
870
871 return ret;
872 }
873
874 static int wpa_driver_restart_csi(struct i802_bss *bss, int *status);
875
wpa_driver_nl80211_driver_event(struct wpa_driver_nl80211_data * drv,u32 vendor_id,u32 subcmd,u8 * data,size_t len)876 int wpa_driver_nl80211_driver_event(struct wpa_driver_nl80211_data *drv,
877 u32 vendor_id, u32 subcmd,
878 u8 *data, size_t len)
879 {
880 int ret = -1;
881 int status = -1;
882 struct i802_bss *bss;
883
884 ret = wpa_driver_nl80211_oem_event(drv, vendor_id, subcmd,
885 data, len);
886
887 if(ret != WPA_DRIVER_OEM_STATUS_ENOSUPP)
888 return ret;
889
890 switch(subcmd) {
891 case QCA_NL80211_VENDOR_SUBCMD_CONFIG_TWT:
892 ret = wpa_driver_twt_async_resp_event(drv, vendor_id, subcmd,
893 data, len);
894 break;
895 case QCA_NL80211_VENDOR_SUBCMD_KEY_MGMT_ROAM_AUTH:
896 if(g_csi_param.current_state == CSI_STATE_START) {
897 bss = get_bss_ifindex(drv, drv->ifindex);
898 if(bss == NULL) {
899 wpa_printf(MSG_DEBUG, "%s: bss is NULL",
900 __func__);
901 break;
902 }
903 if(wpa_driver_restart_csi(bss, &status))
904 wpa_printf(MSG_DEBUG, "csi_restart failed %d",
905 status);
906 }
907 break;
908 default:
909 break;
910 }
911 return ret;
912 }
913
finish_handler(struct nl_msg * msg,void * arg)914 static int finish_handler(struct nl_msg *msg, void *arg)
915 {
916 int *ret = (int *)arg;
917
918 *ret = 0;
919 return NL_SKIP;
920 }
921
922
error_handler(struct sockaddr_nl * nla,struct nlmsgerr * err,void * arg)923 static int error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err,
924 void *arg)
925 {
926 int *ret = (int *)arg;
927
928 *ret = err->error;
929 wpa_printf(MSG_ERROR,"%s received : %d - %s", __func__,
930 err->error, strerror(err->error));
931 return NL_SKIP;
932 }
933
934
no_seq_check(struct nl_msg * msg,void * arg)935 static int no_seq_check(struct nl_msg *msg, void *arg)
936 {
937 return NL_OK;
938 }
939
send_nlmsg(struct nl_sock * cmd_sock,struct nl_msg * nlmsg,nl_recvmsg_msg_cb_t customer_cb,void * arg)940 static int send_nlmsg(struct nl_sock *cmd_sock, struct nl_msg *nlmsg,
941 nl_recvmsg_msg_cb_t customer_cb, void *arg)
942 {
943 int err = 0;
944 struct nl_cb *cb = nl_cb_alloc(NL_CB_DEFAULT);
945
946 if (!cb)
947 goto out;
948
949 err = nl_send_auto_complete(cmd_sock, nlmsg); /* send message */
950 if (err < 0)
951 goto out;
952
953 err = 1;
954
955 nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL);
956 nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err);
957 nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err);
958 nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err);
959 if (customer_cb)
960 nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, customer_cb, arg);
961
962 while (err > 0) { /* wait for reply */
963 int res = nl_recvmsgs(cmd_sock, cb);
964
965 if (res)
966 wpa_printf(MSG_ERROR,"nl80211: %s->nl_recvmsgs failed: %d",
967 __func__, res);
968 }
969 out:
970 nl_cb_put(cb);
971 if (nlmsg)
972 nlmsg_free(nlmsg);
973 return err;
974 }
975
chartohex(char c)976 static int chartohex(char c)
977 {
978 int val = -1;
979
980 if (c >= '0' && c <= '9')
981 val = c - '0';
982 else if (c >= 'a' && c <= 'f')
983 val = c - 'a' + 10;
984 else if (c >= 'A' && c <= 'F')
985 val = c - 'A' + 10;
986
987 return val;
988 }
989
convert_string_to_bytes(u8 * addr,const char * text,u16 max_bytes)990 static int convert_string_to_bytes(u8 *addr, const char *text, u16 max_bytes)
991 {
992 u16 i = 0;
993 int nibble;
994 const char *temp = text;
995
996 while (temp && *temp != '\0' && i < max_bytes) {
997 nibble = chartohex(*temp++);
998 if (nibble == -1)
999 return -1;
1000 addr[i] = nibble << 4;
1001 nibble = chartohex(*temp++);
1002 if (nibble == -1)
1003 return -1;
1004 addr[i++] += nibble;
1005 if (*temp == ':')
1006 temp++;
1007 }
1008
1009 return i;
1010 }
1011
1012 /*
1013 * Client can send the cell switch mode in below format
1014 *
1015 * SETCELLSWITCHMODE <cs mode>
1016 *
1017 * examples:
1018 * For Default Mode - "SETCELLSWITCHMODE 0"
1019 * To Disable Roaming - "SETCELLSWITCHMODE 1"
1020 * For Partial Scan - "SETCELLSWITCHMODE 2"
1021 */
parse_and_populate_setcellswitchmode(struct nl_msg * nlmsg,char * cmd)1022 static int parse_and_populate_setcellswitchmode(struct nl_msg *nlmsg,
1023 char *cmd)
1024 {
1025 uint32_t all_trigger_bitmap, scan_scheme_bitmap;
1026 uint32_t cellswm;
1027 struct nlattr *config;
1028
1029 cellswm = atoi(cmd);
1030 if (cellswm < 0 || cellswm > 2) {
1031 wpa_printf(MSG_ERROR,"Invalid cell switch mode: %d", cellswm);
1032 return -1;
1033 }
1034 wpa_printf(MSG_DEBUG, "cell switch mode: %d", cellswm);
1035
1036 all_trigger_bitmap = QCA_ROAM_TRIGGER_REASON_PER |
1037 QCA_ROAM_TRIGGER_REASON_BEACON_MISS |
1038 QCA_ROAM_TRIGGER_REASON_POOR_RSSI |
1039 QCA_ROAM_TRIGGER_REASON_BETTER_RSSI |
1040 QCA_ROAM_TRIGGER_REASON_PERIODIC |
1041 QCA_ROAM_TRIGGER_REASON_DENSE |
1042 QCA_ROAM_TRIGGER_REASON_BTM |
1043 QCA_ROAM_TRIGGER_REASON_BSS_LOAD |
1044 QCA_ROAM_TRIGGER_REASON_USER_TRIGGER |
1045 QCA_ROAM_TRIGGER_REASON_DEAUTH |
1046 QCA_ROAM_TRIGGER_REASON_IDLE |
1047 QCA_ROAM_TRIGGER_REASON_TX_FAILURES |
1048 QCA_ROAM_TRIGGER_REASON_EXTERNAL_SCAN;
1049
1050 scan_scheme_bitmap = QCA_ROAM_TRIGGER_REASON_PER |
1051 QCA_ROAM_TRIGGER_REASON_BEACON_MISS |
1052 QCA_ROAM_TRIGGER_REASON_POOR_RSSI |
1053 QCA_ROAM_TRIGGER_REASON_BSS_LOAD |
1054 QCA_ROAM_TRIGGER_REASON_BTM;
1055
1056 if (nla_put_u32(nlmsg, QCA_WLAN_VENDOR_ATTR_ROAMING_SUBCMD,
1057 QCA_WLAN_VENDOR_ROAMING_SUBCMD_CONTROL_SET) ||
1058 nla_put_u32(nlmsg, QCA_WLAN_VENDOR_ATTR_ROAMING_REQ_ID, 1)) {
1059 wpa_printf(MSG_ERROR,"Failed to put: roam_subcmd/REQ_ID");
1060 }
1061
1062 config = nla_nest_start(nlmsg,
1063 QCA_WLAN_VENDOR_ATTR_ROAMING_PARAM_CONTROL);
1064 if (config == NULL)
1065 goto fail;
1066
1067 switch (cellswm){
1068 case 0:
1069 if (nla_put_u32(nlmsg, QCA_ATTR_ROAM_CONTROL_TRIGGERS, all_trigger_bitmap)) {
1070 wpa_printf(MSG_ERROR,"Failed to set: ROAM_CONTROL_TRIGGERS");
1071 goto fail;
1072 }
1073 break;
1074 case 1:
1075 if (nla_put_u32(nlmsg, QCA_ATTR_ROAM_CONTROL_TRIGGERS, 0)) {
1076 wpa_printf(MSG_ERROR,"Failed to unset: ROAM_CONTROL_TRIGGERS");
1077 goto fail;
1078 }
1079 break;
1080 case 2:
1081 if (nla_put_u32(nlmsg, QCA_ATTR_ROAM_CONTROL_TRIGGERS, all_trigger_bitmap) ||
1082 nla_put_u32(nlmsg, QCA_ATTR_ROAM_CONTROL_SCAN_SCHEME_TRIGGERS, scan_scheme_bitmap)) {
1083 wpa_printf(MSG_ERROR,"Failed to set: ROAM_CONTROL_TRIGGERS_SCAN_SCHEME");
1084 goto fail;
1085 }
1086 break;
1087 }
1088 nla_nest_end(nlmsg, config);
1089
1090 return 0;
1091 fail:
1092 return -1;
1093
1094 }
1095
populate_nlmsg(struct nl_msg * nlmsg,char * cmd,enum get_info_cmd type)1096 static int populate_nlmsg(struct nl_msg *nlmsg, char *cmd,
1097 enum get_info_cmd type)
1098 {
1099 struct nlattr *attr;
1100
1101 attr = nla_nest_start(nlmsg, NL80211_ATTR_VENDOR_DATA);
1102 if (attr == NULL)
1103 return -1;
1104
1105 switch (type) {
1106 case GETSTATSBSSINFO:
1107 if (nla_put_flag(nlmsg,
1108 QCA_WLAN_VENDOR_ATTR_GET_STATION_INFO)) {
1109 wpa_printf(MSG_ERROR,"Failed to put flag QCA_WLAN_VENDOR_ATTR_GET_STATION_INFO");
1110 return -1;
1111 }
1112 break;
1113 case SETCELLSWITCHMODE:
1114 if (parse_and_populate_setcellswitchmode(nlmsg, cmd) != 0) {
1115 wpa_printf(MSG_ERROR, "Failed to populate nlmsg");
1116 return -1;
1117 }
1118 break;
1119 default:
1120 wpa_printf(MSG_ERROR,"Unsupported command: %d", type);
1121 break;
1122 }
1123
1124 nla_nest_end(nlmsg, attr);
1125 return 0;
1126 }
1127
skip_white_space(char * cmd)1128 static char *skip_white_space(char *cmd)
1129 {
1130 char *pos = cmd;
1131
1132 while (*pos == ' ')
1133 pos++;
1134
1135 return pos;
1136 }
1137
ap_sta_copy_supp_op_classes(const u8 * supp_op_classes,size_t supp_op_classes_len)1138 void ap_sta_copy_supp_op_classes(const u8 *supp_op_classes,
1139 size_t supp_op_classes_len) {
1140 if (!supp_op_classes)
1141 return;
1142 os_free(g_sta_info.supp_op_classes);
1143 g_sta_info.supp_op_classes = malloc(1 + supp_op_classes_len);
1144 if(!g_sta_info.supp_op_classes)
1145 return;
1146 g_sta_info.supp_op_classes[0] = supp_op_classes_len;
1147 os_memcpy(g_sta_info.supp_op_classes + 1, supp_op_classes,
1148 supp_op_classes_len);
1149 }
1150
ap_sta_copy_channels(const u8 * supp_channels,size_t supp_channels_len)1151 void ap_sta_copy_channels(const u8 *supp_channels,
1152 size_t supp_channels_len) {
1153 if (!supp_channels)
1154 return;
1155 os_free(g_sta_info.supp_channels);
1156 g_sta_info.supp_channels = malloc(1 + supp_channels_len);
1157 if(!g_sta_info.supp_channels)
1158 return;
1159 g_sta_info.supp_channels[0] = supp_channels_len;
1160 os_memcpy(g_sta_info.supp_channels + 1, supp_channels,
1161 supp_channels_len);
1162 }
1163
parse_ext_ie(const u8 * ie,int ie_len)1164 static void parse_ext_ie(const u8 *ie, int ie_len)
1165 {
1166 u8 ext_id;
1167
1168 if (ie_len < 1) {
1169 wpa_printf(MSG_ERROR,"parse error, ie_len = %d", ie_len);
1170 return;
1171 }
1172
1173 ext_id = *ie++;
1174 ie_len--;
1175
1176 switch (ext_id) {
1177 case WLAN_EID_EXT_HE_CAPABILITIES:
1178 wpa_printf(MSG_INFO,"HE supported");
1179 g_sta_info.flags.he_supported = 1;
1180 break;
1181 default:
1182 wpa_printf(MSG_DEBUG,"ext_id = %d", ext_id);
1183 break;
1184 }
1185
1186 return;
1187 }
1188
parse_assoc_req_ies(const u8 * ies,int ies_len)1189 static void parse_assoc_req_ies(const u8 *ies, int ies_len)
1190 {
1191 int left = ies_len;
1192 const u8 *pos = ies;
1193
1194 while (left >= 2) {
1195 u8 id, ie_len;
1196 id = *pos++;
1197 ie_len = *pos++;
1198 left -= 2;
1199
1200 if (ie_len > left) {
1201 wpa_printf(MSG_ERROR,"parse error, id = %d, ie_len = %d, left = %d",
1202 id, ie_len, left);
1203 return;
1204 }
1205
1206 switch (id) {
1207 case WLAN_EID_SUPPORTED_OPERATING_CLASSES:
1208 ap_sta_copy_supp_op_classes(pos, ie_len);
1209 break;
1210 case WLAN_EID_SUPPORTED_CHANNELS:
1211 ap_sta_copy_channels(pos, ie_len);
1212 break;
1213 case WLAN_EID_HT_CAP:
1214 wpa_printf(MSG_INFO,"HT supported");
1215 g_sta_info.flags.ht_supported = 1;
1216 break;
1217 case WLAN_EID_VHT_CAP:
1218 wpa_printf(MSG_INFO,"VHT supported");
1219 g_sta_info.flags.vht_supported = 1;
1220 break;
1221 case WLAN_EID_EXTENSION:
1222 parse_ext_ie(pos, ie_len);
1223 break;
1224 default:
1225 break;
1226 }
1227
1228 left -= ie_len;
1229 pos += ie_len;
1230 }
1231
1232 if (left)
1233 wpa_printf(MSG_ERROR,"parse error, left = %d", left);
1234 }
1235
op_class_band_conversion(u8 * op_classes)1236 void op_class_band_conversion(u8 *op_classes) {
1237 int count = (g_sta_info.supp_op_classes[0]);
1238 int i = 1;
1239 int temp;
1240
1241 if (count <= 1)
1242 g_sta_info.supported_band = 0;
1243 while((count-1) != 0) {
1244 temp = g_sta_info.supp_op_classes[i];
1245 if (temp >= 81 && temp <= 84)
1246 g_sta_info.supported_band |= BIT(0);
1247 else if (temp >= 115 && temp <= 130)
1248 g_sta_info.supported_band |= BIT(1);
1249 else if (temp >= 131 && temp <= 135)
1250 g_sta_info.supported_band |= BIT(2);
1251 i++;
1252 count--;
1253 }
1254 }
1255
supp_channels_band_conversion(u8 * supp_channels)1256 void supp_channels_band_conversion(u8 *supp_channels) {
1257 int count = 0;
1258 int i = 1;
1259 int temp = 0;
1260
1261 count = (g_sta_info.supp_channels[0]);
1262 if (count < 2)
1263 g_sta_info.supported_band = 0;
1264
1265 while((count-1) >= 0) {
1266 temp = g_sta_info.supp_channels[i];
1267 if (temp >= 1 && temp <= 13)
1268 g_sta_info.supported_band |= BIT(0);
1269 else if (temp >= 32 && temp <= 173)
1270 g_sta_info.supported_band |= BIT(1);
1271 i += 2;
1272 count -= 2;
1273 }
1274 }
1275
fill_sta_info(struct remote_sta_info * sta_info,char * buf,size_t buf_len)1276 static int fill_sta_info(struct remote_sta_info *sta_info,
1277 char *buf, size_t buf_len)
1278 {
1279 int ret;
1280 if (sta_info->num_sta == 1) {
1281 if (sta_info->show_band)
1282 ret = snprintf(buf, buf_len,
1283 "%02x:%02x:%02x:%02x:%02x:%02x %d %d %04x %02x:%02x:%02x %d %d %d %d %d %d %d %d %d %s %d %d %d %d %d %d %d %d %d %d %d %d %d",
1284 sta_info->mac_addr[0], sta_info->mac_addr[1],
1285 sta_info->mac_addr[2], sta_info->mac_addr[3],
1286 sta_info->mac_addr[4], sta_info->mac_addr[5],
1287 sta_info->rx_retry_pkts, sta_info->rx_bcmc_pkts,
1288 sta_info->cap, sta_info->mac_addr[0],
1289 sta_info->mac_addr[1], sta_info->mac_addr[2],
1290 sta_info->freq,
1291 sta_info->bandwidth,
1292 sta_info->rssi,
1293 sta_info->data_rate,
1294 sta_info->dot11_mode,
1295 -1,
1296 -1,
1297 sta_info->reason,
1298 sta_info->supported_mode,
1299 sta_info->country,
1300 sta_info->ani_level,
1301 -1,
1302 -1,
1303 -1,
1304 sta_info->roam_trigger_reason,
1305 sta_info->roam_fail_reason,
1306 sta_info->roam_invoke_fail_reason,
1307 sta_info->tsf_out_of_sync_count,
1308 sta_info->latest_tx_power,
1309 sta_info->latest_tx_rate,
1310 sta_info->target_power_24g_1mbps,
1311 sta_info->target_power_24g_6mbps,
1312 sta_info->target_power_5g_6mbps);
1313 else
1314 ret = snprintf(buf, buf_len,
1315 "%02x:%02x:%02x:%02x:%02x:%02x %d %d %04x %02x:%02x:%02x %d %d %d %d %d %d %d %d %u %s %d %d %d %d %d %d %d %d %d %d %d %d %d",
1316 sta_info->mac_addr[0], sta_info->mac_addr[1],
1317 sta_info->mac_addr[2], sta_info->mac_addr[3],
1318 sta_info->mac_addr[4], sta_info->mac_addr[5],
1319 sta_info->rx_retry_pkts, sta_info->rx_bcmc_pkts,
1320 sta_info->cap, sta_info->mac_addr[0],
1321 sta_info->mac_addr[1], sta_info->mac_addr[2],
1322 sta_info->freq,
1323 sta_info->bandwidth,
1324 sta_info->rssi,
1325 sta_info->data_rate,
1326 sta_info->supported_mode,
1327 -1,
1328 -1,
1329 sta_info->reason,
1330 sta_info->supported_band,
1331 sta_info->country,
1332 sta_info->ani_level,
1333 -1,
1334 -1,
1335 -1,
1336 sta_info->roam_trigger_reason,
1337 sta_info->roam_fail_reason,
1338 sta_info->roam_invoke_fail_reason,
1339 sta_info->tsf_out_of_sync_count,
1340 sta_info->latest_tx_power,
1341 sta_info->latest_tx_rate,
1342 sta_info->target_power_24g_1mbps,
1343 sta_info->target_power_24g_6mbps,
1344 sta_info->target_power_5g_6mbps);
1345 } else {
1346 ret = snprintf(buf, buf_len,
1347 "%d %d %04x %d %d %d %d %d %d %d %d %d %s",
1348 sta_info->rx_retry_pkts, sta_info->rx_bcmc_pkts,
1349 -1, /* CAP */
1350 -1, /* Channel */
1351 -1, /* Bandwidth */
1352 -1, /* Rssi */
1353 -1, /* Data_rate */
1354 -1, /* 11_mode */
1355 -1,
1356 -1,
1357 -1, /* Reason */
1358 -1, /* Support_mode */
1359 sta_info->country);
1360 }
1361 return ret;
1362 }
1363
get_sta_info_legacy_handler(struct nl_msg * msg,void * arg)1364 static int get_sta_info_legacy_handler(struct nl_msg *msg, void *arg)
1365 {
1366 struct genlmsghdr *msg_hdr;
1367 struct nlattr *tb[NL80211_ATTR_MAX_INTERNAL + 1];
1368 struct nlattr *tb_vendor[NL80211_ATTR_MAX_INTERNAL + 1];
1369 struct nlattr *vendor_data, *attr_link_info;
1370 int vendor_len;
1371 struct resp_info *info = (struct resp_info *)arg;
1372 u8 *assoc_req_ie = NULL;
1373 size_t assoc_req_ie_len = 0;
1374
1375 if (!info) {
1376 wpa_printf(MSG_ERROR,"Invalid arg");
1377 return -1;
1378 }
1379
1380 wpa_printf(MSG_INFO,"Recv STA info %02x:%02x:%02x:%02x:%02x:%02x",
1381 info->mac_addr[0], info->mac_addr[1], info->mac_addr[2],
1382 info->mac_addr[3], info->mac_addr[4], info->mac_addr[5]);
1383
1384 msg_hdr = (struct genlmsghdr *)nlmsg_data(nlmsg_hdr(msg));
1385 nla_parse(tb, NL80211_ATTR_MAX_INTERNAL, genlmsg_attrdata(msg_hdr, 0),
1386 genlmsg_attrlen(msg_hdr, 0), NULL);
1387
1388 if (!tb[NL80211_ATTR_VENDOR_DATA]) {
1389 wpa_printf(MSG_ERROR,"NL80211_ATTR_VENDOR_DATA parse error");
1390 return -1;
1391 }
1392
1393 vendor_data = nla_data(tb[NL80211_ATTR_VENDOR_DATA]);
1394 vendor_len = nla_len(tb[NL80211_ATTR_VENDOR_DATA]);
1395
1396 if (nla_parse(tb_vendor, NL80211_ATTR_MAX_INTERNAL,
1397 vendor_data, vendor_len, NULL)) {
1398 wpa_printf(MSG_ERROR,"NL80211_ATTR_VENDOR_DATA not found");
1399 return -1;
1400 }
1401
1402 attr_link_info = tb_vendor[QCA_WLAN_VENDOR_ATTR_GET_STATION_LINK_INFO_ATTR];
1403 if (attr_link_info) {
1404 struct nlattr *tb_link_info[NL80211_ATTR_MAX + 1];
1405 struct nlattr *attr_survey_info, *attr_sta_info;
1406
1407 nla_parse(tb_link_info, NL80211_ATTR_MAX, nla_data(attr_link_info),
1408 nla_len(attr_link_info), NULL);
1409 attr_survey_info = tb_link_info[NL80211_ATTR_SURVEY_INFO];
1410 if (attr_survey_info) {
1411 struct nlattr *tb_survey_info[NL80211_SURVEY_INFO_MAX + 1];
1412
1413 nla_parse(tb_survey_info, NL80211_SURVEY_INFO_MAX,
1414 nla_data(attr_survey_info),
1415 nla_len(attr_survey_info), NULL);
1416 if (tb_survey_info[NL80211_SURVEY_INFO_FREQUENCY]) {
1417 g_sta_info.freq =
1418 nla_get_u32(tb_survey_info[NL80211_SURVEY_INFO_FREQUENCY]);
1419 wpa_printf(MSG_INFO,"channel %d", g_sta_info.freq);
1420 }
1421 }
1422
1423 attr_sta_info = tb_link_info[NL80211_ATTR_STA_INFO];
1424 if (attr_sta_info) {
1425 struct nlattr *tb_sta_info[NL80211_STA_INFO_MAX + 1];
1426
1427 nla_parse(tb_sta_info, NL80211_STA_INFO_MAX,
1428 nla_data(attr_sta_info),
1429 nla_len(attr_sta_info), NULL);
1430 if (tb_sta_info[NL80211_STA_INFO_SIGNAL]) {
1431 g_sta_info.rssi = nla_get_u8(tb_sta_info[NL80211_STA_INFO_SIGNAL]);
1432 g_sta_info.rssi -= NOISE_FLOOR_DBM;
1433 wpa_printf(MSG_INFO,"rssi %d", g_sta_info.rssi);
1434 }
1435 if (tb_sta_info[NL80211_STA_INFO_TX_BITRATE]) {
1436 struct nlattr *tb_antenna_info[NL80211_RATE_INFO_MAX + 1];
1437 nla_parse(tb_antenna_info, NL80211_RATE_INFO_MAX,
1438 nla_data(tb_sta_info[NL80211_STA_INFO_TX_BITRATE]),
1439 nla_len(tb_sta_info[NL80211_STA_INFO_TX_BITRATE]),
1440 NULL);
1441 }
1442 }
1443
1444 if (tb_link_info[NL80211_ATTR_REASON_CODE]) {
1445 g_sta_info.reason =
1446 nla_get_u32(tb_link_info[NL80211_ATTR_REASON_CODE]);
1447 wpa_printf(MSG_INFO,"reason %d", g_sta_info.reason);
1448 }
1449
1450 if (tb_link_info[NL80211_ATTR_STA_CAPABILITY]) {
1451 g_sta_info.cap =
1452 nla_get_u16(tb_link_info[NL80211_ATTR_STA_CAPABILITY]);
1453 wpa_printf(MSG_INFO,"cap %04x", g_sta_info.cap);
1454 }
1455 }
1456
1457 if (tb_vendor[GET_STATION_INFO_REMOTE_LAST_RX_RATE]) {
1458 g_sta_info.data_rate =
1459 nla_get_u32(tb_vendor[GET_STATION_INFO_REMOTE_LAST_RX_RATE]);
1460 wpa_printf(MSG_INFO,"data_rate %d", g_sta_info.data_rate);
1461 }
1462
1463 if (tb_vendor[GET_STATION_INFO_REMOTE_RX_RETRY_COUNT]) {
1464 g_sta_info.rx_retry_pkts +=
1465 nla_get_u32(tb_vendor[GET_STATION_INFO_REMOTE_RX_RETRY_COUNT]);
1466 wpa_printf(MSG_INFO,"rx_retry_pkts %d", g_sta_info.rx_retry_pkts);
1467 }
1468
1469 if (tb_vendor[GET_STATION_INFO_REMOTE_RX_BC_MC_COUNT]) {
1470 g_sta_info.rx_bcmc_pkts +=
1471 nla_get_u32(tb_vendor[GET_STATION_INFO_REMOTE_RX_BC_MC_COUNT]);
1472 wpa_printf(MSG_INFO,"rx_bcmc_pkts %d", g_sta_info.rx_bcmc_pkts);
1473 }
1474
1475 if (tb_vendor[GET_STATION_INFO_REMOTE_CH_WIDTH]) {
1476 g_sta_info.bandwidth =
1477 nla_get_u8(tb_vendor[GET_STATION_INFO_REMOTE_CH_WIDTH]);
1478 wpa_printf(MSG_INFO,"bandwidth %d", g_sta_info.bandwidth);
1479 }
1480
1481 if (tb_vendor[QCA_WLAN_VENDOR_ATTR_802_11_MODE]) {
1482 g_sta_info.dot11_mode =
1483 nla_get_u32(tb_vendor[QCA_WLAN_VENDOR_ATTR_802_11_MODE]);
1484 wpa_printf(MSG_INFO,"dot11_mode %d", g_sta_info.dot11_mode);
1485 }
1486
1487 if (tb_vendor[GET_STATION_INFO_REMOTE_SUPPORTED_MODE]) {
1488 g_sta_info.supported_mode =
1489 nla_get_u8(tb_vendor[GET_STATION_INFO_REMOTE_SUPPORTED_MODE]);
1490 wpa_printf(MSG_INFO,"supported_mode %d", g_sta_info.supported_mode);
1491 }
1492
1493 if (tb_vendor[GET_STATION_INFO_ASSOC_REQ_IES]) {
1494 assoc_req_ie =
1495 nla_data(tb_vendor[GET_STATION_INFO_ASSOC_REQ_IES]);
1496 assoc_req_ie_len =
1497 nla_len(tb_vendor[GET_STATION_INFO_ASSOC_REQ_IES]);
1498 }
1499
1500 parse_assoc_req_ies(assoc_req_ie, assoc_req_ie_len);
1501
1502 if (g_sta_info.supp_op_classes) {
1503 op_class_band_conversion(g_sta_info.supp_op_classes);
1504 g_sta_info.show_band = true;
1505 }
1506 else if (g_sta_info.supp_channels) {
1507 supp_channels_band_conversion(g_sta_info.supp_channels);
1508 g_sta_info.show_band = true;
1509 }
1510 else
1511 wpa_printf(MSG_ERROR,"supp_op_classes and supp_channels both are null");
1512
1513 g_sta_info.num_received_vendor_sta_info++;
1514
1515 wpa_printf(MSG_INFO,"num_received_vendor_sta_info %d",
1516 g_sta_info.num_received_vendor_sta_info);
1517
1518 return 0;
1519 }
1520
1521 static int
wpa_driver_send_get_sta_info_legacy_cmd(struct i802_bss * bss,u8 * mac,int * status)1522 wpa_driver_send_get_sta_info_legacy_cmd(struct i802_bss *bss, u8 *mac,
1523 int *status)
1524 {
1525 struct wpa_driver_nl80211_data *drv = bss->drv;
1526 struct nl_msg *nlmsg;
1527 struct nlattr *attr;
1528 struct resp_info info;
1529
1530 memset(&info, 0, sizeof(info));
1531 os_memcpy(&info.mac_addr[0], mac, MAC_ADDR_LEN);
1532 os_memcpy(&g_sta_info.mac_addr[0], mac, MAC_ADDR_LEN);
1533
1534 nlmsg = prepare_vendor_nlmsg(drv, bss->ifname,
1535 QCA_NL80211_VENDOR_SUBCMD_GET_STATION);
1536 if (!nlmsg) {
1537 wpa_printf(MSG_ERROR,"Failed to allocate nl message");
1538 return -1;
1539 }
1540
1541 attr = nla_nest_start(nlmsg, NL80211_ATTR_VENDOR_DATA);
1542 if (!attr) {
1543 nlmsg_free(nlmsg);
1544 return -1;
1545 }
1546
1547 if (nla_put(nlmsg, QCA_WLAN_VENDOR_ATTR_GET_STATION_REMOTE,
1548 MAC_ADDR_LEN, mac)) {
1549 wpa_printf(MSG_ERROR,"Failed to put QCA_WLAN_VENDOR_ATTR_GET_STATION_REMOTE");
1550 nlmsg_free(nlmsg);
1551 return -1;
1552 }
1553
1554 nla_nest_end(nlmsg, attr);
1555
1556 *status = send_nlmsg((struct nl_sock *)drv->global->nl, nlmsg,
1557 get_sta_info_legacy_handler, &info);
1558 if (*status != 0) {
1559 wpa_printf(MSG_ERROR,"Failed to send nl message with err %d", *status);
1560 return -1;
1561 }
1562
1563 return 0;
1564 }
1565
get_sta_info_handler(struct nl_msg * msg,void * arg)1566 static int get_sta_info_handler(struct nl_msg *msg, void *arg)
1567 {
1568 struct genlmsghdr *msg_hdr;
1569 struct nlattr *tb[NL80211_ATTR_MAX_INTERNAL + 1];
1570 struct nlattr *tb_vendor[GET_STA_INFO_MAX + 1];
1571 struct nlattr *vendor_data;
1572 int vendor_len;
1573 struct resp_info *info = (struct resp_info *)arg;
1574 uint8_t mac_addr[MAC_ADDR_LEN];
1575
1576 if (!info) {
1577 wpa_printf(MSG_ERROR,"Invalid arg");
1578 return -1;
1579 }
1580
1581 msg_hdr = (struct genlmsghdr *)nlmsg_data(nlmsg_hdr(msg));
1582 nla_parse(tb, NL80211_ATTR_MAX_INTERNAL, genlmsg_attrdata(msg_hdr, 0),
1583 genlmsg_attrlen(msg_hdr, 0), NULL);
1584
1585 if (!tb[NL80211_ATTR_VENDOR_DATA]) {
1586 wpa_printf(MSG_ERROR,"NL80211_ATTR_VENDOR_DATA not found");
1587 return -1;
1588 }
1589
1590 vendor_data = nla_data(tb[NL80211_ATTR_VENDOR_DATA]);
1591 vendor_len = nla_len(tb[NL80211_ATTR_VENDOR_DATA]);
1592
1593 if (nla_parse(tb_vendor, GET_STA_INFO_MAX,
1594 vendor_data, vendor_len, NULL)) {
1595 wpa_printf(MSG_ERROR,"NL80211_ATTR_VENDOR_DATA parse error");
1596 return -1;
1597 }
1598
1599 if (tb_vendor[GET_STA_INFO_MAC]) {
1600 nla_memcpy(mac_addr,
1601 tb_vendor[GET_STA_INFO_MAC],MAC_ADDR_LEN);
1602 if (os_memcmp(mac_addr, info->mac_addr, MAC_ADDR_LEN)) {
1603 wpa_printf(MSG_ERROR,"MAC address mismatch");
1604 return -1;
1605 }
1606 }
1607
1608 wpa_printf(MSG_INFO,"Recv STA info %02x:%02x:%02x:%02x:%02x:%02x",
1609 info->mac_addr[0], info->mac_addr[1], info->mac_addr[2],
1610 info->mac_addr[3], info->mac_addr[4], info->mac_addr[5]);
1611
1612 if (tb_vendor[GET_STA_INFO_RX_RETRY_COUNT]) {
1613 g_sta_info.rx_retry_pkts +=
1614 nla_get_u32(tb_vendor[GET_STA_INFO_RX_RETRY_COUNT]);
1615 wpa_printf(MSG_INFO,"rx_retry_pkts %d", g_sta_info.rx_retry_pkts);
1616 }
1617
1618 if (tb_vendor[GET_STA_INFO_RX_BC_MC_COUNT]) {
1619 g_sta_info.rx_bcmc_pkts +=
1620 nla_get_u32(tb_vendor[GET_STA_INFO_RX_BC_MC_COUNT]);
1621 wpa_printf(MSG_INFO,"rx_bcmc_pkts %d", g_sta_info.rx_bcmc_pkts);
1622 }
1623
1624 if (tb_vendor[GET_STA_INFO_TX_RETRY_SUCCEED]) {
1625 g_sta_info.tx_pkts_retried +=
1626 nla_get_u32(tb_vendor[GET_STA_INFO_TX_RETRY_SUCCEED]);
1627 wpa_printf(MSG_INFO,"tx_pkts_retried %d", g_sta_info.tx_pkts_retried);
1628 }
1629
1630 if (tb_vendor[GET_STA_INFO_TX_RETRY_EXHAUSTED]) {
1631 g_sta_info.tx_pkts_retry_exhausted +=
1632 nla_get_u32(tb_vendor[GET_STA_INFO_TX_RETRY_EXHAUSTED]);
1633 wpa_printf(MSG_INFO,"tx_pkts_retry_exhausted %d", g_sta_info.tx_pkts_retry_exhausted);
1634 }
1635
1636 if (tb_vendor[GET_STA_INFO_TARGET_TX_TOTAL]) {
1637 g_sta_info.tx_pkts_fw_total +=
1638 nla_get_u32(tb_vendor[GET_STA_INFO_TARGET_TX_TOTAL]);
1639 wpa_printf(MSG_INFO,"tx_pkts_fw_total %d", g_sta_info.tx_pkts_fw_total);
1640 }
1641
1642 if (tb_vendor[GET_STA_INFO_TARGET_TX_RETRY]) {
1643 g_sta_info.tx_pkts_fw_retries +=
1644 nla_get_u32(tb_vendor[GET_STA_INFO_TARGET_TX_RETRY]);
1645 wpa_printf(MSG_INFO,"tx_pkts_fw_retries %d", g_sta_info.tx_pkts_fw_retries);
1646 }
1647
1648 if (tb_vendor[GET_STA_INFO_TARGET_TX_RETRY_EXHAUSTED]) {
1649 g_sta_info.tx_pkts_fw_retry_exhausted +=
1650 nla_get_u32(tb_vendor[GET_STA_INFO_TARGET_TX_RETRY_EXHAUSTED]);
1651 wpa_printf(MSG_INFO,"tx_pkts_fw_retry_exhausted %d", g_sta_info.tx_pkts_fw_retry_exhausted);
1652 }
1653
1654 if (tb_vendor[GET_STA_INFO_ANI_LEVEL]) {
1655 g_sta_info.ani_level =
1656 nla_get_u32(tb_vendor[GET_STA_INFO_ANI_LEVEL]);
1657 wpa_printf(MSG_INFO,"ani_level %d", g_sta_info.ani_level);
1658 }
1659
1660 if (tb_vendor[GET_STA_INFO_ROAM_TRIGGER_REASON]) {
1661 g_sta_info.roam_trigger_reason =
1662 nla_get_u32(tb_vendor[GET_STA_INFO_ROAM_TRIGGER_REASON]);
1663 wpa_printf(MSG_INFO,"roam_trigger_reason %d", g_sta_info.roam_trigger_reason);
1664 }
1665
1666 if (tb_vendor[GET_STA_INFO_ROAM_FAIL_REASON]) {
1667 g_sta_info.roam_fail_reason =
1668 nla_get_u32(tb_vendor[GET_STA_INFO_ROAM_FAIL_REASON]);
1669 wpa_printf(MSG_INFO,"roam_fail_reason %d", g_sta_info.roam_fail_reason);
1670 }
1671
1672 if (tb_vendor[GET_STA_INFO_ROAM_INVOKE_FAIL_REASON]) {
1673 g_sta_info.roam_invoke_fail_reason =
1674 nla_get_u32(tb_vendor[GET_STA_INFO_ROAM_INVOKE_FAIL_REASON]);
1675 wpa_printf(MSG_INFO,"roam_invoke_fail_reason %d", g_sta_info.roam_invoke_fail_reason);
1676 }
1677
1678 if (tb_vendor[GET_STA_INFO_TSF_OUT_OF_SYNC_COUNT]) {
1679 g_sta_info.tsf_out_of_sync_count =
1680 nla_get_u32(tb_vendor[GET_STA_INFO_TSF_OUT_OF_SYNC_COUNT]);
1681 wpa_printf(MSG_INFO,"tsf_out_of_sync_count %d", g_sta_info.tsf_out_of_sync_count);
1682 }
1683
1684 if (tb_vendor[GET_STA_INFO_LATEST_TX_POWER]) {
1685 g_sta_info.latest_tx_power =
1686 nla_get_u32(tb_vendor[GET_STA_INFO_LATEST_TX_POWER]);
1687 wpa_printf(MSG_INFO,"latest_tx_power %d", g_sta_info.latest_tx_power);
1688 }
1689
1690 if (tb_vendor[GET_STA_INFO_LATEST_TX_RATE]) {
1691 g_sta_info.latest_tx_rate =
1692 nla_get_u32(tb_vendor[GET_STA_INFO_LATEST_TX_RATE]);
1693 wpa_printf(MSG_INFO,"latest_tx_rate %d", g_sta_info.latest_tx_rate);
1694 }
1695
1696 if (tb_vendor[GET_STA_INFO_TARGET_POWER_24G_1MBPS]) {
1697 g_sta_info.target_power_24g_1mbps =
1698 nla_get_u32(tb_vendor[GET_STA_INFO_TARGET_POWER_24G_1MBPS]);
1699 wpa_printf(MSG_INFO,"target_power_24g_1mbps %d", g_sta_info.target_power_24g_1mbps);
1700 }
1701
1702 if (tb_vendor[GET_STA_INFO_TARGET_POWER_24G_6MBPS]) {
1703 g_sta_info.target_power_24g_6mbps =
1704 nla_get_u32(tb_vendor[GET_STA_INFO_TARGET_POWER_24G_6MBPS]);
1705 wpa_printf(MSG_INFO,"target_power_24g_6mbps %d", g_sta_info.target_power_24g_6mbps);
1706 }
1707
1708 if (tb_vendor[GET_STA_INFO_TARGET_POWER_5G_6MBPS]) {
1709 g_sta_info.target_power_5g_6mbps =
1710 nla_get_u32(tb_vendor[GET_STA_INFO_TARGET_POWER_5G_6MBPS]);
1711 wpa_printf(MSG_INFO,"target_power_5g_6mbps %d", g_sta_info.target_power_5g_6mbps);
1712 }
1713
1714 if (tb_vendor[GET_STA_INFO_LATEST_RIX]) {
1715 g_sta_info.latest_rix =
1716 nla_get_u32(tb_vendor[GET_STA_INFO_LATEST_RIX]);
1717 wpa_printf(MSG_INFO,"latest_rix %d", g_sta_info.latest_rix);
1718 }
1719
1720
1721 g_sta_info.num_received_vendor_sta_info++;
1722
1723 wpa_printf(MSG_INFO,"num_received_vendor_sta_info %d",
1724 g_sta_info.num_received_vendor_sta_info);
1725
1726 return 0;
1727 }
1728
wpa_driver_ioctl(struct i802_bss * bss,char * cmd,char * buf,size_t buf_len,int * status,struct wpa_driver_nl80211_data * drv)1729 static int wpa_driver_ioctl(struct i802_bss *bss, char *cmd,
1730 char *buf, size_t buf_len, int *status,
1731 struct wpa_driver_nl80211_data *drv) {
1732 struct ifreq ifr;
1733 android_wifi_priv_cmd priv_cmd;
1734 memset(&ifr, 0, sizeof(ifr));
1735 memset(&priv_cmd, 0, sizeof(priv_cmd));
1736 os_memcpy(buf, cmd, strlen(cmd) + 1);
1737 os_strlcpy(ifr.ifr_name, bss->ifname, IFNAMSIZ);
1738 priv_cmd.buf = buf;
1739 priv_cmd.used_len = buf_len;
1740 priv_cmd.total_len = buf_len;
1741 ifr.ifr_data = &priv_cmd;
1742
1743 if ((ioctl(drv->global->ioctl_sock, SIOCDEVPRIVATE + 1, &ifr)) < 0) {
1744 wpa_printf(MSG_ERROR,"%s: failed to issue private commands\n", __func__);
1745 *status = 1;
1746 return WPA_DRIVER_OEM_STATUS_FAILURE;
1747 } else {
1748 wpa_printf(MSG_ERROR,"Response: %s", buf);
1749 return WPA_DRIVER_OEM_STATUS_SUCCESS;
1750 }
1751 }
1752
wpa_driver_send_get_sta_info_cmd(struct i802_bss * bss,u8 * mac,int * status,bool * new_cmd)1753 static int wpa_driver_send_get_sta_info_cmd(struct i802_bss *bss, u8 *mac,
1754 int *status, bool *new_cmd)
1755 {
1756 struct wpa_driver_nl80211_data *drv = bss->drv;
1757 struct nl_msg *nlmsg;
1758 struct nlattr *attr;
1759 struct resp_info info;
1760
1761 memset(&info, 0, sizeof(info));
1762 os_memcpy(&info.mac_addr[0], mac, MAC_ADDR_LEN);
1763 os_memcpy(&g_sta_info.mac_addr[0], mac, MAC_ADDR_LEN);
1764
1765 *new_cmd = true;
1766
1767
1768 nlmsg = prepare_vendor_nlmsg(drv, bss->ifname,
1769 QCA_NL80211_VENDOR_SUBCMD_GET_STA_INFO);
1770 if (!nlmsg) {
1771 wpa_printf(MSG_ERROR,"Failed to allocate nl message");
1772 return -1;
1773 }
1774
1775 attr = nla_nest_start(nlmsg, NL80211_ATTR_VENDOR_DATA);
1776 if (!attr) {
1777 nlmsg_free(nlmsg);
1778 return -1;
1779 }
1780
1781 if (nla_put(nlmsg, GET_STA_INFO_MAC,
1782 MAC_ADDR_LEN, mac)) {
1783 wpa_printf(MSG_ERROR,"Failed to put GET_STA_INFO_MAC");
1784 nlmsg_free(nlmsg);
1785 return -1;
1786 }
1787
1788 nla_nest_end(nlmsg, attr);
1789
1790 *status = send_nlmsg((struct nl_sock *)drv->global->nl, nlmsg,
1791 get_sta_info_handler, &info);
1792 if (*status != 0) {
1793 wpa_printf(MSG_ERROR,"Failed to send nl message with err %d, retrying with legacy command", *status);
1794 *new_cmd = false;
1795 return wpa_driver_send_get_sta_info_legacy_cmd(bss,mac,status);
1796 }
1797
1798 g_sta_info.num_request_vendor_sta_info++;
1799 wpa_printf(MSG_INFO,"num_request_vendor_sta_info %d",
1800 g_sta_info.num_request_vendor_sta_info);
1801
1802 return 0;
1803 }
1804
get_station_handler(struct nl_msg * msg,void * arg)1805 static int get_station_handler(struct nl_msg *msg, void *arg)
1806 {
1807 struct genlmsghdr *msg_hdr;
1808 struct nlattr *tb[NL80211_ATTR_MAX + 1];
1809 struct nlattr *tb_sinfo[NL80211_STA_INFO_MAX + 1];
1810 struct nlattr *tb_rate[NL80211_RATE_INFO_MAX + 1];
1811 struct nlattr *sinfo_data, *attr;
1812 int sinfo_len, tmp, num_chain = 0;
1813 struct resp_info *info = (struct resp_info *)arg;
1814 uint8_t mac_addr[MAC_ADDR_LEN];
1815
1816 if (!info) {
1817 wpa_printf(MSG_ERROR,"Invalid arg");
1818 return -1;
1819 }
1820
1821 msg_hdr = (struct genlmsghdr *)nlmsg_data(nlmsg_hdr(msg));
1822 nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(msg_hdr, 0),
1823 genlmsg_attrlen(msg_hdr, 0), NULL);
1824
1825 if (tb[NL80211_ATTR_MAC]) {
1826 nla_memcpy(mac_addr, tb[NL80211_ATTR_MAC], MAC_ADDR_LEN);
1827 if (os_memcmp(mac_addr, info->mac_addr, MAC_ADDR_LEN)) {
1828 wpa_printf(MSG_ERROR,"MAC address mismatch");
1829 return -1;
1830 }
1831 }
1832
1833 wpa_printf(MSG_INFO,"Recv STA info %02x:%02x:%02x:%02x:%02x:%02x",
1834 info->mac_addr[0], info->mac_addr[1], info->mac_addr[2],
1835 info->mac_addr[3], info->mac_addr[4], info->mac_addr[5]);
1836
1837 if (!tb[NL80211_ATTR_STA_INFO]) {
1838 wpa_printf(MSG_ERROR,"NL80211_ATTR_STA_INFO not found");
1839 return -1;
1840 }
1841
1842 sinfo_data = nla_data(tb[NL80211_ATTR_STA_INFO]);
1843 sinfo_len = nla_len(tb[NL80211_ATTR_STA_INFO]);
1844
1845 if (nla_parse(tb_sinfo, NL80211_STA_INFO_MAX, sinfo_data, sinfo_len,
1846 NULL)) {
1847 wpa_printf(MSG_ERROR,"NL80211_ATTR_STA_INFO parse error");
1848 return -1;
1849 }
1850
1851 /* No need to read for summary */
1852 if (g_sta_info.num_sta == 1) {
1853 if (tb[NL80211_ATTR_IE])
1854 parse_assoc_req_ies(nla_data(tb[NL80211_ATTR_IE]),
1855 nla_len(tb[NL80211_ATTR_IE]));
1856
1857 attr = tb_sinfo[NL80211_STA_INFO_TX_BITRATE];
1858 if (attr) {
1859 nla_parse(tb_rate, NL80211_RATE_INFO_MAX,
1860 nla_data(attr), nla_len(attr), NULL);
1861
1862 if (tb_rate[NL80211_RATE_INFO_BITRATE32]) {
1863 g_sta_info.tx_rate =
1864 nla_get_u32(tb_rate[NL80211_RATE_INFO_BITRATE32]);
1865 wpa_printf(MSG_INFO,"tx_rate %d", g_sta_info.tx_rate);
1866 }
1867
1868 if (tb_rate[NL80211_RATE_INFO_160_MHZ_WIDTH])
1869 g_sta_info.bandwidth = QCA_VENDOR_WLAN_CHAN_WIDTH_160;
1870 else if (tb_rate[NL80211_RATE_INFO_80P80_MHZ_WIDTH])
1871 g_sta_info.bandwidth = QCA_VENDOR_WLAN_CHAN_WIDTH_80_80;
1872 else if (tb_rate[NL80211_RATE_INFO_80_MHZ_WIDTH])
1873 g_sta_info.bandwidth = QCA_VENDOR_WLAN_CHAN_WIDTH_80;
1874 else if (tb_rate[NL80211_RATE_INFO_40_MHZ_WIDTH])
1875 g_sta_info.bandwidth = QCA_VENDOR_WLAN_CHAN_WIDTH_40;
1876 else
1877 g_sta_info.bandwidth = QCA_VENDOR_WLAN_CHAN_WIDTH_20;
1878 wpa_printf(MSG_INFO,"bandwidth %d", g_sta_info.bandwidth);
1879 }
1880
1881 attr = tb_sinfo[NL80211_STA_INFO_RX_BITRATE];
1882 if (attr) {
1883 nla_parse(tb_rate, NL80211_RATE_INFO_MAX,
1884 nla_data(attr), nla_len(attr), NULL);
1885 if (tb_rate[NL80211_RATE_INFO_BITRATE32]) {
1886 g_sta_info.data_rate =
1887 nla_get_u32(tb_rate[NL80211_RATE_INFO_BITRATE32]);
1888 wpa_printf(MSG_INFO,"data_rate %d", g_sta_info.data_rate);
1889 }
1890 }
1891
1892 if (tb_sinfo[NL80211_STA_INFO_SIGNAL_AVG]) {
1893 g_sta_info.rssi =
1894 nla_get_u8(tb_sinfo[NL80211_STA_INFO_SIGNAL_AVG]);
1895 g_sta_info.rssi -= NOISE_FLOOR_DBM;
1896 wpa_printf(MSG_INFO,"rssi %d", g_sta_info.rssi);
1897 }
1898
1899 if (tb_sinfo[NL80211_STA_INFO_SIGNAL]) {
1900 g_sta_info.rx_lastpkt_rssi =
1901 nla_get_u8(tb_sinfo[NL80211_STA_INFO_SIGNAL]);
1902 g_sta_info.rx_lastpkt_rssi -= NOISE_FLOOR_DBM;
1903 wpa_printf(MSG_INFO,"rx_lastpkt_rssi %d", g_sta_info.rx_lastpkt_rssi);
1904 }
1905
1906 if (tb_sinfo[NL80211_STA_INFO_CHAIN_SIGNAL_AVG]) {
1907 nla_for_each_nested(attr,
1908 tb_sinfo[NL80211_STA_INFO_CHAIN_SIGNAL_AVG],
1909 tmp) {
1910 if (num_chain >= WMI_MAX_CHAINS) {
1911 wpa_printf(MSG_ERROR,"WMI_MAX_CHAINS reached");
1912 break;
1913 }
1914 g_sta_info.avg_rssi_per_chain[num_chain] = nla_get_u8(attr);
1915 g_sta_info.avg_rssi_per_chain[num_chain] -= NOISE_FLOOR_DBM;
1916 wpa_printf(MSG_INFO,"avg_rssi_per_chain[%d] %d", num_chain,
1917 g_sta_info.avg_rssi_per_chain[num_chain]);
1918 num_chain++;
1919 }
1920 }
1921 }
1922
1923 if (tb_sinfo[NL80211_STA_INFO_TX_PACKETS]) {
1924 g_sta_info.tx_pkts_total +=
1925 nla_get_u32(tb_sinfo[NL80211_STA_INFO_TX_PACKETS]);
1926 g_sta_info.tx_pckts +=
1927 nla_get_u32(tb_sinfo[NL80211_STA_INFO_TX_PACKETS]);
1928 wpa_printf(MSG_INFO,"tx_pkts_total %d", g_sta_info.tx_pkts_total);
1929 wpa_printf(MSG_INFO,"tx_pckts %d", g_sta_info.tx_pckts);
1930 }
1931
1932 if (tb_sinfo[NL80211_STA_INFO_TX_FAILED]) {
1933 g_sta_info.tx_failures +=
1934 nla_get_u32(tb_sinfo[NL80211_STA_INFO_TX_FAILED]);
1935 wpa_printf(MSG_INFO,"tx_failures %d", g_sta_info.tx_failures);
1936 }
1937
1938 if (tb_sinfo[NL80211_STA_INFO_TX_RETRIES]) {
1939 g_sta_info.tx_pkts_retries +=
1940 nla_get_u32(tb_sinfo[NL80211_STA_INFO_TX_RETRIES]);
1941 wpa_printf(MSG_INFO,"tx_pkts_retries %d", g_sta_info.tx_pkts_retries);
1942 }
1943
1944
1945 g_sta_info.num_received_nl80211_sta_info++;
1946
1947 wpa_printf(MSG_INFO,"num_received_nl80211_sta_info %d",
1948 g_sta_info.num_received_nl80211_sta_info);
1949
1950 return 0;
1951 }
1952
wpa_driver_send_get_station_cmd(struct i802_bss * bss,u8 * mac,int * status)1953 static int wpa_driver_send_get_station_cmd(struct i802_bss *bss, u8 *mac,
1954 int *status)
1955 {
1956 struct wpa_driver_nl80211_data *drv = bss->drv;
1957 struct nl_msg *nlmsg;
1958 struct resp_info info;
1959
1960 memset(&info, 0, sizeof(info));
1961 os_memcpy(&info.mac_addr[0], mac, MAC_ADDR_LEN);
1962 os_memcpy(&g_sta_info.mac_addr[0], mac, MAC_ADDR_LEN);
1963
1964 nlmsg = prepare_nlmsg(drv, bss->ifname, NL80211_CMD_GET_STATION, 0, 0);
1965 if (!nlmsg) {
1966 wpa_printf(MSG_ERROR,"Failed to allocate nl message");
1967 return -1;
1968 }
1969
1970 if (nla_put(nlmsg, NL80211_ATTR_MAC, MAC_ADDR_LEN, mac)) {
1971 wpa_printf(MSG_ERROR,"Failed to put NL80211_ATTR_MAC");
1972 nlmsg_free(nlmsg);
1973 return -1;
1974 }
1975
1976 *status = send_nlmsg((struct nl_sock *)drv->global->nl, nlmsg,
1977 get_station_handler, &info);
1978 if (*status != 0) {
1979 wpa_printf(MSG_ERROR,"Failed to send nl message with err %d", *status);
1980 return -1;
1981 }
1982
1983 g_sta_info.num_request_nl80211_sta_info++;
1984 wpa_printf(MSG_INFO,"num_request_nl80211_sta_info %d",
1985 g_sta_info.num_request_nl80211_sta_info);
1986
1987 return 0;
1988 }
1989
wpa_driver_get_sta_info(struct i802_bss * bss,u8 * mac,int * status)1990 static int wpa_driver_get_sta_info(struct i802_bss *bss, u8 *mac,
1991 int *status)
1992 {
1993 struct wpa_driver_nl80211_data *drv = bss->drv;
1994 struct hostapd_data *hapd = bss->ctx;
1995 struct sta_info *iter, *sta = NULL;
1996 bool new_cmd;
1997 int ret;
1998 char buf[MAX_DRV_CMD_SIZE];
1999 char *p;
2000
2001 memset(buf, 0, sizeof(buf));
2002
2003 ret = wpa_driver_send_get_sta_info_cmd(bss, mac, status, &new_cmd);
2004 if (ret < 0)
2005 return ret;
2006
2007 if (new_cmd) {
2008 ret = wpa_driver_send_get_station_cmd(bss, mac, status);
2009 if (ret < 0)
2010 return ret;
2011
2012 /* No need to read for summary */
2013 if (g_sta_info.num_sta == 1) {
2014 if (!hapd) {
2015 wpa_printf(MSG_ERROR,"hapd is NULL");
2016 return -1;
2017 }
2018 iter = hapd->sta_list;
2019 while (iter) {
2020 if (os_memcmp(mac, iter->addr, MAC_ADDR_LEN) == 0) {
2021 sta = iter;
2022 break;
2023 }
2024 iter = iter->next;
2025 }
2026 if (!sta) {
2027 wpa_printf(MSG_ERROR,"STA is not found");
2028 return -1;
2029 }
2030
2031 g_sta_info.cap = sta->capability;
2032 wpa_printf(MSG_INFO,"cap %04x", g_sta_info.cap);
2033 g_sta_info.freq = (u32)hapd->iface->freq;
2034 wpa_printf(MSG_INFO,"freq %d", g_sta_info.freq);
2035
2036 if (g_sta_info.flags.he_supported) {
2037 g_sta_info.dot11_mode = QCA_VENDOR_WLAN_802_11_MODE_AX;
2038 g_sta_info.supported_mode = QCA_VENDOR_WLAN_PHY_MODE_HE;
2039 } else if (g_sta_info.flags.vht_supported) {
2040 g_sta_info.dot11_mode = QCA_VENDOR_WLAN_802_11_MODE_AC;
2041 g_sta_info.supported_mode = QCA_VENDOR_WLAN_PHY_MODE_VHT;
2042 } else if (g_sta_info.flags.ht_supported) {
2043 g_sta_info.dot11_mode = QCA_VENDOR_WLAN_802_11_MODE_N;
2044 g_sta_info.supported_mode = QCA_VENDOR_WLAN_PHY_MODE_HT;
2045 } else {
2046 if (g_sta_info.freq < 4900) {
2047 if (hapd->iconf->hw_mode == HOSTAPD_MODE_IEEE80211B)
2048 g_sta_info.dot11_mode =
2049 QCA_VENDOR_WLAN_802_11_MODE_B;
2050 else
2051 g_sta_info.dot11_mode =
2052 QCA_VENDOR_WLAN_802_11_MODE_G;
2053 } else {
2054 g_sta_info.dot11_mode = QCA_VENDOR_WLAN_802_11_MODE_A;
2055 }
2056 g_sta_info.supported_mode = QCA_VENDOR_WLAN_PHY_MODE_LEGACY;
2057 }
2058
2059 wpa_printf(MSG_INFO,"dot11_mode %d", g_sta_info.dot11_mode);
2060 wpa_printf(MSG_INFO,"supported_mode %d", g_sta_info.supported_mode);
2061 }
2062 }
2063
2064 if(wpa_driver_ioctl(bss, "GETCOUNTRYREV", buf, sizeof(buf), &status, drv) == 0){
2065 p = strstr(buf, " ");
2066 if(p != NULL)
2067 memcpy(g_sta_info.country, (p+1), strlen(p+1)+1);//length of p including null
2068 }else{
2069 }
2070
2071 wpa_printf(MSG_INFO,"STA information completed");
2072
2073 return 0;
2074 }
2075
wpa_driver_get_all_sta_info(struct i802_bss * bss,int * status)2076 static int wpa_driver_get_all_sta_info(struct i802_bss *bss, int *status)
2077 {
2078 struct hostapd_data *hapd = bss->ctx;
2079 struct sta_info *sta;
2080 int ret, total_ret = 0;
2081
2082 if(bss->drv && bss->drv->nlmode != NL80211_IFTYPE_AP) {
2083 wpa_printf(MSG_ERROR,"Not a hapd interface");
2084 return -1;
2085 }
2086
2087 if (!hapd) {
2088 wpa_printf(MSG_ERROR,"hapd is NULL");
2089 return -1;
2090 }
2091
2092 g_sta_info.num_sta = hapd->num_sta;
2093
2094 sta = hapd->sta_list;
2095 while (sta) {
2096 ret = wpa_driver_get_sta_info(bss, sta->addr, status);
2097 if (ret < 0)
2098 return ret;
2099 sta = sta->next;
2100 total_ret += ret;
2101 }
2102
2103 wpa_printf(MSG_INFO,"All STAs information completed");
2104
2105 return total_ret;
2106 }
2107
wpa_driver_handle_get_sta_info(struct i802_bss * bss,char * cmd,char * buf,size_t buf_len,int * status)2108 static int wpa_driver_handle_get_sta_info(struct i802_bss *bss, char *cmd,
2109 char *buf, size_t buf_len,
2110 int *status)
2111 {
2112 u8 mac[MAC_ADDR_LEN];
2113 int ret;
2114
2115 os_memset(&g_sta_info, 0, sizeof(g_sta_info));
2116
2117 cmd = skip_white_space(cmd);
2118 if (strlen(cmd) >= MAC_ADDR_LEN * 2 + MAC_ADDR_LEN - 1
2119 && convert_string_to_bytes(mac, cmd, MAC_ADDR_LEN) > 0) {
2120 g_sta_info.num_sta = 1;
2121 ret = wpa_driver_get_sta_info(bss, mac, status);
2122 if (ret < 0)
2123 return ret;
2124 } else {
2125 ret = wpa_driver_get_all_sta_info(bss, status);
2126 if (ret < 0)
2127 return ret;
2128 }
2129
2130 if (ret == 0) {
2131 ret = fill_sta_info(&g_sta_info, buf, buf_len);
2132 wpa_printf(MSG_INFO,"%s", buf);
2133 } else {
2134 wpa_printf(MSG_ERROR,"Failed to get STA info, num_sta %d vendor_sent %d vendor_recv %d nl80211_send %d nl80211 recv %d",
2135 g_sta_info.num_sta,
2136 g_sta_info.num_request_vendor_sta_info,
2137 g_sta_info.num_received_vendor_sta_info,
2138 g_sta_info.num_request_nl80211_sta_info,
2139 g_sta_info.num_received_nl80211_sta_info);
2140 wpa_printf(MSG_ERROR,"GETSTAINFO failed");
2141 }
2142
2143 return ret;
2144 }
2145
thermal_info_handler(struct nl_msg * msg,void * arg)2146 static int thermal_info_handler(struct nl_msg *msg, void *arg)
2147 {
2148 struct nlattr *tb[NL80211_ATTR_MAX + 1];
2149 struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
2150 int *param = arg;
2151 struct nlattr *nl_vendor;
2152 struct nlattr *tb_vendor[QCA_WLAN_VENDOR_ATTR_MAX + 1];
2153
2154 nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
2155 genlmsg_attrlen(gnlh, 0), NULL);
2156
2157 nl_vendor = tb[NL80211_ATTR_VENDOR_DATA];
2158 if (!nl_vendor || nla_parse(tb_vendor, QCA_WLAN_VENDOR_ATTR_MAX,
2159 nla_data(nl_vendor), nla_len(nl_vendor), NULL)) {
2160 wpa_printf(MSG_ERROR, "%s: No vendor data found", __func__);
2161 return NL_SKIP;
2162 }
2163
2164 if (tb_vendor[QCA_WLAN_VENDOR_ATTR_THERMAL_GET_TEMPERATURE_DATA])
2165 *param = (int) nla_get_u32(
2166 tb_vendor[QCA_WLAN_VENDOR_ATTR_THERMAL_GET_TEMPERATURE_DATA]);
2167 else if (tb_vendor[QCA_WLAN_VENDOR_ATTR_THERMAL_LEVEL])
2168 *param = (int) nla_get_u32(
2169 tb_vendor[QCA_WLAN_VENDOR_ATTR_THERMAL_LEVEL]);
2170 else
2171 wpa_printf(MSG_ERROR, "%s: failed to parse data", __func__);
2172
2173 return NL_SKIP;
2174 }
2175
wpa_driver_cmd_get_thermal_info(struct i802_bss * bss,int * result,int attr)2176 static int wpa_driver_cmd_get_thermal_info(struct i802_bss *bss, int *result, int attr)
2177 {
2178 struct wpa_driver_nl80211_data *drv = bss->drv;
2179 struct nl_msg *msg = NULL;
2180 struct nlattr *params = NULL;
2181 int ret = 0;
2182
2183 if (!(msg = nl80211_drv_msg(drv, 0, NL80211_CMD_VENDOR)) ||
2184 nla_put_u32(msg, NL80211_ATTR_VENDOR_ID, OUI_QCA) ||
2185 nla_put_u32(msg, NL80211_ATTR_VENDOR_SUBCMD,
2186 QCA_NL80211_VENDOR_SUBCMD_THERMAL_CMD) ||
2187 !(params = nla_nest_start(msg, NL80211_ATTR_VENDOR_DATA)) ||
2188 nla_put_u32(msg, QCA_WLAN_VENDOR_ATTR_THERMAL_CMD_VALUE, attr)) {
2189 nlmsg_free(msg);
2190 return -1;
2191 }
2192
2193 nla_nest_end(msg, params);
2194 ret = send_and_recv_msgs(drv, msg, thermal_info_handler, result, NULL, NULL);
2195 if (!ret)
2196 return 0;
2197 wpa_printf(MSG_ERROR, "%s: Failed get thermal info, ret=%d(%s)",
2198 __func__, ret, strerror(-ret));
2199 return ret;
2200 }
2201
get_scan_handler(struct nl_msg * msg,void * arg)2202 static int get_scan_handler(struct nl_msg *msg, void *arg)
2203 {
2204 struct genlmsghdr *msg_hdr;
2205 struct nlattr *attr[NL80211_ATTR_MAX + 1];
2206 struct resp_info *info = (struct resp_info *)arg;
2207 struct nlattr *bss_attr[NL80211_BSS_MAX + 1];
2208 char *bssid;
2209 static struct nla_policy get_scan_policy[NL80211_BSS_MAX + 1] = {
2210 [NL80211_BSS_BSSID] = {},
2211 [NL80211_BSS_FREQUENCY] = { .type = NLA_U32 },
2212 [NL80211_BSS_STATUS] = { .type = NLA_U32 },
2213 [NL80211_BSS_CHAN_WIDTH] = { .type = NLA_U32 },
2214 };
2215
2216 if (!info) {
2217 wpa_printf(MSG_DEBUG, "resp_info is NULL");
2218 return NL_SKIP;
2219 }
2220
2221 msg_hdr = (struct genlmsghdr *)nlmsg_data(nlmsg_hdr(msg));
2222 nla_parse(attr, NL80211_ATTR_MAX, genlmsg_attrdata(msg_hdr, 0),
2223 genlmsg_attrlen(msg_hdr, 0), NULL);
2224
2225 if (!attr[NL80211_ATTR_BSS]) {
2226 wpa_printf(MSG_DEBUG, "no bss info");
2227 return NL_SKIP;
2228 }
2229
2230 if (nla_parse_nested(bss_attr, NL80211_BSS_MAX,
2231 attr[NL80211_ATTR_BSS],
2232 get_scan_policy)) {
2233 wpa_printf(MSG_DEBUG, "parse bss attr fail");
2234 return NL_SKIP;
2235 }
2236
2237 if (!bss_attr[NL80211_BSS_BSSID])
2238 return NL_SKIP;
2239
2240 if (!bss_attr[NL80211_BSS_STATUS])
2241 return NL_SKIP;
2242
2243 if (nla_get_u32(bss_attr[NL80211_BSS_STATUS]) !=
2244 NL80211_BSS_STATUS_ASSOCIATED)
2245 return NL_SKIP;
2246
2247 bssid = nla_data(bss_attr[NL80211_BSS_BSSID]);
2248 os_memcpy(g_csi_param.connected_bssid, bssid, MAC_ADDR_LEN);
2249
2250 wpa_printf(MSG_DEBUG, "get connected bss");
2251 if (bss_attr[NL80211_BSS_FREQUENCY])
2252 wpa_printf(MSG_DEBUG, "freq %d", nla_get_u32(bss_attr[NL80211_BSS_FREQUENCY]));
2253
2254 if (bss_attr[NL80211_BSS_CHAN_WIDTH])
2255 wpa_printf(MSG_DEBUG, "BW %d", nla_get_u32(bss_attr[NL80211_BSS_CHAN_WIDTH]));
2256
2257 return 0;
2258 }
2259
wpa_driver_send_get_scan_cmd(struct i802_bss * bss,int * status)2260 static int wpa_driver_send_get_scan_cmd(struct i802_bss *bss, int *status)
2261 {
2262 struct wpa_driver_nl80211_data *drv = bss->drv;
2263 struct nl_msg *nlmsg;
2264 struct resp_info info;
2265
2266 os_memset(g_csi_param.connected_bssid, 0xff, MAC_ADDR_LEN);
2267 nlmsg = prepare_nlmsg(drv, bss->ifname, NL80211_CMD_GET_SCAN, 0, NLM_F_DUMP);
2268 if (!nlmsg) {
2269 wpa_printf(MSG_ERROR, "Failed to allocate nl message");
2270 return WPA_DRIVER_OEM_STATUS_FAILURE;
2271 }
2272
2273 *status = send_nlmsg((struct nl_sock *)drv->global->nl, nlmsg,
2274 get_scan_handler, &info);
2275 if (*status != 0) {
2276 wpa_printf(MSG_ERROR, "Failed to send nl message with err %d", *status);
2277 return WPA_DRIVER_OEM_STATUS_FAILURE;
2278 }
2279
2280 return WPA_DRIVER_OEM_STATUS_SUCCESS;
2281 }
2282
wpa_driver_start_csi_capture(struct i802_bss * bss,int * status,int transport_mode)2283 static int wpa_driver_start_csi_capture(struct i802_bss *bss, int *status,
2284 int transport_mode)
2285 {
2286 struct wpa_driver_nl80211_data *drv = bss->drv;
2287 struct nl_msg *nlmsg;
2288 struct nlattr *attr, *attr_table, *attr_entry;
2289 char ta_mask[MAC_ADDR_LEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
2290
2291 nlmsg = prepare_vendor_nlmsg(drv, bss->ifname,
2292 QCA_NL80211_VENDOR_SUBCMD_PEER_CFR_CAPTURE_CFG);
2293 if (!nlmsg) {
2294 wpa_printf(MSG_ERROR, "Failed to allocate nl message");
2295 return WPA_DRIVER_OEM_STATUS_FAILURE;
2296 }
2297
2298 attr = nla_nest_start(nlmsg, NL80211_ATTR_VENDOR_DATA);
2299 if (!attr) {
2300 nlmsg_free(nlmsg);
2301 return WPA_DRIVER_OEM_STATUS_FAILURE;
2302 }
2303
2304 if (nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_PEER_CFR_VERSION,
2305 ENHANCED_CFR_VER)) {
2306 wpa_printf(MSG_ERROR, "Failed to csi version");
2307 nlmsg_free(nlmsg);
2308 return WPA_DRIVER_OEM_STATUS_FAILURE;
2309 }
2310
2311 if (nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_PEER_CFR_DATA_TRANSPORT_MODE,
2312 transport_mode)) {
2313 wpa_printf(MSG_ERROR, "Failed to set transport mode");
2314 nlmsg_free(nlmsg);
2315 return WPA_DRIVER_OEM_STATUS_FAILURE;
2316 }
2317
2318 if (nla_put_flag(nlmsg, QCA_WLAN_VENDOR_ATTR_PEER_CFR_ENABLE)) {
2319 wpa_printf(MSG_ERROR, "Failed to csi enable flag");
2320 nlmsg_free(nlmsg);
2321 return WPA_DRIVER_OEM_STATUS_FAILURE;
2322 }
2323
2324 if (nla_put_u32(nlmsg, QCA_WLAN_VENDOR_ATTR_PEER_CFR_ENABLE_GROUP_BITMAP,
2325 CSI_GROUP_BITMAP)) {
2326 wpa_printf(MSG_ERROR, "Failed to csi group bitmap");
2327 nlmsg_free(nlmsg);
2328 return WPA_DRIVER_OEM_STATUS_FAILURE;
2329 }
2330
2331 if (nla_put_u32(nlmsg, QCA_WLAN_VENDOR_ATTR_PEER_CFR_CAPTURE_TYPE,
2332 QCA_WLAN_VENDOR_CFR_TA_RA)) {
2333 nlmsg_free(nlmsg);
2334 return WPA_DRIVER_OEM_STATUS_FAILURE;
2335 }
2336
2337 attr_table = nla_nest_start(nlmsg, QCA_WLAN_VENDOR_ATTR_PEER_CFR_GROUP_TABLE);
2338 if (!attr_table) {
2339 nlmsg_free(nlmsg);
2340 return WPA_DRIVER_OEM_STATUS_FAILURE;
2341 }
2342
2343 attr_entry = nla_nest_start(nlmsg, QCA_WLAN_VENDOR_ATTR_PEER_CFR_GROUP_ENTRY);
2344 if (!attr_entry) {
2345 nlmsg_free(nlmsg);
2346 return WPA_DRIVER_OEM_STATUS_FAILURE;
2347 }
2348
2349 if (nla_put_u32(nlmsg, QCA_WLAN_VENDOR_ATTR_PEER_CFR_GROUP_NUMBER,
2350 CSI_DEFAULT_GROUP_ID)) {
2351 nlmsg_free(nlmsg);
2352 return WPA_DRIVER_OEM_STATUS_FAILURE;
2353 }
2354
2355 if (nla_put_u32(nlmsg, QCA_WLAN_VENDOR_ATTR_PEER_CFR_GROUP_MGMT_FILTER,
2356 CSI_MGMT_BEACON)) {
2357 nlmsg_free(nlmsg);
2358 return WPA_DRIVER_OEM_STATUS_FAILURE;
2359 }
2360
2361 if (nla_put(nlmsg, QCA_WLAN_VENDOR_ATTR_PEER_CFR_GROUP_TA,
2362 MAC_ADDR_LEN, g_csi_param.connected_bssid)) {
2363 nlmsg_free(nlmsg);
2364 return WPA_DRIVER_OEM_STATUS_FAILURE;
2365 }
2366
2367 if (nla_put(nlmsg, QCA_WLAN_VENDOR_ATTR_PEER_CFR_GROUP_TA_MASK,
2368 MAC_ADDR_LEN, ta_mask)) {
2369 nlmsg_free(nlmsg);
2370 return WPA_DRIVER_OEM_STATUS_FAILURE;
2371 }
2372 nla_nest_end(nlmsg, attr_entry);
2373 nla_nest_end(nlmsg, attr_table);
2374 nla_nest_end(nlmsg, attr);
2375
2376 *status = send_nlmsg((struct nl_sock *)drv->global->nl, nlmsg, NULL, NULL);
2377 if (*status != 0) {
2378 wpa_printf(MSG_ERROR, "Failed to send nl message with err %d", *status);
2379 return WPA_DRIVER_OEM_STATUS_FAILURE;
2380 }
2381
2382 g_csi_param.current_state = CSI_STATE_START;
2383
2384 return WPA_DRIVER_OEM_STATUS_SUCCESS;
2385 }
2386
wpa_driver_stop_csi_capture(struct i802_bss * bss,int * status)2387 static int wpa_driver_stop_csi_capture(struct i802_bss *bss, int *status)
2388 {
2389 struct wpa_driver_nl80211_data *drv = bss->drv;
2390 struct nl_msg *nlmsg;
2391 struct nlattr *attr;
2392
2393 nlmsg = prepare_vendor_nlmsg(drv, bss->ifname,
2394 QCA_NL80211_VENDOR_SUBCMD_PEER_CFR_CAPTURE_CFG);
2395 if (!nlmsg) {
2396 wpa_printf(MSG_ERROR, "Failed to allocate nl message");
2397 return WPA_DRIVER_OEM_STATUS_FAILURE;
2398 }
2399
2400 attr = nla_nest_start(nlmsg, NL80211_ATTR_VENDOR_DATA);
2401 if (!attr) {
2402 nlmsg_free(nlmsg);
2403 return WPA_DRIVER_OEM_STATUS_FAILURE;
2404 }
2405
2406 if (nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_PEER_CFR_VERSION,
2407 ENHANCED_CFR_VER)) {
2408 wpa_printf(MSG_ERROR, "Failed to csi version");
2409 nlmsg_free(nlmsg);
2410 return WPA_DRIVER_OEM_STATUS_FAILURE;
2411 }
2412
2413 wpa_printf(MSG_DEBUG, "send stop csi cmd");
2414 nla_nest_end(nlmsg, attr);
2415 *status = send_nlmsg((struct nl_sock *)drv->global->nl, nlmsg, NULL, NULL);
2416 if (*status != 0) {
2417 wpa_printf(MSG_ERROR, "Failed to send nl message with err %d", *status);
2418 return WPA_DRIVER_OEM_STATUS_FAILURE;
2419 }
2420
2421 g_csi_param.current_state = CSI_STATE_STOP;
2422
2423 return WPA_DRIVER_OEM_STATUS_SUCCESS;
2424 }
2425
stop_csi_callback(int nsec)2426 static void stop_csi_callback(int nsec)
2427 {
2428 int status = 0;
2429
2430 wpa_printf(MSG_DEBUG, "enter %s, nsec %d", __func__, nsec);
2431
2432 wpa_driver_stop_csi_capture(g_csi_param.bss, &status);
2433 if (status)
2434 wpa_printf(MSG_ERROR, "Stop CSI failed");
2435 }
2436
wpa_driver_handle_csi_cmd(struct i802_bss * bss,char * cmd,char * buf,size_t buf_len,int * status)2437 static int wpa_driver_handle_csi_cmd(struct i802_bss *bss, char *cmd,
2438 char *buf, size_t buf_len,
2439 int *status)
2440 {
2441 int csi_duration = -1;
2442 int transport_mode = -1;
2443 char *next_arg;
2444
2445 cmd = skip_white_space(cmd);
2446 wpa_printf(MSG_DEBUG, "cmd:%s", cmd);
2447 if (os_strncasecmp(cmd, "start", 5) == 0) {
2448 next_arg = get_next_arg(cmd);
2449 csi_duration = atoi(next_arg);
2450
2451 if (csi_duration < 0) {
2452 wpa_printf(MSG_ERROR, "Invalid duration");
2453 snprintf(buf, buf_len, "FAIL, Invalid duration");
2454 *status = CSI_STATUS_REJECTED;
2455 return WPA_DRIVER_OEM_STATUS_FAILURE;
2456 }
2457
2458 wpa_driver_send_get_scan_cmd(bss, status);
2459 if (g_csi_param.connected_bssid[0] == 0xff) {
2460 wpa_printf(MSG_DEBUG, "Not connected");
2461 snprintf(buf, buf_len, "FAIL, Not connected");
2462 *status = CSI_STATUS_REJECTED;
2463 return WPA_DRIVER_OEM_STATUS_FAILURE;
2464 }
2465
2466 if (g_csi_param.current_state == CSI_STATE_START) {
2467 wpa_driver_stop_csi_capture(bss, status);
2468 alarm(0);
2469 }
2470
2471 g_csi_param.bss = bss;
2472 cmd += 6;
2473 next_arg = get_next_arg(cmd);
2474 if (*next_arg != '\0' && *next_arg == ' ')
2475 transport_mode = atoi(next_arg);
2476
2477 if (transport_mode == 1 || transport_mode == -1)
2478 transport_mode = 1;
2479 g_csi_param.transport_mode = transport_mode;
2480
2481 wpa_driver_start_csi_capture(bss, status, transport_mode);
2482 if (*status == 0 && csi_duration > 0) {
2483 signal(SIGALRM, stop_csi_callback);
2484 alarm(csi_duration);
2485 wpa_printf(MSG_DEBUG, "set alarm %ds done", csi_duration);
2486 }
2487 } else if (os_strncasecmp(cmd, "stop", 4) == 0) {
2488 if (g_csi_param.current_state != CSI_STATE_START)
2489 return WPA_DRIVER_OEM_STATUS_SUCCESS;
2490
2491 wpa_driver_stop_csi_capture(bss, status);
2492 wpa_printf(MSG_DEBUG, "stop csi cmd");
2493 } else {
2494 wpa_printf(MSG_ERROR, "invalid command");
2495 *status = CSI_STATUS_REJECTED;
2496 snprintf(buf, buf_len, "FAIL, Invalid command");
2497 return WPA_DRIVER_OEM_STATUS_FAILURE;
2498 }
2499
2500 return WPA_DRIVER_OEM_STATUS_SUCCESS;
2501 }
2502
wpa_driver_restart_csi(struct i802_bss * bss,int * status)2503 static int wpa_driver_restart_csi(struct i802_bss *bss, int *status)
2504 {
2505 wpa_driver_send_get_scan_cmd(bss, status);
2506 if (g_csi_param.connected_bssid[0] == 0xff) {
2507 wpa_printf(MSG_DEBUG, "%s: Not connected", __func__);
2508 *status = CSI_STATUS_REJECTED;
2509 return WPA_DRIVER_OEM_STATUS_FAILURE;
2510 }
2511 /* Stop CSI capture on previous bss */
2512 if(wpa_driver_stop_csi_capture(g_csi_param.bss, status)) {
2513 wpa_printf(MSG_DEBUG, "%s: csi stop failed", __func__);
2514 }
2515 g_csi_param.bss = bss;
2516 if(wpa_driver_start_csi_capture(g_csi_param.bss, status,
2517 g_csi_param.transport_mode)) {
2518 *status = CSI_STATUS_REJECTED;
2519 return WPA_DRIVER_OEM_STATUS_FAILURE;
2520 }
2521 *status = CSI_STATUS_SUCCESS;
2522 return WPA_DRIVER_OEM_STATUS_SUCCESS;
2523 }
2524
2525 /**
2526 * twt_status_to_string() - Mapping twt response status into string
2527 * @status: enum qca_wlan_vendor_twt_status values
2528 *
2529 * This function will map the twt response status into corresponding
2530 * string.
2531 *
2532 * Return: pointer to the string
2533 */
twt_status_to_string(enum qca_wlan_vendor_twt_status status)2534 static const char *twt_status_to_string(enum qca_wlan_vendor_twt_status status)
2535 {
2536 switch (status) {
2537 case QCA_WLAN_VENDOR_TWT_STATUS_OK:
2538 return "QCA_WLAN_VENDOR_TWT_STATUS_OK";
2539 case QCA_WLAN_VENDOR_TWT_STATUS_TWT_NOT_ENABLED:
2540 return "QCA_WLAN_VENDOR_TWT_STATUS_TWT_NOT_ENABLED";
2541 case QCA_WLAN_VENDOR_TWT_STATUS_USED_DIALOG_ID:
2542 return "QCA_WLAN_VENDOR_TWT_STATUS_USED_DIALOG_ID";
2543 case QCA_WLAN_VENDOR_TWT_STATUS_SESSION_BUSY:
2544 return "QCA_WLAN_VENDOR_TWT_STATUS_SESSION_BUSY";
2545 case QCA_WLAN_VENDOR_TWT_STATUS_SESSION_NOT_EXIST:
2546 return "QCA_WLAN_VENDOR_TWT_STATUS_SESSION_NOT_EXIST";
2547 case QCA_WLAN_VENDOR_TWT_STATUS_NOT_SUSPENDED:
2548 return "QCA_WLAN_VENDOR_TWT_STATUS_NOT_SUSPENDED";
2549 case QCA_WLAN_VENDOR_TWT_STATUS_INVALID_PARAM:
2550 return "QCA_WLAN_VENDOR_TWT_STATUS_INVALID_PARAM";
2551 case QCA_WLAN_VENDOR_TWT_STATUS_NOT_READY:
2552 return "QCA_WLAN_VENDOR_TWT_STATUS_NOT_READY";
2553 case QCA_WLAN_VENDOR_TWT_STATUS_NO_RESOURCE:
2554 return "QCA_WLAN_VENDOR_TWT_STATUS_NO_RESOURCE";
2555 case QCA_WLAN_VENDOR_TWT_STATUS_NO_ACK:
2556 return "QCA_WLAN_VENDOR_TWT_STATUS_NO_ACK";
2557 case QCA_WLAN_VENDOR_TWT_STATUS_NO_RESPONSE:
2558 return "QCA_WLAN_VENDOR_TWT_STATUS_NO_RESPONSE";
2559 case QCA_WLAN_VENDOR_TWT_STATUS_DENIED:
2560 return "QCA_WLAN_VENDOR_TWT_STATUS_DENIED";
2561 case QCA_WLAN_VENDOR_TWT_STATUS_UNKNOWN_ERROR:
2562 return "QCA_WLAN_VENDOR_TWT_STATUS_UNKNOWN_ERROR";
2563 case QCA_WLAN_VENDOR_TWT_STATUS_ALREADY_SUSPENDED:
2564 return "QCA_WLAN_VENDOR_TWT_STATUS_ALREADY_SUSPENDED";
2565 case QCA_WLAN_VENDOR_TWT_STATUS_IE_INVALID:
2566 return "QCA_WLAN_VENDOR_TWT_STATUS_IE_INVALID";
2567 case QCA_WLAN_VENDOR_TWT_STATUS_PARAMS_NOT_IN_RANGE:
2568 return "QCA_WLAN_VENDOR_TWT_STATUS_PARAMS_NOT_IN_RANGE";
2569 case QCA_WLAN_VENDOR_TWT_STATUS_PEER_INITIATED_TERMINATE:
2570 return "QCA_WLAN_VENDOR_TWT_STATUS_PEER_INITIATED_TERMINATE";
2571 case QCA_WLAN_VENDOR_TWT_STATUS_ROAM_INITIATED_TERMINATE:
2572 return "QCA_WLAN_VENDOR_TWT_STATUS_ROAM_INITIATED_TERMINATE";
2573 default:
2574 return "INVALID TWT STATUS";
2575 }
2576 }
2577
2578 /**
2579 * check_for_twt_cmd() - Check if the command string is a TWT command
2580 * @cmd: Command string
2581 *
2582 * This function will identify a TWT operation in the command string
2583 * and return one of the values in enum qca_wlan_twt_operation.
2584 *
2585 * Return: A valid TWT opertion if found, or error if not found
2586 *
2587 */
check_for_twt_cmd(char ** cmd)2588 static int check_for_twt_cmd(char **cmd)
2589 {
2590 if (os_strncasecmp(*cmd, TWT_SETUP_STR, TWT_SETUP_STRLEN) == 0) {
2591 *cmd += (TWT_SETUP_STRLEN + 1);
2592 return QCA_WLAN_TWT_SET;
2593 } else if (os_strncasecmp(*cmd, TWT_TERMINATE_STR,
2594 TWT_TERMINATE_STR_LEN) == 0) {
2595 *cmd += (TWT_TERMINATE_STR_LEN + 1);
2596 return QCA_WLAN_TWT_TERMINATE;
2597 } else if (os_strncasecmp(*cmd, TWT_PAUSE_STR, TWT_PAUSE_STR_LEN) == 0) {
2598 *cmd += (TWT_PAUSE_STR_LEN + 1);
2599 return QCA_WLAN_TWT_SUSPEND;
2600 } else if (os_strncasecmp(*cmd, TWT_RESUME_STR, TWT_RESUME_STR_LEN) == 0) {
2601 *cmd += (TWT_RESUME_STR_LEN + 1);
2602 return QCA_WLAN_TWT_RESUME;
2603 } else if (os_strncasecmp(*cmd, TWT_GET_PARAMS_STR,
2604 TWT_GET_PARAMS_STR_LEN) == 0) {
2605 *cmd += (TWT_GET_PARAMS_STR_LEN + 1);
2606 return QCA_WLAN_TWT_GET;
2607 } else if (os_strncasecmp(*cmd, TWT_NUDGE_STR,
2608 TWT_NUDGE_STR_LEN) == 0) {
2609 *cmd += (TWT_NUDGE_STR_LEN + 1);
2610 return QCA_WLAN_TWT_NUDGE;
2611 } else if (os_strncasecmp(*cmd, TWT_GET_STATS_STR,
2612 TWT_GET_STATS_STR_LEN) == 0) {
2613 *cmd += (TWT_GET_STATS_STR_LEN + 1);
2614 return QCA_WLAN_TWT_GET_STATS;
2615 } else if (os_strncasecmp(*cmd, TWT_CLEAR_STATS_STR,
2616 TWT_CLEAR_STATS_STR_LEN) == 0) {
2617 *cmd += (TWT_CLEAR_STATS_STR_LEN + 1);
2618 return QCA_WLAN_TWT_CLEAR_STATS;
2619 } else if (os_strncasecmp(*cmd, TWT_GET_CAP_STR,
2620 TWT_GET_CAP_STR_LEN) == 0) {
2621 *cmd += (TWT_GET_CAP_STR_LEN + 1);
2622 return QCA_WLAN_TWT_GET_CAPABILITIES;
2623 } else if (os_strncasecmp(*cmd, TWT_SET_PARAM_STR,
2624 TWT_SET_PARAM_STR_LEN) == 0) {
2625 *cmd += (TWT_SET_PARAM_STR_LEN + 1);
2626 return QCA_WLAN_TWT_SET_PARAM;
2627 } else {
2628 wpa_printf(MSG_DEBUG, "Not a TWT command");
2629 return TWT_CMD_NOT_EXIST;
2630 }
2631 }
2632
get_u64_from_string(char * cmd_string,int * ret)2633 static u64 get_u64_from_string(char *cmd_string, int *ret)
2634 {
2635 u64 val = 0;
2636 char *cmd = cmd_string;
2637
2638 while (*cmd != ' ')
2639 cmd++;
2640
2641 *ret = 0;
2642 errno = 0;
2643 val = strtoll(cmd_string, NULL, 10);
2644 if (errno == ERANGE || (errno != 0 && val == 0)) {
2645 wpa_printf(MSG_ERROR, "invalid value");
2646 *ret = -EINVAL;
2647 }
2648 return val;
2649 }
2650
2651
get_u32_from_string(char * cmd_string,int * ret)2652 static u32 get_u32_from_string(char *cmd_string, int *ret)
2653 {
2654 u32 val = 0;
2655 char *cmd = cmd_string;
2656
2657 while (*cmd != ' ')
2658 cmd++;
2659
2660 *ret = 0;
2661 errno = 0;
2662 val = strtol(cmd_string, NULL, 10);
2663 if (errno == ERANGE || (errno != 0 && val == 0)) {
2664 wpa_printf(MSG_ERROR, "invalid value");
2665 *ret = -EINVAL;
2666 }
2667 return val;
2668 }
2669
get_u8_from_string(char * cmd_string,int * ret)2670 static u8 get_u8_from_string(char *cmd_string, int *ret)
2671 {
2672 char *cmd = cmd_string;
2673 u8 val = 0;
2674
2675 while (*cmd != ' ')
2676 cmd++;
2677
2678 *ret = 0;
2679 errno = 0;
2680 val = strtol(cmd_string, NULL, 10) & 0xFF;
2681 if (errno == ERANGE || (errno != 0 && val == 0)) {
2682 wpa_printf(MSG_ERROR, "invalid value");
2683 *ret = -EINVAL;
2684 }
2685 return val;
2686 }
2687
move_to_next_str(char * cmd)2688 char *move_to_next_str(char *cmd)
2689 {
2690 if (*cmd == '\0')
2691 return cmd;
2692
2693 while (*cmd != ' ') {
2694 cmd++;
2695 if (*cmd == '\0')
2696 return cmd;
2697 }
2698
2699 while (*cmd == ' ')
2700 cmd++;
2701
2702 return cmd;
2703 }
2704
is_binary(u8 value)2705 static int is_binary(u8 value) {
2706 if(value == 0 || value == 1)
2707 return 0;
2708 return -1;
2709 }
2710
2711 static
print_setup_cmd_values(struct twt_setup_parameters * twt_setup_params)2712 void print_setup_cmd_values(struct twt_setup_parameters *twt_setup_params)
2713 {
2714 wpa_printf(MSG_DEBUG, "TWT: setup dialog_id: %x",
2715 twt_setup_params->dialog_id);
2716 wpa_printf(MSG_DEBUG, "TWT: setup req type: %d ",
2717 twt_setup_params->req_type);
2718 wpa_printf(MSG_DEBUG, "TWT: setup trig type: %d ",
2719 twt_setup_params->trig_type);
2720 wpa_printf(MSG_DEBUG, "TWT: setup flow type: 0x%x",
2721 twt_setup_params->flow_type);
2722 wpa_printf(MSG_DEBUG, "TWT: setup wake exp: 0x%x",
2723 twt_setup_params->wake_intr_exp);
2724 wpa_printf(MSG_DEBUG, "TWT: setup protection: 0x%x",
2725 twt_setup_params->protection);
2726 wpa_printf(MSG_DEBUG, "TWT: setup wake time: 0x%x",
2727 twt_setup_params->wake_time);
2728 wpa_printf(MSG_DEBUG, "TWT: setup wake dur: 0x%x",
2729 twt_setup_params->wake_dur);
2730 wpa_printf(MSG_DEBUG, "TWT: setup wake intr mantissa: 0x%x",
2731 twt_setup_params->wake_intr_mantissa);
2732 wpa_printf(MSG_DEBUG, "TWT: setup bcast: %d ",
2733 twt_setup_params->bcast);
2734 wpa_printf(MSG_DEBUG, "TWT: min wake intvl: %d ",
2735 twt_setup_params->min_wake_intvl);
2736 wpa_printf(MSG_DEBUG, "TWT: max wake intvl: %d ",
2737 twt_setup_params->max_wake_intvl);
2738 wpa_printf(MSG_DEBUG, "TWT: min wake duration: %d ",
2739 twt_setup_params->min_wake_duration);
2740 wpa_printf(MSG_DEBUG, "TWT: max wake duration: %d ",
2741 twt_setup_params->max_wake_duration);
2742 wpa_printf(MSG_DEBUG, "TWT: wake tsf: 0x%lx ",
2743 twt_setup_params->wake_tsf);
2744 wpa_printf(MSG_DEBUG, "TWT: announce timeout(in us): %u",
2745 twt_setup_params->announce_timeout_us);
2746 }
2747
check_cmd_input(char * cmd_string)2748 static int check_cmd_input(char *cmd_string)
2749 {
2750 u32 cmd_string_len;
2751
2752 if (!cmd_string) {
2753 wpa_printf(MSG_ERROR, "cmd string null");
2754 return -EINVAL;
2755 }
2756
2757 cmd_string_len = strlen(cmd_string);
2758
2759 wpa_printf(MSG_DEBUG, "TWT: cmd string - %s len = %u", cmd_string,
2760 cmd_string_len);
2761 if (cmd_string_len < DIALOG_ID_STR_LEN + SINGLE_SPACE_LEN +
2762 SINGLE_DIGIT_LEN) {
2763 wpa_printf(MSG_ERROR, "TWT: Dialog_id parameter missing");
2764 return -EINVAL;
2765 }
2766
2767 return 0;
2768 }
2769
2770 /**
2771 * process_twt_setup_cmd_string() - Process a TWT setup command string
2772 * @cmd: Command string
2773 * @twt_setup_params: TWT setup parameters to fill
2774 *
2775 * This function parses the TWT setup command and populates twt_setup_params.
2776 *
2777 * Return: 0 on successs, error if invalid params are found
2778 *
2779 */
2780 static
process_twt_setup_cmd_string(char * cmd,struct twt_setup_parameters * twt_setup_params)2781 int process_twt_setup_cmd_string(char *cmd,
2782 struct twt_setup_parameters *twt_setup_params)
2783 {
2784 int ret = 0;
2785
2786 if (!twt_setup_params) {
2787 wpa_printf(MSG_ERROR, "cmd or twt_setup_params null");
2788 return -EINVAL;
2789 }
2790
2791 if (check_cmd_input(cmd))
2792 return -EINVAL;
2793
2794 wpa_printf(MSG_DEBUG, "process twt setup command string: %s", cmd);
2795 while (*cmd == ' ')
2796 cmd++;
2797
2798 if (os_strncasecmp(cmd, DIALOG_ID_STR, DIALOG_ID_STR_LEN) == 0) {
2799 cmd += (DIALOG_ID_STR_LEN + 1);
2800 twt_setup_params->dialog_id = get_u8_from_string(cmd, &ret);
2801 if (ret < 0)
2802 return ret;
2803 cmd = move_to_next_str(cmd);
2804 }
2805
2806 if (os_strncasecmp(cmd, REQ_TYPE_STR, REQ_TYPE_STR_LEN) == 0) {
2807 cmd += (REQ_TYPE_STR_LEN + 1);
2808 twt_setup_params->req_type = get_u8_from_string(cmd, &ret);
2809 if (ret < 0)
2810 return ret;
2811 cmd = move_to_next_str(cmd);
2812 }
2813
2814 if (os_strncasecmp(cmd, TRIG_TYPE_STR, TRIG_TYPE_STR_LEN) == 0) {
2815 cmd += (TRIG_TYPE_STR_LEN + 1);
2816 twt_setup_params->trig_type = get_u8_from_string(cmd, &ret);
2817 if (ret < 0)
2818 return ret;
2819
2820 if (is_binary(twt_setup_params->trig_type)) {
2821 wpa_printf(MSG_ERROR, "Invalid trigger type");
2822 return -EINVAL;
2823 }
2824 cmd = move_to_next_str(cmd);
2825 }
2826
2827 if (strncmp(cmd, FLOW_TYPE_STR, FLOW_TYPE_STR_LEN) == 0) {
2828 cmd += (FLOW_TYPE_STR_LEN + 1);
2829 twt_setup_params->flow_type = get_u8_from_string(cmd, &ret);
2830 if (ret < 0)
2831 return ret;
2832
2833 if (is_binary(twt_setup_params->flow_type)) {
2834 wpa_printf(MSG_ERROR, "Invalid flow type");
2835 return -EINVAL;
2836 }
2837 cmd = move_to_next_str(cmd);
2838 }
2839
2840 if (strncmp(cmd, WAKE_INTR_EXP_STR, WAKE_INTR_EXP_STR_LEN) == 0) {
2841 cmd += (WAKE_INTR_EXP_STR_LEN + 1);
2842 twt_setup_params->wake_intr_exp = get_u8_from_string(cmd, &ret);
2843 if (ret < 0)
2844 return ret;
2845
2846 if (twt_setup_params->wake_intr_exp >
2847 TWT_SETUP_WAKE_INTVL_EXP_MAX) {
2848 wpa_printf(MSG_DEBUG, "Invalid wake_intr_exp %u",
2849 twt_setup_params->wake_intr_exp);
2850 return -EINVAL;
2851 }
2852 cmd = move_to_next_str(cmd);
2853 }
2854
2855 if (strncmp(cmd, PROTECTION_STR, PROTECTION_STR_LEN) == 0) {
2856 cmd += (PROTECTION_STR_LEN + 1);
2857 twt_setup_params->protection = get_u8_from_string(cmd, &ret);
2858 if (ret < 0)
2859 return ret;
2860
2861 if (is_binary(twt_setup_params->protection)) {
2862 wpa_printf(MSG_ERROR, "Invalid protection value");
2863 return -EINVAL;
2864 }
2865 cmd = move_to_next_str(cmd);
2866 }
2867
2868 if (strncmp(cmd, WAKE_TIME_STR, WAKE_TIME_STR_LEN) == 0) {
2869 cmd += (WAKE_TIME_STR_LEN + 1);
2870 twt_setup_params->wake_time = get_u32_from_string(cmd, &ret);
2871 if (ret < 0)
2872 return ret;
2873 cmd = move_to_next_str(cmd);
2874 }
2875
2876 if (strncmp(cmd, WAKE_DUR_STR, WAKE_DUR_STR_LEN) == 0) {
2877 cmd += (WAKE_DUR_STR_LEN + 1);
2878 twt_setup_params->wake_dur = get_u32_from_string(cmd, &ret);
2879 if (ret < 0)
2880 return ret;
2881
2882 if (twt_setup_params->wake_dur == 0 ||
2883 twt_setup_params->wake_dur > TWT_SETUP_WAKE_DURATION_MAX) {
2884 wpa_printf(MSG_ERROR, "Invalid wake_dura_us %u",
2885 twt_setup_params->wake_dur);
2886 return -EINVAL;
2887 }
2888
2889 cmd = move_to_next_str(cmd);
2890 }
2891
2892 if (strncmp(cmd, WAKE_INTR_MANTISSA_STR,
2893 WAKE_INTR_MANTISSA_STR_LEN) == 0) {
2894 cmd += (WAKE_INTR_MANTISSA_STR_LEN + 1);
2895 twt_setup_params->wake_intr_mantissa = get_u32_from_string(cmd, &ret);
2896 if (ret < 0)
2897 return ret;
2898 if (twt_setup_params->wake_intr_mantissa >
2899 TWT_SETUP_WAKE_INTVL_MANTISSA_MAX) {
2900 wpa_printf(MSG_ERROR, "Invalid wake_intr_mantissa %u",
2901 twt_setup_params->wake_intr_mantissa);
2902 return -EINVAL;
2903 }
2904
2905 cmd = move_to_next_str(cmd);
2906 }
2907
2908 if (strncmp(cmd, BROADCAST_STR, BROADCAST_STR_LEN) == 0) {
2909 cmd += (BROADCAST_STR_LEN + 1);
2910 twt_setup_params->bcast = get_u8_from_string(cmd, &ret);
2911 if (ret < 0)
2912 return ret;
2913
2914 if (is_binary(twt_setup_params->bcast)) {
2915 wpa_printf(MSG_ERROR, "Invalid broadcast value");
2916 return -EINVAL;
2917 }
2918 cmd = move_to_next_str(cmd);
2919 }
2920
2921 if (strncmp(cmd, MIN_WAKE_INTVL_STR, MIN_WAKE_INTVL_STR_LEN) == 0) {
2922 cmd += (MIN_WAKE_INTVL_STR_LEN + 1);
2923 twt_setup_params->min_wake_intvl = get_u32_from_string(cmd, &ret);
2924 if (ret < 0)
2925 return ret;
2926 cmd = move_to_next_str(cmd);
2927 }
2928
2929 if (strncmp(cmd, MAX_WAKE_INTVL_STR, MAX_WAKE_INTVL_STR_LEN) == 0) {
2930 cmd += (MAX_WAKE_INTVL_STR_LEN + 1);
2931 twt_setup_params->max_wake_intvl = get_u32_from_string(cmd, &ret);
2932 if (ret < 0)
2933 return ret;
2934 cmd = move_to_next_str(cmd);
2935 }
2936
2937 if (strncmp(cmd, MIN_WAKE_DUR_STR, MIN_WAKE_DUR_STR_LEN) == 0) {
2938 cmd += (MIN_WAKE_DUR_STR_LEN + 1);
2939 twt_setup_params->min_wake_duration = get_u32_from_string(cmd, &ret);
2940 if (ret < 0)
2941 return ret;
2942 cmd = move_to_next_str(cmd);
2943 }
2944
2945 if (strncmp(cmd, MAX_WAKE_DUR_STR, MAX_WAKE_DUR_STR_LEN) == 0) {
2946 cmd += (MAX_WAKE_DUR_STR_LEN + 1);
2947 twt_setup_params->max_wake_duration = get_u32_from_string(cmd, &ret);
2948 if (ret < 0)
2949 return ret;
2950 cmd = move_to_next_str(cmd);
2951 }
2952
2953 if (strncmp(cmd, WAKE_TSF_STR, WAKE_TSF_STR_LEN) == 0) {
2954 cmd += (WAKE_TSF_STR_LEN + 1);
2955 twt_setup_params->wake_tsf = get_u64_from_string(cmd, &ret);
2956 if(ret < 0)
2957 return ret;
2958 cmd = move_to_next_str(cmd);
2959 }
2960
2961 if (strncmp(cmd, ANNOUNCE_TIMEOUT_STR, ANNOUNCE_TIMEOUT_STR_LEN) == 0) {
2962 cmd += (ANNOUNCE_TIMEOUT_STR_LEN + 1);
2963 twt_setup_params->announce_timeout_us =
2964 get_u32_from_string(cmd, &ret);
2965 if (ret < 0)
2966 return ret;
2967 cmd = move_to_next_str(cmd);
2968 }
2969
2970 print_setup_cmd_values(twt_setup_params);
2971
2972 return 0;
2973 }
2974
2975 static
prepare_twt_setup_nlmsg(struct nl_msg * nlmsg,struct twt_setup_parameters * twt_setup_params)2976 int prepare_twt_setup_nlmsg(struct nl_msg *nlmsg,
2977 struct twt_setup_parameters *twt_setup_params)
2978 {
2979 struct nlattr *twt_attr;
2980
2981 if (nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_OPERATION,
2982 QCA_WLAN_TWT_SET)) {
2983 wpa_printf(MSG_DEBUG, "TWT: Failed to put QCA_WLAN_TWT_SET");
2984 goto fail;
2985 }
2986
2987 twt_attr = nla_nest_start(nlmsg,
2988 QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_PARAMS);
2989 if (twt_attr == NULL)
2990 goto fail;
2991
2992 if (nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_TWT_SETUP_FLOW_ID,
2993 twt_setup_params->dialog_id)) {
2994 wpa_printf(MSG_DEBUG, "TWT: Failed to put dialog_id");
2995 goto fail;
2996 }
2997
2998 if (nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_TWT_SETUP_REQ_TYPE,
2999 twt_setup_params->req_type)) {
3000 wpa_printf(MSG_DEBUG, "TWT: Failed to put req type");
3001 goto fail;
3002 }
3003
3004 if (twt_setup_params->trig_type) {
3005 if (nla_put_flag(nlmsg, QCA_WLAN_VENDOR_ATTR_TWT_SETUP_TRIGGER)
3006 ) {
3007 wpa_printf(MSG_DEBUG, "TWT: Failed to put trig type");
3008 goto fail;
3009 }
3010 }
3011
3012 /*0 - Announced/ 1 - Unannounced*/
3013 if (nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_TWT_SETUP_FLOW_TYPE,
3014 twt_setup_params->flow_type)) {
3015 wpa_printf(MSG_DEBUG, "TWT: Failed to put flow type");
3016 goto fail;
3017 }
3018
3019 if (nla_put_u8(nlmsg,
3020 QCA_WLAN_VENDOR_ATTR_TWT_SETUP_WAKE_INTVL_EXP,
3021 twt_setup_params->wake_intr_exp)) {
3022 wpa_printf(MSG_DEBUG, "TWT: Failed to put wake exp");
3023 goto fail;
3024 }
3025
3026 if (twt_setup_params->protection) {
3027 if (nla_put_flag(nlmsg,
3028 QCA_WLAN_VENDOR_ATTR_TWT_SETUP_PROTECTION)) {
3029 wpa_printf(MSG_DEBUG,
3030 "TWT: Failed to add protection");
3031 goto fail;
3032 }
3033 }
3034
3035 /*offset to add with TBTT after which 1st SP will start*/
3036 if (nla_put_u32(nlmsg,
3037 QCA_WLAN_VENDOR_ATTR_TWT_SETUP_WAKE_TIME,
3038 twt_setup_params->wake_time)) {
3039 wpa_printf(MSG_DEBUG, "TWT: Failed to put wake time");
3040 goto fail;
3041 }
3042
3043 /*TWT Wake Duration in units of us, must be <= 65280*/
3044 if (nla_put_u32(nlmsg,
3045 QCA_WLAN_VENDOR_ATTR_TWT_SETUP_WAKE_DURATION,
3046 twt_setup_params->wake_dur)) {
3047 wpa_printf(MSG_DEBUG, "TWT: Failed to put wake dur");
3048 goto fail;
3049 }
3050
3051 if (nla_put_u32(nlmsg,
3052 QCA_WLAN_VENDOR_ATTR_TWT_SETUP_WAKE_INTVL2_MANTISSA,
3053 twt_setup_params->wake_intr_mantissa)) {
3054 wpa_printf(MSG_DEBUG, "TWT: Failed to put wake intr mantissa");
3055 goto fail;
3056 }
3057
3058 if (nla_put_u32(nlmsg,
3059 QCA_WLAN_VENDOR_ATTR_TWT_SETUP_WAKE_INTVL_MANTISSA,
3060 twt_setup_params->wake_intr_mantissa/TWT_WAKE_INTERVAL_TU_FACTOR)) {
3061 wpa_printf(MSG_DEBUG, "TWT: Failed to put wake intr mantissa");
3062 goto fail;
3063 }
3064
3065 if (twt_setup_params->bcast) {
3066 if (nla_put_flag(nlmsg,
3067 QCA_WLAN_VENDOR_ATTR_TWT_SETUP_BCAST)) {
3068 wpa_printf(MSG_DEBUG, "TWT: Failed to put bcast");
3069 goto fail;
3070 }
3071 }
3072
3073 if (nla_put_u32(nlmsg,
3074 QCA_WLAN_VENDOR_ATTR_TWT_SETUP_MIN_WAKE_INTVL,
3075 twt_setup_params->min_wake_intvl)) {
3076 wpa_printf(MSG_ERROR, "TWT: Failed to put min wake intr ");
3077 goto fail;
3078 }
3079
3080 if (nla_put_u32(nlmsg,
3081 QCA_WLAN_VENDOR_ATTR_TWT_SETUP_MAX_WAKE_INTVL,
3082 twt_setup_params->max_wake_intvl)) {
3083 wpa_printf(MSG_ERROR,"TWT: Failed to put max wake intr");
3084 goto fail;
3085 }
3086
3087 if (nla_put_u32(nlmsg,
3088 QCA_WLAN_VENDOR_ATTR_TWT_SETUP_MIN_WAKE_DURATION,
3089 twt_setup_params->min_wake_duration)) {
3090 wpa_printf(MSG_ERROR,"TWT: Failed to put min wake dur");
3091 goto fail;
3092 }
3093
3094 if (nla_put_u32(nlmsg,
3095 QCA_WLAN_VENDOR_ATTR_TWT_SETUP_MAX_WAKE_DURATION,
3096 twt_setup_params->max_wake_duration)) {
3097 wpa_printf(MSG_ERROR,"TWT: Failed to put max wake dur");
3098 goto fail;
3099 }
3100
3101 if (twt_setup_params->wake_tsf) {
3102 if (nla_put_u64(nlmsg,
3103 QCA_WLAN_VENDOR_ATTR_TWT_SETUP_WAKE_TIME_TSF,
3104 twt_setup_params->wake_tsf)) {
3105 wpa_printf(MSG_ERROR,"TWT: Failed to put wake time tsf value");
3106 goto fail;
3107 }
3108 }
3109
3110 if (twt_setup_params->announce_timeout_us) {
3111 if (nla_put_u32(nlmsg,
3112 QCA_WLAN_VENDOR_ATTR_TWT_SETUP_ANNOUNCE_TIMEOUT,
3113 twt_setup_params->announce_timeout_us)) {
3114 wpa_printf(MSG_ERROR, "TWT: Failed to put announce timeout value");
3115 goto fail;
3116 }
3117 }
3118
3119 nla_nest_end(nlmsg, twt_attr);
3120 wpa_printf(MSG_DEBUG, "TWT: setup command nla end");
3121 return 0;
3122
3123 fail:
3124 return -EINVAL;
3125 }
3126
prepare_twt_terminate_nlmsg(struct nl_msg * nlmsg,char * cmd)3127 static int prepare_twt_terminate_nlmsg(struct nl_msg *nlmsg, char *cmd)
3128 {
3129 u8 dialog_id;
3130 struct nlattr *twt_attr;
3131 int ret = 0;
3132
3133 if(check_cmd_input(cmd))
3134 return -EINVAL;
3135
3136 while(*cmd == ' ')
3137 cmd++;
3138
3139 if (os_strncasecmp(cmd, DIALOG_ID_STR, DIALOG_ID_STR_LEN) == 0) {
3140 cmd += (DIALOG_ID_STR_LEN + 1);
3141 dialog_id = get_u8_from_string(cmd, &ret);
3142 if (ret < 0)
3143 return ret;
3144 } else {
3145 wpa_printf(MSG_ERROR, "TWT: no dialog_id found");
3146 goto fail;
3147 }
3148
3149 if (nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_OPERATION,
3150 QCA_WLAN_TWT_TERMINATE)) {
3151 wpa_printf(MSG_DEBUG, "TWT: Failed to put QCA_WLAN_TWT_TERMINATE");
3152 goto fail;
3153 }
3154
3155 twt_attr = nla_nest_start(nlmsg,
3156 QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_PARAMS);
3157 if (twt_attr == NULL)
3158 goto fail;
3159
3160 if (nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_TWT_SETUP_FLOW_ID,
3161 dialog_id)) {
3162 wpa_printf(MSG_DEBUG, "TWT: Failed to put dialog_id");
3163 goto fail;
3164 }
3165
3166 nla_nest_end(nlmsg, twt_attr);
3167 wpa_printf(MSG_DEBUG, "TWT: terminate sent with dialog_id: %x",
3168 dialog_id);
3169
3170 return 0;
3171 fail:
3172 return -EINVAL;
3173 }
3174
prepare_twt_pause_nlmsg(struct nl_msg * nlmsg,char * cmd)3175 static int prepare_twt_pause_nlmsg(struct nl_msg *nlmsg, char *cmd)
3176 {
3177 u8 dialog_id;
3178 struct nlattr *twt_attr;
3179 int ret = 0;
3180
3181 if(check_cmd_input(cmd))
3182 return -EINVAL;
3183
3184 while(*cmd == ' ')
3185 cmd++;
3186
3187 if (os_strncasecmp(cmd, DIALOG_ID_STR, DIALOG_ID_STR_LEN) == 0) {
3188 cmd += (DIALOG_ID_STR_LEN + 1);
3189 dialog_id = get_u8_from_string(cmd, &ret);
3190 if(ret < 0)
3191 return ret;
3192 } else {
3193 wpa_printf(MSG_ERROR, "TWT: no dialog_id found");
3194 goto fail;
3195 }
3196
3197 if (nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_OPERATION,
3198 QCA_WLAN_TWT_SUSPEND)) {
3199 wpa_printf(MSG_DEBUG, "TWT: Failed to put QCA_WLAN_TWT_TERMINATE");
3200 goto fail;
3201 }
3202
3203 twt_attr = nla_nest_start(nlmsg,
3204 QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_PARAMS);
3205 if (twt_attr == NULL)
3206 goto fail;
3207
3208 if (nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_TWT_SETUP_FLOW_ID,
3209 dialog_id)) {
3210 wpa_printf(MSG_DEBUG, "TWT: Failed to put dialog_id");
3211 goto fail;
3212 }
3213
3214 nla_nest_end(nlmsg, twt_attr);
3215 wpa_printf(MSG_DEBUG, "TWT: pause sent with dialog_id: %x", dialog_id);
3216
3217 return 0;
3218 fail:
3219 return -EINVAL;
3220 }
3221
3222 /**
3223 * process_twt_resume_cmd_string() - Process a TWT resume command string
3224 * @cmd: Command string
3225 * @resume_params: TWT resume parameters to fill
3226 *
3227 * This function parses the TWT resume command and populates resume_params.
3228 *
3229 * Return: 0 on successs, error if invalid params are found
3230 *
3231 */
3232 static
process_twt_resume_cmd_string(char * cmd,struct twt_resume_parameters * resume_params)3233 int process_twt_resume_cmd_string(char *cmd,
3234 struct twt_resume_parameters *resume_params)
3235 {
3236 int ret = 0;
3237
3238 if (!resume_params) {
3239 wpa_printf(MSG_ERROR, "TWT: cmd or resume_params null");
3240 return -EINVAL;
3241 }
3242
3243 if(check_cmd_input(cmd))
3244 return -EINVAL;
3245
3246 while(*cmd == ' ')
3247 cmd++;
3248
3249 if (os_strncasecmp(cmd, DIALOG_ID_STR, DIALOG_ID_STR_LEN) != 0) {
3250 wpa_printf(MSG_ERROR, "TWT: dialog ID parameter is required");
3251 return -EINVAL;
3252 }
3253 cmd += (DIALOG_ID_STR_LEN + 1);
3254 resume_params->dialog_id = get_u8_from_string(cmd, &ret);
3255 if (ret < 0)
3256 return ret;
3257 cmd = move_to_next_str(cmd);
3258
3259 if (os_strncasecmp(cmd, NEXT_TWT_STR, NEXT_TWT_STR_LEN) == 0) {
3260 cmd += (NEXT_TWT_STR_LEN + 1);
3261 resume_params->next_twt = get_u8_from_string(cmd, &ret);
3262 if (ret < 0)
3263 return ret;
3264 wpa_printf(MSG_DEBUG, "TWT: NEXT TWT %d", resume_params->next_twt);
3265 cmd = move_to_next_str(cmd);
3266 }
3267
3268 if (os_strncasecmp(cmd, NEXT2_TWT_STR, NEXT2_TWT_STR_LEN) == 0) {
3269 cmd += (NEXT2_TWT_STR_LEN + 1);
3270 resume_params->next2_twt = get_u32_from_string(cmd, &ret);
3271 if (ret < 0)
3272 return ret;
3273 wpa_printf(MSG_DEBUG, "TWT: NEXT2 TWT %d", resume_params->next2_twt);
3274 cmd = move_to_next_str(cmd);
3275 }
3276
3277 if (os_strncasecmp(cmd, NEXT_TWT_SIZE_STR, NEXT_TWT_SIZE_STR_LEN) != 0) {
3278 wpa_printf(MSG_ERROR, "TWT: next_twt_size parameter is required");
3279 return -EINVAL;
3280 }
3281 cmd += (NEXT_TWT_SIZE_STR_LEN + 1);
3282 resume_params->next_twt_size = get_u32_from_string(cmd, &ret);
3283 if (ret < 0)
3284 return ret;
3285
3286 return 0;
3287 }
3288
3289 static
prepare_twt_resume_nlmsg(struct nl_msg * nlmsg,struct twt_resume_parameters * resume_params)3290 int prepare_twt_resume_nlmsg(struct nl_msg *nlmsg,
3291 struct twt_resume_parameters *resume_params)
3292 {
3293 struct nlattr *twt_attr;
3294
3295 if (nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_OPERATION,
3296 QCA_WLAN_TWT_RESUME)) {
3297 wpa_printf(MSG_DEBUG, "TWT: Failed to put QCA_WLAN_TWT_RESUME");
3298 return -EINVAL;
3299 }
3300
3301 twt_attr = nla_nest_start(nlmsg,QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_PARAMS);
3302 if (twt_attr == NULL)
3303 return -EINVAL;
3304
3305 if (nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_TWT_RESUME_FLOW_ID,
3306 resume_params->dialog_id)) {
3307 wpa_printf(MSG_DEBUG, "TWT: Failed to put dialog_id");
3308 return -EINVAL;
3309 }
3310 if (nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_TWT_RESUME_NEXT_TWT,
3311 resume_params->next_twt)) {
3312 wpa_printf(MSG_DEBUG, "TWT: next_twt");
3313 return -EINVAL;
3314 }
3315 if (nla_put_u32(nlmsg, QCA_WLAN_VENDOR_ATTR_TWT_RESUME_NEXT2_TWT,
3316 resume_params->next2_twt)) {
3317 wpa_printf(MSG_DEBUG, "TWT: Failed to put next2_twt");
3318 return -EINVAL;
3319 }
3320 if (nla_put_u32(nlmsg, QCA_WLAN_VENDOR_ATTR_TWT_RESUME_NEXT_TWT_SIZE,
3321 resume_params->next_twt_size)) {
3322 wpa_printf(MSG_DEBUG, "TWT: Failed to put next_twt_size");
3323 return -EINVAL;
3324 }
3325 nla_nest_end(nlmsg, twt_attr);
3326
3327 wpa_printf(MSG_DEBUG,"TWT: resume dialog_id: 0x%x next_twt (us): 0x%x next2_twt (us): 0x%x next_twt_size: %u",
3328 resume_params->dialog_id, resume_params->next_twt,
3329 resume_params->next2_twt,resume_params->next_twt_size);
3330
3331 return 0;
3332 }
3333
3334 /**
3335 * process_twt_nudge_cmd_string()- processes command string for nudge command.
3336 *
3337 * @Param cmd: expects the command
3338 * @Param nudge_params: return parsed nudge parameters
3339 *
3340 * @Returns 0 on Success, -EINVAL on Failure
3341 */
3342 static
process_twt_nudge_cmd_string(char * cmd,struct twt_nudge_parameters * nudge_params)3343 int process_twt_nudge_cmd_string(char *cmd,
3344 struct twt_nudge_parameters *nudge_params)
3345 {
3346 int ret = 0;
3347
3348 if (!nudge_params) {
3349 wpa_printf(MSG_ERROR, "TWT: nudge_params null");
3350 return -EINVAL;
3351 }
3352
3353 if(check_cmd_input(cmd))
3354 return -EINVAL;
3355
3356 while(*cmd == ' ')
3357 cmd++;
3358
3359 if (os_strncasecmp(cmd, DIALOG_ID_STR, DIALOG_ID_STR_LEN) != 0) {
3360 wpa_printf(MSG_ERROR, "TWT: dialog_id parameter is required");
3361 return -EINVAL;
3362 }
3363 cmd += (DIALOG_ID_STR_LEN + 1);
3364 nudge_params->dialog_id = get_u8_from_string(cmd, &ret);
3365 if (ret < 0)
3366 return ret;
3367 cmd = move_to_next_str(cmd);
3368
3369 if (os_strncasecmp(cmd, PAUSE_DURATION_STR, PAUSE_DURATION_STR_LEN) == 0) {
3370 cmd += (PAUSE_DURATION_STR_LEN + 1);
3371 nudge_params->wake_time = get_u32_from_string(cmd, &ret);
3372 if (ret < 0)
3373 return ret;
3374 wpa_printf(MSG_DEBUG, "TWT: WAKE TIME %d", nudge_params->wake_time);
3375 cmd = move_to_next_str(cmd);
3376 }
3377
3378 if (os_strncasecmp(cmd, NEXT_TWT_SIZE_STR, NEXT_TWT_SIZE_STR_LEN) != 0) {
3379 wpa_printf(MSG_ERROR, "TWT: next_twt_size parameter is required");
3380 return -EINVAL;
3381 }
3382 cmd += (NEXT_TWT_SIZE_STR_LEN + 1);
3383 nudge_params->next_twt_size = get_u32_from_string(cmd, &ret);
3384 if (ret < 0)
3385 return ret;
3386
3387 return 0;
3388 }
3389
3390 /**
3391 * prepare_twt_nudge_nlmsg()- prepare twt_session_nudge command .
3392 *
3393 * @Param nlmsg: nl command buffer
3394 * @Param nudge_params: nudge parameters to prepare command
3395 *
3396 * @Returns 0 on Success, -EINVAL on Failure
3397 */
3398 static
prepare_twt_nudge_nlmsg(struct nl_msg * nlmsg,struct twt_nudge_parameters * nudge_params)3399 int prepare_twt_nudge_nlmsg(struct nl_msg *nlmsg,
3400 struct twt_nudge_parameters *nudge_params)
3401 {
3402 struct nlattr *twt_attr;
3403
3404 if (nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_OPERATION,
3405 QCA_WLAN_TWT_NUDGE)) {
3406 wpa_printf(MSG_DEBUG, "TWT: Failed to put twt operation");
3407 return -EINVAL;
3408 }
3409
3410 twt_attr = nla_nest_start(nlmsg, QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_PARAMS);
3411 if (twt_attr == NULL)
3412 return -EINVAL;
3413
3414 if (nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_TWT_NUDGE_FLOW_ID,
3415 nudge_params->dialog_id)) {
3416 wpa_printf(MSG_DEBUG, "TWT: Failed to put dialog_id");
3417 return -EINVAL;
3418 }
3419
3420 if (nla_put_u32(nlmsg, QCA_WLAN_VENDOR_ATTR_TWT_NUDGE_WAKE_TIME,
3421 nudge_params->wake_time)) {
3422 wpa_printf(MSG_DEBUG, "TWT: Failed to put wake_time");
3423 return -EINVAL;
3424 }
3425
3426 if (nla_put_u32(nlmsg, QCA_WLAN_VENDOR_ATTR_TWT_NUDGE_NEXT_TWT_SIZE,
3427 nudge_params->next_twt_size)) {
3428 wpa_printf(MSG_DEBUG, "TWT: Failed to put next_twt_size");
3429 return -EINVAL;
3430 }
3431 nla_nest_end(nlmsg, twt_attr);
3432
3433 wpa_printf(MSG_DEBUG,"TWT: nudge dialog_id: 0x%x wake_time(us): 0x%x next_twt_size: %u",
3434 nudge_params->dialog_id, nudge_params->wake_time,
3435 nudge_params->next_twt_size);
3436
3437 return 0;
3438 }
3439
3440 /**
3441 * process_twt_set_param_cmd_string()- processes command
3442 * string for set parameters command.
3443 *
3444 * @Param cmd: expects the command
3445 * @Param set_params: return parsed TWT set parameters
3446 *
3447 * @Returns 0 on Success, -EINVAL on Failure
3448 */
3449 static
process_twt_set_param_cmd_string(char * cmd,struct twt_set_parameters * set_params)3450 int process_twt_set_param_cmd_string(char *cmd,
3451 struct twt_set_parameters *set_params)
3452 {
3453 int ret = -EINVAL;
3454
3455 if (!set_params) {
3456 wpa_printf(MSG_ERROR, "TWT: set_params null");
3457 return -EINVAL;
3458 }
3459
3460 if (check_cmd_input(cmd))
3461 return -EINVAL;
3462
3463 while (*cmd == ' ')
3464 cmd++;
3465
3466 if (os_strncasecmp(cmd, AP_AC_VALUE_STR, AP_AC_VALUE_STR_LEN) == 0) {
3467 cmd += (AP_AC_VALUE_STR_LEN + 1);
3468 set_params->ap_ac_value = get_u8_from_string(cmd, &ret);
3469 wpa_printf(MSG_DEBUG, "TWT: AP AC VALUE: %d", set_params->ap_ac_value);
3470 if (ret < 0)
3471 return ret;
3472 cmd = move_to_next_str(cmd);
3473 }
3474
3475 return ret;
3476 }
3477
3478 /**
3479 * prepare_twt_set_param_nlmsg()- prepare twt_set_param command .
3480 *
3481 * @Param nlmsg: nl command buffer
3482 * @Param set_params: set parameters to prepare command
3483 *
3484 * @Returns 0 on Success, -EINVAL on Failure
3485 */
3486 static
prepare_twt_set_param_nlmsg(struct nl_msg * nlmsg,struct twt_set_parameters * set_params)3487 int prepare_twt_set_param_nlmsg(struct nl_msg *nlmsg,
3488 struct twt_set_parameters *set_params)
3489 {
3490 struct nlattr *twt_attr;
3491
3492 if (nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_OPERATION,
3493 QCA_WLAN_TWT_SET_PARAM)) {
3494 wpa_printf(MSG_DEBUG, "TWT: Failed to put twt operation");
3495 return -EINVAL;
3496 }
3497
3498 twt_attr = nla_nest_start(nlmsg,
3499 QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_PARAMS);
3500 if (!twt_attr)
3501 return -EINVAL;
3502
3503 if (nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_TWT_SET_PARAM_AP_AC_VALUE,
3504 set_params->ap_ac_value)) {
3505 wpa_printf(MSG_DEBUG, "TWT: Failed to put ap_ac_value");
3506 return -EINVAL;
3507 }
3508
3509 nla_nest_end(nlmsg, twt_attr);
3510
3511 wpa_printf(MSG_DEBUG, "TWT: set parameters - ap_ac_value: %d",
3512 set_params->ap_ac_value);
3513
3514 return 0;
3515 }
3516
3517 /**
3518 * prepare_twt_clear_stats_nlmsg()- prepare twt_session_clear_stats command.
3519 *
3520 * @Param nlmsg: nl command buffer
3521 * @Param cmd: command string
3522 *
3523 * @Returns 0 on Success, -EINVAL on Failure
3524 */
prepare_twt_clear_stats_nlmsg(struct nl_msg * nlmsg,char * cmd)3525 static int prepare_twt_clear_stats_nlmsg(struct nl_msg *nlmsg, char *cmd)
3526 {
3527 struct nlattr *twt_attr;
3528 u8 dialog_id;
3529 int ret = 0;
3530
3531 while(*cmd == ' ')
3532 cmd++;
3533
3534 if (nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_OPERATION,
3535 QCA_WLAN_TWT_CLEAR_STATS)) {
3536 wpa_printf(MSG_DEBUG, "TWT: Failed to put QCA_WLAN_TWT_CLEAR_STATS");
3537 goto fail;
3538 }
3539
3540 twt_attr = nla_nest_start(nlmsg,
3541 QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_PARAMS);
3542 if (!twt_attr)
3543 return -EINVAL;
3544
3545 if (os_strncasecmp(cmd, DIALOG_ID_STR, DIALOG_ID_STR_LEN) == 0) {
3546 cmd += DIALOG_ID_STR_LEN + 1;
3547
3548 dialog_id = get_u8_from_string(cmd, &ret);
3549 if (ret < 0)
3550 return ret;
3551 if (nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_TWT_STATS_FLOW_ID,
3552 dialog_id)) {
3553 wpa_printf(MSG_DEBUG, "TWT: Failed to put dialog_id");
3554 return -EINVAL;
3555 }
3556 wpa_printf(MSG_DEBUG, "TWT: clear_stats dialog_id:%d", dialog_id);
3557 cmd = move_to_next_str(cmd);
3558 } else {
3559 wpa_printf(MSG_DEBUG, "TWT: dialog_id not found");
3560 goto fail;
3561 }
3562
3563 nla_nest_end(nlmsg, twt_attr);
3564 return 0;
3565 fail:
3566 return -EINVAL;
3567 }
3568
3569 /**
3570 * prepare_twt_get_stats_nlmsg()- prepare twt_session_get_stats command.
3571 *
3572 * @Param nlmsg: nl command buffer
3573 * @Param cmd: command string
3574 *
3575 * @Returns 0 on Success, -EINVAL on Failure
3576 */
prepare_twt_get_stats_nlmsg(struct nl_msg * nlmsg,char * cmd)3577 static int prepare_twt_get_stats_nlmsg(struct nl_msg *nlmsg, char *cmd)
3578 {
3579 struct nlattr *twt_attr;
3580 u8 dialog_id;
3581 int ret = 0;
3582
3583 while(*cmd == ' ')
3584 cmd++;
3585
3586 if (nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_OPERATION,
3587 QCA_WLAN_TWT_GET_STATS)) {
3588 wpa_printf(MSG_DEBUG, "TWT: Failed to put QCA_WLAN_TWT_GET_STATS");
3589 goto fail;
3590 }
3591
3592 twt_attr = nla_nest_start(nlmsg,
3593 QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_PARAMS);
3594 if (!twt_attr)
3595 return -EINVAL;
3596
3597 if (os_strncasecmp(cmd, DIALOG_ID_STR, DIALOG_ID_STR_LEN) == 0) {
3598 cmd += DIALOG_ID_STR_LEN + 1;
3599
3600 dialog_id = get_u8_from_string(cmd, &ret);
3601 if (ret < 0)
3602 return ret;
3603 if (nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_TWT_STATS_FLOW_ID,
3604 dialog_id)) {
3605 wpa_printf(MSG_DEBUG, "TWT: Failed to put dialog_id");
3606 return -EINVAL;
3607 }
3608 wpa_printf(MSG_DEBUG, "TWT: get_stats dialog_id:%d", dialog_id);
3609 cmd = move_to_next_str(cmd);
3610 } else {
3611 wpa_printf(MSG_DEBUG, "TWT: dialog_id not found");
3612 goto fail;
3613 }
3614
3615 nla_nest_end(nlmsg, twt_attr);
3616 return 0;
3617 fail:
3618 return -EINVAL;
3619 }
3620
prepare_twt_get_params_nlmsg(struct nl_msg * nlmsg,char * cmd)3621 static int prepare_twt_get_params_nlmsg(struct nl_msg *nlmsg, char *cmd)
3622 {
3623 struct nlattr *twt_attr;
3624 u8 dialog_id;
3625 int ret = 0;
3626 uint8_t peer_mac[MAC_ADDR_LEN];
3627
3628 while(*cmd == ' ')
3629 cmd++;
3630
3631 if (nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_OPERATION,
3632 QCA_WLAN_TWT_GET)) {
3633 wpa_printf(MSG_DEBUG, "TWT: Failed to put QCA_WLAN_TWT_GET");
3634 goto fail;
3635 }
3636
3637 twt_attr = nla_nest_start(nlmsg,
3638 QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_PARAMS);
3639 if (!twt_attr)
3640 return -EINVAL;
3641
3642 if (os_strncasecmp(cmd, DIALOG_ID_STR, DIALOG_ID_STR_LEN) == 0) {
3643 cmd += DIALOG_ID_STR_LEN + 1;
3644
3645 dialog_id = get_u8_from_string(cmd, &ret);
3646 if (ret < 0)
3647 return ret;
3648
3649 if (nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_TWT_SETUP_FLOW_ID,
3650 dialog_id)) {
3651 wpa_printf(MSG_DEBUG, "TWT: Failed to put dialog_id");
3652 return -EINVAL;
3653 }
3654 wpa_printf(MSG_DEBUG, "TWT: get_param dialog_id:%d", dialog_id);
3655 cmd = move_to_next_str(cmd);
3656 } else {
3657 wpa_printf(MSG_ERROR, "TWT: dialog_id not found");
3658 goto fail;
3659 }
3660
3661
3662 if (os_strncasecmp(cmd, MAC_ADDRESS_STR, MAC_ADDR_STR_LEN) == 0) {
3663 cmd += MAC_ADDR_STR_LEN + 1;
3664
3665 if (convert_string_to_bytes(peer_mac, cmd, MAC_ADDR_LEN) !=
3666 MAC_ADDR_LEN) {
3667 wpa_printf(MSG_ERROR, "TWT: invalid mac address");
3668 goto fail;
3669 }
3670
3671 if (nla_put(nlmsg, QCA_WLAN_VENDOR_ATTR_TWT_SETUP_MAC_ADDR,
3672 MAC_ADDR_LEN, peer_mac)) {
3673 wpa_printf(MSG_ERROR, "TWT: Failed to put mac address");
3674 goto fail;
3675 }
3676 }
3677
3678 nla_nest_end(nlmsg, twt_attr);
3679 return 0;
3680 fail:
3681 return -EINVAL;
3682 }
3683
3684 /**
3685 * prepare_twt_get_cap_nlmsg()- processes and packs get capability command.
3686 * The command is expected in below format:
3687 * TWT_GET_CAP
3688 *
3689 * @Param nlmsg: stores the nlmsg
3690 * @Param cmd: expects the command
3691 *
3692 * @Returns 0 on Success, -EINVAL on Failure
3693 */
prepare_twt_get_cap_nlmsg(struct nl_msg * nlmsg,char * cmd)3694 static int prepare_twt_get_cap_nlmsg(struct nl_msg *nlmsg, char *cmd)
3695 {
3696 struct nlattr *twt_attr;
3697
3698 if (nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_OPERATION,
3699 QCA_WLAN_TWT_GET_CAPABILITIES)) {
3700 wpa_printf(MSG_ERROR,"TWT: Failed to put QCA_WLAN_TWT_GET_CAPABILITIES");
3701 goto fail;
3702 }
3703
3704 twt_attr = nla_nest_start(nlmsg,
3705 QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_PARAMS);
3706 if (twt_attr == NULL)
3707 goto fail;
3708 nla_nest_end(nlmsg, twt_attr);
3709 return 0;
3710 fail:
3711 return -EINVAL;
3712 }
3713
pack_nlmsg_twt_params(struct nl_msg * twt_nl_msg,char * cmd,enum qca_wlan_twt_operation type)3714 static int pack_nlmsg_twt_params(struct nl_msg *twt_nl_msg, char *cmd,
3715 enum qca_wlan_twt_operation type)
3716 {
3717 struct nlattr *attr;
3718 int ret = 0;
3719
3720 attr = nla_nest_start(twt_nl_msg, NL80211_ATTR_VENDOR_DATA);
3721 if (attr == NULL)
3722 return -EINVAL;
3723
3724 switch (type) {
3725 case QCA_WLAN_TWT_SET:
3726 {
3727 struct twt_setup_parameters setup_params = {0};
3728
3729 if (process_twt_setup_cmd_string(cmd, &setup_params))
3730 return -EINVAL;
3731 ret = prepare_twt_setup_nlmsg(twt_nl_msg, &setup_params);
3732 break;
3733 }
3734 case QCA_WLAN_TWT_TERMINATE:
3735 ret = prepare_twt_terminate_nlmsg(twt_nl_msg, cmd);
3736 break;
3737 case QCA_WLAN_TWT_SUSPEND:
3738 ret = prepare_twt_pause_nlmsg(twt_nl_msg, cmd);
3739 break;
3740 case QCA_WLAN_TWT_RESUME:
3741 {
3742 struct twt_resume_parameters resume_params = {0};
3743
3744 if (process_twt_resume_cmd_string(cmd, &resume_params))
3745 return -EINVAL;
3746 ret = prepare_twt_resume_nlmsg(twt_nl_msg, &resume_params);
3747 break;
3748 }
3749 case QCA_WLAN_TWT_NUDGE:
3750 {
3751 struct twt_nudge_parameters nudge_params = {0};
3752
3753 if (process_twt_nudge_cmd_string(cmd, &nudge_params))
3754 return -EINVAL;
3755 ret = prepare_twt_nudge_nlmsg(twt_nl_msg, &nudge_params);
3756 break;
3757 }
3758 case QCA_WLAN_TWT_SET_PARAM:
3759 {
3760 struct twt_set_parameters set_params = {0};
3761
3762 if (process_twt_set_param_cmd_string(cmd, &set_params))
3763 return -EINVAL;
3764 ret = prepare_twt_set_param_nlmsg(twt_nl_msg, &set_params);
3765 break;
3766 }
3767 case QCA_WLAN_TWT_GET_CAPABILITIES:
3768 ret = prepare_twt_get_cap_nlmsg(twt_nl_msg, cmd);
3769 break;
3770 case QCA_WLAN_TWT_CLEAR_STATS:
3771 ret = prepare_twt_clear_stats_nlmsg(twt_nl_msg, cmd);
3772 break;
3773 case QCA_WLAN_TWT_GET_STATS:
3774 ret = prepare_twt_get_stats_nlmsg(twt_nl_msg, cmd);
3775 break;
3776 case QCA_WLAN_TWT_GET:
3777 ret = prepare_twt_get_params_nlmsg(twt_nl_msg, cmd);
3778 break;
3779 default:
3780 wpa_printf(MSG_DEBUG, "Unsupported command: %d", type);
3781 ret = -EINVAL;
3782 break;
3783 }
3784
3785 if (!ret)
3786 nla_nest_end(twt_nl_msg, attr);
3787
3788 return ret;
3789 }
3790
result_copy_to_buf(char * src,char * dst_buf,int * dst_len)3791 char *result_copy_to_buf(char *src, char *dst_buf, int *dst_len)
3792 {
3793 int str_len, remaining = 0;
3794
3795 remaining = *dst_len;
3796 str_len = strlen(src);
3797 remaining = remaining - (str_len + 1);
3798
3799 if (remaining <= 0) {
3800 wpa_printf(MSG_ERROR, "destination buffer length not enough");
3801 return NULL;
3802 }
3803 os_memcpy(dst_buf, src, str_len);
3804
3805 *dst_len = remaining;
3806 *(dst_buf + str_len) = ' ';
3807
3808 return (dst_buf + str_len + 1);
3809 }
3810
3811 static int
unpack_twt_get_params_resp(struct nlattr ** tb,char * buf,int buf_len)3812 unpack_twt_get_params_resp(struct nlattr **tb, char *buf, int buf_len)
3813 {
3814 int cmd_id, val, len;
3815 uint8_t exp;
3816 uint32_t value;
3817 unsigned long wake_tsf;
3818 char temp[TWT_RESP_BUF_LEN];
3819 char *start = buf;
3820
3821 os_memset(temp, 0, TWT_RESP_BUF_LEN);
3822
3823 /* Mac Address */
3824 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_MAC_ADDR;
3825 if (!tb[cmd_id]) {
3826 wpa_printf(MSG_ERROR, "twt_get_params resp: no mac_addr");
3827 return -EINVAL;
3828 }
3829 os_snprintf(temp, TWT_RESP_BUF_LEN, "<mac_addr " MAC_ADDR_STR,
3830 MAC_ADDR_ARRAY((char *)nla_data(tb[cmd_id])));
3831 buf = result_copy_to_buf(temp, buf, &buf_len);
3832 if (!buf)
3833 return -EINVAL;
3834
3835 /* Flow Id */
3836 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_FLOW_ID;
3837 if (!tb[cmd_id]) {
3838 wpa_printf(MSG_ERROR, "twt_get_params resp: no dialog_id");
3839 return -EINVAL;
3840 }
3841 val = nla_get_u8(tb[cmd_id]);
3842 os_snprintf(temp, TWT_RESP_BUF_LEN, "dialog_id %d", val);
3843 buf = result_copy_to_buf(temp, buf, &buf_len);
3844 if (!buf)
3845 return -EINVAL;
3846
3847
3848 /* Broadcast */
3849 val = 0;
3850 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_BCAST;
3851 if (tb[cmd_id])
3852 val = nla_get_flag(tb[cmd_id]);
3853 os_snprintf(temp, TWT_RESP_BUF_LEN, "bcast %d", val);
3854 buf = result_copy_to_buf(temp, buf, &buf_len);
3855 if (!buf)
3856 return -EINVAL;
3857
3858 /* Trigger Type */
3859 val = 0;
3860 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_TRIGGER;
3861 if (tb[cmd_id])
3862 val = nla_get_flag(tb[cmd_id]);
3863 os_snprintf(temp, TWT_RESP_BUF_LEN, "trig_type %d", val);
3864 buf = result_copy_to_buf(temp, buf, &buf_len);
3865 if (!buf)
3866 return -EINVAL;
3867
3868 /* Announce */
3869 val = 0;
3870 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_FLOW_TYPE;
3871 if (tb[cmd_id])
3872 val = nla_get_flag(tb[cmd_id]);
3873 os_snprintf(temp, TWT_RESP_BUF_LEN, "flow_type %d", val);
3874 buf = result_copy_to_buf(temp, buf, &buf_len);
3875 if (!buf)
3876 return -EINVAL;
3877
3878 /* Protection */
3879 val = 0;
3880 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_PROTECTION;
3881 if (tb[cmd_id])
3882 val = nla_get_flag(tb[cmd_id]);
3883 os_snprintf(temp, TWT_RESP_BUF_LEN, "protection %d", val);
3884 buf = result_copy_to_buf(temp, buf, &buf_len);
3885 if (!buf)
3886 return -EINVAL;
3887
3888 /* info */
3889 val = 0;
3890 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_TWT_INFO_ENABLED;
3891 if (tb[cmd_id])
3892 val = nla_get_flag(tb[cmd_id]);
3893 os_snprintf(temp, TWT_RESP_BUF_LEN, "info_enabled %d", val);
3894 buf = result_copy_to_buf(temp, buf, &buf_len);
3895 if (!buf)
3896 return -EINVAL;
3897
3898 /* Wake Duration */
3899 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_WAKE_DURATION;
3900 if (!tb[cmd_id]) {
3901 wpa_printf(MSG_ERROR, "twt_get_params resp: no wake duration");
3902 return -EINVAL;
3903 }
3904 value = nla_get_u32(tb[cmd_id]);
3905 os_snprintf(temp, TWT_RESP_BUF_LEN, "wake_dur %d", value);
3906 buf = result_copy_to_buf(temp, buf, &buf_len);
3907 if (!buf)
3908 return -EINVAL;
3909
3910 /* Wake Interval Mantissa */
3911 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_WAKE_INTVL2_MANTISSA;
3912 if (!tb[cmd_id]) {
3913 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_WAKE_INTVL_MANTISSA;
3914 if (!tb[cmd_id]) {
3915 wpa_printf(MSG_ERROR, "twt_get_params resp: no wake mantissa");
3916 return -EINVAL;
3917 }
3918 value = nla_get_u32(tb[cmd_id]);
3919 value = value * TWT_WAKE_INTERVAL_TU_FACTOR;
3920 } else {
3921 value = nla_get_u32(tb[cmd_id]);
3922 }
3923 os_snprintf(temp, TWT_RESP_BUF_LEN, "wake_intvl_mantis %d", value);
3924 buf = result_copy_to_buf(temp, buf, &buf_len);
3925 if (!buf)
3926 return -EINVAL;
3927
3928 /* Wake Interval Exponent */
3929 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_WAKE_INTVL_EXP;
3930 if (!tb[cmd_id]) {
3931 wpa_printf(MSG_ERROR, "twt_get_params resp: no wake intvl exp");
3932 return -EINVAL;
3933 }
3934 exp = nla_get_u8(tb[cmd_id]);
3935 os_snprintf(temp, TWT_RESP_BUF_LEN, "wake_intvl_exp %d", exp);
3936 buf = result_copy_to_buf(temp, buf, &buf_len);
3937 if (!buf)
3938 return -EINVAL;
3939
3940 /* TSF Value */
3941 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_WAKE_TIME_TSF;
3942 if (!tb[cmd_id]) {
3943 wpa_printf(MSG_ERROR, "twt_get_params resp: no wake time tsf");
3944 return -EINVAL;
3945 }
3946 wake_tsf = nla_get_u64(tb[cmd_id]);
3947 os_snprintf(temp, TWT_RESP_BUF_LEN, "wake_time_tsf 0x%lx>", wake_tsf);
3948 buf = result_copy_to_buf(temp, buf, &buf_len);
3949 if (!buf)
3950 return -EINVAL;
3951
3952 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_STATE;
3953 if (!tb[cmd_id]) {
3954 wpa_printf(MSG_ERROR, "twt_get_params resp: no state info");
3955 return -EINVAL;
3956 }
3957 value = nla_get_u32(tb[cmd_id]);
3958 os_snprintf(temp, TWT_RESP_BUF_LEN, "state %d", value);
3959 buf = result_copy_to_buf(temp, buf, &buf_len);
3960 if (!buf)
3961 return -EINVAL;
3962
3963 val = 0;
3964 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_RESPONDER_PM_MODE;
3965 if (tb[cmd_id]) {
3966 val = nla_get_u8(tb[cmd_id]);
3967
3968 os_snprintf(temp, TWT_RESP_BUF_LEN, "responder_pm %d", val);
3969 buf = result_copy_to_buf(temp, buf, &buf_len);
3970 if (!buf)
3971 return -EINVAL;
3972 }
3973
3974 len = (buf - start);
3975 *buf = '\0';
3976
3977 return len;
3978 }
3979
wpa_get_twt_setup_resp_val(struct nlattr ** tb2,char * buf,int buf_len)3980 static int wpa_get_twt_setup_resp_val(struct nlattr **tb2, char *buf,
3981 int buf_len)
3982 {
3983 uint32_t wake_intvl_exp = 0, wake_intvl_mantis = 0;
3984 int cmd_id, val;
3985 uint32_t value;
3986 unsigned long wake_tsf;
3987 char temp[TWT_RESP_BUF_LEN];
3988
3989 buf = result_copy_to_buf(TWT_SETUP_RESP, buf, &buf_len);
3990 if (!buf)
3991 return -EINVAL;
3992
3993 os_memset(temp, 0, TWT_RESP_BUF_LEN);
3994 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_FLOW_ID;
3995 if (!tb2[cmd_id]) {
3996 wpa_printf(MSG_ERROR, "TWT dialog id missing");
3997 return -EINVAL;
3998 }
3999
4000 val = nla_get_u8(tb2[cmd_id]);
4001 os_snprintf(temp, TWT_RESP_BUF_LEN, "dialog_id %d ", val);
4002 buf = result_copy_to_buf(temp, buf, &buf_len);
4003 if (!buf)
4004 return -EINVAL;
4005
4006 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_STATUS;
4007 if (!tb2[cmd_id]) {
4008 wpa_printf(MSG_ERROR, "TWT resp status missing");
4009 return -EINVAL;
4010 }
4011
4012 val = nla_get_u8(tb2[cmd_id]);
4013 os_snprintf(temp, TWT_RESP_BUF_LEN, "status %d ", val);
4014 buf = result_copy_to_buf(temp, buf, &buf_len);
4015 if (!buf)
4016 return -EINVAL;
4017
4018 os_memset(temp, 0, TWT_RESP_BUF_LEN);
4019 os_snprintf(temp, TWT_RESP_BUF_LEN, "(%s)", twt_status_to_string(val));
4020 buf = result_copy_to_buf(temp, buf, &buf_len);
4021 if (!buf)
4022 return -EINVAL;
4023
4024 /* In case of failure only status is updated */
4025 if (val != QCA_WLAN_VENDOR_TWT_STATUS_OK) {
4026 *buf = '\0';
4027 return 0;
4028 }
4029
4030 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_RESP_TYPE;
4031 if (!tb2[cmd_id]) {
4032 wpa_printf(MSG_ERROR, "TWT resp type missing");
4033 return -EINVAL;
4034 }
4035 val = nla_get_u8(tb2[cmd_id]);
4036 os_snprintf(temp, TWT_RESP_BUF_LEN, "resp_reason %d ", val);
4037 buf = result_copy_to_buf(temp, buf, &buf_len);
4038 if (!buf)
4039 return -EINVAL;
4040
4041 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_WAKE_INTVL_EXP;
4042 if (!tb2[cmd_id]) {
4043 wpa_printf(MSG_ERROR, "TWT_SETUP_WAKE_INTVL_EXP is must");
4044 return -EINVAL;
4045 }
4046 wake_intvl_exp = nla_get_u8(tb2[cmd_id]);
4047 os_snprintf(temp, TWT_RESP_BUF_LEN, "wake_intvl_exp %d ",
4048 wake_intvl_exp);
4049 buf = result_copy_to_buf(temp, buf, &buf_len);
4050 if (!buf)
4051 return -EINVAL;
4052
4053 val = 0;
4054 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_BCAST;
4055 if (tb2[cmd_id])
4056 val = nla_get_flag(tb2[cmd_id]);
4057
4058 os_snprintf(temp, TWT_RESP_BUF_LEN, "bcast %d", val);
4059 buf = result_copy_to_buf(temp, buf, &buf_len);
4060 if (!buf)
4061 return -EINVAL;
4062
4063 val = 0;
4064 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_TRIGGER;
4065 if (tb2[cmd_id])
4066 val = nla_get_flag(tb2[cmd_id]);
4067
4068 os_snprintf(temp, TWT_RESP_BUF_LEN, "trig_type %d", val);
4069 buf = result_copy_to_buf(temp, buf, &buf_len);
4070 if (!buf)
4071 return -EINVAL;
4072
4073 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_FLOW_TYPE;
4074 if (!tb2[cmd_id]) {
4075 wpa_printf(MSG_ERROR, "TWT_SETUP_FLOW_TYPE is must");
4076 return -EINVAL;
4077 }
4078 val = nla_get_u8(tb2[cmd_id]);
4079 os_snprintf(temp, TWT_RESP_BUF_LEN, "flow_type %d", val);
4080 buf = result_copy_to_buf(temp, buf, &buf_len);
4081 if (!buf)
4082 return -EINVAL;
4083
4084 val = 0;
4085 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_PROTECTION;
4086 if (tb2[cmd_id])
4087 val = nla_get_flag(tb2[cmd_id]);
4088 os_snprintf(temp, TWT_RESP_BUF_LEN, "protection %d", val);
4089 buf = result_copy_to_buf(temp, buf, &buf_len);
4090 if (!buf)
4091 return -EINVAL;
4092
4093 value = 0;
4094 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_WAKE_TIME;
4095 if (tb2[cmd_id])
4096 value = nla_get_u32(tb2[cmd_id]);
4097 os_snprintf(temp, TWT_RESP_BUF_LEN, "wake_time 0x%x", value);
4098 buf = result_copy_to_buf(temp, buf, &buf_len);
4099 if (!buf)
4100 return -EINVAL;
4101
4102 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_WAKE_DURATION;
4103 if (!tb2[cmd_id]) {
4104 wpa_printf(MSG_ERROR, "TWT_SETUP_WAKE_DURATION is must");
4105 return -EINVAL;
4106 }
4107 value = nla_get_u32(tb2[cmd_id]);
4108 os_snprintf(temp, TWT_RESP_BUF_LEN, "wake_dur %d", value);
4109 buf = result_copy_to_buf(temp, buf, &buf_len);
4110 if (!buf)
4111 return -EINVAL;
4112
4113 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_WAKE_INTVL2_MANTISSA;
4114 if (!tb2[cmd_id]) {
4115 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_WAKE_INTVL_MANTISSA;
4116 if (!tb2[cmd_id]) {
4117 wpa_printf(MSG_ERROR, "SETUP_WAKE_INTVL_MANTISSA is must");
4118 return -EINVAL;
4119 }
4120 wake_intvl_mantis = nla_get_u32(tb2[cmd_id]);
4121 wake_intvl_mantis = wake_intvl_mantis * TWT_WAKE_INTERVAL_TU_FACTOR;
4122 } else {
4123 wake_intvl_mantis = nla_get_u32(tb2[cmd_id]);
4124 }
4125 os_snprintf(temp, TWT_RESP_BUF_LEN, "wake_intvl %d", wake_intvl_mantis);
4126 buf = result_copy_to_buf(temp, buf, &buf_len);
4127 if (!buf)
4128 return -EINVAL;
4129
4130 wake_tsf = 0;
4131 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_WAKE_TIME_TSF;
4132 if (tb2[cmd_id])
4133 wake_tsf = nla_get_u64(tb2[cmd_id]);
4134 os_snprintf(temp, TWT_RESP_BUF_LEN, "wake_tsf 0x%lx", wake_tsf);
4135 buf = result_copy_to_buf(temp, buf, &buf_len);
4136 if (!buf)
4137 return -EINVAL;
4138
4139 val = 0;
4140 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_TWT_INFO_ENABLED;
4141 if (tb2[cmd_id])
4142 val = nla_get_flag(tb2[cmd_id]);
4143
4144 os_snprintf(temp, TWT_RESP_BUF_LEN, "info_enabled %d", val);
4145 buf = result_copy_to_buf(temp, buf, &buf_len);
4146 if (!buf)
4147 return -EINVAL;
4148
4149 val = 0;
4150 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_RESPONDER_PM_MODE;
4151 if (tb2[cmd_id]) {
4152 val = nla_get_u8(tb2[cmd_id]);
4153
4154 os_snprintf(temp, TWT_RESP_BUF_LEN, "responder_pm %d", val);
4155 buf = result_copy_to_buf(temp, buf, &buf_len);
4156 if (!buf)
4157 return -EINVAL;
4158 }
4159
4160 *buf = '\0';
4161
4162 return 0;
4163 }
4164
4165 /**
4166 * unpack_twt_get_params_nlmsg()- unpacks and prints the twt get_param
4167 * response recieved from driver synchronously for twt_session_get_params.
4168 *
4169 * @Param tb: vendor nl data
4170 * @Param buf: stores the response
4171 * @Param buf_len: length of the response buffer
4172 *
4173 * @Returns 0 on Success, -1 on Failure
4174 */
unpack_twt_get_params_nlmsg(struct nl_msg ** tb,char * buf,int buf_len)4175 static int unpack_twt_get_params_nlmsg(struct nl_msg **tb, char *buf, int buf_len)
4176 {
4177 int ret, rem, id, len = 0, num_twt_sessions = 0;
4178 struct nlattr *config_attr[QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_MAX + 1];
4179 struct nlattr *setup_attr[QCA_WLAN_VENDOR_ATTR_TWT_SETUP_MAX + 1];
4180 struct nlattr *attr;
4181
4182 if (nla_parse_nested(config_attr, QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_MAX,
4183 tb[NL80211_ATTR_VENDOR_DATA], NULL)) {
4184 wpa_printf(MSG_ERROR, "twt_get_params: nla_parse_nested fail");
4185 return -EINVAL;
4186 }
4187
4188 id = QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_PARAMS;
4189 attr = config_attr[id];
4190 if (!attr) {
4191 wpa_printf(MSG_ERROR, "twt_get_params: config_twt_params fail");
4192 return -EINVAL;
4193 }
4194
4195 num_twt_sessions = 0;
4196 nla_for_each_nested(attr, config_attr[id], rem) {
4197 num_twt_sessions++;
4198 if (nla_parse(setup_attr, QCA_WLAN_VENDOR_ATTR_TWT_SETUP_MAX,
4199 nla_data(attr), nla_len(attr), NULL)) {
4200 wpa_printf(MSG_ERROR, "twt_get_params: nla_parse fail");
4201 return -EINVAL;
4202 }
4203 ret = unpack_twt_get_params_resp(setup_attr, buf + len,
4204 buf_len - len);
4205 if (ret < 0)
4206 return ret;
4207 len += ret;
4208 }
4209
4210 wpa_printf(MSG_ERROR, "twt_get_params: number of twt sessions = %d",
4211 num_twt_sessions);
4212
4213 return 0;
4214 }
4215
4216 /**
4217 * wpa_get_twt_stats_resp_val()- parse twt get_stats response
4218 * recieved from driver synchronously for twt_session_get_stats.
4219 *
4220 * @Param tb: vendor nl data
4221 * @Param buf: stores the response
4222 * @Param buf_len: length of the response buffer
4223 *
4224 * @Returns 0 on Success, -1 on Failure
4225 */
wpa_get_twt_stats_resp_val(struct nlattr ** tb2,char * buf,int buf_len)4226 static int wpa_get_twt_stats_resp_val(struct nlattr **tb2, char *buf,
4227 int buf_len)
4228 {
4229 int cmd_id, len;
4230 u32 val = 0;
4231 u8 val1 = 0;
4232 char temp[TWT_RESP_BUF_LEN];
4233 char *start = buf;
4234
4235 os_memset(temp, 0, TWT_RESP_BUF_LEN);
4236
4237 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_STATS_FLOW_ID;
4238 if (!tb2[cmd_id]) {
4239 wpa_printf(MSG_ERROR,"%s TWT stats flow id missing", __func__);
4240 } else {
4241 val1 = nla_get_u8(tb2[cmd_id]);
4242 os_memset(temp, 0, TWT_RESP_BUF_LEN);
4243 os_snprintf(temp, TWT_RESP_BUF_LEN, "flow_id %u", val1);
4244 buf = result_copy_to_buf(temp, buf, &buf_len);
4245 if (!buf)
4246 return -EINVAL;
4247 wpa_printf(MSG_INFO,"TWT stats flow id : %u", val1);
4248 }
4249
4250 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_STATS_NUM_SP_ITERATIONS;
4251 if (!tb2[cmd_id]) {
4252 wpa_printf(MSG_ERROR,"%s TWT stats num sp iterations missing", __func__);
4253 } else {
4254 val = nla_get_u32(tb2[cmd_id]);
4255 os_memset(temp, 0, TWT_RESP_BUF_LEN);
4256 os_snprintf(temp, TWT_RESP_BUF_LEN, "num_sp_iteration %u", val);
4257 buf = result_copy_to_buf(temp, buf, &buf_len);
4258 if (!buf)
4259 return -EINVAL;
4260 wpa_printf(MSG_INFO,"TWT num sp Iterations : %u", val);
4261 }
4262
4263 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_STATS_MIN_WAKE_DURATION;
4264 if (!tb2[cmd_id]) {
4265 wpa_printf(MSG_ERROR,"%s TWT stats min wake duration missing", __func__);
4266 } else {
4267 val = nla_get_u32(tb2[cmd_id]);
4268 os_memset(temp, 0, TWT_RESP_BUF_LEN);
4269 os_snprintf(temp, TWT_RESP_BUF_LEN, "min_wake_dur %u", val);
4270 buf = result_copy_to_buf(temp, buf, &buf_len);
4271 if (!buf)
4272 return -EINVAL;
4273 wpa_printf(MSG_INFO,"TWT min wake duration : %u", val);
4274 }
4275
4276 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_STATS_MAX_WAKE_DURATION;
4277 if (!tb2[cmd_id]) {
4278 wpa_printf(MSG_ERROR,"%s TWT stats max wake duration missing", __func__);
4279 } else {
4280 val = nla_get_u32(tb2[cmd_id]);
4281 os_memset(temp, 0, TWT_RESP_BUF_LEN);
4282 os_snprintf(temp, TWT_RESP_BUF_LEN, "max_wake_dur %u", val);
4283 buf = result_copy_to_buf(temp, buf, &buf_len);
4284 if (!buf)
4285 return -EINVAL;
4286 wpa_printf(MSG_INFO,"TWT Max wake duration : %u", val);
4287 }
4288
4289 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_STATS_SESSION_WAKE_DURATION;
4290 if (!tb2[cmd_id]) {
4291 wpa_printf(MSG_ERROR,"%s TWT stats sess_wake_dur missing", __func__);
4292 return -EINVAL;
4293 } else {
4294 val = nla_get_u32(tb2[cmd_id]);
4295 os_memset(temp, 0, TWT_RESP_BUF_LEN);
4296 os_snprintf(temp, TWT_RESP_BUF_LEN, "session_wake_dur %u", val);
4297 buf = result_copy_to_buf(temp, buf, &buf_len);
4298 if (!buf)
4299 return -EINVAL;
4300 wpa_printf(MSG_INFO,"TWT stats session wake duration : %u", val);
4301 }
4302
4303 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_STATS_AVG_WAKE_DURATION;
4304 if (!tb2[cmd_id]) {
4305 wpa_printf(MSG_ERROR,"%s TWT stats avg_wake_dur missing", __func__);
4306 return -EINVAL;
4307 } else {
4308 val = nla_get_u32(tb2[cmd_id]);
4309 os_memset(temp, 0, TWT_RESP_BUF_LEN);
4310 os_snprintf(temp, TWT_RESP_BUF_LEN, "avg_wake_dur %u", val);
4311 buf = result_copy_to_buf(temp, buf, &buf_len);
4312 if (!buf)
4313 return -EINVAL;
4314 wpa_printf(MSG_INFO,"TWT stats avg wake duration : %u", val);
4315 }
4316
4317 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_STATS_AVERAGE_TX_MPDU;
4318 if (!tb2[cmd_id]) {
4319 wpa_printf(MSG_ERROR,"%s TWT stats average tx mpdu missing", __func__);
4320 return -EINVAL;
4321 } else {
4322 val = nla_get_u32(tb2[cmd_id]);
4323 os_memset(temp, 0, TWT_RESP_BUF_LEN);
4324 os_snprintf(temp, TWT_RESP_BUF_LEN, "tx_mpdu %u", val);
4325 buf = result_copy_to_buf(temp, buf, &buf_len);
4326 if (!buf)
4327 return -EINVAL;
4328 wpa_printf(MSG_INFO,"TWT stats average tx mpdu : %u", val);
4329 }
4330
4331 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_STATS_AVERAGE_RX_MPDU;
4332 if (!tb2[cmd_id]) {
4333 wpa_printf(MSG_ERROR,"%s TWT stats average rx mpdu missing", __func__);
4334 return -EINVAL;
4335 } else {
4336 val = nla_get_u32(tb2[cmd_id]);
4337 os_memset(temp, 0, TWT_RESP_BUF_LEN);
4338 os_snprintf(temp, TWT_RESP_BUF_LEN, "rx_mpdu %u", val);
4339 buf = result_copy_to_buf(temp, buf, &buf_len);
4340 if (!buf)
4341 return -EINVAL;
4342 wpa_printf(MSG_INFO,"TWT stats average rx mpdu : %u", val);
4343 }
4344
4345 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_STATS_AVERAGE_TX_PACKET_SIZE;
4346 if (!tb2[cmd_id]) {
4347 wpa_printf(MSG_ERROR,"%s TWT stats average tx packet size missing", __func__);
4348 return -EINVAL;
4349 } else {
4350 val = nla_get_u32(tb2[cmd_id]);
4351 os_memset(temp, 0, TWT_RESP_BUF_LEN);
4352 os_snprintf(temp, TWT_RESP_BUF_LEN, "tx_pkt_size %u", val);
4353 buf = result_copy_to_buf(temp, buf, &buf_len);
4354 if (!buf)
4355 return -EINVAL;
4356 wpa_printf(MSG_INFO,"TWT stats average tx packet size : %u", val);
4357 }
4358
4359 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_STATS_AVERAGE_RX_PACKET_SIZE;
4360 if (!tb2[cmd_id]) {
4361 wpa_printf(MSG_ERROR,"%s TWT stats average rx packet size missing", __func__);
4362 return -EINVAL;
4363 } else {
4364 val = nla_get_u32(tb2[cmd_id]);
4365 os_memset(temp, 0, TWT_RESP_BUF_LEN);
4366 os_snprintf(temp, TWT_RESP_BUF_LEN, "rx_pkt_size %u", val);
4367 buf = result_copy_to_buf(temp, buf, &buf_len);
4368 if (!buf)
4369 return -EINVAL;
4370 wpa_printf(MSG_INFO,"TWT stats average rx packet size : %u", val);
4371 }
4372 len = (buf - start);
4373 *buf = '\0';
4374
4375 return len;
4376 }
4377
4378 /**
4379 * unpack_twt_get_stats_nlmsg()- unpacks and prints the twt get_stats
4380 * response recieved from driver synchronously for twt_session_get_stats.
4381 *
4382 * @Param tb: vendor nl data
4383 * @Param buf: stores the response
4384 * @Param buf_len: length of the response buffer
4385 *
4386 * @Returns 0 on Success, -1 on Failure
4387 */
4388 static
unpack_twt_get_stats_nlmsg(struct nl_msg ** tb,char * buf,int buf_len)4389 int unpack_twt_get_stats_nlmsg(struct nl_msg **tb, char *buf, int buf_len)
4390 {
4391 int ret, rem, id, len = 0, num_twt_sessions = 0;
4392 struct nlattr *config_attr[QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_MAX + 1];
4393 struct nlattr *setup_attr[QCA_WLAN_VENDOR_ATTR_TWT_SETUP_MAX + 1];
4394 struct nlattr *attr;
4395
4396 if (nla_parse_nested(config_attr, QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_MAX,
4397 tb[NL80211_ATTR_VENDOR_DATA], NULL)) {
4398 wpa_printf(MSG_ERROR, "twt_get_stats: nla_parse_nested fail");
4399 return -EINVAL;
4400 }
4401
4402 id = QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_PARAMS;
4403 attr = config_attr[id];
4404 if (!attr) {
4405 wpa_printf(MSG_ERROR, "twt_get_stats: config_twt_params fail");
4406 return -EINVAL;
4407 }
4408
4409 num_twt_sessions = 0;
4410 nla_for_each_nested(attr, config_attr[id], rem) {
4411 num_twt_sessions++;
4412 if (nla_parse(setup_attr, QCA_WLAN_VENDOR_ATTR_TWT_SETUP_MAX,
4413 nla_data(attr), nla_len(attr), NULL)) {
4414 wpa_printf(MSG_ERROR, "twt_get_stats: nla_parse fail");
4415 return -EINVAL;
4416 }
4417 ret = wpa_get_twt_stats_resp_val(setup_attr, buf + len,
4418 buf_len - len);
4419 if (ret < 0)
4420 return ret;
4421 len += ret;
4422 }
4423 wpa_printf(MSG_INFO,"twt_get_stats: number of twt sessions = %d", num_twt_sessions);
4424
4425 return 0;
4426 }
4427
wpa_get_twt_capabilities_resp_val(struct nlattr ** tb2,char * buf,int buf_len)4428 static int wpa_get_twt_capabilities_resp_val(struct nlattr **tb2, char *buf,
4429 int buf_len)
4430 {
4431 int cmd_id;
4432 u16 msb, lsb;
4433 u32 val;
4434 char temp[TWT_RESP_BUF_LEN];
4435
4436 os_memset(temp, 0, TWT_RESP_BUF_LEN);
4437
4438 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_CAPABILITIES_SELF;
4439 if (!tb2[cmd_id]) {
4440 wpa_printf(MSG_INFO,"%s TWT self capabilities missing", __func__);
4441 return -EINVAL;
4442 } else {
4443 msb = nla_get_u16(tb2[cmd_id]);
4444 }
4445
4446 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_CAPABILITIES_PEER;
4447 if (!tb2[cmd_id]) {
4448 wpa_printf(MSG_INFO,"%s TWT peer capabilities missing", __func__);
4449 return -EINVAL;
4450 } else {
4451 lsb = nla_get_u16(tb2[cmd_id]);
4452 }
4453 wpa_printf(MSG_INFO,"TWT self_capab:%d, TWT peer_capab:%d", msb, lsb);
4454 val = (msb << 16) | lsb;
4455 os_snprintf(temp, TWT_RESP_BUF_LEN, "0x%x", val);
4456 buf = result_copy_to_buf(temp, buf, &buf_len);
4457 if (!buf)
4458 return -EINVAL;
4459 *buf = '\0';
4460
4461 return 0;
4462 }
4463
4464 /**
4465 * unpack_twt_get_capab_nlmsg()- unpacks and prints the twt get capabilities
4466 * response recieved from driver synchronously for TWT_GET_CAP command.
4467 * The response is printed in below hex-format:
4468 * 0xHIGHLOW
4469 * HIGH: Self capabilities
4470 * LOW: Peer capabilities
4471 *
4472 * @Param tb: vendor nl data
4473 * @Param buf: stores the response
4474 * @Param buf_len: length of the response buffer
4475 *
4476 * @Returns 0 on Success, -1 on Failure
4477 */
unpack_twt_get_capab_nlmsg(struct nl_msg ** tb,char * buf,int buf_len)4478 static int unpack_twt_get_capab_nlmsg(struct nl_msg **tb, char *buf, int buf_len)
4479 {
4480 int ret, id;
4481 struct nlattr *config_attr[QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_MAX + 1];
4482 struct nlattr *setup_attr[QCA_WLAN_VENDOR_ATTR_TWT_SETUP_MAX + 1];
4483 struct nlattr *attr;
4484
4485 if (nla_parse_nested(config_attr, QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_MAX,
4486 tb[NL80211_ATTR_VENDOR_DATA], NULL)) {
4487 wpa_printf(MSG_ERROR, "twt_get_capability: nla_parse_nested fail");
4488 return -EINVAL;
4489 }
4490
4491 id = QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_PARAMS;
4492 attr = config_attr[id];
4493 if (!attr) {
4494 wpa_printf(MSG_ERROR, "twt_get_capability: config_twt_params fail");
4495 return -EINVAL;
4496 }
4497
4498 if (nla_parse(setup_attr, QCA_WLAN_VENDOR_ATTR_TWT_SETUP_MAX,
4499 nla_data(attr), nla_len(attr), NULL)) {
4500 wpa_printf(MSG_ERROR, "twt_get_capability: nla_parse fail");
4501 return -EINVAL;
4502 }
4503
4504 ret = wpa_get_twt_capabilities_resp_val(setup_attr, buf, buf_len);
4505 return ret;
4506 }
4507
4508 /**
4509 * unpack_twt_setup_nlmsg()- unpacks twt_session_setup response recieved
4510 * The response is printed in below format:
4511 * CTRL-EVENT-TWT SETUP dialog_id <dialog_id> status <status> ..
4512 *
4513 * @Param tb: vendor nl data
4514 * @Param buf: stores the response
4515 * @Param buf_len: length of the response buffer
4516 *
4517 * @Returns 0 on Success, -EINVAL on invalid response
4518 */
unpack_twt_setup_nlmsg(struct nlattr ** tb,char * buf,int buf_len)4519 static int unpack_twt_setup_nlmsg(struct nlattr **tb, char *buf, int buf_len)
4520 {
4521 int ret = 0;
4522 struct nlattr *tb2[QCA_WLAN_VENDOR_ATTR_TWT_SETUP_MAX + 1];
4523
4524 if (nla_parse_nested(tb2, QCA_WLAN_VENDOR_ATTR_TWT_SETUP_MAX,
4525 tb[QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_PARAMS], NULL)) {
4526 wpa_printf(MSG_ERROR, "nla_parse failed\n");
4527 return -EINVAL;
4528 }
4529
4530 ret = wpa_get_twt_setup_resp_val(tb2, buf, buf_len);
4531
4532 return ret;
4533 }
4534
unpack_nlmsg_twt_params(struct nl_msg * twt_nl_msg,enum qca_wlan_twt_operation type,char * buf,int buf_len)4535 static int unpack_nlmsg_twt_params(struct nl_msg *twt_nl_msg,
4536 enum qca_wlan_twt_operation type,
4537 char *buf, int buf_len)
4538 {
4539 int ret = 0;
4540 struct nlattr *tb[NL80211_ATTR_MAX + 1];
4541 struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(twt_nl_msg));
4542
4543 nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
4544 genlmsg_attrlen(gnlh, 0), NULL);
4545
4546 switch (type) {
4547 case QCA_WLAN_TWT_GET:
4548 ret = unpack_twt_get_params_nlmsg(tb, buf, buf_len);
4549 break;
4550 case QCA_WLAN_TWT_GET_STATS:
4551 ret = unpack_twt_get_stats_nlmsg(tb, buf, buf_len);
4552 break;
4553 case QCA_WLAN_TWT_GET_CAPABILITIES:
4554 ret = unpack_twt_get_capab_nlmsg(tb, buf, buf_len);
4555 break;
4556 default:
4557 wpa_printf(MSG_DEBUG, "Unsupported command: %d", type);
4558 ret = -EINVAL;
4559 break;
4560 }
4561
4562 return ret;
4563 }
4564
twt_response_handler(struct nl_msg * msg,void * arg)4565 static int twt_response_handler(struct nl_msg *msg, void *arg)
4566 {
4567 struct twt_resp_info *info = (struct twt_resp_info *) arg;
4568 struct wpa_driver_nl80211_data *drv = NULL;
4569 int ret;
4570
4571 drv = info->drv;
4572 ret = unpack_nlmsg_twt_params(msg, info->twt_oper, info->reply_buf,
4573 info->reply_buf_len);
4574 wpa_printf(MSG_DEBUG, "%s - twt_oper %d", __func__, info->twt_oper);
4575 if (!ret)
4576 wpa_msg(drv->ctx, MSG_INFO,
4577 TWT_CTRL_EVENT " %s : OK", info->reply_buf);
4578 else
4579 wpa_msg(drv->ctx, MSG_INFO,
4580 TWT_CTRL_EVENT " %s : Error = %d",
4581 info->reply_buf, ret);
4582
4583 return ret;
4584 }
4585
4586 struct features_info {
4587 u8 *flags;
4588 size_t flags_len;
4589 };
4590
4591 static
check_feature(enum qca_wlan_vendor_features feature,struct features_info * info)4592 int check_feature(enum qca_wlan_vendor_features feature,
4593 struct features_info *info)
4594 {
4595 size_t idx = feature / 8;
4596
4597 return (idx < info->flags_len) &&
4598 (info->flags[idx] & BIT(feature % 8));
4599 }
4600
4601 /* features_info_handler() - parse sync response for get_feature cmd
4602 *
4603 * @param msg: nl_msg buffer
4604 * @Param arg: feature infor
4605 *
4606 * @Returns 0 on Success, error code on invalid response
4607 */
4608 static
features_info_handler(struct nl_msg * msg,void * arg)4609 int features_info_handler(struct nl_msg *msg, void *arg)
4610 {
4611 struct genlmsghdr *mHeader;
4612 struct nlattr *mAttributes[NL80211_ATTR_MAX_INTERNAL + 1];
4613 struct nlattr *vendata, *attr;
4614 int datalen;
4615
4616 struct features_info *info = (struct features_info *) arg;
4617 int status = 0;
4618
4619
4620 mHeader = (struct genlmsghdr *)nlmsg_data(nlmsg_hdr(msg));
4621 nla_parse(mAttributes, NL80211_ATTR_MAX_INTERNAL,
4622 genlmsg_attrdata(mHeader, 0),
4623 genlmsg_attrlen(mHeader, 0), NULL);
4624
4625 if (mAttributes[NL80211_ATTR_VENDOR_DATA]) {
4626 vendata = nla_data(mAttributes[NL80211_ATTR_VENDOR_DATA]);
4627 datalen = nla_len(mAttributes[NL80211_ATTR_VENDOR_DATA]);
4628 if (!vendata) {
4629 wpa_printf(MSG_ERROR,"Vendor data not found");
4630 return -1;
4631 }
4632 struct nlattr *tb_vendor[QCA_WLAN_VENDOR_ATTR_MAX + 1];
4633
4634 nla_parse(tb_vendor, QCA_WLAN_VENDOR_ATTR_MAX,
4635 vendata, datalen, NULL);
4636
4637 attr = tb_vendor[QCA_WLAN_VENDOR_ATTR_FEATURE_FLAGS];
4638 if (attr) {
4639 int len = nla_len(attr);
4640 info->flags = os_malloc(len);
4641 if (info->flags != NULL) {
4642 os_memcpy(info->flags, nla_data(attr), len);
4643 info->flags_len = len;
4644 }
4645 } else {
4646 wpa_printf(MSG_ERROR,"VENDOR_ATTR_FEATURE_FLAGS not found");
4647 }
4648 } else {
4649 wpa_printf(MSG_ERROR,"NL80211_ATTR_VENDOR_DATA not found");
4650 status = -1;
4651 }
4652
4653 return status;
4654 }
4655
4656 /* pack_nlmsg_vendor_feature_hdr() - pack get_features command
4657 *
4658 * @param drv_nl_msg: nl_msg buffer
4659 * @Param drv: nl data
4660 * @Param ifname: interface name
4661 *
4662 * @Returns 0 on Success, error code on invalid response
4663 */
4664 static
pack_nlmsg_vendor_feature_hdr(struct nl_msg * drv_nl_msg,struct wpa_driver_nl80211_data * drv,char * ifname)4665 int pack_nlmsg_vendor_feature_hdr(struct nl_msg *drv_nl_msg,
4666 struct wpa_driver_nl80211_data *drv,
4667 char *ifname)
4668 {
4669 int ret;
4670 int ifindex;
4671
4672 genlmsg_put(drv_nl_msg, NL_AUTO_PORT, NL_AUTO_SEQ,
4673 drv->global->nl80211_id, 0, 0,
4674 NL80211_CMD_VENDOR, 0);
4675
4676 ret = nla_put_u32(drv_nl_msg, NL80211_ATTR_VENDOR_ID, OUI_QCA);
4677 if (ret < 0) {
4678 wpa_printf(MSG_ERROR, "Failed to put vendor id");
4679 return ret;
4680 }
4681
4682 ret = nla_put_u32(drv_nl_msg, NL80211_ATTR_VENDOR_SUBCMD,
4683 QCA_NL80211_VENDOR_SUBCMD_GET_FEATURES);
4684 if (ret < 0) {
4685 wpa_printf(MSG_DEBUG, "nl put twt vendor subcmd failed");
4686 return ret;
4687 }
4688
4689 if (ifname && (strlen(ifname) > 0))
4690 ifindex = if_nametoindex(ifname);
4691 else
4692 ifindex = if_nametoindex(DEFAULT_IFNAME);
4693
4694 ret = nla_put_u32(drv_nl_msg, NL80211_ATTR_IFINDEX, ifindex);
4695 if (ret < 0) {
4696 wpa_printf(MSG_DEBUG, "nl put iface: %s failed", ifname);
4697 return ret;
4698 }
4699 return ret;
4700 }
4701
4702 /* check_wifi_twt_async_feature() - check if driver supports twt async feature
4703 *
4704 * @Param drv: nl data
4705 * @Param ifname: interface name
4706 *
4707 * @Returns 1 if twt async feature is supported, 0 otherwise
4708 */
check_wifi_twt_async_feature(struct wpa_driver_nl80211_data * drv,char * ifname)4709 static int check_wifi_twt_async_feature(struct wpa_driver_nl80211_data *drv,
4710 char *ifname)
4711 {
4712 struct nl_msg *nlmsg;
4713 struct features_info info;
4714 int ret;
4715
4716 if (twt_async_support != -1) {
4717 return twt_async_support;
4718 }
4719
4720 nlmsg = nlmsg_alloc();
4721 if (!nlmsg) {
4722 wpa_printf(MSG_ERROR, "nlmg alloc failure");
4723 return -ENOMEM;
4724 }
4725
4726 pack_nlmsg_vendor_feature_hdr(nlmsg, drv, ifname);
4727 os_memset(&info, 0, sizeof(info));
4728 ret = send_nlmsg((struct nl_sock *)drv->global->nl, nlmsg,
4729 features_info_handler, &info);
4730
4731 if (ret || !info.flags) {
4732 nlmsg_free(nlmsg);
4733 return 0;
4734 }
4735
4736 if (check_feature(QCA_WLAN_VENDOR_FEATURE_TWT_ASYNC_SUPPORT, &info)) {
4737 twt_async_support = 1;
4738 } else {
4739 twt_async_support = 0;
4740 }
4741
4742 os_free(info.flags);
4743 return twt_async_support;
4744 }
4745
wpa_driver_twt_cmd_handler(struct wpa_driver_nl80211_data * drv,char * ifname,enum qca_wlan_twt_operation twt_oper,char * param,char * buf,size_t buf_len,int * status)4746 static int wpa_driver_twt_cmd_handler(struct wpa_driver_nl80211_data *drv,
4747 char *ifname,
4748 enum qca_wlan_twt_operation twt_oper,
4749 char *param, char *buf,
4750 size_t buf_len, int *status)
4751 {
4752 struct nl_msg *twt_nl_msg;
4753 struct twt_resp_info reply_info;
4754 int ret = 0;
4755
4756 if (!param) {
4757 wpa_printf(MSG_ERROR, "%s:TWT cmd args missing\n", __func__);
4758 return -EINVAL;
4759 }
4760
4761 if (!buf) {
4762 wpa_printf(MSG_ERROR, "buf is NULL");
4763 return -EINVAL;
4764 }
4765
4766 wpa_printf(MSG_DEBUG, "TWT DRIVER cmd: %s", param);
4767
4768 memset(&reply_info, 0, sizeof(struct twt_resp_info));
4769 os_memset(buf, 0, buf_len);
4770
4771 reply_info.twt_oper = twt_oper;
4772 reply_info.reply_buf = buf;
4773 reply_info.reply_buf_len = buf_len;
4774 reply_info.drv = drv;
4775
4776 twt_nl_msg = prepare_nlmsg(drv, ifname, NL80211_CMD_VENDOR,
4777 QCA_NL80211_VENDOR_SUBCMD_CONFIG_TWT, 0);
4778 if (!twt_nl_msg) {
4779 ret = -EINVAL;
4780 goto err_msg;
4781 }
4782
4783 ret = pack_nlmsg_twt_params(twt_nl_msg, param, twt_oper);
4784 if (ret) {
4785 nlmsg_free(twt_nl_msg);
4786 goto err_msg;
4787 }
4788
4789 switch(twt_oper) {
4790 case QCA_WLAN_TWT_GET:
4791 case QCA_WLAN_TWT_GET_CAPABILITIES:
4792 case QCA_WLAN_TWT_GET_STATS:
4793 *status = send_nlmsg((struct nl_sock *)drv->global->nl,
4794 twt_nl_msg, twt_response_handler,
4795 &reply_info);
4796 if (*status != 0) {
4797 wpa_printf(MSG_ERROR, "Failed to send nlmsg - err %d", *status);
4798 ret = -EINVAL;
4799 }
4800 break;
4801 case QCA_WLAN_TWT_CLEAR_STATS:
4802 *status = send_nlmsg((struct nl_sock *)drv->global->nl,
4803 twt_nl_msg, NULL, NULL);
4804 if (*status != 0) {
4805 wpa_printf(MSG_ERROR, "Failed to send nlmsg - err %d", *status);
4806 ret = -EINVAL;
4807 }
4808 break;
4809 case QCA_WLAN_TWT_SET:
4810 case QCA_WLAN_TWT_TERMINATE:
4811 case QCA_WLAN_TWT_SUSPEND:
4812 case QCA_WLAN_TWT_RESUME:
4813 case QCA_WLAN_TWT_NUDGE:
4814 case QCA_WLAN_TWT_SET_PARAM:
4815 if(check_wifi_twt_async_feature(drv, ifname) == 0) {
4816 wpa_printf(MSG_ERROR, "Asynchronous TWT Feature is missing");
4817 ret = -EINVAL;
4818 } else {
4819 *status = send_nlmsg((struct nl_sock *)drv->global->nl,
4820 twt_nl_msg, NULL, NULL);
4821 if (*status != 0) {
4822 wpa_printf(MSG_ERROR, "Failed to send nlmsg - err %d", *status);
4823 ret = -EINVAL;
4824 }
4825 }
4826 break;
4827 default:
4828 wpa_printf(MSG_ERROR, "nlmg send failure");
4829 ret = -EINVAL;
4830 goto err_msg;
4831 }
4832
4833 err_msg:
4834 wpa_printf(MSG_ERROR, "sent nlmsg - status %d", *status);
4835 return ret;
4836 }
4837
4838 /**
4839 * unpack_twt_terminate_event()- unpacks twt_session_terminate response recieved
4840 * The response is printed in below format:
4841 * CTRL-EVENT-TWT TERMINATE dialog_id <dialog_id> status <status>
4842 *
4843 * @Param tb: vendor nl data
4844 * @Param buf: stores the response
4845 * @Param buf_len: length of the response buffer
4846 *
4847 * @Returns 0 on Success, -EINVAL on invalid response
4848 */
4849 static
unpack_twt_terminate_event(struct nlattr ** tb,char * buf,int buf_len)4850 int unpack_twt_terminate_event(struct nlattr **tb, char *buf, int buf_len)
4851 {
4852 int cmd_id;
4853 u8 val;
4854 char temp[TWT_RESP_BUF_LEN];
4855 struct nlattr *tb2[QCA_WLAN_VENDOR_ATTR_TWT_SETUP_MAX + 1];
4856
4857 if (nla_parse_nested(tb2, QCA_WLAN_VENDOR_ATTR_TWT_SETUP_MAX,
4858 tb[QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_PARAMS], NULL)) {
4859 wpa_printf(MSG_ERROR, "nla_parse failed");
4860 return -EINVAL;
4861 }
4862
4863 buf = result_copy_to_buf(TWT_TEARDOWN_RESP, buf, &buf_len);
4864 if (!buf)
4865 return -EINVAL;
4866
4867 os_memset(temp, 0, TWT_TEARDOWN_RESP_LEN);
4868 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_FLOW_ID;
4869 if (!tb2[cmd_id]) {
4870 wpa_printf(MSG_ERROR, "%s TWT dialog id missing", __func__);
4871 return -EINVAL;
4872 } else {
4873 val = nla_get_u8(tb2[cmd_id]);
4874 if(val == 255) {
4875 val = 0;
4876 }
4877 os_snprintf(temp, TWT_RESP_BUF_LEN, "dialog_id %u", val);
4878 buf = result_copy_to_buf(temp, buf, &buf_len);
4879 if (!buf)
4880 return -EINVAL;
4881 }
4882
4883 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_STATUS;
4884 if (!tb2[cmd_id]) {
4885 wpa_printf(MSG_ERROR, "%s TWT resp status missing", __func__);
4886 return -EINVAL;
4887 } else {
4888 val = nla_get_u8(tb2[cmd_id]);
4889 os_memset(temp, 0, TWT_RESP_BUF_LEN);
4890 os_snprintf(temp, TWT_RESP_BUF_LEN, "status %u ", val);
4891 buf = result_copy_to_buf(temp, buf, &buf_len);
4892 if (!buf)
4893 return -EINVAL;
4894
4895 os_memset(temp, 0, TWT_RESP_BUF_LEN);
4896 os_snprintf(temp, TWT_RESP_BUF_LEN, "(%s)", twt_status_to_string(val));
4897 buf = result_copy_to_buf(temp, buf, &buf_len);
4898 if (!buf)
4899 return -EINVAL;
4900 }
4901 *buf = '\0';
4902
4903 return 0;
4904 }
4905
4906 /**
4907 * unpack_twt_suspend_event()- unpacks twt_session_pause response recieved
4908 * The response is printed in below format:
4909 * CTRL-EVENT-TWT PAUSE dialog_id <dialog_id> status <status>
4910 *
4911 * @Param tb: vendor nl data
4912 * @Param buf: stores the response
4913 * @Param buf_len: length of the response buffer
4914 *
4915 * @Returns 0 on Success, -EINVAL on invalid response
4916 */
4917 static
unpack_twt_suspend_event(struct nlattr ** tb,char * buf,int buf_len)4918 int unpack_twt_suspend_event(struct nlattr **tb, char *buf, int buf_len)
4919 {
4920 int cmd_id;
4921 u8 val;
4922 char temp[TWT_RESP_BUF_LEN];
4923 struct nlattr *tb2[QCA_WLAN_VENDOR_ATTR_TWT_SETUP_MAX + 1];
4924
4925 if (nla_parse_nested(tb2, QCA_WLAN_VENDOR_ATTR_TWT_SETUP_MAX,
4926 tb[QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_PARAMS], NULL)) {
4927 wpa_printf(MSG_ERROR, "nla_parse failed");
4928 return -1;
4929 }
4930
4931 buf = result_copy_to_buf(TWT_PAUSE_RESP, buf, &buf_len);
4932 if (!buf)
4933 return -EINVAL;
4934
4935 os_memset(temp, 0, TWT_PAUSE_RESP_LEN);
4936 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_FLOW_ID;
4937 if (!tb2[cmd_id]) {
4938 wpa_printf(MSG_ERROR, "%s TWT dialog id missing", __func__);
4939 return -EINVAL;
4940 } else {
4941 val = nla_get_u8(tb2[cmd_id]);
4942 if(val == 255) {
4943 val = 0;
4944 }
4945 os_snprintf(temp, TWT_RESP_BUF_LEN, "dialog_id %u", val);
4946 buf = result_copy_to_buf(temp, buf, &buf_len);
4947 if (!buf)
4948 return -EINVAL;
4949 }
4950
4951 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_STATUS;
4952 if (!tb2[cmd_id]) {
4953 wpa_printf(MSG_ERROR, "%s TWT resp status missing", __func__);
4954 return -EINVAL;
4955 } else {
4956 val = nla_get_u8(tb2[cmd_id]);
4957 os_memset(temp, 0, TWT_RESP_BUF_LEN);
4958 os_snprintf(temp, TWT_RESP_BUF_LEN, "status %u ", val);
4959 buf = result_copy_to_buf(temp, buf, &buf_len);
4960 if (!buf)
4961 return -EINVAL;
4962
4963 os_memset(temp, 0, TWT_RESP_BUF_LEN);
4964 os_snprintf(temp, TWT_RESP_BUF_LEN, "(%s)", twt_status_to_string(val));
4965 buf = result_copy_to_buf(temp, buf, &buf_len);
4966 if (!buf)
4967 return -EINVAL;
4968 }
4969 *buf = '\0';
4970
4971 return 0;
4972 }
4973
4974 /**
4975 * unpack_twt_resume_event()- unpacks twt_session_resume response recieved
4976 * The response is printed in below format:
4977 * CTRL-EVENT-TWT RESUME dialog_id <dialog_id> status <status>
4978 *
4979 * @Param tb: vendor nl data
4980 * @Param buf: stores the response
4981 * @Param buf_len: length of the response buffer
4982 *
4983 * @Returns 0 on Success, -EINVAL on invalid response
4984 */
4985 static
unpack_twt_resume_event(struct nlattr ** tb,char * buf,int buf_len)4986 int unpack_twt_resume_event(struct nlattr **tb, char *buf, int buf_len)
4987 {
4988 int cmd_id;
4989 u8 val;
4990 char temp[TWT_RESP_BUF_LEN];
4991 struct nlattr *tb2[QCA_WLAN_VENDOR_ATTR_TWT_SETUP_MAX + 1];
4992
4993 if (nla_parse_nested(tb2, QCA_WLAN_VENDOR_ATTR_TWT_SETUP_MAX,
4994 tb[QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_PARAMS], NULL)) {
4995 wpa_printf(MSG_ERROR, "nla_parse failed");
4996 return -1;
4997 }
4998
4999 buf = result_copy_to_buf(TWT_RESUME_RESP, buf, &buf_len);
5000 if (!buf)
5001 return -EINVAL;
5002
5003 os_memset(temp, 0, TWT_RESUME_RESP_LEN);
5004 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_RESUME_FLOW_ID;
5005 if (!tb2[cmd_id]) {
5006 wpa_printf(MSG_ERROR, "%s TWT dialog id missing", __func__);
5007 return -EINVAL;
5008 } else {
5009 val = nla_get_u8(tb2[cmd_id]);
5010 if(val == 255) {
5011 val = 0;
5012 }
5013 os_snprintf(temp, TWT_RESP_BUF_LEN, "dialog_id %u", val);
5014 buf = result_copy_to_buf(temp, buf, &buf_len);
5015 if (!buf)
5016 return -EINVAL;
5017 }
5018
5019 cmd_id = QCA_WLAN_VENDOR_ATTR_TWT_SETUP_STATUS;
5020 if (!tb2[cmd_id]) {
5021 wpa_printf(MSG_ERROR, "%s TWT resp status missing", __func__);
5022 return -EINVAL;
5023 } else {
5024 val = nla_get_u8(tb2[cmd_id]);
5025 os_memset(temp, 0, TWT_RESP_BUF_LEN);
5026 os_snprintf(temp, TWT_RESP_BUF_LEN, "status %u ", val);
5027 buf = result_copy_to_buf(temp, buf, &buf_len);
5028 if (!buf)
5029 return -EINVAL;
5030
5031 os_memset(temp, 0, TWT_RESP_BUF_LEN);
5032 os_snprintf(temp, TWT_RESP_BUF_LEN, "(%s)", twt_status_to_string(val));
5033 buf = result_copy_to_buf(temp, buf, &buf_len);
5034 if (!buf)
5035 return -EINVAL;
5036 }
5037 *buf = '\0';
5038
5039 return 0;
5040 }
5041
5042 /**
5043 * unpack_twt_notify_event()- prints the twt notify response recieved from driver
5044 * asynchronously for QCA_WLAN_TWT_SETUP_READY_NOTIFY event.
5045 * The response is printed in below format:
5046 * CTRL-EVENT-TWT NOTIFY
5047 *
5048 * @Param tb: vendor nl data
5049 * @Param buf: stores the response
5050 * @Param buf_len: length of the response buffer
5051 *
5052 * @Returns 0 on Success, -EINVAL on invalid response
5053 */
unpack_twt_notify_event(struct nlattr ** tb,char * buf,int buf_len)5054 int unpack_twt_notify_event(struct nlattr **tb, char *buf, int buf_len)
5055 {
5056 char temp[TWT_RESP_BUF_LEN];
5057
5058 os_memset(temp, 0, TWT_RESP_BUF_LEN);
5059
5060 buf = result_copy_to_buf(TWT_NOTIFY_RESP, buf, &buf_len);
5061 if (!buf)
5062 return -EINVAL;
5063
5064 *buf = '\0';
5065 return 0;
5066 }
5067
5068 /**
5069 * wpa_driver_twt_async_resp_handler()- handler for asynchronous twt vendor event
5070 * recieved from the driver.
5071 *
5072 * @Param drv- wpa_driver_nl80211_data
5073 * @Param vendor_id- vendor id for vendor specific command
5074 * @Param subcmd- subcmd as defined by enum qca_nl80211_vendor_subcmds
5075 * @Param data- vendor data
5076 * @Param len- vendor data length
5077 *
5078 * @Returns 0 for Success, -1 for Failure
5079 */
wpa_driver_twt_async_resp_event(struct wpa_driver_nl80211_data * drv,u32 vendor_id,u32 subcmd,u8 * data,size_t len)5080 static int wpa_driver_twt_async_resp_event(struct wpa_driver_nl80211_data *drv,
5081 u32 vendor_id, u32 subcmd, u8 *data, size_t len)
5082 {
5083 int ret = 0;
5084 char *buf;
5085 buf = (char *)malloc(TWT_RESP_BUF_LEN);
5086 int buf_len = TWT_RESP_BUF_LEN;
5087 struct nlattr *tb[QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_MAX + 1];
5088 u8 twt_operation_type;
5089
5090 if (!buf)
5091 return -1;
5092
5093 ret = nla_parse(tb, QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_MAX,
5094 (struct nlattr *) data, len, NULL);
5095 if (ret) {
5096 wpa_printf(MSG_ERROR, "nla_parse failed %d", ret);
5097 goto fail;
5098 }
5099
5100 if (!buf) {
5101 wpa_printf(MSG_ERROR, "buf not allocated");
5102 return -1;
5103 }
5104
5105 memset(buf, 0, TWT_RESP_BUF_LEN);
5106
5107 twt_operation_type = nla_get_u8(tb[QCA_WLAN_VENDOR_ATTR_CONFIG_TWT_OPERATION]);
5108
5109 switch(twt_operation_type) {
5110 case QCA_WLAN_TWT_SET:
5111 ret = unpack_twt_setup_nlmsg(tb, buf, buf_len);
5112 break;
5113 case QCA_WLAN_TWT_TERMINATE:
5114 ret = unpack_twt_terminate_event(tb, buf, buf_len);
5115 break;
5116 case QCA_WLAN_TWT_SUSPEND:
5117 ret = unpack_twt_suspend_event(tb, buf, buf_len);
5118 break;
5119 case QCA_WLAN_TWT_RESUME:
5120 ret = unpack_twt_resume_event(tb, buf, buf_len);
5121 break;
5122 case QCA_WLAN_TWT_SETUP_READY_NOTIFY:
5123 ret = unpack_twt_notify_event(tb, buf, buf_len);
5124 break;
5125 default:
5126 ret = -1;
5127 }
5128
5129 if(ret) {
5130 wpa_printf(MSG_ERROR, "Async event parsing failed for operation %d",
5131 twt_operation_type);
5132 goto fail;
5133 }
5134 wpa_printf(MSG_ERROR,"%s", buf);
5135 wpa_msg(drv->ctx, MSG_INFO, "%s", buf);
5136 fail:
5137 free(buf);
5138 return ret;
5139 }
5140
wpa_driver_form_clear_mcc_quota_msg(struct i802_bss * bss,char * cmd)5141 static int wpa_driver_form_clear_mcc_quota_msg(struct i802_bss *bss,
5142 char *cmd)
5143 {
5144 struct wpa_driver_nl80211_data *drv = bss->drv;
5145 struct nlattr *nl_attr;
5146 struct nl_msg *nlmsg;
5147 uint32_t if_index = 0;
5148 int ret;
5149
5150 /* First comes interface name - optional */
5151 if (os_strncasecmp(cmd, "iface", 5) == 0) {
5152 char *iface;
5153 cmd = move_to_next_str(cmd);
5154 /* null terminate the iface name in the cmd string */
5155 iface = strchr(cmd, ' ');
5156 if (iface == NULL) {
5157 wpa_printf(MSG_ERROR, "mcc_quota: iface is not found"
5158 " in cmd string");
5159 return -EINVAL;
5160 }
5161 *iface = '\0';
5162 iface = cmd;
5163 errno = 0;
5164 if_index = if_nametoindex(cmd);
5165 if (if_index == 0) {
5166 wpa_printf(MSG_ERROR, "mcc_quota: iface %s not found %d",
5167 cmd, errno);
5168 return -EINVAL;
5169 }
5170 wpa_printf(MSG_INFO, "mcc_quota: ifindex %u", if_index);
5171 cmd += strlen(iface) + 1;
5172 }
5173
5174 nlmsg = prepare_vendor_nlmsg(drv, bss->ifname,
5175 QCA_NL80211_VENDOR_SUBCMD_MCC_QUOTA);
5176 if (!nlmsg) {
5177 wpa_printf(MSG_ERROR, "mcc_quota: Failed to allocate nl message");
5178 return -ENOMEM;
5179 }
5180
5181 nl_attr = nla_nest_start(nlmsg, NL80211_ATTR_VENDOR_DATA);
5182 if (!nl_attr) {
5183 wpa_printf(MSG_ERROR, "mcc_quota: Failed to alloc nlattr");
5184 ret = -ENOMEM;
5185 goto fail;
5186 }
5187 /* Put the quota type */
5188 ret = nla_put_u32(nlmsg, QCA_WLAN_VENDOR_ATTR_MCC_QUOTA_TYPE,
5189 QCA_WLAN_VENDOR_MCC_QUOTA_TYPE_CLEAR);
5190 if (ret) {
5191 wpa_printf(MSG_ERROR, "mcc_quota: Failed to add type attr %d", ret);
5192 goto fail;
5193 }
5194
5195 if (if_index) {
5196 ret = nla_put_u32(nlmsg, QCA_WLAN_VENDOR_ATTR_MCC_QUOTA_IFINDEX, if_index);
5197 if (ret) {
5198 wpa_printf(MSG_ERROR, "mcc_quota: Failed to add if_index attr %d", ret);
5199 goto fail;
5200 }
5201 }
5202 nla_nest_end(nlmsg, nl_attr);
5203
5204 ret = send_nlmsg((struct nl_sock *)drv->global->nl, nlmsg, NULL, NULL);
5205
5206 if (ret) {
5207 wpa_printf(MSG_ERROR, "mcc_quota: Error sending nlmsg %d", ret);
5208 return ret;
5209 }
5210
5211 return 0;
5212 fail:
5213 nlmsg_free(nlmsg);
5214 return ret;
5215 }
5216
wpa_driver_form_set_mcc_quota_msg(struct i802_bss * bss,char * cmd)5217 static int wpa_driver_form_set_mcc_quota_msg(struct i802_bss *bss,
5218 char *cmd)
5219 {
5220 struct wpa_driver_nl80211_data *drv = bss->drv;
5221 struct mcc_quota mccquota[MCC_QUOTA_ENTRIES_MAX];
5222 uint32_t if_index, quota;
5223 struct nlattr *nl_attr, *mcc_attr_list;
5224 struct nlattr *mcc_attr;
5225 struct nl_msg *nlmsg;
5226 int ret, entry, i;
5227
5228 wpa_printf(MSG_INFO, "mcc_quota: %s", cmd);
5229
5230 entry = 0;
5231 while (*cmd != '\0') {
5232 if (entry >= MCC_QUOTA_ENTRIES_MAX) {
5233 wpa_printf(MSG_INFO, "mcc_quota: Only %d entries accepted", entry);
5234 break;
5235 }
5236 /* First comes interface name */
5237 if (os_strncasecmp(cmd, "iface", 5) == 0) {
5238 char *iface;
5239 cmd = move_to_next_str(cmd);
5240 /* null terminate the iface name in the cmd string */
5241 iface = strchr(cmd, ' ');
5242 if (iface == NULL) {
5243 wpa_printf(MSG_ERROR, "mcc_quota: iface is not"
5244 " found in cmd string");
5245 return -EINVAL;
5246 }
5247 *iface = '\0';
5248 iface = cmd;
5249 errno = 0;
5250 if_index = if_nametoindex(cmd);
5251 if (if_index == 0) {
5252 wpa_printf(MSG_ERROR, "mcc_quota: iface %s not found %d",
5253 cmd, errno);
5254 return -EINVAL;
5255 }
5256 wpa_printf(MSG_INFO, "mcc_quota: ifindex %u", if_index);
5257 cmd += strlen(iface) + 1;
5258 } else {
5259 wpa_printf(MSG_ERROR, "mcc_quota: Iface name not in order");
5260 return -EINVAL;
5261 }
5262
5263 /* Second comes quota value */
5264 if (os_strncasecmp(cmd, "quota", 5) == 0) {
5265 cmd = move_to_next_str(cmd);
5266 quota = get_u32_from_string(cmd, &ret);
5267 if (ret < 0)
5268 return ret;
5269 } else {
5270 wpa_printf(MSG_ERROR, "mcc_quota: Quota not in order");
5271 return -EINVAL;
5272 }
5273
5274 if (quota < MCC_QUOTA_MIN || quota > MCC_QUOTA_MAX) {
5275 wpa_printf(MSG_ERROR, "mcc_quota: Invalid quota value %u", quota);
5276 return -EINVAL;
5277 }
5278
5279 mccquota[entry].if_idx = if_index;
5280 mccquota[entry].quota = quota;
5281 cmd = move_to_next_str(cmd);
5282 entry++;
5283 }
5284 wpa_printf(MSG_INFO, "mcc_quota: Entries : %d", entry);
5285 if (entry < 1) {
5286 wpa_printf(MSG_ERROR, "mcc_quota: No valid entries?");
5287 return -EINVAL;
5288 }
5289
5290 nlmsg = prepare_vendor_nlmsg(drv, bss->ifname,
5291 QCA_NL80211_VENDOR_SUBCMD_MCC_QUOTA);
5292 if (!nlmsg) {
5293 wpa_printf(MSG_ERROR,"mcc_quota: Failed to allocate nl message");
5294 return -ENOMEM;
5295 }
5296
5297 nl_attr = nla_nest_start(nlmsg, NL80211_ATTR_VENDOR_DATA);
5298 if (nl_attr == NULL) {
5299 wpa_printf(MSG_ERROR, "mcc_quota: Failed to alloc nlattr");
5300 ret = -ENOMEM;
5301 goto fail;
5302 }
5303
5304 /* Put the quota type */
5305 ret = nla_put_u32(nlmsg, QCA_WLAN_VENDOR_ATTR_MCC_QUOTA_TYPE,
5306 QCA_WLAN_VENDOR_MCC_QUOTA_TYPE_FIXED);
5307 if (ret) {
5308 wpa_printf(MSG_ERROR, "mcc_quota: Failed to add type attr %d", ret);
5309 goto fail;
5310 }
5311
5312 mcc_attr_list = nla_nest_start(nlmsg, QCA_WLAN_VENDOR_ATTR_MCC_QUOTA_ENTRIES);
5313 if (!mcc_attr_list) {
5314 wpa_printf(MSG_ERROR, "mcc_quota: Failed to alloc mcc_attr_list");
5315 ret = -ENOMEM;
5316 goto fail;
5317 }
5318
5319 for (i = 0; i < entry && entry <= MCC_QUOTA_ENTRIES_MAX; i++) {
5320 /* Nest the (iface ,quota) */
5321 mcc_attr = nla_nest_start(nlmsg, QCA_WLAN_VENDOR_ATTR_MCC_QUOTA_ENTRIES);
5322 if (mcc_attr == NULL) {
5323 wpa_printf(MSG_ERROR, "mcc_quota: Failed to alloc mccattr");
5324 ret = -ENOMEM;
5325 goto fail;
5326 }
5327 ret = nla_put_u32(nlmsg, QCA_WLAN_VENDOR_ATTR_MCC_QUOTA_IFINDEX, mccquota[i].if_idx);
5328 if (ret) {
5329 wpa_printf(MSG_ERROR, "mcc_quota: Failed to add if_index attr %d", ret);
5330 goto fail;
5331 }
5332
5333 ret = nla_put_u32(nlmsg, QCA_WLAN_VENDOR_ATTR_MCC_QUOTA_CHAN_TIME_PERCENTAGE, mccquota[i].quota);
5334 if (ret) {
5335 wpa_printf(MSG_ERROR, "mcc_quota: Failed to add quota attr %d", ret);
5336 goto fail;
5337 }
5338
5339 nla_nest_end(nlmsg, mcc_attr);
5340 }
5341 nla_nest_end(nlmsg, mcc_attr_list);
5342
5343 nla_nest_end(nlmsg, nl_attr);
5344
5345 ret = send_nlmsg((struct nl_sock *)drv->global->nl, nlmsg, NULL, NULL);
5346
5347 if (ret) {
5348 wpa_printf(MSG_ERROR, "mcc_quota: Error sending nlmsg %d", ret);
5349 return ret;
5350 }
5351
5352 return 0;
5353 fail:
5354 nlmsg_free(nlmsg);
5355 return ret;
5356 }
5357
wpa_driver_cmd_send_mcc_quota(struct i802_bss * bss,char * cmd)5358 int wpa_driver_cmd_send_mcc_quota(struct i802_bss *bss,
5359 char *cmd)
5360 {
5361 int ret;
5362
5363 wpa_printf(MSG_INFO, "mcc_quota: %s", cmd);
5364
5365 if (os_strncasecmp(cmd, "set", 3) == 0) {
5366 cmd = move_to_next_str(cmd);
5367 ret = wpa_driver_form_set_mcc_quota_msg(bss, cmd);
5368 return ret;
5369 }
5370
5371 if (os_strncasecmp(cmd, "clear", 5) == 0) {
5372 cmd = move_to_next_str(cmd);
5373 ret = wpa_driver_form_clear_mcc_quota_msg(bss, cmd);
5374 return ret;
5375 }
5376
5377 wpa_printf(MSG_ERROR, "mcc_quota: Unknown operation");
5378 return -EINVAL;
5379 }
5380
wpa_driver_form_flush_queue_config_msg(struct i802_bss * bss,char * cmd)5381 static int wpa_driver_form_flush_queue_config_msg(struct i802_bss *bss, char *cmd)
5382 {
5383 uint32_t tid_mask = 0, flush_policy = 0;
5384 u8 mac[MAC_ADDR_LEN], ac_mask = 0;
5385 struct nl_msg *nlmsg = NULL;
5386 struct nlattr *nl_attr;
5387 char *ptr = cmd;
5388 int ret;
5389
5390 wpa_printf(MSG_DEBUG, "flush_queue_config: %s", cmd);
5391
5392 /* First parameter: MAC address of peer */
5393 if (os_strncasecmp(cmd, "peer", 4) == 0) {
5394 cmd = move_to_next_str(cmd);
5395 if ((strlen(cmd) < (MAC_ADDR_LEN * 2 + MAC_ADDR_LEN - 1)) ||
5396 convert_string_to_bytes(mac, cmd, MAC_ADDR_LEN) !=
5397 MAC_ADDR_LEN) {
5398 wpa_printf(MSG_ERROR, "flush_queue_config: Invalid MAC address");
5399 wpa_printf(MSG_ERROR, "flush_queue_config cmd: %s", ptr);
5400 return -EINVAL;
5401 }
5402 cmd = move_to_next_str(cmd);
5403 } else {
5404 wpa_printf(MSG_ERROR, "flush_queue_config: peer MAC address is missing");
5405 wpa_printf(MSG_ERROR, "flush_queue_config cmd: %s", ptr);
5406 return -EINVAL;
5407 }
5408
5409 /* Second parameter: flush config */
5410 if (os_strncasecmp(cmd, "policy", 6) == 0) {
5411 cmd = move_to_next_str(cmd);
5412 flush_policy = get_u32_from_string(cmd, &ret);
5413 if (ret < 0) {
5414 wpa_printf(MSG_ERROR, "flush_queue_config: Invalid flush policy");
5415 wpa_printf(MSG_ERROR, "flush_queue_config cmd: %s", ptr);
5416 return ret;
5417 }
5418 cmd = move_to_next_str(cmd);
5419 }
5420
5421 /* Third parameter: Follow QCA_WLAN_VENDOR_ATTR_AC for ac bit mask */
5422 if (os_strncasecmp(cmd, "ac", 2) == 0) {
5423 cmd = move_to_next_str(cmd);
5424 ac_mask = get_u8_from_string(cmd, &ret);
5425 if (ret < 0) {
5426 wpa_printf(MSG_ERROR, "flush_queue_config: AC mask error");
5427 wpa_printf(MSG_ERROR, "flush_queue_config cmd: %s", ptr);
5428 return ret;
5429 }
5430
5431 if (!(ac_mask & (BIT(QCA_WLAN_VENDOR_TOS_BK) | BIT(QCA_WLAN_VENDOR_TOS_BE) |
5432 BIT(QCA_WLAN_VENDOR_TOS_VI) | BIT(QCA_WLAN_VENDOR_TOS_VO)))) {
5433 wpa_printf(MSG_ERROR, "flush_queue_config: Invalid AC mask");
5434 wpa_printf(MSG_ERROR, "flush_queue_config cmd: %s", ptr);
5435 return -EINVAL;
5436 }
5437 cmd = move_to_next_str(cmd);
5438 }
5439
5440 /* Fourth parameter: tid mask */
5441 if (os_strncasecmp(cmd, "tid", 3) == 0) {
5442 cmd = move_to_next_str(cmd);
5443 tid_mask = get_u32_from_string(cmd, &ret);
5444 if (ret < 0) {
5445 wpa_printf(MSG_ERROR, "flush_queue_config: TID mask error");
5446 wpa_printf(MSG_ERROR, "flush_queue_config cmd: %s", ptr);
5447 return ret;
5448 }
5449 cmd = move_to_next_str(cmd);
5450 }
5451
5452 if (!tid_mask && !ac_mask) {
5453 wpa_printf(MSG_ERROR, "flush_queue_config: Neither TID not AC mask provided");
5454 wpa_printf(MSG_ERROR, "flush_queue_config cmd: %s", ptr);
5455 return -EINVAL;
5456 }
5457
5458 nlmsg =
5459 prepare_vendor_nlmsg(bss->drv, bss->ifname,
5460 QCA_NL80211_VENDOR_SUBCMD_PEER_FLUSH_PENDING);
5461 if (!nlmsg) {
5462 wpa_printf(MSG_ERROR, "flush_queue_config: Failed to allocate nl message");
5463 return -ENOMEM;
5464 }
5465
5466 nl_attr = nla_nest_start(nlmsg, NL80211_ATTR_VENDOR_DATA);
5467 if (!nl_attr) {
5468 wpa_printf(MSG_ERROR, "flush_queue_config: Failed to alloc nlattr");
5469 ret = -ENOMEM;
5470 goto fail;
5471 }
5472
5473 /* Put the peer MAC address */
5474 ret = nla_put(nlmsg, QCA_WLAN_VENDOR_ATTR_PEER_ADDR, MAC_ADDR_LEN, mac);
5475 if (ret) {
5476 wpa_printf(MSG_ERROR, "flush_queue_config: Error add hw addr attr %d", ret);
5477 goto fail;
5478 }
5479
5480 /* Put the flush_policy */
5481 ret = nla_put_u32(nlmsg, QCA_WLAN_VENDOR_ATTR_FLUSH_PENDING_POLICY,
5482 flush_policy);
5483 if (ret) {
5484 wpa_printf(MSG_ERROR,
5485 "flush_queue_config: Error add policy attr %d", ret);
5486 goto fail;
5487 }
5488
5489 if (tid_mask) {
5490 /* Put the tid mask */
5491 ret = nla_put_u32(nlmsg, QCA_WLAN_VENDOR_ATTR_TID_MASK,
5492 tid_mask);
5493 if (ret) {
5494 wpa_printf(MSG_ERROR, "flush_queue_config: Error add tid mask attr %d", ret);
5495 goto fail;
5496 }
5497 } else {
5498 /* Put the ac mask */
5499 ret = nla_put_u8(nlmsg, QCA_WLAN_VENDOR_ATTR_AC, ac_mask);
5500 if (ret) {
5501 wpa_printf(MSG_ERROR, "flush_queue_config: Error add ac attr %d", ret);
5502 goto fail;
5503 }
5504 }
5505
5506 nla_nest_end(nlmsg, nl_attr);
5507 ret = send_nlmsg((struct nl_sock *)bss->drv->global->nl, nlmsg,
5508 NULL, NULL);
5509 if (ret) {
5510 wpa_printf(MSG_ERROR, "flush_queue_config: Error sending nlmsg %d", ret);
5511 return ret;
5512 }
5513 return 0;
5514 fail:
5515 if (nlmsg)
5516 nlmsg_free(nlmsg);
5517 return ret;
5518 }
5519
wpa_driver_cmd_send_peer_flush_queue_config(struct i802_bss * bss,char * cmd)5520 int wpa_driver_cmd_send_peer_flush_queue_config(struct i802_bss *bss, char *cmd)
5521 {
5522 int ret;
5523
5524 if (os_strncasecmp(cmd, "set", 3) == 0) {
5525 cmd = move_to_next_str(cmd);
5526 ret = wpa_driver_form_flush_queue_config_msg(bss, cmd);
5527 return ret;
5528 }
5529
5530 wpa_printf(MSG_ERROR, "peer_flush_config: Unknown operation");
5531 return -EINVAL;
5532 }
5533
wpa_driver_nl80211_driver_cmd(void * priv,char * cmd,char * buf,size_t buf_len)5534 int wpa_driver_nl80211_driver_cmd(void *priv, char *cmd, char *buf,
5535 size_t buf_len )
5536 {
5537 struct i802_bss *bss = priv;
5538 struct wpa_driver_nl80211_data *drv = NULL;
5539 struct wpa_driver_nl80211_data *driver;
5540 struct ifreq ifr;
5541 android_wifi_priv_cmd priv_cmd;
5542 int ret = 0, status = 0, lib_n = 0;
5543
5544 if (bss) {
5545 drv = bss->drv;
5546 } else {
5547 if (os_strncasecmp(cmd, "SET_AP_SUSPEND", 14)) {
5548 wpa_printf(MSG_ERROR, "%s: bss is NULL for cmd %s\n",
5549 __func__, cmd);
5550 return -EINVAL;
5551 }
5552 }
5553
5554 if (wpa_driver_oem_initialize(&oem_cb_table) != WPA_DRIVER_OEM_STATUS_FAILURE &&
5555 oem_cb_table) {
5556
5557 for (lib_n = 0;
5558 oem_cb_table[lib_n].wpa_driver_driver_cmd_oem_cb != NULL;
5559 lib_n++)
5560 {
5561 ret = oem_cb_table[lib_n].wpa_driver_driver_cmd_oem_cb(
5562 priv, cmd, buf, buf_len, &status);
5563 if (ret == WPA_DRIVER_OEM_STATUS_SUCCESS ) {
5564 return strlen(buf);
5565 } else if (ret == WPA_DRIVER_OEM_STATUS_ENOSUPP) {
5566 continue;
5567 } else if ((ret == WPA_DRIVER_OEM_STATUS_FAILURE) &&
5568 (status != 0)) {
5569 wpa_printf(MSG_DEBUG, "%s: Received error: %d",
5570 __func__, status);
5571 return status;
5572 }
5573 }
5574 /* else proceed with legacy handling as below */
5575 }
5576
5577 if (!drv) {
5578 wpa_printf(MSG_ERROR, "%s: drv is NULL for cmd %s\n",
5579 __func__, cmd);
5580 return -EINVAL;
5581 }
5582
5583 if (os_strcasecmp(cmd, "START") == 0) {
5584 dl_list_for_each(driver, &drv->global->interfaces, struct wpa_driver_nl80211_data, list) {
5585 linux_set_iface_flags(drv->global->ioctl_sock, driver->first_bss->ifname, 1);
5586 wpa_msg(drv->ctx, MSG_INFO, WPA_EVENT_DRIVER_STATE "STARTED");
5587 }
5588 } else if (os_strcasecmp(cmd, "MACADDR") == 0) {
5589 u8 macaddr[ETH_ALEN] = {};
5590
5591 ret = linux_get_ifhwaddr(drv->global->ioctl_sock, bss->ifname, macaddr);
5592 if (!ret)
5593 ret = os_snprintf(buf, buf_len,
5594 "Macaddr = " MACSTR "\n", MAC2STR(macaddr));
5595 } else if (os_strncasecmp(cmd, "SET_CONGESTION_REPORT ", 22) == 0) {
5596 return wpa_driver_cmd_set_congestion_report(priv, cmd + 22);
5597 } else if (os_strncasecmp(cmd, "SET_TXPOWER ", 12) == 0) {
5598 return wpa_driver_cmd_set_tx_power(priv, cmd + 12);
5599 } else if (os_strncasecmp(cmd, "CSI", 3) == 0) {
5600 cmd += 3;
5601 return wpa_driver_handle_csi_cmd(bss, cmd, buf, buf_len, &status);
5602 } else if(os_strncasecmp(cmd, "GETSTATSBSSINFO", 15) == 0) {
5603
5604 struct resp_info info,info2;
5605 struct nl_msg *nlmsg;
5606 struct nlattr *attr;
5607
5608 os_memset(&g_bss_info, 0, sizeof(struct bss_info));
5609
5610 memset(&info, 0, sizeof(struct resp_info));
5611 memset(&info2, 0, sizeof(struct resp_info));
5612
5613 info.subcmd = QCA_NL80211_VENDOR_SUBCMD_GET_STATION;
5614 info.cmd_type = GETSTATSBSSINFO;
5615 char *p;
5616 if(wpa_driver_ioctl(bss, "GETCOUNTRYREV", buf, buf_len, &status, drv) == 0){
5617 p = strstr(buf, " ");
5618 if(p != NULL)
5619 memcpy(info.country, (p+1), strlen(p+1)+1);//length of p including null
5620 }
5621 cmd += 16;
5622 os_memset(buf, 0, buf_len);
5623
5624 u8 mac[MAC_ADDR_LEN];
5625
5626 cmd = skip_white_space(cmd);
5627
5628 if (strlen(cmd) >= MAC_ADDR_LEN * 2 + MAC_ADDR_LEN - 1
5629 && convert_string_to_bytes(mac, cmd, MAC_ADDR_LEN) > 0) {
5630 wpa_printf(MSG_INFO,"invoking QCA_NL80211_VENDOR_SUBCMD_GET_STA_INFO to retrieve new attributes");
5631 os_memcpy(&info2.mac_addr[0], mac, MAC_ADDR_LEN);
5632 nlmsg = prepare_vendor_nlmsg(bss->drv, bss->ifname,
5633 QCA_NL80211_VENDOR_SUBCMD_GET_STA_INFO);
5634 if (!nlmsg) {
5635 wpa_printf(MSG_ERROR,"Failed to allocate nl message");
5636 return -1;
5637 }
5638
5639 attr = nla_nest_start(nlmsg, NL80211_ATTR_VENDOR_DATA);
5640 if (!attr) {
5641 nlmsg_free(nlmsg);
5642 return -1;
5643 }
5644
5645 if (nla_put(nlmsg, GET_STA_INFO_MAC,
5646 MAC_ADDR_LEN, mac)) {
5647 wpa_printf(MSG_ERROR,"Failed to put GET_STA_INFO_MAC");
5648 nlmsg_free(nlmsg);
5649 return -1;
5650 }
5651
5652 nla_nest_end(nlmsg, attr);
5653
5654 ret = send_nlmsg((struct nl_sock *)drv->global->nl, nlmsg,
5655 get_sta_info_handler, &info2);
5656 if (ret != 0) {
5657 if (ret == -EOPNOTSUPP) {
5658 wpa_printf(MSG_INFO,"Command is not supported, sending -1 for all new vendor attributes");
5659 } else {
5660 wpa_printf(MSG_ERROR,"Failed to send nl message with err %d", ret);
5661 return -1;
5662 }
5663 g_bss_info.ani_level = -1;
5664 g_bss_info.roam_trigger_reason = -1;
5665 g_bss_info.roam_fail_reason = -1;
5666 g_bss_info.roam_invoke_fail_reason = -1;
5667 g_bss_info.tsf_out_of_sync_count = -1;
5668 g_bss_info.latest_tx_power = -1;
5669 g_bss_info.latest_tx_rate = -1;
5670 g_bss_info.target_power_24g_1mbps = -1;
5671 g_bss_info.target_power_24g_6mbps = -1;
5672 g_bss_info.target_power_5g_6mbps = -1;
5673 } else {
5674 wpa_printf(MSG_INFO,"Command successfully invoked");
5675 g_bss_info.ani_level = g_sta_info.ani_level;
5676 g_bss_info.roam_trigger_reason = g_sta_info.roam_trigger_reason;
5677 g_bss_info.roam_fail_reason = g_sta_info.roam_fail_reason;
5678 g_bss_info.roam_invoke_fail_reason = g_sta_info.roam_invoke_fail_reason;
5679 g_bss_info.tsf_out_of_sync_count = g_sta_info.tsf_out_of_sync_count;
5680 g_bss_info.latest_tx_power = g_sta_info.latest_tx_power;
5681 g_bss_info.latest_tx_rate = g_sta_info.latest_tx_rate;
5682 g_bss_info.target_power_24g_1mbps = g_sta_info.target_power_24g_1mbps;
5683 g_bss_info.target_power_24g_6mbps = g_sta_info.target_power_24g_6mbps;
5684 g_bss_info.target_power_5g_6mbps = g_sta_info.target_power_5g_6mbps;
5685 }
5686 }
5687
5688 info.reply_buf = buf;
5689 info.reply_buf_len = buf_len;
5690
5691 nlmsg = prepare_vendor_nlmsg(drv, bss->ifname,
5692 info.subcmd);
5693 if (!nlmsg) {
5694 wpa_printf(MSG_ERROR,"Failed to allocate nl message");
5695 return -1;
5696 }
5697
5698 if (populate_nlmsg(nlmsg, cmd, info.cmd_type)) {
5699 wpa_printf(MSG_ERROR,"Failed to populate nl message");
5700 nlmsg_free(nlmsg);
5701 return -1;
5702 }
5703
5704 status = send_nlmsg((struct nl_sock *)drv->global->nl, nlmsg,
5705 response_handler, &info);
5706 if (status != 0) {
5707 wpa_printf(MSG_ERROR,"Failed to send nl message with err %d", status);
5708 return -1;
5709 }
5710
5711 return strlen(info.reply_buf);
5712 } else if (os_strncasecmp(cmd, "GETSTATSSTAINFO", 15) == 0) {
5713 cmd += 15;
5714 return wpa_driver_handle_get_sta_info(bss, cmd, buf, buf_len,
5715 &status);
5716 } else if (os_strncasecmp(cmd, "SETCELLSWITCHMODE", 17) == 0) {
5717 cmd += 17;
5718 struct resp_info info;
5719 struct nl_msg *nlmsg;
5720
5721 memset(&info, 0, sizeof(struct resp_info));
5722
5723 info.subcmd = QCA_NL80211_VENDOR_SUBCMD_ROAM;
5724 info.cmd_type = SETCELLSWITCHMODE;
5725
5726 nlmsg = prepare_vendor_nlmsg(drv, bss->ifname,
5727 info.subcmd);
5728 if (!nlmsg) {
5729 wpa_printf(MSG_ERROR,"Failed to allocate nl message");
5730 return WPA_DRIVER_OEM_STATUS_FAILURE;
5731 }
5732
5733 if (populate_nlmsg(nlmsg, cmd, info.cmd_type)) {
5734 wpa_printf(MSG_ERROR,"Failed to populate nl message");
5735 nlmsg_free(nlmsg);
5736 return WPA_DRIVER_OEM_STATUS_FAILURE;
5737 }
5738
5739 status = send_nlmsg((struct nl_sock *)drv->global->nl, nlmsg,
5740 NULL, NULL);
5741 if (status != 0) {
5742 wpa_printf(MSG_ERROR,"Failed to send nl message with err %d", status);
5743 return WPA_DRIVER_OEM_STATUS_FAILURE;
5744 }
5745
5746 return WPA_DRIVER_OEM_STATUS_SUCCESS;
5747 } else if (os_strncasecmp(cmd, "SET_ANI_LEVEL ", 14) == 0) {
5748 char *endptr = NULL;
5749 int mode = 0;
5750 int ofdmlvl = 0;
5751 mode = strtol(cmd + 14, &endptr, 10);
5752 if (mode == 1) {
5753 if(!(*endptr)) {
5754 wpa_printf(MSG_ERROR, "%s: failed to set ani setting,\
5755 invalid cmd: %s\n", __func__, cmd);
5756 return -EINVAL;
5757 }
5758 ofdmlvl = strtol(endptr, NULL, 10);
5759 }
5760 return wpa_driver_cmd_set_ani_level(priv, mode, ofdmlvl);
5761 } else if (os_strncasecmp(cmd, "GET_THERMAL_INFO", 16) == 0) {
5762 int temperature = -1;
5763 int thermal_state = -1;
5764 int ret, ret2;
5765
5766 ret = wpa_driver_cmd_get_thermal_info(priv, &temperature,
5767 QCA_WLAN_VENDOR_ATTR_THERMAL_CMD_TYPE_GET_TEMPERATURE);
5768 if (ret)
5769 return -1;
5770 ret2 = wpa_driver_cmd_get_thermal_info(priv, &thermal_state,
5771 QCA_WLAN_VENDOR_ATTR_THERMAL_CMD_TYPE_GET_LEVEL);
5772 if (ret2)
5773 return -1;
5774
5775 snprintf(buf, buf_len, "%d %d", temperature, thermal_state);
5776 return strlen(buf);
5777 } else if (os_strncasecmp(cmd, "GET_DRIVER_SUPPORTED_FEATURES", 29) == 0) {
5778 struct resp_info info;
5779 struct nl_msg *nlmsg;
5780 memset(&info, 0, sizeof(struct resp_info));
5781 info.subcmd = QCA_NL80211_VENDOR_SUBCMD_GET_FEATURES;
5782 os_memset(buf, 0, buf_len);
5783 info.reply_buf = buf;
5784 info.reply_buf_len = buf_len;
5785 nlmsg = prepare_vendor_nlmsg(drv, bss->ifname,
5786 info.subcmd);
5787 if (!nlmsg) {
5788 wpa_printf(MSG_ERROR,"Failed to allocate nl message");
5789 return -1;
5790 }
5791
5792 status = send_nlmsg((struct nl_sock *)drv->global->nl, nlmsg,
5793 response_handler, &info);
5794 if (status != 0) {
5795 wpa_printf(MSG_ERROR,"Failed to send nl message with err %d", status);
5796 return -1;
5797 }
5798
5799 return WPA_DRIVER_OEM_STATUS_SUCCESS;
5800 } else if ((ret = check_for_twt_cmd(&cmd)) != TWT_CMD_NOT_EXIST) {
5801 enum qca_wlan_twt_operation twt_oper = ret;
5802 u8 is_twt_feature_supported = 0;
5803
5804 if (oem_cb_table) {
5805 for (lib_n = 0;
5806 oem_cb_table[lib_n].wpa_driver_driver_cmd_oem_cb != NULL;
5807 lib_n++)
5808 {
5809 if (oem_cb_table[lib_n].wpa_driver_oem_feature_check_cb) {
5810 if (oem_cb_table[lib_n].wpa_driver_oem_feature_check_cb(FEATURE_TWT_SUPPORT))
5811 is_twt_feature_supported = 1;
5812 break;
5813 }
5814 }
5815 }
5816
5817 if (is_twt_feature_supported) {
5818 wpa_printf(MSG_ERROR, "%s: TWT feature already supported by oem lib %d\n", __func__, lib_n);
5819 ret = -EINVAL;
5820 } else {
5821 ret = wpa_driver_twt_cmd_handler(drv, bss->ifname, twt_oper, cmd, buf, buf_len,
5822 &status);
5823 if (ret)
5824 ret = os_snprintf(buf, buf_len, "TWT failed for operation %d", twt_oper);
5825 }
5826 } else if (os_strncasecmp(cmd, "MCC_QUOTA", 9) == 0) {
5827 /* DRIVER MCC_QUOTA set iface <name> quota <val>
5828 * DRIVER MCC_QUOTA clear iface <name>
5829 */
5830 /* Move cmd by string len and space */
5831 cmd += 10;
5832 return wpa_driver_cmd_send_mcc_quota(priv, cmd);
5833 } else if (os_strncasecmp(cmd, "FLUSH_QUEUE_CONFIG", 18) == 0) {
5834 /* DRIVER FLUSH_QUEUE_CONFIG set peer <mac addr> policy <val>
5835 * tid <tid mask> ac <ac mask>
5836 */
5837 /* Move cmd by string len and space */
5838 cmd += 19;
5839 return wpa_driver_cmd_send_peer_flush_queue_config(priv, cmd);
5840 } else { /* Use private command */
5841 memset(&ifr, 0, sizeof(ifr));
5842 memset(&priv_cmd, 0, sizeof(priv_cmd));
5843 os_memcpy(buf, cmd, strlen(cmd) + 1);
5844 os_strlcpy(ifr.ifr_name, bss->ifname, IFNAMSIZ);
5845
5846 priv_cmd.buf = buf;
5847 priv_cmd.used_len = buf_len;
5848 priv_cmd.total_len = buf_len;
5849 ifr.ifr_data = &priv_cmd;
5850
5851 if ((ret = ioctl(drv->global->ioctl_sock, SIOCDEVPRIVATE + 1, &ifr)) < 0) {
5852 wpa_printf(MSG_ERROR, "%s: failed to issue private commands, ret:%d, errno:%d\n", __func__, ret, errno);
5853 } else {
5854 drv_errors = 0;
5855 if((os_strncasecmp(cmd, "SETBAND", 7) == 0) &&
5856 ret == DO_NOT_SEND_CHANNEL_CHANGE_EVENT) {
5857 return 0;
5858 }
5859
5860 ret = 0;
5861 if ((os_strcasecmp(cmd, "LINKSPEED") == 0) ||
5862 (os_strcasecmp(cmd, "RSSI") == 0) ||
5863 (os_strstr(cmd, "GET") != NULL))
5864 ret = strlen(buf);
5865 else if (os_strcasecmp(cmd, "P2P_DEV_ADDR") == 0)
5866 wpa_printf(MSG_DEBUG, "%s: P2P: Device address ("MACSTR")",
5867 __func__, MAC2STR(buf));
5868 else if (os_strcasecmp(cmd, "P2P_SET_PS") == 0)
5869 wpa_printf(MSG_DEBUG, "%s: P2P: %s ", __func__, buf);
5870 else if (os_strcasecmp(cmd, "P2P_SET_NOA") == 0)
5871 wpa_printf(MSG_DEBUG, "%s: P2P: %s ", __func__, buf);
5872 else if (os_strcasecmp(cmd, "STOP") == 0) {
5873 wpa_printf(MSG_DEBUG, "%s: %s ", __func__, buf);
5874 dl_list_for_each(driver, &drv->global->interfaces, struct wpa_driver_nl80211_data, list) {
5875 linux_set_iface_flags(drv->global->ioctl_sock, driver->first_bss->ifname, 0);
5876 wpa_msg(drv->ctx, MSG_INFO, WPA_EVENT_DRIVER_STATE "STOPPED");
5877 }
5878 }
5879 else
5880 wpa_printf(MSG_DEBUG, "%s %s len = %d, %zu", __func__, buf, ret, buf_len);
5881 wpa_driver_notify_country_change(drv->ctx, cmd);
5882 }
5883 }
5884 return ret;
5885 }
5886
wpa_driver_set_p2p_noa(void * priv,u8 count,int start,int duration)5887 int wpa_driver_set_p2p_noa(void *priv, u8 count, int start, int duration)
5888 {
5889 char buf[MAX_DRV_CMD_SIZE];
5890 char reply_buf[MAX_DRV_CMD_SIZE];
5891
5892 memset(buf, 0, sizeof(buf));
5893 memset(reply_buf, 0, sizeof(reply_buf));
5894 wpa_printf(MSG_DEBUG, "%s: Entry", __func__);
5895 snprintf(buf, sizeof(buf), "P2P_SET_NOA %d %d %d", count, start, duration);
5896 return wpa_driver_nl80211_driver_cmd(priv, buf, reply_buf, sizeof(reply_buf));
5897 }
5898
wpa_driver_get_p2p_noa(void * priv,u8 * buf,size_t len)5899 int wpa_driver_get_p2p_noa(void *priv, u8 *buf, size_t len)
5900 {
5901 UNUSED(priv), UNUSED(buf), UNUSED(len);
5902 /* Return 0 till we handle p2p_presence request completely in the driver */
5903 return 0;
5904 }
5905
wpa_driver_set_p2p_ps(void * priv,int legacy_ps,int opp_ps,int ctwindow)5906 int wpa_driver_set_p2p_ps(void *priv, int legacy_ps, int opp_ps, int ctwindow)
5907 {
5908 char buf[MAX_DRV_CMD_SIZE];
5909 char reply_buf[MAX_DRV_CMD_SIZE];
5910
5911 memset(buf, 0, sizeof(buf));
5912 memset(reply_buf, 0, sizeof(reply_buf));
5913 wpa_printf(MSG_DEBUG, "%s: Entry", __func__);
5914 snprintf(buf, sizeof(buf), "P2P_SET_PS %d %d %d", legacy_ps, opp_ps, ctwindow);
5915 return wpa_driver_nl80211_driver_cmd(priv, buf, reply_buf, sizeof(reply_buf));
5916 }
5917
wpa_driver_set_ap_wps_p2p_ie(void * priv,const struct wpabuf * beacon,const struct wpabuf * proberesp,const struct wpabuf * assocresp)5918 int wpa_driver_set_ap_wps_p2p_ie(void *priv, const struct wpabuf *beacon,
5919 const struct wpabuf *proberesp,
5920 const struct wpabuf *assocresp)
5921 {
5922 UNUSED(priv), UNUSED(beacon), UNUSED(proberesp), UNUSED(assocresp);
5923 return 0;
5924 }
5925