diff options
author | John W. Linville <linville@tuxdriver.com> | 2012-04-18 14:17:13 -0400 |
---|---|---|
committer | John W. Linville <linville@tuxdriver.com> | 2012-04-18 14:27:48 -0400 |
commit | 59ef43e681d103a51c3727dad0315e093f07ec61 (patch) | |
tree | 87f6320f1440ce3ce6c0c15ad3cef8bc98186f88 /net/nfc | |
parent | 91fbe33034c184c6a60e31c2207a2f7ec2f180dc (diff) | |
parent | b5abcf0219263f4e961dca71cbe26e06c5b0ee68 (diff) |
Merge branch 'master' of git://git.kernel.org/pub/scm/linux/kernel/git/linville/wireless-next into for-davem
Conflicts:
drivers/net/wireless/iwlwifi/iwl-testmode.c
include/net/nfc/nfc.h
net/nfc/netlink.c
net/wireless/nl80211.c
Diffstat (limited to 'net/nfc')
-rw-r--r-- | net/nfc/Kconfig | 1 | ||||
-rw-r--r-- | net/nfc/Makefile | 1 | ||||
-rw-r--r-- | net/nfc/core.c | 140 | ||||
-rw-r--r-- | net/nfc/hci/Kconfig | 16 | ||||
-rw-r--r-- | net/nfc/hci/Makefile | 8 | ||||
-rw-r--r-- | net/nfc/hci/command.c | 354 | ||||
-rw-r--r-- | net/nfc/hci/core.c | 830 | ||||
-rw-r--r-- | net/nfc/hci/hci.h | 139 | ||||
-rw-r--r-- | net/nfc/hci/hcp.c | 156 | ||||
-rw-r--r-- | net/nfc/hci/shdlc.c | 945 | ||||
-rw-r--r-- | net/nfc/llcp/commands.c | 8 | ||||
-rw-r--r-- | net/nfc/llcp/llcp.c | 21 | ||||
-rw-r--r-- | net/nfc/nci/core.c | 2 | ||||
-rw-r--r-- | net/nfc/nci/ntf.c | 11 | ||||
-rw-r--r-- | net/nfc/netlink.c | 31 | ||||
-rw-r--r-- | net/nfc/nfc.h | 1 | ||||
-rw-r--r-- | net/nfc/rawsock.c | 6 |
17 files changed, 2654 insertions, 16 deletions
diff --git a/net/nfc/Kconfig b/net/nfc/Kconfig index 44c865b86d6f..8d8d9bc4b6ff 100644 --- a/net/nfc/Kconfig +++ b/net/nfc/Kconfig | |||
@@ -14,6 +14,7 @@ menuconfig NFC | |||
14 | be called nfc. | 14 | be called nfc. |
15 | 15 | ||
16 | source "net/nfc/nci/Kconfig" | 16 | source "net/nfc/nci/Kconfig" |
17 | source "net/nfc/hci/Kconfig" | ||
17 | source "net/nfc/llcp/Kconfig" | 18 | source "net/nfc/llcp/Kconfig" |
18 | 19 | ||
19 | source "drivers/nfc/Kconfig" | 20 | source "drivers/nfc/Kconfig" |
diff --git a/net/nfc/Makefile b/net/nfc/Makefile index 7b4a6dcfa566..d1a117c2c401 100644 --- a/net/nfc/Makefile +++ b/net/nfc/Makefile | |||
@@ -4,6 +4,7 @@ | |||
4 | 4 | ||
5 | obj-$(CONFIG_NFC) += nfc.o | 5 | obj-$(CONFIG_NFC) += nfc.o |
6 | obj-$(CONFIG_NFC_NCI) += nci/ | 6 | obj-$(CONFIG_NFC_NCI) += nci/ |
7 | obj-$(CONFIG_NFC_HCI) += hci/ | ||
7 | 8 | ||
8 | nfc-objs := core.o netlink.o af_nfc.o rawsock.o | 9 | nfc-objs := core.o netlink.o af_nfc.o rawsock.o |
9 | nfc-$(CONFIG_NFC_LLCP) += llcp/llcp.o llcp/commands.o llcp/sock.o | 10 | nfc-$(CONFIG_NFC_LLCP) += llcp/llcp.o llcp/commands.o llcp/sock.o |
diff --git a/net/nfc/core.c b/net/nfc/core.c index 32a7b615e65f..3192c3f589ee 100644 --- a/net/nfc/core.c +++ b/net/nfc/core.c | |||
@@ -33,6 +33,8 @@ | |||
33 | 33 | ||
34 | #define VERSION "0.1" | 34 | #define VERSION "0.1" |
35 | 35 | ||
36 | #define NFC_CHECK_PRES_FREQ_MS 2000 | ||
37 | |||
36 | int nfc_devlist_generation; | 38 | int nfc_devlist_generation; |
37 | DEFINE_MUTEX(nfc_devlist_mutex); | 39 | DEFINE_MUTEX(nfc_devlist_mutex); |
38 | 40 | ||
@@ -95,7 +97,7 @@ int nfc_dev_down(struct nfc_dev *dev) | |||
95 | goto error; | 97 | goto error; |
96 | } | 98 | } |
97 | 99 | ||
98 | if (dev->polling || dev->remote_activated) { | 100 | if (dev->polling || dev->activated_target_idx != NFC_TARGET_IDX_NONE) { |
99 | rc = -EBUSY; | 101 | rc = -EBUSY; |
100 | goto error; | 102 | goto error; |
101 | } | 103 | } |
@@ -211,6 +213,8 @@ int nfc_dep_link_up(struct nfc_dev *dev, int target_index, u8 comm_mode) | |||
211 | } | 213 | } |
212 | 214 | ||
213 | rc = dev->ops->dep_link_up(dev, target_index, comm_mode, gb, gb_len); | 215 | rc = dev->ops->dep_link_up(dev, target_index, comm_mode, gb, gb_len); |
216 | if (!rc) | ||
217 | dev->activated_target_idx = target_index; | ||
214 | 218 | ||
215 | error: | 219 | error: |
216 | device_unlock(&dev->dev); | 220 | device_unlock(&dev->dev); |
@@ -246,6 +250,7 @@ int nfc_dep_link_down(struct nfc_dev *dev) | |||
246 | rc = dev->ops->dep_link_down(dev); | 250 | rc = dev->ops->dep_link_down(dev); |
247 | if (!rc) { | 251 | if (!rc) { |
248 | dev->dep_link_up = false; | 252 | dev->dep_link_up = false; |
253 | dev->activated_target_idx = NFC_TARGET_IDX_NONE; | ||
249 | nfc_llcp_mac_is_down(dev); | 254 | nfc_llcp_mac_is_down(dev); |
250 | nfc_genl_dep_link_down_event(dev); | 255 | nfc_genl_dep_link_down_event(dev); |
251 | } | 256 | } |
@@ -289,8 +294,13 @@ int nfc_activate_target(struct nfc_dev *dev, u32 target_idx, u32 protocol) | |||
289 | } | 294 | } |
290 | 295 | ||
291 | rc = dev->ops->activate_target(dev, target_idx, protocol); | 296 | rc = dev->ops->activate_target(dev, target_idx, protocol); |
292 | if (!rc) | 297 | if (!rc) { |
293 | dev->remote_activated = true; | 298 | dev->activated_target_idx = target_idx; |
299 | |||
300 | if (dev->ops->check_presence) | ||
301 | mod_timer(&dev->check_pres_timer, jiffies + | ||
302 | msecs_to_jiffies(NFC_CHECK_PRES_FREQ_MS)); | ||
303 | } | ||
294 | 304 | ||
295 | error: | 305 | error: |
296 | device_unlock(&dev->dev); | 306 | device_unlock(&dev->dev); |
@@ -317,8 +327,11 @@ int nfc_deactivate_target(struct nfc_dev *dev, u32 target_idx) | |||
317 | goto error; | 327 | goto error; |
318 | } | 328 | } |
319 | 329 | ||
330 | if (dev->ops->check_presence) | ||
331 | del_timer_sync(&dev->check_pres_timer); | ||
332 | |||
320 | dev->ops->deactivate_target(dev, target_idx); | 333 | dev->ops->deactivate_target(dev, target_idx); |
321 | dev->remote_activated = false; | 334 | dev->activated_target_idx = NFC_TARGET_IDX_NONE; |
322 | 335 | ||
323 | error: | 336 | error: |
324 | device_unlock(&dev->dev); | 337 | device_unlock(&dev->dev); |
@@ -352,8 +365,27 @@ int nfc_data_exchange(struct nfc_dev *dev, u32 target_idx, struct sk_buff *skb, | |||
352 | goto error; | 365 | goto error; |
353 | } | 366 | } |
354 | 367 | ||
368 | if (dev->activated_target_idx == NFC_TARGET_IDX_NONE) { | ||
369 | rc = -ENOTCONN; | ||
370 | kfree_skb(skb); | ||
371 | goto error; | ||
372 | } | ||
373 | |||
374 | if (target_idx != dev->activated_target_idx) { | ||
375 | rc = -EADDRNOTAVAIL; | ||
376 | kfree_skb(skb); | ||
377 | goto error; | ||
378 | } | ||
379 | |||
380 | if (dev->ops->check_presence) | ||
381 | del_timer_sync(&dev->check_pres_timer); | ||
382 | |||
355 | rc = dev->ops->data_exchange(dev, target_idx, skb, cb, cb_context); | 383 | rc = dev->ops->data_exchange(dev, target_idx, skb, cb, cb_context); |
356 | 384 | ||
385 | if (!rc && dev->ops->check_presence) | ||
386 | mod_timer(&dev->check_pres_timer, jiffies + | ||
387 | msecs_to_jiffies(NFC_CHECK_PRES_FREQ_MS)); | ||
388 | |||
357 | error: | 389 | error: |
358 | device_unlock(&dev->dev); | 390 | device_unlock(&dev->dev); |
359 | return rc; | 391 | return rc; |
@@ -428,10 +460,15 @@ EXPORT_SYMBOL(nfc_alloc_recv_skb); | |||
428 | int nfc_targets_found(struct nfc_dev *dev, | 460 | int nfc_targets_found(struct nfc_dev *dev, |
429 | struct nfc_target *targets, int n_targets) | 461 | struct nfc_target *targets, int n_targets) |
430 | { | 462 | { |
463 | int i; | ||
464 | |||
431 | pr_debug("dev_name=%s n_targets=%d\n", dev_name(&dev->dev), n_targets); | 465 | pr_debug("dev_name=%s n_targets=%d\n", dev_name(&dev->dev), n_targets); |
432 | 466 | ||
433 | dev->polling = false; | 467 | dev->polling = false; |
434 | 468 | ||
469 | for (i = 0; i < n_targets; i++) | ||
470 | targets[i].idx = dev->target_next_idx++; | ||
471 | |||
435 | spin_lock_bh(&dev->targets_lock); | 472 | spin_lock_bh(&dev->targets_lock); |
436 | 473 | ||
437 | dev->targets_generation++; | 474 | dev->targets_generation++; |
@@ -455,17 +492,92 @@ int nfc_targets_found(struct nfc_dev *dev, | |||
455 | } | 492 | } |
456 | EXPORT_SYMBOL(nfc_targets_found); | 493 | EXPORT_SYMBOL(nfc_targets_found); |
457 | 494 | ||
495 | int nfc_target_lost(struct nfc_dev *dev, u32 target_idx) | ||
496 | { | ||
497 | struct nfc_target *tg; | ||
498 | int i; | ||
499 | |||
500 | pr_debug("dev_name %s n_target %d\n", dev_name(&dev->dev), target_idx); | ||
501 | |||
502 | spin_lock_bh(&dev->targets_lock); | ||
503 | |||
504 | for (i = 0; i < dev->n_targets; i++) { | ||
505 | tg = &dev->targets[i]; | ||
506 | if (tg->idx == target_idx) | ||
507 | break; | ||
508 | } | ||
509 | |||
510 | if (i == dev->n_targets) { | ||
511 | spin_unlock_bh(&dev->targets_lock); | ||
512 | return -EINVAL; | ||
513 | } | ||
514 | |||
515 | dev->targets_generation++; | ||
516 | dev->n_targets--; | ||
517 | dev->activated_target_idx = NFC_TARGET_IDX_NONE; | ||
518 | |||
519 | if (dev->n_targets) { | ||
520 | memcpy(&dev->targets[i], &dev->targets[i + 1], | ||
521 | (dev->n_targets - i) * sizeof(struct nfc_target)); | ||
522 | } else { | ||
523 | kfree(dev->targets); | ||
524 | dev->targets = NULL; | ||
525 | } | ||
526 | |||
527 | spin_unlock_bh(&dev->targets_lock); | ||
528 | |||
529 | nfc_genl_target_lost(dev, target_idx); | ||
530 | |||
531 | return 0; | ||
532 | } | ||
533 | EXPORT_SYMBOL(nfc_target_lost); | ||
534 | |||
458 | static void nfc_release(struct device *d) | 535 | static void nfc_release(struct device *d) |
459 | { | 536 | { |
460 | struct nfc_dev *dev = to_nfc_dev(d); | 537 | struct nfc_dev *dev = to_nfc_dev(d); |
461 | 538 | ||
462 | pr_debug("dev_name=%s\n", dev_name(&dev->dev)); | 539 | pr_debug("dev_name=%s\n", dev_name(&dev->dev)); |
463 | 540 | ||
541 | if (dev->ops->check_presence) { | ||
542 | del_timer_sync(&dev->check_pres_timer); | ||
543 | destroy_workqueue(dev->check_pres_wq); | ||
544 | } | ||
545 | |||
464 | nfc_genl_data_exit(&dev->genl_data); | 546 | nfc_genl_data_exit(&dev->genl_data); |
465 | kfree(dev->targets); | 547 | kfree(dev->targets); |
466 | kfree(dev); | 548 | kfree(dev); |
467 | } | 549 | } |
468 | 550 | ||
551 | static void nfc_check_pres_work(struct work_struct *work) | ||
552 | { | ||
553 | struct nfc_dev *dev = container_of(work, struct nfc_dev, | ||
554 | check_pres_work); | ||
555 | int rc; | ||
556 | |||
557 | device_lock(&dev->dev); | ||
558 | |||
559 | if (dev->activated_target_idx != NFC_TARGET_IDX_NONE && | ||
560 | timer_pending(&dev->check_pres_timer) == 0) { | ||
561 | rc = dev->ops->check_presence(dev, dev->activated_target_idx); | ||
562 | if (!rc) { | ||
563 | mod_timer(&dev->check_pres_timer, jiffies + | ||
564 | msecs_to_jiffies(NFC_CHECK_PRES_FREQ_MS)); | ||
565 | } else { | ||
566 | nfc_target_lost(dev, dev->activated_target_idx); | ||
567 | dev->activated_target_idx = NFC_TARGET_IDX_NONE; | ||
568 | } | ||
569 | } | ||
570 | |||
571 | device_unlock(&dev->dev); | ||
572 | } | ||
573 | |||
574 | static void nfc_check_pres_timeout(unsigned long data) | ||
575 | { | ||
576 | struct nfc_dev *dev = (struct nfc_dev *)data; | ||
577 | |||
578 | queue_work(dev->check_pres_wq, &dev->check_pres_work); | ||
579 | } | ||
580 | |||
469 | struct class nfc_class = { | 581 | struct class nfc_class = { |
470 | .name = "nfc", | 582 | .name = "nfc", |
471 | .dev_release = nfc_release, | 583 | .dev_release = nfc_release, |
@@ -531,6 +643,26 @@ struct nfc_dev *nfc_allocate_device(struct nfc_ops *ops, | |||
531 | /* first generation must not be 0 */ | 643 | /* first generation must not be 0 */ |
532 | dev->targets_generation = 1; | 644 | dev->targets_generation = 1; |
533 | 645 | ||
646 | dev->activated_target_idx = NFC_TARGET_IDX_NONE; | ||
647 | |||
648 | if (ops->check_presence) { | ||
649 | char name[32]; | ||
650 | init_timer(&dev->check_pres_timer); | ||
651 | dev->check_pres_timer.data = (unsigned long)dev; | ||
652 | dev->check_pres_timer.function = nfc_check_pres_timeout; | ||
653 | |||
654 | INIT_WORK(&dev->check_pres_work, nfc_check_pres_work); | ||
655 | snprintf(name, sizeof(name), "nfc%d_check_pres_wq", dev->idx); | ||
656 | dev->check_pres_wq = alloc_workqueue(name, WQ_NON_REENTRANT | | ||
657 | WQ_UNBOUND | | ||
658 | WQ_MEM_RECLAIM, 1); | ||
659 | if (dev->check_pres_wq == NULL) { | ||
660 | kfree(dev); | ||
661 | return NULL; | ||
662 | } | ||
663 | } | ||
664 | |||
665 | |||
534 | return dev; | 666 | return dev; |
535 | } | 667 | } |
536 | EXPORT_SYMBOL(nfc_allocate_device); | 668 | EXPORT_SYMBOL(nfc_allocate_device); |
diff --git a/net/nfc/hci/Kconfig b/net/nfc/hci/Kconfig new file mode 100644 index 000000000000..17213a6362b4 --- /dev/null +++ b/net/nfc/hci/Kconfig | |||
@@ -0,0 +1,16 @@ | |||
1 | config NFC_HCI | ||
2 | depends on NFC | ||
3 | tristate "NFC HCI implementation" | ||
4 | default n | ||
5 | help | ||
6 | Say Y here if you want to build support for a kernel NFC HCI | ||
7 | implementation. This is mostly needed for devices that only process | ||
8 | HCI frames, like for example the NXP pn544. | ||
9 | |||
10 | config NFC_SHDLC | ||
11 | depends on NFC_HCI | ||
12 | bool "SHDLC link layer for HCI based NFC drivers" | ||
13 | default n | ||
14 | ---help--- | ||
15 | Say yes if you use an NFC HCI driver that requires SHDLC link layer. | ||
16 | If unsure, say N here. | ||
diff --git a/net/nfc/hci/Makefile b/net/nfc/hci/Makefile new file mode 100644 index 000000000000..f9c44b2fb065 --- /dev/null +++ b/net/nfc/hci/Makefile | |||
@@ -0,0 +1,8 @@ | |||
1 | # | ||
2 | # Makefile for the Linux NFC HCI layer. | ||
3 | # | ||
4 | |||
5 | obj-$(CONFIG_NFC_HCI) += hci.o | ||
6 | |||
7 | hci-y := core.o hcp.o command.o | ||
8 | hci-$(CONFIG_NFC_SHDLC) += shdlc.o | ||
diff --git a/net/nfc/hci/command.c b/net/nfc/hci/command.c new file mode 100644 index 000000000000..8729abf5f18b --- /dev/null +++ b/net/nfc/hci/command.c | |||
@@ -0,0 +1,354 @@ | |||
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/sched.h> | ||
25 | #include <linux/module.h> | ||
26 | |||
27 | #include <net/nfc/hci.h> | ||
28 | |||
29 | #include "hci.h" | ||
30 | |||
31 | static int nfc_hci_result_to_errno(u8 result) | ||
32 | { | ||
33 | switch (result) { | ||
34 | case NFC_HCI_ANY_OK: | ||
35 | return 0; | ||
36 | case NFC_HCI_ANY_E_TIMEOUT: | ||
37 | return -ETIMEDOUT; | ||
38 | default: | ||
39 | return -1; | ||
40 | } | ||
41 | } | ||
42 | |||
43 | static void nfc_hci_execute_cb(struct nfc_hci_dev *hdev, u8 result, | ||
44 | struct sk_buff *skb, void *cb_data) | ||
45 | { | ||
46 | struct hcp_exec_waiter *hcp_ew = (struct hcp_exec_waiter *)cb_data; | ||
47 | |||
48 | pr_debug("HCI Cmd completed with HCI result=%d\n", result); | ||
49 | |||
50 | hcp_ew->exec_result = nfc_hci_result_to_errno(result); | ||
51 | if (hcp_ew->exec_result == 0) | ||
52 | hcp_ew->result_skb = skb; | ||
53 | else | ||
54 | kfree_skb(skb); | ||
55 | hcp_ew->exec_complete = true; | ||
56 | |||
57 | wake_up(hcp_ew->wq); | ||
58 | } | ||
59 | |||
60 | static int nfc_hci_execute_cmd(struct nfc_hci_dev *hdev, u8 pipe, u8 cmd, | ||
61 | const u8 *param, size_t param_len, | ||
62 | struct sk_buff **skb) | ||
63 | { | ||
64 | DECLARE_WAIT_QUEUE_HEAD_ONSTACK(ew_wq); | ||
65 | struct hcp_exec_waiter hcp_ew; | ||
66 | hcp_ew.wq = &ew_wq; | ||
67 | hcp_ew.exec_complete = false; | ||
68 | hcp_ew.result_skb = NULL; | ||
69 | |||
70 | pr_debug("through pipe=%d, cmd=%d, plen=%zd\n", pipe, cmd, param_len); | ||
71 | |||
72 | /* TODO: Define hci cmd execution delay. Should it be the same | ||
73 | * for all commands? | ||
74 | */ | ||
75 | hcp_ew.exec_result = nfc_hci_hcp_message_tx(hdev, pipe, | ||
76 | NFC_HCI_HCP_COMMAND, cmd, | ||
77 | param, param_len, | ||
78 | nfc_hci_execute_cb, &hcp_ew, | ||
79 | 3000); | ||
80 | if (hcp_ew.exec_result < 0) | ||
81 | return hcp_ew.exec_result; | ||
82 | |||
83 | wait_event(ew_wq, hcp_ew.exec_complete == true); | ||
84 | |||
85 | if (hcp_ew.exec_result == 0) { | ||
86 | if (skb) | ||
87 | *skb = hcp_ew.result_skb; | ||
88 | else | ||
89 | kfree_skb(hcp_ew.result_skb); | ||
90 | } | ||
91 | |||
92 | return hcp_ew.exec_result; | ||
93 | } | ||
94 | |||
95 | int nfc_hci_send_event(struct nfc_hci_dev *hdev, u8 gate, u8 event, | ||
96 | const u8 *param, size_t param_len) | ||
97 | { | ||
98 | u8 pipe; | ||
99 | |||
100 | pr_debug("%d to gate %d\n", event, gate); | ||
101 | |||
102 | pipe = hdev->gate2pipe[gate]; | ||
103 | if (pipe == NFC_HCI_INVALID_PIPE) | ||
104 | return -EADDRNOTAVAIL; | ||
105 | |||
106 | return nfc_hci_hcp_message_tx(hdev, pipe, NFC_HCI_HCP_EVENT, event, | ||
107 | param, param_len, NULL, NULL, 0); | ||
108 | } | ||
109 | EXPORT_SYMBOL(nfc_hci_send_event); | ||
110 | |||
111 | int nfc_hci_send_response(struct nfc_hci_dev *hdev, u8 gate, u8 response, | ||
112 | const u8 *param, size_t param_len) | ||
113 | { | ||
114 | u8 pipe; | ||
115 | |||
116 | pr_debug("\n"); | ||
117 | |||
118 | pipe = hdev->gate2pipe[gate]; | ||
119 | if (pipe == NFC_HCI_INVALID_PIPE) | ||
120 | return -EADDRNOTAVAIL; | ||
121 | |||
122 | return nfc_hci_hcp_message_tx(hdev, pipe, NFC_HCI_HCP_RESPONSE, | ||
123 | response, param, param_len, NULL, NULL, | ||
124 | 0); | ||
125 | } | ||
126 | EXPORT_SYMBOL(nfc_hci_send_response); | ||
127 | |||
128 | /* | ||
129 | * Execute an hci command sent to gate. | ||
130 | * skb will contain response data if success. skb can be NULL if you are not | ||
131 | * interested by the response. | ||
132 | */ | ||
133 | int nfc_hci_send_cmd(struct nfc_hci_dev *hdev, u8 gate, u8 cmd, | ||
134 | const u8 *param, size_t param_len, struct sk_buff **skb) | ||
135 | { | ||
136 | u8 pipe; | ||
137 | |||
138 | pr_debug("\n"); | ||
139 | |||
140 | pipe = hdev->gate2pipe[gate]; | ||
141 | if (pipe == NFC_HCI_INVALID_PIPE) | ||
142 | return -EADDRNOTAVAIL; | ||
143 | |||
144 | return nfc_hci_execute_cmd(hdev, pipe, cmd, param, param_len, skb); | ||
145 | } | ||
146 | EXPORT_SYMBOL(nfc_hci_send_cmd); | ||
147 | |||
148 | int nfc_hci_set_param(struct nfc_hci_dev *hdev, u8 gate, u8 idx, | ||
149 | const u8 *param, size_t param_len) | ||
150 | { | ||
151 | int r; | ||
152 | u8 *tmp; | ||
153 | |||
154 | /* TODO ELa: reg idx must be inserted before param, but we don't want | ||
155 | * to ask the caller to do it to keep a simpler API. | ||
156 | * For now, just create a new temporary param buffer. This is far from | ||
157 | * optimal though, and the plan is to modify APIs to pass idx down to | ||
158 | * nfc_hci_hcp_message_tx where the frame is actually built, thereby | ||
159 | * eliminating the need for the temp allocation-copy here. | ||
160 | */ | ||
161 | |||
162 | pr_debug("idx=%d to gate %d\n", idx, gate); | ||
163 | |||
164 | tmp = kmalloc(1 + param_len, GFP_KERNEL); | ||
165 | if (tmp == NULL) | ||
166 | return -ENOMEM; | ||
167 | |||
168 | *tmp = idx; | ||
169 | memcpy(tmp + 1, param, param_len); | ||
170 | |||
171 | r = nfc_hci_send_cmd(hdev, gate, NFC_HCI_ANY_SET_PARAMETER, | ||
172 | tmp, param_len + 1, NULL); | ||
173 | |||
174 | kfree(tmp); | ||
175 | |||
176 | return r; | ||
177 | } | ||
178 | EXPORT_SYMBOL(nfc_hci_set_param); | ||
179 | |||
180 | int nfc_hci_get_param(struct nfc_hci_dev *hdev, u8 gate, u8 idx, | ||
181 | struct sk_buff **skb) | ||
182 | { | ||
183 | pr_debug("gate=%d regidx=%d\n", gate, idx); | ||
184 | |||
185 | return nfc_hci_send_cmd(hdev, gate, NFC_HCI_ANY_GET_PARAMETER, | ||
186 | &idx, 1, skb); | ||
187 | } | ||
188 | EXPORT_SYMBOL(nfc_hci_get_param); | ||
189 | |||
190 | static int nfc_hci_open_pipe(struct nfc_hci_dev *hdev, u8 pipe) | ||
191 | { | ||
192 | struct sk_buff *skb; | ||
193 | int r; | ||
194 | |||
195 | pr_debug("pipe=%d\n", pipe); | ||
196 | |||
197 | r = nfc_hci_execute_cmd(hdev, pipe, NFC_HCI_ANY_OPEN_PIPE, | ||
198 | NULL, 0, &skb); | ||
199 | if (r == 0) { | ||
200 | /* dest host other than host controller will send | ||
201 | * number of pipes already open on this gate before | ||
202 | * execution. The number can be found in skb->data[0] | ||
203 | */ | ||
204 | kfree_skb(skb); | ||
205 | } | ||
206 | |||
207 | return r; | ||
208 | } | ||
209 | |||
210 | static int nfc_hci_close_pipe(struct nfc_hci_dev *hdev, u8 pipe) | ||
211 | { | ||
212 | pr_debug("\n"); | ||
213 | |||
214 | return nfc_hci_execute_cmd(hdev, pipe, NFC_HCI_ANY_CLOSE_PIPE, | ||
215 | NULL, 0, NULL); | ||
216 | } | ||
217 | |||
218 | static u8 nfc_hci_create_pipe(struct nfc_hci_dev *hdev, u8 dest_host, | ||
219 | u8 dest_gate, int *result) | ||
220 | { | ||
221 | struct sk_buff *skb; | ||
222 | struct hci_create_pipe_params params; | ||
223 | struct hci_create_pipe_resp *resp; | ||
224 | u8 pipe; | ||
225 | |||
226 | pr_debug("gate=%d\n", dest_gate); | ||
227 | |||
228 | params.src_gate = NFC_HCI_ADMIN_GATE; | ||
229 | params.dest_host = dest_host; | ||
230 | params.dest_gate = dest_gate; | ||
231 | |||
232 | *result = nfc_hci_execute_cmd(hdev, NFC_HCI_ADMIN_PIPE, | ||
233 | NFC_HCI_ADM_CREATE_PIPE, | ||
234 | (u8 *) ¶ms, sizeof(params), &skb); | ||
235 | if (*result == 0) { | ||
236 | resp = (struct hci_create_pipe_resp *)skb->data; | ||
237 | pipe = resp->pipe; | ||
238 | kfree_skb(skb); | ||
239 | |||
240 | pr_debug("pipe created=%d\n", pipe); | ||
241 | |||
242 | return pipe; | ||
243 | } else | ||
244 | return NFC_HCI_INVALID_PIPE; | ||
245 | } | ||
246 | |||
247 | static int nfc_hci_delete_pipe(struct nfc_hci_dev *hdev, u8 pipe) | ||
248 | { | ||
249 | pr_debug("\n"); | ||
250 | |||
251 | return nfc_hci_execute_cmd(hdev, NFC_HCI_ADMIN_PIPE, | ||
252 | NFC_HCI_ADM_DELETE_PIPE, &pipe, 1, NULL); | ||
253 | } | ||
254 | |||
255 | static int nfc_hci_clear_all_pipes(struct nfc_hci_dev *hdev) | ||
256 | { | ||
257 | int r; | ||
258 | |||
259 | u8 param[2]; | ||
260 | |||
261 | /* TODO: Find out what the identity reference data is | ||
262 | * and fill param with it. HCI spec 6.1.3.5 */ | ||
263 | |||
264 | pr_debug("\n"); | ||
265 | |||
266 | r = nfc_hci_execute_cmd(hdev, NFC_HCI_ADMIN_PIPE, | ||
267 | NFC_HCI_ADM_CLEAR_ALL_PIPE, param, 2, NULL); | ||
268 | |||
269 | return 0; | ||
270 | } | ||
271 | |||
272 | int nfc_hci_disconnect_gate(struct nfc_hci_dev *hdev, u8 gate) | ||
273 | { | ||
274 | int r; | ||
275 | u8 pipe = hdev->gate2pipe[gate]; | ||
276 | |||
277 | pr_debug("\n"); | ||
278 | |||
279 | if (pipe == NFC_HCI_INVALID_PIPE) | ||
280 | return -EADDRNOTAVAIL; | ||
281 | |||
282 | r = nfc_hci_close_pipe(hdev, pipe); | ||
283 | if (r < 0) | ||
284 | return r; | ||
285 | |||
286 | if (pipe != NFC_HCI_LINK_MGMT_PIPE && pipe != NFC_HCI_ADMIN_PIPE) { | ||
287 | r = nfc_hci_delete_pipe(hdev, pipe); | ||
288 | if (r < 0) | ||
289 | return r; | ||
290 | } | ||
291 | |||
292 | hdev->gate2pipe[gate] = NFC_HCI_INVALID_PIPE; | ||
293 | |||
294 | return 0; | ||
295 | } | ||
296 | EXPORT_SYMBOL(nfc_hci_disconnect_gate); | ||
297 | |||
298 | int nfc_hci_disconnect_all_gates(struct nfc_hci_dev *hdev) | ||
299 | { | ||
300 | int r; | ||
301 | |||
302 | pr_debug("\n"); | ||
303 | |||
304 | r = nfc_hci_clear_all_pipes(hdev); | ||
305 | if (r < 0) | ||
306 | return r; | ||
307 | |||
308 | memset(hdev->gate2pipe, NFC_HCI_INVALID_PIPE, sizeof(hdev->gate2pipe)); | ||
309 | |||
310 | return 0; | ||
311 | } | ||
312 | EXPORT_SYMBOL(nfc_hci_disconnect_all_gates); | ||
313 | |||
314 | int nfc_hci_connect_gate(struct nfc_hci_dev *hdev, u8 dest_host, u8 dest_gate) | ||
315 | { | ||
316 | u8 pipe = NFC_HCI_INVALID_PIPE; | ||
317 | bool pipe_created = false; | ||
318 | int r; | ||
319 | |||
320 | pr_debug("\n"); | ||
321 | |||
322 | if (hdev->gate2pipe[dest_gate] != NFC_HCI_INVALID_PIPE) | ||
323 | return -EADDRINUSE; | ||
324 | |||
325 | switch (dest_gate) { | ||
326 | case NFC_HCI_LINK_MGMT_GATE: | ||
327 | pipe = NFC_HCI_LINK_MGMT_PIPE; | ||
328 | break; | ||
329 | case NFC_HCI_ADMIN_GATE: | ||
330 | pipe = NFC_HCI_ADMIN_PIPE; | ||
331 | break; | ||
332 | default: | ||
333 | pipe = nfc_hci_create_pipe(hdev, dest_host, dest_gate, &r); | ||
334 | if (pipe == NFC_HCI_INVALID_PIPE) | ||
335 | return r; | ||
336 | pipe_created = true; | ||
337 | break; | ||
338 | } | ||
339 | |||
340 | r = nfc_hci_open_pipe(hdev, pipe); | ||
341 | if (r < 0) { | ||
342 | if (pipe_created) | ||
343 | if (nfc_hci_delete_pipe(hdev, pipe) < 0) { | ||
344 | /* TODO: Cannot clean by deleting pipe... | ||
345 | * -> inconsistent state */ | ||
346 | } | ||
347 | return r; | ||
348 | } | ||
349 | |||
350 | hdev->gate2pipe[dest_gate] = pipe; | ||
351 | |||
352 | return 0; | ||
353 | } | ||
354 | EXPORT_SYMBOL(nfc_hci_connect_gate); | ||
diff --git a/net/nfc/hci/core.c b/net/nfc/hci/core.c new file mode 100644 index 000000000000..86fd00d5a099 --- /dev/null +++ b/net/nfc/hci/core.c | |||
@@ -0,0 +1,830 @@ | |||
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 | if (r < 0) | ||
239 | goto exit; | ||
240 | |||
241 | kfree(hdev->targets); | ||
242 | hdev->targets = targets; | ||
243 | targets = NULL; | ||
244 | hdev->target_count = 1; | ||
245 | |||
246 | exit: | ||
247 | kfree(targets); | ||
248 | kfree_skb(atqa_skb); | ||
249 | kfree_skb(sak_skb); | ||
250 | |||
251 | return r; | ||
252 | } | ||
253 | |||
254 | void nfc_hci_event_received(struct nfc_hci_dev *hdev, u8 pipe, u8 event, | ||
255 | struct sk_buff *skb) | ||
256 | { | ||
257 | int r = 0; | ||
258 | |||
259 | switch (event) { | ||
260 | case NFC_HCI_EVT_TARGET_DISCOVERED: | ||
261 | if (hdev->poll_started == false) { | ||
262 | r = -EPROTO; | ||
263 | goto exit; | ||
264 | } | ||
265 | |||
266 | if (skb->len < 1) { /* no status data? */ | ||
267 | r = -EPROTO; | ||
268 | goto exit; | ||
269 | } | ||
270 | |||
271 | if (skb->data[0] == 3) { | ||
272 | /* TODO: Multiple targets in field, none activated | ||
273 | * poll is supposedly stopped, but there is no | ||
274 | * single target to activate, so nothing to report | ||
275 | * up. | ||
276 | * if we need to restart poll, we must save the | ||
277 | * protocols from the initial poll and reuse here. | ||
278 | */ | ||
279 | } | ||
280 | |||
281 | if (skb->data[0] != 0) { | ||
282 | r = -EPROTO; | ||
283 | goto exit; | ||
284 | } | ||
285 | |||
286 | r = nfc_hci_target_discovered(hdev, | ||
287 | nfc_hci_pipe2gate(hdev, pipe)); | ||
288 | break; | ||
289 | default: | ||
290 | /* TODO: Unknown events are hardware specific | ||
291 | * pass them to the driver (needs a new hci_ops) */ | ||
292 | break; | ||
293 | } | ||
294 | |||
295 | exit: | ||
296 | kfree_skb(skb); | ||
297 | |||
298 | if (r) { | ||
299 | /* TODO: There was an error dispatching the event, | ||
300 | * how to propagate up to nfc core? | ||
301 | */ | ||
302 | } | ||
303 | } | ||
304 | |||
305 | static void nfc_hci_cmd_timeout(unsigned long data) | ||
306 | { | ||
307 | struct nfc_hci_dev *hdev = (struct nfc_hci_dev *)data; | ||
308 | |||
309 | queue_work(hdev->msg_tx_wq, &hdev->msg_tx_work); | ||
310 | } | ||
311 | |||
312 | static int hci_dev_connect_gates(struct nfc_hci_dev *hdev, u8 gate_count, | ||
313 | u8 gates[]) | ||
314 | { | ||
315 | int r; | ||
316 | u8 *p = gates; | ||
317 | while (gate_count--) { | ||
318 | r = nfc_hci_connect_gate(hdev, NFC_HCI_HOST_CONTROLLER_ID, *p); | ||
319 | if (r < 0) | ||
320 | return r; | ||
321 | p++; | ||
322 | } | ||
323 | |||
324 | return 0; | ||
325 | } | ||
326 | |||
327 | static int hci_dev_session_init(struct nfc_hci_dev *hdev) | ||
328 | { | ||
329 | struct sk_buff *skb = NULL; | ||
330 | int r; | ||
331 | u8 hci_gates[] = { /* NFC_HCI_ADMIN_GATE MUST be first */ | ||
332 | NFC_HCI_ADMIN_GATE, NFC_HCI_LOOPBACK_GATE, | ||
333 | NFC_HCI_ID_MGMT_GATE, NFC_HCI_LINK_MGMT_GATE, | ||
334 | NFC_HCI_RF_READER_B_GATE, NFC_HCI_RF_READER_A_GATE | ||
335 | }; | ||
336 | |||
337 | r = nfc_hci_connect_gate(hdev, NFC_HCI_HOST_CONTROLLER_ID, | ||
338 | NFC_HCI_ADMIN_GATE); | ||
339 | if (r < 0) | ||
340 | goto exit; | ||
341 | |||
342 | r = nfc_hci_get_param(hdev, NFC_HCI_ADMIN_GATE, | ||
343 | NFC_HCI_ADMIN_SESSION_IDENTITY, &skb); | ||
344 | if (r < 0) | ||
345 | goto disconnect_all; | ||
346 | |||
347 | if (skb->len && skb->len == strlen(hdev->init_data.session_id)) | ||
348 | if (memcmp(hdev->init_data.session_id, skb->data, | ||
349 | skb->len) == 0) { | ||
350 | /* TODO ELa: restore gate<->pipe table from | ||
351 | * some TBD location. | ||
352 | * note: it doesn't seem possible to get the chip | ||
353 | * currently open gate/pipe table. | ||
354 | * It is only possible to obtain the supported | ||
355 | * gate list. | ||
356 | */ | ||
357 | |||
358 | /* goto exit | ||
359 | * For now, always do a full initialization */ | ||
360 | } | ||
361 | |||
362 | r = nfc_hci_disconnect_all_gates(hdev); | ||
363 | if (r < 0) | ||
364 | goto exit; | ||
365 | |||
366 | r = hci_dev_connect_gates(hdev, sizeof(hci_gates), hci_gates); | ||
367 | if (r < 0) | ||
368 | goto disconnect_all; | ||
369 | |||
370 | r = hci_dev_connect_gates(hdev, hdev->init_data.gate_count, | ||
371 | hdev->init_data.gates); | ||
372 | if (r < 0) | ||
373 | goto disconnect_all; | ||
374 | |||
375 | r = nfc_hci_set_param(hdev, NFC_HCI_ADMIN_GATE, | ||
376 | NFC_HCI_ADMIN_SESSION_IDENTITY, | ||
377 | hdev->init_data.session_id, | ||
378 | strlen(hdev->init_data.session_id)); | ||
379 | if (r == 0) | ||
380 | goto exit; | ||
381 | |||
382 | disconnect_all: | ||
383 | nfc_hci_disconnect_all_gates(hdev); | ||
384 | |||
385 | exit: | ||
386 | if (skb) | ||
387 | kfree_skb(skb); | ||
388 | |||
389 | return r; | ||
390 | } | ||
391 | |||
392 | static int hci_dev_version(struct nfc_hci_dev *hdev) | ||
393 | { | ||
394 | int r; | ||
395 | struct sk_buff *skb; | ||
396 | |||
397 | r = nfc_hci_get_param(hdev, NFC_HCI_ID_MGMT_GATE, | ||
398 | NFC_HCI_ID_MGMT_VERSION_SW, &skb); | ||
399 | if (r < 0) | ||
400 | return r; | ||
401 | |||
402 | if (skb->len != 3) { | ||
403 | kfree_skb(skb); | ||
404 | return -EINVAL; | ||
405 | } | ||
406 | |||
407 | hdev->sw_romlib = (skb->data[0] & 0xf0) >> 4; | ||
408 | hdev->sw_patch = skb->data[0] & 0x0f; | ||
409 | hdev->sw_flashlib_major = skb->data[1]; | ||
410 | hdev->sw_flashlib_minor = skb->data[2]; | ||
411 | |||
412 | kfree_skb(skb); | ||
413 | |||
414 | r = nfc_hci_get_param(hdev, NFC_HCI_ID_MGMT_GATE, | ||
415 | NFC_HCI_ID_MGMT_VERSION_HW, &skb); | ||
416 | if (r < 0) | ||
417 | return r; | ||
418 | |||
419 | if (skb->len != 3) { | ||
420 | kfree_skb(skb); | ||
421 | return -EINVAL; | ||
422 | } | ||
423 | |||
424 | hdev->hw_derivative = (skb->data[0] & 0xe0) >> 5; | ||
425 | hdev->hw_version = skb->data[0] & 0x1f; | ||
426 | hdev->hw_mpw = (skb->data[1] & 0xc0) >> 6; | ||
427 | hdev->hw_software = skb->data[1] & 0x3f; | ||
428 | hdev->hw_bsid = skb->data[2]; | ||
429 | |||
430 | kfree_skb(skb); | ||
431 | |||
432 | pr_info("SOFTWARE INFO:\n"); | ||
433 | pr_info("RomLib : %d\n", hdev->sw_romlib); | ||
434 | pr_info("Patch : %d\n", hdev->sw_patch); | ||
435 | pr_info("FlashLib Major : %d\n", hdev->sw_flashlib_major); | ||
436 | pr_info("FlashLib Minor : %d\n", hdev->sw_flashlib_minor); | ||
437 | pr_info("HARDWARE INFO:\n"); | ||
438 | pr_info("Derivative : %d\n", hdev->hw_derivative); | ||
439 | pr_info("HW Version : %d\n", hdev->hw_version); | ||
440 | pr_info("#MPW : %d\n", hdev->hw_mpw); | ||
441 | pr_info("Software : %d\n", hdev->hw_software); | ||
442 | pr_info("BSID Version : %d\n", hdev->hw_bsid); | ||
443 | |||
444 | return 0; | ||
445 | } | ||
446 | |||
447 | static int hci_dev_up(struct nfc_dev *nfc_dev) | ||
448 | { | ||
449 | struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev); | ||
450 | int r = 0; | ||
451 | |||
452 | if (hdev->ops->open) { | ||
453 | r = hdev->ops->open(hdev); | ||
454 | if (r < 0) | ||
455 | return r; | ||
456 | } | ||
457 | |||
458 | r = hci_dev_session_init(hdev); | ||
459 | if (r < 0) | ||
460 | goto exit; | ||
461 | |||
462 | r = nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE, | ||
463 | NFC_HCI_EVT_END_OPERATION, NULL, 0); | ||
464 | if (r < 0) | ||
465 | goto exit; | ||
466 | |||
467 | if (hdev->ops->hci_ready) { | ||
468 | r = hdev->ops->hci_ready(hdev); | ||
469 | if (r < 0) | ||
470 | goto exit; | ||
471 | } | ||
472 | |||
473 | r = hci_dev_version(hdev); | ||
474 | if (r < 0) | ||
475 | goto exit; | ||
476 | |||
477 | exit: | ||
478 | if (r < 0) | ||
479 | if (hdev->ops->close) | ||
480 | hdev->ops->close(hdev); | ||
481 | return r; | ||
482 | } | ||
483 | |||
484 | static int hci_dev_down(struct nfc_dev *nfc_dev) | ||
485 | { | ||
486 | struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev); | ||
487 | |||
488 | if (hdev->ops->close) | ||
489 | hdev->ops->close(hdev); | ||
490 | |||
491 | memset(hdev->gate2pipe, NFC_HCI_INVALID_PIPE, sizeof(hdev->gate2pipe)); | ||
492 | |||
493 | return 0; | ||
494 | } | ||
495 | |||
496 | static int hci_start_poll(struct nfc_dev *nfc_dev, u32 protocols) | ||
497 | { | ||
498 | struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev); | ||
499 | int r; | ||
500 | |||
501 | if (hdev->ops->start_poll) | ||
502 | r = hdev->ops->start_poll(hdev, protocols); | ||
503 | else | ||
504 | r = nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE, | ||
505 | NFC_HCI_EVT_READER_REQUESTED, NULL, 0); | ||
506 | if (r == 0) | ||
507 | hdev->poll_started = true; | ||
508 | |||
509 | return r; | ||
510 | } | ||
511 | |||
512 | static void hci_stop_poll(struct nfc_dev *nfc_dev) | ||
513 | { | ||
514 | struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev); | ||
515 | |||
516 | if (hdev->poll_started) { | ||
517 | nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE, | ||
518 | NFC_HCI_EVT_END_OPERATION, NULL, 0); | ||
519 | hdev->poll_started = false; | ||
520 | } | ||
521 | } | ||
522 | |||
523 | static struct nfc_target *hci_find_target(struct nfc_hci_dev *hdev, | ||
524 | u32 target_idx) | ||
525 | { | ||
526 | int i; | ||
527 | if (hdev->poll_started == false || hdev->targets == NULL) | ||
528 | return NULL; | ||
529 | |||
530 | for (i = 0; i < hdev->target_count; i++) { | ||
531 | if (hdev->targets[i].idx == target_idx) | ||
532 | return &hdev->targets[i]; | ||
533 | } | ||
534 | |||
535 | return NULL; | ||
536 | } | ||
537 | |||
538 | static int hci_activate_target(struct nfc_dev *nfc_dev, u32 target_idx, | ||
539 | u32 protocol) | ||
540 | { | ||
541 | struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev); | ||
542 | |||
543 | if (hci_find_target(hdev, target_idx) == NULL) | ||
544 | return -ENOMEDIUM; | ||
545 | |||
546 | return 0; | ||
547 | } | ||
548 | |||
549 | static void hci_deactivate_target(struct nfc_dev *nfc_dev, u32 target_idx) | ||
550 | { | ||
551 | } | ||
552 | |||
553 | static int hci_data_exchange(struct nfc_dev *nfc_dev, u32 target_idx, | ||
554 | struct sk_buff *skb, data_exchange_cb_t cb, | ||
555 | void *cb_context) | ||
556 | { | ||
557 | struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev); | ||
558 | int r; | ||
559 | struct nfc_target *target; | ||
560 | struct sk_buff *res_skb = NULL; | ||
561 | |||
562 | pr_debug("target_idx=%d\n", target_idx); | ||
563 | |||
564 | target = hci_find_target(hdev, target_idx); | ||
565 | if (target == NULL) | ||
566 | return -ENOMEDIUM; | ||
567 | |||
568 | switch (target->hci_reader_gate) { | ||
569 | case NFC_HCI_RF_READER_A_GATE: | ||
570 | case NFC_HCI_RF_READER_B_GATE: | ||
571 | if (hdev->ops->data_exchange) { | ||
572 | r = hdev->ops->data_exchange(hdev, target, skb, | ||
573 | &res_skb); | ||
574 | if (r <= 0) /* handled */ | ||
575 | break; | ||
576 | } | ||
577 | |||
578 | *skb_push(skb, 1) = 0; /* CTR, see spec:10.2.2.1 */ | ||
579 | r = nfc_hci_send_cmd(hdev, target->hci_reader_gate, | ||
580 | NFC_HCI_WR_XCHG_DATA, | ||
581 | skb->data, skb->len, &res_skb); | ||
582 | /* | ||
583 | * TODO: Check RF Error indicator to make sure data is valid. | ||
584 | * It seems that HCI cmd can complete without error, but data | ||
585 | * can be invalid if an RF error occured? Ignore for now. | ||
586 | */ | ||
587 | if (r == 0) | ||
588 | skb_trim(res_skb, res_skb->len - 1); /* RF Err ind */ | ||
589 | break; | ||
590 | default: | ||
591 | if (hdev->ops->data_exchange) { | ||
592 | r = hdev->ops->data_exchange(hdev, target, skb, | ||
593 | &res_skb); | ||
594 | if (r == 1) | ||
595 | r = -ENOTSUPP; | ||
596 | } | ||
597 | else | ||
598 | r = -ENOTSUPP; | ||
599 | } | ||
600 | |||
601 | kfree_skb(skb); | ||
602 | |||
603 | cb(cb_context, res_skb, r); | ||
604 | |||
605 | return 0; | ||
606 | } | ||
607 | |||
608 | struct nfc_ops hci_nfc_ops = { | ||
609 | .dev_up = hci_dev_up, | ||
610 | .dev_down = hci_dev_down, | ||
611 | .start_poll = hci_start_poll, | ||
612 | .stop_poll = hci_stop_poll, | ||
613 | .activate_target = hci_activate_target, | ||
614 | .deactivate_target = hci_deactivate_target, | ||
615 | .data_exchange = hci_data_exchange, | ||
616 | }; | ||
617 | |||
618 | struct nfc_hci_dev *nfc_hci_allocate_device(struct nfc_hci_ops *ops, | ||
619 | struct nfc_hci_init_data *init_data, | ||
620 | u32 protocols, | ||
621 | int tx_headroom, | ||
622 | int tx_tailroom, | ||
623 | int max_link_payload) | ||
624 | { | ||
625 | struct nfc_hci_dev *hdev; | ||
626 | |||
627 | if (ops->xmit == NULL) | ||
628 | return NULL; | ||
629 | |||
630 | if (protocols == 0) | ||
631 | return NULL; | ||
632 | |||
633 | hdev = kzalloc(sizeof(struct nfc_hci_dev), GFP_KERNEL); | ||
634 | if (hdev == NULL) | ||
635 | return NULL; | ||
636 | |||
637 | hdev->ndev = nfc_allocate_device(&hci_nfc_ops, protocols, | ||
638 | tx_headroom + HCI_CMDS_HEADROOM, | ||
639 | tx_tailroom); | ||
640 | if (!hdev->ndev) { | ||
641 | kfree(hdev); | ||
642 | return NULL; | ||
643 | } | ||
644 | |||
645 | hdev->ops = ops; | ||
646 | hdev->max_data_link_payload = max_link_payload; | ||
647 | hdev->init_data = *init_data; | ||
648 | |||
649 | nfc_set_drvdata(hdev->ndev, hdev); | ||
650 | |||
651 | memset(hdev->gate2pipe, NFC_HCI_INVALID_PIPE, sizeof(hdev->gate2pipe)); | ||
652 | |||
653 | return hdev; | ||
654 | } | ||
655 | EXPORT_SYMBOL(nfc_hci_allocate_device); | ||
656 | |||
657 | void nfc_hci_free_device(struct nfc_hci_dev *hdev) | ||
658 | { | ||
659 | nfc_free_device(hdev->ndev); | ||
660 | kfree(hdev); | ||
661 | } | ||
662 | EXPORT_SYMBOL(nfc_hci_free_device); | ||
663 | |||
664 | int nfc_hci_register_device(struct nfc_hci_dev *hdev) | ||
665 | { | ||
666 | struct device *dev = &hdev->ndev->dev; | ||
667 | const char *devname = dev_name(dev); | ||
668 | char name[32]; | ||
669 | int r = 0; | ||
670 | |||
671 | mutex_init(&hdev->msg_tx_mutex); | ||
672 | |||
673 | INIT_LIST_HEAD(&hdev->msg_tx_queue); | ||
674 | |||
675 | INIT_WORK(&hdev->msg_tx_work, nfc_hci_msg_tx_work); | ||
676 | snprintf(name, sizeof(name), "%s_hci_msg_tx_wq", devname); | ||
677 | hdev->msg_tx_wq = alloc_workqueue(name, WQ_NON_REENTRANT | WQ_UNBOUND | | ||
678 | WQ_MEM_RECLAIM, 1); | ||
679 | if (hdev->msg_tx_wq == NULL) { | ||
680 | r = -ENOMEM; | ||
681 | goto exit; | ||
682 | } | ||
683 | |||
684 | init_timer(&hdev->cmd_timer); | ||
685 | hdev->cmd_timer.data = (unsigned long)hdev; | ||
686 | hdev->cmd_timer.function = nfc_hci_cmd_timeout; | ||
687 | |||
688 | skb_queue_head_init(&hdev->rx_hcp_frags); | ||
689 | |||
690 | INIT_WORK(&hdev->msg_rx_work, nfc_hci_msg_rx_work); | ||
691 | snprintf(name, sizeof(name), "%s_hci_msg_rx_wq", devname); | ||
692 | hdev->msg_rx_wq = alloc_workqueue(name, WQ_NON_REENTRANT | WQ_UNBOUND | | ||
693 | WQ_MEM_RECLAIM, 1); | ||
694 | if (hdev->msg_rx_wq == NULL) { | ||
695 | r = -ENOMEM; | ||
696 | goto exit; | ||
697 | } | ||
698 | |||
699 | skb_queue_head_init(&hdev->msg_rx_queue); | ||
700 | |||
701 | r = nfc_register_device(hdev->ndev); | ||
702 | |||
703 | exit: | ||
704 | if (r < 0) { | ||
705 | if (hdev->msg_tx_wq) | ||
706 | destroy_workqueue(hdev->msg_tx_wq); | ||
707 | if (hdev->msg_rx_wq) | ||
708 | destroy_workqueue(hdev->msg_rx_wq); | ||
709 | } | ||
710 | |||
711 | return r; | ||
712 | } | ||
713 | EXPORT_SYMBOL(nfc_hci_register_device); | ||
714 | |||
715 | void nfc_hci_unregister_device(struct nfc_hci_dev *hdev) | ||
716 | { | ||
717 | struct hci_msg *msg; | ||
718 | |||
719 | skb_queue_purge(&hdev->rx_hcp_frags); | ||
720 | skb_queue_purge(&hdev->msg_rx_queue); | ||
721 | |||
722 | while ((msg = list_first_entry(&hdev->msg_tx_queue, struct hci_msg, | ||
723 | msg_l)) != NULL) { | ||
724 | list_del(&msg->msg_l); | ||
725 | skb_queue_purge(&msg->msg_frags); | ||
726 | kfree(msg); | ||
727 | } | ||
728 | |||
729 | del_timer_sync(&hdev->cmd_timer); | ||
730 | |||
731 | nfc_unregister_device(hdev->ndev); | ||
732 | |||
733 | destroy_workqueue(hdev->msg_tx_wq); | ||
734 | |||
735 | destroy_workqueue(hdev->msg_rx_wq); | ||
736 | } | ||
737 | EXPORT_SYMBOL(nfc_hci_unregister_device); | ||
738 | |||
739 | void nfc_hci_set_clientdata(struct nfc_hci_dev *hdev, void *clientdata) | ||
740 | { | ||
741 | hdev->clientdata = clientdata; | ||
742 | } | ||
743 | EXPORT_SYMBOL(nfc_hci_set_clientdata); | ||
744 | |||
745 | void *nfc_hci_get_clientdata(struct nfc_hci_dev *hdev) | ||
746 | { | ||
747 | return hdev->clientdata; | ||
748 | } | ||
749 | EXPORT_SYMBOL(nfc_hci_get_clientdata); | ||
750 | |||
751 | void nfc_hci_recv_frame(struct nfc_hci_dev *hdev, struct sk_buff *skb) | ||
752 | { | ||
753 | struct hcp_packet *packet; | ||
754 | u8 type; | ||
755 | u8 instruction; | ||
756 | struct sk_buff *hcp_skb; | ||
757 | u8 pipe; | ||
758 | struct sk_buff *frag_skb; | ||
759 | int msg_len; | ||
760 | |||
761 | if (skb == NULL) { | ||
762 | /* TODO ELa: lower layer had permanent failure, need to | ||
763 | * propagate that up | ||
764 | */ | ||
765 | |||
766 | skb_queue_purge(&hdev->rx_hcp_frags); | ||
767 | |||
768 | return; | ||
769 | } | ||
770 | |||
771 | packet = (struct hcp_packet *)skb->data; | ||
772 | if ((packet->header & ~NFC_HCI_FRAGMENT) == 0) { | ||
773 | skb_queue_tail(&hdev->rx_hcp_frags, skb); | ||
774 | return; | ||
775 | } | ||
776 | |||
777 | /* it's the last fragment. Does it need re-aggregation? */ | ||
778 | if (skb_queue_len(&hdev->rx_hcp_frags)) { | ||
779 | pipe = packet->header & NFC_HCI_FRAGMENT; | ||
780 | skb_queue_tail(&hdev->rx_hcp_frags, skb); | ||
781 | |||
782 | msg_len = 0; | ||
783 | skb_queue_walk(&hdev->rx_hcp_frags, frag_skb) { | ||
784 | msg_len += (frag_skb->len - | ||
785 | NFC_HCI_HCP_PACKET_HEADER_LEN); | ||
786 | } | ||
787 | |||
788 | hcp_skb = nfc_alloc_recv_skb(NFC_HCI_HCP_PACKET_HEADER_LEN + | ||
789 | msg_len, GFP_KERNEL); | ||
790 | if (hcp_skb == NULL) { | ||
791 | /* TODO ELa: cannot deliver HCP message. How to | ||
792 | * propagate error up? | ||
793 | */ | ||
794 | } | ||
795 | |||
796 | *skb_put(hcp_skb, NFC_HCI_HCP_PACKET_HEADER_LEN) = pipe; | ||
797 | |||
798 | skb_queue_walk(&hdev->rx_hcp_frags, frag_skb) { | ||
799 | msg_len = frag_skb->len - NFC_HCI_HCP_PACKET_HEADER_LEN; | ||
800 | memcpy(skb_put(hcp_skb, msg_len), | ||
801 | frag_skb->data + NFC_HCI_HCP_PACKET_HEADER_LEN, | ||
802 | msg_len); | ||
803 | } | ||
804 | |||
805 | skb_queue_purge(&hdev->rx_hcp_frags); | ||
806 | } else { | ||
807 | packet->header &= NFC_HCI_FRAGMENT; | ||
808 | hcp_skb = skb; | ||
809 | } | ||
810 | |||
811 | /* if this is a response, dispatch immediately to | ||
812 | * unblock waiting cmd context. Otherwise, enqueue to dispatch | ||
813 | * in separate context where handler can also execute command. | ||
814 | */ | ||
815 | packet = (struct hcp_packet *)hcp_skb->data; | ||
816 | type = HCP_MSG_GET_TYPE(packet->message.header); | ||
817 | if (type == NFC_HCI_HCP_RESPONSE) { | ||
818 | pipe = packet->header; | ||
819 | instruction = HCP_MSG_GET_CMD(packet->message.header); | ||
820 | skb_pull(hcp_skb, NFC_HCI_HCP_PACKET_HEADER_LEN + | ||
821 | NFC_HCI_HCP_MESSAGE_HEADER_LEN); | ||
822 | nfc_hci_hcp_message_rx(hdev, pipe, type, instruction, hcp_skb); | ||
823 | } else { | ||
824 | skb_queue_tail(&hdev->msg_rx_queue, hcp_skb); | ||
825 | queue_work(hdev->msg_rx_wq, &hdev->msg_rx_work); | ||
826 | } | ||
827 | } | ||
828 | EXPORT_SYMBOL(nfc_hci_recv_frame); | ||
829 | |||
830 | MODULE_LICENSE("GPL"); | ||
diff --git a/net/nfc/hci/hci.h b/net/nfc/hci/hci.h new file mode 100644 index 000000000000..45f2fe4fd486 --- /dev/null +++ b/net/nfc/hci/hci.h | |||
@@ -0,0 +1,139 @@ | |||
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 | #ifndef __LOCAL_HCI_H | ||
21 | #define __LOCAL_HCI_H | ||
22 | |||
23 | struct gate_pipe_map { | ||
24 | u8 gate; | ||
25 | u8 pipe; | ||
26 | }; | ||
27 | |||
28 | struct hcp_message { | ||
29 | u8 header; /* type -cmd,evt,rsp- + instruction */ | ||
30 | u8 data[]; | ||
31 | } __packed; | ||
32 | |||
33 | struct hcp_packet { | ||
34 | u8 header; /* cbit+pipe */ | ||
35 | struct hcp_message message; | ||
36 | } __packed; | ||
37 | |||
38 | /* | ||
39 | * HCI command execution completion callback. | ||
40 | * result will be one of the HCI response codes. | ||
41 | * skb contains the response data and must be disposed. | ||
42 | */ | ||
43 | typedef void (*hci_cmd_cb_t) (struct nfc_hci_dev *hdev, u8 result, | ||
44 | struct sk_buff *skb, void *cb_data); | ||
45 | |||
46 | struct hcp_exec_waiter { | ||
47 | wait_queue_head_t *wq; | ||
48 | bool exec_complete; | ||
49 | int exec_result; | ||
50 | struct sk_buff *result_skb; | ||
51 | }; | ||
52 | |||
53 | struct hci_msg { | ||
54 | struct list_head msg_l; | ||
55 | struct sk_buff_head msg_frags; | ||
56 | bool wait_response; | ||
57 | hci_cmd_cb_t cb; | ||
58 | void *cb_context; | ||
59 | unsigned long completion_delay; | ||
60 | }; | ||
61 | |||
62 | struct hci_create_pipe_params { | ||
63 | u8 src_gate; | ||
64 | u8 dest_host; | ||
65 | u8 dest_gate; | ||
66 | } __packed; | ||
67 | |||
68 | struct hci_create_pipe_resp { | ||
69 | u8 src_host; | ||
70 | u8 src_gate; | ||
71 | u8 dest_host; | ||
72 | u8 dest_gate; | ||
73 | u8 pipe; | ||
74 | } __packed; | ||
75 | |||
76 | #define NFC_HCI_FRAGMENT 0x7f | ||
77 | |||
78 | #define HCP_HEADER(type, instr) ((((type) & 0x03) << 6) | ((instr) & 0x3f)) | ||
79 | #define HCP_MSG_GET_TYPE(header) ((header & 0xc0) >> 6) | ||
80 | #define HCP_MSG_GET_CMD(header) (header & 0x3f) | ||
81 | |||
82 | int nfc_hci_hcp_message_tx(struct nfc_hci_dev *hdev, u8 pipe, | ||
83 | u8 type, u8 instruction, | ||
84 | const u8 *payload, size_t payload_len, | ||
85 | hci_cmd_cb_t cb, void *cb_data, | ||
86 | unsigned long completion_delay); | ||
87 | |||
88 | u8 nfc_hci_pipe2gate(struct nfc_hci_dev *hdev, u8 pipe); | ||
89 | |||
90 | void nfc_hci_hcp_message_rx(struct nfc_hci_dev *hdev, u8 pipe, u8 type, | ||
91 | u8 instruction, struct sk_buff *skb); | ||
92 | |||
93 | /* HCP headers */ | ||
94 | #define NFC_HCI_HCP_PACKET_HEADER_LEN 1 | ||
95 | #define NFC_HCI_HCP_MESSAGE_HEADER_LEN 1 | ||
96 | #define NFC_HCI_HCP_HEADER_LEN 2 | ||
97 | |||
98 | /* HCP types */ | ||
99 | #define NFC_HCI_HCP_COMMAND 0x00 | ||
100 | #define NFC_HCI_HCP_EVENT 0x01 | ||
101 | #define NFC_HCI_HCP_RESPONSE 0x02 | ||
102 | |||
103 | /* Generic commands */ | ||
104 | #define NFC_HCI_ANY_SET_PARAMETER 0x01 | ||
105 | #define NFC_HCI_ANY_GET_PARAMETER 0x02 | ||
106 | #define NFC_HCI_ANY_OPEN_PIPE 0x03 | ||
107 | #define NFC_HCI_ANY_CLOSE_PIPE 0x04 | ||
108 | |||
109 | /* Reader RF commands */ | ||
110 | #define NFC_HCI_WR_XCHG_DATA 0x10 | ||
111 | |||
112 | /* Admin commands */ | ||
113 | #define NFC_HCI_ADM_CREATE_PIPE 0x10 | ||
114 | #define NFC_HCI_ADM_DELETE_PIPE 0x11 | ||
115 | #define NFC_HCI_ADM_NOTIFY_PIPE_CREATED 0x12 | ||
116 | #define NFC_HCI_ADM_NOTIFY_PIPE_DELETED 0x13 | ||
117 | #define NFC_HCI_ADM_CLEAR_ALL_PIPE 0x14 | ||
118 | #define NFC_HCI_ADM_NOTIFY_ALL_PIPE_CLEARED 0x15 | ||
119 | |||
120 | /* Generic responses */ | ||
121 | #define NFC_HCI_ANY_OK 0x00 | ||
122 | #define NFC_HCI_ANY_E_NOT_CONNECTED 0x01 | ||
123 | #define NFC_HCI_ANY_E_CMD_PAR_UNKNOWN 0x02 | ||
124 | #define NFC_HCI_ANY_E_NOK 0x03 | ||
125 | #define NFC_HCI_ANY_E_PIPES_FULL 0x04 | ||
126 | #define NFC_HCI_ANY_E_REG_PAR_UNKNOWN 0x05 | ||
127 | #define NFC_HCI_ANY_E_PIPE_NOT_OPENED 0x06 | ||
128 | #define NFC_HCI_ANY_E_CMD_NOT_SUPPORTED 0x07 | ||
129 | #define NFC_HCI_ANY_E_INHIBITED 0x08 | ||
130 | #define NFC_HCI_ANY_E_TIMEOUT 0x09 | ||
131 | #define NFC_HCI_ANY_E_REG_ACCESS_DENIED 0x0a | ||
132 | #define NFC_HCI_ANY_E_PIPE_ACCESS_DENIED 0x0b | ||
133 | |||
134 | /* Pipes */ | ||
135 | #define NFC_HCI_INVALID_PIPE 0x80 | ||
136 | #define NFC_HCI_LINK_MGMT_PIPE 0x00 | ||
137 | #define NFC_HCI_ADMIN_PIPE 0x01 | ||
138 | |||
139 | #endif /* __LOCAL_HCI_H */ | ||
diff --git a/net/nfc/hci/hcp.c b/net/nfc/hci/hcp.c new file mode 100644 index 000000000000..7212cf2c5785 --- /dev/null +++ b/net/nfc/hci/hcp.c | |||
@@ -0,0 +1,156 @@ | |||
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 | |||
26 | #include <net/nfc/hci.h> | ||
27 | |||
28 | #include "hci.h" | ||
29 | |||
30 | /* | ||
31 | * Payload is the HCP message data only. Instruction will be prepended. | ||
32 | * Guarantees that cb will be called upon completion or timeout delay | ||
33 | * counted from the moment the cmd is sent to the transport. | ||
34 | */ | ||
35 | int nfc_hci_hcp_message_tx(struct nfc_hci_dev *hdev, u8 pipe, | ||
36 | u8 type, u8 instruction, | ||
37 | const u8 *payload, size_t payload_len, | ||
38 | hci_cmd_cb_t cb, void *cb_data, | ||
39 | unsigned long completion_delay) | ||
40 | { | ||
41 | struct nfc_dev *ndev = hdev->ndev; | ||
42 | struct hci_msg *cmd; | ||
43 | const u8 *ptr = payload; | ||
44 | int hci_len, err; | ||
45 | bool firstfrag = true; | ||
46 | |||
47 | cmd = kzalloc(sizeof(struct hci_msg), GFP_KERNEL); | ||
48 | if (cmd == NULL) | ||
49 | return -ENOMEM; | ||
50 | |||
51 | INIT_LIST_HEAD(&cmd->msg_l); | ||
52 | skb_queue_head_init(&cmd->msg_frags); | ||
53 | cmd->wait_response = (type == NFC_HCI_HCP_COMMAND) ? true : false; | ||
54 | cmd->cb = cb; | ||
55 | cmd->cb_context = cb_data; | ||
56 | cmd->completion_delay = completion_delay; | ||
57 | |||
58 | hci_len = payload_len + 1; | ||
59 | while (hci_len > 0) { | ||
60 | struct sk_buff *skb; | ||
61 | int skb_len, data_link_len; | ||
62 | struct hcp_packet *packet; | ||
63 | |||
64 | if (NFC_HCI_HCP_PACKET_HEADER_LEN + hci_len <= | ||
65 | hdev->max_data_link_payload) | ||
66 | data_link_len = hci_len; | ||
67 | else | ||
68 | data_link_len = hdev->max_data_link_payload - | ||
69 | NFC_HCI_HCP_PACKET_HEADER_LEN; | ||
70 | |||
71 | skb_len = ndev->tx_headroom + NFC_HCI_HCP_PACKET_HEADER_LEN + | ||
72 | data_link_len + ndev->tx_tailroom; | ||
73 | hci_len -= data_link_len; | ||
74 | |||
75 | skb = alloc_skb(skb_len, GFP_KERNEL); | ||
76 | if (skb == NULL) { | ||
77 | err = -ENOMEM; | ||
78 | goto out_skb_err; | ||
79 | } | ||
80 | skb_reserve(skb, ndev->tx_headroom); | ||
81 | |||
82 | skb_put(skb, NFC_HCI_HCP_PACKET_HEADER_LEN + data_link_len); | ||
83 | |||
84 | /* Only the last fragment will have the cb bit set to 1 */ | ||
85 | packet = (struct hcp_packet *)skb->data; | ||
86 | packet->header = pipe; | ||
87 | if (firstfrag) { | ||
88 | firstfrag = false; | ||
89 | packet->message.header = HCP_HEADER(type, instruction); | ||
90 | if (ptr) { | ||
91 | memcpy(packet->message.data, ptr, | ||
92 | data_link_len - 1); | ||
93 | ptr += data_link_len - 1; | ||
94 | } | ||
95 | } else { | ||
96 | memcpy(&packet->message, ptr, data_link_len); | ||
97 | ptr += data_link_len; | ||
98 | } | ||
99 | |||
100 | /* This is the last fragment, set the cb bit */ | ||
101 | if (hci_len == 0) | ||
102 | packet->header |= ~NFC_HCI_FRAGMENT; | ||
103 | |||
104 | skb_queue_tail(&cmd->msg_frags, skb); | ||
105 | } | ||
106 | |||
107 | mutex_lock(&hdev->msg_tx_mutex); | ||
108 | list_add_tail(&hdev->msg_tx_queue, &cmd->msg_l); | ||
109 | mutex_unlock(&hdev->msg_tx_mutex); | ||
110 | |||
111 | queue_work(hdev->msg_tx_wq, &hdev->msg_tx_work); | ||
112 | |||
113 | return 0; | ||
114 | |||
115 | out_skb_err: | ||
116 | skb_queue_purge(&cmd->msg_frags); | ||
117 | kfree(cmd); | ||
118 | |||
119 | return err; | ||
120 | } | ||
121 | |||
122 | u8 nfc_hci_pipe2gate(struct nfc_hci_dev *hdev, u8 pipe) | ||
123 | { | ||
124 | int gate; | ||
125 | |||
126 | for (gate = 0; gate < NFC_HCI_MAX_GATES; gate++) | ||
127 | if (hdev->gate2pipe[gate] == pipe) | ||
128 | return gate; | ||
129 | |||
130 | return 0xff; | ||
131 | } | ||
132 | |||
133 | /* | ||
134 | * Receive hcp message for pipe, with type and cmd. | ||
135 | * skb contains optional message data only. | ||
136 | */ | ||
137 | void nfc_hci_hcp_message_rx(struct nfc_hci_dev *hdev, u8 pipe, u8 type, | ||
138 | u8 instruction, struct sk_buff *skb) | ||
139 | { | ||
140 | switch (type) { | ||
141 | case NFC_HCI_HCP_RESPONSE: | ||
142 | nfc_hci_resp_received(hdev, instruction, skb); | ||
143 | break; | ||
144 | case NFC_HCI_HCP_COMMAND: | ||
145 | nfc_hci_cmd_received(hdev, pipe, instruction, skb); | ||
146 | break; | ||
147 | case NFC_HCI_HCP_EVENT: | ||
148 | nfc_hci_event_received(hdev, pipe, instruction, skb); | ||
149 | break; | ||
150 | default: | ||
151 | pr_err("UNKNOWN MSG Type %d, instruction=%d\n", | ||
152 | type, instruction); | ||
153 | kfree_skb(skb); | ||
154 | break; | ||
155 | } | ||
156 | } | ||
diff --git a/net/nfc/hci/shdlc.c b/net/nfc/hci/shdlc.c new file mode 100644 index 000000000000..923bdf7c26d6 --- /dev/null +++ b/net/nfc/hci/shdlc.c | |||
@@ -0,0 +1,945 @@ | |||
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) "shdlc: %s: " fmt, __func__ | ||
21 | |||
22 | #include <linux/sched.h> | ||
23 | #include <linux/export.h> | ||
24 | #include <linux/wait.h> | ||
25 | #include <linux/crc-ccitt.h> | ||
26 | #include <linux/slab.h> | ||
27 | #include <linux/skbuff.h> | ||
28 | |||
29 | #include <net/nfc/hci.h> | ||
30 | #include <net/nfc/shdlc.h> | ||
31 | |||
32 | #define SHDLC_LLC_HEAD_ROOM 2 | ||
33 | #define SHDLC_LLC_TAIL_ROOM 2 | ||
34 | |||
35 | #define SHDLC_MAX_WINDOW 4 | ||
36 | #define SHDLC_SREJ_SUPPORT false | ||
37 | |||
38 | #define SHDLC_CONTROL_HEAD_MASK 0xe0 | ||
39 | #define SHDLC_CONTROL_HEAD_I 0x80 | ||
40 | #define SHDLC_CONTROL_HEAD_I2 0xa0 | ||
41 | #define SHDLC_CONTROL_HEAD_S 0xc0 | ||
42 | #define SHDLC_CONTROL_HEAD_U 0xe0 | ||
43 | |||
44 | #define SHDLC_CONTROL_NS_MASK 0x38 | ||
45 | #define SHDLC_CONTROL_NR_MASK 0x07 | ||
46 | #define SHDLC_CONTROL_TYPE_MASK 0x18 | ||
47 | |||
48 | #define SHDLC_CONTROL_M_MASK 0x1f | ||
49 | |||
50 | enum sframe_type { | ||
51 | S_FRAME_RR = 0x00, | ||
52 | S_FRAME_REJ = 0x01, | ||
53 | S_FRAME_RNR = 0x02, | ||
54 | S_FRAME_SREJ = 0x03 | ||
55 | }; | ||
56 | |||
57 | enum uframe_modifier { | ||
58 | U_FRAME_UA = 0x06, | ||
59 | U_FRAME_RSET = 0x19 | ||
60 | }; | ||
61 | |||
62 | #define SHDLC_CONNECT_VALUE_MS 5 | ||
63 | #define SHDLC_T1_VALUE_MS(w) ((5 * w) / 4) | ||
64 | #define SHDLC_T2_VALUE_MS 300 | ||
65 | |||
66 | #define SHDLC_DUMP_SKB(info, skb) \ | ||
67 | do { \ | ||
68 | pr_debug("%s:\n", info); \ | ||
69 | print_hex_dump(KERN_DEBUG, "shdlc: ", DUMP_PREFIX_OFFSET, \ | ||
70 | 16, 1, skb->data, skb->len, 0); \ | ||
71 | } while (0) | ||
72 | |||
73 | /* checks x < y <= z modulo 8 */ | ||
74 | static bool nfc_shdlc_x_lt_y_lteq_z(int x, int y, int z) | ||
75 | { | ||
76 | if (x < z) | ||
77 | return ((x < y) && (y <= z)) ? true : false; | ||
78 | else | ||
79 | return ((y > x) || (y <= z)) ? true : false; | ||
80 | } | ||
81 | |||
82 | /* checks x <= y < z modulo 8 */ | ||
83 | static bool nfc_shdlc_x_lteq_y_lt_z(int x, int y, int z) | ||
84 | { | ||
85 | if (x <= z) | ||
86 | return ((x <= y) && (y < z)) ? true : false; | ||
87 | else /* x > z -> z+8 > x */ | ||
88 | return ((y >= x) || (y < z)) ? true : false; | ||
89 | } | ||
90 | |||
91 | static struct sk_buff *nfc_shdlc_alloc_skb(struct nfc_shdlc *shdlc, | ||
92 | int payload_len) | ||
93 | { | ||
94 | struct sk_buff *skb; | ||
95 | |||
96 | skb = alloc_skb(shdlc->client_headroom + SHDLC_LLC_HEAD_ROOM + | ||
97 | shdlc->client_tailroom + SHDLC_LLC_TAIL_ROOM + | ||
98 | payload_len, GFP_KERNEL); | ||
99 | if (skb) | ||
100 | skb_reserve(skb, shdlc->client_headroom + SHDLC_LLC_HEAD_ROOM); | ||
101 | |||
102 | return skb; | ||
103 | } | ||
104 | |||
105 | static void nfc_shdlc_add_len_crc(struct sk_buff *skb) | ||
106 | { | ||
107 | u16 crc; | ||
108 | int len; | ||
109 | |||
110 | len = skb->len + 2; | ||
111 | *skb_push(skb, 1) = len; | ||
112 | |||
113 | crc = crc_ccitt(0xffff, skb->data, skb->len); | ||
114 | crc = ~crc; | ||
115 | *skb_put(skb, 1) = crc & 0xff; | ||
116 | *skb_put(skb, 1) = crc >> 8; | ||
117 | } | ||
118 | |||
119 | /* immediately sends an S frame. */ | ||
120 | static int nfc_shdlc_send_s_frame(struct nfc_shdlc *shdlc, | ||
121 | enum sframe_type sframe_type, int nr) | ||
122 | { | ||
123 | int r; | ||
124 | struct sk_buff *skb; | ||
125 | |||
126 | pr_debug("sframe_type=%d nr=%d\n", sframe_type, nr); | ||
127 | |||
128 | skb = nfc_shdlc_alloc_skb(shdlc, 0); | ||
129 | if (skb == NULL) | ||
130 | return -ENOMEM; | ||
131 | |||
132 | *skb_push(skb, 1) = SHDLC_CONTROL_HEAD_S | (sframe_type << 3) | nr; | ||
133 | |||
134 | nfc_shdlc_add_len_crc(skb); | ||
135 | |||
136 | r = shdlc->ops->xmit(shdlc, skb); | ||
137 | |||
138 | kfree_skb(skb); | ||
139 | |||
140 | return r; | ||
141 | } | ||
142 | |||
143 | /* immediately sends an U frame. skb may contain optional payload */ | ||
144 | static int nfc_shdlc_send_u_frame(struct nfc_shdlc *shdlc, | ||
145 | struct sk_buff *skb, | ||
146 | enum uframe_modifier uframe_modifier) | ||
147 | { | ||
148 | int r; | ||
149 | |||
150 | pr_debug("uframe_modifier=%d\n", uframe_modifier); | ||
151 | |||
152 | *skb_push(skb, 1) = SHDLC_CONTROL_HEAD_U | uframe_modifier; | ||
153 | |||
154 | nfc_shdlc_add_len_crc(skb); | ||
155 | |||
156 | r = shdlc->ops->xmit(shdlc, skb); | ||
157 | |||
158 | kfree_skb(skb); | ||
159 | |||
160 | return r; | ||
161 | } | ||
162 | |||
163 | /* | ||
164 | * Free ack_pending frames until y_nr - 1, and reset t2 according to | ||
165 | * the remaining oldest ack_pending frame sent time | ||
166 | */ | ||
167 | static void nfc_shdlc_reset_t2(struct nfc_shdlc *shdlc, int y_nr) | ||
168 | { | ||
169 | struct sk_buff *skb; | ||
170 | int dnr = shdlc->dnr; /* MUST initially be < y_nr */ | ||
171 | |||
172 | pr_debug("release ack pending up to frame %d excluded\n", y_nr); | ||
173 | |||
174 | while (dnr != y_nr) { | ||
175 | pr_debug("release ack pending frame %d\n", dnr); | ||
176 | |||
177 | skb = skb_dequeue(&shdlc->ack_pending_q); | ||
178 | kfree_skb(skb); | ||
179 | |||
180 | dnr = (dnr + 1) % 8; | ||
181 | } | ||
182 | |||
183 | if (skb_queue_empty(&shdlc->ack_pending_q)) { | ||
184 | if (shdlc->t2_active) { | ||
185 | del_timer_sync(&shdlc->t2_timer); | ||
186 | shdlc->t2_active = false; | ||
187 | |||
188 | pr_debug | ||
189 | ("All sent frames acked. Stopped T2(retransmit)\n"); | ||
190 | } | ||
191 | } else { | ||
192 | skb = skb_peek(&shdlc->ack_pending_q); | ||
193 | |||
194 | mod_timer(&shdlc->t2_timer, *(unsigned long *)skb->cb + | ||
195 | msecs_to_jiffies(SHDLC_T2_VALUE_MS)); | ||
196 | shdlc->t2_active = true; | ||
197 | |||
198 | pr_debug | ||
199 | ("Start T2(retransmit) for remaining unacked sent frames\n"); | ||
200 | } | ||
201 | } | ||
202 | |||
203 | /* | ||
204 | * Receive validated frames from lower layer. skb contains HCI payload only. | ||
205 | * Handle according to algorithm at spec:10.8.2 | ||
206 | */ | ||
207 | static void nfc_shdlc_rcv_i_frame(struct nfc_shdlc *shdlc, | ||
208 | struct sk_buff *skb, int ns, int nr) | ||
209 | { | ||
210 | int x_ns = ns; | ||
211 | int y_nr = nr; | ||
212 | |||
213 | pr_debug("recvd I-frame %d, remote waiting frame %d\n", ns, nr); | ||
214 | |||
215 | if (shdlc->state != SHDLC_CONNECTED) | ||
216 | goto exit; | ||
217 | |||
218 | if (x_ns != shdlc->nr) { | ||
219 | nfc_shdlc_send_s_frame(shdlc, S_FRAME_REJ, shdlc->nr); | ||
220 | goto exit; | ||
221 | } | ||
222 | |||
223 | if (shdlc->t1_active == false) { | ||
224 | shdlc->t1_active = true; | ||
225 | mod_timer(&shdlc->t1_timer, | ||
226 | msecs_to_jiffies(SHDLC_T1_VALUE_MS(shdlc->w))); | ||
227 | pr_debug("(re)Start T1(send ack)\n"); | ||
228 | } | ||
229 | |||
230 | if (skb->len) { | ||
231 | nfc_hci_recv_frame(shdlc->hdev, skb); | ||
232 | skb = NULL; | ||
233 | } | ||
234 | |||
235 | shdlc->nr = (shdlc->nr + 1) % 8; | ||
236 | |||
237 | if (nfc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) { | ||
238 | nfc_shdlc_reset_t2(shdlc, y_nr); | ||
239 | |||
240 | shdlc->dnr = y_nr; | ||
241 | } | ||
242 | |||
243 | exit: | ||
244 | if (skb) | ||
245 | kfree_skb(skb); | ||
246 | } | ||
247 | |||
248 | static void nfc_shdlc_rcv_ack(struct nfc_shdlc *shdlc, int y_nr) | ||
249 | { | ||
250 | pr_debug("remote acked up to frame %d excluded\n", y_nr); | ||
251 | |||
252 | if (nfc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) { | ||
253 | nfc_shdlc_reset_t2(shdlc, y_nr); | ||
254 | shdlc->dnr = y_nr; | ||
255 | } | ||
256 | } | ||
257 | |||
258 | static void nfc_shdlc_requeue_ack_pending(struct nfc_shdlc *shdlc) | ||
259 | { | ||
260 | struct sk_buff *skb; | ||
261 | |||
262 | pr_debug("ns reset to %d\n", shdlc->dnr); | ||
263 | |||
264 | while ((skb = skb_dequeue_tail(&shdlc->ack_pending_q))) { | ||
265 | skb_pull(skb, 2); /* remove len+control */ | ||
266 | skb_trim(skb, skb->len - 2); /* remove crc */ | ||
267 | skb_queue_head(&shdlc->send_q, skb); | ||
268 | } | ||
269 | shdlc->ns = shdlc->dnr; | ||
270 | } | ||
271 | |||
272 | static void nfc_shdlc_rcv_rej(struct nfc_shdlc *shdlc, int y_nr) | ||
273 | { | ||
274 | struct sk_buff *skb; | ||
275 | |||
276 | pr_debug("remote asks retransmition from frame %d\n", y_nr); | ||
277 | |||
278 | if (nfc_shdlc_x_lteq_y_lt_z(shdlc->dnr, y_nr, shdlc->ns)) { | ||
279 | if (shdlc->t2_active) { | ||
280 | del_timer_sync(&shdlc->t2_timer); | ||
281 | shdlc->t2_active = false; | ||
282 | pr_debug("Stopped T2(retransmit)\n"); | ||
283 | } | ||
284 | |||
285 | if (shdlc->dnr != y_nr) { | ||
286 | while ((shdlc->dnr = ((shdlc->dnr + 1) % 8)) != y_nr) { | ||
287 | skb = skb_dequeue(&shdlc->ack_pending_q); | ||
288 | kfree_skb(skb); | ||
289 | } | ||
290 | } | ||
291 | |||
292 | nfc_shdlc_requeue_ack_pending(shdlc); | ||
293 | } | ||
294 | } | ||
295 | |||
296 | /* See spec RR:10.8.3 REJ:10.8.4 */ | ||
297 | static void nfc_shdlc_rcv_s_frame(struct nfc_shdlc *shdlc, | ||
298 | enum sframe_type s_frame_type, int nr) | ||
299 | { | ||
300 | struct sk_buff *skb; | ||
301 | |||
302 | if (shdlc->state != SHDLC_CONNECTED) | ||
303 | return; | ||
304 | |||
305 | switch (s_frame_type) { | ||
306 | case S_FRAME_RR: | ||
307 | nfc_shdlc_rcv_ack(shdlc, nr); | ||
308 | if (shdlc->rnr == true) { /* see SHDLC 10.7.7 */ | ||
309 | shdlc->rnr = false; | ||
310 | if (shdlc->send_q.qlen == 0) { | ||
311 | skb = nfc_shdlc_alloc_skb(shdlc, 0); | ||
312 | if (skb) | ||
313 | skb_queue_tail(&shdlc->send_q, skb); | ||
314 | } | ||
315 | } | ||
316 | break; | ||
317 | case S_FRAME_REJ: | ||
318 | nfc_shdlc_rcv_rej(shdlc, nr); | ||
319 | break; | ||
320 | case S_FRAME_RNR: | ||
321 | nfc_shdlc_rcv_ack(shdlc, nr); | ||
322 | shdlc->rnr = true; | ||
323 | break; | ||
324 | default: | ||
325 | break; | ||
326 | } | ||
327 | } | ||
328 | |||
329 | static void nfc_shdlc_connect_complete(struct nfc_shdlc *shdlc, int r) | ||
330 | { | ||
331 | pr_debug("result=%d\n", r); | ||
332 | |||
333 | del_timer_sync(&shdlc->connect_timer); | ||
334 | |||
335 | if (r == 0) { | ||
336 | shdlc->ns = 0; | ||
337 | shdlc->nr = 0; | ||
338 | shdlc->dnr = 0; | ||
339 | |||
340 | shdlc->state = SHDLC_CONNECTED; | ||
341 | } else { | ||
342 | shdlc->state = SHDLC_DISCONNECTED; | ||
343 | |||
344 | /* | ||
345 | * TODO: Could it be possible that there are pending | ||
346 | * executing commands that are waiting for connect to complete | ||
347 | * before they can be carried? As connect is a blocking | ||
348 | * operation, it would require that the userspace process can | ||
349 | * send commands on the same device from a second thread before | ||
350 | * the device is up. I don't think that is possible, is it? | ||
351 | */ | ||
352 | } | ||
353 | |||
354 | shdlc->connect_result = r; | ||
355 | |||
356 | wake_up(shdlc->connect_wq); | ||
357 | } | ||
358 | |||
359 | static int nfc_shdlc_connect_initiate(struct nfc_shdlc *shdlc) | ||
360 | { | ||
361 | struct sk_buff *skb; | ||
362 | |||
363 | pr_debug("\n"); | ||
364 | |||
365 | skb = nfc_shdlc_alloc_skb(shdlc, 2); | ||
366 | if (skb == NULL) | ||
367 | return -ENOMEM; | ||
368 | |||
369 | *skb_put(skb, 1) = SHDLC_MAX_WINDOW; | ||
370 | *skb_put(skb, 1) = SHDLC_SREJ_SUPPORT ? 1 : 0; | ||
371 | |||
372 | return nfc_shdlc_send_u_frame(shdlc, skb, U_FRAME_RSET); | ||
373 | } | ||
374 | |||
375 | static int nfc_shdlc_connect_send_ua(struct nfc_shdlc *shdlc) | ||
376 | { | ||
377 | struct sk_buff *skb; | ||
378 | |||
379 | pr_debug("\n"); | ||
380 | |||
381 | skb = nfc_shdlc_alloc_skb(shdlc, 0); | ||
382 | if (skb == NULL) | ||
383 | return -ENOMEM; | ||
384 | |||
385 | return nfc_shdlc_send_u_frame(shdlc, skb, U_FRAME_UA); | ||
386 | } | ||
387 | |||
388 | static void nfc_shdlc_rcv_u_frame(struct nfc_shdlc *shdlc, | ||
389 | struct sk_buff *skb, | ||
390 | enum uframe_modifier u_frame_modifier) | ||
391 | { | ||
392 | u8 w = SHDLC_MAX_WINDOW; | ||
393 | bool srej_support = SHDLC_SREJ_SUPPORT; | ||
394 | int r; | ||
395 | |||
396 | pr_debug("u_frame_modifier=%d\n", u_frame_modifier); | ||
397 | |||
398 | switch (u_frame_modifier) { | ||
399 | case U_FRAME_RSET: | ||
400 | if (shdlc->state == SHDLC_NEGOCIATING) { | ||
401 | /* we sent RSET, but chip wants to negociate */ | ||
402 | if (skb->len > 0) | ||
403 | w = skb->data[0]; | ||
404 | |||
405 | if (skb->len > 1) | ||
406 | srej_support = skb->data[1] & 0x01 ? true : | ||
407 | false; | ||
408 | |||
409 | if ((w <= SHDLC_MAX_WINDOW) && | ||
410 | (SHDLC_SREJ_SUPPORT || (srej_support == false))) { | ||
411 | shdlc->w = w; | ||
412 | shdlc->srej_support = srej_support; | ||
413 | r = nfc_shdlc_connect_send_ua(shdlc); | ||
414 | nfc_shdlc_connect_complete(shdlc, r); | ||
415 | } | ||
416 | } else if (shdlc->state > SHDLC_NEGOCIATING) { | ||
417 | /* | ||
418 | * TODO: Chip wants to reset link | ||
419 | * send ua, empty skb lists, reset counters | ||
420 | * propagate info to HCI layer | ||
421 | */ | ||
422 | } | ||
423 | break; | ||
424 | case U_FRAME_UA: | ||
425 | if ((shdlc->state == SHDLC_CONNECTING && | ||
426 | shdlc->connect_tries > 0) || | ||
427 | (shdlc->state == SHDLC_NEGOCIATING)) | ||
428 | nfc_shdlc_connect_complete(shdlc, 0); | ||
429 | break; | ||
430 | default: | ||
431 | break; | ||
432 | } | ||
433 | |||
434 | kfree_skb(skb); | ||
435 | } | ||
436 | |||
437 | static void nfc_shdlc_handle_rcv_queue(struct nfc_shdlc *shdlc) | ||
438 | { | ||
439 | struct sk_buff *skb; | ||
440 | u8 control; | ||
441 | int nr; | ||
442 | int ns; | ||
443 | enum sframe_type s_frame_type; | ||
444 | enum uframe_modifier u_frame_modifier; | ||
445 | |||
446 | if (shdlc->rcv_q.qlen) | ||
447 | pr_debug("rcvQlen=%d\n", shdlc->rcv_q.qlen); | ||
448 | |||
449 | while ((skb = skb_dequeue(&shdlc->rcv_q)) != NULL) { | ||
450 | control = skb->data[0]; | ||
451 | skb_pull(skb, 1); | ||
452 | switch (control & SHDLC_CONTROL_HEAD_MASK) { | ||
453 | case SHDLC_CONTROL_HEAD_I: | ||
454 | case SHDLC_CONTROL_HEAD_I2: | ||
455 | ns = (control & SHDLC_CONTROL_NS_MASK) >> 3; | ||
456 | nr = control & SHDLC_CONTROL_NR_MASK; | ||
457 | nfc_shdlc_rcv_i_frame(shdlc, skb, ns, nr); | ||
458 | break; | ||
459 | case SHDLC_CONTROL_HEAD_S: | ||
460 | s_frame_type = (control & SHDLC_CONTROL_TYPE_MASK) >> 3; | ||
461 | nr = control & SHDLC_CONTROL_NR_MASK; | ||
462 | nfc_shdlc_rcv_s_frame(shdlc, s_frame_type, nr); | ||
463 | kfree_skb(skb); | ||
464 | break; | ||
465 | case SHDLC_CONTROL_HEAD_U: | ||
466 | u_frame_modifier = control & SHDLC_CONTROL_M_MASK; | ||
467 | nfc_shdlc_rcv_u_frame(shdlc, skb, u_frame_modifier); | ||
468 | break; | ||
469 | default: | ||
470 | pr_err("UNKNOWN Control=%d\n", control); | ||
471 | kfree_skb(skb); | ||
472 | break; | ||
473 | } | ||
474 | } | ||
475 | } | ||
476 | |||
477 | static int nfc_shdlc_w_used(int ns, int dnr) | ||
478 | { | ||
479 | int unack_count; | ||
480 | |||
481 | if (dnr <= ns) | ||
482 | unack_count = ns - dnr; | ||
483 | else | ||
484 | unack_count = 8 - dnr + ns; | ||
485 | |||
486 | return unack_count; | ||
487 | } | ||
488 | |||
489 | /* Send frames according to algorithm at spec:10.8.1 */ | ||
490 | static void nfc_shdlc_handle_send_queue(struct nfc_shdlc *shdlc) | ||
491 | { | ||
492 | struct sk_buff *skb; | ||
493 | int r; | ||
494 | unsigned long time_sent; | ||
495 | |||
496 | if (shdlc->send_q.qlen) | ||
497 | pr_debug | ||
498 | ("sendQlen=%d ns=%d dnr=%d rnr=%s w_room=%d unackQlen=%d\n", | ||
499 | shdlc->send_q.qlen, shdlc->ns, shdlc->dnr, | ||
500 | shdlc->rnr == false ? "false" : "true", | ||
501 | shdlc->w - nfc_shdlc_w_used(shdlc->ns, shdlc->dnr), | ||
502 | shdlc->ack_pending_q.qlen); | ||
503 | |||
504 | while (shdlc->send_q.qlen && shdlc->ack_pending_q.qlen < shdlc->w && | ||
505 | (shdlc->rnr == false)) { | ||
506 | |||
507 | if (shdlc->t1_active) { | ||
508 | del_timer_sync(&shdlc->t1_timer); | ||
509 | shdlc->t1_active = false; | ||
510 | pr_debug("Stopped T1(send ack)\n"); | ||
511 | } | ||
512 | |||
513 | skb = skb_dequeue(&shdlc->send_q); | ||
514 | |||
515 | *skb_push(skb, 1) = SHDLC_CONTROL_HEAD_I | (shdlc->ns << 3) | | ||
516 | shdlc->nr; | ||
517 | |||
518 | pr_debug("Sending I-Frame %d, waiting to rcv %d\n", shdlc->ns, | ||
519 | shdlc->nr); | ||
520 | /* SHDLC_DUMP_SKB("shdlc frame written", skb); */ | ||
521 | |||
522 | nfc_shdlc_add_len_crc(skb); | ||
523 | |||
524 | r = shdlc->ops->xmit(shdlc, skb); | ||
525 | if (r < 0) { | ||
526 | /* | ||
527 | * TODO: Cannot send, shdlc machine is dead, we | ||
528 | * must propagate the information up to HCI. | ||
529 | */ | ||
530 | shdlc->hard_fault = r; | ||
531 | break; | ||
532 | } | ||
533 | |||
534 | shdlc->ns = (shdlc->ns + 1) % 8; | ||
535 | |||
536 | time_sent = jiffies; | ||
537 | *(unsigned long *)skb->cb = time_sent; | ||
538 | |||
539 | skb_queue_tail(&shdlc->ack_pending_q, skb); | ||
540 | |||
541 | if (shdlc->t2_active == false) { | ||
542 | shdlc->t2_active = true; | ||
543 | mod_timer(&shdlc->t2_timer, time_sent + | ||
544 | msecs_to_jiffies(SHDLC_T2_VALUE_MS)); | ||
545 | pr_debug("Started T2 (retransmit)\n"); | ||
546 | } | ||
547 | } | ||
548 | } | ||
549 | |||
550 | static void nfc_shdlc_connect_timeout(unsigned long data) | ||
551 | { | ||
552 | struct nfc_shdlc *shdlc = (struct nfc_shdlc *)data; | ||
553 | |||
554 | pr_debug("\n"); | ||
555 | |||
556 | queue_work(shdlc->sm_wq, &shdlc->sm_work); | ||
557 | } | ||
558 | |||
559 | static void nfc_shdlc_t1_timeout(unsigned long data) | ||
560 | { | ||
561 | struct nfc_shdlc *shdlc = (struct nfc_shdlc *)data; | ||
562 | |||
563 | pr_debug("SoftIRQ: need to send ack\n"); | ||
564 | |||
565 | queue_work(shdlc->sm_wq, &shdlc->sm_work); | ||
566 | } | ||
567 | |||
568 | static void nfc_shdlc_t2_timeout(unsigned long data) | ||
569 | { | ||
570 | struct nfc_shdlc *shdlc = (struct nfc_shdlc *)data; | ||
571 | |||
572 | pr_debug("SoftIRQ: need to retransmit\n"); | ||
573 | |||
574 | queue_work(shdlc->sm_wq, &shdlc->sm_work); | ||
575 | } | ||
576 | |||
577 | static void nfc_shdlc_sm_work(struct work_struct *work) | ||
578 | { | ||
579 | struct nfc_shdlc *shdlc = container_of(work, struct nfc_shdlc, sm_work); | ||
580 | int r; | ||
581 | |||
582 | pr_debug("\n"); | ||
583 | |||
584 | mutex_lock(&shdlc->state_mutex); | ||
585 | |||
586 | switch (shdlc->state) { | ||
587 | case SHDLC_DISCONNECTED: | ||
588 | skb_queue_purge(&shdlc->rcv_q); | ||
589 | skb_queue_purge(&shdlc->send_q); | ||
590 | skb_queue_purge(&shdlc->ack_pending_q); | ||
591 | break; | ||
592 | case SHDLC_CONNECTING: | ||
593 | if (shdlc->connect_tries++ < 5) | ||
594 | r = nfc_shdlc_connect_initiate(shdlc); | ||
595 | else | ||
596 | r = -ETIME; | ||
597 | if (r < 0) | ||
598 | nfc_shdlc_connect_complete(shdlc, r); | ||
599 | else { | ||
600 | mod_timer(&shdlc->connect_timer, jiffies + | ||
601 | msecs_to_jiffies(SHDLC_CONNECT_VALUE_MS)); | ||
602 | |||
603 | shdlc->state = SHDLC_NEGOCIATING; | ||
604 | } | ||
605 | break; | ||
606 | case SHDLC_NEGOCIATING: | ||
607 | if (timer_pending(&shdlc->connect_timer) == 0) { | ||
608 | shdlc->state = SHDLC_CONNECTING; | ||
609 | queue_work(shdlc->sm_wq, &shdlc->sm_work); | ||
610 | } | ||
611 | |||
612 | nfc_shdlc_handle_rcv_queue(shdlc); | ||
613 | break; | ||
614 | case SHDLC_CONNECTED: | ||
615 | nfc_shdlc_handle_rcv_queue(shdlc); | ||
616 | nfc_shdlc_handle_send_queue(shdlc); | ||
617 | |||
618 | if (shdlc->t1_active && timer_pending(&shdlc->t1_timer) == 0) { | ||
619 | pr_debug | ||
620 | ("Handle T1(send ack) elapsed (T1 now inactive)\n"); | ||
621 | |||
622 | shdlc->t1_active = false; | ||
623 | r = nfc_shdlc_send_s_frame(shdlc, S_FRAME_RR, | ||
624 | shdlc->nr); | ||
625 | if (r < 0) | ||
626 | shdlc->hard_fault = r; | ||
627 | } | ||
628 | |||
629 | if (shdlc->t2_active && timer_pending(&shdlc->t2_timer) == 0) { | ||
630 | pr_debug | ||
631 | ("Handle T2(retransmit) elapsed (T2 inactive)\n"); | ||
632 | |||
633 | shdlc->t2_active = false; | ||
634 | |||
635 | nfc_shdlc_requeue_ack_pending(shdlc); | ||
636 | nfc_shdlc_handle_send_queue(shdlc); | ||
637 | } | ||
638 | |||
639 | if (shdlc->hard_fault) { | ||
640 | /* | ||
641 | * TODO: Handle hard_fault that occured during | ||
642 | * this invocation of the shdlc worker | ||
643 | */ | ||
644 | } | ||
645 | break; | ||
646 | default: | ||
647 | break; | ||
648 | } | ||
649 | mutex_unlock(&shdlc->state_mutex); | ||
650 | } | ||
651 | |||
652 | /* | ||
653 | * Called from syscall context to establish shdlc link. Sleeps until | ||
654 | * link is ready or failure. | ||
655 | */ | ||
656 | static int nfc_shdlc_connect(struct nfc_shdlc *shdlc) | ||
657 | { | ||
658 | DECLARE_WAIT_QUEUE_HEAD_ONSTACK(connect_wq); | ||
659 | |||
660 | pr_debug("\n"); | ||
661 | |||
662 | mutex_lock(&shdlc->state_mutex); | ||
663 | |||
664 | shdlc->state = SHDLC_CONNECTING; | ||
665 | shdlc->connect_wq = &connect_wq; | ||
666 | shdlc->connect_tries = 0; | ||
667 | shdlc->connect_result = 1; | ||
668 | |||
669 | mutex_unlock(&shdlc->state_mutex); | ||
670 | |||
671 | queue_work(shdlc->sm_wq, &shdlc->sm_work); | ||
672 | |||
673 | wait_event(connect_wq, shdlc->connect_result != 1); | ||
674 | |||
675 | return shdlc->connect_result; | ||
676 | } | ||
677 | |||
678 | static void nfc_shdlc_disconnect(struct nfc_shdlc *shdlc) | ||
679 | { | ||
680 | pr_debug("\n"); | ||
681 | |||
682 | mutex_lock(&shdlc->state_mutex); | ||
683 | |||
684 | shdlc->state = SHDLC_DISCONNECTED; | ||
685 | |||
686 | mutex_unlock(&shdlc->state_mutex); | ||
687 | |||
688 | queue_work(shdlc->sm_wq, &shdlc->sm_work); | ||
689 | } | ||
690 | |||
691 | /* | ||
692 | * Receive an incoming shdlc frame. Frame has already been crc-validated. | ||
693 | * skb contains only LLC header and payload. | ||
694 | * If skb == NULL, it is a notification that the link below is dead. | ||
695 | */ | ||
696 | void nfc_shdlc_recv_frame(struct nfc_shdlc *shdlc, struct sk_buff *skb) | ||
697 | { | ||
698 | if (skb == NULL) { | ||
699 | pr_err("NULL Frame -> link is dead\n"); | ||
700 | shdlc->hard_fault = -EREMOTEIO; | ||
701 | } else { | ||
702 | SHDLC_DUMP_SKB("incoming frame", skb); | ||
703 | skb_queue_tail(&shdlc->rcv_q, skb); | ||
704 | } | ||
705 | |||
706 | queue_work(shdlc->sm_wq, &shdlc->sm_work); | ||
707 | } | ||
708 | EXPORT_SYMBOL(nfc_shdlc_recv_frame); | ||
709 | |||
710 | static int nfc_shdlc_open(struct nfc_hci_dev *hdev) | ||
711 | { | ||
712 | struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev); | ||
713 | int r; | ||
714 | |||
715 | pr_debug("\n"); | ||
716 | |||
717 | if (shdlc->ops->open) { | ||
718 | r = shdlc->ops->open(shdlc); | ||
719 | if (r < 0) | ||
720 | return r; | ||
721 | } | ||
722 | |||
723 | r = nfc_shdlc_connect(shdlc); | ||
724 | if (r < 0 && shdlc->ops->close) | ||
725 | shdlc->ops->close(shdlc); | ||
726 | |||
727 | return r; | ||
728 | } | ||
729 | |||
730 | static void nfc_shdlc_close(struct nfc_hci_dev *hdev) | ||
731 | { | ||
732 | struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev); | ||
733 | |||
734 | pr_debug("\n"); | ||
735 | |||
736 | nfc_shdlc_disconnect(shdlc); | ||
737 | |||
738 | if (shdlc->ops->close) | ||
739 | shdlc->ops->close(shdlc); | ||
740 | } | ||
741 | |||
742 | static int nfc_shdlc_hci_ready(struct nfc_hci_dev *hdev) | ||
743 | { | ||
744 | struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev); | ||
745 | int r = 0; | ||
746 | |||
747 | pr_debug("\n"); | ||
748 | |||
749 | if (shdlc->ops->hci_ready) | ||
750 | r = shdlc->ops->hci_ready(shdlc); | ||
751 | |||
752 | return r; | ||
753 | } | ||
754 | |||
755 | static int nfc_shdlc_xmit(struct nfc_hci_dev *hdev, struct sk_buff *skb) | ||
756 | { | ||
757 | struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev); | ||
758 | |||
759 | SHDLC_DUMP_SKB("queuing HCP packet to shdlc", skb); | ||
760 | |||
761 | skb_queue_tail(&shdlc->send_q, skb); | ||
762 | |||
763 | queue_work(shdlc->sm_wq, &shdlc->sm_work); | ||
764 | |||
765 | return 0; | ||
766 | } | ||
767 | |||
768 | static int nfc_shdlc_start_poll(struct nfc_hci_dev *hdev, u32 protocols) | ||
769 | { | ||
770 | struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev); | ||
771 | |||
772 | pr_debug("\n"); | ||
773 | |||
774 | if (shdlc->ops->start_poll) | ||
775 | return shdlc->ops->start_poll(shdlc, protocols); | ||
776 | |||
777 | return 0; | ||
778 | } | ||
779 | |||
780 | static int nfc_shdlc_target_from_gate(struct nfc_hci_dev *hdev, u8 gate, | ||
781 | struct nfc_target *target) | ||
782 | { | ||
783 | struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev); | ||
784 | |||
785 | if (shdlc->ops->target_from_gate) | ||
786 | return shdlc->ops->target_from_gate(shdlc, gate, target); | ||
787 | |||
788 | return -EPERM; | ||
789 | } | ||
790 | |||
791 | static int nfc_shdlc_complete_target_discovered(struct nfc_hci_dev *hdev, | ||
792 | u8 gate, | ||
793 | struct nfc_target *target) | ||
794 | { | ||
795 | struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev); | ||
796 | |||
797 | pr_debug("\n"); | ||
798 | |||
799 | if (shdlc->ops->complete_target_discovered) | ||
800 | return shdlc->ops->complete_target_discovered(shdlc, gate, | ||
801 | target); | ||
802 | |||
803 | return 0; | ||
804 | } | ||
805 | |||
806 | static int nfc_shdlc_data_exchange(struct nfc_hci_dev *hdev, | ||
807 | struct nfc_target *target, | ||
808 | struct sk_buff *skb, | ||
809 | struct sk_buff **res_skb) | ||
810 | { | ||
811 | struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev); | ||
812 | |||
813 | if (shdlc->ops->data_exchange) | ||
814 | return shdlc->ops->data_exchange(shdlc, target, skb, res_skb); | ||
815 | |||
816 | return -EPERM; | ||
817 | } | ||
818 | |||
819 | static struct nfc_hci_ops shdlc_ops = { | ||
820 | .open = nfc_shdlc_open, | ||
821 | .close = nfc_shdlc_close, | ||
822 | .hci_ready = nfc_shdlc_hci_ready, | ||
823 | .xmit = nfc_shdlc_xmit, | ||
824 | .start_poll = nfc_shdlc_start_poll, | ||
825 | .target_from_gate = nfc_shdlc_target_from_gate, | ||
826 | .complete_target_discovered = nfc_shdlc_complete_target_discovered, | ||
827 | .data_exchange = nfc_shdlc_data_exchange, | ||
828 | }; | ||
829 | |||
830 | struct nfc_shdlc *nfc_shdlc_allocate(struct nfc_shdlc_ops *ops, | ||
831 | struct nfc_hci_init_data *init_data, | ||
832 | u32 protocols, | ||
833 | int tx_headroom, int tx_tailroom, | ||
834 | int max_link_payload, const char *devname) | ||
835 | { | ||
836 | struct nfc_shdlc *shdlc; | ||
837 | int r; | ||
838 | char name[32]; | ||
839 | |||
840 | if (ops->xmit == NULL) | ||
841 | return NULL; | ||
842 | |||
843 | shdlc = kzalloc(sizeof(struct nfc_shdlc), GFP_KERNEL); | ||
844 | if (shdlc == NULL) | ||
845 | return NULL; | ||
846 | |||
847 | mutex_init(&shdlc->state_mutex); | ||
848 | shdlc->ops = ops; | ||
849 | shdlc->state = SHDLC_DISCONNECTED; | ||
850 | |||
851 | init_timer(&shdlc->connect_timer); | ||
852 | shdlc->connect_timer.data = (unsigned long)shdlc; | ||
853 | shdlc->connect_timer.function = nfc_shdlc_connect_timeout; | ||
854 | |||
855 | init_timer(&shdlc->t1_timer); | ||
856 | shdlc->t1_timer.data = (unsigned long)shdlc; | ||
857 | shdlc->t1_timer.function = nfc_shdlc_t1_timeout; | ||
858 | |||
859 | init_timer(&shdlc->t2_timer); | ||
860 | shdlc->t2_timer.data = (unsigned long)shdlc; | ||
861 | shdlc->t2_timer.function = nfc_shdlc_t2_timeout; | ||
862 | |||
863 | shdlc->w = SHDLC_MAX_WINDOW; | ||
864 | shdlc->srej_support = SHDLC_SREJ_SUPPORT; | ||
865 | |||
866 | skb_queue_head_init(&shdlc->rcv_q); | ||
867 | skb_queue_head_init(&shdlc->send_q); | ||
868 | skb_queue_head_init(&shdlc->ack_pending_q); | ||
869 | |||
870 | INIT_WORK(&shdlc->sm_work, nfc_shdlc_sm_work); | ||
871 | snprintf(name, sizeof(name), "%s_shdlc_sm_wq", devname); | ||
872 | shdlc->sm_wq = alloc_workqueue(name, WQ_NON_REENTRANT | WQ_UNBOUND | | ||
873 | WQ_MEM_RECLAIM, 1); | ||
874 | if (shdlc->sm_wq == NULL) | ||
875 | goto err_allocwq; | ||
876 | |||
877 | shdlc->client_headroom = tx_headroom; | ||
878 | shdlc->client_tailroom = tx_tailroom; | ||
879 | |||
880 | shdlc->hdev = nfc_hci_allocate_device(&shdlc_ops, init_data, protocols, | ||
881 | tx_headroom + SHDLC_LLC_HEAD_ROOM, | ||
882 | tx_tailroom + SHDLC_LLC_TAIL_ROOM, | ||
883 | max_link_payload); | ||
884 | if (shdlc->hdev == NULL) | ||
885 | goto err_allocdev; | ||
886 | |||
887 | nfc_hci_set_clientdata(shdlc->hdev, shdlc); | ||
888 | |||
889 | r = nfc_hci_register_device(shdlc->hdev); | ||
890 | if (r < 0) | ||
891 | goto err_regdev; | ||
892 | |||
893 | return shdlc; | ||
894 | |||
895 | err_regdev: | ||
896 | nfc_hci_free_device(shdlc->hdev); | ||
897 | |||
898 | err_allocdev: | ||
899 | destroy_workqueue(shdlc->sm_wq); | ||
900 | |||
901 | err_allocwq: | ||
902 | kfree(shdlc); | ||
903 | |||
904 | return NULL; | ||
905 | } | ||
906 | EXPORT_SYMBOL(nfc_shdlc_allocate); | ||
907 | |||
908 | void nfc_shdlc_free(struct nfc_shdlc *shdlc) | ||
909 | { | ||
910 | pr_debug("\n"); | ||
911 | |||
912 | /* TODO: Check that this cannot be called while still in use */ | ||
913 | |||
914 | nfc_hci_unregister_device(shdlc->hdev); | ||
915 | nfc_hci_free_device(shdlc->hdev); | ||
916 | |||
917 | destroy_workqueue(shdlc->sm_wq); | ||
918 | |||
919 | skb_queue_purge(&shdlc->rcv_q); | ||
920 | skb_queue_purge(&shdlc->send_q); | ||
921 | skb_queue_purge(&shdlc->ack_pending_q); | ||
922 | |||
923 | kfree(shdlc); | ||
924 | } | ||
925 | EXPORT_SYMBOL(nfc_shdlc_free); | ||
926 | |||
927 | void nfc_shdlc_set_clientdata(struct nfc_shdlc *shdlc, void *clientdata) | ||
928 | { | ||
929 | pr_debug("\n"); | ||
930 | |||
931 | shdlc->clientdata = clientdata; | ||
932 | } | ||
933 | EXPORT_SYMBOL(nfc_shdlc_set_clientdata); | ||
934 | |||
935 | void *nfc_shdlc_get_clientdata(struct nfc_shdlc *shdlc) | ||
936 | { | ||
937 | return shdlc->clientdata; | ||
938 | } | ||
939 | EXPORT_SYMBOL(nfc_shdlc_get_clientdata); | ||
940 | |||
941 | struct nfc_hci_dev *nfc_shdlc_get_hci_dev(struct nfc_shdlc *shdlc) | ||
942 | { | ||
943 | return shdlc->hdev; | ||
944 | } | ||
945 | EXPORT_SYMBOL(nfc_shdlc_get_hci_dev); | ||
diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c index ef10ffcb4b6f..11a3b7d98dc5 100644 --- a/net/nfc/llcp/commands.c +++ b/net/nfc/llcp/commands.c | |||
@@ -102,7 +102,7 @@ u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length) | |||
102 | length = llcp_tlv_length[type]; | 102 | length = llcp_tlv_length[type]; |
103 | if (length == 0 && value_length == 0) | 103 | if (length == 0 && value_length == 0) |
104 | return NULL; | 104 | return NULL; |
105 | else | 105 | else if (length == 0) |
106 | length = value_length; | 106 | length = value_length; |
107 | 107 | ||
108 | *tlv_length = 2 + length; | 108 | *tlv_length = 2 + length; |
@@ -248,7 +248,7 @@ int nfc_llcp_disconnect(struct nfc_llcp_sock *sock) | |||
248 | 248 | ||
249 | skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE); | 249 | skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE); |
250 | 250 | ||
251 | skb = llcp_add_header(skb, sock->ssap, sock->dsap, LLCP_PDU_DISC); | 251 | skb = llcp_add_header(skb, sock->dsap, sock->ssap, LLCP_PDU_DISC); |
252 | 252 | ||
253 | skb_queue_tail(&local->tx_queue, skb); | 253 | skb_queue_tail(&local->tx_queue, skb); |
254 | 254 | ||
@@ -416,7 +416,7 @@ int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason) | |||
416 | 416 | ||
417 | skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE); | 417 | skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE); |
418 | 418 | ||
419 | skb = llcp_add_header(skb, ssap, dsap, LLCP_PDU_DM); | 419 | skb = llcp_add_header(skb, dsap, ssap, LLCP_PDU_DM); |
420 | 420 | ||
421 | memcpy(skb_put(skb, 1), &reason, 1); | 421 | memcpy(skb_put(skb, 1), &reason, 1); |
422 | 422 | ||
@@ -522,7 +522,7 @@ int nfc_llcp_send_rr(struct nfc_llcp_sock *sock) | |||
522 | 522 | ||
523 | skb_put(skb, LLCP_SEQUENCE_SIZE); | 523 | skb_put(skb, LLCP_SEQUENCE_SIZE); |
524 | 524 | ||
525 | skb->data[2] = sock->recv_n % 16; | 525 | skb->data[2] = sock->recv_n; |
526 | 526 | ||
527 | skb_queue_head(&local->tx_queue, skb); | 527 | skb_queue_head(&local->tx_queue, skb); |
528 | 528 | ||
diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c index 17a578f641f1..92988aa620dc 100644 --- a/net/nfc/llcp/llcp.c +++ b/net/nfc/llcp/llcp.c | |||
@@ -307,6 +307,8 @@ static int nfc_llcp_build_gb(struct nfc_llcp_local *local) | |||
307 | u8 *gb_cur, *version_tlv, version, version_length; | 307 | u8 *gb_cur, *version_tlv, version, version_length; |
308 | u8 *lto_tlv, lto, lto_length; | 308 | u8 *lto_tlv, lto, lto_length; |
309 | u8 *wks_tlv, wks_length; | 309 | u8 *wks_tlv, wks_length; |
310 | u8 *miux_tlv, miux_length; | ||
311 | __be16 miux; | ||
310 | u8 gb_len = 0; | 312 | u8 gb_len = 0; |
311 | 313 | ||
312 | version = LLCP_VERSION_11; | 314 | version = LLCP_VERSION_11; |
@@ -316,7 +318,7 @@ static int nfc_llcp_build_gb(struct nfc_llcp_local *local) | |||
316 | 318 | ||
317 | /* 1500 ms */ | 319 | /* 1500 ms */ |
318 | lto = 150; | 320 | lto = 150; |
319 | lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, <o, 1, <o_length); | 321 | lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_LTO, <o, 1, <o_length); |
320 | gb_len += lto_length; | 322 | gb_len += lto_length; |
321 | 323 | ||
322 | pr_debug("Local wks 0x%lx\n", local->local_wks); | 324 | pr_debug("Local wks 0x%lx\n", local->local_wks); |
@@ -324,6 +326,11 @@ static int nfc_llcp_build_gb(struct nfc_llcp_local *local) | |||
324 | &wks_length); | 326 | &wks_length); |
325 | gb_len += wks_length; | 327 | gb_len += wks_length; |
326 | 328 | ||
329 | miux = cpu_to_be16(LLCP_MAX_MIUX); | ||
330 | miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0, | ||
331 | &miux_length); | ||
332 | gb_len += miux_length; | ||
333 | |||
327 | gb_len += ARRAY_SIZE(llcp_magic); | 334 | gb_len += ARRAY_SIZE(llcp_magic); |
328 | 335 | ||
329 | if (gb_len > NFC_MAX_GT_LEN) { | 336 | if (gb_len > NFC_MAX_GT_LEN) { |
@@ -345,6 +352,9 @@ static int nfc_llcp_build_gb(struct nfc_llcp_local *local) | |||
345 | memcpy(gb_cur, wks_tlv, wks_length); | 352 | memcpy(gb_cur, wks_tlv, wks_length); |
346 | gb_cur += wks_length; | 353 | gb_cur += wks_length; |
347 | 354 | ||
355 | memcpy(gb_cur, miux_tlv, miux_length); | ||
356 | gb_cur += miux_length; | ||
357 | |||
348 | kfree(version_tlv); | 358 | kfree(version_tlv); |
349 | kfree(lto_tlv); | 359 | kfree(lto_tlv); |
350 | 360 | ||
@@ -388,6 +398,9 @@ static void nfc_llcp_tx_work(struct work_struct *work) | |||
388 | skb = skb_dequeue(&local->tx_queue); | 398 | skb = skb_dequeue(&local->tx_queue); |
389 | if (skb != NULL) { | 399 | if (skb != NULL) { |
390 | pr_debug("Sending pending skb\n"); | 400 | pr_debug("Sending pending skb\n"); |
401 | print_hex_dump(KERN_DEBUG, "LLCP Tx: ", DUMP_PREFIX_OFFSET, | ||
402 | 16, 1, skb->data, skb->len, true); | ||
403 | |||
391 | nfc_data_exchange(local->dev, local->target_idx, | 404 | nfc_data_exchange(local->dev, local->target_idx, |
392 | skb, nfc_llcp_recv, local); | 405 | skb, nfc_llcp_recv, local); |
393 | } else { | 406 | } else { |
@@ -425,7 +438,7 @@ static u8 nfc_llcp_nr(struct sk_buff *pdu) | |||
425 | 438 | ||
426 | static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu) | 439 | static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu) |
427 | { | 440 | { |
428 | pdu->data[2] = (sock->send_n << 4) | (sock->recv_n % 16); | 441 | pdu->data[2] = (sock->send_n << 4) | (sock->recv_n); |
429 | sock->send_n = (sock->send_n + 1) % 16; | 442 | sock->send_n = (sock->send_n + 1) % 16; |
430 | sock->recv_ack_n = (sock->recv_n - 1) % 16; | 443 | sock->recv_ack_n = (sock->recv_n - 1) % 16; |
431 | } | 444 | } |
@@ -814,6 +827,10 @@ static void nfc_llcp_rx_work(struct work_struct *work) | |||
814 | 827 | ||
815 | pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap); | 828 | pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap); |
816 | 829 | ||
830 | if (ptype != LLCP_PDU_SYMM) | ||
831 | print_hex_dump(KERN_DEBUG, "LLCP Rx: ", DUMP_PREFIX_OFFSET, | ||
832 | 16, 1, skb->data, skb->len, true); | ||
833 | |||
817 | switch (ptype) { | 834 | switch (ptype) { |
818 | case LLCP_PDU_SYMM: | 835 | case LLCP_PDU_SYMM: |
819 | pr_debug("SYMM\n"); | 836 | pr_debug("SYMM\n"); |
diff --git a/net/nfc/nci/core.c b/net/nfc/nci/core.c index 9ec065bb9ee1..8737c2089fdd 100644 --- a/net/nfc/nci/core.c +++ b/net/nfc/nci/core.c | |||
@@ -477,7 +477,7 @@ static int nci_activate_target(struct nfc_dev *nfc_dev, __u32 target_idx, | |||
477 | } | 477 | } |
478 | 478 | ||
479 | if (atomic_read(&ndev->state) == NCI_W4_HOST_SELECT) { | 479 | if (atomic_read(&ndev->state) == NCI_W4_HOST_SELECT) { |
480 | param.rf_discovery_id = target->idx; | 480 | param.rf_discovery_id = target->logical_idx; |
481 | 481 | ||
482 | if (protocol == NFC_PROTO_JEWEL) | 482 | if (protocol == NFC_PROTO_JEWEL) |
483 | param.rf_protocol = NCI_RF_PROTOCOL_T1T; | 483 | param.rf_protocol = NCI_RF_PROTOCOL_T1T; |
diff --git a/net/nfc/nci/ntf.c b/net/nfc/nci/ntf.c index 2e3dee42196d..99e1632e6aac 100644 --- a/net/nfc/nci/ntf.c +++ b/net/nfc/nci/ntf.c | |||
@@ -227,7 +227,7 @@ static void nci_add_new_target(struct nci_dev *ndev, | |||
227 | 227 | ||
228 | for (i = 0; i < ndev->n_targets; i++) { | 228 | for (i = 0; i < ndev->n_targets; i++) { |
229 | target = &ndev->targets[i]; | 229 | target = &ndev->targets[i]; |
230 | if (target->idx == ntf->rf_discovery_id) { | 230 | if (target->logical_idx == ntf->rf_discovery_id) { |
231 | /* This target already exists, add the new protocol */ | 231 | /* This target already exists, add the new protocol */ |
232 | nci_add_new_protocol(ndev, target, ntf->rf_protocol, | 232 | nci_add_new_protocol(ndev, target, ntf->rf_protocol, |
233 | ntf->rf_tech_and_mode, | 233 | ntf->rf_tech_and_mode, |
@@ -248,10 +248,10 @@ static void nci_add_new_target(struct nci_dev *ndev, | |||
248 | ntf->rf_tech_and_mode, | 248 | ntf->rf_tech_and_mode, |
249 | &ntf->rf_tech_specific_params); | 249 | &ntf->rf_tech_specific_params); |
250 | if (!rc) { | 250 | if (!rc) { |
251 | target->idx = ntf->rf_discovery_id; | 251 | target->logical_idx = ntf->rf_discovery_id; |
252 | ndev->n_targets++; | 252 | ndev->n_targets++; |
253 | 253 | ||
254 | pr_debug("target_idx %d, n_targets %d\n", target->idx, | 254 | pr_debug("logical idx %d, n_targets %d\n", target->logical_idx, |
255 | ndev->n_targets); | 255 | ndev->n_targets); |
256 | } | 256 | } |
257 | } | 257 | } |
@@ -372,10 +372,11 @@ static void nci_target_auto_activated(struct nci_dev *ndev, | |||
372 | if (rc) | 372 | if (rc) |
373 | return; | 373 | return; |
374 | 374 | ||
375 | target->idx = ntf->rf_discovery_id; | 375 | target->logical_idx = ntf->rf_discovery_id; |
376 | ndev->n_targets++; | 376 | ndev->n_targets++; |
377 | 377 | ||
378 | pr_debug("target_idx %d, n_targets %d\n", target->idx, ndev->n_targets); | 378 | pr_debug("logical idx %d, n_targets %d\n", |
379 | target->logical_idx, ndev->n_targets); | ||
379 | 380 | ||
380 | nfc_targets_found(ndev->nfc_dev, ndev->targets, ndev->n_targets); | 381 | nfc_targets_found(ndev->nfc_dev, ndev->targets, ndev->n_targets); |
381 | } | 382 | } |
diff --git a/net/nfc/netlink.c b/net/nfc/netlink.c index 8937664674fa..f1829f6ae9c5 100644 --- a/net/nfc/netlink.c +++ b/net/nfc/netlink.c | |||
@@ -188,6 +188,37 @@ free_msg: | |||
188 | return -EMSGSIZE; | 188 | return -EMSGSIZE; |
189 | } | 189 | } |
190 | 190 | ||
191 | int nfc_genl_target_lost(struct nfc_dev *dev, u32 target_idx) | ||
192 | { | ||
193 | struct sk_buff *msg; | ||
194 | void *hdr; | ||
195 | |||
196 | msg = nlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); | ||
197 | if (!msg) | ||
198 | return -ENOMEM; | ||
199 | |||
200 | hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0, | ||
201 | NFC_EVENT_TARGET_LOST); | ||
202 | if (!hdr) | ||
203 | goto free_msg; | ||
204 | |||
205 | if (nla_put_string(msg, NFC_ATTR_DEVICE_NAME, nfc_device_name(dev)) || | ||
206 | nla_put_u32(msg, NFC_ATTR_TARGET_INDEX, target_idx)) | ||
207 | goto nla_put_failure; | ||
208 | |||
209 | genlmsg_end(msg, hdr); | ||
210 | |||
211 | genlmsg_multicast(msg, 0, nfc_genl_event_mcgrp.id, GFP_KERNEL); | ||
212 | |||
213 | return 0; | ||
214 | |||
215 | nla_put_failure: | ||
216 | genlmsg_cancel(msg, hdr); | ||
217 | free_msg: | ||
218 | nlmsg_free(msg); | ||
219 | return -EMSGSIZE; | ||
220 | } | ||
221 | |||
191 | int nfc_genl_device_added(struct nfc_dev *dev) | 222 | int nfc_genl_device_added(struct nfc_dev *dev) |
192 | { | 223 | { |
193 | struct sk_buff *msg; | 224 | struct sk_buff *msg; |
diff --git a/net/nfc/nfc.h b/net/nfc/nfc.h index 061a830b0a88..7d589a81942e 100644 --- a/net/nfc/nfc.h +++ b/net/nfc/nfc.h | |||
@@ -119,6 +119,7 @@ void nfc_genl_data_init(struct nfc_genl_data *genl_data); | |||
119 | void nfc_genl_data_exit(struct nfc_genl_data *genl_data); | 119 | void nfc_genl_data_exit(struct nfc_genl_data *genl_data); |
120 | 120 | ||
121 | int nfc_genl_targets_found(struct nfc_dev *dev); | 121 | int nfc_genl_targets_found(struct nfc_dev *dev); |
122 | int nfc_genl_target_lost(struct nfc_dev *dev, u32 target_idx); | ||
122 | 123 | ||
123 | int nfc_genl_device_added(struct nfc_dev *dev); | 124 | int nfc_genl_device_added(struct nfc_dev *dev); |
124 | int nfc_genl_device_removed(struct nfc_dev *dev); | 125 | int nfc_genl_device_removed(struct nfc_dev *dev); |
diff --git a/net/nfc/rawsock.c b/net/nfc/rawsock.c index 5a839ceb2e82..ec1134c9e07f 100644 --- a/net/nfc/rawsock.c +++ b/net/nfc/rawsock.c | |||
@@ -92,6 +92,12 @@ static int rawsock_connect(struct socket *sock, struct sockaddr *_addr, | |||
92 | goto error; | 92 | goto error; |
93 | } | 93 | } |
94 | 94 | ||
95 | if (addr->target_idx > dev->target_next_idx - 1 || | ||
96 | addr->target_idx < dev->target_next_idx - dev->n_targets) { | ||
97 | rc = -EINVAL; | ||
98 | goto error; | ||
99 | } | ||
100 | |||
95 | rc = nfc_activate_target(dev, addr->target_idx, addr->nfc_protocol); | 101 | rc = nfc_activate_target(dev, addr->target_idx, addr->nfc_protocol); |
96 | if (rc) | 102 | if (rc) |
97 | goto put_dev; | 103 | goto put_dev; |