src/netlink.c

   1 /*
   2  *
   3  *  neard - Near Field Communication manager
   4  *
   5  *  Copyright 2007, 2008        Johannes Berg <johannes@sipsolutions.net>
   6  *  Copyright (C) 2011 Instituto Nokia de Tecnologia
   7  *  Copyright (C) 2011  Intel Corporation. All rights reserved.
   8  *
   9  *  This program is free software; you can redistribute it and/or modify
  10  *  it under the terms of the GNU General Public License version 2 as
  11  *  published by the Free Software Foundation.
  12  *
  13  *  This program is distributed in the hope that it will be useful,
  14  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16  *  GNU General Public License for more details.
  17  *
  18  *  You should have received a copy of the GNU General Public License
  19  *  along with this program; if not, write to the Free Software
  20  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  21  *
  22  */
  23
  24 #ifdef HAVE_CONFIG_H
  25 #include <config.h>
  26 #endif
  27
  28 #include <stdio.h>
  29 #include <stdlib.h>
  30 #include <errno.h>
  31
  32 #include <netlink/netlink.h>
  33 #include <netlink/genl/genl.h>
  34 #include <netlink/genl/family.h>
  35 #include <netlink/genl/ctrl.h>
  36
  37 #include "near.h"
  38
  39 #ifdef NEED_LIBNL_COMPAT
  40 #define nl_sock nl_handle
  41
  42 static inline struct nl_handle *nl_socket_alloc(void)
  43 {
  44         return nl_handle_alloc();
  45 }
  46
  47 static inline void nl_socket_free(struct nl_sock *h)
  48 {
  49         nl_handle_destroy(h);
  50 }
  51
  52 #define NLE_MISSING_ATTR        14
  53
  54 static inline void __nl_perror(int error, const char *s)
  55 {
  56         nl_perror(s);
  57 }
  58 #define nl_perror __nl_perror
  59 #endif
  60
  61 struct nlnfc_state {
  62         struct nl_sock *cmd_sock;
  63         struct nl_sock *event_sock;
  64         int nfc_id;
  65         int mcid;
  66 };
  67
  68 static struct nlnfc_state *nfc_state;
  69 static GIOChannel *netlink_channel = NULL;
  70
  71 struct send_msg_data {
  72         void *data;
  73         int *done;
  74         int (*finish_handler)(struct nl_msg *, void *);
  75 };
  76
  77 static int error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err,
  78                          void *arg)
  79 {
  80         int *ret = arg;
  81
  82         DBG("");
  83
  84         *ret = err->error;
  85
  86         return NL_STOP;
  87 }
  88
  89 static int __finish_handler(struct nl_msg *msg, void *arg)
  90 {
  91         struct send_msg_data *data = arg;
  92
  93
  94         DBG("");
  95
  96         if (data->finish_handler)
  97                 data->finish_handler(msg, data->data);
  98
  99         *(data->done) = 1;
 100
 101         return NL_SKIP;
 102 }
 103
 104 static int ack_handler(struct nl_msg *msg, void *arg)
 105 {
 106         int *ret = arg;
 107
 108         DBG("");
 109
 110         *ret = 1;
 111
 112         return NL_STOP;
 113 }
 114
 115 static int __nl_send_msg(struct nl_sock *sock, struct nl_msg *msg,
 116                         int (*rx_handler)(struct nl_msg *, void *),
 117                         int (*finish_handler)(struct nl_msg *, void *),
 118                         void *data)
 119 {
 120         struct nl_cb *cb;
 121         int err, done;
 122         struct send_msg_data send_data;
 123
 124         DBG("");
 125
 126         cb = nl_cb_alloc(NL_CB_DEFAULT);
 127         if (!cb)
 128                 return -ENOMEM;
 129
 130         err = nl_send_auto_complete(sock, msg);
 131         if (err < 0) {
 132                 nl_cb_put(cb);
 133                 near_error("%s", strerror(err));
 134
 135                 return err;
 136         }
 137
 138         err = done = 0;
 139         send_data.done = &done;
 140         send_data.data = data;
 141         send_data.finish_handler = finish_handler;
 142
 143         nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, __finish_handler, &send_data);
 144         nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err);
 145         nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &done);
 146
 147         if (rx_handler)
 148                 nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, rx_handler, data);
 149
 150         while (err == 0 && done == 0)
 151                 nl_recvmsgs(sock, cb);
 152
 153         nl_cb_put(cb);
 154
 155         return err;
 156 }
 157
 158 static inline int nl_send_msg(struct nl_sock *sock, struct nl_msg *msg,
 159                         int (*rx_handler)(struct nl_msg *, void *),
 160                         void *data)
 161 {
 162         return __nl_send_msg(sock, msg, rx_handler, NULL, data);
 163 }
 164
 165 static int get_devices_handler(struct nl_msg *n, void *arg)
 166 {
 167         struct nlmsghdr *nlh = nlmsg_hdr(n);
 168         struct nlattr *attrs[NFC_ATTR_MAX + 1];
 169         char *name;
 170         uint32_t idx, protocols;
 171         bool powered;
 172
 173         DBG("");
 174
 175         genlmsg_parse(nlh, 0, attrs, NFC_ATTR_MAX, NULL);
 176
 177         if (!attrs[NFC_ATTR_DEVICE_INDEX] ||
 178             !attrs[NFC_ATTR_DEVICE_NAME] ||
 179             !attrs[NFC_ATTR_PROTOCOLS]) {
 180                 nl_perror(NLE_MISSING_ATTR, "NFC_CMD_GET_DEVICE");
 181                 return NL_STOP;
 182         }
 183
 184         idx = nla_get_u32(attrs[NFC_ATTR_DEVICE_INDEX]);
 185         name = nla_get_string(attrs[NFC_ATTR_DEVICE_NAME]);
 186         protocols = nla_get_u32(attrs[NFC_ATTR_PROTOCOLS]);
 187
 188         if (!attrs[NFC_ATTR_DEVICE_POWERED])
 189                 powered = false;
 190         else
 191                 powered = nla_get_u8(attrs[NFC_ATTR_DEVICE_POWERED]);
 192
 193         __near_manager_adapter_add(idx, name, protocols, powered);
 194
 195         return NL_SKIP;
 196 }
 197
 198 int __near_netlink_get_adapters(void)
 199 {
 200         struct nl_msg *msg;
 201         void *hdr;
 202         int err;
 203
 204         DBG("");
 205
 206         if (!nfc_state || nfc_state->nfc_id < 0)
 207                 return -ENODEV;
 208
 209         msg = nlmsg_alloc();
 210         if (!msg)
 211                 return -ENOMEM;
 212
 213         hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, nfc_state->nfc_id, 0,
 214                           NLM_F_DUMP, NFC_CMD_GET_DEVICE, NFC_GENL_VERSION);
 215         if (!hdr) {
 216                 err = -EINVAL;
 217                 goto out;
 218         }
 219
 220         err = nl_send_msg(nfc_state->cmd_sock, msg, get_devices_handler, NULL);
 221
 222 out:
 223         nlmsg_free(msg);
 224
 225         return err;
 226 }
 227
 228 int __near_netlink_start_poll(int idx,
 229                                 uint32_t im_protocols, uint32_t tm_protocols)
 230 {
 231         struct nl_msg *msg;
 232         void *hdr;
 233         int err;
 234
 235         DBG("IM protos 0x%x TM protos 0x%x", im_protocols, tm_protocols);
 236
 237         msg = nlmsg_alloc();
 238         if (!msg)
 239                 return -ENOMEM;
 240
 241         hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, nfc_state->nfc_id, 0,
 242                         NLM_F_REQUEST, NFC_CMD_START_POLL, NFC_GENL_VERSION);
 243         if (!hdr) {
 244                 err = -EINVAL;
 245                 goto nla_put_failure;
 246         }
 247
 248         err = -EMSGSIZE;
 249
 250         NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, idx);
 251         if (im_protocols != 0) {
 252                 NLA_PUT_U32(msg, NFC_ATTR_IM_PROTOCOLS, im_protocols);
 253                 NLA_PUT_U32(msg, NFC_ATTR_PROTOCOLS, im_protocols);
 254         }
 255         if (tm_protocols != 0)
 256                 NLA_PUT_U32(msg, NFC_ATTR_TM_PROTOCOLS, tm_protocols);
 257
 258         err = nl_send_msg(nfc_state->cmd_sock, msg, NULL, NULL);
 259
 260 nla_put_failure:
 261         nlmsg_free(msg);
 262
 263         return err;
 264 }
 265
 266 int __near_netlink_stop_poll(int idx)
 267 {
 268         struct nl_msg *msg;
 269         void *hdr;
 270         int err;
 271
 272         DBG("");
 273
 274         msg = nlmsg_alloc();
 275         if (!msg)
 276                 return -ENOMEM;
 277
 278         hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, nfc_state->nfc_id, 0,
 279                         NLM_F_REQUEST, NFC_CMD_STOP_POLL, NFC_GENL_VERSION);
 280         if (!hdr) {
 281                 err = -EINVAL;
 282                 goto nla_put_failure;
 283         }
 284
 285         err = -EMSGSIZE;
 286
 287         NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, idx);
 288
 289         err = nl_send_msg(nfc_state->cmd_sock, msg, NULL, NULL);
 290
 291 nla_put_failure:
 292         nlmsg_free(msg);
 293
 294         return err;
 295 }
 296
 297 int __near_netlink_dep_link_up(uint32_t idx, uint32_t target_idx,
 298                                 uint8_t comm_mode, uint8_t rf_mode)
 299 {
 300         struct nl_msg *msg;
 301         void *hdr;
 302         int err;
 303
 304         DBG("");
 305
 306         msg = nlmsg_alloc();
 307         if (!msg)
 308                 return -ENOMEM;
 309
 310         hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, nfc_state->nfc_id, 0,
 311                         NLM_F_REQUEST, NFC_CMD_DEP_LINK_UP, NFC_GENL_VERSION);
 312         if (!hdr) {
 313                 err = -EINVAL;
 314                 goto nla_put_failure;
 315         }
 316
 317         err = -EMSGSIZE;
 318
 319         NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, idx);
 320         NLA_PUT_U32(msg, NFC_ATTR_TARGET_INDEX, target_idx);
 321         NLA_PUT_U8(msg, NFC_ATTR_COMM_MODE, comm_mode);
 322         NLA_PUT_U8(msg, NFC_ATTR_RF_MODE, rf_mode);
 323
 324         err = nl_send_msg(nfc_state->cmd_sock, msg, NULL, NULL);
 325
 326 nla_put_failure:
 327         nlmsg_free(msg);
 328
 329         return err;
 330 }
 331
 332 int __near_netlink_dep_link_down(uint32_t idx)
 333 {
 334         struct nl_msg *msg;
 335         void *hdr;
 336         int err;
 337
 338         DBG("");
 339
 340         msg = nlmsg_alloc();
 341         if (!msg)
 342                 return -ENOMEM;
 343
 344         hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, nfc_state->nfc_id, 0,
 345                         NLM_F_REQUEST, NFC_CMD_DEP_LINK_DOWN, NFC_GENL_VERSION);
 346         if (!hdr) {
 347                 err = -EINVAL;
 348                 goto nla_put_failure;
 349         }
 350
 351         err = -EMSGSIZE;
 352
 353         NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, idx);
 354
 355         err = nl_send_msg(nfc_state->cmd_sock, msg, NULL, NULL);
 356
 357 nla_put_failure:
 358         nlmsg_free(msg);
 359
 360         return err;
 361 }
 362
 363 int __near_netlink_adapter_enable(int idx, bool enable)
 364 {
 365         struct nl_msg *msg;
 366         void *hdr;
 367         int err;
 368         uint8_t cmd;
 369
 370         DBG("");
 371
 372         msg = nlmsg_alloc();
 373         if (!msg)
 374                 return -ENOMEM;
 375
 376         if (enable)
 377                 cmd = NFC_CMD_DEV_UP;
 378         else
 379                 cmd = NFC_CMD_DEV_DOWN;
 380
 381         hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, nfc_state->nfc_id, 0,
 382                         NLM_F_REQUEST, cmd, NFC_GENL_VERSION);
 383         if (!hdr) {
 384                 err = -EINVAL;
 385                 goto nla_put_failure;
 386         }
 387
 388         err = -EMSGSIZE;
 389
 390         NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, idx);
 391
 392         err = nl_send_msg(nfc_state->cmd_sock, msg, NULL, NULL);
 393
 394 nla_put_failure:
 395         nlmsg_free(msg);
 396
 397         return err;
 398 }
 399
 400
 401 static int no_seq_check(struct nl_msg *n, void *arg)
 402 {
 403         DBG("");
 404
 405         return NL_OK;
 406 }
 407
 408 static int nfc_netlink_event_adapter(struct genlmsghdr *gnlh, bool add)
 409 {
 410         struct nlattr *attrs[NFC_ATTR_MAX + 1];
 411         uint32_t idx;
 412
 413         DBG("");
 414
 415         nla_parse(attrs, NFC_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
 416                   genlmsg_attrlen(gnlh, 0), NULL);
 417         if (!attrs[NFC_ATTR_DEVICE_INDEX]) {
 418                 near_error("Missing device index");
 419                 return -ENODEV;
 420         }
 421
 422         idx = nla_get_u32(attrs[NFC_ATTR_DEVICE_INDEX]);
 423
 424         if (add &&
 425                 (!attrs[NFC_ATTR_DEVICE_NAME] ||
 426                         !attrs[NFC_ATTR_PROTOCOLS])) {
 427                 near_error("Missing attributes");
 428                 return -EINVAL;
 429         }
 430
 431         if (add) {
 432                 char *name;
 433                 uint32_t protocols;
 434                 bool powered;
 435
 436                 name = nla_get_string(attrs[NFC_ATTR_DEVICE_NAME]);
 437                 protocols = nla_get_u32(attrs[NFC_ATTR_PROTOCOLS]);
 438                 if (!attrs[NFC_ATTR_DEVICE_POWERED])
 439                         powered = false;
 440                 else
 441                         powered = nla_get_u8(attrs[NFC_ATTR_DEVICE_POWERED]);
 442
 443                 return __near_manager_adapter_add(idx, name,
 444                                                 protocols, powered);
 445         } else {
 446                 __near_manager_adapter_remove(idx);
 447         }
 448
 449         return 0;
 450 }
 451
 452 static int get_targets_handler(struct nl_msg *n, void *arg)
 453 {
 454         struct nlmsghdr *nlh = nlmsg_hdr(n);
 455         struct nlattr *attrs[NFC_ATTR_MAX + 1];
 456         uint32_t adapter_idx, target_idx, protocols;
 457         uint16_t sens_res = 0;
 458         uint8_t sel_res = 0;
 459         uint8_t nfcid[NFC_MAX_NFCID1_LEN], nfcid_len;
 460
 461         DBG("");
 462
 463         genlmsg_parse(nlh, 0, attrs, NFC_ATTR_MAX, NULL);
 464
 465         adapter_idx = *((uint32_t *)arg);
 466         target_idx = nla_get_u32(attrs[NFC_ATTR_TARGET_INDEX]);
 467         protocols = nla_get_u32(attrs[NFC_ATTR_PROTOCOLS]);
 468
 469         if (attrs[NFC_ATTR_TARGET_SENS_RES])
 470                 sens_res =
 471                         nla_get_u16(attrs[NFC_ATTR_TARGET_SENS_RES]);
 472
 473         if (attrs[NFC_ATTR_TARGET_SEL_RES])
 474                 sel_res =
 475                         nla_get_u16(attrs[NFC_ATTR_TARGET_SEL_RES]);
 476
 477         if (attrs[NFC_ATTR_TARGET_NFCID1]) {
 478                 nfcid_len = nla_len(attrs[NFC_ATTR_TARGET_NFCID1]);
 479                 if (nfcid_len <= NFC_MAX_NFCID1_LEN)
 480                         memcpy(nfcid, nla_data(attrs[NFC_ATTR_TARGET_NFCID1]),
 481                                                                 nfcid_len);
 482         } else {
 483                 nfcid_len = 0;
 484         }
 485
 486         DBG("target idx %d proto 0x%x sens_res 0x%x sel_res 0x%x NFCID len %d",
 487             target_idx, protocols, sens_res, sel_res, nfcid_len);
 488
 489         __near_adapter_add_target(adapter_idx, target_idx, protocols,
 490                                   sens_res, sel_res, nfcid, nfcid_len);
 491
 492         return 0;
 493 }
 494
 495 static int get_targets_finish_handler(struct nl_msg *n, void *arg)
 496 {
 497         uint32_t adapter_idx;
 498
 499         DBG("");
 500
 501         adapter_idx = *((uint32_t *)arg);
 502
 503         return __near_adapter_get_targets_done(adapter_idx);
 504 }
 505
 506 static int nfc_netlink_event_targets_found(struct genlmsghdr *gnlh)
 507 {
 508         struct nlattr *attr[NFC_ATTR_MAX + 1];
 509         struct nl_msg *msg;
 510         void *hdr;
 511         int err;
 512         uint32_t adapter_idx;
 513
 514         DBG("");
 515
 516         nla_parse(attr, NFC_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
 517                         genlmsg_attrlen(gnlh, 0), NULL);
 518         if (!attr[NFC_ATTR_DEVICE_INDEX])
 519                 return -ENODEV;
 520
 521         adapter_idx = nla_get_u32(attr[NFC_ATTR_DEVICE_INDEX]);
 522
 523         DBG("adapter %d", adapter_idx);
 524
 525         msg = nlmsg_alloc();
 526         if (!msg)
 527                 return -ENOMEM;
 528
 529         hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, nfc_state->nfc_id, 0,
 530                         NLM_F_DUMP, NFC_CMD_GET_TARGET, NFC_GENL_VERSION);
 531         if (!hdr) {
 532                 err = -EINVAL;
 533                 goto nla_put_failure;
 534         }
 535
 536         err = -EMSGSIZE;
 537
 538         NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, adapter_idx);
 539
 540         err = __nl_send_msg(nfc_state->cmd_sock, msg,
 541                                 get_targets_handler, get_targets_finish_handler,
 542                                                                 &adapter_idx);
 543
 544 nla_put_failure:
 545         nlmsg_free(msg);
 546
 547         return err;
 548 }
 549
 550 static int nfc_netlink_event_target_lost(struct genlmsghdr *gnlh)
 551 {
 552         struct nlattr *attr[NFC_ATTR_MAX + 1];
 553         uint32_t adapter_idx, target_idx;
 554
 555         DBG("");
 556
 557         nla_parse(attr, NFC_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
 558                         genlmsg_attrlen(gnlh, 0), NULL);
 559
 560         if (!attr[NFC_ATTR_DEVICE_INDEX])
 561                 return -ENODEV;
 562
 563         if (!attr[NFC_ATTR_TARGET_INDEX])
 564                 return -ENODEV;
 565
 566         adapter_idx = nla_get_u32(attr[NFC_ATTR_DEVICE_INDEX]);
 567         target_idx = nla_get_u32(attr[NFC_ATTR_TARGET_INDEX]);
 568
 569         DBG("adapter %d target %d", adapter_idx, target_idx);
 570
 571         return __near_adapter_remove_target(adapter_idx, target_idx);
 572 }
 573
 574 static int nfc_netlink_event_dep_up(struct genlmsghdr *gnlh)
 575 {
 576         struct nlattr *attrs[NFC_ATTR_MAX + 1];
 577         uint32_t idx, target_idx = 0;
 578         uint8_t rf_mode;
 579
 580         DBG("");
 581
 582         nla_parse(attrs, NFC_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
 583                   genlmsg_attrlen(gnlh, 0), NULL);
 584         if (!attrs[NFC_ATTR_DEVICE_INDEX]) {
 585                 near_error("Missing device index");
 586                 return -ENODEV;
 587         }
 588
 589         if (!attrs[NFC_ATTR_COMM_MODE] ||
 590                         !attrs[NFC_ATTR_RF_MODE]) {
 591                 near_error("Missing rf or comm modes");
 592                 return -ENODEV;
 593         }
 594
 595         idx = nla_get_u32(attrs[NFC_ATTR_DEVICE_INDEX]);
 596         rf_mode = nla_get_u8(attrs[NFC_ATTR_RF_MODE]);
 597
 598         if (rf_mode == NFC_RF_INITIATOR) {
 599                 if (!attrs[NFC_ATTR_TARGET_INDEX]) {
 600                         near_error("Missing target index");
 601                         return -ENODEV;
 602                 };
 603
 604                 target_idx = nla_get_u32(attrs[NFC_ATTR_TARGET_INDEX]);
 605
 606                 DBG("%d %d", idx, target_idx);
 607
 608                 return __near_adapter_set_dep_state(idx, true);
 609         } else {
 610                 return -EOPNOTSUPP;
 611         }
 612 }
 613
 614 static int nfc_netlink_event_dep_down(struct genlmsghdr *gnlh)
 615 {
 616         struct nlattr *attrs[NFC_ATTR_MAX + 1];
 617         uint32_t idx;
 618
 619         DBG("");
 620
 621         nla_parse(attrs, NFC_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
 622                   genlmsg_attrlen(gnlh, 0), NULL);
 623         if (!attrs[NFC_ATTR_DEVICE_INDEX]) {
 624                 near_error("Missing device index");
 625                 return -ENODEV;
 626         }
 627
 628         idx = nla_get_u32(attrs[NFC_ATTR_DEVICE_INDEX]);
 629
 630         __near_adapter_set_dep_state(idx, false);
 631
 632         return 0;
 633 }
 634
 635 static int nfc_netlink_event_tm_activated(struct genlmsghdr *gnlh)
 636 {
 637         struct nlattr *attrs[NFC_ATTR_MAX + 1];
 638         uint32_t idx;
 639
 640         DBG("");
 641
 642         nla_parse(attrs, NFC_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
 643                   genlmsg_attrlen(gnlh, 0), NULL);
 644         if (!attrs[NFC_ATTR_DEVICE_INDEX]) {
 645                 near_error("Missing device index");
 646                 return -ENODEV;
 647         }
 648
 649         idx = nla_get_u32(attrs[NFC_ATTR_DEVICE_INDEX]);
 650
 651         DBG("%d", idx);
 652
 653         return __near_adapter_add_device(idx, NULL, 0);
 654 }
 655
 656 static int nfc_netlink_event_tm_deactivated(struct genlmsghdr *gnlh)
 657 {
 658         struct nlattr *attrs[NFC_ATTR_MAX + 1];
 659         uint32_t idx;
 660
 661         DBG("");
 662
 663         nla_parse(attrs, NFC_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
 664                   genlmsg_attrlen(gnlh, 0), NULL);
 665         if (!attrs[NFC_ATTR_DEVICE_INDEX]) {
 666                 near_error("Missing device index");
 667                 return -ENODEV;
 668         }
 669
 670         idx = nla_get_u32(attrs[NFC_ATTR_DEVICE_INDEX]);
 671
 672         DBG("%d", idx);
 673
 674         return __near_adapter_remove_device(idx);
 675 }
 676
 677 static int nfc_netlink_event(struct nl_msg *n, void *arg)
 678 {
 679         struct sockaddr_nl *src = nlmsg_get_src(n);
 680         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(n));
 681
 682         DBG("event 0x%x", gnlh->cmd);
 683
 684         if (src->nl_pid) {
 685                 near_error("WARNING: Wrong netlink message sender %d",
 686                                                                 src->nl_pid);
 687                 return NL_SKIP;
 688         }
 689
 690         switch (gnlh->cmd) {
 691         case NFC_EVENT_TARGETS_FOUND:
 692                 DBG("Targets found");
 693                 nfc_netlink_event_targets_found(gnlh);
 694                 break;
 695         case NFC_EVENT_TARGET_LOST:
 696                 DBG("Target lost");
 697                 nfc_netlink_event_target_lost(gnlh);
 698                 break;
 699         case NFC_EVENT_DEVICE_ADDED:
 700                 DBG("Adapter added");
 701                 nfc_netlink_event_adapter(gnlh, true);
 702
 703                 break;
 704         case NFC_EVENT_DEVICE_REMOVED:
 705                 DBG("Adapter removed");
 706                 nfc_netlink_event_adapter(gnlh, false);
 707
 708                 break;
 709         case NFC_CMD_DEP_LINK_UP:
 710                 DBG("DEP link is up");
 711                 nfc_netlink_event_dep_up(gnlh);
 712
 713                 break;
 714         case NFC_CMD_DEP_LINK_DOWN:
 715                 DBG("DEP link is down");
 716                 nfc_netlink_event_dep_down(gnlh);
 717
 718                 break;
 719         case NFC_EVENT_TM_ACTIVATED:
 720                 DBG("Target mode activated");
 721                 nfc_netlink_event_tm_activated(gnlh);
 722
 723                 break;
 724         case NFC_EVENT_TM_DEACTIVATED:
 725                 DBG("Target mode deactivated");
 726                 nfc_netlink_event_tm_deactivated(gnlh);
 727
 728                 break;
 729         }
 730
 731         return NL_SKIP;
 732 }
 733
 734 static gboolean __nfc_netlink_event(GIOChannel *channel,
 735                                 GIOCondition cond, gpointer data)
 736 {
 737         struct nl_cb *cb;
 738         struct nlnfc_state *state = data;
 739
 740         if (cond & (G_IO_NVAL | G_IO_HUP | G_IO_ERR))
 741                 return FALSE;
 742
 743         cb = nl_cb_alloc(NL_CB_VERBOSE);
 744         if (!cb)
 745                 return TRUE;
 746
 747         nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL);
 748         nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, nfc_netlink_event, data);
 749
 750         nl_recvmsgs(state->event_sock, cb);
 751
 752         nl_cb_put(cb);
 753
 754         return TRUE;
 755 }
 756
 757 static int nfc_event_listener(struct nlnfc_state *state)
 758 {
 759         int sock;
 760
 761         sock = nl_socket_get_fd(state->event_sock);
 762         netlink_channel = g_io_channel_unix_new(sock);
 763         g_io_channel_set_close_on_unref(netlink_channel, TRUE);
 764
 765         g_io_channel_set_encoding(netlink_channel, NULL, NULL);
 766         g_io_channel_set_buffered(netlink_channel, FALSE);
 767
 768         g_io_add_watch(netlink_channel,
 769                                 G_IO_IN | G_IO_NVAL | G_IO_HUP | G_IO_ERR,
 770                                                 __nfc_netlink_event, state);
 771
 772         return 0;
 773 }
 774
 775 struct handler_args {
 776         const char *group;
 777         int id;
 778 };
 779
 780 static int family_handler(struct nl_msg *msg, void *arg)
 781 {
 782         struct handler_args *grp = arg;
 783         struct nlattr *tb[CTRL_ATTR_MAX + 1];
 784         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 785         struct nlattr *mcgrp;
 786         int rem_mcgrp;
 787
 788         DBG("");
 789
 790         nla_parse(tb, CTRL_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
 791                   genlmsg_attrlen(gnlh, 0), NULL);
 792
 793         if (!tb[CTRL_ATTR_MCAST_GROUPS])
 794                 return NL_SKIP;
 795
 796         nla_for_each_nested(mcgrp, tb[CTRL_ATTR_MCAST_GROUPS], rem_mcgrp) {
 797                 struct nlattr *tb_mcgrp[CTRL_ATTR_MCAST_GRP_MAX + 1];
 798
 799                 nla_parse(tb_mcgrp, CTRL_ATTR_MCAST_GRP_MAX,
 800                           nla_data(mcgrp), nla_len(mcgrp), NULL);
 801
 802                 if (!tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME] ||
 803                     !tb_mcgrp[CTRL_ATTR_MCAST_GRP_ID])
 804                         continue;
 805                 if (strncmp(nla_data(tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME]),
 806                             grp->group, nla_len(tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME])))
 807                         continue;
 808                 grp->id = nla_get_u32(tb_mcgrp[CTRL_ATTR_MCAST_GRP_ID]);
 809                 break;
 810         }
 811
 812         return NL_SKIP;
 813 }
 814
 815 static int nl_get_multicast_id(struct nl_sock *sock, const char *family,
 816                                 const char *group)
 817 {
 818         struct nl_msg *msg;
 819         int err, ctrlid;
 820         struct handler_args grp = {
 821                 .group = group,
 822                 .id = -ENOENT,
 823         };
 824
 825         DBG("");
 826
 827         msg = nlmsg_alloc();
 828         if (!msg)
 829                 return -ENOMEM;
 830
 831         ctrlid = genl_ctrl_resolve(sock, "nlctrl");
 832
 833         genlmsg_put(msg, 0, 0, ctrlid, 0, 0, CTRL_CMD_GETFAMILY, 0);
 834
 835         err = -EMSGSIZE;
 836
 837         NLA_PUT_STRING(msg, CTRL_ATTR_FAMILY_NAME, family);
 838
 839         err = nl_send_msg(sock, msg, family_handler, &grp);
 840         if (err)
 841                 goto nla_put_failure;
 842
 843         DBG("multicast id %d", grp.id);
 844
 845         err = grp.id;
 846
 847 nla_put_failure:
 848         nlmsg_free(msg);
 849
 850         return err;
 851 }
 852
 853 int __near_netlink_init(void)
 854 {
 855         int err;
 856
 857         DBG("");
 858
 859         nfc_state = g_try_malloc0(sizeof(struct nlnfc_state));
 860         if (!nfc_state)
 861                 return -ENOMEM;
 862
 863         nfc_state->cmd_sock = nl_socket_alloc();
 864         if (!nfc_state->cmd_sock) {
 865                 near_error("Failed to allocate NFC command netlink socket");
 866                 err = -ENOMEM;
 867                 goto state_free;
 868         }
 869
 870         nfc_state->event_sock = nl_socket_alloc();
 871         if (!nfc_state->event_sock) {
 872                 near_error("Failed to allocate NFC event netlink socket");
 873                 err = -ENOMEM;
 874                 goto handle_cmd_destroy;
 875         }
 876
 877         if (genl_connect(nfc_state->cmd_sock)) {
 878                 near_error("Failed to connect to generic netlink");
 879                 err = -ENOLINK;
 880                 goto handle_event_destroy;
 881         }
 882
 883         if (genl_connect(nfc_state->event_sock)) {
 884                 near_error("Failed to connect to generic netlink");
 885                 err = -ENOLINK;
 886                 goto handle_event_destroy;
 887         }
 888
 889         nfc_state->nfc_id = genl_ctrl_resolve(nfc_state->cmd_sock, "nfc");
 890         if (nfc_state->nfc_id < 0) {
 891                 near_error("Unable to find NFC netlink family");
 892                 err = -ENOENT;
 893                 goto handle_event_destroy;
 894         }
 895
 896         nfc_state->mcid = nl_get_multicast_id(nfc_state->cmd_sock, NFC_GENL_NAME,
 897                                                 NFC_GENL_MCAST_EVENT_NAME);
 898         if (nfc_state->mcid <= 0) {
 899                 near_error("Wrong mcast id %d", nfc_state->mcid);
 900                 err = nfc_state->mcid;
 901                 goto handle_event_destroy;
 902         }
 903
 904         err = nl_socket_add_membership(nfc_state->event_sock, nfc_state->mcid);
 905         if (err) {
 906                 near_error("Error adding nl event socket to membership");
 907                 goto handle_event_destroy;
 908         }
 909
 910         return nfc_event_listener(nfc_state);
 911
 912 handle_event_destroy:
 913         nl_socket_free(nfc_state->event_sock);
 914
 915 handle_cmd_destroy:
 916         nl_socket_free(nfc_state->cmd_sock);
 917
 918 state_free:
 919         g_free(nfc_state);
 920
 921         nfc_state = NULL;
 922
 923         near_error("netlink init failed");
 924
 925         return err;
 926 }
 927
 928 void __near_netlink_cleanup(void)
 929 {
 930         if (netlink_channel) {
 931                 g_io_channel_shutdown(netlink_channel, TRUE, NULL);
 932                 g_io_channel_unref(netlink_channel);
 933
 934                 netlink_channel = NULL;
 935         }
 936
 937         if (!nfc_state)
 938                 return;
 939
 940         nl_socket_free(nfc_state->cmd_sock);
 941         nl_socket_free(nfc_state->event_sock);
 942
 943         g_free(nfc_state);
 944
 945         DBG("");
 946 }