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                         NLA_PUT(msg, NL80211_WOWLAN_TCP_DATA_PAYLOAD_TOKEN,
 163                                 sizeof(*tok) + stream_len, tok);
 164                         free(stream);
 165                         free(tok);
 166                 } else {
 167                         if (buf[0] == '#')
 168                                 continue;
 169                         goto close;
 170                 }
 171         }
 172
 173         err = 0;
 174         goto close;
 175  nla_put_failure:
 176         err = -ENOBUFS;
 177  close:
 178         fclose(f);
 179         nla_nest_end(msg, tcp);
 180         return err;
 181 }
 182
 183 static int wowlan_parse_net_detect(struct nl_msg *msg, int *argc, char ***argv)
 184 {
 185         struct nlattr *nd;
 186         int err = 0;
 187
 188         nd = nla_nest_start(msg, NL80211_WOWLAN_TRIG_NET_DETECT);
 189         if (!nd)
 190                 return -ENOBUFS;
 191
 192         err = parse_sched_scan(msg, argc, argv);
 193
 194         nla_nest_end(msg, nd);
 195
 196         return err;
 197 }
 198
 199 static int handle_wowlan_enable(struct nl80211_state *state,
 200                                 struct nl_msg *msg, int argc, char **argv,
 201                                 enum id_input id)
 202 {
 203         struct nlattr *wowlan, *pattern;
 204         struct nl_msg *patterns = NULL;
 205         enum {
 206                 PS_REG,
 207                 PS_PAT,
 208         } parse_state = PS_REG;
 209         int err = -ENOBUFS;
 210         unsigned char *pat, *mask;
 211         size_t patlen;
 212         int patnum = 0, pkt_offset;
 213         char *eptr, *value1, *value2, *sptr = NULL;
 214
 215         wowlan = nla_nest_start(msg, NL80211_ATTR_WOWLAN_TRIGGERS);
 216         if (!wowlan)
 217                 return -ENOBUFS;
 218
 219         while (argc) {
 220                 switch (parse_state) {
 221                 case PS_REG:
 222                         if (strcmp(argv[0], "any") == 0)
 223                                 NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_ANY);
 224                         else if (strcmp(argv[0], "disconnect") == 0)
 225                                 NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_DISCONNECT);
 226                         else if (strcmp(argv[0], "magic-packet") == 0)
 227                                 NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_MAGIC_PKT);
 228                         else if (strcmp(argv[0], "gtk-rekey-failure") == 0)
 229                                 NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_GTK_REKEY_FAILURE);
 230                         else if (strcmp(argv[0], "eap-identity-request") == 0)
 231                                 NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_EAP_IDENT_REQUEST);
 232                         else if (strcmp(argv[0], "4way-handshake") == 0)
 233                                 NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_4WAY_HANDSHAKE);
 234                         else if (strcmp(argv[0], "rfkill-release") == 0)
 235                                 NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_RFKILL_RELEASE);
 236                         else if (strcmp(argv[0], "tcp") == 0) {
 237                                 argv++;
 238                                 argc--;
 239                                 if (!argc) {
 240                                         err = 1;
 241                                         goto nla_put_failure;
 242                                 }
 243                                 err = wowlan_parse_tcp_file(msg, argv[0]);
 244                                 if (err)
 245                                         goto nla_put_failure;
 246                         } else if (strcmp(argv[0], "patterns") == 0) {
 247                                 parse_state = PS_PAT;
 248                                 patterns = nlmsg_alloc();
 249                                 if (!patterns) {
 250                                         err = -ENOMEM;
 251                                         goto nla_put_failure;
 252                                 }
 253                         } else if (strcmp(argv[0], "net-detect") == 0) {
 254                                 argv++;
 255                                 argc--;
 256                                 if (!argc) {
 257                                         err = 1;
 258                                         goto nla_put_failure;
 259                                 }
 260                                 err = wowlan_parse_net_detect(msg, &argc, &argv);
 261                                 if (err)
 262                                         goto nla_put_failure;
 263                                 continue;
 264                         } else {
 265                                 err = 1;
 266                                 goto nla_put_failure;
 267                         }
 268                         break;
 269                 case PS_PAT:
 270                         value1 = strtok_r(argv[0], "+", &sptr);
 271                         value2 = strtok_r(NULL, "+", &sptr);
 272
 273                         if (!value2) {
 274                                 pkt_offset = 0;
 275                                 value2 = value1;
 276                         } else {
 277                                 pkt_offset = strtoul(value1, &eptr, 10);
 278                                 if (eptr != value1 + strlen(value1)) {
 279                                         err = 1;
 280                                         goto nla_put_failure;
 281                                 }
 282                         }
 283
 284                         if (parse_hex_mask(value2, &pat, &patlen, &mask)) {
 285                                 err = 1;
 286                                 goto nla_put_failure;
 287                         }
 288
 289                         pattern = nla_nest_start(patterns, ++patnum);
 290                         NLA_PUT(patterns, NL80211_PKTPAT_MASK,
 291                                 DIV_ROUND_UP(patlen, 8), mask);
 292                         NLA_PUT(patterns, NL80211_PKTPAT_PATTERN, patlen, pat);
 293                         NLA_PUT_U32(patterns, NL80211_PKTPAT_OFFSET,
 294                                     pkt_offset);
 295                         nla_nest_end(patterns, pattern);
 296                         free(mask);
 297                         free(pat);
 298                         break;
 299                 }
 300                 argv++;
 301                 argc--;
 302         }
 303
 304         if (patterns)
 305                 nla_put_nested(msg, NL80211_WOWLAN_TRIG_PKT_PATTERN,
 306                                 patterns);
 307
 308         nla_nest_end(msg, wowlan);
 309         err = 0;
 310  nla_put_failure:
 311         nlmsg_free(patterns);
 312         return err;
 313 }
 314 COMMAND(wowlan, enable, "[any] [disconnect] [magic-packet] [gtk-rekey-failure] [eap-identity-request]"
 315         " [4way-handshake] [rfkill-release] [net-detect " SCHED_SCAN_OPTIONS "]"
 316         " [tcp <config-file>] [patterns [offset1+]<pattern1> ...]",
 317         NL80211_CMD_SET_WOWLAN, 0, CIB_PHY, handle_wowlan_enable,
 318         "Enable WoWLAN with the given triggers.\n"
 319         "Each pattern is given as a bytestring with '-' in places where any byte\n"
 320         "may be present, e.g. 00:11:22:-:44 will match 00:11:22:33:44 and\n"
 321         "00:11:22:33:ff:44 etc.\n"
 322         "Offset and pattern should be separated by '+', e.g. 18+43:34:00:12 will match "
 323         "'43:34:00:12' after 18 bytes of offset in Rx packet.\n\n"
 324         "The TCP configuration file contains:\n"
 325         "  source=ip[:port]\n"
 326         "  dest=ip:port@mac\n"
 327         "  data=<hex data packet>\n"
 328         "  data.interval=seconds\n"
 329         "  [wake=<hex packet with masked out bytes indicated by '-'>]\n"
 330         "  [data.seq=len,offset[,start]]\n"
 331         "  [data.tok=len,offset,<token stream>]\n\n"
 332         "Net-detect configuration example:\n"
 333         " iw phy0 wowlan enable net-detect interval 5000 delay 30 freqs 2412 2422 matches ssid foo ssid bar");
 334
 335
 336 static int handle_wowlan_disable(struct nl80211_state *state,
 337                                  struct nl_msg *msg, int argc, char **argv,
 338                                  enum id_input id)
 339 {
 340         /* just a set w/o wowlan attribute */
 341         return 0;
 342 }
 343 COMMAND(wowlan, disable, "", NL80211_CMD_SET_WOWLAN, 0, CIB_PHY, handle_wowlan_disable,
 344         "Disable WoWLAN.");
 345
 346
 347 static int print_wowlan_handler(struct nl_msg *msg, void *arg)
 348 {
 349         struct nlattr *attrs[NL80211_ATTR_MAX + 1];
 350         struct nlattr *trig[NUM_NL80211_WOWLAN_TRIG];
 351         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 352         struct nlattr *pattern;
 353         int rem_pattern;
 354
 355         nla_parse(attrs, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
 356                   genlmsg_attrlen(gnlh, 0), NULL);
 357
 358         if (!attrs[NL80211_ATTR_WOWLAN_TRIGGERS]) {
 359                 printf("WoWLAN is disabled.\n");
 360                 return NL_SKIP;
 361         }
 362
 363         /* XXX: use policy */
 364         nla_parse(trig, MAX_NL80211_WOWLAN_TRIG,
 365                   nla_data(attrs[NL80211_ATTR_WOWLAN_TRIGGERS]),
 366                   nla_len(attrs[NL80211_ATTR_WOWLAN_TRIGGERS]),
 367                   NULL);
 368
 369         printf("WoWLAN is enabled:\n");
 370         if (trig[NL80211_WOWLAN_TRIG_ANY])
 371                 printf(" * wake up on special any trigger\n");
 372         if (trig[NL80211_WOWLAN_TRIG_DISCONNECT])
 373                 printf(" * wake up on disconnect\n");
 374         if (trig[NL80211_WOWLAN_TRIG_MAGIC_PKT])
 375                 printf(" * wake up on magic packet\n");
 376         if (trig[NL80211_WOWLAN_TRIG_GTK_REKEY_FAILURE])
 377                 printf(" * wake up on GTK rekeying failure\n");
 378         if (trig[NL80211_WOWLAN_TRIG_EAP_IDENT_REQUEST])
 379                 printf(" * wake up on EAP identity request\n");
 380         if (trig[NL80211_WOWLAN_TRIG_4WAY_HANDSHAKE])
 381                 printf(" * wake up on 4-way handshake\n");
 382         if (trig[NL80211_WOWLAN_TRIG_RFKILL_RELEASE])
 383                 printf(" * wake up on RF-kill release\n");
 384         if (trig[NL80211_WOWLAN_TRIG_NET_DETECT]) {
 385                 struct nlattr *match, *freq,
 386                         *nd[NUM_NL80211_ATTR], *tb[NUM_NL80211_ATTR];
 387                 int rem_match;
 388
 389                 printf(" * wake up on network detection\n");
 390                 nla_parse(nd, NUM_NL80211_ATTR,
 391                           nla_data(trig[NL80211_WOWLAN_TRIG_NET_DETECT]),
 392                           nla_len(trig[NL80211_WOWLAN_TRIG_NET_DETECT]), NULL);
 393
 394                 if (nd[NL80211_ATTR_SCHED_SCAN_INTERVAL])
 395                         printf("\tscan interval: %u msecs\n",
 396                                nla_get_u32(nd[NL80211_ATTR_SCHED_SCAN_INTERVAL]));
 397
 398                 if (nd[NL80211_ATTR_SCHED_SCAN_DELAY])
 399                         printf("\tinitial scan delay: %u secs\n",
 400                                nla_get_u32(nd[NL80211_ATTR_SCHED_SCAN_DELAY]));
 401
 402                 if (nd[NL80211_ATTR_SCHED_SCAN_MATCH]) {
 403                         printf("\tmatches:\n");
 404                         nla_for_each_nested(match,
 405                                             nd[NL80211_ATTR_SCHED_SCAN_MATCH],
 406                                             rem_match) {
 407                                 nla_parse(tb, NUM_NL80211_ATTR, nla_data(match),
 408                                           nla_len(match),
 409                                           NULL);
 410                                 printf("\t\tSSID: ");
 411                                 print_ssid_escaped(
 412                                         nla_len(tb[NL80211_SCHED_SCAN_MATCH_ATTR_SSID]),
 413                                         nla_data(tb[NL80211_SCHED_SCAN_MATCH_ATTR_SSID]));
 414                                 printf("\n");
 415                         }
 416                 }
 417                 if (nd[NL80211_ATTR_SCAN_FREQUENCIES]) {
 418                         printf("\tfrequencies:");
 419                         nla_for_each_nested(freq,
 420                                             nd[NL80211_ATTR_SCAN_FREQUENCIES],
 421                                             rem_match) {
 422                                 printf(" %d", nla_get_u32(freq));
 423                         }
 424                         printf("\n");
 425                 }
 426         }
 427         if (trig[NL80211_WOWLAN_TRIG_PKT_PATTERN]) {
 428                 nla_for_each_nested(pattern,
 429                                     trig[NL80211_WOWLAN_TRIG_PKT_PATTERN],
 430                                     rem_pattern) {
 431                         struct nlattr *patattr[NUM_NL80211_PKTPAT];
 432                         int i, patlen, masklen;
 433                         uint8_t *mask, *pat;
 434                         nla_parse(patattr, MAX_NL80211_PKTPAT,
 435                                   nla_data(pattern), nla_len(pattern), NULL);
 436                         if (!patattr[NL80211_PKTPAT_MASK] ||
 437                             !patattr[NL80211_PKTPAT_PATTERN]) {
 438                                 printf(" * (invalid pattern specification)\n");
 439                                 continue;
 440                         }
 441                         masklen = nla_len(patattr[NL80211_PKTPAT_MASK]);
 442                         patlen = nla_len(patattr[NL80211_PKTPAT_PATTERN]);
 443                         if (DIV_ROUND_UP(patlen, 8) != masklen) {
 444                                 printf(" * (invalid pattern specification)\n");
 445                                 continue;
 446                         }
 447                         if (patattr[NL80211_PKTPAT_OFFSET]) {
 448                                 int pkt_offset =
 449                                         nla_get_u32(patattr[NL80211_PKTPAT_OFFSET]);
 450                                 printf(" * wake up on packet offset: %d", pkt_offset);
 451                         }
 452                         printf(" pattern: ");
 453                         pat = nla_data(patattr[NL80211_PKTPAT_PATTERN]);
 454                         mask = nla_data(patattr[NL80211_PKTPAT_MASK]);
 455                         for (i = 0; i < patlen; i++) {
 456                                 if (mask[i / 8] & (1 << (i % 8)))
 457                                         printf("%.2x", pat[i]);
 458                                 else
 459                                         printf("--");
 460                                 if (i != patlen - 1)
 461                                         printf(":");
 462                         }
 463                         printf("\n");
 464                 }
 465         }
 466         if (trig[NL80211_WOWLAN_TRIG_TCP_CONNECTION])
 467                 printf(" * wake up on TCP connection\n");
 468
 469         return NL_SKIP;
 470 }
 471
 472 static int handle_wowlan_show(struct nl80211_state *state,
 473                               struct nl_msg *msg, int argc, char **argv,
 474                               enum id_input id)
 475 {
 476         register_handler(print_wowlan_handler, NULL);
 477
 478         return 0;
 479 }
 480 COMMAND(wowlan, show, "", NL80211_CMD_GET_WOWLAN, 0, CIB_PHY, handle_wowlan_show,
 481         "Show WoWLAN status.");