aboutsummaryrefslogtreecommitdiffstats
path: root/net
diff options
context:
space:
mode:
authorSamuel Ortiz <sameo@linux.intel.com>2011-12-14 10:43:12 -0500
committerJohn W. Linville <linville@tuxdriver.com>2011-12-14 14:50:13 -0500
commitd646960f7986fefb460a2b062d5ccc8ccfeacc3a (patch)
tree0624d338715a8d275a39fbfce074df5c5d2783f5 /net
parent361f3cb7f9cfdb82c80926d0e7843c098c034545 (diff)
NFC: Initial LLCP support
This patch is an initial implementation for the NFC Logical Link Control protocol. It's also known as NFC peer to peer mode. This is a basic implementation as it lacks SDP (services Discovery Protocol), frames aggregation support, and frame rejecion parsing. Follow up patches will implement those missing features. This code has been tested against a Nexus S phone implementing LLCP 1.0. Signed-off-by: Samuel Ortiz <sameo@linux.intel.com> Signed-off-by: John W. Linville <linville@tuxdriver.com>
Diffstat (limited to 'net')
-rw-r--r--net/nfc/Kconfig1
-rw-r--r--net/nfc/Makefile1
-rw-r--r--net/nfc/core.c20
-rw-r--r--net/nfc/llcp/Kconfig7
-rw-r--r--net/nfc/llcp/commands.c399
-rw-r--r--net/nfc/llcp/llcp.c973
-rw-r--r--net/nfc/llcp/llcp.h193
-rw-r--r--net/nfc/llcp/sock.c675
-rw-r--r--net/nfc/nfc.h54
9 files changed, 2321 insertions, 2 deletions
diff --git a/net/nfc/Kconfig b/net/nfc/Kconfig
index 58cddadf8e8e..44c865b86d6f 100644
--- a/net/nfc/Kconfig
+++ b/net/nfc/Kconfig
@@ -14,5 +14,6 @@ menuconfig NFC
14 be called nfc. 14 be called nfc.
15 15
16source "net/nfc/nci/Kconfig" 16source "net/nfc/nci/Kconfig"
17source "net/nfc/llcp/Kconfig"
17 18
18source "drivers/nfc/Kconfig" 19source "drivers/nfc/Kconfig"
diff --git a/net/nfc/Makefile b/net/nfc/Makefile
index fbb550f2377b..7b4a6dcfa566 100644
--- a/net/nfc/Makefile
+++ b/net/nfc/Makefile
@@ -6,3 +6,4 @@ obj-$(CONFIG_NFC) += nfc.o
6obj-$(CONFIG_NFC_NCI) += nci/ 6obj-$(CONFIG_NFC_NCI) += nci/
7 7
8nfc-objs := core.o netlink.o af_nfc.o rawsock.o 8nfc-objs := core.o netlink.o af_nfc.o rawsock.o
9nfc-$(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 3a45f21b3b97..3ddf6e698df0 100644
--- a/net/nfc/core.c
+++ b/net/nfc/core.c
@@ -240,6 +240,7 @@ int nfc_dep_link_down(struct nfc_dev *dev)
240 rc = dev->ops->dep_link_down(dev); 240 rc = dev->ops->dep_link_down(dev);
241 if (!rc) { 241 if (!rc) {
242 dev->dep_link_up = false; 242 dev->dep_link_up = false;
243 nfc_llcp_mac_is_down(dev);
243 nfc_genl_dep_link_down_event(dev); 244 nfc_genl_dep_link_down_event(dev);
244 } 245 }
245 246
@@ -254,6 +255,8 @@ int nfc_dep_link_is_up(struct nfc_dev *dev, u32 target_idx,
254 dev->dep_link_up = true; 255 dev->dep_link_up = true;
255 dev->dep_rf_mode = rf_mode; 256 dev->dep_rf_mode = rf_mode;
256 257
258 nfc_llcp_mac_is_up(dev, target_idx, comm_mode, rf_mode);
259
257 return nfc_genl_dep_link_up_event(dev, target_idx, comm_mode, rf_mode); 260 return nfc_genl_dep_link_up_event(dev, target_idx, comm_mode, rf_mode);
258} 261}
259EXPORT_SYMBOL(nfc_dep_link_is_up); 262EXPORT_SYMBOL(nfc_dep_link_is_up);
@@ -360,13 +363,13 @@ int nfc_set_remote_general_bytes(struct nfc_dev *dev, u8 *gb, u8 gb_len)
360 if (gb_len > NFC_MAX_GT_LEN) 363 if (gb_len > NFC_MAX_GT_LEN)
361 return -EINVAL; 364 return -EINVAL;
362 365
363 return 0; 366 return nfc_llcp_set_remote_gb(dev, gb, gb_len);
364} 367}
365EXPORT_SYMBOL(nfc_set_remote_general_bytes); 368EXPORT_SYMBOL(nfc_set_remote_general_bytes);
366 369
367u8 *nfc_get_local_general_bytes(struct nfc_dev *dev, u8 *gt_len) 370u8 *nfc_get_local_general_bytes(struct nfc_dev *dev, u8 *gt_len)
368{ 371{
369 return NULL; 372 return nfc_llcp_general_bytes(dev, gt_len);
370} 373}
371EXPORT_SYMBOL(nfc_get_local_general_bytes); 374EXPORT_SYMBOL(nfc_get_local_general_bytes);
372 375
@@ -560,6 +563,10 @@ int nfc_register_device(struct nfc_dev *dev)
560 if (rc < 0) 563 if (rc < 0)
561 return rc; 564 return rc;
562 565
566 rc = nfc_llcp_register_device(dev);
567 if (rc)
568 pr_err("Could not register llcp device\n");
569
563 rc = nfc_genl_device_added(dev); 570 rc = nfc_genl_device_added(dev);
564 if (rc) 571 if (rc)
565 pr_debug("The userspace won't be notified that the device %s was added\n", 572 pr_debug("The userspace won't be notified that the device %s was added\n",
@@ -591,6 +598,8 @@ void nfc_unregister_device(struct nfc_dev *dev)
591 598
592 mutex_unlock(&nfc_devlist_mutex); 599 mutex_unlock(&nfc_devlist_mutex);
593 600
601 nfc_llcp_unregister_device(dev);
602
594 rc = nfc_genl_device_removed(dev); 603 rc = nfc_genl_device_removed(dev);
595 if (rc) 604 if (rc)
596 pr_debug("The userspace won't be notified that the device %s was removed\n", 605 pr_debug("The userspace won't be notified that the device %s was removed\n",
@@ -620,6 +629,10 @@ static int __init nfc_init(void)
620 if (rc) 629 if (rc)
621 goto err_rawsock; 630 goto err_rawsock;
622 631
632 rc = nfc_llcp_init();
633 if (rc)
634 goto err_llcp_sock;
635
623 rc = af_nfc_init(); 636 rc = af_nfc_init();
624 if (rc) 637 if (rc)
625 goto err_af_nfc; 638 goto err_af_nfc;
@@ -627,6 +640,8 @@ static int __init nfc_init(void)
627 return 0; 640 return 0;
628 641
629err_af_nfc: 642err_af_nfc:
643 nfc_llcp_exit();
644err_llcp_sock:
630 rawsock_exit(); 645 rawsock_exit();
631err_rawsock: 646err_rawsock:
632 nfc_genl_exit(); 647 nfc_genl_exit();
@@ -638,6 +653,7 @@ err_genl:
638static void __exit nfc_exit(void) 653static void __exit nfc_exit(void)
639{ 654{
640 af_nfc_exit(); 655 af_nfc_exit();
656 nfc_llcp_exit();
641 rawsock_exit(); 657 rawsock_exit();
642 nfc_genl_exit(); 658 nfc_genl_exit();
643 class_unregister(&nfc_class); 659 class_unregister(&nfc_class);
diff --git a/net/nfc/llcp/Kconfig b/net/nfc/llcp/Kconfig
new file mode 100644
index 000000000000..fbf5e8150908
--- /dev/null
+++ b/net/nfc/llcp/Kconfig
@@ -0,0 +1,7 @@
1config NFC_LLCP
2 depends on NFC && EXPERIMENTAL
3 bool "NFC LLCP support (EXPERIMENTAL)"
4 default n
5 help
6 Say Y here if you want to build support for a kernel NFC LLCP
7 implementation. \ No newline at end of file
diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c
new file mode 100644
index 000000000000..151f2ef429c4
--- /dev/null
+++ b/net/nfc/llcp/commands.c
@@ -0,0 +1,399 @@
1/*
2 * Copyright (C) 2011 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) "llcp: %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
29#include "../nfc.h"
30#include "llcp.h"
31
32static u8 llcp_tlv_length[LLCP_TLV_MAX] = {
33 0,
34 1, /* VERSION */
35 2, /* MIUX */
36 2, /* WKS */
37 1, /* LTO */
38 1, /* RW */
39 0, /* SN */
40 1, /* OPT */
41 0, /* SDREQ */
42 2, /* SDRES */
43
44};
45
46static u8 llcp_tlv8(u8 *tlv, u8 type)
47{
48 if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]])
49 return 0;
50
51 return tlv[2];
52}
53
54static u8 llcp_tlv16(u8 *tlv, u8 type)
55{
56 if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]])
57 return 0;
58
59 return be16_to_cpu(*((__be16 *)(tlv + 2)));
60}
61
62
63static u8 llcp_tlv_version(u8 *tlv)
64{
65 return llcp_tlv8(tlv, LLCP_TLV_VERSION);
66}
67
68static u16 llcp_tlv_miux(u8 *tlv)
69{
70 return llcp_tlv16(tlv, LLCP_TLV_MIUX) & 0x7f;
71}
72
73static u16 llcp_tlv_wks(u8 *tlv)
74{
75 return llcp_tlv16(tlv, LLCP_TLV_WKS);
76}
77
78static u16 llcp_tlv_lto(u8 *tlv)
79{
80 return llcp_tlv8(tlv, LLCP_TLV_LTO);
81}
82
83static u8 llcp_tlv_opt(u8 *tlv)
84{
85 return llcp_tlv8(tlv, LLCP_TLV_OPT);
86}
87
88static u8 llcp_tlv_rw(u8 *tlv)
89{
90 return llcp_tlv8(tlv, LLCP_TLV_RW) & 0xf;
91}
92
93u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length)
94{
95 u8 *tlv, length;
96
97 pr_debug("type %d\n", type);
98
99 if (type >= LLCP_TLV_MAX)
100 return NULL;
101
102 length = llcp_tlv_length[type];
103 if (length == 0 && value_length == 0)
104 return NULL;
105 else
106 length = value_length;
107
108 *tlv_length = 2 + length;
109 tlv = kzalloc(2 + length, GFP_KERNEL);
110 if (tlv == NULL)
111 return tlv;
112
113 tlv[0] = type;
114 tlv[1] = length;
115 memcpy(tlv + 2, value, length);
116
117 return tlv;
118}
119
120int nfc_llcp_parse_tlv(struct nfc_llcp_local *local,
121 u8 *tlv_array, u16 tlv_array_len)
122{
123 u8 *tlv = tlv_array, type, length, offset = 0;
124
125 pr_debug("TLV array length %d\n", tlv_array_len);
126
127 if (local == NULL)
128 return -ENODEV;
129
130 while (offset < tlv_array_len) {
131 type = tlv[0];
132 length = tlv[1];
133
134 pr_debug("type 0x%x length %d\n", type, length);
135
136 switch (type) {
137 case LLCP_TLV_VERSION:
138 local->remote_version = llcp_tlv_version(tlv);
139 break;
140 case LLCP_TLV_MIUX:
141 local->remote_miu = llcp_tlv_miux(tlv) + 128;
142 break;
143 case LLCP_TLV_WKS:
144 local->remote_wks = llcp_tlv_wks(tlv);
145 break;
146 case LLCP_TLV_LTO:
147 local->remote_lto = llcp_tlv_lto(tlv) * 10;
148 break;
149 case LLCP_TLV_OPT:
150 local->remote_opt = llcp_tlv_opt(tlv);
151 break;
152 case LLCP_TLV_RW:
153 local->remote_rw = llcp_tlv_rw(tlv);
154 break;
155 default:
156 pr_err("Invalid gt tlv value 0x%x\n", type);
157 break;
158 }
159
160 offset += length + 2;
161 tlv += length + 2;
162 }
163
164 pr_debug("version 0x%x miu %d lto %d opt 0x%x wks 0x%x rw %d\n",
165 local->remote_version, local->remote_miu,
166 local->remote_lto, local->remote_opt,
167 local->remote_wks, local->remote_rw);
168
169 return 0;
170}
171
172static struct sk_buff *llcp_add_header(struct sk_buff *pdu,
173 u8 dsap, u8 ssap, u8 ptype)
174{
175 u8 header[2];
176
177 pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap);
178
179 header[0] = (u8)((dsap << 2) | (ptype >> 2));
180 header[1] = (u8)((ptype << 6) | ssap);
181
182 pr_debug("header 0x%x 0x%x\n", header[0], header[1]);
183
184 memcpy(skb_put(pdu, LLCP_HEADER_SIZE), header, LLCP_HEADER_SIZE);
185
186 return pdu;
187}
188
189static struct sk_buff *llcp_add_tlv(struct sk_buff *pdu, u8 *tlv, u8 tlv_length)
190{
191 /* XXX Add an skb length check */
192
193 if (tlv == NULL)
194 return NULL;
195
196 memcpy(skb_put(pdu, tlv_length), tlv, tlv_length);
197
198 return pdu;
199}
200
201static struct sk_buff *llcp_allocate_pdu(struct nfc_llcp_sock *sock,
202 u8 cmd, u16 size)
203{
204 struct sk_buff *skb;
205 int err;
206
207 if (sock->ssap == 0)
208 return NULL;
209
210 skb = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT,
211 size + LLCP_HEADER_SIZE, &err);
212 if (skb == NULL) {
213 pr_err("Could not allocate PDU\n");
214 return NULL;
215 }
216
217 skb = llcp_add_header(skb, sock->dsap, sock->ssap, cmd);
218
219 return skb;
220}
221
222int nfc_llcp_disconnect(struct nfc_llcp_sock *sock)
223{
224 struct sk_buff *skb;
225 struct nfc_dev *dev;
226 struct nfc_llcp_local *local;
227 u16 size = 0;
228
229 pr_debug("Sending DISC\n");
230
231 local = sock->local;
232 if (local == NULL)
233 return -ENODEV;
234
235 dev = sock->dev;
236 if (dev == NULL)
237 return -ENODEV;
238
239 size += LLCP_HEADER_SIZE;
240 size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
241
242 skb = alloc_skb(size, GFP_ATOMIC);
243 if (skb == NULL)
244 return -ENOMEM;
245
246 skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
247
248 skb = llcp_add_header(skb, sock->ssap, sock->dsap, LLCP_PDU_DISC);
249
250 skb_queue_tail(&local->tx_queue, skb);
251
252 return 0;
253}
254
255int nfc_llcp_send_symm(struct nfc_dev *dev)
256{
257 struct sk_buff *skb;
258 struct nfc_llcp_local *local;
259 u16 size = 0;
260
261 pr_debug("Sending SYMM\n");
262
263 local = nfc_llcp_find_local(dev);
264 if (local == NULL)
265 return -ENODEV;
266
267 size += LLCP_HEADER_SIZE;
268 size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
269
270 skb = alloc_skb(size, GFP_KERNEL);
271 if (skb == NULL)
272 return -ENOMEM;
273
274 skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
275
276 skb = llcp_add_header(skb, 0, 0, LLCP_PDU_SYMM);
277
278 return nfc_data_exchange(dev, local->target_idx, skb,
279 nfc_llcp_recv, local);
280}
281
282int nfc_llcp_send_connect(struct nfc_llcp_sock *sock)
283{
284 struct nfc_llcp_local *local;
285 struct sk_buff *skb;
286 u8 *service_name_tlv = NULL, service_name_tlv_length;
287 int err;
288 u16 size = 0;
289
290 pr_debug("Sending CONNECT\n");
291
292 local = sock->local;
293 if (local == NULL)
294 return -ENODEV;
295
296 if (sock->service_name != NULL) {
297 service_name_tlv = nfc_llcp_build_tlv(LLCP_TLV_SN,
298 sock->service_name,
299 sock->service_name_len,
300 &service_name_tlv_length);
301 size += service_name_tlv_length;
302 }
303
304 pr_debug("SKB size %d SN length %zu\n", size, sock->service_name_len);
305
306 skb = llcp_allocate_pdu(sock, LLCP_PDU_CONNECT, size);
307 if (skb == NULL) {
308 err = -ENOMEM;
309 goto error_tlv;
310 }
311
312 if (service_name_tlv != NULL)
313 skb = llcp_add_tlv(skb, service_name_tlv,
314 service_name_tlv_length);
315
316 skb_queue_tail(&local->tx_queue, skb);
317
318 return 0;
319
320error_tlv:
321 pr_err("error %d\n", err);
322
323 kfree(service_name_tlv);
324
325 return err;
326}
327
328int nfc_llcp_send_cc(struct nfc_llcp_sock *sock)
329{
330 struct nfc_llcp_local *local;
331 struct sk_buff *skb;
332
333 pr_debug("Sending CC\n");
334
335 local = sock->local;
336 if (local == NULL)
337 return -ENODEV;
338
339 skb = llcp_allocate_pdu(sock, LLCP_PDU_CC, 0);
340 if (skb == NULL)
341 return -ENOMEM;
342
343 skb_queue_tail(&local->tx_queue, skb);
344
345 return 0;
346}
347
348int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason)
349{
350 struct sk_buff *skb;
351 struct nfc_dev *dev;
352 u16 size = 1; /* Reason code */
353
354 pr_debug("Sending DM reason 0x%x\n", reason);
355
356 if (local == NULL)
357 return -ENODEV;
358
359 dev = local->dev;
360 if (dev == NULL)
361 return -ENODEV;
362
363 size += LLCP_HEADER_SIZE;
364 size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
365
366 skb = alloc_skb(size, GFP_KERNEL);
367 if (skb == NULL)
368 return -ENOMEM;
369
370 skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
371
372 skb = llcp_add_header(skb, ssap, dsap, LLCP_PDU_DM);
373
374 memcpy(skb_put(skb, 1), &reason, 1);
375
376 skb_queue_head(&local->tx_queue, skb);
377
378 return 0;
379}
380
381int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock)
382{
383 struct sk_buff *skb;
384 struct nfc_llcp_local *local;
385
386 pr_debug("Send DISC\n");
387
388 local = sock->local;
389 if (local == NULL)
390 return -ENODEV;
391
392 skb = llcp_allocate_pdu(sock, LLCP_PDU_DISC, 0);
393 if (skb == NULL)
394 return -ENOMEM;
395
396 skb_queue_head(&local->tx_queue, skb);
397
398 return 0;
399}
diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
new file mode 100644
index 000000000000..67756b23eac5
--- /dev/null
+++ b/net/nfc/llcp/llcp.c
@@ -0,0 +1,973 @@
1/*
2 * Copyright (C) 2011 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) "llcp: %s: " fmt, __func__
21
22#include <linux/init.h>
23#include <linux/kernel.h>
24#include <linux/list.h>
25#include <linux/nfc.h>
26
27#include "../nfc.h"
28#include "llcp.h"
29
30static u8 llcp_magic[3] = {0x46, 0x66, 0x6d};
31
32static struct list_head llcp_devices;
33
34static void nfc_llcp_socket_release(struct nfc_llcp_local *local)
35{
36 struct nfc_llcp_sock *parent, *s, *n;
37 struct sock *sk, *parent_sk;
38 int i;
39
40
41 mutex_lock(&local->socket_lock);
42
43 for (i = 0; i < LLCP_MAX_SAP; i++) {
44 parent = local->sockets[i];
45 if (parent == NULL)
46 continue;
47
48 /* Release all child sockets */
49 list_for_each_entry_safe(s, n, &parent->list, list) {
50 list_del(&s->list);
51 sk = &s->sk;
52
53 lock_sock(sk);
54
55 if (sk->sk_state == LLCP_CONNECTED)
56 nfc_put_device(s->dev);
57
58 sk->sk_state = LLCP_CLOSED;
59 sock_set_flag(sk, SOCK_DEAD);
60
61 release_sock(sk);
62 }
63
64 parent_sk = &parent->sk;
65
66 lock_sock(parent_sk);
67
68 if (parent_sk->sk_state == LLCP_LISTEN) {
69 struct nfc_llcp_sock *lsk, *n;
70 struct sock *accept_sk;
71
72 list_for_each_entry_safe(lsk, n, &parent->accept_queue,
73 accept_queue) {
74 accept_sk = &lsk->sk;
75 lock_sock(accept_sk);
76
77 nfc_llcp_accept_unlink(accept_sk);
78
79 accept_sk->sk_state = LLCP_CLOSED;
80 sock_set_flag(accept_sk, SOCK_DEAD);
81
82 release_sock(accept_sk);
83
84 sock_orphan(accept_sk);
85 }
86 }
87
88 if (parent_sk->sk_state == LLCP_CONNECTED)
89 nfc_put_device(parent->dev);
90
91 parent_sk->sk_state = LLCP_CLOSED;
92 sock_set_flag(parent_sk, SOCK_DEAD);
93
94 release_sock(parent_sk);
95 }
96
97 mutex_unlock(&local->socket_lock);
98}
99
100static void nfc_llcp_timeout_work(struct work_struct *work)
101{
102 struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
103 timeout_work);
104
105 nfc_dep_link_down(local->dev);
106}
107
108static void nfc_llcp_symm_timer(unsigned long data)
109{
110 struct nfc_llcp_local *local = (struct nfc_llcp_local *) data;
111
112 pr_err("SYMM timeout\n");
113
114 queue_work(local->timeout_wq, &local->timeout_work);
115}
116
117struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev)
118{
119 struct nfc_llcp_local *local, *n;
120
121 list_for_each_entry_safe(local, n, &llcp_devices, list)
122 if (local->dev == dev)
123 return local;
124
125 pr_debug("No device found\n");
126
127 return NULL;
128}
129
130static char *wks[] = {
131 NULL,
132 NULL, /* SDP */
133 "urn:nfc:sn:ip",
134 "urn:nfc:sn:obex",
135 "urn:nfc:sn:snep",
136};
137
138static int nfc_llcp_wks_sap(char *service_name, size_t service_name_len)
139{
140 int sap, num_wks;
141
142 pr_debug("%s\n", service_name);
143
144 if (service_name == NULL)
145 return -EINVAL;
146
147 num_wks = ARRAY_SIZE(wks);
148
149 for (sap = 0 ; sap < num_wks; sap++) {
150 if (wks[sap] == NULL)
151 continue;
152
153 if (strncmp(wks[sap], service_name, service_name_len) == 0)
154 return sap;
155 }
156
157 return -EINVAL;
158}
159
160u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
161 struct nfc_llcp_sock *sock)
162{
163 mutex_lock(&local->sdp_lock);
164
165 if (sock->service_name != NULL && sock->service_name_len > 0) {
166 int ssap = nfc_llcp_wks_sap(sock->service_name,
167 sock->service_name_len);
168
169 if (ssap > 0) {
170 pr_debug("WKS %d\n", ssap);
171
172 /* This is a WKS, let's check if it's free */
173 if (local->local_wks & BIT(ssap)) {
174 mutex_unlock(&local->sdp_lock);
175
176 return LLCP_SAP_MAX;
177 }
178
179 set_bit(BIT(ssap), &local->local_wks);
180 mutex_unlock(&local->sdp_lock);
181
182 return ssap;
183 }
184
185 /*
186 * This is not a well known service,
187 * we should try to find a local SDP free spot
188 */
189 ssap = find_first_zero_bit(&local->local_sdp, LLCP_SDP_NUM_SAP);
190 if (ssap == LLCP_SDP_NUM_SAP) {
191 mutex_unlock(&local->sdp_lock);
192
193 return LLCP_SAP_MAX;
194 }
195
196 pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap);
197
198 set_bit(BIT(ssap), &local->local_sdp);
199 mutex_unlock(&local->sdp_lock);
200
201 return LLCP_WKS_NUM_SAP + ssap;
202
203 } else if (sock->ssap != 0) {
204 if (sock->ssap < LLCP_WKS_NUM_SAP) {
205 if (!(local->local_wks & BIT(sock->ssap))) {
206 set_bit(BIT(sock->ssap), &local->local_wks);
207 mutex_unlock(&local->sdp_lock);
208
209 return sock->ssap;
210 }
211
212 } else if (sock->ssap < LLCP_SDP_NUM_SAP) {
213 if (!(local->local_sdp &
214 BIT(sock->ssap - LLCP_WKS_NUM_SAP))) {
215 set_bit(BIT(sock->ssap - LLCP_WKS_NUM_SAP),
216 &local->local_sdp);
217 mutex_unlock(&local->sdp_lock);
218
219 return sock->ssap;
220 }
221 }
222 }
223
224 mutex_unlock(&local->sdp_lock);
225
226 return LLCP_SAP_MAX;
227}
228
229u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local)
230{
231 u8 local_ssap;
232
233 mutex_lock(&local->sdp_lock);
234
235 local_ssap = find_first_zero_bit(&local->local_sap, LLCP_LOCAL_NUM_SAP);
236 if (local_ssap == LLCP_LOCAL_NUM_SAP) {
237 mutex_unlock(&local->sdp_lock);
238 return LLCP_SAP_MAX;
239 }
240
241 set_bit(BIT(local_ssap), &local->local_sap);
242
243 mutex_unlock(&local->sdp_lock);
244
245 return local_ssap + LLCP_LOCAL_SAP_OFFSET;
246}
247
248void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap)
249{
250 u8 local_ssap;
251 unsigned long *sdp;
252
253 if (ssap < LLCP_WKS_NUM_SAP) {
254 local_ssap = ssap;
255 sdp = &local->local_wks;
256 } else if (ssap < LLCP_LOCAL_NUM_SAP) {
257 local_ssap = ssap - LLCP_WKS_NUM_SAP;
258 sdp = &local->local_sdp;
259 } else if (ssap < LLCP_MAX_SAP) {
260 local_ssap = ssap - LLCP_LOCAL_NUM_SAP;
261 sdp = &local->local_sap;
262 } else {
263 return;
264 }
265
266 mutex_lock(&local->sdp_lock);
267
268 clear_bit(1 << local_ssap, sdp);
269
270 mutex_unlock(&local->sdp_lock);
271}
272
273u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, u8 *general_bytes_len)
274{
275 struct nfc_llcp_local *local;
276
277 local = nfc_llcp_find_local(dev);
278 if (local == NULL) {
279 *general_bytes_len = 0;
280 return NULL;
281 }
282
283 *general_bytes_len = local->gb_len;
284
285 return local->gb;
286}
287
288static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
289{
290 u8 *gb_cur, *version_tlv, version, version_length;
291 u8 *lto_tlv, lto, lto_length;
292 u8 *wks_tlv, wks_length;
293 u8 gb_len = 0;
294
295 version = LLCP_VERSION_11;
296 version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version,
297 1, &version_length);
298 gb_len += version_length;
299
300 /* 1500 ms */
301 lto = 150;
302 lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &lto, 1, &lto_length);
303 gb_len += lto_length;
304
305 pr_debug("Local wks 0x%lx\n", local->local_wks);
306 wks_tlv = nfc_llcp_build_tlv(LLCP_TLV_WKS, (u8 *)&local->local_wks, 2,
307 &wks_length);
308 gb_len += wks_length;
309
310 gb_len += ARRAY_SIZE(llcp_magic);
311
312 if (gb_len > NFC_MAX_GT_LEN) {
313 kfree(version_tlv);
314 return -EINVAL;
315 }
316
317 gb_cur = local->gb;
318
319 memcpy(gb_cur, llcp_magic, ARRAY_SIZE(llcp_magic));
320 gb_cur += ARRAY_SIZE(llcp_magic);
321
322 memcpy(gb_cur, version_tlv, version_length);
323 gb_cur += version_length;
324
325 memcpy(gb_cur, lto_tlv, lto_length);
326 gb_cur += lto_length;
327
328 memcpy(gb_cur, wks_tlv, wks_length);
329 gb_cur += wks_length;
330
331 kfree(version_tlv);
332 kfree(lto_tlv);
333
334 local->gb_len = gb_len;
335
336 return 0;
337}
338
339int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len)
340{
341 struct nfc_llcp_local *local = nfc_llcp_find_local(dev);
342
343 if (local == NULL) {
344 pr_err("No LLCP device\n");
345 return -ENODEV;
346 }
347
348 memset(local->remote_gb, 0, NFC_MAX_GT_LEN);
349 memcpy(local->remote_gb, gb, gb_len);
350 local->remote_gb_len = gb_len;
351
352 if (local->remote_gb == NULL ||
353 local->remote_gb_len == 0)
354 return -ENODEV;
355
356 if (memcmp(local->remote_gb, llcp_magic, 3)) {
357 pr_err("MAC does not support LLCP\n");
358 return -EINVAL;
359 }
360
361 return nfc_llcp_parse_tlv(local,
362 &local->remote_gb[3], local->remote_gb_len - 3);
363}
364
365static void nfc_llcp_tx_work(struct work_struct *work)
366{
367 struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
368 tx_work);
369 struct sk_buff *skb;
370
371 skb = skb_dequeue(&local->tx_queue);
372 if (skb != NULL) {
373 pr_debug("Sending pending skb\n");
374 nfc_data_exchange(local->dev, local->target_idx,
375 skb, nfc_llcp_recv, local);
376 } else {
377 nfc_llcp_send_symm(local->dev);
378 }
379
380 mod_timer(&local->link_timer,
381 jiffies + msecs_to_jiffies(local->remote_lto));
382}
383
384static u8 nfc_llcp_dsap(struct sk_buff *pdu)
385{
386 return (pdu->data[0] & 0xfc) >> 2;
387}
388
389static u8 nfc_llcp_ptype(struct sk_buff *pdu)
390{
391 return ((pdu->data[0] & 0x03) << 2) | ((pdu->data[1] & 0xc0) >> 6);
392}
393
394static u8 nfc_llcp_ssap(struct sk_buff *pdu)
395{
396 return pdu->data[1] & 0x3f;
397}
398
399static u8 nfc_llcp_ns(struct sk_buff *pdu)
400{
401 return pdu->data[2] >> 4;
402}
403
404static u8 nfc_llcp_nr(struct sk_buff *pdu)
405{
406 return pdu->data[2] & 0xf;
407}
408
409static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu)
410{
411 pdu->data[2] = (sock->send_n << 4) | ((sock->recv_n - 1) % 16);
412 sock->send_n = (sock->send_n + 1) % 16;
413 sock->recv_ack_n = (sock->recv_n - 1) % 16;
414}
415
416static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
417 u8 ssap, u8 dsap)
418{
419 struct nfc_llcp_sock *sock, *llcp_sock, *n;
420
421 if (ssap == 0 && dsap == 0)
422 return NULL;
423
424 mutex_lock(&local->socket_lock);
425 sock = local->sockets[ssap];
426 if (sock == NULL) {
427 mutex_unlock(&local->socket_lock);
428 return NULL;
429 }
430
431 pr_debug("root dsap %d (%d)\n", sock->dsap, dsap);
432
433 if (sock->dsap == dsap) {
434 sock_hold(&sock->sk);
435 mutex_unlock(&local->socket_lock);
436 return sock;
437 }
438
439 list_for_each_entry_safe(llcp_sock, n, &sock->list, list) {
440 pr_debug("llcp_sock %p sk %p dsap %d\n", llcp_sock,
441 &llcp_sock->sk, llcp_sock->dsap);
442 if (llcp_sock->dsap == dsap) {
443 sock_hold(&llcp_sock->sk);
444 mutex_unlock(&local->socket_lock);
445 return llcp_sock;
446 }
447 }
448
449 pr_err("Could not find socket for %d %d\n", ssap, dsap);
450
451 mutex_unlock(&local->socket_lock);
452
453 return NULL;
454}
455
456static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock)
457{
458 sock_put(&sock->sk);
459}
460
461static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len)
462{
463 u8 *tlv = &skb->data[2], type, length;
464 size_t tlv_array_len = skb->len - LLCP_HEADER_SIZE, offset = 0;
465
466 while (offset < tlv_array_len) {
467 type = tlv[0];
468 length = tlv[1];
469
470 pr_debug("type 0x%x length %d\n", type, length);
471
472 if (type == LLCP_TLV_SN) {
473 *sn_len = length;
474 return &tlv[2];
475 }
476
477 offset += length + 2;
478 tlv += length + 2;
479 }
480
481 return NULL;
482}
483
484static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
485 struct sk_buff *skb)
486{
487 struct sock *new_sk, *parent;
488 struct nfc_llcp_sock *sock, *new_sock;
489 u8 dsap, ssap, bound_sap, reason;
490
491 dsap = nfc_llcp_dsap(skb);
492 ssap = nfc_llcp_ssap(skb);
493
494 pr_debug("%d %d\n", dsap, ssap);
495
496 nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE],
497 skb->len - LLCP_HEADER_SIZE);
498
499 if (dsap != LLCP_SAP_SDP) {
500 bound_sap = dsap;
501
502 mutex_lock(&local->socket_lock);
503 sock = local->sockets[dsap];
504 if (sock == NULL) {
505 mutex_unlock(&local->socket_lock);
506 reason = LLCP_DM_NOBOUND;
507 goto fail;
508 }
509
510 sock_hold(&sock->sk);
511 mutex_unlock(&local->socket_lock);
512
513 lock_sock(&sock->sk);
514
515 if (sock->dsap == LLCP_SAP_SDP &&
516 sock->sk.sk_state == LLCP_LISTEN)
517 goto enqueue;
518 } else {
519 u8 *sn;
520 size_t sn_len;
521
522 sn = nfc_llcp_connect_sn(skb, &sn_len);
523 if (sn == NULL) {
524 reason = LLCP_DM_NOBOUND;
525 goto fail;
526 }
527
528 pr_debug("Service name length %zu\n", sn_len);
529
530 mutex_lock(&local->socket_lock);
531 for (bound_sap = 0; bound_sap < LLCP_LOCAL_SAP_OFFSET;
532 bound_sap++) {
533 sock = local->sockets[bound_sap];
534 if (sock == NULL)
535 continue;
536
537 if (sock->service_name == NULL ||
538 sock->service_name_len == 0)
539 continue;
540
541 if (sock->service_name_len != sn_len)
542 continue;
543
544 if (sock->dsap == LLCP_SAP_SDP &&
545 sock->sk.sk_state == LLCP_LISTEN &&
546 !memcmp(sn, sock->service_name, sn_len)) {
547 pr_debug("Found service name at SAP %d\n",
548 bound_sap);
549 sock_hold(&sock->sk);
550 mutex_unlock(&local->socket_lock);
551
552 lock_sock(&sock->sk);
553
554 goto enqueue;
555 }
556 }
557
558 }
559
560 mutex_unlock(&local->socket_lock);
561
562 reason = LLCP_DM_NOBOUND;
563 goto fail;
564
565enqueue:
566 parent = &sock->sk;
567
568 if (sk_acceptq_is_full(parent)) {
569 reason = LLCP_DM_REJ;
570 release_sock(&sock->sk);
571 sock_put(&sock->sk);
572 goto fail;
573 }
574
575 new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type,
576 GFP_ATOMIC);
577 if (new_sk == NULL) {
578 reason = LLCP_DM_REJ;
579 release_sock(&sock->sk);
580 sock_put(&sock->sk);
581 goto fail;
582 }
583
584 new_sock = nfc_llcp_sock(new_sk);
585 new_sock->dev = local->dev;
586 new_sock->local = local;
587 new_sock->nfc_protocol = sock->nfc_protocol;
588 new_sock->ssap = bound_sap;
589 new_sock->dsap = ssap;
590 new_sock->parent = parent;
591
592 pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk);
593
594 list_add_tail(&new_sock->list, &sock->list);
595
596 nfc_llcp_accept_enqueue(&sock->sk, new_sk);
597
598 nfc_get_device(local->dev->idx);
599
600 new_sk->sk_state = LLCP_CONNECTED;
601
602 /* Wake the listening processes */
603 parent->sk_data_ready(parent, 0);
604
605 /* Send CC */
606 nfc_llcp_send_cc(new_sock);
607
608 release_sock(&sock->sk);
609 sock_put(&sock->sk);
610
611 return;
612
613fail:
614 /* Send DM */
615 nfc_llcp_send_dm(local, dsap, ssap, reason);
616
617 return;
618
619}
620
621static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
622 struct sk_buff *skb)
623{
624 struct nfc_llcp_sock *llcp_sock;
625 struct sock *sk;
626 u8 dsap, ssap, ptype, ns, nr;
627
628 ptype = nfc_llcp_ptype(skb);
629 dsap = nfc_llcp_dsap(skb);
630 ssap = nfc_llcp_ssap(skb);
631 ns = nfc_llcp_ns(skb);
632 nr = nfc_llcp_nr(skb);
633
634 pr_debug("%d %d R %d S %d\n", dsap, ssap, nr, ns);
635
636 llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
637 if (llcp_sock == NULL) {
638 nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
639 return;
640 }
641
642 sk = &llcp_sock->sk;
643 lock_sock(sk);
644 if (sk->sk_state == LLCP_CLOSED) {
645 release_sock(sk);
646 nfc_llcp_sock_put(llcp_sock);
647 }
648
649 if (ns == llcp_sock->recv_n)
650 llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16;
651 else
652 pr_err("Received out of sequence I PDU\n");
653
654 /* Pass the payload upstream */
655 if (ptype == LLCP_PDU_I) {
656 pr_debug("I frame, queueing on %p\n", &llcp_sock->sk);
657
658 skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE);
659 if (sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
660 pr_err("receive queue is full\n");
661 skb_queue_head(&llcp_sock->tx_backlog_queue, skb);
662 }
663 }
664
665 /* Remove skbs from the pending queue */
666 if (llcp_sock->send_ack_n != nr) {
667 struct sk_buff *s, *tmp;
668
669 llcp_sock->send_ack_n = nr;
670
671 skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp)
672 if (nfc_llcp_ns(s) <= nr) {
673 skb_unlink(s, &llcp_sock->tx_pending_queue);
674 kfree_skb(s);
675 }
676 }
677
678 /* Queue some I frames for transmission */
679 while (llcp_sock->remote_ready &&
680 skb_queue_len(&llcp_sock->tx_pending_queue) <= local->remote_rw) {
681 struct sk_buff *pdu, *pending_pdu;
682
683 pdu = skb_dequeue(&llcp_sock->tx_queue);
684 if (pdu == NULL)
685 break;
686
687 /* Update N(S)/N(R) */
688 nfc_llcp_set_nrns(llcp_sock, pdu);
689
690 pending_pdu = skb_clone(pdu, GFP_KERNEL);
691
692 skb_queue_tail(&local->tx_queue, pdu);
693 skb_queue_tail(&llcp_sock->tx_pending_queue, pending_pdu);
694 }
695
696 release_sock(sk);
697 nfc_llcp_sock_put(llcp_sock);
698}
699
700static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
701 struct sk_buff *skb)
702{
703 struct nfc_llcp_sock *llcp_sock;
704 struct sock *sk;
705 u8 dsap, ssap;
706
707 dsap = nfc_llcp_dsap(skb);
708 ssap = nfc_llcp_ssap(skb);
709
710 llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
711 if (llcp_sock == NULL) {
712 nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
713 return;
714 }
715
716 sk = &llcp_sock->sk;
717 lock_sock(sk);
718 if (sk->sk_state == LLCP_CLOSED) {
719 release_sock(sk);
720 nfc_llcp_sock_put(llcp_sock);
721 }
722
723
724 if (sk->sk_state == LLCP_CONNECTED) {
725 nfc_put_device(local->dev);
726 sk->sk_state = LLCP_CLOSED;
727 sk->sk_state_change(sk);
728 }
729
730 nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_DISC);
731
732 release_sock(sk);
733 nfc_llcp_sock_put(llcp_sock);
734}
735
736static void nfc_llcp_recv_cc(struct nfc_llcp_local *local,
737 struct sk_buff *skb)
738{
739 struct nfc_llcp_sock *llcp_sock;
740 u8 dsap, ssap;
741
742
743 dsap = nfc_llcp_dsap(skb);
744 ssap = nfc_llcp_ssap(skb);
745
746 llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
747
748 if (llcp_sock == NULL)
749 llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
750
751 if (llcp_sock == NULL) {
752 pr_err("Invalid CC\n");
753 nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
754
755 return;
756 }
757
758 llcp_sock->dsap = ssap;
759
760 nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE],
761 skb->len - LLCP_HEADER_SIZE);
762
763 nfc_llcp_sock_put(llcp_sock);
764}
765
766static void nfc_llcp_rx_work(struct work_struct *work)
767{
768 struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
769 rx_work);
770 u8 dsap, ssap, ptype;
771 struct sk_buff *skb;
772
773 skb = local->rx_pending;
774 if (skb == NULL) {
775 pr_debug("No pending SKB\n");
776 return;
777 }
778
779 ptype = nfc_llcp_ptype(skb);
780 dsap = nfc_llcp_dsap(skb);
781 ssap = nfc_llcp_ssap(skb);
782
783 pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap);
784
785 switch (ptype) {
786 case LLCP_PDU_SYMM:
787 pr_debug("SYMM\n");
788 break;
789
790 case LLCP_PDU_CONNECT:
791 pr_debug("CONNECT\n");
792 nfc_llcp_recv_connect(local, skb);
793 break;
794
795 case LLCP_PDU_DISC:
796 pr_debug("DISC\n");
797 nfc_llcp_recv_disc(local, skb);
798 break;
799
800 case LLCP_PDU_CC:
801 pr_debug("CC\n");
802 nfc_llcp_recv_cc(local, skb);
803 break;
804
805 case LLCP_PDU_I:
806 case LLCP_PDU_RR:
807 pr_debug("I frame\n");
808 nfc_llcp_recv_hdlc(local, skb);
809 break;
810
811 }
812
813 queue_work(local->tx_wq, &local->tx_work);
814 kfree_skb(local->rx_pending);
815 local->rx_pending = NULL;
816
817 return;
818}
819
820void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
821{
822 struct nfc_llcp_local *local = (struct nfc_llcp_local *) data;
823
824 pr_debug("Received an LLCP PDU\n");
825 if (err < 0) {
826 pr_err("err %d", err);
827 return;
828 }
829
830 local->rx_pending = skb_get(skb);
831 del_timer(&local->link_timer);
832 queue_work(local->rx_wq, &local->rx_work);
833
834 return;
835}
836
837void nfc_llcp_mac_is_down(struct nfc_dev *dev)
838{
839 struct nfc_llcp_local *local;
840
841 local = nfc_llcp_find_local(dev);
842 if (local == NULL)
843 return;
844
845 /* Close and purge all existing sockets */
846 nfc_llcp_socket_release(local);
847}
848
849void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
850 u8 comm_mode, u8 rf_mode)
851{
852 struct nfc_llcp_local *local;
853
854 pr_debug("rf mode %d\n", rf_mode);
855
856 local = nfc_llcp_find_local(dev);
857 if (local == NULL)
858 return;
859
860 local->target_idx = target_idx;
861 local->comm_mode = comm_mode;
862 local->rf_mode = rf_mode;
863
864 if (rf_mode == NFC_RF_INITIATOR) {
865 pr_debug("Queueing Tx work\n");
866
867 queue_work(local->tx_wq, &local->tx_work);
868 } else {
869 mod_timer(&local->link_timer,
870 jiffies + msecs_to_jiffies(local->remote_lto));
871 }
872}
873
874int nfc_llcp_register_device(struct nfc_dev *ndev)
875{
876 struct device *dev = &ndev->dev;
877 struct nfc_llcp_local *local;
878 char name[32];
879 int err;
880
881 local = kzalloc(sizeof(struct nfc_llcp_local), GFP_KERNEL);
882 if (local == NULL)
883 return -ENOMEM;
884
885 local->dev = ndev;
886 INIT_LIST_HEAD(&local->list);
887 mutex_init(&local->sdp_lock);
888 mutex_init(&local->socket_lock);
889 init_timer(&local->link_timer);
890 local->link_timer.data = (unsigned long) local;
891 local->link_timer.function = nfc_llcp_symm_timer;
892
893 skb_queue_head_init(&local->tx_queue);
894 INIT_WORK(&local->tx_work, nfc_llcp_tx_work);
895 snprintf(name, sizeof(name), "%s_llcp_tx_wq", dev_name(dev));
896 local->tx_wq = alloc_workqueue(name,
897 WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, 1);
898 if (local->tx_wq == NULL) {
899 err = -ENOMEM;
900 goto err_local;
901 }
902
903 local->rx_pending = NULL;
904 INIT_WORK(&local->rx_work, nfc_llcp_rx_work);
905 snprintf(name, sizeof(name), "%s_llcp_rx_wq", dev_name(dev));
906 local->rx_wq = alloc_workqueue(name,
907 WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, 1);
908 if (local->rx_wq == NULL) {
909 err = -ENOMEM;
910 goto err_tx_wq;
911 }
912
913 INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work);
914 snprintf(name, sizeof(name), "%s_llcp_timeout_wq", dev_name(dev));
915 local->timeout_wq = alloc_workqueue(name,
916 WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, 1);
917 if (local->timeout_wq == NULL) {
918 err = -ENOMEM;
919 goto err_rx_wq;
920 }
921
922 nfc_llcp_build_gb(local);
923
924 local->remote_miu = LLCP_DEFAULT_MIU;
925 local->remote_lto = LLCP_DEFAULT_LTO;
926 local->remote_rw = LLCP_DEFAULT_RW;
927
928 list_add(&llcp_devices, &local->list);
929
930 return 0;
931
932err_rx_wq:
933 destroy_workqueue(local->rx_wq);
934
935err_tx_wq:
936 destroy_workqueue(local->tx_wq);
937
938err_local:
939 kfree(local);
940
941 return 0;
942}
943
944void nfc_llcp_unregister_device(struct nfc_dev *dev)
945{
946 struct nfc_llcp_local *local = nfc_llcp_find_local(dev);
947
948 if (local == NULL) {
949 pr_debug("No such device\n");
950 return;
951 }
952
953 list_del(&local->list);
954 nfc_llcp_socket_release(local);
955 del_timer_sync(&local->link_timer);
956 skb_queue_purge(&local->tx_queue);
957 destroy_workqueue(local->tx_wq);
958 destroy_workqueue(local->rx_wq);
959 kfree(local->rx_pending);
960 kfree(local);
961}
962
963int __init nfc_llcp_init(void)
964{
965 INIT_LIST_HEAD(&llcp_devices);
966
967 return nfc_llcp_sock_init();
968}
969
970void nfc_llcp_exit(void)
971{
972 nfc_llcp_sock_exit();
973}
diff --git a/net/nfc/llcp/llcp.h b/net/nfc/llcp/llcp.h
new file mode 100644
index 000000000000..0ad2e3361584
--- /dev/null
+++ b/net/nfc/llcp/llcp.h
@@ -0,0 +1,193 @@
1/*
2 * Copyright (C) 2011 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
20enum llcp_state {
21 LLCP_CONNECTED = 1, /* wait_for_packet() wants that */
22 LLCP_CLOSED,
23 LLCP_BOUND,
24 LLCP_LISTEN,
25};
26
27#define LLCP_DEFAULT_LTO 100
28#define LLCP_DEFAULT_RW 1
29#define LLCP_DEFAULT_MIU 128
30
31#define LLCP_WKS_NUM_SAP 16
32#define LLCP_SDP_NUM_SAP 16
33#define LLCP_LOCAL_NUM_SAP 32
34#define LLCP_LOCAL_SAP_OFFSET (LLCP_WKS_NUM_SAP + LLCP_SDP_NUM_SAP)
35#define LLCP_MAX_SAP (LLCP_WKS_NUM_SAP + LLCP_SDP_NUM_SAP + LLCP_LOCAL_NUM_SAP)
36
37struct nfc_llcp_sock;
38
39struct nfc_llcp_local {
40 struct list_head list;
41 struct nfc_dev *dev;
42
43 struct mutex sdp_lock;
44 struct mutex socket_lock;
45
46 struct timer_list link_timer;
47 struct sk_buff_head tx_queue;
48 struct workqueue_struct *tx_wq;
49 struct work_struct tx_work;
50 struct workqueue_struct *rx_wq;
51 struct work_struct rx_work;
52 struct sk_buff *rx_pending;
53 struct workqueue_struct *timeout_wq;
54 struct work_struct timeout_work;
55
56 u32 target_idx;
57 u8 rf_mode;
58 u8 comm_mode;
59 unsigned long local_wks; /* Well known services */
60 unsigned long local_sdp; /* Local services */
61 unsigned long local_sap; /* Local SAPs, not available for discovery */
62
63 /* local */
64 u8 gb[NFC_MAX_GT_LEN];
65 u8 gb_len;
66
67 /* remote */
68 u8 remote_gb[NFC_MAX_GT_LEN];
69 u8 remote_gb_len;
70
71 u8 remote_version;
72 u16 remote_miu;
73 u16 remote_lto;
74 u8 remote_opt;
75 u16 remote_wks;
76 u8 remote_rw;
77
78 /* sockets array */
79 struct nfc_llcp_sock *sockets[LLCP_MAX_SAP];
80};
81
82struct nfc_llcp_sock {
83 struct sock sk;
84 struct list_head list;
85 struct nfc_dev *dev;
86 struct nfc_llcp_local *local;
87 u32 target_idx;
88 u32 nfc_protocol;
89
90 u8 ssap;
91 u8 dsap;
92 char *service_name;
93 size_t service_name_len;
94
95 /* Link variables */
96 u8 send_n;
97 u8 send_ack_n;
98 u8 recv_n;
99 u8 recv_ack_n;
100
101 /* Is the remote peer ready to receive */
102 u8 remote_ready;
103
104 struct sk_buff_head tx_queue;
105 struct sk_buff_head tx_pending_queue;
106 struct sk_buff_head tx_backlog_queue;
107
108 struct list_head accept_queue;
109 struct sock *parent;
110};
111
112#define nfc_llcp_sock(sk) ((struct nfc_llcp_sock *) (sk))
113#define nfc_llcp_dev(sk) (nfc_llcp_sock((sk))->dev)
114
115#define LLCP_HEADER_SIZE 2
116#define LLCP_SEQUENCE_SIZE 1
117
118/* LLCP versions: 1.1 is 1.0 plus SDP */
119#define LLCP_VERSION_10 0x10
120#define LLCP_VERSION_11 0x11
121
122/* LLCP PDU types */
123#define LLCP_PDU_SYMM 0x0
124#define LLCP_PDU_PAX 0x1
125#define LLCP_PDU_AGF 0x2
126#define LLCP_PDU_UI 0x3
127#define LLCP_PDU_CONNECT 0x4
128#define LLCP_PDU_DISC 0x5
129#define LLCP_PDU_CC 0x6
130#define LLCP_PDU_DM 0x7
131#define LLCP_PDU_FRMR 0x8
132#define LLCP_PDU_SNL 0x9
133#define LLCP_PDU_I 0xc
134#define LLCP_PDU_RR 0xd
135#define LLCP_PDU_RNR 0xe
136
137/* Parameters TLV types */
138#define LLCP_TLV_VERSION 0x1
139#define LLCP_TLV_MIUX 0x2
140#define LLCP_TLV_WKS 0x3
141#define LLCP_TLV_LTO 0x4
142#define LLCP_TLV_RW 0x5
143#define LLCP_TLV_SN 0x6
144#define LLCP_TLV_OPT 0x7
145#define LLCP_TLV_SDREQ 0x8
146#define LLCP_TLV_SDRES 0x9
147#define LLCP_TLV_MAX 0xa
148
149/* Well known LLCP SAP */
150#define LLCP_SAP_SDP 0x1
151#define LLCP_SAP_IP 0x2
152#define LLCP_SAP_OBEX 0x3
153#define LLCP_SAP_SNEP 0x4
154#define LLCP_SAP_MAX 0xff
155
156/* Disconnection reason code */
157#define LLCP_DM_DISC 0x00
158#define LLCP_DM_NOCONN 0x01
159#define LLCP_DM_NOBOUND 0x02
160#define LLCP_DM_REJ 0x03
161
162
163struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev);
164u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
165 struct nfc_llcp_sock *sock);
166u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local);
167void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap);
168
169/* Sock API */
170struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp);
171void nfc_llcp_sock_free(struct nfc_llcp_sock *sock);
172void nfc_llcp_accept_unlink(struct sock *sk);
173void nfc_llcp_accept_enqueue(struct sock *parent, struct sock *sk);
174struct sock *nfc_llcp_accept_dequeue(struct sock *sk, struct socket *newsock);
175
176/* TLV API */
177int nfc_llcp_parse_tlv(struct nfc_llcp_local *local,
178 u8 *tlv_array, u16 tlv_array_len);
179
180/* Commands API */
181void nfc_llcp_recv(void *data, struct sk_buff *skb, int err);
182u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length);
183void nfc_llcp_recv(void *data, struct sk_buff *skb, int err);
184int nfc_llcp_disconnect(struct nfc_llcp_sock *sock);
185int nfc_llcp_send_symm(struct nfc_dev *dev);
186int nfc_llcp_send_connect(struct nfc_llcp_sock *sock);
187int nfc_llcp_send_cc(struct nfc_llcp_sock *sock);
188int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason);
189int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock);
190
191/* Socket API */
192int __init nfc_llcp_sock_init(void);
193void nfc_llcp_sock_exit(void);
diff --git a/net/nfc/llcp/sock.c b/net/nfc/llcp/sock.c
new file mode 100644
index 000000000000..f738ccd535f1
--- /dev/null
+++ b/net/nfc/llcp/sock.c
@@ -0,0 +1,675 @@
1/*
2 * Copyright (C) 2011 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) "llcp: %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 "../nfc.h"
28#include "llcp.h"
29
30static struct proto llcp_sock_proto = {
31 .name = "NFC_LLCP",
32 .owner = THIS_MODULE,
33 .obj_size = sizeof(struct nfc_llcp_sock),
34};
35
36static int llcp_sock_bind(struct socket *sock, struct sockaddr *addr, int alen)
37{
38 struct sock *sk = sock->sk;
39 struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
40 struct nfc_llcp_local *local;
41 struct nfc_dev *dev;
42 struct sockaddr_nfc_llcp llcp_addr;
43 int len, ret = 0;
44
45 pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family);
46
47 if (!addr || addr->sa_family != AF_NFC)
48 return -EINVAL;
49
50 memset(&llcp_addr, 0, sizeof(llcp_addr));
51 len = min_t(unsigned int, sizeof(llcp_addr), alen);
52 memcpy(&llcp_addr, addr, len);
53
54 /* This is going to be a listening socket, dsap must be 0 */
55 if (llcp_addr.dsap != 0)
56 return -EINVAL;
57
58 lock_sock(sk);
59
60 if (sk->sk_state != LLCP_CLOSED) {
61 ret = -EBADFD;
62 goto error;
63 }
64
65 dev = nfc_get_device(llcp_addr.dev_idx);
66 if (dev == NULL) {
67 ret = -ENODEV;
68 goto error;
69 }
70
71 local = nfc_llcp_find_local(dev);
72 if (local == NULL) {
73 ret = -ENODEV;
74 goto put_dev;
75 }
76
77 llcp_sock->dev = dev;
78 llcp_sock->local = local;
79 llcp_sock->nfc_protocol = llcp_addr.nfc_protocol;
80 llcp_sock->service_name_len = min_t(unsigned int,
81 llcp_addr.service_name_len, NFC_LLCP_MAX_SERVICE_NAME);
82 llcp_sock->service_name = kmemdup(llcp_addr.service_name,
83 llcp_sock->service_name_len, GFP_KERNEL);
84
85 llcp_sock->ssap = nfc_llcp_get_sdp_ssap(local, llcp_sock);
86 if (llcp_sock->ssap == LLCP_MAX_SAP)
87 goto put_dev;
88
89 local->sockets[llcp_sock->ssap] = llcp_sock;
90
91 pr_debug("Socket bound to SAP %d\n", llcp_sock->ssap);
92
93 sk->sk_state = LLCP_BOUND;
94
95put_dev:
96 nfc_put_device(dev);
97
98error:
99 release_sock(sk);
100 return ret;
101}
102
103static int llcp_sock_listen(struct socket *sock, int backlog)
104{
105 struct sock *sk = sock->sk;
106 int ret = 0;
107
108 pr_debug("sk %p backlog %d\n", sk, backlog);
109
110 lock_sock(sk);
111
112 if ((sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM)
113 || sk->sk_state != LLCP_BOUND) {
114 ret = -EBADFD;
115 goto error;
116 }
117
118 sk->sk_max_ack_backlog = backlog;
119 sk->sk_ack_backlog = 0;
120
121 pr_debug("Socket listening\n");
122 sk->sk_state = LLCP_LISTEN;
123
124error:
125 release_sock(sk);
126
127 return ret;
128}
129
130void nfc_llcp_accept_unlink(struct sock *sk)
131{
132 struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
133
134 pr_debug("state %d\n", sk->sk_state);
135
136 list_del_init(&llcp_sock->accept_queue);
137 sk_acceptq_removed(llcp_sock->parent);
138 llcp_sock->parent = NULL;
139
140 sock_put(sk);
141}
142
143void nfc_llcp_accept_enqueue(struct sock *parent, struct sock *sk)
144{
145 struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
146 struct nfc_llcp_sock *llcp_sock_parent = nfc_llcp_sock(parent);
147
148 /* Lock will be free from unlink */
149 sock_hold(sk);
150
151 list_add_tail(&llcp_sock->accept_queue,
152 &llcp_sock_parent->accept_queue);
153 llcp_sock->parent = parent;
154 sk_acceptq_added(parent);
155}
156
157struct sock *nfc_llcp_accept_dequeue(struct sock *parent,
158 struct socket *newsock)
159{
160 struct nfc_llcp_sock *lsk, *n, *llcp_parent;
161 struct sock *sk;
162
163 llcp_parent = nfc_llcp_sock(parent);
164
165 list_for_each_entry_safe(lsk, n, &llcp_parent->accept_queue,
166 accept_queue) {
167 sk = &lsk->sk;
168 lock_sock(sk);
169
170 if (sk->sk_state == LLCP_CLOSED) {
171 release_sock(sk);
172 nfc_llcp_accept_unlink(sk);
173 continue;
174 }
175
176 if (sk->sk_state == LLCP_CONNECTED || !newsock) {
177 nfc_llcp_accept_unlink(sk);
178 if (newsock)
179 sock_graft(sk, newsock);
180
181 release_sock(sk);
182
183 pr_debug("Returning sk state %d\n", sk->sk_state);
184
185 return sk;
186 }
187
188 release_sock(sk);
189 }
190
191 return NULL;
192}
193
194static int llcp_sock_accept(struct socket *sock, struct socket *newsock,
195 int flags)
196{
197 DECLARE_WAITQUEUE(wait, current);
198 struct sock *sk = sock->sk, *new_sk;
199 long timeo;
200 int ret = 0;
201
202 pr_debug("parent %p\n", sk);
203
204 lock_sock_nested(sk, SINGLE_DEPTH_NESTING);
205
206 if (sk->sk_state != LLCP_LISTEN) {
207 ret = -EBADFD;
208 goto error;
209 }
210
211 timeo = sock_rcvtimeo(sk, flags & O_NONBLOCK);
212
213 /* Wait for an incoming connection. */
214 add_wait_queue_exclusive(sk_sleep(sk), &wait);
215 while (!(new_sk = nfc_llcp_accept_dequeue(sk, newsock))) {
216 set_current_state(TASK_INTERRUPTIBLE);
217
218 if (!timeo) {
219 ret = -EAGAIN;
220 break;
221 }
222
223 if (signal_pending(current)) {
224 ret = sock_intr_errno(timeo);
225 break;
226 }
227
228 release_sock(sk);
229 timeo = schedule_timeout(timeo);
230 lock_sock_nested(sk, SINGLE_DEPTH_NESTING);
231 }
232 __set_current_state(TASK_RUNNING);
233 remove_wait_queue(sk_sleep(sk), &wait);
234
235 if (ret)
236 goto error;
237
238 newsock->state = SS_CONNECTED;
239
240 pr_debug("new socket %p\n", new_sk);
241
242error:
243 release_sock(sk);
244
245 return ret;
246}
247
248static int llcp_sock_getname(struct socket *sock, struct sockaddr *addr,
249 int *len, int peer)
250{
251 struct sockaddr_nfc_llcp *llcp_addr = (struct sockaddr_nfc_llcp *) addr;
252 struct sock *sk = sock->sk;
253 struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
254
255 pr_debug("%p\n", sk);
256
257 addr->sa_family = AF_NFC;
258 *len = sizeof(struct sockaddr_nfc_llcp);
259
260 llcp_addr->dev_idx = llcp_sock->dev->idx;
261 llcp_addr->dsap = llcp_sock->dsap;
262 llcp_addr->ssap = llcp_sock->ssap;
263 llcp_addr->service_name_len = llcp_sock->service_name_len;
264 memcpy(llcp_addr->service_name, llcp_sock->service_name,
265 llcp_addr->service_name_len);
266
267 return 0;
268}
269
270static inline unsigned int llcp_accept_poll(struct sock *parent)
271{
272 struct nfc_llcp_sock *llcp_sock, *n, *parent_sock;
273 struct sock *sk;
274
275 parent_sock = nfc_llcp_sock(parent);
276
277 list_for_each_entry_safe(llcp_sock, n, &parent_sock->accept_queue,
278 accept_queue) {
279 sk = &llcp_sock->sk;
280
281 if (sk->sk_state == LLCP_CONNECTED)
282 return POLLIN | POLLRDNORM;
283 }
284
285 return 0;
286}
287
288static unsigned int llcp_sock_poll(struct file *file, struct socket *sock,
289 poll_table *wait)
290{
291 struct sock *sk = sock->sk;
292 unsigned int mask = 0;
293
294 pr_debug("%p\n", sk);
295
296 sock_poll_wait(file, sk_sleep(sk), wait);
297
298 if (sk->sk_state == LLCP_LISTEN)
299 return llcp_accept_poll(sk);
300
301 if (sk->sk_err || !skb_queue_empty(&sk->sk_error_queue))
302 mask |= POLLERR;
303
304 if (!skb_queue_empty(&sk->sk_receive_queue))
305 mask |= POLLIN;
306
307 if (sk->sk_state == LLCP_CLOSED)
308 mask |= POLLHUP;
309
310 return mask;
311}
312
313static int llcp_sock_release(struct socket *sock)
314{
315 struct sock *sk = sock->sk;
316 struct nfc_llcp_local *local;
317 struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
318
319 if (!sk)
320 return 0;
321
322 pr_debug("%p\n", sk);
323
324 local = llcp_sock->local;
325 if (local == NULL)
326 return -ENODEV;
327
328 mutex_lock(&local->socket_lock);
329
330 if (llcp_sock == local->sockets[llcp_sock->ssap]) {
331 local->sockets[llcp_sock->ssap] = NULL;
332 } else {
333 struct nfc_llcp_sock *parent, *s, *n;
334
335 parent = local->sockets[llcp_sock->ssap];
336
337 list_for_each_entry_safe(s, n, &parent->list, list)
338 if (llcp_sock == s) {
339 list_del(&s->list);
340 break;
341 }
342
343 }
344
345 mutex_unlock(&local->socket_lock);
346
347 lock_sock(sk);
348
349 /* Send a DISC */
350 if (sk->sk_state == LLCP_CONNECTED)
351 nfc_llcp_disconnect(llcp_sock);
352
353 if (sk->sk_state == LLCP_LISTEN) {
354 struct nfc_llcp_sock *lsk, *n;
355 struct sock *accept_sk;
356
357 list_for_each_entry_safe(lsk, n, &llcp_sock->accept_queue,
358 accept_queue) {
359 accept_sk = &lsk->sk;
360 lock_sock(accept_sk);
361
362 nfc_llcp_disconnect(lsk);
363 nfc_llcp_accept_unlink(accept_sk);
364
365 release_sock(accept_sk);
366
367 sock_set_flag(sk, SOCK_DEAD);
368 sock_orphan(accept_sk);
369 sock_put(accept_sk);
370 }
371 }
372
373 /* Freeing the SAP */
374 if ((sk->sk_state == LLCP_CONNECTED
375 && llcp_sock->ssap > LLCP_LOCAL_SAP_OFFSET) ||
376 sk->sk_state == LLCP_BOUND ||
377 sk->sk_state == LLCP_LISTEN)
378 nfc_llcp_put_ssap(llcp_sock->local, llcp_sock->ssap);
379
380 sock_set_flag(sk, SOCK_DEAD);
381
382 release_sock(sk);
383
384 sock_orphan(sk);
385 sock_put(sk);
386
387 return 0;
388}
389
390static int llcp_sock_connect(struct socket *sock, struct sockaddr *_addr,
391 int len, int flags)
392{
393 struct sock *sk = sock->sk;
394 struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
395 struct sockaddr_nfc_llcp *addr = (struct sockaddr_nfc_llcp *)_addr;
396 struct nfc_dev *dev;
397 struct nfc_llcp_local *local;
398 int ret = 0;
399
400 pr_debug("sock %p sk %p flags 0x%x\n", sock, sk, flags);
401
402 if (!addr || len < sizeof(struct sockaddr_nfc) ||
403 addr->sa_family != AF_NFC) {
404 pr_err("Invalid socket\n");
405 return -EINVAL;
406 }
407
408 if (addr->service_name_len == 0 && addr->dsap == 0) {
409 pr_err("Missing service name or dsap\n");
410 return -EINVAL;
411 }
412
413 pr_debug("addr dev_idx=%u target_idx=%u protocol=%u\n", addr->dev_idx,
414 addr->target_idx, addr->nfc_protocol);
415
416 lock_sock(sk);
417
418 if (sk->sk_state == LLCP_CONNECTED) {
419 ret = -EISCONN;
420 goto error;
421 }
422
423 dev = nfc_get_device(addr->dev_idx);
424 if (dev == NULL) {
425 ret = -ENODEV;
426 goto error;
427 }
428
429 local = nfc_llcp_find_local(dev);
430 if (local == NULL) {
431 ret = -ENODEV;
432 goto put_dev;
433 }
434
435 device_lock(&dev->dev);
436 if (dev->dep_link_up == false) {
437 ret = -ENOLINK;
438 device_unlock(&dev->dev);
439 goto put_dev;
440 }
441 device_unlock(&dev->dev);
442
443 if (local->rf_mode == NFC_RF_INITIATOR &&
444 addr->target_idx != local->target_idx) {
445 ret = -ENOLINK;
446 goto put_dev;
447 }
448
449 llcp_sock->dev = dev;
450 llcp_sock->local = local;
451 llcp_sock->ssap = nfc_llcp_get_local_ssap(local);
452 if (llcp_sock->ssap == LLCP_SAP_MAX) {
453 ret = -ENOMEM;
454 goto put_dev;
455 }
456 if (addr->service_name_len == 0)
457 llcp_sock->dsap = addr->dsap;
458 else
459 llcp_sock->dsap = LLCP_SAP_SDP;
460 llcp_sock->nfc_protocol = addr->nfc_protocol;
461 llcp_sock->service_name_len = min_t(unsigned int,
462 addr->service_name_len, NFC_LLCP_MAX_SERVICE_NAME);
463 llcp_sock->service_name = kmemdup(addr->service_name,
464 llcp_sock->service_name_len, GFP_KERNEL);
465
466 local->sockets[llcp_sock->ssap] = llcp_sock;
467
468 ret = nfc_llcp_send_connect(llcp_sock);
469 if (ret)
470 goto put_dev;
471
472 sk->sk_state = LLCP_CONNECTED;
473
474 release_sock(sk);
475 return 0;
476
477put_dev:
478 nfc_put_device(dev);
479
480error:
481 release_sock(sk);
482 return ret;
483}
484
485static int llcp_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
486 struct msghdr *msg, size_t len, int flags)
487{
488 int noblock = flags & MSG_DONTWAIT;
489 struct sock *sk = sock->sk;
490 unsigned int copied, rlen;
491 struct sk_buff *skb, *cskb;
492 int err = 0;
493
494 pr_debug("%p %zu\n", sk, len);
495
496 lock_sock(sk);
497
498 if (sk->sk_state == LLCP_CLOSED &&
499 skb_queue_empty(&sk->sk_receive_queue)) {
500 release_sock(sk);
501 return 0;
502 }
503
504 release_sock(sk);
505
506 if (flags & (MSG_OOB))
507 return -EOPNOTSUPP;
508
509 skb = skb_recv_datagram(sk, flags, noblock, &err);
510 if (!skb) {
511 pr_err("Recv datagram failed state %d %d %d",
512 sk->sk_state, err, sock_error(sk));
513
514 if (sk->sk_shutdown & RCV_SHUTDOWN)
515 return 0;
516
517 return err;
518 }
519
520 rlen = skb->len; /* real length of skb */
521 copied = min_t(unsigned int, rlen, len);
522
523 cskb = skb;
524 if (memcpy_toiovec(msg->msg_iov, cskb->data, copied)) {
525 if (!(flags & MSG_PEEK))
526 skb_queue_head(&sk->sk_receive_queue, skb);
527 return -EFAULT;
528 }
529
530 /* Mark read part of skb as used */
531 if (!(flags & MSG_PEEK)) {
532
533 /* SOCK_STREAM: re-queue skb if it contains unreceived data */
534 if (sk->sk_type == SOCK_STREAM) {
535 skb_pull(skb, copied);
536 if (skb->len) {
537 skb_queue_head(&sk->sk_receive_queue, skb);
538 goto done;
539 }
540 }
541
542 kfree_skb(skb);
543 }
544
545 /* XXX Queue backlogged skbs */
546
547done:
548 /* SOCK_SEQPACKET: return real length if MSG_TRUNC is set */
549 if (sk->sk_type == SOCK_SEQPACKET && (flags & MSG_TRUNC))
550 copied = rlen;
551
552 return copied;
553}
554
555static const struct proto_ops llcp_sock_ops = {
556 .family = PF_NFC,
557 .owner = THIS_MODULE,
558 .bind = llcp_sock_bind,
559 .connect = llcp_sock_connect,
560 .release = llcp_sock_release,
561 .socketpair = sock_no_socketpair,
562 .accept = llcp_sock_accept,
563 .getname = llcp_sock_getname,
564 .poll = llcp_sock_poll,
565 .ioctl = sock_no_ioctl,
566 .listen = llcp_sock_listen,
567 .shutdown = sock_no_shutdown,
568 .setsockopt = sock_no_setsockopt,
569 .getsockopt = sock_no_getsockopt,
570 .sendmsg = sock_no_sendmsg,
571 .recvmsg = llcp_sock_recvmsg,
572 .mmap = sock_no_mmap,
573};
574
575static void llcp_sock_destruct(struct sock *sk)
576{
577 struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
578
579 pr_debug("%p\n", sk);
580
581 if (sk->sk_state == LLCP_CONNECTED)
582 nfc_put_device(llcp_sock->dev);
583
584 skb_queue_purge(&sk->sk_receive_queue);
585
586 nfc_llcp_sock_free(llcp_sock);
587
588 if (!sock_flag(sk, SOCK_DEAD)) {
589 pr_err("Freeing alive NFC LLCP socket %p\n", sk);
590 return;
591 }
592}
593
594struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp)
595{
596 struct sock *sk;
597 struct nfc_llcp_sock *llcp_sock;
598
599 sk = sk_alloc(&init_net, PF_NFC, gfp, &llcp_sock_proto);
600 if (!sk)
601 return NULL;
602
603 llcp_sock = nfc_llcp_sock(sk);
604
605 sock_init_data(sock, sk);
606 sk->sk_state = LLCP_CLOSED;
607 sk->sk_protocol = NFC_SOCKPROTO_LLCP;
608 sk->sk_type = type;
609 sk->sk_destruct = llcp_sock_destruct;
610
611 llcp_sock->ssap = 0;
612 llcp_sock->dsap = LLCP_SAP_SDP;
613 llcp_sock->send_n = llcp_sock->send_ack_n = 0;
614 llcp_sock->recv_n = llcp_sock->recv_ack_n = 0;
615 llcp_sock->remote_ready = 1;
616 skb_queue_head_init(&llcp_sock->tx_queue);
617 skb_queue_head_init(&llcp_sock->tx_pending_queue);
618 skb_queue_head_init(&llcp_sock->tx_backlog_queue);
619 INIT_LIST_HEAD(&llcp_sock->list);
620 INIT_LIST_HEAD(&llcp_sock->accept_queue);
621
622 if (sock != NULL)
623 sock->state = SS_UNCONNECTED;
624
625 return sk;
626}
627
628void nfc_llcp_sock_free(struct nfc_llcp_sock *sock)
629{
630 kfree(sock->service_name);
631
632 skb_queue_purge(&sock->tx_queue);
633 skb_queue_purge(&sock->tx_pending_queue);
634 skb_queue_purge(&sock->tx_backlog_queue);
635
636 list_del_init(&sock->accept_queue);
637
638 sock->parent = NULL;
639}
640
641static int llcp_sock_create(struct net *net, struct socket *sock,
642 const struct nfc_protocol *nfc_proto)
643{
644 struct sock *sk;
645
646 pr_debug("%p\n", sock);
647
648 if (sock->type != SOCK_STREAM && sock->type != SOCK_DGRAM)
649 return -ESOCKTNOSUPPORT;
650
651 sock->ops = &llcp_sock_ops;
652
653 sk = nfc_llcp_sock_alloc(sock, sock->type, GFP_ATOMIC);
654 if (sk == NULL)
655 return -ENOMEM;
656
657 return 0;
658}
659
660static const struct nfc_protocol llcp_nfc_proto = {
661 .id = NFC_SOCKPROTO_LLCP,
662 .proto = &llcp_sock_proto,
663 .owner = THIS_MODULE,
664 .create = llcp_sock_create
665};
666
667int __init nfc_llcp_sock_init(void)
668{
669 return nfc_proto_register(&llcp_nfc_proto);
670}
671
672void nfc_llcp_sock_exit(void)
673{
674 nfc_proto_unregister(&llcp_nfc_proto);
675}
diff --git a/net/nfc/nfc.h b/net/nfc/nfc.h
index 4d0fb125d033..2c2c4015c68b 100644
--- a/net/nfc/nfc.h
+++ b/net/nfc/nfc.h
@@ -46,6 +46,60 @@ struct nfc_rawsock {
46#define to_rawsock_sk(_tx_work) \ 46#define to_rawsock_sk(_tx_work) \
47 ((struct sock *) container_of(_tx_work, struct nfc_rawsock, tx_work)) 47 ((struct sock *) container_of(_tx_work, struct nfc_rawsock, tx_work))
48 48
49#ifdef CONFIG_NFC_LLCP
50
51void nfc_llcp_mac_is_down(struct nfc_dev *dev);
52void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
53 u8 comm_mode, u8 rf_mode);
54int nfc_llcp_register_device(struct nfc_dev *dev);
55void nfc_llcp_unregister_device(struct nfc_dev *dev);
56int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len);
57u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, u8 *general_bytes_len);
58int __init nfc_llcp_init(void);
59void nfc_llcp_exit(void);
60
61#else
62
63void nfc_llcp_mac_is_down(struct nfc_dev *dev)
64{
65}
66
67void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
68 u8 comm_mode, u8 rf_mode)
69{
70}
71
72static inline int nfc_llcp_register_device(struct nfc_dev *dev)
73{
74 return 0;
75}
76
77static inline void nfc_llcp_unregister_device(struct nfc_dev *dev)
78{
79}
80
81static inline int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len)
82{
83 return 0;
84}
85
86static inline u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, u8 *gb_len)
87{
88 *gb_len = 0;
89 return NULL;
90}
91
92static inline int nfc_llcp_init(void)
93{
94 return 0;
95}
96
97static inline void nfc_llcp_exit(void)
98{
99}
100
101#endif
102
49int __init rawsock_init(void); 103int __init rawsock_init(void);
50void rawsock_exit(void); 104void rawsock_exit(void);
51 105
generated by cgit v1.2.2 (git 2.25.0) at 2024-11-12 07:22:05 -0500