net/nfc/netlink.c

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