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