1 #include <errno.h>
2
3 #include "nl80211.h"
4 #include "iw.h"
5 #include <unistd.h>
6
7 SECTION(measurement);
8
put_preamble(struct nl_msg * msg,char * s)9 static int put_preamble(struct nl_msg *msg, char *s)
10 {
11 static const struct {
12 const char *name;
13 unsigned int val;
14 } preamble_map[] = {
15 { .name = "legacy", .val = NL80211_PREAMBLE_LEGACY, },
16 { .name = "ht", .val = NL80211_PREAMBLE_HT, },
17 { .name = "vht", .val = NL80211_PREAMBLE_VHT, },
18 { .name = "dmg", .val = NL80211_PREAMBLE_DMG, },
19 };
20 unsigned int i;
21
22 for (i = 0; i < ARRAY_SIZE(preamble_map); i++) {
23 if (strcasecmp(preamble_map[i].name, s) == 0) {
24 NLA_PUT_U32(msg, NL80211_PMSR_FTM_REQ_ATTR_PREAMBLE,
25 preamble_map[i].val);
26 return 0;
27 }
28 }
29
30 nla_put_failure:
31 return -1;
32 }
33
parse_ftm_target(struct nl_msg * msg,char * str,int peer_index)34 static int parse_ftm_target(struct nl_msg *msg, char *str, int peer_index)
35 {
36 unsigned char addr[ETH_ALEN];
37 int res, consumed;
38 char *bw = NULL, *pos, *tmp, *save_ptr, *delims = " \t\n";
39 struct nlattr *peer, *req, *reqdata, *ftm, *chan;
40 bool report_ap_tsf = false, preamble = false;
41 unsigned int freq = 0, cf1 = 0, cf2 = 0;
42
43 res = sscanf(str, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx%n",
44 &addr[0], &addr[1], &addr[2], &addr[3], &addr[4], &addr[5],
45 &consumed);
46
47 if (res != ETH_ALEN) {
48 printf("Invalid MAC address\n");
49 return HANDLER_RET_USAGE;
50 }
51
52 peer = nla_nest_start(msg, peer_index);
53
54 NLA_PUT(msg, NL80211_PMSR_PEER_ATTR_ADDR, ETH_ALEN, addr);
55
56 req = nla_nest_start(msg, NL80211_PMSR_PEER_ATTR_REQ);
57 if (!req)
58 goto nla_put_failure;
59 reqdata = nla_nest_start(msg, NL80211_PMSR_REQ_ATTR_DATA);
60 if (!reqdata)
61 goto nla_put_failure;
62 ftm = nla_nest_start(msg, NL80211_PMSR_TYPE_FTM);
63 if (!ftm)
64 goto nla_put_failure;
65
66 str += consumed;
67 pos = strtok_r(str, delims, &save_ptr);
68
69 while (pos) {
70 if (strncmp(pos, "cf=", 3) == 0) {
71 freq = strtol(pos + 3, &tmp, 0);
72 if (*tmp) {
73 printf("Invalid cf value!\n");
74 return HANDLER_RET_USAGE;
75 }
76 } else if (strncmp(pos, "bw=", 3) == 0) {
77 bw = pos + 3;
78 } else if (strncmp(pos, "cf1=", 4) == 0) {
79 cf1 = strtol(pos + 4, &tmp, 0);
80 if (*tmp) {
81 printf("Invalid cf1 value!\n");
82 return HANDLER_RET_USAGE;
83 }
84 } else if (strncmp(pos, "cf2=", 4) == 0) {
85 cf2 = strtol(pos + 4, &tmp, 0);
86 if (*tmp) {
87 printf("Invalid cf2 value!\n");
88 return HANDLER_RET_USAGE;
89 }
90 } else if (strncmp(pos, "bursts_exp=", 11) == 0) {
91 NLA_PUT_U8(msg,
92 NL80211_PMSR_FTM_REQ_ATTR_NUM_BURSTS_EXP,
93 strtol(pos + 11, &tmp, 0));
94 if (*tmp) {
95 printf("Invalid bursts_exp value!\n");
96 return HANDLER_RET_USAGE;
97 }
98 } else if (strncmp(pos, "burst_period=", 13) == 0) {
99 NLA_PUT_U16(msg, NL80211_PMSR_FTM_REQ_ATTR_BURST_PERIOD,
100 strtol(pos + 13, &tmp, 0));
101 if (*tmp) {
102 printf("Invalid burst_period value!\n");
103 return HANDLER_RET_USAGE;
104 }
105 } else if (strncmp(pos, "retries=", 8) == 0) {
106 NLA_PUT_U8(msg,
107 NL80211_PMSR_FTM_REQ_ATTR_NUM_FTMR_RETRIES,
108 strtol(pos + 8, &tmp, 0));
109 if (*tmp) {
110 printf("Invalid retries value!\n");
111 return HANDLER_RET_USAGE;
112 }
113 } else if (strncmp(pos, "burst_duration=", 15) == 0) {
114 NLA_PUT_U8(msg,
115 NL80211_PMSR_FTM_REQ_ATTR_BURST_DURATION,
116 strtol(pos + 15, &tmp, 0));
117 if (*tmp) {
118 printf("Invalid burst_duration value!\n");
119 return HANDLER_RET_USAGE;
120 }
121 } else if (strncmp(pos, "ftms_per_burst=", 15) == 0) {
122 NLA_PUT_U8(msg,
123 NL80211_PMSR_FTM_REQ_ATTR_FTMS_PER_BURST,
124 strtol(pos + 15, &tmp, 0));
125 if (*tmp) {
126 printf("Invalid ftms_per_burst value!\n");
127 return HANDLER_RET_USAGE;
128 }
129 } else if (strcmp(pos, "asap") == 0) {
130 NLA_PUT_FLAG(msg, NL80211_PMSR_FTM_REQ_ATTR_ASAP);
131 } else if (strcmp(pos, "ap-tsf") == 0) {
132 report_ap_tsf = true;
133 } else if (strcmp(pos, "civic") == 0) {
134 NLA_PUT_FLAG(msg, NL80211_PMSR_FTM_REQ_ATTR_REQUEST_CIVICLOC);
135 } else if (strcmp(pos, "lci") == 0) {
136 NLA_PUT_FLAG(msg, NL80211_PMSR_FTM_REQ_ATTR_REQUEST_LCI);
137 } else if (strncmp(pos, "preamble=", 9) == 0) {
138 if (put_preamble(msg, pos + 9)) {
139 printf("Invalid preamble %s\n", pos + 9);
140 return HANDLER_RET_USAGE;
141 }
142 preamble = true;
143 } else if (strncmp(pos, "tb", 2) == 0) {
144 NLA_PUT_FLAG(msg,
145 NL80211_PMSR_FTM_REQ_ATTR_TRIGGER_BASED);
146 NLA_PUT_U32(msg, NL80211_PMSR_FTM_REQ_ATTR_PREAMBLE,
147 NL80211_PREAMBLE_HE);
148 preamble = true;
149 } else if (strncmp(pos, "non_tb", 6) == 0) {
150 NLA_PUT_FLAG(msg,
151 NL80211_PMSR_FTM_REQ_ATTR_NON_TRIGGER_BASED);
152 NLA_PUT_U32(msg, NL80211_PMSR_FTM_REQ_ATTR_PREAMBLE,
153 NL80211_PREAMBLE_HE);
154 preamble = true;
155 } else if (strncmp(pos, "lmr_feedback", 12) == 0) {
156 NLA_PUT_FLAG(msg,
157 NL80211_PMSR_FTM_REQ_ATTR_LMR_FEEDBACK);
158 } else if (strncmp(pos, "bss_color=", 10) == 0) {
159 NLA_PUT_U8(msg,
160 NL80211_PMSR_FTM_REQ_ATTR_BSS_COLOR,
161 strtol(pos + 10, &tmp, 0));
162 if (*tmp) {
163 printf("Invalid bss_color value!\n");
164 return HANDLER_RET_USAGE;
165 }
166 } else {
167 printf("Unknown parameter %s\n", pos);
168 return HANDLER_RET_USAGE;
169 }
170
171 pos = strtok_r(NULL, delims, &save_ptr);
172 }
173
174 if (!preamble) {
175 int preamble = -1;
176
177 switch (str_to_bw(bw)) {
178 case NL80211_CHAN_WIDTH_20_NOHT:
179 case NL80211_CHAN_WIDTH_5:
180 case NL80211_CHAN_WIDTH_10:
181 preamble = NL80211_PREAMBLE_LEGACY;
182 break;
183 case NL80211_CHAN_WIDTH_20:
184 case NL80211_CHAN_WIDTH_40:
185 preamble = NL80211_PREAMBLE_HT;
186 break;
187 case NL80211_CHAN_WIDTH_80:
188 case NL80211_CHAN_WIDTH_80P80:
189 case NL80211_CHAN_WIDTH_160:
190 preamble = NL80211_PREAMBLE_VHT;
191 break;
192 default:
193 return HANDLER_RET_USAGE;
194 }
195
196 NLA_PUT_U32(msg, NL80211_PMSR_FTM_REQ_ATTR_PREAMBLE, preamble);
197 }
198
199 nla_nest_end(msg, ftm);
200 if (report_ap_tsf)
201 NLA_PUT_FLAG(msg, NL80211_PMSR_REQ_ATTR_GET_AP_TSF);
202 nla_nest_end(msg, reqdata);
203 nla_nest_end(msg, req);
204
205 /* set the channel */
206 chan = nla_nest_start(msg, NL80211_PMSR_PEER_ATTR_CHAN);
207 if (!chan)
208 goto nla_put_failure;
209 if (freq)
210 NLA_PUT_U32(msg, NL80211_ATTR_WIPHY_FREQ, freq);
211 if (cf1)
212 NLA_PUT_U32(msg, NL80211_ATTR_CENTER_FREQ1, cf1);
213 if (cf2)
214 NLA_PUT_U32(msg, NL80211_ATTR_CENTER_FREQ2, cf2);
215 if (bw)
216 NLA_PUT_U32(msg, NL80211_ATTR_CHANNEL_WIDTH,
217 str_to_bw(bw));
218 nla_nest_end(msg, chan);
219
220 nla_nest_end(msg, peer);
221 return 0;
222 nla_put_failure:
223 return -ENOBUFS;
224 }
225
parse_ftm_config(struct nl_msg * msg,const char * file)226 static int parse_ftm_config(struct nl_msg *msg, const char *file)
227 {
228 FILE *input;
229 char line[256];
230 int line_num;
231
232 input = fopen(file, "r");
233 if (!input) {
234 int err = errno;
235
236 printf("Failed to open file: %s\n", strerror(err));
237 return -err;
238 }
239
240 for (line_num = 1; fgets(line, sizeof(line), input); line_num++) {
241 if (line[0] == '#')
242 continue;
243
244 if (parse_ftm_target(msg, line, line_num)) {
245 printf("Invalid FTM configuration at line %d!\n",
246 line_num);
247 return HANDLER_RET_USAGE;
248 }
249 }
250
251 return 0;
252 }
253
handle_ftm_req(struct nl80211_state * state,struct nl_msg * msg,int argc,char ** argv,enum id_input id)254 static int handle_ftm_req(struct nl80211_state *state, struct nl_msg *msg,
255 int argc, char **argv, enum id_input id)
256 {
257 int err, i;
258 static char **req_argv;
259 static const __u32 wait[] = {
260 NL80211_CMD_PEER_MEASUREMENT_COMPLETE,
261 };
262 static const __u32 print[] = {
263 NL80211_CMD_PEER_MEASUREMENT_RESULT,
264 NL80211_CMD_PEER_MEASUREMENT_COMPLETE,
265 };
266 struct print_event_args printargs = { };
267
268 req_argv = calloc(argc + 1, sizeof(req_argv[0]));
269 req_argv[0] = argv[0];
270 req_argv[1] = "measurement";
271 req_argv[2] = "ftm_request_send";
272 for (i = 3; i < argc; i++)
273 req_argv[i] = argv[i];
274
275 err = handle_cmd(state, id, argc, req_argv);
276
277 free(req_argv);
278
279 if (err)
280 return err;
281
282 __do_listen_events(state,
283 ARRAY_SIZE(wait), wait,
284 ARRAY_SIZE(print), print,
285 &printargs);
286 return 0;
287 }
288
handle_ftm_req_send(struct nl80211_state * state,struct nl_msg * msg,int argc,char ** argv,enum id_input id)289 static int handle_ftm_req_send(struct nl80211_state *state, struct nl_msg *msg,
290 int argc, char **argv, enum id_input id)
291 {
292 struct nlattr *pmsr, *peers;
293 const char *file;
294 int err;
295
296 if (argc < 1)
297 return HANDLER_RET_USAGE;
298
299 file = argv[0];
300 argc--;
301 argv++;
302 while (argc) {
303 if (strncmp(argv[0], "randomise", 9) == 0 ||
304 strncmp(argv[0], "randomize", 9) == 0) {
305 err = parse_random_mac_addr(msg, argv[0] + 9);
306 if (err)
307 return err;
308 } else if (strncmp(argv[0], "timeout=", 8) == 0) {
309 char *end;
310
311 NLA_PUT_U32(msg, NL80211_ATTR_TIMEOUT,
312 strtoul(argv[0] + 8, &end, 0));
313 if (*end)
314 return HANDLER_RET_USAGE;
315 } else {
316 return HANDLER_RET_USAGE;
317 }
318
319 argc--;
320 argv++;
321 }
322
323 pmsr = nla_nest_start(msg, NL80211_ATTR_PEER_MEASUREMENTS);
324 if (!pmsr)
325 goto nla_put_failure;
326 peers = nla_nest_start(msg, NL80211_PMSR_ATTR_PEERS);
327 if (!peers)
328 goto nla_put_failure;
329
330 err = parse_ftm_config(msg, file);
331 if (err)
332 return err;
333
334 nla_nest_end(msg, peers);
335 nla_nest_end(msg, pmsr);
336
337 return 0;
338
339 nla_put_failure:
340 return -ENOBUFS;
341 }
342 COMMAND(measurement, ftm_request, "<config-file> [timeout=<seconds>] [randomise[=<addr>/<mask>]]", 0, 0,
343 CIB_NETDEV, handle_ftm_req,
344 "Send an FTM request to the targets supplied in the config file.\n"
345 "Each line in the file represents a target, with the following format:\n"
346 "<addr> bw=<[20|40|80|80+80|160]> cf=<center_freq> [cf1=<center_freq1>] [cf2=<center_freq2>] [ftms_per_burst=<samples per burst>] [ap-tsf] [asap] [bursts_exp=<num of bursts exponent>] [burst_period=<burst period>] [retries=<num of retries>] [burst_duration=<burst duration>] [preamble=<legacy,ht,vht,dmg>] [lci] [civic] [tb] [non_tb]");
347 HIDDEN(measurement, ftm_request_send, "", NL80211_CMD_PEER_MEASUREMENT_START,
348 0, CIB_NETDEV, handle_ftm_req_send);
349