measurements.c

   1 #include <errno.h>
   2
   3 #include "nl80211.h"
   4 #include "iw.h"
   5 #include <unistd.h>
   6
   7 SECTION(measurement);
   8
   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
  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
 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
 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
 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);