nan.c

   1 #include <net/if.h>
   2 #include <errno.h>
   3 #include <string.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 <ctype.h>
  12 #include <inttypes.h>
  13 #include "nl80211.h"
  14 #include "iw.h"
  15 #include "sha256.h"
  16
  17 SECTION(nan);
  18
  19 static int parse_bands(int argc, char **argv)
  20 {
  21         int i = 0, bands = 0;
  22
  23         for (i = 0; i < argc; i++) {
  24                 if (!strcasecmp("2ghz", argv[i]))
  25                         bands |= BIT(NL80211_BAND_2GHZ);
  26                 else if (!strcasecmp("5ghz", argv[i]))
  27                         bands |= BIT(NL80211_BAND_5GHZ);
  28                 else
  29                         return -EINVAL;
  30         }
  31
  32         return bands;
  33 }
  34
  35 static int handle_nan_start(struct nl80211_state *state,
  36                             struct nl_msg *msg, int argc, char **argv,
  37                             enum id_input id)
  38 {
  39         int bands = 0;
  40
  41         if (argc < 2)
  42                 return -EINVAL;
  43
  44         if (strcmp(argv[0], "pref") == 0) {
  45                 argv++;
  46                 argc--;
  47                 NLA_PUT_U8(msg, NL80211_ATTR_NAN_MASTER_PREF, atoi(argv[0]));
  48                 argv++;
  49                 argc--;
  50         } else {
  51                 /* Master preference is mandatory */
  52                 return -EINVAL;
  53         }
  54
  55         if (argc > 1 && !strcmp(argv[0], "bands")) {
  56                 argv++;
  57                 argc--;
  58
  59                 bands = parse_bands(argc, argv);
  60                 if (bands < 0)
  61                         return bands;
  62
  63                 NLA_PUT_U32(msg, NL80211_ATTR_BANDS, bands);
  64         } else if (argc != 0)
  65                 return -EINVAL;
  66
  67         return 0;
  68 nla_put_failure:
  69         return -ENOBUFS;
  70 }
  71 COMMAND(nan, start, "pref <pref> [bands [2GHz] [5GHz]]",
  72         NL80211_CMD_START_NAN, 0, CIB_WDEV, handle_nan_start, "");
  73
  74 static int handle_nan_stop(struct nl80211_state *state,
  75                            struct nl_msg *msg, int argc, char **argv,
  76                            enum id_input id)
  77 {
  78         return 0;
  79 }
  80 COMMAND(nan, stop, "", NL80211_CMD_STOP_NAN, 0, CIB_WDEV, handle_nan_stop, "");
  81
  82 static int handle_nan_config(struct nl80211_state *state,
  83                              struct nl_msg *msg, int argc, char **argv,
  84                              enum id_input id)
  85 {
  86         int bands = 0;
  87
  88         if (argc < 2)
  89                 return -EINVAL;
  90
  91         if (strcmp(argv[0], "pref") == 0) {
  92                 argv++;
  93                 argc--;
  94                 NLA_PUT_U8(msg, NL80211_ATTR_NAN_MASTER_PREF, atoi(argv[0]));
  95                 argv++;
  96                 argc--;
  97         }
  98
  99         if (argc > 1 && !strcmp(argv[0], "bands")) {
 100                 argv++;
 101                 argc--;
 102
 103                 bands = parse_bands(argc, argv);
 104                 if (bands < 0)
 105                         return bands;
 106
 107                 NLA_PUT_U32(msg, NL80211_ATTR_BANDS, bands);
 108                 argv++;
 109                 argc--;
 110         } else if (argc != 0)
 111                 return -EINVAL;
 112
 113         return 0;
 114 nla_put_failure:
 115         return -ENOBUFS;
 116 }
 117 COMMAND(nan, config, "[pref <pref>] [bands [2GHz] [5GHz]]",
 118         NL80211_CMD_CHANGE_NAN_CONFIG, 0, CIB_WDEV, handle_nan_config, "");
 119
 120 static int handle_nan_rm_func(struct nl80211_state *state,
 121                               struct nl_msg *msg, int argc, char **argv,
 122                               enum id_input id)
 123 {
 124         if (argc != 2)
 125                 return -EINVAL;
 126
 127         if (strcmp(argv[0], "cookie") == 0) {
 128                 argv++;
 129                 argc--;
 130                 NLA_PUT_U64(msg, NL80211_ATTR_COOKIE, atoi(argv[0]));
 131                 argv++;
 132                 argc--;
 133         }
 134
 135         if (argc != 0)
 136                 return -EINVAL;
 137
 138         return 0;
 139 nla_put_failure:
 140         return -ENOBUFS;
 141 }
 142 COMMAND(nan, rm_func, "cookie <cookie>", NL80211_CMD_DEL_NAN_FUNCTION, 0,
 143         CIB_WDEV, handle_nan_rm_func, "");
 144
 145 static int compute_service_id(const unsigned char *serv_name,
 146                               unsigned int len, unsigned char *res)
 147 {
 148         size_t size = len;
 149         unsigned char md_value[32];
 150         int retcode = sha256(serv_name, size, md_value);
 151
 152         if (retcode)
 153                 return retcode;
 154         memcpy(res, md_value, 6);
 155
 156         return 0;
 157 }
 158
 159 static int print_instance_id_handler(struct nl_msg *msg, void *arg)
 160 {
 161         struct nlattr *tb[NL80211_ATTR_MAX + 1];
 162         struct nlattr *func[NL80211_NAN_FUNC_ATTR_MAX + 1];
 163         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 164
 165         nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
 166                   genlmsg_attrlen(gnlh, 0), NULL);
 167
 168         if (!tb[NL80211_ATTR_COOKIE]) {
 169                 fprintf(stderr, "cookie is missing!\n");
 170                 return NL_SKIP;
 171         }
 172
 173         nla_parse_nested(func, NL80211_NAN_FUNC_ATTR_MAX,
 174                          tb[NL80211_ATTR_NAN_FUNC],
 175                          NULL);
 176         if (!func[NL80211_NAN_FUNC_INSTANCE_ID]) {
 177                 fprintf(stderr, "instance id is missing!\n");
 178                 return NL_SKIP;
 179         }
 180
 181         printf("instance_id: %d, cookie: %" PRIu64 "\n",
 182                nla_get_u8(func[NL80211_NAN_FUNC_INSTANCE_ID]),
 183                nla_get_u64(tb[NL80211_ATTR_COOKIE]));
 184
 185         return NL_SKIP;
 186 }
 187
 188 static int parse_srf(char **argv, int argc, struct nl_msg *func_attrs)
 189 {
 190         struct nl_msg *srf_attrs;
 191         int old_argc = argc;
 192         unsigned char mac_addr[ETH_ALEN];
 193         char *cur_mac, *sptr = NULL;
 194
 195         srf_attrs = nlmsg_alloc();
 196         if (strcmp(argv[0], "include") == 0)
 197                 NLA_PUT_FLAG(srf_attrs, NL80211_NAN_SRF_INCLUDE);
 198         else if (strcmp(argv[0], "exclude") != 0)
 199                 return -EINVAL;
 200
 201         argc--;
 202         argv++;
 203         if (strcmp(argv[0], "bf") == 0) {
 204                 unsigned char *srf;
 205                 size_t srf_len;
 206                 __u8 bf_idx;
 207
 208                 argc--;
 209                 argv++;
 210
 211                 if (argc < 3)
 212                         return -EINVAL;
 213
 214                 bf_idx = atoi(argv[0]);
 215                 NLA_PUT_U8(srf_attrs, NL80211_NAN_SRF_BF_IDX, bf_idx);
 216
 217                 argc--;
 218                 argv++;
 219                 srf_len = atoi(argv[0]);
 220                 if (srf_len == 0 || srf_len > NL80211_NAN_FUNC_SRF_MAX_LEN)
 221                         return -EINVAL;
 222
 223                 argc--;
 224                 argv++;
 225                 srf = malloc(srf_len);
 226                 if (!srf)
 227                         return -ENOBUFS;
 228
 229                 memset(srf, 0, srf_len);
 230                 cur_mac = strtok_r(argv[0], ";", &sptr);
 231                 while (cur_mac) {
 232                         if (mac_addr_a2n(mac_addr, cur_mac)) {
 233                                 printf("mac format error %s\n", cur_mac);
 234                                 return -EINVAL;
 235                         }
 236
 237                         nan_bf(bf_idx, srf, srf_len, mac_addr, ETH_ALEN);
 238                         cur_mac = strtok_r(NULL, ";", &sptr);
 239                 }
 240
 241                 NLA_PUT(srf_attrs, NL80211_NAN_SRF_BF, srf_len, srf);
 242                 argv++;
 243                 argc--;
 244         } else if  (strcmp(argv[0], "list") == 0) {
 245                 struct nlattr *nl_macs = nla_nest_start(srf_attrs,
 246                                                 NL80211_NAN_SRF_MAC_ADDRS);
 247                 int i = 0;
 248
 249                 argc--;
 250                 argv++;
 251                 cur_mac = strtok_r(argv[0], ";", &sptr);
 252                 while (cur_mac) {
 253                         if (mac_addr_a2n(mac_addr, cur_mac))
 254                                 return -EINVAL;
 255
 256                         nla_put(srf_attrs, ++i, ETH_ALEN, mac_addr);
 257                         cur_mac = strtok_r(NULL, ";", &sptr);
 258                 }
 259
 260                 nla_nest_end(srf_attrs, nl_macs);
 261                 argv++;
 262                 argc--;
 263         } else {
 264                 return -EINVAL;
 265         }
 266
 267         nla_put_nested(func_attrs, NL80211_NAN_FUNC_SRF, srf_attrs);
 268         return old_argc - argc;
 269 nla_put_failure:
 270         return -ENOBUFS;
 271 }
 272
 273 static void parse_match_filter(char *filter, struct nl_msg *func_attrs, int tx)
 274 {
 275         struct nlattr *nl_filt;
 276         char *cur_filt, *sptr = NULL;
 277         int i = 0;
 278
 279         if (tx)
 280                 nl_filt = nla_nest_start(func_attrs,
 281                                          NL80211_NAN_FUNC_TX_MATCH_FILTER);
 282         else
 283                 nl_filt = nla_nest_start(func_attrs,
 284                                          NL80211_NAN_FUNC_RX_MATCH_FILTER);
 285
 286         cur_filt = strtok_r(filter, ":", &sptr);
 287         while (cur_filt) {
 288                 if (strcmp(cur_filt, "*") != 0)
 289                         nla_put(func_attrs, ++i, strlen(cur_filt), cur_filt);
 290                 else
 291                         nla_put(func_attrs, ++i, 0, NULL);
 292
 293                 cur_filt = strtok_r(NULL, ":", &sptr);
 294         }
 295
 296         nla_nest_end(func_attrs, nl_filt);
 297 }
 298
 299 static int handle_nan_add_func(struct nl80211_state *state,
 300                                struct nl_msg *msg, int argc, char **argv,
 301                                enum id_input id)
 302 {
 303         struct nl_msg *func_attrs = NULL;
 304         int err = 0;
 305         __u8 type;
 306
 307         func_attrs = nlmsg_alloc();
 308         if (!func_attrs) {
 309                 err = -ENOBUFS;
 310                 goto out;
 311         }
 312
 313         if (argc > 1 && strcmp(argv[0], "type") == 0) {
 314                 argv++;
 315                 argc--;
 316                 if (strcmp(argv[0], "publish") == 0)
 317                         type = NL80211_NAN_FUNC_PUBLISH;
 318                 else if (strcmp(argv[0], "subscribe") == 0)
 319                         type = NL80211_NAN_FUNC_SUBSCRIBE;
 320                 else if (strcmp(argv[0], "followup") == 0)
 321                         type = NL80211_NAN_FUNC_FOLLOW_UP;
 322                 else
 323                         return -EINVAL;
 324                 argv++;
 325                 argc--;
 326
 327                 NLA_PUT_U8(func_attrs, NL80211_NAN_FUNC_TYPE, type);
 328         } else {
 329                 return -EINVAL;
 330         }
 331
 332         if (type == NL80211_NAN_FUNC_SUBSCRIBE) {
 333                 if (argc > 1 && strcmp(argv[0], "active") == 0) {
 334                         argv++;
 335                         argc--;
 336                         NLA_PUT_FLAG(func_attrs,
 337                                      NL80211_NAN_FUNC_SUBSCRIBE_ACTIVE);
 338                 }
 339         }
 340
 341         if (type == NL80211_NAN_FUNC_PUBLISH) {
 342                 __u8 publish_type = 0;
 343
 344                 if (argc > 1 && strcmp(argv[0], "solicited") == 0) {
 345                         argv++;
 346                         argc--;
 347                         publish_type |= NL80211_NAN_SOLICITED_PUBLISH;
 348                 }
 349
 350                 if (argc > 1 && strcmp(argv[0], "unsolicited") == 0) {
 351                         argv++;
 352                         argc--;
 353                         publish_type |= NL80211_NAN_UNSOLICITED_PUBLISH;
 354                 }
 355
 356                 NLA_PUT_U8(func_attrs, NL80211_NAN_FUNC_PUBLISH_TYPE,
 357                            publish_type);
 358
 359                 /* only allow for solicited publish */
 360                 if (argc > 1 && strcmp(argv[0], "bcast") == 0) {
 361                         argv++;
 362                         argc--;
 363                         if (!(publish_type & NL80211_NAN_SOLICITED_PUBLISH))
 364                                 return -EINVAL;
 365
 366                         NLA_PUT_FLAG(func_attrs,
 367                                      NL80211_NAN_FUNC_PUBLISH_BCAST);
 368                 }
 369         }
 370
 371         if (argc > 1 && strcmp(argv[0], "close_range") == 0) {
 372                 argv++;
 373                 argc--;
 374                 NLA_PUT_FLAG(func_attrs, NL80211_NAN_FUNC_CLOSE_RANGE);
 375         }
 376
 377         if (argc > 1 && strcmp(argv[0], "name") == 0) {
 378                 unsigned char serv_id_c[6] = {0};
 379                 __u64 service_id;
 380
 381                 argv++;
 382                 argc--;
 383                 compute_service_id((const unsigned char *)argv[0],
 384                                    strlen(argv[0]), serv_id_c);
 385                 service_id = (__u64)serv_id_c[0] << 0  |
 386                              (__u64)serv_id_c[1] << 8  |
 387                              (__u64)serv_id_c[2] << 16 |
 388                              (__u64)serv_id_c[3] << 24 |
 389                              (__u64)serv_id_c[4] << 32 |
 390                              (__u64)serv_id_c[5] << 40;
 391
 392                 NLA_PUT(func_attrs, NL80211_NAN_FUNC_SERVICE_ID, 6,
 393                         &service_id);
 394                 argv++;
 395                 argc--;
 396         } else {
 397                 return -EINVAL;
 398         }
 399
 400         if (argc > 1 && strcmp(argv[0], "info") == 0) {
 401                 argv++;
 402                 argc--;
 403                 NLA_PUT(func_attrs, NL80211_NAN_FUNC_SERVICE_INFO,
 404                         strlen(argv[0]), argv[0]);
 405                 argv++;
 406                 argc--;
 407         }
 408
 409         if (type == NL80211_NAN_FUNC_FOLLOW_UP) {
 410                 if (argc > 1 && strcmp(argv[0], "flw_up_id") == 0) {
 411                         argv++;
 412                         argc--;
 413                         NLA_PUT_U8(func_attrs, NL80211_NAN_FUNC_FOLLOW_UP_ID,
 414                                    atoi(argv[0]));
 415                         argv++;
 416                         argc--;
 417                 }
 418
 419                 if (argc > 1 && strcmp(argv[0], "flw_up_req_id") == 0) {
 420                         argv++;
 421                         argc--;
 422                         NLA_PUT_U8(func_attrs,
 423                                    NL80211_NAN_FUNC_FOLLOW_UP_REQ_ID,
 424                                    atoi(argv[0]));
 425                         argv++;
 426                         argc--;
 427                 }
 428
 429                 if (argc > 1 && strcmp(argv[0], "flw_up_dest") == 0) {
 430                         unsigned char addr[6];
 431
 432                         argv++;
 433                         argc--;
 434                         if (mac_addr_a2n(addr, argv[0]))
 435                                 goto nla_put_failure;
 436                         nla_put(func_attrs, NL80211_NAN_FUNC_FOLLOW_UP_DEST,
 437                                 ETH_ALEN, addr);
 438                         argv++;
 439                         argc--;
 440                 }
 441         }
 442
 443         if (type != NL80211_NAN_FUNC_FOLLOW_UP &&
 444             argc > 1 && strcmp(argv[0], "ttl") == 0) {
 445                 argv++;
 446                 argc--;
 447                 NLA_PUT_U32(func_attrs, NL80211_NAN_FUNC_TTL, atoi(argv[0]));
 448                 argv++;
 449                 argc--;
 450         }
 451
 452         if (type != NL80211_NAN_FUNC_FOLLOW_UP &&
 453             argc >= 4 && strcmp(argv[0], "srf") == 0) {
 454                 int res;
 455
 456                 argv++;
 457                 argc--;
 458                 res = parse_srf(argv, argc, func_attrs);
 459                 if (res < 0)
 460                         return -EINVAL;
 461
 462                 argc -= res;
 463                 argv += res;
 464         }
 465
 466         if (type != NL80211_NAN_FUNC_FOLLOW_UP &&
 467             argc > 1 && strcmp(argv[0], "rx_filter") == 0) {
 468                 argv++;
 469                 argc--;
 470                 parse_match_filter(argv[0], func_attrs, 0);
 471
 472                 argv++;
 473                 argc--;
 474         }
 475
 476         if (type != NL80211_NAN_FUNC_FOLLOW_UP &&
 477             argc > 1 && strcmp(argv[0], "tx_filter") == 0) {
 478                 argv++;
 479                 argc--;
 480                 parse_match_filter(argv[0], func_attrs, 1);
 481
 482                 argv++;
 483                 argc--;
 484         }
 485
 486         if (argc != 0)
 487                 return -EINVAL;
 488
 489         nla_put_nested(msg, NL80211_ATTR_NAN_FUNC, func_attrs);
 490         register_handler(print_instance_id_handler, NULL);
 491
 492         return err;
 493 nla_put_failure:
 494         return -ENOBUFS;
 495 out:
 496         return err;
 497 }
 498 COMMAND(nan, add_func,
 499         "type <publish|subscribe|followup> [active] [solicited] [unsolicited] [bcast] [close_range] name <name> [info <info>] [flw_up_id <id> flw_up_req_id <id> flw_up_dest <mac>] [ttl <ttl>] [srf <include|exclude> <bf|list> [bf_idx] [bf_len] <mac1;mac2...>] [rx_filter <str1:str2...>] [tx_filter <str1:str2...>]",
 500         NL80211_CMD_ADD_NAN_FUNCTION, 0, CIB_WDEV,
 501         handle_nan_add_func, "");