net/nfc/hci/core.c

   1 /*
   2  * Copyright (C) 2012  Intel Corporation. All rights reserved.
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with this program; if not, write to the
  16  * Free Software Foundation, Inc.,
  17  * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  18  */
  19
  20 #define pr_fmt(fmt) "hci: %s: " fmt, __func__
  21
  22 #include <linux/init.h>
  23 #include <linux/kernel.h>
  24 #include <linux/module.h>
  25 #include <linux/nfc.h>
  26
  27 #include <net/nfc/nfc.h>
  28 #include <net/nfc/hci.h>
  29
  30 #include "hci.h"
  31
  32 /* Largest headroom needed for outgoing HCI commands */
  33 #define HCI_CMDS_HEADROOM 1
  34
  35 static void nfc_hci_msg_tx_work(struct work_struct *work)
  36 {
  37         struct nfc_hci_dev *hdev = container_of(work, struct nfc_hci_dev,
  38                                                 msg_tx_work);
  39         struct hci_msg *msg;
  40         struct sk_buff *skb;
  41         int r = 0;
  42
  43         mutex_lock(&hdev->msg_tx_mutex);
  44
  45         if (hdev->cmd_pending_msg) {
  46                 if (timer_pending(&hdev->cmd_timer) == 0) {
  47                         if (hdev->cmd_pending_msg->cb)
  48                                 hdev->cmd_pending_msg->cb(hdev,
  49                                                           NFC_HCI_ANY_E_TIMEOUT,
  50                                                           NULL,
  51                                                           hdev->
  52                                                           cmd_pending_msg->
  53                                                           cb_context);
  54                         kfree(hdev->cmd_pending_msg);
  55                         hdev->cmd_pending_msg = NULL;
  56                 } else
  57                         goto exit;
  58         }
  59
  60 next_msg:
  61         if (list_empty(&hdev->msg_tx_queue))
  62                 goto exit;
  63
  64         msg = list_first_entry(&hdev->msg_tx_queue, struct hci_msg, msg_l);
  65         list_del(&msg->msg_l);
  66
  67         pr_debug("msg_tx_queue has a cmd to send\n");
  68         while ((skb = skb_dequeue(&msg->msg_frags)) != NULL) {
  69                 r = hdev->ops->xmit(hdev, skb);
  70                 if (r < 0) {
  71                         kfree_skb(skb);
  72                         skb_queue_purge(&msg->msg_frags);
  73                         if (msg->cb)
  74                                 msg->cb(hdev, NFC_HCI_ANY_E_NOK, NULL,
  75                                         msg->cb_context);
  76                         kfree(msg);
  77                         break;
  78                 }
  79         }
  80
  81         if (r)
  82                 goto next_msg;
  83
  84         if (msg->wait_response == false) {
  85                 kfree(msg);
  86                 goto next_msg;
  87         }
  88
  89         hdev->cmd_pending_msg = msg;
  90         mod_timer(&hdev->cmd_timer, jiffies +
  91                   msecs_to_jiffies(hdev->cmd_pending_msg->completion_delay));
  92
  93 exit:
  94         mutex_unlock(&hdev->msg_tx_mutex);
  95 }
  96
  97 static void nfc_hci_msg_rx_work(struct work_struct *work)
  98 {
  99         struct nfc_hci_dev *hdev = container_of(work, struct nfc_hci_dev,
 100                                                 msg_rx_work);
 101         struct sk_buff *skb;
 102         struct hcp_message *message;
 103         u8 pipe;
 104         u8 type;
 105         u8 instruction;
 106
 107         while ((skb = skb_dequeue(&hdev->msg_rx_queue)) != NULL) {
 108                 pipe = skb->data[0];
 109                 skb_pull(skb, NFC_HCI_HCP_PACKET_HEADER_LEN);
 110                 message = (struct hcp_message *)skb->data;
 111                 type = HCP_MSG_GET_TYPE(message->header);
 112                 instruction = HCP_MSG_GET_CMD(message->header);
 113                 skb_pull(skb, NFC_HCI_HCP_MESSAGE_HEADER_LEN);
 114
 115                 nfc_hci_hcp_message_rx(hdev, pipe, type, instruction, skb);
 116         }
 117 }
 118
 119 void nfc_hci_resp_received(struct nfc_hci_dev *hdev, u8 result,
 120                            struct sk_buff *skb)
 121 {
 122         mutex_lock(&hdev->msg_tx_mutex);
 123
 124         if (hdev->cmd_pending_msg == NULL) {
 125                 kfree_skb(skb);
 126                 goto exit;
 127         }
 128
 129         del_timer_sync(&hdev->cmd_timer);
 130
 131         if (hdev->cmd_pending_msg->cb)
 132                 hdev->cmd_pending_msg->cb(hdev, result, skb,
 133                                           hdev->cmd_pending_msg->cb_context);
 134         else
 135                 kfree_skb(skb);
 136
 137         kfree(hdev->cmd_pending_msg);
 138         hdev->cmd_pending_msg = NULL;
 139
 140         queue_work(hdev->msg_tx_wq, &hdev->msg_tx_work);
 141
 142 exit:
 143         mutex_unlock(&hdev->msg_tx_mutex);
 144 }
 145
 146 void nfc_hci_cmd_received(struct nfc_hci_dev *hdev, u8 pipe, u8 cmd,
 147                           struct sk_buff *skb)
 148 {
 149         kfree_skb(skb);
 150 }
 151
 152 static u32 nfc_hci_sak_to_protocol(u8 sak)
 153 {
 154         switch (NFC_HCI_TYPE_A_SEL_PROT(sak)) {
 155         case NFC_HCI_TYPE_A_SEL_PROT_MIFARE:
 156                 return NFC_PROTO_MIFARE_MASK;
 157         case NFC_HCI_TYPE_A_SEL_PROT_ISO14443:
 158                 return NFC_PROTO_ISO14443_MASK;
 159         case NFC_HCI_TYPE_A_SEL_PROT_DEP:
 160                 return NFC_PROTO_NFC_DEP_MASK;
 161         case NFC_HCI_TYPE_A_SEL_PROT_ISO14443_DEP:
 162                 return NFC_PROTO_ISO14443_MASK | NFC_PROTO_NFC_DEP_MASK;
 163         default:
 164                 return 0xffffffff;
 165         }
 166 }
 167
 168 static int nfc_hci_target_discovered(struct nfc_hci_dev *hdev, u8 gate)
 169 {
 170         struct nfc_target *targets;
 171         struct sk_buff *atqa_skb = NULL;
 172         struct sk_buff *sak_skb = NULL;
 173         int r;
 174
 175         pr_debug("from gate %d\n", gate);
 176
 177         targets = kzalloc(sizeof(struct nfc_target), GFP_KERNEL);
 178         if (targets == NULL)
 179                 return -ENOMEM;
 180
 181         switch (gate) {
 182         case NFC_HCI_RF_READER_A_GATE:
 183                 r = nfc_hci_get_param(hdev, NFC_HCI_RF_READER_A_GATE,
 184                                       NFC_HCI_RF_READER_A_ATQA, &atqa_skb);
 185                 if (r < 0)
 186                         goto exit;
 187
 188                 r = nfc_hci_get_param(hdev, NFC_HCI_RF_READER_A_GATE,
 189                                       NFC_HCI_RF_READER_A_SAK, &sak_skb);
 190                 if (r < 0)
 191                         goto exit;
 192
 193                 if (atqa_skb->len != 2 || sak_skb->len != 1) {
 194                         r = -EPROTO;
 195                         goto exit;
 196                 }
 197
 198                 targets->supported_protocols =
 199                                 nfc_hci_sak_to_protocol(sak_skb->data[0]);
 200                 if (targets->supported_protocols == 0xffffffff) {
 201                         r = -EPROTO;
 202                         goto exit;
 203                 }
 204
 205                 targets->sens_res = be16_to_cpu(*(u16 *)atqa_skb->data);
 206                 targets->sel_res = sak_skb->data[0];
 207
 208                 if (hdev->ops->complete_target_discovered) {
 209                         r = hdev->ops->complete_target_discovered(hdev, gate,
 210                                                                   targets);
 211                         if (r < 0)
 212                                 goto exit;
 213                 }
 214                 break;
 215         case NFC_HCI_RF_READER_B_GATE:
 216                 targets->supported_protocols = NFC_PROTO_ISO14443_MASK;
 217                 break;
 218         default:
 219                 if (hdev->ops->target_from_gate)
 220                         r = hdev->ops->target_from_gate(hdev, gate, targets);
 221                 else
 222                         r = -EPROTO;
 223                 if (r < 0)
 224                         goto exit;
 225
 226                 if (hdev->ops->complete_target_discovered) {
 227                         r = hdev->ops->complete_target_discovered(hdev, gate,
 228                                                                   targets);
 229                         if (r < 0)
 230                                 goto exit;
 231                 }
 232                 break;
 233         }
 234
 235         targets->hci_reader_gate = gate;
 236
 237         r = nfc_targets_found(hdev->ndev, targets, 1);
 238
 239 exit:
 240         kfree(targets);
 241         kfree_skb(atqa_skb);
 242         kfree_skb(sak_skb);
 243
 244         return r;
 245 }
 246
 247 void nfc_hci_event_received(struct nfc_hci_dev *hdev, u8 pipe, u8 event,
 248                             struct sk_buff *skb)
 249 {
 250         int r = 0;
 251
 252         switch (event) {
 253         case NFC_HCI_EVT_TARGET_DISCOVERED:
 254                 if (skb->len < 1) {     /* no status data? */
 255                         r = -EPROTO;
 256                         goto exit;
 257                 }
 258
 259                 if (skb->data[0] == 3) {
 260                         /* TODO: Multiple targets in field, none activated
 261                          * poll is supposedly stopped, but there is no
 262                          * single target to activate, so nothing to report
 263                          * up.
 264                          * if we need to restart poll, we must save the
 265                          * protocols from the initial poll and reuse here.
 266                          */
 267                 }
 268
 269                 if (skb->data[0] != 0) {
 270                         r = -EPROTO;
 271                         goto exit;
 272                 }
 273
 274                 r = nfc_hci_target_discovered(hdev,
 275                                               nfc_hci_pipe2gate(hdev, pipe));
 276                 break;
 277         default:
 278                 /* TODO: Unknown events are hardware specific
 279                  * pass them to the driver (needs a new hci_ops) */
 280                 break;
 281         }
 282
 283 exit:
 284         kfree_skb(skb);
 285
 286         if (r) {
 287                 /* TODO: There was an error dispatching the event,
 288                  * how to propagate up to nfc core?
 289                  */
 290         }
 291 }
 292
 293 static void nfc_hci_cmd_timeout(unsigned long data)
 294 {
 295         struct nfc_hci_dev *hdev = (struct nfc_hci_dev *)data;
 296
 297         queue_work(hdev->msg_tx_wq, &hdev->msg_tx_work);
 298 }
 299
 300 static int hci_dev_connect_gates(struct nfc_hci_dev *hdev, u8 gate_count,
 301                                  u8 gates[])
 302 {
 303         int r;
 304         u8 *p = gates;
 305         while (gate_count--) {
 306                 r = nfc_hci_connect_gate(hdev, NFC_HCI_HOST_CONTROLLER_ID, *p);
 307                 if (r < 0)
 308                         return r;
 309                 p++;
 310         }
 311
 312         return 0;
 313 }
 314
 315 static int hci_dev_session_init(struct nfc_hci_dev *hdev)
 316 {
 317         struct sk_buff *skb = NULL;
 318         int r;
 319         u8 hci_gates[] = {      /* NFC_HCI_ADMIN_GATE MUST be first */
 320                 NFC_HCI_ADMIN_GATE, NFC_HCI_LOOPBACK_GATE,
 321                 NFC_HCI_ID_MGMT_GATE, NFC_HCI_LINK_MGMT_GATE,
 322                 NFC_HCI_RF_READER_B_GATE, NFC_HCI_RF_READER_A_GATE
 323         };
 324
 325         r = nfc_hci_connect_gate(hdev, NFC_HCI_HOST_CONTROLLER_ID,
 326                                  NFC_HCI_ADMIN_GATE);
 327         if (r < 0)
 328                 goto exit;
 329
 330         r = nfc_hci_get_param(hdev, NFC_HCI_ADMIN_GATE,
 331                               NFC_HCI_ADMIN_SESSION_IDENTITY, &skb);
 332         if (r < 0)
 333                 goto disconnect_all;
 334
 335         if (skb->len && skb->len == strlen(hdev->init_data.session_id))
 336                 if (memcmp(hdev->init_data.session_id, skb->data,
 337                            skb->len) == 0) {
 338                         /* TODO ELa: restore gate<->pipe table from
 339                          * some TBD location.
 340                          * note: it doesn't seem possible to get the chip
 341                          * currently open gate/pipe table.
 342                          * It is only possible to obtain the supported
 343                          * gate list.
 344                          */
 345
 346                         /* goto exit
 347                          * For now, always do a full initialization */
 348                 }
 349
 350         r = nfc_hci_disconnect_all_gates(hdev);
 351         if (r < 0)
 352                 goto exit;
 353
 354         r = hci_dev_connect_gates(hdev, sizeof(hci_gates), hci_gates);
 355         if (r < 0)
 356                 goto disconnect_all;
 357
 358         r = hci_dev_connect_gates(hdev, hdev->init_data.gate_count,
 359                                   hdev->init_data.gates);
 360         if (r < 0)
 361                 goto disconnect_all;
 362
 363         r = nfc_hci_set_param(hdev, NFC_HCI_ADMIN_GATE,
 364                               NFC_HCI_ADMIN_SESSION_IDENTITY,
 365                               hdev->init_data.session_id,
 366                               strlen(hdev->init_data.session_id));
 367         if (r == 0)
 368                 goto exit;
 369
 370 disconnect_all:
 371         nfc_hci_disconnect_all_gates(hdev);
 372
 373 exit:
 374         if (skb)
 375                 kfree_skb(skb);
 376
 377         return r;
 378 }
 379
 380 static int hci_dev_version(struct nfc_hci_dev *hdev)
 381 {
 382         int r;
 383         struct sk_buff *skb;
 384
 385         r = nfc_hci_get_param(hdev, NFC_HCI_ID_MGMT_GATE,
 386                               NFC_HCI_ID_MGMT_VERSION_SW, &skb);
 387         if (r < 0)
 388                 return r;
 389
 390         if (skb->len != 3) {
 391                 kfree_skb(skb);
 392                 return -EINVAL;
 393         }
 394
 395         hdev->sw_romlib = (skb->data[0] & 0xf0) >> 4;
 396         hdev->sw_patch = skb->data[0] & 0x0f;
 397         hdev->sw_flashlib_major = skb->data[1];
 398         hdev->sw_flashlib_minor = skb->data[2];
 399
 400         kfree_skb(skb);
 401
 402         r = nfc_hci_get_param(hdev, NFC_HCI_ID_MGMT_GATE,
 403                               NFC_HCI_ID_MGMT_VERSION_HW, &skb);
 404         if (r < 0)
 405                 return r;
 406
 407         if (skb->len != 3) {
 408                 kfree_skb(skb);
 409                 return -EINVAL;
 410         }
 411
 412         hdev->hw_derivative = (skb->data[0] & 0xe0) >> 5;
 413         hdev->hw_version = skb->data[0] & 0x1f;
 414         hdev->hw_mpw = (skb->data[1] & 0xc0) >> 6;
 415         hdev->hw_software = skb->data[1] & 0x3f;
 416         hdev->hw_bsid = skb->data[2];
 417
 418         kfree_skb(skb);
 419
 420         pr_info("SOFTWARE INFO:\n");
 421         pr_info("RomLib         : %d\n", hdev->sw_romlib);
 422         pr_info("Patch          : %d\n", hdev->sw_patch);
 423         pr_info("FlashLib Major : %d\n", hdev->sw_flashlib_major);
 424         pr_info("FlashLib Minor : %d\n", hdev->sw_flashlib_minor);
 425         pr_info("HARDWARE INFO:\n");
 426         pr_info("Derivative     : %d\n", hdev->hw_derivative);
 427         pr_info("HW Version     : %d\n", hdev->hw_version);
 428         pr_info("#MPW           : %d\n", hdev->hw_mpw);
 429         pr_info("Software       : %d\n", hdev->hw_software);
 430         pr_info("BSID Version   : %d\n", hdev->hw_bsid);
 431
 432         return 0;
 433 }
 434
 435 static int hci_dev_up(struct nfc_dev *nfc_dev)
 436 {
 437         struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
 438         int r = 0;
 439
 440         if (hdev->ops->open) {
 441                 r = hdev->ops->open(hdev);
 442                 if (r < 0)
 443                         return r;
 444         }
 445
 446         r = hci_dev_session_init(hdev);
 447         if (r < 0)
 448                 goto exit;
 449
 450         r = nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
 451                                NFC_HCI_EVT_END_OPERATION, NULL, 0);
 452         if (r < 0)
 453                 goto exit;
 454
 455         if (hdev->ops->hci_ready) {
 456                 r = hdev->ops->hci_ready(hdev);
 457                 if (r < 0)
 458                         goto exit;
 459         }
 460
 461         r = hci_dev_version(hdev);
 462         if (r < 0)
 463                 goto exit;
 464
 465 exit:
 466         if (r < 0)
 467                 if (hdev->ops->close)
 468                         hdev->ops->close(hdev);
 469         return r;
 470 }
 471
 472 static int hci_dev_down(struct nfc_dev *nfc_dev)
 473 {
 474         struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
 475
 476         if (hdev->ops->close)
 477                 hdev->ops->close(hdev);
 478
 479         memset(hdev->gate2pipe, NFC_HCI_INVALID_PIPE, sizeof(hdev->gate2pipe));
 480
 481         return 0;
 482 }
 483
 484 static int hci_start_poll(struct nfc_dev *nfc_dev, u32 protocols)
 485 {
 486         struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
 487
 488         if (hdev->ops->start_poll)
 489                 return hdev->ops->start_poll(hdev, protocols);
 490         else
 491                 return nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
 492                                        NFC_HCI_EVT_READER_REQUESTED, NULL, 0);
 493 }
 494
 495 static void hci_stop_poll(struct nfc_dev *nfc_dev)
 496 {
 497         struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
 498
 499         nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
 500                            NFC_HCI_EVT_END_OPERATION, NULL, 0);
 501 }
 502
 503 static int hci_activate_target(struct nfc_dev *nfc_dev,
 504                                struct nfc_target *target, u32 protocol)
 505 {
 506         return 0;
 507 }
 508
 509 static void hci_deactivate_target(struct nfc_dev *nfc_dev,
 510                                   struct nfc_target *target)
 511 {
 512 }
 513
 514 static int hci_data_exchange(struct nfc_dev *nfc_dev, struct nfc_target *target,
 515                              struct sk_buff *skb, data_exchange_cb_t cb,
 516                              void *cb_context)
 517 {
 518         struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
 519         int r;
 520         struct sk_buff *res_skb = NULL;
 521
 522         pr_debug("target_idx=%d\n", target->idx);
 523
 524         switch (target->hci_reader_gate) {
 525         case NFC_HCI_RF_READER_A_GATE:
 526         case NFC_HCI_RF_READER_B_GATE:
 527                 if (hdev->ops->data_exchange) {
 528                         r = hdev->ops->data_exchange(hdev, target, skb,
 529                                                      &res_skb);
 530                         if (r <= 0)     /* handled */
 531                                 break;
 532                 }
 533
 534                 *skb_push(skb, 1) = 0;  /* CTR, see spec:10.2.2.1 */
 535                 r = nfc_hci_send_cmd(hdev, target->hci_reader_gate,
 536                                      NFC_HCI_WR_XCHG_DATA,
 537                                      skb->data, skb->len, &res_skb);
 538                 /*
 539                  * TODO: Check RF Error indicator to make sure data is valid.
 540                  * It seems that HCI cmd can complete without error, but data
 541                  * can be invalid if an RF error occured? Ignore for now.
 542                  */
 543                 if (r == 0)
 544                         skb_trim(res_skb, res_skb->len - 1); /* RF Err ind */
 545                 break;
 546         default:
 547                 if (hdev->ops->data_exchange) {
 548                         r = hdev->ops->data_exchange(hdev, target, skb,
 549                                                      &res_skb);
 550                         if (r == 1)
 551                                 r = -ENOTSUPP;
 552                 }
 553                 else
 554                         r = -ENOTSUPP;
 555         }
 556
 557         kfree_skb(skb);
 558
 559         cb(cb_context, res_skb, r);
 560
 561         return 0;
 562 }
 563
 564 static int hci_check_presence(struct nfc_dev *nfc_dev,
 565                               struct nfc_target *target)
 566 {
 567         struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
 568
 569         if (hdev->ops->check_presence)
 570                 return hdev->ops->check_presence(hdev, target);
 571
 572         return 0;
 573 }
 574
 575 static struct nfc_ops hci_nfc_ops = {
 576         .dev_up = hci_dev_up,
 577         .dev_down = hci_dev_down,
 578         .start_poll = hci_start_poll,
 579         .stop_poll = hci_stop_poll,
 580         .activate_target = hci_activate_target,
 581         .deactivate_target = hci_deactivate_target,
 582         .data_exchange = hci_data_exchange,
 583         .check_presence = hci_check_presence,
 584 };
 585
 586 struct nfc_hci_dev *nfc_hci_allocate_device(struct nfc_hci_ops *ops,
 587                                             struct nfc_hci_init_data *init_data,
 588                                             u32 protocols,
 589                                             int tx_headroom,
 590                                             int tx_tailroom,
 591                                             int max_link_payload)
 592 {
 593         struct nfc_hci_dev *hdev;
 594
 595         if (ops->xmit == NULL)
 596                 return NULL;
 597
 598         if (protocols == 0)
 599                 return NULL;
 600
 601         hdev = kzalloc(sizeof(struct nfc_hci_dev), GFP_KERNEL);
 602         if (hdev == NULL)
 603                 return NULL;
 604
 605         hdev->ndev = nfc_allocate_device(&hci_nfc_ops, protocols,
 606                                          tx_headroom + HCI_CMDS_HEADROOM,
 607                                          tx_tailroom);
 608         if (!hdev->ndev) {
 609                 kfree(hdev);
 610                 return NULL;
 611         }
 612
 613         hdev->ops = ops;
 614         hdev->max_data_link_payload = max_link_payload;
 615         hdev->init_data = *init_data;
 616
 617         nfc_set_drvdata(hdev->ndev, hdev);
 618
 619         memset(hdev->gate2pipe, NFC_HCI_INVALID_PIPE, sizeof(hdev->gate2pipe));
 620
 621         return hdev;
 622 }
 623 EXPORT_SYMBOL(nfc_hci_allocate_device);
 624
 625 void nfc_hci_free_device(struct nfc_hci_dev *hdev)
 626 {
 627         nfc_free_device(hdev->ndev);
 628         kfree(hdev);
 629 }
 630 EXPORT_SYMBOL(nfc_hci_free_device);
 631
 632 int nfc_hci_register_device(struct nfc_hci_dev *hdev)
 633 {
 634         struct device *dev = &hdev->ndev->dev;
 635         const char *devname = dev_name(dev);
 636         char name[32];
 637         int r = 0;
 638
 639         mutex_init(&hdev->msg_tx_mutex);
 640
 641         INIT_LIST_HEAD(&hdev->msg_tx_queue);
 642
 643         INIT_WORK(&hdev->msg_tx_work, nfc_hci_msg_tx_work);
 644         snprintf(name, sizeof(name), "%s_hci_msg_tx_wq", devname);
 645         hdev->msg_tx_wq = alloc_workqueue(name, WQ_NON_REENTRANT | WQ_UNBOUND |
 646                                           WQ_MEM_RECLAIM, 1);
 647         if (hdev->msg_tx_wq == NULL) {
 648                 r = -ENOMEM;
 649                 goto exit;
 650         }
 651
 652         init_timer(&hdev->cmd_timer);
 653         hdev->cmd_timer.data = (unsigned long)hdev;
 654         hdev->cmd_timer.function = nfc_hci_cmd_timeout;
 655
 656         skb_queue_head_init(&hdev->rx_hcp_frags);
 657
 658         INIT_WORK(&hdev->msg_rx_work, nfc_hci_msg_rx_work);
 659         snprintf(name, sizeof(name), "%s_hci_msg_rx_wq", devname);
 660         hdev->msg_rx_wq = alloc_workqueue(name, WQ_NON_REENTRANT | WQ_UNBOUND |
 661                                           WQ_MEM_RECLAIM, 1);
 662         if (hdev->msg_rx_wq == NULL) {
 663                 r = -ENOMEM;
 664                 goto exit;
 665         }
 666
 667         skb_queue_head_init(&hdev->msg_rx_queue);
 668
 669         r = nfc_register_device(hdev->ndev);
 670
 671 exit:
 672         if (r < 0) {
 673                 if (hdev->msg_tx_wq)
 674                         destroy_workqueue(hdev->msg_tx_wq);
 675                 if (hdev->msg_rx_wq)
 676                         destroy_workqueue(hdev->msg_rx_wq);
 677         }
 678
 679         return r;
 680 }
 681 EXPORT_SYMBOL(nfc_hci_register_device);
 682
 683 void nfc_hci_unregister_device(struct nfc_hci_dev *hdev)
 684 {
 685         struct hci_msg *msg;
 686
 687         skb_queue_purge(&hdev->rx_hcp_frags);
 688         skb_queue_purge(&hdev->msg_rx_queue);
 689
 690         while ((msg = list_first_entry(&hdev->msg_tx_queue, struct hci_msg,
 691                                        msg_l)) != NULL) {
 692                 list_del(&msg->msg_l);
 693                 skb_queue_purge(&msg->msg_frags);
 694                 kfree(msg);
 695         }
 696
 697         del_timer_sync(&hdev->cmd_timer);
 698
 699         nfc_unregister_device(hdev->ndev);
 700
 701         destroy_workqueue(hdev->msg_tx_wq);
 702
 703         destroy_workqueue(hdev->msg_rx_wq);
 704 }
 705 EXPORT_SYMBOL(nfc_hci_unregister_device);
 706
 707 void nfc_hci_set_clientdata(struct nfc_hci_dev *hdev, void *clientdata)
 708 {
 709         hdev->clientdata = clientdata;
 710 }
 711 EXPORT_SYMBOL(nfc_hci_set_clientdata);
 712
 713 void *nfc_hci_get_clientdata(struct nfc_hci_dev *hdev)
 714 {
 715         return hdev->clientdata;
 716 }
 717 EXPORT_SYMBOL(nfc_hci_get_clientdata);
 718
 719 void nfc_hci_recv_frame(struct nfc_hci_dev *hdev, struct sk_buff *skb)
 720 {
 721         struct hcp_packet *packet;
 722         u8 type;
 723         u8 instruction;
 724         struct sk_buff *hcp_skb;
 725         u8 pipe;
 726         struct sk_buff *frag_skb;
 727         int msg_len;
 728
 729         if (skb == NULL) {
 730                 /* TODO ELa: lower layer had permanent failure, need to
 731                  * propagate that up
 732                  */
 733
 734                 skb_queue_purge(&hdev->rx_hcp_frags);
 735
 736                 return;
 737         }
 738
 739         packet = (struct hcp_packet *)skb->data;
 740         if ((packet->header & ~NFC_HCI_FRAGMENT) == 0) {
 741                 skb_queue_tail(&hdev->rx_hcp_frags, skb);
 742                 return;
 743         }
 744
 745         /* it's the last fragment. Does it need re-aggregation? */
 746         if (skb_queue_len(&hdev->rx_hcp_frags)) {
 747                 pipe = packet->header & NFC_HCI_FRAGMENT;
 748                 skb_queue_tail(&hdev->rx_hcp_frags, skb);
 749
 750                 msg_len = 0;
 751                 skb_queue_walk(&hdev->rx_hcp_frags, frag_skb) {
 752                         msg_len += (frag_skb->len -
 753                                     NFC_HCI_HCP_PACKET_HEADER_LEN);
 754                 }
 755
 756                 hcp_skb = nfc_alloc_recv_skb(NFC_HCI_HCP_PACKET_HEADER_LEN +
 757                                              msg_len, GFP_KERNEL);
 758                 if (hcp_skb == NULL) {
 759                         /* TODO ELa: cannot deliver HCP message. How to
 760                          * propagate error up?
 761                          */
 762                 }
 763
 764                 *skb_put(hcp_skb, NFC_HCI_HCP_PACKET_HEADER_LEN) = pipe;
 765
 766                 skb_queue_walk(&hdev->rx_hcp_frags, frag_skb) {
 767                         msg_len = frag_skb->len - NFC_HCI_HCP_PACKET_HEADER_LEN;
 768                         memcpy(skb_put(hcp_skb, msg_len),
 769                                frag_skb->data + NFC_HCI_HCP_PACKET_HEADER_LEN,
 770                                msg_len);
 771                 }
 772
 773                 skb_queue_purge(&hdev->rx_hcp_frags);
 774         } else {
 775                 packet->header &= NFC_HCI_FRAGMENT;
 776                 hcp_skb = skb;
 777         }
 778
 779         /* if this is a response, dispatch immediately to
 780          * unblock waiting cmd context. Otherwise, enqueue to dispatch
 781          * in separate context where handler can also execute command.
 782          */
 783         packet = (struct hcp_packet *)hcp_skb->data;
 784         type = HCP_MSG_GET_TYPE(packet->message.header);
 785         if (type == NFC_HCI_HCP_RESPONSE) {
 786                 pipe = packet->header;
 787                 instruction = HCP_MSG_GET_CMD(packet->message.header);
 788                 skb_pull(hcp_skb, NFC_HCI_HCP_PACKET_HEADER_LEN +
 789                          NFC_HCI_HCP_MESSAGE_HEADER_LEN);
 790                 nfc_hci_hcp_message_rx(hdev, pipe, type, instruction, hcp_skb);
 791         } else {
 792                 skb_queue_tail(&hdev->msg_rx_queue, hcp_skb);
 793                 queue_work(hdev->msg_rx_wq, &hdev->msg_rx_work);
 794         }
 795 }
 796 EXPORT_SYMBOL(nfc_hci_recv_frame);
 797
 798 MODULE_LICENSE("GPL");