wowlan.c

   1 #include <errno.h>
   2 #include <string.h>
   3 #include <stdio.h>
   4
   5 #include <netlink/genl/genl.h>
   6 #include <netlink/genl/family.h>
   7 #include <netlink/genl/ctrl.h>
   8 #include <netlink/msg.h>
   9 #include <netlink/attr.h>
  10
  11 #include <arpa/inet.h>
  12
  13 #include "nl80211.h"
  14 #include "iw.h"
  15
  16 SECTION(wowlan);
  17
  18 static int wowlan_parse_tcp_file(struct nl_msg *msg, const char *fn)
  19 {
  20         char buf[16768];
  21         int err = 1;
  22         FILE *f = fopen(fn, "r");
  23         struct nlattr *tcp;
  24
  25         if (!f)
  26                 return 1;
  27         tcp = nla_nest_start(msg, NL80211_WOWLAN_TRIG_TCP_CONNECTION);
  28         if (!tcp)
  29                 goto nla_put_failure;
  30
  31         while (!feof(f)) {
  32                 char *eol;
  33
  34                 if (!fgets(buf, sizeof(buf), f))
  35                         break;
  36
  37                 eol = strchr(buf + 5, '\r');
  38                 if (eol)
  39                         *eol = 0;
  40                 eol = strchr(buf + 5, '\n');
  41                 if (eol)
  42                         *eol = 0;
  43
  44                 if (strncmp(buf, "source=", 7) == 0) {
  45                         struct in_addr in_addr;
  46                         char *addr = buf + 7;
  47                         char *port = strchr(buf + 7, ':');
  48
  49                         if (port) {
  50                                 *port = 0;
  51                                 port++;
  52                         }
  53                         if (inet_aton(addr, &in_addr) == 0)
  54                                 goto close;
  55                         NLA_PUT_U32(msg, NL80211_WOWLAN_TCP_SRC_IPV4,
  56                                     in_addr.s_addr);
  57                         if (port)
  58                                 NLA_PUT_U16(msg, NL80211_WOWLAN_TCP_SRC_PORT,
  59                                             atoi(port));
  60                 } else if (strncmp(buf, "dest=", 5) == 0) {
  61                         struct in_addr in_addr;
  62                         char *addr = buf + 5;
  63                         char *port = strchr(buf + 5, ':');
  64                         char *mac;
  65                         unsigned char macbuf[6];
  66
  67                         if (!port)
  68                                 goto close;
  69                         *port = 0;
  70                         port++;
  71                         mac = strchr(port, '@');
  72                         if (!mac)
  73                                 goto close;
  74                         *mac = 0;
  75                         mac++;
  76                         if (inet_aton(addr, &in_addr) == 0)
  77                                 goto close;
  78                         NLA_PUT_U32(msg, NL80211_WOWLAN_TCP_DST_IPV4,
  79                                     in_addr.s_addr);
  80                         NLA_PUT_U16(msg, NL80211_WOWLAN_TCP_DST_PORT,
  81                                     atoi(port));
  82                         if (mac_addr_a2n(macbuf, mac))
  83                                 goto close;
  84                         NLA_PUT(msg, NL80211_WOWLAN_TCP_DST_MAC,
  85                                 6, macbuf);
  86                 } else if (strncmp(buf, "data=", 5) == 0) {
  87                         size_t len;
  88                         unsigned char *pkt = parse_hex(buf + 5, &len);
  89
  90                         if (!pkt)
  91                                 goto close;
  92                         NLA_PUT(msg, NL80211_WOWLAN_TCP_DATA_PAYLOAD, len, pkt);
  93                         free(pkt);
  94                 } else if (strncmp(buf, "data.interval=", 14) == 0) {
  95                         NLA_PUT_U32(msg, NL80211_WOWLAN_TCP_DATA_INTERVAL,
  96                                     atoi(buf + 14));
  97                 } else if (strncmp(buf, "wake=", 5) == 0) {
  98                         unsigned char *pat, *mask;
  99                         size_t patlen;
 100
 101                         if (parse_hex_mask(buf + 5, &pat, &patlen, &mask))
 102                                 goto close;
 103                         NLA_PUT(msg, NL80211_WOWLAN_TCP_WAKE_MASK,
 104                                 DIV_ROUND_UP(patlen, 8), mask);
 105                         NLA_PUT(msg, NL80211_WOWLAN_TCP_WAKE_PAYLOAD,
 106                                 patlen, pat);
 107                         free(mask);
 108                         free(pat);
 109                 } else if (strncmp(buf, "data.seq=", 9) == 0) {
 110                         struct nl80211_wowlan_tcp_data_seq seq = {};
 111                         char *len, *offs, *start;
 112
 113                         len = buf + 9;
 114                         offs = strchr(len, ',');
 115                         if (!offs)
 116                                 goto close;
 117                         *offs = 0;
 118                         offs++;
 119                         start = strchr(offs, ',');
 120                         if (start) {
 121                                 *start = 0;
 122                                 start++;
 123                                 seq.start = atoi(start);
 124                         }
 125                         seq.len = atoi(len);
 126                         seq.offset = atoi(offs);
 127
 128                         NLA_PUT(msg, NL80211_WOWLAN_TCP_DATA_PAYLOAD_SEQ,
 129                                 sizeof(seq), &seq);
 130                 } else if (strncmp(buf, "data.tok=", 9) == 0) {
 131                         struct nl80211_wowlan_tcp_data_token *tok;
 132                         size_t stream_len;
 133                         char *len, *offs, *toks;
 134                         unsigned char *stream;
 135
 136                         len = buf + 9;
 137                         offs = strchr(len, ',');
 138                         if (!offs)
 139                                 goto close;
 140                         *offs = 0;
 141                         offs++;
 142                         toks = strchr(offs, ',');
 143                         if (!toks)
 144                                 goto close;
 145                         *toks = 0;
 146                         toks++;
 147
 148                         stream = parse_hex(toks, &stream_len);
 149                         if (!stream)
 150                                 goto close;
 151                         tok = malloc(sizeof(*tok) + stream_len);
 152                         if (!tok) {
 153                                 free(stream);
 154                                 err = -ENOMEM;
 155                                 goto close;
 156                         }
 157
 158                         tok->len = atoi(len);
 159                         tok->offset = atoi(offs);
 160                         memcpy(tok->token_stream, stream, stream_len);
 161
 162                         if (nla_put(msg, NL80211_WOWLAN_TCP_DATA_PAYLOAD_TOKEN,
 163                                 sizeof(*tok) + stream_len, tok) < 0) {
 164                                 free(stream);
 165                                 free(tok);
 166                                 goto nla_put_failure;
 167                         }
 168                         free(stream);
 169                         free(tok);
 170                 } else {
 171                         if (buf[0] == '#')
 172                                 continue;
 173                         goto close;
 174                 }
 175         }
 176
 177         err = 0;
 178         goto close;
 179  nla_put_failure:
 180         err = -ENOBUFS;
 181  close:
 182         fclose(f);
 183         nla_nest_end(msg, tcp);
 184         return err;
 185 }
 186
 187 static int wowlan_parse_net_detect(struct nl_msg *msg, int *argc, char ***argv)
 188 {
 189         struct nlattr *nd;
 190         int err = 0;
 191
 192         nd = nla_nest_start(msg, NL80211_WOWLAN_TRIG_NET_DETECT);
 193         if (!nd)
 194                 return -ENOBUFS;
 195
 196         err = parse_sched_scan(msg, argc, argv);
 197
 198         nla_nest_end(msg, nd);
 199
 200         return err;
 201 }
 202
 203 static int handle_wowlan_enable(struct nl80211_state *state,
 204                                 struct nl_msg *msg, int argc, char **argv,
 205                                 enum id_input id)
 206 {
 207         struct nlattr *wowlan, *pattern;
 208         struct nl_msg *patterns = NULL;
 209         enum {
 210                 PS_REG,
 211                 PS_PAT,
 212         } parse_state = PS_REG;
 213         int err = -ENOBUFS;
 214         unsigned char *pat, *mask;
 215         size_t patlen;
 216         int patnum = 0, pkt_offset;
 217         char *eptr, *value1, *value2, *sptr = NULL;
 218
 219         wowlan = nla_nest_start(msg, NL80211_ATTR_WOWLAN_TRIGGERS);
 220         if (!wowlan)
 221                 return -ENOBUFS;
 222
 223         while (argc) {
 224                 switch (parse_state) {
 225                 case PS_REG:
 226                         if (strcmp(argv[0], "any") == 0)
 227                                 NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_ANY);
 228                         else if (strcmp(argv[0], "disconnect") == 0)
 229                                 NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_DISCONNECT);
 230                         else if (strcmp(argv[0], "magic-packet") == 0)
 231                                 NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_MAGIC_PKT);
 232                         else if (strcmp(argv[0], "gtk-rekey-failure") == 0)
 233                                 NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_GTK_REKEY_FAILURE);
 234                         else if (strcmp(argv[0], "eap-identity-request") == 0)
 235                                 NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_EAP_IDENT_REQUEST);
 236                         else if (strcmp(argv[0], "4way-handshake") == 0)
 237                                 NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_4WAY_HANDSHAKE);
 238                         else if (strcmp(argv[0], "rfkill-release") == 0)
 239                                 NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_RFKILL_RELEASE);
 240                         else if (strcmp(argv[0], "tcp") == 0) {
 241                                 argv++;
 242                                 argc--;
 243                                 if (!argc) {
 244                                         err = 1;
 245                                         goto nla_put_failure;
 246                                 }
 247                                 err = wowlan_parse_tcp_file(msg, argv[0]);
 248                                 if (err)
 249                                         goto nla_put_failure;
 250                         } else if (strcmp(argv[0], "patterns") == 0) {
 251                                 parse_state = PS_PAT;
 252                                 patterns = nlmsg_alloc();
 253                                 if (!patterns) {
 254                                         err = -ENOMEM;
 255                                         goto nla_put_failure;
 256                                 }
 257                         } else if (strcmp(argv[0], "net-detect") == 0) {
 258                                 argv++;
 259                                 argc--;
 260                                 if (!argc) {
 261                                         err = 1;
 262                                         goto nla_put_failure;
 263                                 }
 264                                 err = wowlan_parse_net_detect(msg, &argc, &argv);
 265                                 if (err)
 266                                         goto nla_put_failure;
 267                                 continue;
 268                         } else {
 269                                 err = 1;
 270                                 goto nla_put_failure;
 271                         }
 272                         break;
 273                 case PS_PAT:
 274                         value1 = strtok_r(argv[0], "+", &sptr);
 275                         value2 = strtok_r(NULL, "+", &sptr);
 276
 277                         if (!value2) {
 278                                 pkt_offset = 0;
 279                                 value2 = value1;
 280                         } else {
 281                                 pkt_offset = strtoul(value1, &eptr, 10);
 282                                 if (eptr != value1 + strlen(value1)) {
 283                                         err = 1;
 284                                         goto nla_put_failure;
 285                                 }
 286                         }
 287
 288                         if (parse_hex_mask(value2, &pat, &patlen, &mask)) {
 289                                 err = 1;
 290                                 goto nla_put_failure;
 291                         }
 292
 293                         pattern = nla_nest_start(patterns, ++patnum);
 294                         NLA_PUT(patterns, NL80211_PKTPAT_MASK,
 295                                 DIV_ROUND_UP(patlen, 8), mask);
 296                         NLA_PUT(patterns, NL80211_PKTPAT_PATTERN, patlen, pat);
 297                         NLA_PUT_U32(patterns, NL80211_PKTPAT_OFFSET,
 298                                     pkt_offset);
 299                         nla_nest_end(patterns, pattern);
 300                         free(mask);
 301                         free(pat);
 302                         break;
 303                 }
 304                 argv++;
 305                 argc--;
 306         }
 307
 308         if (patterns)
 309                 nla_put_nested(msg, NL80211_WOWLAN_TRIG_PKT_PATTERN,
 310                                 patterns);
 311
 312         nla_nest_end(msg, wowlan);
 313         err = 0;
 314  nla_put_failure:
 315         nlmsg_free(patterns);
 316         return err;
 317 }
 318 COMMAND(wowlan, enable, "[any] [disconnect] [magic-packet] [gtk-rekey-failure] [eap-identity-request]"
 319         " [4way-handshake] [rfkill-release] [net-detect " SCHED_SCAN_OPTIONS "]"
 320         " [tcp <config-file>] [patterns [offset1+]<pattern1> ...]",
 321         NL80211_CMD_SET_WOWLAN, 0, CIB_PHY, handle_wowlan_enable,
 322         "Enable WoWLAN with the given triggers.\n"
 323         "Each pattern is given as a bytestring with '-' in places where any byte\n"
 324         "may be present, e.g. 00:11:22:-:44 will match 00:11:22:33:44 and\n"
 325         "00:11:22:33:ff:44 etc.\n"
 326         "Offset and pattern should be separated by '+', e.g. 18+43:34:00:12 will match "
 327         "'43:34:00:12' after 18 bytes of offset in Rx packet.\n\n"
 328         "The TCP configuration file contains:\n"
 329         "  source=ip[:port]\n"
 330         "  dest=ip:port@mac\n"
 331         "  data=<hex data packet>\n"
 332         "  data.interval=seconds\n"
 333         "  [wake=<hex packet with masked out bytes indicated by '-'>]\n"
 334         "  [data.seq=len,offset[,start]]\n"
 335         "  [data.tok=len,offset,<token stream>]\n\n"
 336         "Net-detect configuration example:\n"
 337         " iw phy0 wowlan enable net-detect interval 5000 delay 30 freqs 2412 2422 matches ssid foo ssid bar");
 338
 339
 340 static int handle_wowlan_disable(struct nl80211_state *state,
 341                                  struct nl_msg *msg, int argc, char **argv,
 342                                  enum id_input id)
 343 {
 344         /* just a set w/o wowlan attribute */
 345         return 0;
 346 }
 347 COMMAND(wowlan, disable, "", NL80211_CMD_SET_WOWLAN, 0, CIB_PHY, handle_wowlan_disable,
 348         "Disable WoWLAN.");
 349
 350
 351 static int print_wowlan_handler(struct nl_msg *msg, void *arg)
 352 {
 353         struct nlattr *attrs[NL80211_ATTR_MAX + 1];
 354         struct nlattr *trig[NUM_NL80211_WOWLAN_TRIG];
 355         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 356         struct nlattr *pattern;
 357         int rem_pattern;
 358
 359         nla_parse(attrs, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
 360                   genlmsg_attrlen(gnlh, 0), NULL);
 361
 362         if (!attrs[NL80211_ATTR_WOWLAN_TRIGGERS]) {
 363                 printf("WoWLAN is disabled.\n");
 364                 return NL_SKIP;
 365         }
 366
 367         /* XXX: use policy */
 368         nla_parse(trig, MAX_NL80211_WOWLAN_TRIG,
 369                   nla_data(attrs[NL80211_ATTR_WOWLAN_TRIGGERS]),
 370                   nla_len(attrs[NL80211_ATTR_WOWLAN_TRIGGERS]),
 371                   NULL);
 372
 373         printf("WoWLAN is enabled:\n");
 374         if (trig[NL80211_WOWLAN_TRIG_ANY])
 375                 printf(" * wake up on special any trigger\n");
 376         if (trig[NL80211_WOWLAN_TRIG_DISCONNECT])
 377                 printf(" * wake up on disconnect\n");
 378         if (trig[NL80211_WOWLAN_TRIG_MAGIC_PKT])
 379                 printf(" * wake up on magic packet\n");
 380         if (trig[NL80211_WOWLAN_TRIG_GTK_REKEY_FAILURE])
 381                 printf(" * wake up on GTK rekeying failure\n");
 382         if (trig[NL80211_WOWLAN_TRIG_EAP_IDENT_REQUEST])
 383                 printf(" * wake up on EAP identity request\n");
 384         if (trig[NL80211_WOWLAN_TRIG_4WAY_HANDSHAKE])
 385                 printf(" * wake up on 4-way handshake\n");
 386         if (trig[NL80211_WOWLAN_TRIG_RFKILL_RELEASE])
 387                 printf(" * wake up on RF-kill release\n");
 388         if (trig[NL80211_WOWLAN_TRIG_NET_DETECT]) {
 389                 struct nlattr *match, *freq,
 390                         *nd[NUM_NL80211_ATTR], *tb[NUM_NL80211_ATTR];
 391                 int rem_match;
 392
 393                 printf(" * wake up on network detection\n");
 394                 nla_parse(nd, NUM_NL80211_ATTR,
 395                           nla_data(trig[NL80211_WOWLAN_TRIG_NET_DETECT]),
 396                           nla_len(trig[NL80211_WOWLAN_TRIG_NET_DETECT]), NULL);
 397
 398                 if (nd[NL80211_ATTR_SCHED_SCAN_INTERVAL])
 399                         printf("\tscan interval: %u msecs\n",
 400                                nla_get_u32(nd[NL80211_ATTR_SCHED_SCAN_INTERVAL]));
 401
 402                 if (nd[NL80211_ATTR_SCHED_SCAN_DELAY])
 403                         printf("\tinitial scan delay: %u secs\n",
 404                                nla_get_u32(nd[NL80211_ATTR_SCHED_SCAN_DELAY]));
 405
 406                 if (nd[NL80211_ATTR_SCHED_SCAN_MATCH]) {
 407                         printf("\tmatches:\n");
 408                         nla_for_each_nested(match,
 409                                             nd[NL80211_ATTR_SCHED_SCAN_MATCH],
 410                                             rem_match) {
 411                                 nla_parse(tb, NUM_NL80211_ATTR, nla_data(match),
 412                                           nla_len(match),
 413                                           NULL);
 414                                 printf("\t\tSSID: ");
 415                                 print_ssid_escaped(
 416                                         nla_len(tb[NL80211_SCHED_SCAN_MATCH_ATTR_SSID]),
 417                                         nla_data(tb[NL80211_SCHED_SCAN_MATCH_ATTR_SSID]));
 418                                 printf("\n");
 419                         }
 420                 }
 421                 if (nd[NL80211_ATTR_SCAN_FREQUENCIES]) {
 422                         printf("\tfrequencies:");
 423                         nla_for_each_nested(freq,
 424                                             nd[NL80211_ATTR_SCAN_FREQUENCIES],
 425                                             rem_match) {
 426                                 printf(" %d", nla_get_u32(freq));
 427                         }
 428                         printf("\n");
 429                 }
 430         }
 431         if (trig[NL80211_WOWLAN_TRIG_PKT_PATTERN]) {
 432                 nla_for_each_nested(pattern,
 433                                     trig[NL80211_WOWLAN_TRIG_PKT_PATTERN],
 434                                     rem_pattern) {
 435                         struct nlattr *patattr[NUM_NL80211_PKTPAT];
 436                         int i, patlen, masklen;
 437                         uint8_t *mask, *pat;
 438                         nla_parse(patattr, MAX_NL80211_PKTPAT,
 439                                   nla_data(pattern), nla_len(pattern), NULL);
 440                         if (!patattr[NL80211_PKTPAT_MASK] ||
 441                             !patattr[NL80211_PKTPAT_PATTERN]) {
 442                                 printf(" * (invalid pattern specification)\n");
 443                                 continue;
 444                         }
 445                         masklen = nla_len(patattr[NL80211_PKTPAT_MASK]);
 446                         patlen = nla_len(patattr[NL80211_PKTPAT_PATTERN]);
 447                         if (DIV_ROUND_UP(patlen, 8) != masklen) {
 448                                 printf(" * (invalid pattern specification)\n");
 449                                 continue;
 450                         }
 451                         if (patattr[NL80211_PKTPAT_OFFSET]) {
 452                                 int pkt_offset =
 453                                         nla_get_u32(patattr[NL80211_PKTPAT_OFFSET]);
 454                                 printf(" * wake up on packet offset: %d", pkt_offset);
 455                         }
 456                         printf(" pattern: ");
 457                         pat = nla_data(patattr[NL80211_PKTPAT_PATTERN]);
 458                         mask = nla_data(patattr[NL80211_PKTPAT_MASK]);
 459                         for (i = 0; i < patlen; i++) {
 460                                 if (mask[i / 8] & (1 << (i % 8)))
 461                                         printf("%.2x", pat[i]);
 462                                 else
 463                                         printf("--");
 464                                 if (i != patlen - 1)
 465                                         printf(":");
 466                         }
 467                         printf("\n");
 468                 }
 469         }
 470         if (trig[NL80211_WOWLAN_TRIG_TCP_CONNECTION])
 471                 printf(" * wake up on TCP connection\n");
 472
 473         return NL_SKIP;
 474 }
 475
 476 static int handle_wowlan_show(struct nl80211_state *state,
 477                               struct nl_msg *msg, int argc, char **argv,
 478                               enum id_input id)
 479 {
 480         register_handler(print_wowlan_handler, NULL);
 481
 482         return 0;
 483 }
 484 COMMAND(wowlan, show, "", NL80211_CMD_GET_WOWLAN, 0, CIB_PHY, handle_wowlan_show,
 485         "Show WoWLAN status.");