wowlan.c

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