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 handle_wowlan_enable(struct nl80211_state *state, struct nl_cb *cb,
 185                                 struct nl_msg *msg, int argc, char **argv,
 186                                 enum id_input id)
 187 {
 188         struct nlattr *wowlan, *pattern;
 189         struct nl_msg *patterns = NULL;
 190         enum {
 191                 PS_REG,
 192                 PS_PAT,
 193         } parse_state = PS_REG;
 194         int err = -ENOBUFS;
 195         unsigned char *pat, *mask;
 196         size_t patlen;
 197         int patnum = 0, pkt_offset;
 198         char *eptr, *value1, *value2, *sptr = NULL;
 199
 200         wowlan = nla_nest_start(msg, NL80211_ATTR_WOWLAN_TRIGGERS);
 201         if (!wowlan)
 202                 return -ENOBUFS;
 203
 204         while (argc) {
 205                 switch (parse_state) {
 206                 case PS_REG:
 207                         if (strcmp(argv[0], "any") == 0)
 208                                 NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_ANY);
 209                         else if (strcmp(argv[0], "disconnect") == 0)
 210                                 NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_DISCONNECT);
 211                         else if (strcmp(argv[0], "magic-packet") == 0)
 212                                 NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_MAGIC_PKT);
 213                         else if (strcmp(argv[0], "gtk-rekey-failure") == 0)
 214                                 NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_GTK_REKEY_FAILURE);
 215                         else if (strcmp(argv[0], "eap-identity-request") == 0)
 216                                 NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_EAP_IDENT_REQUEST);
 217                         else if (strcmp(argv[0], "4way-handshake") == 0)
 218                                 NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_4WAY_HANDSHAKE);
 219                         else if (strcmp(argv[0], "rfkill-release") == 0)
 220                                 NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_RFKILL_RELEASE);
 221                         else if (strcmp(argv[0], "tcp") == 0) {
 222                                 argv++;
 223                                 argc--;
 224                                 if (!argc) {
 225                                         err = 1;
 226                                         goto nla_put_failure;
 227                                 }
 228                                 err = wowlan_parse_tcp_file(msg, argv[0]);
 229                                 if (err)
 230                                         goto nla_put_failure;
 231                         } else if (strcmp(argv[0], "patterns") == 0) {
 232                                 parse_state = PS_PAT;
 233                                 patterns = nlmsg_alloc();
 234                                 if (!patterns) {
 235                                         err = -ENOMEM;
 236                                         goto nla_put_failure;
 237                                 }
 238                         } else {
 239                                 err = 1;
 240                                 goto nla_put_failure;
 241                         }
 242                         break;
 243                 case PS_PAT:
 244                         value1 = strtok_r(argv[0], "+", &sptr);
 245                         value2 = strtok_r(NULL, "+", &sptr);
 246
 247                         if (!value2) {
 248                                 pkt_offset = 0;
 249                                 value2 = value1;
 250                         } else {
 251                                 pkt_offset = strtoul(value1, &eptr, 10);
 252                                 if (eptr != value1 + strlen(value1)) {
 253                                         err = 1;
 254                                         goto nla_put_failure;
 255                                 }
 256                         }
 257
 258                         if (parse_hex_mask(value2, &pat, &patlen, &mask)) {
 259                                 err = 1;
 260                                 goto nla_put_failure;
 261                         }
 262
 263                         pattern = nla_nest_start(patterns, ++patnum);
 264                         NLA_PUT(patterns, NL80211_PKTPAT_MASK,
 265                                 DIV_ROUND_UP(patlen, 8), mask);
 266                         NLA_PUT(patterns, NL80211_PKTPAT_PATTERN, patlen, pat);
 267                         NLA_PUT_U32(patterns, NL80211_PKTPAT_OFFSET,
 268                                     pkt_offset);
 269                         nla_nest_end(patterns, pattern);
 270                         free(mask);
 271                         free(pat);
 272                         break;
 273                 }
 274                 argv++;
 275                 argc--;
 276         }
 277
 278         if (patterns)
 279                 nla_put_nested(msg, NL80211_WOWLAN_TRIG_PKT_PATTERN,
 280                                 patterns);
 281
 282         nla_nest_end(msg, wowlan);
 283         err = 0;
 284  nla_put_failure:
 285         nlmsg_free(patterns);
 286         return err;
 287 }
 288 COMMAND(wowlan, enable, "[any] [disconnect] [magic-packet] [gtk-rekey-failure] [eap-identity-request]"
 289         " [4way-handshake] [rfkill-release] [tcp <config-file>] [patterns [offset1+]<pattern1> ...]",
 290         NL80211_CMD_SET_WOWLAN, 0, CIB_PHY, handle_wowlan_enable,
 291         "Enable WoWLAN with the given triggers.\n"
 292         "Each pattern is given as a bytestring with '-' in places where any byte\n"
 293         "may be present, e.g. 00:11:22:-:44 will match 00:11:22:33:44 and\n"
 294         "00:11:22:33:ff:44 etc.\n"
 295         "Offset and pattern should be separated by '+', e.g. 18+43:34:00:12 will match "
 296         "'43:34:00:12' after 18 bytes of offset in Rx packet.\n\n"
 297         "The TCP configuration file contains:\n"
 298         "  source=ip[:port]\n"
 299         "  dest=ip:port@mac\n"
 300         "  data=<hex data packet>\n"
 301         "  data.interval=seconds\n"
 302         "  [wake=<hex packet with masked out bytes indicated by '-'>]\n"
 303         "  [data.seq=len,offset[,start]]\n"
 304         "  [data.tok=len,offset,<token stream>]");
 305
 306
 307 static int handle_wowlan_disable(struct nl80211_state *state, struct nl_cb *cb,
 308                                  struct nl_msg *msg, int argc, char **argv,
 309                                  enum id_input id)
 310 {
 311         /* just a set w/o wowlan attribute */
 312         return 0;
 313 }
 314 COMMAND(wowlan, disable, "", NL80211_CMD_SET_WOWLAN, 0, CIB_PHY, handle_wowlan_disable,
 315         "Disable WoWLAN.");
 316
 317
 318 static int print_wowlan_handler(struct nl_msg *msg, void *arg)
 319 {
 320         struct nlattr *attrs[NL80211_ATTR_MAX + 1];
 321         struct nlattr *trig[NUM_NL80211_WOWLAN_TRIG];
 322         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 323         struct nlattr *pattern;
 324         int rem_pattern;
 325
 326         nla_parse(attrs, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
 327                   genlmsg_attrlen(gnlh, 0), NULL);
 328
 329         if (!attrs[NL80211_ATTR_WOWLAN_TRIGGERS]) {
 330                 printf("WoWLAN is disabled.\n");
 331                 return NL_SKIP;
 332         }
 333
 334         /* XXX: use policy */
 335         nla_parse(trig, MAX_NL80211_WOWLAN_TRIG,
 336                   nla_data(attrs[NL80211_ATTR_WOWLAN_TRIGGERS]),
 337                   nla_len(attrs[NL80211_ATTR_WOWLAN_TRIGGERS]),
 338                   NULL);
 339
 340         printf("WoWLAN is enabled:\n");
 341         if (trig[NL80211_WOWLAN_TRIG_ANY])
 342                 printf(" * wake up on special any trigger\n");
 343         if (trig[NL80211_WOWLAN_TRIG_DISCONNECT])
 344                 printf(" * wake up on disconnect\n");
 345         if (trig[NL80211_WOWLAN_TRIG_MAGIC_PKT])
 346                 printf(" * wake up on magic packet\n");
 347         if (trig[NL80211_WOWLAN_TRIG_GTK_REKEY_FAILURE])
 348                 printf(" * wake up on GTK rekeying failure\n");
 349         if (trig[NL80211_WOWLAN_TRIG_EAP_IDENT_REQUEST])
 350                 printf(" * wake up on EAP identity request\n");
 351         if (trig[NL80211_WOWLAN_TRIG_4WAY_HANDSHAKE])
 352                 printf(" * wake up on 4-way handshake\n");
 353         if (trig[NL80211_WOWLAN_TRIG_RFKILL_RELEASE])
 354                 printf(" * wake up on RF-kill release\n");
 355         if (trig[NL80211_WOWLAN_TRIG_PKT_PATTERN]) {
 356                 nla_for_each_nested(pattern,
 357                                     trig[NL80211_WOWLAN_TRIG_PKT_PATTERN],
 358                                     rem_pattern) {
 359                         struct nlattr *patattr[NUM_NL80211_PKTPAT];
 360                         int i, patlen, masklen;
 361                         uint8_t *mask, *pat;
 362                         nla_parse(patattr, MAX_NL80211_PKTPAT,
 363                                   nla_data(pattern), nla_len(pattern), NULL);
 364                         if (!patattr[NL80211_PKTPAT_MASK] ||
 365                             !patattr[NL80211_PKTPAT_PATTERN]) {
 366                                 printf(" * (invalid pattern specification)\n");
 367                                 continue;
 368                         }
 369                         masklen = nla_len(patattr[NL80211_PKTPAT_MASK]);
 370                         patlen = nla_len(patattr[NL80211_PKTPAT_PATTERN]);
 371                         if (DIV_ROUND_UP(patlen, 8) != masklen) {
 372                                 printf(" * (invalid pattern specification)\n");
 373                                 continue;
 374                         }
 375                         if (patattr[NL80211_PKTPAT_OFFSET]) {
 376                                 int pkt_offset =
 377                                         nla_get_u32(patattr[NL80211_PKTPAT_OFFSET]);
 378                                 printf(" * wake up on packet offset: %d", pkt_offset);
 379                         }
 380                         printf(" pattern: ");
 381                         pat = nla_data(patattr[NL80211_PKTPAT_PATTERN]);
 382                         mask = nla_data(patattr[NL80211_PKTPAT_MASK]);
 383                         for (i = 0; i < patlen; i++) {
 384                                 if (mask[i / 8] & (1 << (i % 8)))
 385                                         printf("%.2x", pat[i]);
 386                                 else
 387                                         printf("--");
 388                                 if (i != patlen - 1)
 389                                         printf(":");
 390                         }
 391                         printf("\n");
 392                 }
 393         }
 394         if (trig[NL80211_WOWLAN_TRIG_TCP_CONNECTION])
 395                 printf(" * wake up on TCP connection\n");
 396
 397         return NL_SKIP;
 398 }
 399
 400 static int handle_wowlan_show(struct nl80211_state *state, struct nl_cb *cb,
 401                               struct nl_msg *msg, int argc, char **argv,
 402                               enum id_input id)
 403 {
 404         nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM,
 405                   print_wowlan_handler, NULL);
 406
 407         return 0;
 408 }
 409 COMMAND(wowlan, show, "", NL80211_CMD_GET_WOWLAN, 0, CIB_PHY, handle_wowlan_show,
 410         "Show WoWLAN status.");