349 lines
9.2 KiB
C
349 lines
9.2 KiB
C
|
#include <errno.h>
|
||
|
|
|
||
|
|
#include "nl80211.h"
|
||
|
|
#include "iw.h"
|
||
|
|
#include <unistd.h>
|
||
|
|
|
||
|
|
SECTION(measurement);
|
||
|
|
|
||
|
|
static int put_preamble(struct nl_msg *msg, char *s)
|
||
|
|
{
|
||
|
|
static const struct {
|
||
|
|
const char *name;
|
||
|
|
unsigned int val;
|
||
|
|
} preamble_map[] = {
|
||
|
|
{ .name = "legacy", .val = NL80211_PREAMBLE_LEGACY, },
|
||
|
|
{ .name = "ht", .val = NL80211_PREAMBLE_HT, },
|
||
|
|
{ .name = "vht", .val = NL80211_PREAMBLE_VHT, },
|
||
|
|
{ .name = "dmg", .val = NL80211_PREAMBLE_DMG, },
|
||
|
|
};
|
||
|
|
unsigned int i;
|
||
|
|
|
||
|
|
for (i = 0; i < ARRAY_SIZE(preamble_map); i++) {
|
||
|
|
if (strcasecmp(preamble_map[i].name, s) == 0) {
|
||
|
|
NLA_PUT_U32(msg, NL80211_PMSR_FTM_REQ_ATTR_PREAMBLE,
|
||
|
|
preamble_map[i].val);
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
nla_put_failure:
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
|
||
|
|
static int parse_ftm_target(struct nl_msg *msg, char *str, int peer_index)
|
||
|
|
{
|
||
|
|
unsigned char addr[ETH_ALEN];
|
||
|
|
int res, consumed;
|
||
|
|
char *bw = NULL, *pos, *tmp, *save_ptr, *delims = " \t\n";
|
||
|
|
struct nlattr *peer, *req, *reqdata, *ftm, *chan;
|
||
|
|
bool report_ap_tsf = false, preamble = false;
|
||
|
|
unsigned int freq = 0, cf1 = 0, cf2 = 0;
|
||
|
|
|
||
|
|
res = sscanf(str, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx%n",
|
||
|
|
&addr[0], &addr[1], &addr[2], &addr[3], &addr[4], &addr[5],
|
||
|
|
&consumed);
|
||
|
|
|
||
|
|
if (res != ETH_ALEN) {
|
||
|
|
printf("Invalid MAC address\n");
|
||
|
|
return HANDLER_RET_USAGE;
|
||
|
|
}
|
||
|
|
|
||
|
|
peer = nla_nest_start(msg, peer_index);
|
||
|
|
|
||
|
|
NLA_PUT(msg, NL80211_PMSR_PEER_ATTR_ADDR, ETH_ALEN, addr);
|
||
|
|
|
||
|
|
req = nla_nest_start(msg, NL80211_PMSR_PEER_ATTR_REQ);
|
||
|
|
if (!req)
|
||
|
|
goto nla_put_failure;
|
||
|
|
reqdata = nla_nest_start(msg, NL80211_PMSR_REQ_ATTR_DATA);
|
||
|
|
if (!reqdata)
|
||
|
|
goto nla_put_failure;
|
||
|
|
ftm = nla_nest_start(msg, NL80211_PMSR_TYPE_FTM);
|
||
|
|
if (!ftm)
|
||
|
|
goto nla_put_failure;
|
||
|
|
|
||
|
|
str += consumed;
|
||
|
|
pos = strtok_r(str, delims, &save_ptr);
|
||
|
|
|
||
|
|
while (pos) {
|
||
|
|
if (strncmp(pos, "cf=", 3) == 0) {
|
||
|
|
freq = strtol(pos + 3, &tmp, 0);
|
||
|
|
if (*tmp) {
|
||
|
|
printf("Invalid cf value!\n");
|
||
|
|
return HANDLER_RET_USAGE;
|
||
|
|
}
|
||
|
|
} else if (strncmp(pos, "bw=", 3) == 0) {
|
||
|
|
bw = pos + 3;
|
||
|
|
} else if (strncmp(pos, "cf1=", 4) == 0) {
|
||
|
|
cf1 = strtol(pos + 4, &tmp, 0);
|
||
|
|
if (*tmp) {
|
||
|
|
printf("Invalid cf1 value!\n");
|
||
|
|
return HANDLER_RET_USAGE;
|
||
|
|
}
|
||
|
|
} else if (strncmp(pos, "cf2=", 4) == 0) {
|
||
|
|
cf2 = strtol(pos + 4, &tmp, 0);
|
||
|
|
if (*tmp) {
|
||
|
|
printf("Invalid cf2 value!\n");
|
||
|
|
return HANDLER_RET_USAGE;
|
||
|
|
}
|
||
|
|
} else if (strncmp(pos, "bursts_exp=", 11) == 0) {
|
||
|
|
NLA_PUT_U8(msg,
|
||
|
|
NL80211_PMSR_FTM_REQ_ATTR_NUM_BURSTS_EXP,
|
||
|
|
strtol(pos + 11, &tmp, 0));
|
||
|
|
if (*tmp) {
|
||
|
|
printf("Invalid bursts_exp value!\n");
|
||
|
|
return HANDLER_RET_USAGE;
|
||
|
|
}
|
||
|
|
} else if (strncmp(pos, "burst_period=", 13) == 0) {
|
||
|
|
NLA_PUT_U16(msg, NL80211_PMSR_FTM_REQ_ATTR_BURST_PERIOD,
|
||
|
|
strtol(pos + 13, &tmp, 0));
|
||
|
|
if (*tmp) {
|
||
|
|
printf("Invalid burst_period value!\n");
|
||
|
|
return HANDLER_RET_USAGE;
|
||
|
|
}
|
||
|
|
} else if (strncmp(pos, "retries=", 8) == 0) {
|
||
|
|
NLA_PUT_U8(msg,
|
||
|
|
NL80211_PMSR_FTM_REQ_ATTR_NUM_FTMR_RETRIES,
|
||
|
|
strtol(pos + 8, &tmp, 0));
|
||
|
|
if (*tmp) {
|
||
|
|
printf("Invalid retries value!\n");
|
||
|
|
return HANDLER_RET_USAGE;
|
||
|
|
}
|
||
|
|
} else if (strncmp(pos, "burst_duration=", 15) == 0) {
|
||
|
|
NLA_PUT_U8(msg,
|
||
|
|
NL80211_PMSR_FTM_REQ_ATTR_BURST_DURATION,
|
||
|
|
strtol(pos + 15, &tmp, 0));
|
||
|
|
if (*tmp) {
|
||
|
|
printf("Invalid burst_duration value!\n");
|
||
|
|
return HANDLER_RET_USAGE;
|
||
|
|
}
|
||
|
|
} else if (strncmp(pos, "ftms_per_burst=", 15) == 0) {
|
||
|
|
NLA_PUT_U8(msg,
|
||
|
|
NL80211_PMSR_FTM_REQ_ATTR_FTMS_PER_BURST,
|
||
|
|
strtol(pos + 15, &tmp, 0));
|
||
|
|
if (*tmp) {
|
||
|
|
printf("Invalid ftms_per_burst value!\n");
|
||
|
|
return HANDLER_RET_USAGE;
|
||
|
|
}
|
||
|
|
} else if (strcmp(pos, "asap") == 0) {
|
||
|
|
NLA_PUT_FLAG(msg, NL80211_PMSR_FTM_REQ_ATTR_ASAP);
|
||
|
|
} else if (strcmp(pos, "ap-tsf") == 0) {
|
||
|
|
report_ap_tsf = true;
|
||
|
|
} else if (strcmp(pos, "civic") == 0) {
|
||
|
|
NLA_PUT_FLAG(msg, NL80211_PMSR_FTM_REQ_ATTR_REQUEST_CIVICLOC);
|
||
|
|
} else if (strcmp(pos, "lci") == 0) {
|
||
|
|
NLA_PUT_FLAG(msg, NL80211_PMSR_FTM_REQ_ATTR_REQUEST_LCI);
|
||
|
|
} else if (strncmp(pos, "preamble=", 9) == 0) {
|
||
|
|
if (put_preamble(msg, pos + 9)) {
|
||
|
|
printf("Invalid preamble %s\n", pos + 9);
|
||
|
|
return HANDLER_RET_USAGE;
|
||
|
|
}
|
||
|
|
preamble = true;
|
||
|
|
} else if (strncmp(pos, "tb", 2) == 0) {
|
||
|
|
NLA_PUT_FLAG(msg,
|
||
|
|
NL80211_PMSR_FTM_REQ_ATTR_TRIGGER_BASED);
|
||
|
|
NLA_PUT_U32(msg, NL80211_PMSR_FTM_REQ_ATTR_PREAMBLE,
|
||
|
|
NL80211_PREAMBLE_HE);
|
||
|
|
preamble = true;
|
||
|
|
} else if (strncmp(pos, "non_tb", 6) == 0) {
|
||
|
|
NLA_PUT_FLAG(msg,
|
||
|
|
NL80211_PMSR_FTM_REQ_ATTR_NON_TRIGGER_BASED);
|
||
|
|
NLA_PUT_U32(msg, NL80211_PMSR_FTM_REQ_ATTR_PREAMBLE,
|
||
|
|
NL80211_PREAMBLE_HE);
|
||
|
|
preamble = true;
|
||
|
|
} else if (strncmp(pos, "lmr_feedback", 12) == 0) {
|
||
|
|
NLA_PUT_FLAG(msg,
|
||
|
|
NL80211_PMSR_FTM_REQ_ATTR_LMR_FEEDBACK);
|
||
|
|
} else if (strncmp(pos, "bss_color=", 10) == 0) {
|
||
|
|
NLA_PUT_U8(msg,
|
||
|
|
NL80211_PMSR_FTM_REQ_ATTR_BSS_COLOR,
|
||
|
|
strtol(pos + 10, &tmp, 0));
|
||
|
|
if (*tmp) {
|
||
|
|
printf("Invalid bss_color value!\n");
|
||
|
|
return HANDLER_RET_USAGE;
|
||
|
|
}
|
||
|
|
} else {
|
||
|
|
printf("Unknown parameter %s\n", pos);
|
||
|
|
return HANDLER_RET_USAGE;
|
||
|
|
}
|
||
|
|
|
||
|
|
pos = strtok_r(NULL, delims, &save_ptr);
|
||
|
|
}
|
||
|
|
|
||
|
|
if (!preamble) {
|
||
|
|
int preamble = -1;
|
||
|
|
|
||
|
|
switch (str_to_bw(bw)) {
|
||
|
|
case NL80211_CHAN_WIDTH_20_NOHT:
|
||
|
|
case NL80211_CHAN_WIDTH_5:
|
||
|
|
case NL80211_CHAN_WIDTH_10:
|
||
|
|
preamble = NL80211_PREAMBLE_LEGACY;
|
||
|
|
break;
|
||
|
|
case NL80211_CHAN_WIDTH_20:
|
||
|
|
case NL80211_CHAN_WIDTH_40:
|
||
|
|
preamble = NL80211_PREAMBLE_HT;
|
||
|
|
break;
|
||
|
|
case NL80211_CHAN_WIDTH_80:
|
||
|
|
case NL80211_CHAN_WIDTH_80P80:
|
||
|
|
case NL80211_CHAN_WIDTH_160:
|
||
|
|
preamble = NL80211_PREAMBLE_VHT;
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
return HANDLER_RET_USAGE;
|
||
|
|
}
|
||
|
|
|
||
|
|
NLA_PUT_U32(msg, NL80211_PMSR_FTM_REQ_ATTR_PREAMBLE, preamble);
|
||
|
|
}
|
||
|
|
|
||
|
|
nla_nest_end(msg, ftm);
|
||
|
|
if (report_ap_tsf)
|
||
|
|
NLA_PUT_FLAG(msg, NL80211_PMSR_REQ_ATTR_GET_AP_TSF);
|
||
|
|
nla_nest_end(msg, reqdata);
|
||
|
|
nla_nest_end(msg, req);
|
||
|
|
|
||
|
|
/* set the channel */
|
||
|
|
chan = nla_nest_start(msg, NL80211_PMSR_PEER_ATTR_CHAN);
|
||
|
|
if (!chan)
|
||
|
|
goto nla_put_failure;
|
||
|
|
if (freq)
|
||
|
|
NLA_PUT_U32(msg, NL80211_ATTR_WIPHY_FREQ, freq);
|
||
|
|
if (cf1)
|
||
|
|
NLA_PUT_U32(msg, NL80211_ATTR_CENTER_FREQ1, cf1);
|
||
|
|
if (cf2)
|
||
|
|
NLA_PUT_U32(msg, NL80211_ATTR_CENTER_FREQ2, cf2);
|
||
|
|
if (bw)
|
||
|
|
NLA_PUT_U32(msg, NL80211_ATTR_CHANNEL_WIDTH,
|
||
|
|
str_to_bw(bw));
|
||
|
|
nla_nest_end(msg, chan);
|
||
|
|
|
||
|
|
nla_nest_end(msg, peer);
|
||
|
|
return 0;
|
||
|
|
nla_put_failure:
|
||
|
|
return -ENOBUFS;
|
||
|
|
}
|
||
|
|
|
||
|
|
static int parse_ftm_config(struct nl_msg *msg, const char *file)
|
||
|
|
{
|
||
|
|
FILE *input;
|
||
|
|
char line[256];
|
||
|
|
int line_num;
|
||
|
|
|
||
|
|
input = fopen(file, "r");
|
||
|
|
if (!input) {
|
||
|
|
int err = errno;
|
||
|
|
|
||
|
|
printf("Failed to open file: %s\n", strerror(err));
|
||
|
|
return -err;
|
||
|
|
}
|
||
|
|
|
||
|
|
for (line_num = 1; fgets(line, sizeof(line), input); line_num++) {
|
||
|
|
if (line[0] == '#')
|
||
|
|
continue;
|
||
|
|
|
||
|
|
if (parse_ftm_target(msg, line, line_num)) {
|
||
|
|
printf("Invalid FTM configuration at line %d!\n",
|
||
|
|
line_num);
|
||
|
|
return HANDLER_RET_USAGE;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
static int handle_ftm_req(struct nl80211_state *state, struct nl_msg *msg,
|
||
|
|
int argc, char **argv, enum id_input id)
|
||
|
|
{
|
||
|
|
int err, i;
|
||
|
|
static char **req_argv;
|
||
|
|
static const __u32 wait[] = {
|
||
|
|
NL80211_CMD_PEER_MEASUREMENT_COMPLETE,
|
||
|
|
};
|
||
|
|
static const __u32 print[] = {
|
||
|
|
NL80211_CMD_PEER_MEASUREMENT_RESULT,
|
||
|
|
NL80211_CMD_PEER_MEASUREMENT_COMPLETE,
|
||
|
|
};
|
||
|
|
struct print_event_args printargs = { };
|
||
|
|
|
||
|
|
req_argv = calloc(argc + 1, sizeof(req_argv[0]));
|
||
|
|
req_argv[0] = argv[0];
|
||
|
|
req_argv[1] = "measurement";
|
||
|
|
req_argv[2] = "ftm_request_send";
|
||
|
|
for (i = 3; i < argc; i++)
|
||
|
|
req_argv[i] = argv[i];
|
||
|
|
|
||
|
|
err = handle_cmd(state, id, argc, req_argv);
|
||
|
|
|
||
|
|
free(req_argv);
|
||
|
|
|
||
|
|
if (err)
|
||
|
|
return err;
|
||
|
|
|
||
|
|
__do_listen_events(state,
|
||
|
|
ARRAY_SIZE(wait), wait,
|
||
|
|
ARRAY_SIZE(print), print,
|
||
|
|
&printargs);
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
static int handle_ftm_req_send(struct nl80211_state *state, struct nl_msg *msg,
|
||
|
|
int argc, char **argv, enum id_input id)
|
||
|
|
{
|
||
|
|
struct nlattr *pmsr, *peers;
|
||
|
|
const char *file;
|
||
|
|
int err;
|
||
|
|
|
||
|
|
if (argc < 1)
|
||
|
|
return HANDLER_RET_USAGE;
|
||
|
|
|
||
|
|
file = argv[0];
|
||
|
|
argc--;
|
||
|
|
argv++;
|
||
|
|
while (argc) {
|
||
|
|
if (strncmp(argv[0], "randomise", 9) == 0 ||
|
||
|
|
strncmp(argv[0], "randomize", 9) == 0) {
|
||
|
|
err = parse_random_mac_addr(msg, argv[0] + 9);
|
||
|
|
if (err)
|
||
|
|
return err;
|
||
|
|
} else if (strncmp(argv[0], "timeout=", 8) == 0) {
|
||
|
|
char *end;
|
||
|
|
|
||
|
|
NLA_PUT_U32(msg, NL80211_ATTR_TIMEOUT,
|
||
|
|
strtoul(argv[0] + 8, &end, 0));
|
||
|
|
if (*end)
|
||
|
|
return HANDLER_RET_USAGE;
|
||
|
|
} else {
|
||
|
|
return HANDLER_RET_USAGE;
|
||
|
|
}
|
||
|
|
|
||
|
|
argc--;
|
||
|
|
argv++;
|
||
|
|
}
|
||
|
|
|
||
|
|
pmsr = nla_nest_start(msg, NL80211_ATTR_PEER_MEASUREMENTS);
|
||
|
|
if (!pmsr)
|
||
|
|
goto nla_put_failure;
|
||
|
|
peers = nla_nest_start(msg, NL80211_PMSR_ATTR_PEERS);
|
||
|
|
if (!peers)
|
||
|
|
goto nla_put_failure;
|
||
|
|
|
||
|
|
err = parse_ftm_config(msg, file);
|
||
|
|
if (err)
|
||
|
|
return err;
|
||
|
|
|
||
|
|
nla_nest_end(msg, peers);
|
||
|
|
nla_nest_end(msg, pmsr);
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
|
||
|
|
nla_put_failure:
|
||
|
|
return -ENOBUFS;
|
||
|
|
}
|
||
|
|
COMMAND(measurement, ftm_request, "<config-file> [timeout=<seconds>] [randomise[=<addr>/<mask>]]", 0, 0,
|
||
|
|
CIB_NETDEV, handle_ftm_req,
|
||
|
|
"Send an FTM request to the targets supplied in the config file.\n"
|
||
|
|
"Each line in the file represents a target, with the following format:\n"
|
||
|
|
"<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]");
|
||
|
|
HIDDEN(measurement, ftm_request_send, "", NL80211_CMD_PEER_MEASUREMENT_START,
|
||
|
|
0, CIB_NETDEV, handle_ftm_req_send);
|