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