net/nfc/netlink.c

   1 /*
   2  * Copyright (C) 2011 Instituto Nokia de Tecnologia
   3  *
   4  * Authors:
   5  *    Lauro Ramos Venancio <lauro.venancio@openbossa.org>
   6  *    Aloisio Almeida Jr <aloisio.almeida@openbossa.org>
   7  *
   8  * Vendor commands implementation based on net/wireless/nl80211.c
   9  * which is:
  10  *
  11  * Copyright 2006-2010  Johannes Berg <johannes@sipsolutions.net>
  12  * Copyright 2013-2014  Intel Mobile Communications GmbH
  13  *
  14  * This program is free software; you can redistribute it and/or modify
  15  * it under the terms of the GNU General Public License as published by
  16  * the Free Software Foundation; either version 2 of the License, or
  17  * (at your option) any later version.
  18  *
  19  * This program is distributed in the hope that it will be useful,
  20  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  21  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  22  * GNU General Public License for more details.
  23  *
  24  * You should have received a copy of the GNU General Public License
  25  * along with this program; if not, see <http://www.gnu.org/licenses/>.
  26  */
  27
  28 #define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__
  29
  30 #include <net/genetlink.h>
  31 #include <linux/nfc.h>
  32 #include <linux/slab.h>
  33
  34 #include "nfc.h"
  35 #include "llcp.h"
  36
  37 static const struct genl_multicast_group nfc_genl_mcgrps[] = {
  38         { .name = NFC_GENL_MCAST_EVENT_NAME, },
  39 };
  40
  41 static struct genl_family nfc_genl_family;
  42 static const struct nla_policy nfc_genl_policy[NFC_ATTR_MAX + 1] = {
  43         [NFC_ATTR_DEVICE_INDEX] = { .type = NLA_U32 },
  44         [NFC_ATTR_DEVICE_NAME] = { .type = NLA_STRING,
  45                                 .len = NFC_DEVICE_NAME_MAXSIZE },
  46         [NFC_ATTR_PROTOCOLS] = { .type = NLA_U32 },
  47         [NFC_ATTR_TARGET_INDEX] = { .type = NLA_U32 },
  48         [NFC_ATTR_COMM_MODE] = { .type = NLA_U8 },
  49         [NFC_ATTR_RF_MODE] = { .type = NLA_U8 },
  50         [NFC_ATTR_DEVICE_POWERED] = { .type = NLA_U8 },
  51         [NFC_ATTR_IM_PROTOCOLS] = { .type = NLA_U32 },
  52         [NFC_ATTR_TM_PROTOCOLS] = { .type = NLA_U32 },
  53         [NFC_ATTR_LLC_PARAM_LTO] = { .type = NLA_U8 },
  54         [NFC_ATTR_LLC_PARAM_RW] = { .type = NLA_U8 },
  55         [NFC_ATTR_LLC_PARAM_MIUX] = { .type = NLA_U16 },
  56         [NFC_ATTR_LLC_SDP] = { .type = NLA_NESTED },
  57         [NFC_ATTR_FIRMWARE_NAME] = { .type = NLA_STRING,
  58                                      .len = NFC_FIRMWARE_NAME_MAXSIZE },
  59         [NFC_ATTR_SE_INDEX] = { .type = NLA_U32 },
  60         [NFC_ATTR_SE_APDU] = { .type = NLA_BINARY },
  61         [NFC_ATTR_VENDOR_ID] = { .type = NLA_U32 },
  62         [NFC_ATTR_VENDOR_SUBCMD] = { .type = NLA_U32 },
  63         [NFC_ATTR_VENDOR_DATA] = { .type = NLA_BINARY },
  64
  65 };
  66
  67 static const struct nla_policy nfc_sdp_genl_policy[NFC_SDP_ATTR_MAX + 1] = {
  68         [NFC_SDP_ATTR_URI] = { .type = NLA_STRING,
  69                                .len = U8_MAX - 4 },
  70         [NFC_SDP_ATTR_SAP] = { .type = NLA_U8 },
  71 };
  72
  73 static int nfc_genl_send_target(struct sk_buff *msg, struct nfc_target *target,
  74                                 struct netlink_callback *cb, int flags)
  75 {
  76         void *hdr;
  77
  78         hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).portid, cb->nlh->nlmsg_seq,
  79                           &nfc_genl_family, flags, NFC_CMD_GET_TARGET);
  80         if (!hdr)
  81                 return -EMSGSIZE;
  82
  83         genl_dump_check_consistent(cb, hdr);
  84
  85         if (nla_put_u32(msg, NFC_ATTR_TARGET_INDEX, target->idx) ||
  86             nla_put_u32(msg, NFC_ATTR_PROTOCOLS, target->supported_protocols) ||
  87             nla_put_u16(msg, NFC_ATTR_TARGET_SENS_RES, target->sens_res) ||
  88             nla_put_u8(msg, NFC_ATTR_TARGET_SEL_RES, target->sel_res))
  89                 goto nla_put_failure;
  90         if (target->nfcid1_len > 0 &&
  91             nla_put(msg, NFC_ATTR_TARGET_NFCID1, target->nfcid1_len,
  92                     target->nfcid1))
  93                 goto nla_put_failure;
  94         if (target->sensb_res_len > 0 &&
  95             nla_put(msg, NFC_ATTR_TARGET_SENSB_RES, target->sensb_res_len,
  96                     target->sensb_res))
  97                 goto nla_put_failure;
  98         if (target->sensf_res_len > 0 &&
  99             nla_put(msg, NFC_ATTR_TARGET_SENSF_RES, target->sensf_res_len,
 100                     target->sensf_res))
 101                 goto nla_put_failure;
 102
 103         if (target->is_iso15693) {
 104                 if (nla_put_u8(msg, NFC_ATTR_TARGET_ISO15693_DSFID,
 105                                target->iso15693_dsfid) ||
 106                     nla_put(msg, NFC_ATTR_TARGET_ISO15693_UID,
 107                             sizeof(target->iso15693_uid), target->iso15693_uid))
 108                         goto nla_put_failure;
 109         }
 110
 111         genlmsg_end(msg, hdr);
 112         return 0;
 113
 114 nla_put_failure:
 115         genlmsg_cancel(msg, hdr);
 116         return -EMSGSIZE;
 117 }
 118
 119 static struct nfc_dev *__get_device_from_cb(struct netlink_callback *cb)
 120 {
 121         struct nlattr **attrbuf = genl_family_attrbuf(&nfc_genl_family);
 122         struct nfc_dev *dev;
 123         int rc;
 124         u32 idx;
 125
 126         rc = nlmsg_parse(cb->nlh, GENL_HDRLEN + nfc_genl_family.hdrsize,
 127                          attrbuf, nfc_genl_family.maxattr, nfc_genl_policy,
 128                          NULL);
 129         if (rc < 0)
 130                 return ERR_PTR(rc);
 131
 132         if (!attrbuf[NFC_ATTR_DEVICE_INDEX])
 133                 return ERR_PTR(-EINVAL);
 134
 135         idx = nla_get_u32(attrbuf[NFC_ATTR_DEVICE_INDEX]);
 136
 137         dev = nfc_get_device(idx);
 138         if (!dev)
 139                 return ERR_PTR(-ENODEV);
 140
 141         return dev;
 142 }
 143
 144 static int nfc_genl_dump_targets(struct sk_buff *skb,
 145                                  struct netlink_callback *cb)
 146 {
 147         int i = cb->args[0];
 148         struct nfc_dev *dev = (struct nfc_dev *) cb->args[1];
 149         int rc;
 150
 151         if (!dev) {
 152                 dev = __get_device_from_cb(cb);
 153                 if (IS_ERR(dev))
 154                         return PTR_ERR(dev);
 155
 156                 cb->args[1] = (long) dev;
 157         }
 158
 159         device_lock(&dev->dev);
 160
 161         cb->seq = dev->targets_generation;
 162
 163         while (i < dev->n_targets) {
 164                 rc = nfc_genl_send_target(skb, &dev->targets[i], cb,
 165                                           NLM_F_MULTI);
 166                 if (rc < 0)
 167                         break;
 168
 169                 i++;
 170         }
 171
 172         device_unlock(&dev->dev);
 173
 174         cb->args[0] = i;
 175
 176         return skb->len;
 177 }
 178
 179 static int nfc_genl_dump_targets_done(struct netlink_callback *cb)
 180 {
 181         struct nfc_dev *dev = (struct nfc_dev *) cb->args[1];
 182
 183         if (dev)
 184                 nfc_put_device(dev);
 185
 186         return 0;
 187 }
 188
 189 int nfc_genl_targets_found(struct nfc_dev *dev)
 190 {
 191         struct sk_buff *msg;
 192         void *hdr;
 193
 194         dev->genl_data.poll_req_portid = 0;
 195
 196         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
 197         if (!msg)
 198                 return -ENOMEM;
 199
 200         hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
 201                           NFC_EVENT_TARGETS_FOUND);
 202         if (!hdr)
 203                 goto free_msg;
 204
 205         if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
 206                 goto nla_put_failure;
 207
 208         genlmsg_end(msg, hdr);
 209
 210         return genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_ATOMIC);
 211
 212 nla_put_failure:
 213 free_msg:
 214         nlmsg_free(msg);
 215         return -EMSGSIZE;
 216 }
 217
 218 int nfc_genl_target_lost(struct nfc_dev *dev, u32 target_idx)
 219 {
 220         struct sk_buff *msg;
 221         void *hdr;
 222
 223         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
 224         if (!msg)
 225                 return -ENOMEM;
 226
 227         hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
 228                           NFC_EVENT_TARGET_LOST);
 229         if (!hdr)
 230                 goto free_msg;
 231
 232         if (nla_put_string(msg, NFC_ATTR_DEVICE_NAME, nfc_device_name(dev)) ||
 233             nla_put_u32(msg, NFC_ATTR_TARGET_INDEX, target_idx))
 234                 goto nla_put_failure;
 235
 236         genlmsg_end(msg, hdr);
 237
 238         genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL);
 239
 240         return 0;
 241
 242 nla_put_failure:
 243 free_msg:
 244         nlmsg_free(msg);
 245         return -EMSGSIZE;
 246 }
 247
 248 int nfc_genl_tm_activated(struct nfc_dev *dev, u32 protocol)
 249 {
 250         struct sk_buff *msg;
 251         void *hdr;
 252
 253         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
 254         if (!msg)
 255                 return -ENOMEM;
 256
 257         hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
 258                           NFC_EVENT_TM_ACTIVATED);
 259         if (!hdr)
 260                 goto free_msg;
 261
 262         if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
 263                 goto nla_put_failure;
 264         if (nla_put_u32(msg, NFC_ATTR_TM_PROTOCOLS, protocol))
 265                 goto nla_put_failure;
 266
 267         genlmsg_end(msg, hdr);
 268
 269         genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL);
 270
 271         return 0;
 272
 273 nla_put_failure:
 274 free_msg:
 275         nlmsg_free(msg);
 276         return -EMSGSIZE;
 277 }
 278
 279 int nfc_genl_tm_deactivated(struct nfc_dev *dev)
 280 {
 281         struct sk_buff *msg;
 282         void *hdr;
 283
 284         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
 285         if (!msg)
 286                 return -ENOMEM;
 287
 288         hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
 289                           NFC_EVENT_TM_DEACTIVATED);
 290         if (!hdr)
 291                 goto free_msg;
 292
 293         if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
 294                 goto nla_put_failure;
 295
 296         genlmsg_end(msg, hdr);
 297
 298         genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL);
 299
 300         return 0;
 301
 302 nla_put_failure:
 303 free_msg:
 304         nlmsg_free(msg);
 305         return -EMSGSIZE;
 306 }
 307
 308 static int nfc_genl_setup_device_added(struct nfc_dev *dev, struct sk_buff *msg)
 309 {
 310         if (nla_put_string(msg, NFC_ATTR_DEVICE_NAME, nfc_device_name(dev)) ||
 311             nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx) ||
 312             nla_put_u32(msg, NFC_ATTR_PROTOCOLS, dev->supported_protocols) ||
 313             nla_put_u8(msg, NFC_ATTR_DEVICE_POWERED, dev->dev_up) ||
 314             nla_put_u8(msg, NFC_ATTR_RF_MODE, dev->rf_mode))
 315                 return -1;
 316         return 0;
 317 }
 318
 319 int nfc_genl_device_added(struct nfc_dev *dev)
 320 {
 321         struct sk_buff *msg;
 322         void *hdr;
 323
 324         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
 325         if (!msg)
 326                 return -ENOMEM;
 327
 328         hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
 329                           NFC_EVENT_DEVICE_ADDED);
 330         if (!hdr)
 331                 goto free_msg;
 332
 333         if (nfc_genl_setup_device_added(dev, msg))
 334                 goto nla_put_failure;
 335
 336         genlmsg_end(msg, hdr);
 337
 338         genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL);
 339
 340         return 0;
 341
 342 nla_put_failure:
 343 free_msg:
 344         nlmsg_free(msg);
 345         return -EMSGSIZE;
 346 }
 347
 348 int nfc_genl_device_removed(struct nfc_dev *dev)
 349 {
 350         struct sk_buff *msg;
 351         void *hdr;
 352
 353         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
 354         if (!msg)
 355                 return -ENOMEM;
 356
 357         hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
 358                           NFC_EVENT_DEVICE_REMOVED);
 359         if (!hdr)
 360                 goto free_msg;
 361
 362         if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
 363                 goto nla_put_failure;
 364
 365         genlmsg_end(msg, hdr);
 366
 367         genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL);
 368
 369         return 0;
 370
 371 nla_put_failure:
 372 free_msg:
 373         nlmsg_free(msg);
 374         return -EMSGSIZE;
 375 }
 376
 377 int nfc_genl_llc_send_sdres(struct nfc_dev *dev, struct hlist_head *sdres_list)
 378 {
 379         struct sk_buff *msg;
 380         struct nlattr *sdp_attr, *uri_attr;
 381         struct nfc_llcp_sdp_tlv *sdres;
 382         struct hlist_node *n;
 383         void *hdr;
 384         int rc = -EMSGSIZE;
 385         int i;
 386
 387         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
 388         if (!msg)
 389                 return -ENOMEM;
 390
 391         hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
 392                           NFC_EVENT_LLC_SDRES);
 393         if (!hdr)
 394                 goto free_msg;
 395
 396         if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
 397                 goto nla_put_failure;
 398
 399         sdp_attr = nla_nest_start(msg, NFC_ATTR_LLC_SDP);
 400         if (sdp_attr == NULL) {
 401                 rc = -ENOMEM;
 402                 goto nla_put_failure;
 403         }
 404
 405         i = 1;
 406         hlist_for_each_entry_safe(sdres, n, sdres_list, node) {
 407                 pr_debug("uri: %s, sap: %d\n", sdres->uri, sdres->sap);
 408
 409                 uri_attr = nla_nest_start(msg, i++);
 410                 if (uri_attr == NULL) {
 411                         rc = -ENOMEM;
 412                         goto nla_put_failure;
 413                 }
 414
 415                 if (nla_put_u8(msg, NFC_SDP_ATTR_SAP, sdres->sap))
 416                         goto nla_put_failure;
 417
 418                 if (nla_put_string(msg, NFC_SDP_ATTR_URI, sdres->uri))
 419                         goto nla_put_failure;
 420
 421                 nla_nest_end(msg, uri_attr);
 422
 423                 hlist_del(&sdres->node);
 424
 425                 nfc_llcp_free_sdp_tlv(sdres);
 426         }
 427
 428         nla_nest_end(msg, sdp_attr);
 429
 430         genlmsg_end(msg, hdr);
 431
 432         return genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_ATOMIC);
 433
 434 nla_put_failure:
 435 free_msg:
 436         nlmsg_free(msg);
 437
 438         nfc_llcp_free_sdp_tlv_list(sdres_list);
 439
 440         return rc;
 441 }
 442
 443 int nfc_genl_se_added(struct nfc_dev *dev, u32 se_idx, u16 type)
 444 {
 445         struct sk_buff *msg;
 446         void *hdr;
 447
 448         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
 449         if (!msg)
 450                 return -ENOMEM;
 451
 452         hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
 453                           NFC_EVENT_SE_ADDED);
 454         if (!hdr)
 455                 goto free_msg;
 456
 457         if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx) ||
 458             nla_put_u32(msg, NFC_ATTR_SE_INDEX, se_idx) ||
 459             nla_put_u8(msg, NFC_ATTR_SE_TYPE, type))
 460                 goto nla_put_failure;
 461
 462         genlmsg_end(msg, hdr);
 463
 464         genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL);
 465
 466         return 0;
 467
 468 nla_put_failure:
 469 free_msg:
 470         nlmsg_free(msg);
 471         return -EMSGSIZE;
 472 }
 473
 474 int nfc_genl_se_removed(struct nfc_dev *dev, u32 se_idx)
 475 {
 476         struct sk_buff *msg;
 477         void *hdr;
 478
 479         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
 480         if (!msg)
 481                 return -ENOMEM;
 482
 483         hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
 484                           NFC_EVENT_SE_REMOVED);
 485         if (!hdr)
 486                 goto free_msg;
 487
 488         if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx) ||
 489             nla_put_u32(msg, NFC_ATTR_SE_INDEX, se_idx))
 490                 goto nla_put_failure;
 491
 492         genlmsg_end(msg, hdr);
 493
 494         genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL);
 495
 496         return 0;
 497
 498 nla_put_failure:
 499 free_msg:
 500         nlmsg_free(msg);
 501         return -EMSGSIZE;
 502 }
 503
 504 int nfc_genl_se_transaction(struct nfc_dev *dev, u8 se_idx,
 505                             struct nfc_evt_transaction *evt_transaction)
 506 {
 507         struct nfc_se *se;
 508         struct sk_buff *msg;
 509         void *hdr;
 510
 511         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
 512         if (!msg)
 513                 return -ENOMEM;
 514
 515         hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
 516                           NFC_EVENT_SE_TRANSACTION);
 517         if (!hdr)
 518                 goto free_msg;
 519
 520         se = nfc_find_se(dev, se_idx);
 521         if (!se)
 522                 goto free_msg;
 523
 524         if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx) ||
 525             nla_put_u32(msg, NFC_ATTR_SE_INDEX, se_idx) ||
 526             nla_put_u8(msg, NFC_ATTR_SE_TYPE, se->type) ||
 527             nla_put(msg, NFC_ATTR_SE_AID, evt_transaction->aid_len,
 528                     evt_transaction->aid) ||
 529             nla_put(msg, NFC_ATTR_SE_PARAMS, evt_transaction->params_len,
 530                     evt_transaction->params))
 531                 goto nla_put_failure;
 532
 533         /* evt_transaction is no more used */
 534         devm_kfree(&dev->dev, evt_transaction);
 535
 536         genlmsg_end(msg, hdr);
 537
 538         genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL);
 539
 540         return 0;
 541
 542 nla_put_failure:
 543 free_msg:
 544         /* evt_transaction is no more used */
 545         devm_kfree(&dev->dev, evt_transaction);
 546         nlmsg_free(msg);
 547         return -EMSGSIZE;
 548 }
 549
 550 int nfc_genl_se_connectivity(struct nfc_dev *dev, u8 se_idx)
 551 {
 552         struct nfc_se *se;
 553         struct sk_buff *msg;
 554         void *hdr;
 555
 556         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
 557         if (!msg)
 558                 return -ENOMEM;
 559
 560         hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
 561                           NFC_EVENT_SE_CONNECTIVITY);
 562         if (!hdr)
 563                 goto free_msg;
 564
 565         se = nfc_find_se(dev, se_idx);
 566         if (!se)
 567                 goto free_msg;
 568
 569         if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx) ||
 570             nla_put_u32(msg, NFC_ATTR_SE_INDEX, se_idx) ||
 571             nla_put_u8(msg, NFC_ATTR_SE_TYPE, se->type))
 572                 goto nla_put_failure;
 573
 574         genlmsg_end(msg, hdr);
 575
 576         genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL);
 577
 578         return 0;
 579
 580 nla_put_failure:
 581 free_msg:
 582         nlmsg_free(msg);
 583         return -EMSGSIZE;
 584 }
 585
 586 static int nfc_genl_send_device(struct sk_buff *msg, struct nfc_dev *dev,
 587                                 u32 portid, u32 seq,
 588                                 struct netlink_callback *cb,
 589                                 int flags)
 590 {
 591         void *hdr;
 592
 593         hdr = genlmsg_put(msg, portid, seq, &nfc_genl_family, flags,
 594                           NFC_CMD_GET_DEVICE);
 595         if (!hdr)
 596                 return -EMSGSIZE;
 597
 598         if (cb)
 599                 genl_dump_check_consistent(cb, hdr);
 600
 601         if (nfc_genl_setup_device_added(dev, msg))
 602                 goto nla_put_failure;
 603
 604         genlmsg_end(msg, hdr);
 605         return 0;
 606
 607 nla_put_failure:
 608         genlmsg_cancel(msg, hdr);
 609         return -EMSGSIZE;
 610 }
 611
 612 static int nfc_genl_dump_devices(struct sk_buff *skb,
 613                                  struct netlink_callback *cb)
 614 {
 615         struct class_dev_iter *iter = (struct class_dev_iter *) cb->args[0];
 616         struct nfc_dev *dev = (struct nfc_dev *) cb->args[1];
 617         bool first_call = false;
 618
 619         if (!iter) {
 620                 first_call = true;
 621                 iter = kmalloc(sizeof(struct class_dev_iter), GFP_KERNEL);
 622                 if (!iter)
 623                         return -ENOMEM;
 624                 cb->args[0] = (long) iter;
 625         }
 626
 627         mutex_lock(&nfc_devlist_mutex);
 628
 629         cb->seq = nfc_devlist_generation;
 630
 631         if (first_call) {
 632                 nfc_device_iter_init(iter);
 633                 dev = nfc_device_iter_next(iter);
 634         }
 635
 636         while (dev) {
 637                 int rc;
 638
 639                 rc = nfc_genl_send_device(skb, dev, NETLINK_CB(cb->skb).portid,
 640                                           cb->nlh->nlmsg_seq, cb, NLM_F_MULTI);
 641                 if (rc < 0)
 642                         break;
 643
 644                 dev = nfc_device_iter_next(iter);
 645         }
 646
 647         mutex_unlock(&nfc_devlist_mutex);
 648
 649         cb->args[1] = (long) dev;
 650
 651         return skb->len;
 652 }
 653
 654 static int nfc_genl_dump_devices_done(struct netlink_callback *cb)
 655 {
 656         struct class_dev_iter *iter = (struct class_dev_iter *) cb->args[0];
 657
 658         nfc_device_iter_exit(iter);
 659         kfree(iter);
 660
 661         return 0;
 662 }
 663
 664 int nfc_genl_dep_link_up_event(struct nfc_dev *dev, u32 target_idx,
 665                                u8 comm_mode, u8 rf_mode)
 666 {
 667         struct sk_buff *msg;
 668         void *hdr;
 669
 670         pr_debug("DEP link is up\n");
 671
 672         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
 673         if (!msg)
 674                 return -ENOMEM;
 675
 676         hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0, NFC_CMD_DEP_LINK_UP);
 677         if (!hdr)
 678                 goto free_msg;
 679
 680         if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
 681                 goto nla_put_failure;
 682         if (rf_mode == NFC_RF_INITIATOR &&
 683             nla_put_u32(msg, NFC_ATTR_TARGET_INDEX, target_idx))
 684                 goto nla_put_failure;
 685         if (nla_put_u8(msg, NFC_ATTR_COMM_MODE, comm_mode) ||
 686             nla_put_u8(msg, NFC_ATTR_RF_MODE, rf_mode))
 687                 goto nla_put_failure;
 688
 689         genlmsg_end(msg, hdr);
 690
 691         dev->dep_link_up = true;
 692
 693         genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_ATOMIC);
 694
 695         return 0;
 696
 697 nla_put_failure:
 698 free_msg:
 699         nlmsg_free(msg);
 700         return -EMSGSIZE;
 701 }
 702
 703 int nfc_genl_dep_link_down_event(struct nfc_dev *dev)
 704 {
 705         struct sk_buff *msg;
 706         void *hdr;
 707
 708         pr_debug("DEP link is down\n");
 709
 710         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
 711         if (!msg)
 712                 return -ENOMEM;
 713
 714         hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
 715                           NFC_CMD_DEP_LINK_DOWN);
 716         if (!hdr)
 717                 goto free_msg;
 718
 719         if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
 720                 goto nla_put_failure;
 721
 722         genlmsg_end(msg, hdr);
 723
 724         genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_ATOMIC);
 725
 726         return 0;
 727
 728 nla_put_failure:
 729 free_msg:
 730         nlmsg_free(msg);
 731         return -EMSGSIZE;
 732 }
 733
 734 static int nfc_genl_get_device(struct sk_buff *skb, struct genl_info *info)
 735 {
 736         struct sk_buff *msg;
 737         struct nfc_dev *dev;
 738         u32 idx;
 739         int rc = -ENOBUFS;
 740
 741         if (!info->attrs[NFC_ATTR_DEVICE_INDEX])
 742                 return -EINVAL;
 743
 744         idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
 745
 746         dev = nfc_get_device(idx);
 747         if (!dev)
 748                 return -ENODEV;
 749
 750         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
 751         if (!msg) {
 752                 rc = -ENOMEM;
 753                 goto out_putdev;
 754         }
 755
 756         rc = nfc_genl_send_device(msg, dev, info->snd_portid, info->snd_seq,
 757                                   NULL, 0);
 758         if (rc < 0)
 759                 goto out_free;
 760
 761         nfc_put_device(dev);
 762
 763         return genlmsg_reply(msg, info);
 764
 765 out_free:
 766         nlmsg_free(msg);
 767 out_putdev:
 768         nfc_put_device(dev);
 769         return rc;
 770 }
 771
 772 static int nfc_genl_dev_up(struct sk_buff *skb, struct genl_info *info)
 773 {
 774         struct nfc_dev *dev;
 775         int rc;
 776         u32 idx;
 777
 778         if (!info->attrs[NFC_ATTR_DEVICE_INDEX])
 779                 return -EINVAL;
 780
 781         idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
 782
 783         dev = nfc_get_device(idx);
 784         if (!dev)
 785                 return -ENODEV;
 786
 787         rc = nfc_dev_up(dev);
 788
 789         nfc_put_device(dev);
 790         return rc;
 791 }
 792
 793 static int nfc_genl_dev_down(struct sk_buff *skb, struct genl_info *info)
 794 {
 795         struct nfc_dev *dev;
 796         int rc;
 797         u32 idx;
 798
 799         if (!info->attrs[NFC_ATTR_DEVICE_INDEX])
 800                 return -EINVAL;
 801
 802         idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
 803
 804         dev = nfc_get_device(idx);
 805         if (!dev)
 806                 return -ENODEV;
 807
 808         rc = nfc_dev_down(dev);
 809
 810         nfc_put_device(dev);
 811         return rc;
 812 }
 813
 814 static int nfc_genl_start_poll(struct sk_buff *skb, struct genl_info *info)
 815 {
 816         struct nfc_dev *dev;
 817         int rc;
 818         u32 idx;
 819         u32 im_protocols = 0, tm_protocols = 0;
 820
 821         pr_debug("Poll start\n");
 822
 823         if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
 824             ((!info->attrs[NFC_ATTR_IM_PROTOCOLS] &&
 825               !info->attrs[NFC_ATTR_PROTOCOLS]) &&
 826               !info->attrs[NFC_ATTR_TM_PROTOCOLS]))
 827                 return -EINVAL;
 828
 829         idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
 830
 831         if (info->attrs[NFC_ATTR_TM_PROTOCOLS])
 832                 tm_protocols = nla_get_u32(info->attrs[NFC_ATTR_TM_PROTOCOLS]);
 833
 834         if (info->attrs[NFC_ATTR_IM_PROTOCOLS])
 835                 im_protocols = nla_get_u32(info->attrs[NFC_ATTR_IM_PROTOCOLS]);
 836         else if (info->attrs[NFC_ATTR_PROTOCOLS])
 837                 im_protocols = nla_get_u32(info->attrs[NFC_ATTR_PROTOCOLS]);
 838
 839         dev = nfc_get_device(idx);
 840         if (!dev)
 841                 return -ENODEV;
 842
 843         mutex_lock(&dev->genl_data.genl_data_mutex);
 844
 845         rc = nfc_start_poll(dev, im_protocols, tm_protocols);
 846         if (!rc)
 847                 dev->genl_data.poll_req_portid = info->snd_portid;
 848
 849         mutex_unlock(&dev->genl_data.genl_data_mutex);
 850
 851         nfc_put_device(dev);
 852         return rc;
 853 }
 854
 855 static int nfc_genl_stop_poll(struct sk_buff *skb, struct genl_info *info)
 856 {
 857         struct nfc_dev *dev;
 858         int rc;
 859         u32 idx;
 860
 861         if (!info->attrs[NFC_ATTR_DEVICE_INDEX])
 862                 return -EINVAL;
 863
 864         idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
 865
 866         dev = nfc_get_device(idx);
 867         if (!dev)
 868                 return -ENODEV;
 869
 870         device_lock(&dev->dev);
 871
 872         if (!dev->polling) {
 873                 device_unlock(&dev->dev);
 874                 nfc_put_device(dev);
 875                 return -EINVAL;
 876         }
 877
 878         device_unlock(&dev->dev);
 879
 880         mutex_lock(&dev->genl_data.genl_data_mutex);
 881
 882         if (dev->genl_data.poll_req_portid != info->snd_portid) {
 883                 rc = -EBUSY;
 884                 goto out;
 885         }
 886
 887         rc = nfc_stop_poll(dev);
 888         dev->genl_data.poll_req_portid = 0;
 889
 890 out:
 891         mutex_unlock(&dev->genl_data.genl_data_mutex);
 892         nfc_put_device(dev);
 893         return rc;
 894 }
 895
 896 static int nfc_genl_activate_target(struct sk_buff *skb, struct genl_info *info)
 897 {
 898         struct nfc_dev *dev;
 899         u32 device_idx, target_idx, protocol;
 900         int rc;
 901
 902         if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
 903             !info->attrs[NFC_ATTR_TARGET_INDEX] ||
 904             !info->attrs[NFC_ATTR_PROTOCOLS])
 905                 return -EINVAL;
 906
 907         device_idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
 908
 909         dev = nfc_get_device(device_idx);
 910         if (!dev)
 911                 return -ENODEV;
 912
 913         target_idx = nla_get_u32(info->attrs[NFC_ATTR_TARGET_INDEX]);
 914         protocol = nla_get_u32(info->attrs[NFC_ATTR_PROTOCOLS]);
 915
 916         nfc_deactivate_target(dev, target_idx, NFC_TARGET_MODE_SLEEP);
 917         rc = nfc_activate_target(dev, target_idx, protocol);
 918
 919         nfc_put_device(dev);
 920         return rc;
 921 }
 922
 923 static int nfc_genl_deactivate_target(struct sk_buff *skb,
 924                                       struct genl_info *info)
 925 {
 926         struct nfc_dev *dev;
 927         u32 device_idx, target_idx;
 928         int rc;
 929
 930         if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
 931             !info->attrs[NFC_ATTR_TARGET_INDEX])
 932                 return -EINVAL;
 933
 934         device_idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
 935
 936         dev = nfc_get_device(device_idx);
 937         if (!dev)
 938                 return -ENODEV;
 939
 940         target_idx = nla_get_u32(info->attrs[NFC_ATTR_TARGET_INDEX]);
 941
 942         rc = nfc_deactivate_target(dev, target_idx, NFC_TARGET_MODE_SLEEP);
 943
 944         nfc_put_device(dev);
 945         return rc;
 946 }
 947
 948 static int nfc_genl_dep_link_up(struct sk_buff *skb, struct genl_info *info)
 949 {
 950         struct nfc_dev *dev;
 951         int rc, tgt_idx;
 952         u32 idx;
 953         u8 comm;
 954
 955         pr_debug("DEP link up\n");
 956
 957         if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
 958             !info->attrs[NFC_ATTR_COMM_MODE])
 959                 return -EINVAL;
 960
 961         idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
 962         if (!info->attrs[NFC_ATTR_TARGET_INDEX])
 963                 tgt_idx = NFC_TARGET_IDX_ANY;
 964         else
 965                 tgt_idx = nla_get_u32(info->attrs[NFC_ATTR_TARGET_INDEX]);
 966
 967         comm = nla_get_u8(info->attrs[NFC_ATTR_COMM_MODE]);
 968
 969         if (comm != NFC_COMM_ACTIVE && comm != NFC_COMM_PASSIVE)
 970                 return -EINVAL;
 971
 972         dev = nfc_get_device(idx);
 973         if (!dev)
 974                 return -ENODEV;
 975
 976         rc = nfc_dep_link_up(dev, tgt_idx, comm);
 977
 978         nfc_put_device(dev);
 979
 980         return rc;
 981 }
 982
 983 static int nfc_genl_dep_link_down(struct sk_buff *skb, struct genl_info *info)
 984 {
 985         struct nfc_dev *dev;
 986         int rc;
 987         u32 idx;
 988
 989         if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
 990             !info->attrs[NFC_ATTR_TARGET_INDEX])
 991                 return -EINVAL;
 992
 993         idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
 994
 995         dev = nfc_get_device(idx);
 996         if (!dev)
 997                 return -ENODEV;
 998
 999         rc = nfc_dep_link_down(dev);
1000
1001         nfc_put_device(dev);
1002         return rc;
1003 }
1004
1005 static int nfc_genl_send_params(struct sk_buff *msg,
1006                                 struct nfc_llcp_local *local,
1007                                 u32 portid, u32 seq)
1008 {
1009         void *hdr;
1010
1011         hdr = genlmsg_put(msg, portid, seq, &nfc_genl_family, 0,
1012                           NFC_CMD_LLC_GET_PARAMS);
1013         if (!hdr)
1014                 return -EMSGSIZE;
1015
1016         if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, local->dev->idx) ||
1017             nla_put_u8(msg, NFC_ATTR_LLC_PARAM_LTO, local->lto) ||
1018             nla_put_u8(msg, NFC_ATTR_LLC_PARAM_RW, local->rw) ||
1019             nla_put_u16(msg, NFC_ATTR_LLC_PARAM_MIUX, be16_to_cpu(local->miux)))
1020                 goto nla_put_failure;
1021
1022         genlmsg_end(msg, hdr);
1023         return 0;
1024
1025 nla_put_failure:
1026         genlmsg_cancel(msg, hdr);
1027         return -EMSGSIZE;
1028 }
1029
1030 static int nfc_genl_llc_get_params(struct sk_buff *skb, struct genl_info *info)
1031 {
1032         struct nfc_dev *dev;
1033         struct nfc_llcp_local *local;
1034         int rc = 0;
1035         struct sk_buff *msg = NULL;
1036         u32 idx;
1037
1038         if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
1039             !info->attrs[NFC_ATTR_FIRMWARE_NAME])
1040                 return -EINVAL;
1041
1042         idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
1043
1044         dev = nfc_get_device(idx);
1045         if (!dev)
1046                 return -ENODEV;
1047
1048         device_lock(&dev->dev);
1049
1050         local = nfc_llcp_find_local(dev);
1051         if (!local) {
1052                 rc = -ENODEV;
1053                 goto exit;
1054         }
1055
1056         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
1057         if (!msg) {
1058                 rc = -ENOMEM;
1059                 goto exit;
1060         }
1061
1062         rc = nfc_genl_send_params(msg, local, info->snd_portid, info->snd_seq);
1063
1064 exit:
1065         device_unlock(&dev->dev);
1066
1067         nfc_put_device(dev);
1068
1069         if (rc < 0) {
1070                 if (msg)
1071                         nlmsg_free(msg);
1072
1073                 return rc;
1074         }
1075
1076         return genlmsg_reply(msg, info);
1077 }
1078
1079 static int nfc_genl_llc_set_params(struct sk_buff *skb, struct genl_info *info)
1080 {
1081         struct nfc_dev *dev;
1082         struct nfc_llcp_local *local;
1083         u8 rw = 0;
1084         u16 miux = 0;
1085         u32 idx;
1086         int rc = 0;
1087
1088         if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
1089             (!info->attrs[NFC_ATTR_LLC_PARAM_LTO] &&
1090              !info->attrs[NFC_ATTR_LLC_PARAM_RW] &&
1091              !info->attrs[NFC_ATTR_LLC_PARAM_MIUX]))
1092                 return -EINVAL;
1093
1094         if (info->attrs[NFC_ATTR_LLC_PARAM_RW]) {
1095                 rw = nla_get_u8(info->attrs[NFC_ATTR_LLC_PARAM_RW]);
1096
1097                 if (rw > LLCP_MAX_RW)
1098                         return -EINVAL;
1099         }
1100
1101         if (info->attrs[NFC_ATTR_LLC_PARAM_MIUX]) {
1102                 miux = nla_get_u16(info->attrs[NFC_ATTR_LLC_PARAM_MIUX]);
1103
1104                 if (miux > LLCP_MAX_MIUX)
1105                         return -EINVAL;
1106         }
1107
1108         idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
1109
1110         dev = nfc_get_device(idx);
1111         if (!dev)
1112                 return -ENODEV;
1113
1114         device_lock(&dev->dev);
1115
1116         local = nfc_llcp_find_local(dev);
1117         if (!local) {
1118                 rc = -ENODEV;
1119                 goto exit;
1120         }
1121
1122         if (info->attrs[NFC_ATTR_LLC_PARAM_LTO]) {
1123                 if (dev->dep_link_up) {
1124                         rc = -EINPROGRESS;
1125                         goto exit;
1126                 }
1127
1128                 local->lto = nla_get_u8(info->attrs[NFC_ATTR_LLC_PARAM_LTO]);
1129         }
1130
1131         if (info->attrs[NFC_ATTR_LLC_PARAM_RW])
1132                 local->rw = rw;
1133
1134         if (info->attrs[NFC_ATTR_LLC_PARAM_MIUX])
1135                 local->miux = cpu_to_be16(miux);
1136
1137 exit:
1138         device_unlock(&dev->dev);
1139
1140         nfc_put_device(dev);
1141
1142         return rc;
1143 }
1144
1145 static int nfc_genl_llc_sdreq(struct sk_buff *skb, struct genl_info *info)
1146 {
1147         struct nfc_dev *dev;
1148         struct nfc_llcp_local *local;
1149         struct nlattr *attr, *sdp_attrs[NFC_SDP_ATTR_MAX+1];
1150         u32 idx;
1151         u8 tid;
1152         char *uri;
1153         int rc = 0, rem;
1154         size_t uri_len, tlvs_len;
1155         struct hlist_head sdreq_list;
1156         struct nfc_llcp_sdp_tlv *sdreq;
1157
1158         if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
1159             !info->attrs[NFC_ATTR_LLC_SDP])
1160                 return -EINVAL;
1161
1162         idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
1163
1164         dev = nfc_get_device(idx);
1165         if (!dev)
1166                 return -ENODEV;
1167
1168         device_lock(&dev->dev);
1169
1170         if (dev->dep_link_up == false) {
1171                 rc = -ENOLINK;
1172                 goto exit;
1173         }
1174
1175         local = nfc_llcp_find_local(dev);
1176         if (!local) {
1177                 rc = -ENODEV;
1178                 goto exit;
1179         }
1180
1181         INIT_HLIST_HEAD(&sdreq_list);
1182
1183         tlvs_len = 0;
1184
1185         nla_for_each_nested(attr, info->attrs[NFC_ATTR_LLC_SDP], rem) {
1186                 rc = nla_parse_nested(sdp_attrs, NFC_SDP_ATTR_MAX, attr,
1187                                       nfc_sdp_genl_policy, info->extack);
1188
1189                 if (rc != 0) {
1190                         rc = -EINVAL;
1191                         goto exit;
1192                 }
1193
1194                 if (!sdp_attrs[NFC_SDP_ATTR_URI])
1195                         continue;
1196
1197                 uri_len = nla_len(sdp_attrs[NFC_SDP_ATTR_URI]);
1198                 if (uri_len == 0)
1199                         continue;
1200
1201                 uri = nla_data(sdp_attrs[NFC_SDP_ATTR_URI]);
1202                 if (uri == NULL || *uri == 0)
1203                         continue;
1204
1205                 tid = local->sdreq_next_tid++;
1206
1207                 sdreq = nfc_llcp_build_sdreq_tlv(tid, uri, uri_len);
1208                 if (sdreq == NULL) {
1209                         rc = -ENOMEM;
1210                         goto exit;
1211                 }
1212
1213                 tlvs_len += sdreq->tlv_len;
1214
1215                 hlist_add_head(&sdreq->node, &sdreq_list);
1216         }
1217
1218         if (hlist_empty(&sdreq_list)) {
1219                 rc = -EINVAL;
1220                 goto exit;
1221         }
1222
1223         rc = nfc_llcp_send_snl_sdreq(local, &sdreq_list, tlvs_len);
1224 exit:
1225         device_unlock(&dev->dev);
1226
1227         nfc_put_device(dev);
1228
1229         return rc;
1230 }
1231
1232 static int nfc_genl_fw_download(struct sk_buff *skb, struct genl_info *info)
1233 {
1234         struct nfc_dev *dev;
1235         int rc;
1236         u32 idx;
1237         char firmware_name[NFC_FIRMWARE_NAME_MAXSIZE + 1];
1238
1239         if (!info->attrs[NFC_ATTR_DEVICE_INDEX] || !info->attrs[NFC_ATTR_FIRMWARE_NAME])
1240                 return -EINVAL;
1241
1242         idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
1243
1244         dev = nfc_get_device(idx);
1245         if (!dev)
1246                 return -ENODEV;
1247
1248         nla_strlcpy(firmware_name, info->attrs[NFC_ATTR_FIRMWARE_NAME],
1249                     sizeof(firmware_name));
1250
1251         rc = nfc_fw_download(dev, firmware_name);
1252
1253         nfc_put_device(dev);
1254         return rc;
1255 }
1256
1257 int nfc_genl_fw_download_done(struct nfc_dev *dev, const char *firmware_name,
1258                               u32 result)
1259 {
1260         struct sk_buff *msg;
1261         void *hdr;
1262
1263         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
1264         if (!msg)
1265                 return -ENOMEM;
1266
1267         hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
1268                           NFC_CMD_FW_DOWNLOAD);
1269         if (!hdr)
1270                 goto free_msg;
1271
1272         if (nla_put_string(msg, NFC_ATTR_FIRMWARE_NAME, firmware_name) ||
1273             nla_put_u32(msg, NFC_ATTR_FIRMWARE_DOWNLOAD_STATUS, result) ||
1274             nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
1275                 goto nla_put_failure;
1276
1277         genlmsg_end(msg, hdr);
1278
1279         genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL);
1280
1281         return 0;
1282
1283 nla_put_failure:
1284 free_msg:
1285         nlmsg_free(msg);
1286         return -EMSGSIZE;
1287 }
1288
1289 static int nfc_genl_enable_se(struct sk_buff *skb, struct genl_info *info)
1290 {
1291         struct nfc_dev *dev;
1292         int rc;
1293         u32 idx, se_idx;
1294
1295         if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
1296             !info->attrs[NFC_ATTR_SE_INDEX])
1297                 return -EINVAL;
1298
1299         idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
1300         se_idx = nla_get_u32(info->attrs[NFC_ATTR_SE_INDEX]);
1301
1302         dev = nfc_get_device(idx);
1303         if (!dev)
1304                 return -ENODEV;
1305
1306         rc = nfc_enable_se(dev, se_idx);
1307
1308         nfc_put_device(dev);
1309         return rc;
1310 }
1311
1312 static int nfc_genl_disable_se(struct sk_buff *skb, struct genl_info *info)
1313 {
1314         struct nfc_dev *dev;
1315         int rc;
1316         u32 idx, se_idx;
1317
1318         if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
1319             !info->attrs[NFC_ATTR_SE_INDEX])
1320                 return -EINVAL;
1321
1322         idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
1323         se_idx = nla_get_u32(info->attrs[NFC_ATTR_SE_INDEX]);
1324
1325         dev = nfc_get_device(idx);
1326         if (!dev)
1327                 return -ENODEV;
1328
1329         rc = nfc_disable_se(dev, se_idx);
1330
1331         nfc_put_device(dev);
1332         return rc;
1333 }
1334
1335 static int nfc_genl_send_se(struct sk_buff *msg, struct nfc_dev *dev,
1336                                 u32 portid, u32 seq,
1337                                 struct netlink_callback *cb,
1338                                 int flags)
1339 {
1340         void *hdr;
1341         struct nfc_se *se, *n;
1342
1343         list_for_each_entry_safe(se, n, &dev->secure_elements, list) {
1344                 hdr = genlmsg_put(msg, portid, seq, &nfc_genl_family, flags,
1345                                   NFC_CMD_GET_SE);
1346                 if (!hdr)
1347                         goto nla_put_failure;
1348
1349                 if (cb)
1350                         genl_dump_check_consistent(cb, hdr);
1351
1352                 if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx) ||
1353                     nla_put_u32(msg, NFC_ATTR_SE_INDEX, se->idx) ||
1354                     nla_put_u8(msg, NFC_ATTR_SE_TYPE, se->type))
1355                         goto nla_put_failure;
1356
1357                 genlmsg_end(msg, hdr);
1358         }
1359
1360         return 0;
1361
1362 nla_put_failure:
1363         genlmsg_cancel(msg, hdr);
1364         return -EMSGSIZE;
1365 }
1366
1367 static int nfc_genl_dump_ses(struct sk_buff *skb,
1368                                  struct netlink_callback *cb)
1369 {
1370         struct class_dev_iter *iter = (struct class_dev_iter *) cb->args[0];
1371         struct nfc_dev *dev = (struct nfc_dev *) cb->args[1];
1372         bool first_call = false;
1373
1374         if (!iter) {
1375                 first_call = true;
1376                 iter = kmalloc(sizeof(struct class_dev_iter), GFP_KERNEL);
1377                 if (!iter)
1378                         return -ENOMEM;
1379                 cb->args[0] = (long) iter;
1380         }
1381
1382         mutex_lock(&nfc_devlist_mutex);
1383
1384         cb->seq = nfc_devlist_generation;
1385
1386         if (first_call) {
1387                 nfc_device_iter_init(iter);
1388                 dev = nfc_device_iter_next(iter);
1389         }
1390
1391         while (dev) {
1392                 int rc;
1393
1394                 rc = nfc_genl_send_se(skb, dev, NETLINK_CB(cb->skb).portid,
1395                                           cb->nlh->nlmsg_seq, cb, NLM_F_MULTI);
1396                 if (rc < 0)
1397                         break;
1398
1399                 dev = nfc_device_iter_next(iter);
1400         }
1401
1402         mutex_unlock(&nfc_devlist_mutex);
1403
1404         cb->args[1] = (long) dev;
1405
1406         return skb->len;
1407 }
1408
1409 static int nfc_genl_dump_ses_done(struct netlink_callback *cb)
1410 {
1411         struct class_dev_iter *iter = (struct class_dev_iter *) cb->args[0];
1412
1413         nfc_device_iter_exit(iter);
1414         kfree(iter);
1415
1416         return 0;
1417 }
1418
1419 static int nfc_se_io(struct nfc_dev *dev, u32 se_idx,
1420                      u8 *apdu, size_t apdu_length,
1421                      se_io_cb_t cb, void *cb_context)
1422 {
1423         struct nfc_se *se;
1424         int rc;
1425
1426         pr_debug("%s se index %d\n", dev_name(&dev->dev), se_idx);
1427
1428         device_lock(&dev->dev);
1429
1430         if (!device_is_registered(&dev->dev)) {
1431                 rc = -ENODEV;
1432                 goto error;
1433         }
1434
1435         if (!dev->dev_up) {
1436                 rc = -ENODEV;
1437                 goto error;
1438         }
1439
1440         if (!dev->ops->se_io) {
1441                 rc = -EOPNOTSUPP;
1442                 goto error;
1443         }
1444
1445         se = nfc_find_se(dev, se_idx);
1446         if (!se) {
1447                 rc = -EINVAL;
1448                 goto error;
1449         }
1450
1451         if (se->state != NFC_SE_ENABLED) {
1452                 rc = -ENODEV;
1453                 goto error;
1454         }
1455
1456         rc = dev->ops->se_io(dev, se_idx, apdu,
1457                         apdu_length, cb, cb_context);
1458
1459 error:
1460         device_unlock(&dev->dev);
1461         return rc;
1462 }
1463
1464 struct se_io_ctx {
1465         u32 dev_idx;
1466         u32 se_idx;
1467 };
1468
1469 static void se_io_cb(void *context, u8 *apdu, size_t apdu_len, int err)
1470 {
1471         struct se_io_ctx *ctx = context;
1472         struct sk_buff *msg;
1473         void *hdr;
1474
1475         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
1476         if (!msg) {
1477                 kfree(ctx);
1478                 return;
1479         }
1480
1481         hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
1482                           NFC_CMD_SE_IO);
1483         if (!hdr)
1484                 goto free_msg;
1485
1486         if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, ctx->dev_idx) ||
1487             nla_put_u32(msg, NFC_ATTR_SE_INDEX, ctx->se_idx) ||
1488             nla_put(msg, NFC_ATTR_SE_APDU, apdu_len, apdu))
1489                 goto nla_put_failure;
1490
1491         genlmsg_end(msg, hdr);
1492
1493         genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL);
1494
1495         kfree(ctx);
1496
1497         return;
1498
1499 nla_put_failure:
1500 free_msg:
1501         nlmsg_free(msg);
1502         kfree(ctx);
1503
1504         return;
1505 }
1506
1507 static int nfc_genl_se_io(struct sk_buff *skb, struct genl_info *info)
1508 {
1509         struct nfc_dev *dev;
1510         struct se_io_ctx *ctx;
1511         u32 dev_idx, se_idx;
1512         u8 *apdu;
1513         size_t apdu_len;
1514
1515         if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
1516             !info->attrs[NFC_ATTR_SE_INDEX] ||
1517             !info->attrs[NFC_ATTR_SE_APDU])
1518                 return -EINVAL;
1519
1520         dev_idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
1521         se_idx = nla_get_u32(info->attrs[NFC_ATTR_SE_INDEX]);
1522
1523         dev = nfc_get_device(dev_idx);
1524         if (!dev)
1525                 return -ENODEV;
1526
1527         if (!dev->ops || !dev->ops->se_io)
1528                 return -ENOTSUPP;
1529
1530         apdu_len = nla_len(info->attrs[NFC_ATTR_SE_APDU]);
1531         if (apdu_len == 0)
1532                 return -EINVAL;
1533
1534         apdu = nla_data(info->attrs[NFC_ATTR_SE_APDU]);
1535         if (!apdu)
1536                 return -EINVAL;
1537
1538         ctx = kzalloc(sizeof(struct se_io_ctx), GFP_KERNEL);
1539         if (!ctx)
1540                 return -ENOMEM;
1541
1542         ctx->dev_idx = dev_idx;
1543         ctx->se_idx = se_idx;
1544
1545         return nfc_se_io(dev, se_idx, apdu, apdu_len, se_io_cb, ctx);
1546 }
1547
1548 static int nfc_genl_vendor_cmd(struct sk_buff *skb,
1549                                struct genl_info *info)
1550 {
1551         struct nfc_dev *dev;
1552         struct nfc_vendor_cmd *cmd;
1553         u32 dev_idx, vid, subcmd;
1554         u8 *data;
1555         size_t data_len;
1556         int i, err;
1557
1558         if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
1559             !info->attrs[NFC_ATTR_VENDOR_ID] ||
1560             !info->attrs[NFC_ATTR_VENDOR_SUBCMD])
1561                 return -EINVAL;
1562
1563         dev_idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
1564         vid = nla_get_u32(info->attrs[NFC_ATTR_VENDOR_ID]);
1565         subcmd = nla_get_u32(info->attrs[NFC_ATTR_VENDOR_SUBCMD]);
1566
1567         dev = nfc_get_device(dev_idx);
1568         if (!dev || !dev->vendor_cmds || !dev->n_vendor_cmds)
1569                 return -ENODEV;
1570
1571         if (info->attrs[NFC_ATTR_VENDOR_DATA]) {
1572                 data = nla_data(info->attrs[NFC_ATTR_VENDOR_DATA]);
1573                 data_len = nla_len(info->attrs[NFC_ATTR_VENDOR_DATA]);
1574                 if (data_len == 0)
1575                         return -EINVAL;
1576         } else {
1577                 data = NULL;
1578                 data_len = 0;
1579         }
1580
1581         for (i = 0; i < dev->n_vendor_cmds; i++) {
1582                 cmd = &dev->vendor_cmds[i];
1583
1584                 if (cmd->vendor_id != vid || cmd->subcmd != subcmd)
1585                         continue;
1586
1587                 dev->cur_cmd_info = info;
1588                 err = cmd->doit(dev, data, data_len);
1589                 dev->cur_cmd_info = NULL;
1590                 return err;
1591         }
1592
1593         return -EOPNOTSUPP;
1594 }
1595
1596 /* message building helper */
1597 static inline void *nfc_hdr_put(struct sk_buff *skb, u32 portid, u32 seq,
1598                                 int flags, u8 cmd)
1599 {
1600         /* since there is no private header just add the generic one */
1601         return genlmsg_put(skb, portid, seq, &nfc_genl_family, flags, cmd);
1602 }
1603
1604 static struct sk_buff *
1605 __nfc_alloc_vendor_cmd_skb(struct nfc_dev *dev, int approxlen,
1606                            u32 portid, u32 seq,
1607                            enum nfc_attrs attr,
1608                            u32 oui, u32 subcmd, gfp_t gfp)
1609 {
1610         struct sk_buff *skb;
1611         void *hdr;
1612
1613         skb = nlmsg_new(approxlen + 100, gfp);
1614         if (!skb)
1615                 return NULL;
1616
1617         hdr = nfc_hdr_put(skb, portid, seq, 0, NFC_CMD_VENDOR);
1618         if (!hdr) {
1619                 kfree_skb(skb);
1620                 return NULL;
1621         }
1622
1623         if (nla_put_u32(skb, NFC_ATTR_DEVICE_INDEX, dev->idx))
1624                 goto nla_put_failure;
1625         if (nla_put_u32(skb, NFC_ATTR_VENDOR_ID, oui))
1626                 goto nla_put_failure;
1627         if (nla_put_u32(skb, NFC_ATTR_VENDOR_SUBCMD, subcmd))
1628                 goto nla_put_failure;
1629
1630         ((void **)skb->cb)[0] = dev;
1631         ((void **)skb->cb)[1] = hdr;
1632
1633         return skb;
1634
1635 nla_put_failure:
1636         kfree_skb(skb);
1637         return NULL;
1638 }
1639
1640 struct sk_buff *__nfc_alloc_vendor_cmd_reply_skb(struct nfc_dev *dev,
1641                                                  enum nfc_attrs attr,
1642                                                  u32 oui, u32 subcmd,
1643                                                  int approxlen)
1644 {
1645         if (WARN_ON(!dev->cur_cmd_info))
1646                 return NULL;
1647
1648         return __nfc_alloc_vendor_cmd_skb(dev, approxlen,
1649                                           dev->cur_cmd_info->snd_portid,
1650                                           dev->cur_cmd_info->snd_seq, attr,
1651                                           oui, subcmd, GFP_KERNEL);
1652 }
1653 EXPORT_SYMBOL(__nfc_alloc_vendor_cmd_reply_skb);
1654
1655 int nfc_vendor_cmd_reply(struct sk_buff *skb)
1656 {
1657         struct nfc_dev *dev = ((void **)skb->cb)[0];
1658         void *hdr = ((void **)skb->cb)[1];
1659
1660         /* clear CB data for netlink core to own from now on */
1661         memset(skb->cb, 0, sizeof(skb->cb));
1662
1663         if (WARN_ON(!dev->cur_cmd_info)) {
1664                 kfree_skb(skb);
1665                 return -EINVAL;
1666         }
1667
1668         genlmsg_end(skb, hdr);
1669         return genlmsg_reply(skb, dev->cur_cmd_info);
1670 }
1671 EXPORT_SYMBOL(nfc_vendor_cmd_reply);
1672
1673 static const struct genl_ops nfc_genl_ops[] = {
1674         {
1675                 .cmd = NFC_CMD_GET_DEVICE,
1676                 .doit = nfc_genl_get_device,
1677                 .dumpit = nfc_genl_dump_devices,
1678                 .done = nfc_genl_dump_devices_done,
1679                 .policy = nfc_genl_policy,
1680         },
1681         {
1682                 .cmd = NFC_CMD_DEV_UP,
1683                 .doit = nfc_genl_dev_up,
1684                 .policy = nfc_genl_policy,
1685         },
1686         {
1687                 .cmd = NFC_CMD_DEV_DOWN,
1688                 .doit = nfc_genl_dev_down,
1689                 .policy = nfc_genl_policy,
1690         },
1691         {
1692                 .cmd = NFC_CMD_START_POLL,
1693                 .doit = nfc_genl_start_poll,
1694                 .policy = nfc_genl_policy,
1695         },
1696         {
1697                 .cmd = NFC_CMD_STOP_POLL,
1698                 .doit = nfc_genl_stop_poll,
1699                 .policy = nfc_genl_policy,
1700         },
1701         {
1702                 .cmd = NFC_CMD_DEP_LINK_UP,
1703                 .doit = nfc_genl_dep_link_up,
1704                 .policy = nfc_genl_policy,
1705         },
1706         {
1707                 .cmd = NFC_CMD_DEP_LINK_DOWN,
1708                 .doit = nfc_genl_dep_link_down,
1709                 .policy = nfc_genl_policy,
1710         },
1711         {
1712                 .cmd = NFC_CMD_GET_TARGET,
1713                 .dumpit = nfc_genl_dump_targets,
1714                 .done = nfc_genl_dump_targets_done,
1715                 .policy = nfc_genl_policy,
1716         },
1717         {
1718                 .cmd = NFC_CMD_LLC_GET_PARAMS,
1719                 .doit = nfc_genl_llc_get_params,
1720                 .policy = nfc_genl_policy,
1721         },
1722         {
1723                 .cmd = NFC_CMD_LLC_SET_PARAMS,
1724                 .doit = nfc_genl_llc_set_params,
1725                 .policy = nfc_genl_policy,
1726         },
1727         {
1728                 .cmd = NFC_CMD_LLC_SDREQ,
1729                 .doit = nfc_genl_llc_sdreq,
1730                 .policy = nfc_genl_policy,
1731         },
1732         {
1733                 .cmd = NFC_CMD_FW_DOWNLOAD,
1734                 .doit = nfc_genl_fw_download,
1735                 .policy = nfc_genl_policy,
1736         },
1737         {
1738                 .cmd = NFC_CMD_ENABLE_SE,
1739                 .doit = nfc_genl_enable_se,
1740                 .policy = nfc_genl_policy,
1741         },
1742         {
1743                 .cmd = NFC_CMD_DISABLE_SE,
1744                 .doit = nfc_genl_disable_se,
1745                 .policy = nfc_genl_policy,
1746         },
1747         {
1748                 .cmd = NFC_CMD_GET_SE,
1749                 .dumpit = nfc_genl_dump_ses,
1750                 .done = nfc_genl_dump_ses_done,
1751                 .policy = nfc_genl_policy,
1752         },
1753         {
1754                 .cmd = NFC_CMD_SE_IO,
1755                 .doit = nfc_genl_se_io,
1756                 .policy = nfc_genl_policy,
1757         },
1758         {
1759                 .cmd = NFC_CMD_ACTIVATE_TARGET,
1760                 .doit = nfc_genl_activate_target,
1761                 .policy = nfc_genl_policy,
1762         },
1763         {
1764                 .cmd = NFC_CMD_VENDOR,
1765                 .doit = nfc_genl_vendor_cmd,
1766                 .policy = nfc_genl_policy,
1767         },
1768         {
1769                 .cmd = NFC_CMD_DEACTIVATE_TARGET,
1770                 .doit = nfc_genl_deactivate_target,
1771                 .policy = nfc_genl_policy,
1772         },
1773 };
1774
1775 static struct genl_family nfc_genl_family __ro_after_init = {
1776         .hdrsize = 0,
1777         .name = NFC_GENL_NAME,
1778         .version = NFC_GENL_VERSION,
1779         .maxattr = NFC_ATTR_MAX,
1780         .module = THIS_MODULE,
1781         .ops = nfc_genl_ops,
1782         .n_ops = ARRAY_SIZE(nfc_genl_ops),
1783         .mcgrps = nfc_genl_mcgrps,
1784         .n_mcgrps = ARRAY_SIZE(nfc_genl_mcgrps),
1785 };
1786
1787
1788 struct urelease_work {
1789         struct  work_struct w;
1790         u32     portid;
1791 };
1792
1793 static void nfc_urelease_event_work(struct work_struct *work)
1794 {
1795         struct urelease_work *w = container_of(work, struct urelease_work, w);
1796         struct class_dev_iter iter;
1797         struct nfc_dev *dev;
1798
1799         pr_debug("portid %d\n", w->portid);
1800
1801         mutex_lock(&nfc_devlist_mutex);
1802
1803         nfc_device_iter_init(&iter);
1804         dev = nfc_device_iter_next(&iter);
1805
1806         while (dev) {
1807                 mutex_lock(&dev->genl_data.genl_data_mutex);
1808
1809                 if (dev->genl_data.poll_req_portid == w->portid) {
1810                         nfc_stop_poll(dev);
1811                         dev->genl_data.poll_req_portid = 0;
1812                 }
1813
1814                 mutex_unlock(&dev->genl_data.genl_data_mutex);
1815
1816                 dev = nfc_device_iter_next(&iter);
1817         }
1818
1819         nfc_device_iter_exit(&iter);
1820
1821         mutex_unlock(&nfc_devlist_mutex);
1822
1823         kfree(w);
1824 }
1825
1826 static int nfc_genl_rcv_nl_event(struct notifier_block *this,
1827                                  unsigned long event, void *ptr)
1828 {
1829         struct netlink_notify *n = ptr;
1830         struct urelease_work *w;
1831
1832         if (event != NETLINK_URELEASE || n->protocol != NETLINK_GENERIC)
1833                 goto out;
1834
1835         pr_debug("NETLINK_URELEASE event from id %d\n", n->portid);
1836
1837         w = kmalloc(sizeof(*w), GFP_ATOMIC);
1838         if (w) {
1839                 INIT_WORK((struct work_struct *) w, nfc_urelease_event_work);
1840                 w->portid = n->portid;
1841                 schedule_work((struct work_struct *) w);
1842         }
1843
1844 out:
1845         return NOTIFY_DONE;
1846 }
1847
1848 void nfc_genl_data_init(struct nfc_genl_data *genl_data)
1849 {
1850         genl_data->poll_req_portid = 0;
1851         mutex_init(&genl_data->genl_data_mutex);
1852 }
1853
1854 void nfc_genl_data_exit(struct nfc_genl_data *genl_data)
1855 {
1856         mutex_destroy(&genl_data->genl_data_mutex);
1857 }
1858
1859 static struct notifier_block nl_notifier = {
1860         .notifier_call  = nfc_genl_rcv_nl_event,
1861 };
1862
1863 /**
1864  * nfc_genl_init() - Initialize netlink interface
1865  *
1866  * This initialization function registers the nfc netlink family.
1867  */
1868 int __init nfc_genl_init(void)
1869 {
1870         int rc;
1871
1872         rc = genl_register_family(&nfc_genl_family);
1873         if (rc)
1874                 return rc;
1875
1876         netlink_register_notifier(&nl_notifier);
1877
1878         return 0;
1879 }
1880
1881 /**
1882  * nfc_genl_exit() - Deinitialize netlink interface
1883  *
1884  * This exit function unregisters the nfc netlink family.
1885  */
1886 void nfc_genl_exit(void)
1887 {
1888         netlink_unregister_notifier(&nl_notifier);
1889         genl_unregister_family(&nfc_genl_family);
1890 }