kernel/net/nfc/nci/core.c

   1 /*
   2  *  The NFC Controller Interface is the communication protocol between an
   3  *  NFC Controller (NFCC) and a Device Host (DH).
   4  *
   5  *  Copyright (C) 2011 Texas Instruments, Inc.
   6  *  Copyright (C) 2014 Marvell International Ltd.
   7  *
   8  *  Written by Ilan Elias <ilane@ti.com>
   9  *
  10  *  Acknowledgements:
  11  *  This file is based on hci_core.c, which was written
  12  *  by Maxim Krasnyansky.
  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 version 2
  16  *  as published by the Free Software Foundation
  17  *
  18  *  This program is distributed in the hope that it will be useful,
  19  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  20  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  21  *  GNU General Public License for more details.
  22  *
  23  *  You should have received a copy of the GNU General Public License
  24  *  along with this program; if not, see <http://www.gnu.org/licenses/>.
  25  *
  26  */
  27
  28 #define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__
  29
  30 #include <linux/module.h>
  31 #include <linux/kernel.h>
  32 #include <linux/types.h>
  33 #include <linux/workqueue.h>
  34 #include <linux/completion.h>
  35 #include <linux/export.h>
  36 #include <linux/sched.h>
  37 #include <linux/bitops.h>
  38 #include <linux/skbuff.h>
  39
  40 #include "../nfc.h"
  41 #include <net/nfc/nci.h>
  42 #include <net/nfc/nci_core.h>
  43 #include <linux/nfc.h>
  44
  45 struct core_conn_create_data {
  46         int length;
  47         struct nci_core_conn_create_cmd *cmd;
  48 };
  49
  50 static void nci_cmd_work(struct work_struct *work);
  51 static void nci_rx_work(struct work_struct *work);
  52 static void nci_tx_work(struct work_struct *work);
  53
  54 struct nci_conn_info *nci_get_conn_info_by_conn_id(struct nci_dev *ndev,
  55                                                    int conn_id)
  56 {
  57         struct nci_conn_info *conn_info;
  58
  59         list_for_each_entry(conn_info, &ndev->conn_info_list, list) {
  60                 if (conn_info->conn_id == conn_id)
  61                         return conn_info;
  62         }
  63
  64         return NULL;
  65 }
  66
  67 int nci_get_conn_info_by_id(struct nci_dev *ndev, u8 id)
  68 {
  69         struct nci_conn_info *conn_info;
  70
  71         list_for_each_entry(conn_info, &ndev->conn_info_list, list) {
  72                 if (conn_info->id == id)
  73                         return conn_info->conn_id;
  74         }
  75
  76         return -EINVAL;
  77 }
  78 EXPORT_SYMBOL(nci_get_conn_info_by_id);
  79
  80 /* ---- NCI requests ---- */
  81
  82 void nci_req_complete(struct nci_dev *ndev, int result)
  83 {
  84         if (ndev->req_status == NCI_REQ_PEND) {
  85                 ndev->req_result = result;
  86                 ndev->req_status = NCI_REQ_DONE;
  87                 complete(&ndev->req_completion);
  88         }
  89 }
  90 EXPORT_SYMBOL(nci_req_complete);
  91
  92 static void nci_req_cancel(struct nci_dev *ndev, int err)
  93 {
  94         if (ndev->req_status == NCI_REQ_PEND) {
  95                 ndev->req_result = err;
  96                 ndev->req_status = NCI_REQ_CANCELED;
  97                 complete(&ndev->req_completion);
  98         }
  99 }
 100
 101 /* Execute request and wait for completion. */
 102 static int __nci_request(struct nci_dev *ndev,
 103                          void (*req)(struct nci_dev *ndev, unsigned long opt),
 104                          unsigned long opt, __u32 timeout)
 105 {
 106         int rc = 0;
 107         long completion_rc;
 108
 109         ndev->req_status = NCI_REQ_PEND;
 110
 111         reinit_completion(&ndev->req_completion);
 112         req(ndev, opt);
 113         completion_rc =
 114                 wait_for_completion_interruptible_timeout(&ndev->req_completion,
 115                                                           timeout);
 116
 117         pr_debug("wait_for_completion return %ld\n", completion_rc);
 118
 119         if (completion_rc > 0) {
 120                 switch (ndev->req_status) {
 121                 case NCI_REQ_DONE:
 122                         rc = nci_to_errno(ndev->req_result);
 123                         break;
 124
 125                 case NCI_REQ_CANCELED:
 126                         rc = -ndev->req_result;
 127                         break;
 128
 129                 default:
 130                         rc = -ETIMEDOUT;
 131                         break;
 132                 }
 133         } else {
 134                 pr_err("wait_for_completion_interruptible_timeout failed %ld\n",
 135                        completion_rc);
 136
 137                 rc = ((completion_rc == 0) ? (-ETIMEDOUT) : (completion_rc));
 138         }
 139
 140         ndev->req_status = ndev->req_result = 0;
 141
 142         return rc;
 143 }
 144
 145 inline int nci_request(struct nci_dev *ndev,
 146                        void (*req)(struct nci_dev *ndev,
 147                                    unsigned long opt),
 148                        unsigned long opt, __u32 timeout)
 149 {
 150         int rc;
 151
 152         if (!test_bit(NCI_UP, &ndev->flags))
 153                 return -ENETDOWN;
 154
 155         /* Serialize all requests */
 156         mutex_lock(&ndev->req_lock);
 157         rc = __nci_request(ndev, req, opt, timeout);
 158         mutex_unlock(&ndev->req_lock);
 159
 160         return rc;
 161 }
 162
 163 static void nci_reset_req(struct nci_dev *ndev, unsigned long opt)
 164 {
 165         struct nci_core_reset_cmd cmd;
 166
 167         cmd.reset_type = NCI_RESET_TYPE_RESET_CONFIG;
 168         nci_send_cmd(ndev, NCI_OP_CORE_RESET_CMD, 1, &cmd);
 169 }
 170
 171 static void nci_init_req(struct nci_dev *ndev, unsigned long opt)
 172 {
 173         nci_send_cmd(ndev, NCI_OP_CORE_INIT_CMD, 0, NULL);
 174 }
 175
 176 static void nci_init_complete_req(struct nci_dev *ndev, unsigned long opt)
 177 {
 178         struct nci_rf_disc_map_cmd cmd;
 179         struct disc_map_config *cfg = cmd.mapping_configs;
 180         __u8 *num = &cmd.num_mapping_configs;
 181         int i;
 182
 183         /* set rf mapping configurations */
 184         *num = 0;
 185
 186         /* by default mapping is set to NCI_RF_INTERFACE_FRAME */
 187         for (i = 0; i < ndev->num_supported_rf_interfaces; i++) {
 188                 if (ndev->supported_rf_interfaces[i] ==
 189                     NCI_RF_INTERFACE_ISO_DEP) {
 190                         cfg[*num].rf_protocol = NCI_RF_PROTOCOL_ISO_DEP;
 191                         cfg[*num].mode = NCI_DISC_MAP_MODE_POLL |
 192                                 NCI_DISC_MAP_MODE_LISTEN;
 193                         cfg[*num].rf_interface = NCI_RF_INTERFACE_ISO_DEP;
 194                         (*num)++;
 195                 } else if (ndev->supported_rf_interfaces[i] ==
 196                            NCI_RF_INTERFACE_NFC_DEP) {
 197                         cfg[*num].rf_protocol = NCI_RF_PROTOCOL_NFC_DEP;
 198                         cfg[*num].mode = NCI_DISC_MAP_MODE_POLL |
 199                                 NCI_DISC_MAP_MODE_LISTEN;
 200                         cfg[*num].rf_interface = NCI_RF_INTERFACE_NFC_DEP;
 201                         (*num)++;
 202                 }
 203
 204                 if (*num == NCI_MAX_NUM_MAPPING_CONFIGS)
 205                         break;
 206         }
 207
 208         nci_send_cmd(ndev, NCI_OP_RF_DISCOVER_MAP_CMD,
 209                      (1 + ((*num) * sizeof(struct disc_map_config))), &cmd);
 210 }
 211
 212 struct nci_set_config_param {
 213         __u8    id;
 214         size_t  len;
 215         __u8    *val;
 216 };
 217
 218 static void nci_set_config_req(struct nci_dev *ndev, unsigned long opt)
 219 {
 220         struct nci_set_config_param *param = (struct nci_set_config_param *)opt;
 221         struct nci_core_set_config_cmd cmd;
 222
 223         BUG_ON(param->len > NCI_MAX_PARAM_LEN);
 224
 225         cmd.num_params = 1;
 226         cmd.param.id = param->id;
 227         cmd.param.len = param->len;
 228         memcpy(cmd.param.val, param->val, param->len);
 229
 230         nci_send_cmd(ndev, NCI_OP_CORE_SET_CONFIG_CMD, (3 + param->len), &cmd);
 231 }
 232
 233 struct nci_rf_discover_param {
 234         __u32   im_protocols;
 235         __u32   tm_protocols;
 236 };
 237
 238 static void nci_rf_discover_req(struct nci_dev *ndev, unsigned long opt)
 239 {
 240         struct nci_rf_discover_param *param =
 241                 (struct nci_rf_discover_param *)opt;
 242         struct nci_rf_disc_cmd cmd;
 243
 244         cmd.num_disc_configs = 0;
 245
 246         if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS) &&
 247             (param->im_protocols & NFC_PROTO_JEWEL_MASK ||
 248              param->im_protocols & NFC_PROTO_MIFARE_MASK ||
 249              param->im_protocols & NFC_PROTO_ISO14443_MASK ||
 250              param->im_protocols & NFC_PROTO_NFC_DEP_MASK)) {
 251                 cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode =
 252                         NCI_NFC_A_PASSIVE_POLL_MODE;
 253                 cmd.disc_configs[cmd.num_disc_configs].frequency = 1;
 254                 cmd.num_disc_configs++;
 255         }
 256
 257         if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS) &&
 258             (param->im_protocols & NFC_PROTO_ISO14443_B_MASK)) {
 259                 cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode =
 260                         NCI_NFC_B_PASSIVE_POLL_MODE;
 261                 cmd.disc_configs[cmd.num_disc_configs].frequency = 1;
 262                 cmd.num_disc_configs++;
 263         }
 264
 265         if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS) &&
 266             (param->im_protocols & NFC_PROTO_FELICA_MASK ||
 267              param->im_protocols & NFC_PROTO_NFC_DEP_MASK)) {
 268                 cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode =
 269                         NCI_NFC_F_PASSIVE_POLL_MODE;
 270                 cmd.disc_configs[cmd.num_disc_configs].frequency = 1;
 271                 cmd.num_disc_configs++;
 272         }
 273
 274         if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS) &&
 275             (param->im_protocols & NFC_PROTO_ISO15693_MASK)) {
 276                 cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode =
 277                         NCI_NFC_V_PASSIVE_POLL_MODE;
 278                 cmd.disc_configs[cmd.num_disc_configs].frequency = 1;
 279                 cmd.num_disc_configs++;
 280         }
 281
 282         if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS - 1) &&
 283             (param->tm_protocols & NFC_PROTO_NFC_DEP_MASK)) {
 284                 cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode =
 285                         NCI_NFC_A_PASSIVE_LISTEN_MODE;
 286                 cmd.disc_configs[cmd.num_disc_configs].frequency = 1;
 287                 cmd.num_disc_configs++;
 288                 cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode =
 289                         NCI_NFC_F_PASSIVE_LISTEN_MODE;
 290                 cmd.disc_configs[cmd.num_disc_configs].frequency = 1;
 291                 cmd.num_disc_configs++;
 292         }
 293
 294         nci_send_cmd(ndev, NCI_OP_RF_DISCOVER_CMD,
 295                      (1 + (cmd.num_disc_configs * sizeof(struct disc_config))),
 296                      &cmd);
 297 }
 298
 299 struct nci_rf_discover_select_param {
 300         __u8    rf_discovery_id;
 301         __u8    rf_protocol;
 302 };
 303
 304 static void nci_rf_discover_select_req(struct nci_dev *ndev, unsigned long opt)
 305 {
 306         struct nci_rf_discover_select_param *param =
 307                 (struct nci_rf_discover_select_param *)opt;
 308         struct nci_rf_discover_select_cmd cmd;
 309
 310         cmd.rf_discovery_id = param->rf_discovery_id;
 311         cmd.rf_protocol = param->rf_protocol;
 312
 313         switch (cmd.rf_protocol) {
 314         case NCI_RF_PROTOCOL_ISO_DEP:
 315                 cmd.rf_interface = NCI_RF_INTERFACE_ISO_DEP;
 316                 break;
 317
 318         case NCI_RF_PROTOCOL_NFC_DEP:
 319                 cmd.rf_interface = NCI_RF_INTERFACE_NFC_DEP;
 320                 break;
 321
 322         default:
 323                 cmd.rf_interface = NCI_RF_INTERFACE_FRAME;
 324                 break;
 325         }
 326
 327         nci_send_cmd(ndev, NCI_OP_RF_DISCOVER_SELECT_CMD,
 328                      sizeof(struct nci_rf_discover_select_cmd), &cmd);
 329 }
 330
 331 static void nci_rf_deactivate_req(struct nci_dev *ndev, unsigned long opt)
 332 {
 333         struct nci_rf_deactivate_cmd cmd;
 334
 335         cmd.type = opt;
 336
 337         nci_send_cmd(ndev, NCI_OP_RF_DEACTIVATE_CMD,
 338                      sizeof(struct nci_rf_deactivate_cmd), &cmd);
 339 }
 340
 341 struct nci_cmd_param {
 342         __u16 opcode;
 343         size_t len;
 344         __u8 *payload;
 345 };
 346
 347 static void nci_generic_req(struct nci_dev *ndev, unsigned long opt)
 348 {
 349         struct nci_cmd_param *param =
 350                 (struct nci_cmd_param *)opt;
 351
 352         nci_send_cmd(ndev, param->opcode, param->len, param->payload);
 353 }
 354
 355 int nci_prop_cmd(struct nci_dev *ndev, __u8 oid, size_t len, __u8 *payload)
 356 {
 357         struct nci_cmd_param param;
 358
 359         param.opcode = nci_opcode_pack(NCI_GID_PROPRIETARY, oid);
 360         param.len = len;
 361         param.payload = payload;
 362
 363         return __nci_request(ndev, nci_generic_req, (unsigned long)&param,
 364                              msecs_to_jiffies(NCI_CMD_TIMEOUT));
 365 }
 366 EXPORT_SYMBOL(nci_prop_cmd);
 367
 368 int nci_core_cmd(struct nci_dev *ndev, __u16 opcode, size_t len, __u8 *payload)
 369 {
 370         struct nci_cmd_param param;
 371
 372         param.opcode = opcode;
 373         param.len = len;
 374         param.payload = payload;
 375
 376         return __nci_request(ndev, nci_generic_req, (unsigned long)&param,
 377                              msecs_to_jiffies(NCI_CMD_TIMEOUT));
 378 }
 379 EXPORT_SYMBOL(nci_core_cmd);
 380
 381 int nci_core_reset(struct nci_dev *ndev)
 382 {
 383         return __nci_request(ndev, nci_reset_req, 0,
 384                              msecs_to_jiffies(NCI_RESET_TIMEOUT));
 385 }
 386 EXPORT_SYMBOL(nci_core_reset);
 387
 388 int nci_core_init(struct nci_dev *ndev)
 389 {
 390         return __nci_request(ndev, nci_init_req, 0,
 391                              msecs_to_jiffies(NCI_INIT_TIMEOUT));
 392 }
 393 EXPORT_SYMBOL(nci_core_init);
 394
 395 static int nci_open_device(struct nci_dev *ndev)
 396 {
 397         int rc = 0;
 398
 399         mutex_lock(&ndev->req_lock);
 400
 401         if (test_bit(NCI_UP, &ndev->flags)) {
 402                 rc = -EALREADY;
 403                 goto done;
 404         }
 405
 406         if (ndev->ops->open(ndev)) {
 407                 rc = -EIO;
 408                 goto done;
 409         }
 410
 411         atomic_set(&ndev->cmd_cnt, 1);
 412
 413         set_bit(NCI_INIT, &ndev->flags);
 414
 415         if (ndev->ops->init)
 416                 rc = ndev->ops->init(ndev);
 417
 418         if (!rc) {
 419                 rc = __nci_request(ndev, nci_reset_req, 0,
 420                                    msecs_to_jiffies(NCI_RESET_TIMEOUT));
 421         }
 422
 423         if (!rc && ndev->ops->setup) {
 424                 rc = ndev->ops->setup(ndev);
 425         }
 426
 427         if (!rc) {
 428                 rc = __nci_request(ndev, nci_init_req, 0,
 429                                    msecs_to_jiffies(NCI_INIT_TIMEOUT));
 430         }
 431
 432         if (!rc && ndev->ops->post_setup)
 433                 rc = ndev->ops->post_setup(ndev);
 434
 435         if (!rc) {
 436                 rc = __nci_request(ndev, nci_init_complete_req, 0,
 437                                    msecs_to_jiffies(NCI_INIT_TIMEOUT));
 438         }
 439
 440         clear_bit(NCI_INIT, &ndev->flags);
 441
 442         if (!rc) {
 443                 set_bit(NCI_UP, &ndev->flags);
 444                 nci_clear_target_list(ndev);
 445                 atomic_set(&ndev->state, NCI_IDLE);
 446         } else {
 447                 /* Init failed, cleanup */
 448                 skb_queue_purge(&ndev->cmd_q);
 449                 skb_queue_purge(&ndev->rx_q);
 450                 skb_queue_purge(&ndev->tx_q);
 451
 452                 ndev->ops->close(ndev);
 453                 ndev->flags = 0;
 454         }
 455
 456 done:
 457         mutex_unlock(&ndev->req_lock);
 458         return rc;
 459 }
 460
 461 static int nci_close_device(struct nci_dev *ndev)
 462 {
 463         nci_req_cancel(ndev, ENODEV);
 464         mutex_lock(&ndev->req_lock);
 465
 466         if (!test_and_clear_bit(NCI_UP, &ndev->flags)) {
 467                 del_timer_sync(&ndev->cmd_timer);
 468                 del_timer_sync(&ndev->data_timer);
 469                 mutex_unlock(&ndev->req_lock);
 470                 return 0;
 471         }
 472
 473         /* Drop RX and TX queues */
 474         skb_queue_purge(&ndev->rx_q);
 475         skb_queue_purge(&ndev->tx_q);
 476
 477         /* Flush RX and TX wq */
 478         flush_workqueue(ndev->rx_wq);
 479         flush_workqueue(ndev->tx_wq);
 480
 481         /* Reset device */
 482         skb_queue_purge(&ndev->cmd_q);
 483         atomic_set(&ndev->cmd_cnt, 1);
 484
 485         set_bit(NCI_INIT, &ndev->flags);
 486         __nci_request(ndev, nci_reset_req, 0,
 487                       msecs_to_jiffies(NCI_RESET_TIMEOUT));
 488
 489         /* After this point our queues are empty
 490          * and no works are scheduled.
 491          */
 492         ndev->ops->close(ndev);
 493
 494         clear_bit(NCI_INIT, &ndev->flags);
 495
 496         del_timer_sync(&ndev->cmd_timer);
 497
 498         /* Flush cmd wq */
 499         flush_workqueue(ndev->cmd_wq);
 500
 501         /* Clear flags */
 502         ndev->flags = 0;
 503
 504         mutex_unlock(&ndev->req_lock);
 505
 506         return 0;
 507 }
 508
 509 /* NCI command timer function */
 510 static void nci_cmd_timer(unsigned long arg)
 511 {
 512         struct nci_dev *ndev = (void *) arg;
 513
 514         atomic_set(&ndev->cmd_cnt, 1);
 515         queue_work(ndev->cmd_wq, &ndev->cmd_work);
 516 }
 517
 518 /* NCI data exchange timer function */
 519 static void nci_data_timer(unsigned long arg)
 520 {
 521         struct nci_dev *ndev = (void *) arg;
 522
 523         set_bit(NCI_DATA_EXCHANGE_TO, &ndev->flags);
 524         queue_work(ndev->rx_wq, &ndev->rx_work);
 525 }
 526
 527 static int nci_dev_up(struct nfc_dev *nfc_dev)
 528 {
 529         struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
 530
 531         return nci_open_device(ndev);
 532 }
 533
 534 static int nci_dev_down(struct nfc_dev *nfc_dev)
 535 {
 536         struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
 537
 538         return nci_close_device(ndev);
 539 }
 540
 541 int nci_set_config(struct nci_dev *ndev, __u8 id, size_t len, __u8 *val)
 542 {
 543         struct nci_set_config_param param;
 544
 545         if (!val || !len)
 546                 return 0;
 547
 548         param.id = id;
 549         param.len = len;
 550         param.val = val;
 551
 552         return __nci_request(ndev, nci_set_config_req, (unsigned long)&param,
 553                              msecs_to_jiffies(NCI_SET_CONFIG_TIMEOUT));
 554 }
 555 EXPORT_SYMBOL(nci_set_config);
 556
 557 static void nci_nfcee_discover_req(struct nci_dev *ndev, unsigned long opt)
 558 {
 559         struct nci_nfcee_discover_cmd cmd;
 560         __u8 action = opt;
 561
 562         cmd.discovery_action = action;
 563
 564         nci_send_cmd(ndev, NCI_OP_NFCEE_DISCOVER_CMD, 1, &cmd);
 565 }
 566
 567 int nci_nfcee_discover(struct nci_dev *ndev, u8 action)
 568 {
 569         return __nci_request(ndev, nci_nfcee_discover_req, action,
 570                                 msecs_to_jiffies(NCI_CMD_TIMEOUT));
 571 }
 572 EXPORT_SYMBOL(nci_nfcee_discover);
 573
 574 static void nci_nfcee_mode_set_req(struct nci_dev *ndev, unsigned long opt)
 575 {
 576         struct nci_nfcee_mode_set_cmd *cmd =
 577                                         (struct nci_nfcee_mode_set_cmd *)opt;
 578
 579         nci_send_cmd(ndev, NCI_OP_NFCEE_MODE_SET_CMD,
 580                      sizeof(struct nci_nfcee_mode_set_cmd), cmd);
 581 }
 582
 583 int nci_nfcee_mode_set(struct nci_dev *ndev, u8 nfcee_id, u8 nfcee_mode)
 584 {
 585         struct nci_nfcee_mode_set_cmd cmd;
 586
 587         cmd.nfcee_id = nfcee_id;
 588         cmd.nfcee_mode = nfcee_mode;
 589
 590         return __nci_request(ndev, nci_nfcee_mode_set_req,
 591                              (unsigned long)&cmd,
 592                              msecs_to_jiffies(NCI_CMD_TIMEOUT));
 593 }
 594 EXPORT_SYMBOL(nci_nfcee_mode_set);
 595
 596 static void nci_core_conn_create_req(struct nci_dev *ndev, unsigned long opt)
 597 {
 598         struct core_conn_create_data *data =
 599                                         (struct core_conn_create_data *)opt;
 600
 601         nci_send_cmd(ndev, NCI_OP_CORE_CONN_CREATE_CMD, data->length, data->cmd);
 602 }
 603
 604 int nci_core_conn_create(struct nci_dev *ndev, u8 destination_type,
 605                          u8 number_destination_params,
 606                          size_t params_len,
 607                          struct core_conn_create_dest_spec_params *params)
 608 {
 609         int r;
 610         struct nci_core_conn_create_cmd *cmd;
 611         struct core_conn_create_data data;
 612
 613         data.length = params_len + sizeof(struct nci_core_conn_create_cmd);
 614         cmd = kzalloc(data.length, GFP_KERNEL);
 615         if (!cmd)
 616                 return -ENOMEM;
 617
 618         if (!number_destination_params)
 619                 return -EINVAL;
 620
 621         cmd->destination_type = destination_type;
 622         cmd->number_destination_params = number_destination_params;
 623         memcpy(cmd->params, params, params_len);
 624
 625         data.cmd = cmd;
 626
 627         if (params->length > 0)
 628                 ndev->cur_id = params->value[DEST_SPEC_PARAMS_ID_INDEX];
 629         else
 630                 ndev->cur_id = 0;
 631
 632         r = __nci_request(ndev, nci_core_conn_create_req,
 633                           (unsigned long)&data,
 634                           msecs_to_jiffies(NCI_CMD_TIMEOUT));
 635         kfree(cmd);
 636         return r;
 637 }
 638 EXPORT_SYMBOL(nci_core_conn_create);
 639
 640 static void nci_core_conn_close_req(struct nci_dev *ndev, unsigned long opt)
 641 {
 642         __u8 conn_id = opt;
 643
 644         nci_send_cmd(ndev, NCI_OP_CORE_CONN_CLOSE_CMD, 1, &conn_id);
 645 }
 646
 647 int nci_core_conn_close(struct nci_dev *ndev, u8 conn_id)
 648 {
 649         return __nci_request(ndev, nci_core_conn_close_req, conn_id,
 650                              msecs_to_jiffies(NCI_CMD_TIMEOUT));
 651 }
 652 EXPORT_SYMBOL(nci_core_conn_close);
 653
 654 static int nci_set_local_general_bytes(struct nfc_dev *nfc_dev)
 655 {
 656         struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
 657         struct nci_set_config_param param;
 658         int rc;
 659
 660         param.val = nfc_get_local_general_bytes(nfc_dev, &param.len);
 661         if ((param.val == NULL) || (param.len == 0))
 662                 return 0;
 663
 664         if (param.len > NFC_MAX_GT_LEN)
 665                 return -EINVAL;
 666
 667         param.id = NCI_PN_ATR_REQ_GEN_BYTES;
 668
 669         rc = nci_request(ndev, nci_set_config_req, (unsigned long)&param,
 670                          msecs_to_jiffies(NCI_SET_CONFIG_TIMEOUT));
 671         if (rc)
 672                 return rc;
 673
 674         param.id = NCI_LN_ATR_RES_GEN_BYTES;
 675
 676         return nci_request(ndev, nci_set_config_req, (unsigned long)&param,
 677                            msecs_to_jiffies(NCI_SET_CONFIG_TIMEOUT));
 678 }
 679
 680 static int nci_set_listen_parameters(struct nfc_dev *nfc_dev)
 681 {
 682         struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
 683         int rc;
 684         __u8 val;
 685
 686         val = NCI_LA_SEL_INFO_NFC_DEP_MASK;
 687
 688         rc = nci_set_config(ndev, NCI_LA_SEL_INFO, 1, &val);
 689         if (rc)
 690                 return rc;
 691
 692         val = NCI_LF_PROTOCOL_TYPE_NFC_DEP_MASK;
 693
 694         rc = nci_set_config(ndev, NCI_LF_PROTOCOL_TYPE, 1, &val);
 695         if (rc)
 696                 return rc;
 697
 698         val = NCI_LF_CON_BITR_F_212 | NCI_LF_CON_BITR_F_424;
 699
 700         return nci_set_config(ndev, NCI_LF_CON_BITR_F, 1, &val);
 701 }
 702
 703 static int nci_start_poll(struct nfc_dev *nfc_dev,
 704                           __u32 im_protocols, __u32 tm_protocols)
 705 {
 706         struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
 707         struct nci_rf_discover_param param;
 708         int rc;
 709
 710         if ((atomic_read(&ndev->state) == NCI_DISCOVERY) ||
 711             (atomic_read(&ndev->state) == NCI_W4_ALL_DISCOVERIES)) {
 712                 pr_err("unable to start poll, since poll is already active\n");
 713                 return -EBUSY;
 714         }
 715
 716         if (ndev->target_active_prot) {
 717                 pr_err("there is an active target\n");
 718                 return -EBUSY;
 719         }
 720
 721         if ((atomic_read(&ndev->state) == NCI_W4_HOST_SELECT) ||
 722             (atomic_read(&ndev->state) == NCI_POLL_ACTIVE)) {
 723                 pr_debug("target active or w4 select, implicitly deactivate\n");
 724
 725                 rc = nci_request(ndev, nci_rf_deactivate_req,
 726                                  NCI_DEACTIVATE_TYPE_IDLE_MODE,
 727                                  msecs_to_jiffies(NCI_RF_DEACTIVATE_TIMEOUT));
 728                 if (rc)
 729                         return -EBUSY;
 730         }
 731
 732         if ((im_protocols | tm_protocols) & NFC_PROTO_NFC_DEP_MASK) {
 733                 rc = nci_set_local_general_bytes(nfc_dev);
 734                 if (rc) {
 735                         pr_err("failed to set local general bytes\n");
 736                         return rc;
 737                 }
 738         }
 739
 740         if (tm_protocols & NFC_PROTO_NFC_DEP_MASK) {
 741                 rc = nci_set_listen_parameters(nfc_dev);
 742                 if (rc)
 743                         pr_err("failed to set listen parameters\n");
 744         }
 745
 746         param.im_protocols = im_protocols;
 747         param.tm_protocols = tm_protocols;
 748         rc = nci_request(ndev, nci_rf_discover_req, (unsigned long)&param,
 749                          msecs_to_jiffies(NCI_RF_DISC_TIMEOUT));
 750
 751         if (!rc)
 752                 ndev->poll_prots = im_protocols;
 753
 754         return rc;
 755 }
 756
 757 static void nci_stop_poll(struct nfc_dev *nfc_dev)
 758 {
 759         struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
 760
 761         if ((atomic_read(&ndev->state) != NCI_DISCOVERY) &&
 762             (atomic_read(&ndev->state) != NCI_W4_ALL_DISCOVERIES)) {
 763                 pr_err("unable to stop poll, since poll is not active\n");
 764                 return;
 765         }
 766
 767         nci_request(ndev, nci_rf_deactivate_req, NCI_DEACTIVATE_TYPE_IDLE_MODE,
 768                     msecs_to_jiffies(NCI_RF_DEACTIVATE_TIMEOUT));
 769 }
 770
 771 static int nci_activate_target(struct nfc_dev *nfc_dev,
 772                                struct nfc_target *target, __u32 protocol)
 773 {
 774         struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
 775         struct nci_rf_discover_select_param param;
 776         struct nfc_target *nci_target = NULL;
 777         int i;
 778         int rc = 0;
 779
 780         pr_debug("target_idx %d, protocol 0x%x\n", target->idx, protocol);
 781
 782         if ((atomic_read(&ndev->state) != NCI_W4_HOST_SELECT) &&
 783             (atomic_read(&ndev->state) != NCI_POLL_ACTIVE)) {
 784                 pr_err("there is no available target to activate\n");
 785                 return -EINVAL;
 786         }
 787
 788         if (ndev->target_active_prot) {
 789                 pr_err("there is already an active target\n");
 790                 return -EBUSY;
 791         }
 792
 793         for (i = 0; i < ndev->n_targets; i++) {
 794                 if (ndev->targets[i].idx == target->idx) {
 795                         nci_target = &ndev->targets[i];
 796                         break;
 797                 }
 798         }
 799
 800         if (!nci_target) {
 801                 pr_err("unable to find the selected target\n");
 802                 return -EINVAL;
 803         }
 804
 805         if (!(nci_target->supported_protocols & (1 << protocol))) {
 806                 pr_err("target does not support the requested protocol 0x%x\n",
 807                        protocol);
 808                 return -EINVAL;
 809         }
 810
 811         if (atomic_read(&ndev->state) == NCI_W4_HOST_SELECT) {
 812                 param.rf_discovery_id = nci_target->logical_idx;
 813
 814                 if (protocol == NFC_PROTO_JEWEL)
 815                         param.rf_protocol = NCI_RF_PROTOCOL_T1T;
 816                 else if (protocol == NFC_PROTO_MIFARE)
 817                         param.rf_protocol = NCI_RF_PROTOCOL_T2T;
 818                 else if (protocol == NFC_PROTO_FELICA)
 819                         param.rf_protocol = NCI_RF_PROTOCOL_T3T;
 820                 else if (protocol == NFC_PROTO_ISO14443 ||
 821                          protocol == NFC_PROTO_ISO14443_B)
 822                         param.rf_protocol = NCI_RF_PROTOCOL_ISO_DEP;
 823                 else
 824                         param.rf_protocol = NCI_RF_PROTOCOL_NFC_DEP;
 825
 826                 rc = nci_request(ndev, nci_rf_discover_select_req,
 827                                  (unsigned long)&param,
 828                                  msecs_to_jiffies(NCI_RF_DISC_SELECT_TIMEOUT));
 829         }
 830
 831         if (!rc)
 832                 ndev->target_active_prot = protocol;
 833
 834         return rc;
 835 }
 836
 837 static void nci_deactivate_target(struct nfc_dev *nfc_dev,
 838                                   struct nfc_target *target,
 839                                   __u8 mode)
 840 {
 841         struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
 842         u8 nci_mode = NCI_DEACTIVATE_TYPE_IDLE_MODE;
 843
 844         pr_debug("entry\n");
 845
 846         if (!ndev->target_active_prot) {
 847                 pr_err("unable to deactivate target, no active target\n");
 848                 return;
 849         }
 850
 851         ndev->target_active_prot = 0;
 852
 853         switch (mode) {
 854         case NFC_TARGET_MODE_SLEEP:
 855                 nci_mode = NCI_DEACTIVATE_TYPE_SLEEP_MODE;
 856                 break;
 857         }
 858
 859         if (atomic_read(&ndev->state) == NCI_POLL_ACTIVE) {
 860                 nci_request(ndev, nci_rf_deactivate_req, nci_mode,
 861                             msecs_to_jiffies(NCI_RF_DEACTIVATE_TIMEOUT));
 862         }
 863 }
 864
 865 static int nci_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
 866                            __u8 comm_mode, __u8 *gb, size_t gb_len)
 867 {
 868         struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
 869         int rc;
 870
 871         pr_debug("target_idx %d, comm_mode %d\n", target->idx, comm_mode);
 872
 873         rc = nci_activate_target(nfc_dev, target, NFC_PROTO_NFC_DEP);
 874         if (rc)
 875                 return rc;
 876
 877         rc = nfc_set_remote_general_bytes(nfc_dev, ndev->remote_gb,
 878                                           ndev->remote_gb_len);
 879         if (!rc)
 880                 rc = nfc_dep_link_is_up(nfc_dev, target->idx, NFC_COMM_PASSIVE,
 881                                         NFC_RF_INITIATOR);
 882
 883         return rc;
 884 }
 885
 886 static int nci_dep_link_down(struct nfc_dev *nfc_dev)
 887 {
 888         struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
 889         int rc;
 890
 891         pr_debug("entry\n");
 892
 893         if (nfc_dev->rf_mode == NFC_RF_INITIATOR) {
 894                 nci_deactivate_target(nfc_dev, NULL, NCI_DEACTIVATE_TYPE_IDLE_MODE);
 895         } else {
 896                 if (atomic_read(&ndev->state) == NCI_LISTEN_ACTIVE ||
 897                     atomic_read(&ndev->state) == NCI_DISCOVERY) {
 898                         nci_request(ndev, nci_rf_deactivate_req, 0,
 899                                 msecs_to_jiffies(NCI_RF_DEACTIVATE_TIMEOUT));
 900                 }
 901
 902                 rc = nfc_tm_deactivated(nfc_dev);
 903                 if (rc)
 904                         pr_err("error when signaling tm deactivation\n");
 905         }
 906
 907         return 0;
 908 }
 909
 910
 911 static int nci_transceive(struct nfc_dev *nfc_dev, struct nfc_target *target,
 912                           struct sk_buff *skb,
 913                           data_exchange_cb_t cb, void *cb_context)
 914 {
 915         struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
 916         int rc;
 917         struct nci_conn_info    *conn_info;
 918
 919         conn_info = ndev->rf_conn_info;
 920         if (!conn_info)
 921                 return -EPROTO;
 922
 923         pr_debug("target_idx %d, len %d\n", target->idx, skb->len);
 924
 925         if (!ndev->target_active_prot) {
 926                 pr_err("unable to exchange data, no active target\n");
 927                 return -EINVAL;
 928         }
 929
 930         if (test_and_set_bit(NCI_DATA_EXCHANGE, &ndev->flags))
 931                 return -EBUSY;
 932
 933         /* store cb and context to be used on receiving data */
 934         conn_info->data_exchange_cb = cb;
 935         conn_info->data_exchange_cb_context = cb_context;
 936
 937         rc = nci_send_data(ndev, NCI_STATIC_RF_CONN_ID, skb);
 938         if (rc)
 939                 clear_bit(NCI_DATA_EXCHANGE, &ndev->flags);
 940
 941         return rc;
 942 }
 943
 944 static int nci_tm_send(struct nfc_dev *nfc_dev, struct sk_buff *skb)
 945 {
 946         struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
 947         int rc;
 948
 949         rc = nci_send_data(ndev, NCI_STATIC_RF_CONN_ID, skb);
 950         if (rc)
 951                 pr_err("unable to send data\n");
 952
 953         return rc;
 954 }
 955
 956 static int nci_enable_se(struct nfc_dev *nfc_dev, u32 se_idx)
 957 {
 958         struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
 959
 960         if (ndev->ops->enable_se)
 961                 return ndev->ops->enable_se(ndev, se_idx);
 962
 963         return 0;
 964 }
 965
 966 static int nci_disable_se(struct nfc_dev *nfc_dev, u32 se_idx)
 967 {
 968         struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
 969
 970         if (ndev->ops->disable_se)
 971                 return ndev->ops->disable_se(ndev, se_idx);
 972
 973         return 0;
 974 }
 975
 976 static int nci_discover_se(struct nfc_dev *nfc_dev)
 977 {
 978         int r;
 979         struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
 980
 981         if (ndev->ops->discover_se) {
 982                 r = nci_nfcee_discover(ndev, NCI_NFCEE_DISCOVERY_ACTION_ENABLE);
 983                 if (r != NCI_STATUS_OK)
 984                         return -EPROTO;
 985
 986                 return ndev->ops->discover_se(ndev);
 987         }
 988
 989         return 0;
 990 }
 991
 992 static int nci_se_io(struct nfc_dev *nfc_dev, u32 se_idx,
 993                      u8 *apdu, size_t apdu_length,
 994                      se_io_cb_t cb, void *cb_context)
 995 {
 996         struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
 997
 998         if (ndev->ops->se_io)
 999                 return ndev->ops->se_io(ndev, se_idx, apdu,
1000                                 apdu_length, cb, cb_context);
1001
1002         return 0;
1003 }
1004
1005 static int nci_fw_download(struct nfc_dev *nfc_dev, const char *firmware_name)
1006 {
1007         struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
1008
1009         if (!ndev->ops->fw_download)
1010                 return -ENOTSUPP;
1011
1012         return ndev->ops->fw_download(ndev, firmware_name);
1013 }
1014
1015 static struct nfc_ops nci_nfc_ops = {
1016         .dev_up = nci_dev_up,
1017         .dev_down = nci_dev_down,
1018         .start_poll = nci_start_poll,
1019         .stop_poll = nci_stop_poll,
1020         .dep_link_up = nci_dep_link_up,
1021         .dep_link_down = nci_dep_link_down,
1022         .activate_target = nci_activate_target,
1023         .deactivate_target = nci_deactivate_target,
1024         .im_transceive = nci_transceive,
1025         .tm_send = nci_tm_send,
1026         .enable_se = nci_enable_se,
1027         .disable_se = nci_disable_se,
1028         .discover_se = nci_discover_se,
1029         .se_io = nci_se_io,
1030         .fw_download = nci_fw_download,
1031 };
1032
1033 /* ---- Interface to NCI drivers ---- */
1034 /**
1035  * nci_allocate_device - allocate a new nci device
1036  *
1037  * @ops: device operations
1038  * @supported_protocols: NFC protocols supported by the device
1039  */
1040 struct nci_dev *nci_allocate_device(struct nci_ops *ops,
1041                                     __u32 supported_protocols,
1042                                     int tx_headroom, int tx_tailroom)
1043 {
1044         struct nci_dev *ndev;
1045
1046         pr_debug("supported_protocols 0x%x\n", supported_protocols);
1047
1048         if (!ops->open || !ops->close || !ops->send)
1049                 return NULL;
1050
1051         if (!supported_protocols)
1052                 return NULL;
1053
1054         ndev = kzalloc(sizeof(struct nci_dev), GFP_KERNEL);
1055         if (!ndev)
1056                 return NULL;
1057
1058         ndev->ops = ops;
1059
1060         if (ops->n_prop_ops > NCI_MAX_PROPRIETARY_CMD) {
1061                 pr_err("Too many proprietary commands: %zd\n",
1062                        ops->n_prop_ops);
1063                 ops->prop_ops = NULL;
1064                 ops->n_prop_ops = 0;
1065         }
1066
1067         ndev->tx_headroom = tx_headroom;
1068         ndev->tx_tailroom = tx_tailroom;
1069         init_completion(&ndev->req_completion);
1070
1071         ndev->nfc_dev = nfc_allocate_device(&nci_nfc_ops,
1072                                             supported_protocols,
1073                                             tx_headroom + NCI_DATA_HDR_SIZE,
1074                                             tx_tailroom);
1075         if (!ndev->nfc_dev)
1076                 goto free_nci;
1077
1078         ndev->hci_dev = nci_hci_allocate(ndev);
1079         if (!ndev->hci_dev)
1080                 goto free_nfc;
1081
1082         nfc_set_drvdata(ndev->nfc_dev, ndev);
1083
1084         return ndev;
1085
1086 free_nfc:
1087         kfree(ndev->nfc_dev);
1088
1089 free_nci:
1090         kfree(ndev);
1091         return NULL;
1092 }
1093 EXPORT_SYMBOL(nci_allocate_device);
1094
1095 /**
1096  * nci_free_device - deallocate nci device
1097  *
1098  * @ndev: The nci device to deallocate
1099  */
1100 void nci_free_device(struct nci_dev *ndev)
1101 {
1102         nfc_free_device(ndev->nfc_dev);
1103         kfree(ndev);
1104 }
1105 EXPORT_SYMBOL(nci_free_device);
1106
1107 /**
1108  * nci_register_device - register a nci device in the nfc subsystem
1109  *
1110  * @dev: The nci device to register
1111  */
1112 int nci_register_device(struct nci_dev *ndev)
1113 {
1114         int rc;
1115         struct device *dev = &ndev->nfc_dev->dev;
1116         char name[32];
1117
1118         ndev->flags = 0;
1119
1120         INIT_WORK(&ndev->cmd_work, nci_cmd_work);
1121         snprintf(name, sizeof(name), "%s_nci_cmd_wq", dev_name(dev));
1122         ndev->cmd_wq = create_singlethread_workqueue(name);
1123         if (!ndev->cmd_wq) {
1124                 rc = -ENOMEM;
1125                 goto exit;
1126         }
1127
1128         INIT_WORK(&ndev->rx_work, nci_rx_work);
1129         snprintf(name, sizeof(name), "%s_nci_rx_wq", dev_name(dev));
1130         ndev->rx_wq = create_singlethread_workqueue(name);
1131         if (!ndev->rx_wq) {
1132                 rc = -ENOMEM;
1133                 goto destroy_cmd_wq_exit;
1134         }
1135
1136         INIT_WORK(&ndev->tx_work, nci_tx_work);
1137         snprintf(name, sizeof(name), "%s_nci_tx_wq", dev_name(dev));
1138         ndev->tx_wq = create_singlethread_workqueue(name);
1139         if (!ndev->tx_wq) {
1140                 rc = -ENOMEM;
1141                 goto destroy_rx_wq_exit;
1142         }
1143
1144         skb_queue_head_init(&ndev->cmd_q);
1145         skb_queue_head_init(&ndev->rx_q);
1146         skb_queue_head_init(&ndev->tx_q);
1147
1148         setup_timer(&ndev->cmd_timer, nci_cmd_timer,
1149                     (unsigned long) ndev);
1150         setup_timer(&ndev->data_timer, nci_data_timer,
1151                     (unsigned long) ndev);
1152
1153         mutex_init(&ndev->req_lock);
1154         INIT_LIST_HEAD(&ndev->conn_info_list);
1155
1156         rc = nfc_register_device(ndev->nfc_dev);
1157         if (rc)
1158                 goto destroy_rx_wq_exit;
1159
1160         goto exit;
1161
1162 destroy_rx_wq_exit:
1163         destroy_workqueue(ndev->rx_wq);
1164
1165 destroy_cmd_wq_exit:
1166         destroy_workqueue(ndev->cmd_wq);
1167
1168 exit:
1169         return rc;
1170 }
1171 EXPORT_SYMBOL(nci_register_device);
1172
1173 /**
1174  * nci_unregister_device - unregister a nci device in the nfc subsystem
1175  *
1176  * @dev: The nci device to unregister
1177  */
1178 void nci_unregister_device(struct nci_dev *ndev)
1179 {
1180         struct nci_conn_info    *conn_info, *n;
1181
1182         nci_close_device(ndev);
1183
1184         destroy_workqueue(ndev->cmd_wq);
1185         destroy_workqueue(ndev->rx_wq);
1186         destroy_workqueue(ndev->tx_wq);
1187
1188         list_for_each_entry_safe(conn_info, n, &ndev->conn_info_list, list) {
1189                 list_del(&conn_info->list);
1190                 /* conn_info is allocated with devm_kzalloc */
1191         }
1192
1193         nfc_unregister_device(ndev->nfc_dev);
1194 }
1195 EXPORT_SYMBOL(nci_unregister_device);
1196
1197 /**
1198  * nci_recv_frame - receive frame from NCI drivers
1199  *
1200  * @ndev: The nci device
1201  * @skb: The sk_buff to receive
1202  */
1203 int nci_recv_frame(struct nci_dev *ndev, struct sk_buff *skb)
1204 {
1205         pr_debug("len %d\n", skb->len);
1206
1207         if (!ndev || (!test_bit(NCI_UP, &ndev->flags) &&
1208             !test_bit(NCI_INIT, &ndev->flags))) {
1209                 kfree_skb(skb);
1210                 return -ENXIO;
1211         }
1212
1213         /* Queue frame for rx worker thread */
1214         skb_queue_tail(&ndev->rx_q, skb);
1215         queue_work(ndev->rx_wq, &ndev->rx_work);
1216
1217         return 0;
1218 }
1219 EXPORT_SYMBOL(nci_recv_frame);
1220
1221 int nci_send_frame(struct nci_dev *ndev, struct sk_buff *skb)
1222 {
1223         pr_debug("len %d\n", skb->len);
1224
1225         if (!ndev) {
1226                 kfree_skb(skb);
1227                 return -ENODEV;
1228         }
1229
1230         /* Get rid of skb owner, prior to sending to the driver. */
1231         skb_orphan(skb);
1232
1233         /* Send copy to sniffer */
1234         nfc_send_to_raw_sock(ndev->nfc_dev, skb,
1235                              RAW_PAYLOAD_NCI, NFC_DIRECTION_TX);
1236
1237         return ndev->ops->send(ndev, skb);
1238 }
1239 EXPORT_SYMBOL(nci_send_frame);
1240
1241 /* Send NCI command */
1242 int nci_send_cmd(struct nci_dev *ndev, __u16 opcode, __u8 plen, void *payload)
1243 {
1244         struct nci_ctrl_hdr *hdr;
1245         struct sk_buff *skb;
1246
1247         pr_debug("opcode 0x%x, plen %d\n", opcode, plen);
1248
1249         skb = nci_skb_alloc(ndev, (NCI_CTRL_HDR_SIZE + plen), GFP_KERNEL);
1250         if (!skb) {
1251                 pr_err("no memory for command\n");
1252                 return -ENOMEM;
1253         }
1254
1255         hdr = (struct nci_ctrl_hdr *) skb_put(skb, NCI_CTRL_HDR_SIZE);
1256         hdr->gid = nci_opcode_gid(opcode);
1257         hdr->oid = nci_opcode_oid(opcode);
1258         hdr->plen = plen;
1259
1260         nci_mt_set((__u8 *)hdr, NCI_MT_CMD_PKT);
1261         nci_pbf_set((__u8 *)hdr, NCI_PBF_LAST);
1262
1263         if (plen)
1264                 memcpy(skb_put(skb, plen), payload, plen);
1265
1266         skb_queue_tail(&ndev->cmd_q, skb);
1267         queue_work(ndev->cmd_wq, &ndev->cmd_work);
1268
1269         return 0;
1270 }
1271 EXPORT_SYMBOL(nci_send_cmd);
1272
1273 /* Proprietary commands API */
1274 static struct nci_driver_ops *ops_cmd_lookup(struct nci_driver_ops *ops,
1275                                              size_t n_ops,
1276                                              __u16 opcode)
1277 {
1278         size_t i;
1279         struct nci_driver_ops *op;
1280
1281         if (!ops || !n_ops)
1282                 return NULL;
1283
1284         for (i = 0; i < n_ops; i++) {
1285                 op = &ops[i];
1286                 if (op->opcode == opcode)
1287                         return op;
1288         }
1289
1290         return NULL;
1291 }
1292
1293 static int nci_op_rsp_packet(struct nci_dev *ndev, __u16 rsp_opcode,
1294                              struct sk_buff *skb, struct nci_driver_ops *ops,
1295                              size_t n_ops)
1296 {
1297         struct nci_driver_ops *op;
1298
1299         op = ops_cmd_lookup(ops, n_ops, rsp_opcode);
1300         if (!op || !op->rsp)
1301                 return -ENOTSUPP;
1302
1303         return op->rsp(ndev, skb);
1304 }
1305
1306 static int nci_op_ntf_packet(struct nci_dev *ndev, __u16 ntf_opcode,
1307                              struct sk_buff *skb, struct nci_driver_ops *ops,
1308                              size_t n_ops)
1309 {
1310         struct nci_driver_ops *op;
1311
1312         op = ops_cmd_lookup(ops, n_ops, ntf_opcode);
1313         if (!op || !op->ntf)
1314                 return -ENOTSUPP;
1315
1316         return op->ntf(ndev, skb);
1317 }
1318
1319 int nci_prop_rsp_packet(struct nci_dev *ndev, __u16 opcode,
1320                         struct sk_buff *skb)
1321 {
1322         return nci_op_rsp_packet(ndev, opcode, skb, ndev->ops->prop_ops,
1323                                  ndev->ops->n_prop_ops);
1324 }
1325
1326 int nci_prop_ntf_packet(struct nci_dev *ndev, __u16 opcode,
1327                         struct sk_buff *skb)
1328 {
1329         return nci_op_ntf_packet(ndev, opcode, skb, ndev->ops->prop_ops,
1330                                  ndev->ops->n_prop_ops);
1331 }
1332
1333 int nci_core_rsp_packet(struct nci_dev *ndev, __u16 opcode,
1334                         struct sk_buff *skb)
1335 {
1336         return nci_op_rsp_packet(ndev, opcode, skb, ndev->ops->core_ops,
1337                                   ndev->ops->n_core_ops);
1338 }
1339
1340 int nci_core_ntf_packet(struct nci_dev *ndev, __u16 opcode,
1341                         struct sk_buff *skb)
1342 {
1343         return nci_op_ntf_packet(ndev, opcode, skb, ndev->ops->core_ops,
1344                                  ndev->ops->n_core_ops);
1345 }
1346
1347 /* ---- NCI TX Data worker thread ---- */
1348
1349 static void nci_tx_work(struct work_struct *work)
1350 {
1351         struct nci_dev *ndev = container_of(work, struct nci_dev, tx_work);
1352         struct nci_conn_info    *conn_info;
1353         struct sk_buff *skb;
1354
1355         conn_info = nci_get_conn_info_by_conn_id(ndev, ndev->cur_conn_id);
1356         if (!conn_info)
1357                 return;
1358
1359         pr_debug("credits_cnt %d\n", atomic_read(&conn_info->credits_cnt));
1360
1361         /* Send queued tx data */
1362         while (atomic_read(&conn_info->credits_cnt)) {
1363                 skb = skb_dequeue(&ndev->tx_q);
1364                 if (!skb)
1365                         return;
1366
1367                 /* Check if data flow control is used */
1368                 if (atomic_read(&conn_info->credits_cnt) !=
1369                     NCI_DATA_FLOW_CONTROL_NOT_USED)
1370                         atomic_dec(&conn_info->credits_cnt);
1371
1372                 pr_debug("NCI TX: MT=data, PBF=%d, conn_id=%d, plen=%d\n",
1373                          nci_pbf(skb->data),
1374                          nci_conn_id(skb->data),
1375                          nci_plen(skb->data));
1376
1377                 nci_send_frame(ndev, skb);
1378
1379                 mod_timer(&ndev->data_timer,
1380                           jiffies + msecs_to_jiffies(NCI_DATA_TIMEOUT));
1381         }
1382 }
1383
1384 /* ----- NCI RX worker thread (data & control) ----- */
1385
1386 static void nci_rx_work(struct work_struct *work)
1387 {
1388         struct nci_dev *ndev = container_of(work, struct nci_dev, rx_work);
1389         struct sk_buff *skb;
1390
1391         while ((skb = skb_dequeue(&ndev->rx_q))) {
1392
1393                 /* Send copy to sniffer */
1394                 nfc_send_to_raw_sock(ndev->nfc_dev, skb,
1395                                      RAW_PAYLOAD_NCI, NFC_DIRECTION_RX);
1396
1397                 /* Process frame */
1398                 switch (nci_mt(skb->data)) {
1399                 case NCI_MT_RSP_PKT:
1400                         nci_rsp_packet(ndev, skb);
1401                         break;
1402
1403                 case NCI_MT_NTF_PKT:
1404                         nci_ntf_packet(ndev, skb);
1405                         break;
1406
1407                 case NCI_MT_DATA_PKT:
1408                         nci_rx_data_packet(ndev, skb);
1409                         break;
1410
1411                 default:
1412                         pr_err("unknown MT 0x%x\n", nci_mt(skb->data));
1413                         kfree_skb(skb);
1414                         break;
1415                 }
1416         }
1417
1418         /* check if a data exchange timout has occurred */
1419         if (test_bit(NCI_DATA_EXCHANGE_TO, &ndev->flags)) {
1420                 /* complete the data exchange transaction, if exists */
1421                 if (test_bit(NCI_DATA_EXCHANGE, &ndev->flags))
1422                         nci_data_exchange_complete(ndev, NULL,
1423                                                    ndev->cur_conn_id,
1424                                                    -ETIMEDOUT);
1425
1426                 clear_bit(NCI_DATA_EXCHANGE_TO, &ndev->flags);
1427         }
1428 }
1429
1430 /* ----- NCI TX CMD worker thread ----- */
1431
1432 static void nci_cmd_work(struct work_struct *work)
1433 {
1434         struct nci_dev *ndev = container_of(work, struct nci_dev, cmd_work);
1435         struct sk_buff *skb;
1436
1437         pr_debug("cmd_cnt %d\n", atomic_read(&ndev->cmd_cnt));
1438
1439         /* Send queued command */
1440         if (atomic_read(&ndev->cmd_cnt)) {
1441                 skb = skb_dequeue(&ndev->cmd_q);
1442                 if (!skb)
1443                         return;
1444
1445                 atomic_dec(&ndev->cmd_cnt);
1446
1447                 pr_debug("NCI TX: MT=cmd, PBF=%d, GID=0x%x, OID=0x%x, plen=%d\n",
1448                          nci_pbf(skb->data),
1449                          nci_opcode_gid(nci_opcode(skb->data)),
1450                          nci_opcode_oid(nci_opcode(skb->data)),
1451                          nci_plen(skb->data));
1452
1453                 nci_send_frame(ndev, skb);
1454
1455                 mod_timer(&ndev->cmd_timer,
1456                           jiffies + msecs_to_jiffies(NCI_CMD_TIMEOUT));
1457         }
1458 }
1459
1460 MODULE_LICENSE("GPL");