projects / deliverable / linux.git / blame
? search:
summary | shortlog | log | commit | commitdiff | tree
blob | blame (incremental) | history | HEAD
NFC: llcp: Only keep raw sockets alive when the LLCP local leaves
[deliverable/linux.git] / net / nfc / llcp / llcp.c
CommitLineData
d646960f
SO		 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
098dafcf
TE		 34static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb);
35
a69f32af 36void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *sk)
d646960f 37{
a69f32af
SO		 38 write_lock(&l->lock);
39 sk_add_node(sk, &l->head);
40 write_unlock(&l->lock);
41}
d646960f 42
a69f32af
SO		 43void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *sk)
44{
45 write_lock(&l->lock);
46 sk_del_node_init(sk);
47 write_unlock(&l->lock);
48}
d646960f 49
abd18d43
TE		 50void nfc_llcp_socket_remote_param_init(struct nfc_llcp_sock *sock)
51{
52 sock->remote_rw = LLCP_DEFAULT_RW;
53 sock->remote_miu = LLCP_MAX_MIU + 1;
54}
55
f31652a5
SO		 56static void nfc_llcp_socket_purge(struct nfc_llcp_sock *sock)
57{
58 struct nfc_llcp_local *local = sock->local;
59 struct sk_buff *s, *tmp;
60
61 pr_debug("%p\n", &sock->sk);
62
63 skb_queue_purge(&sock->tx_queue);
64 skb_queue_purge(&sock->tx_pending_queue);
f31652a5
SO		 65
66 if (local == NULL)
67 return;
68
69 /* Search for local pending SKBs that are related to this socket */
70 skb_queue_walk_safe(&local->tx_queue, s, tmp) {
71 if (s->sk != &sock->sk)
72 continue;
73
74 skb_unlink(s, &local->tx_queue);
75 kfree_skb(s);
76 }
77}
78
b436a13d 79static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool device,
3bbc0ceb 80 int err)
a69f32af
SO		 81{
82 struct sock *sk;
b67bfe0d 83 struct hlist_node *tmp;
a69f32af 84 struct nfc_llcp_sock *llcp_sock;
d646960f 85
f31652a5
SO		 86 skb_queue_purge(&local->tx_queue);
87
a69f32af 88 write_lock(&local->sockets.lock);
40c75f81 89
b67bfe0d 90 sk_for_each_safe(sk, tmp, &local->sockets.head) {
a69f32af 91 llcp_sock = nfc_llcp_sock(sk);
d646960f 92
50b78b2a 93 bh_lock_sock(sk);
d646960f 94
f31652a5
SO		 95 nfc_llcp_socket_purge(llcp_sock);
96
a69f32af
SO		 97 if (sk->sk_state == LLCP_CONNECTED)
98 nfc_put_device(llcp_sock->dev);
d646960f 99
a69f32af 100 if (sk->sk_state == LLCP_LISTEN) {
d646960f
SO		 101 struct nfc_llcp_sock *lsk, *n;
102 struct sock *accept_sk;
103
0f450772
SJ		 104 list_for_each_entry_safe(lsk, n,
105 &llcp_sock->accept_queue,
427a2eb1 106 accept_queue) {
d646960f 107 accept_sk = &lsk->sk;
50b78b2a 108 bh_lock_sock(accept_sk);
d646960f
SO		 109
110 nfc_llcp_accept_unlink(accept_sk);
111
3bbc0ceb
SO		 112 if (err)
113 accept_sk->sk_err = err;
d646960f 114 accept_sk->sk_state = LLCP_CLOSED;
3bbc0ceb 115 accept_sk->sk_state_change(sk);
d646960f 116
50b78b2a 117 bh_unlock_sock(accept_sk);
d646960f 118 }
c8512be6
SO		 119 }
120
3bbc0ceb
SO		 121 if (err)
122 sk->sk_err = err;
a69f32af 123 sk->sk_state = LLCP_CLOSED;
3bbc0ceb 124 sk->sk_state_change(sk);
d646960f 125
50b78b2a 126 bh_unlock_sock(sk);
d646960f 127
a69f32af 128 sk_del_node_init(sk);
d646960f
SO		 129 }
130
a69f32af 131 write_unlock(&local->sockets.lock);
e6a3a4bb 132
b436a13d
SO		 133 /* If we still have a device, we keep the RAW sockets alive */
134 if (device == true)
e6a3a4bb
SO		 135 return;
136
137 write_lock(&local->raw_sockets.lock);
138
139 sk_for_each_safe(sk, tmp, &local->raw_sockets.head) {
140 llcp_sock = nfc_llcp_sock(sk);
141
142 bh_lock_sock(sk);
143
144 nfc_llcp_socket_purge(llcp_sock);
145
3bbc0ceb
SO		 146 if (err)
147 sk->sk_err = err;
e6a3a4bb 148 sk->sk_state = LLCP_CLOSED;
e6a3a4bb
SO		 149 sk->sk_state_change(sk);
150
151 bh_unlock_sock(sk);
152
e6a3a4bb
SO		 153 sk_del_node_init(sk);
154 }
155
156 write_unlock(&local->raw_sockets.lock);
d646960f
SO		 157}
158
c7aa1225
SO		 159struct nfc_llcp_local *nfc_llcp_local_get(struct nfc_llcp_local *local)
160{
161 kref_get(&local->ref);
162
163 return local;
164}
165
3536da06 166static void local_cleanup(struct nfc_llcp_local *local, bool listen)
c7aa1225 167{
3bbc0ceb 168 nfc_llcp_socket_release(local, listen, ENXIO);
c7aa1225
SO		 169 del_timer_sync(&local->link_timer);
170 skb_queue_purge(&local->tx_queue);
474fee3d
TH		 171 cancel_work_sync(&local->tx_work);
172 cancel_work_sync(&local->rx_work);
173 cancel_work_sync(&local->timeout_work);
c7aa1225 174 kfree_skb(local->rx_pending);
40213fa8
TE		 175 del_timer_sync(&local->sdreq_timer);
176 cancel_work_sync(&local->sdreq_timeout_work);
d9b8d8e1 177 nfc_llcp_free_sdp_tlv_list(&local->pending_sdreqs);
3536da06
SO		 178}
179
180static void local_release(struct kref *ref)
181{
182 struct nfc_llcp_local *local;
183
184 local = container_of(ref, struct nfc_llcp_local, ref);
185
186 list_del(&local->list);
187 local_cleanup(local, false);
c7aa1225
SO		 188 kfree(local);
189}
190
191int nfc_llcp_local_put(struct nfc_llcp_local *local)
192{
a69f32af
SO		 193 if (local == NULL)
194 return 0;
195
c7aa1225
SO		 196 return kref_put(&local->ref, local_release);
197}
198
8f50020e
SO		 199static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
200 u8 ssap, u8 dsap)
201{
202 struct sock *sk;
a8df0f37 203 struct nfc_llcp_sock *llcp_sock, *tmp_sock;
8f50020e
SO		 204
205 pr_debug("ssap dsap %d %d\n", ssap, dsap);
206
207 if (ssap == 0 && dsap == 0)
208 return NULL;
209
210 read_lock(&local->sockets.lock);
211
212 llcp_sock = NULL;
213
b67bfe0d 214 sk_for_each(sk, &local->sockets.head) {
a8df0f37 215 tmp_sock = nfc_llcp_sock(sk);
8f50020e 216
a8df0f37
SO		 217 if (tmp_sock->ssap == ssap && tmp_sock->dsap == dsap) {
218 llcp_sock = tmp_sock;
8f50020e 219 break;
a8df0f37 220 }
8f50020e
SO		 221 }
222
223 read_unlock(&local->sockets.lock);
224
225 if (llcp_sock == NULL)
226 return NULL;
227
228 sock_hold(&llcp_sock->sk);
229
230 return llcp_sock;
231}
232
233static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock)
234{
235 sock_put(&sock->sk);
236}
237
d646960f
SO		 238static void nfc_llcp_timeout_work(struct work_struct *work)
239{
240 struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
427a2eb1 241 timeout_work);
d646960f
SO		 242
243 nfc_dep_link_down(local->dev);
244}
245
246static void nfc_llcp_symm_timer(unsigned long data)
247{
248 struct nfc_llcp_local *local = (struct nfc_llcp_local *) data;
249
250 pr_err("SYMM timeout\n");
251
916082b0 252 schedule_work(&local->timeout_work);
d646960f
SO		 253}
254
40213fa8
TE		 255static void nfc_llcp_sdreq_timeout_work(struct work_struct *work)
256{
257 unsigned long time;
258 HLIST_HEAD(nl_sdres_list);
259 struct hlist_node *n;
260 struct nfc_llcp_sdp_tlv *sdp;
261 struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
262 sdreq_timeout_work);
263
264 mutex_lock(&local->sdreq_lock);
265
266 time = jiffies - msecs_to_jiffies(3 * local->remote_lto);
267
268 hlist_for_each_entry_safe(sdp, n, &local->pending_sdreqs, node) {
269 if (time_after(sdp->time, time))
270 continue;
271
272 sdp->sap = LLCP_SDP_UNBOUND;
273
274 hlist_del(&sdp->node);
275
276 hlist_add_head(&sdp->node, &nl_sdres_list);
277 }
278
279 if (!hlist_empty(&local->pending_sdreqs))
280 mod_timer(&local->sdreq_timer,
281 jiffies + msecs_to_jiffies(3 * local->remote_lto));
282
283 mutex_unlock(&local->sdreq_lock);
284
285 if (!hlist_empty(&nl_sdres_list))
286 nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list);
287}
288
289static void nfc_llcp_sdreq_timer(unsigned long data)
290{
291 struct nfc_llcp_local *local = (struct nfc_llcp_local *) data;
292
293 schedule_work(&local->sdreq_timeout_work);
294}
295
d646960f
SO		 296struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev)
297{
298 struct nfc_llcp_local *local, *n;
299
300 list_for_each_entry_safe(local, n, &llcp_devices, list)
301 if (local->dev == dev)
302 return local;
303
304 pr_debug("No device found\n");
305
306 return NULL;
307}
308
309static char *wks[] = {
310 NULL,
311 NULL, /* SDP */
312 "urn:nfc:sn:ip",
313 "urn:nfc:sn:obex",
314 "urn:nfc:sn:snep",
315};
316
317static int nfc_llcp_wks_sap(char *service_name, size_t service_name_len)
318{
319 int sap, num_wks;
320
321 pr_debug("%s\n", service_name);
322
323 if (service_name == NULL)
324 return -EINVAL;
325
326 num_wks = ARRAY_SIZE(wks);
327
427a2eb1 328 for (sap = 0; sap < num_wks; sap++) {
d646960f
SO		 329 if (wks[sap] == NULL)
330 continue;
331
332 if (strncmp(wks[sap], service_name, service_name_len) == 0)
333 return sap;
334 }
335
336 return -EINVAL;
337}
338
8f50020e
SO		 339static
340struct nfc_llcp_sock *nfc_llcp_sock_from_sn(struct nfc_llcp_local *local,
341 u8 *sn, size_t sn_len)
342{
343 struct sock *sk;
8f50020e
SO		 344 struct nfc_llcp_sock *llcp_sock, *tmp_sock;
345
346 pr_debug("sn %zd %p\n", sn_len, sn);
347
348 if (sn == NULL || sn_len == 0)
349 return NULL;
350
351 read_lock(&local->sockets.lock);
352
353 llcp_sock = NULL;
354
b67bfe0d 355 sk_for_each(sk, &local->sockets.head) {
8f50020e
SO		 356 tmp_sock = nfc_llcp_sock(sk);
357
358 pr_debug("llcp sock %p\n", tmp_sock);
359
54292d64
SO		 360 if (tmp_sock->sk.sk_type == SOCK_STREAM &&
361 tmp_sock->sk.sk_state != LLCP_LISTEN)
362 continue;
363
364 if (tmp_sock->sk.sk_type == SOCK_DGRAM &&
365 tmp_sock->sk.sk_state != LLCP_BOUND)
8f50020e
SO		 366 continue;
367
368 if (tmp_sock->service_name == NULL ||
369 tmp_sock->service_name_len == 0)
370 continue;
371
372 if (tmp_sock->service_name_len != sn_len)
373 continue;
374
375 if (memcmp(sn, tmp_sock->service_name, sn_len) == 0) {
376 llcp_sock = tmp_sock;
377 break;
378 }
379 }
380
381 read_unlock(&local->sockets.lock);
382
383 pr_debug("Found llcp sock %p\n", llcp_sock);
384
385 return llcp_sock;
386}
387
d646960f 388u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
427a2eb1 389 struct nfc_llcp_sock *sock)
d646960f
SO		 390{
391 mutex_lock(&local->sdp_lock);
392
393 if (sock->service_name != NULL && sock->service_name_len > 0) {
394 int ssap = nfc_llcp_wks_sap(sock->service_name,
427a2eb1 395 sock->service_name_len);
d646960f
SO		 396
397 if (ssap > 0) {
398 pr_debug("WKS %d\n", ssap);
399
400 /* This is a WKS, let's check if it's free */
401 if (local->local_wks & BIT(ssap)) {
402 mutex_unlock(&local->sdp_lock);
403
404 return LLCP_SAP_MAX;
405 }
406
1762c17c 407 set_bit(ssap, &local->local_wks);
d646960f
SO		 408 mutex_unlock(&local->sdp_lock);
409
410 return ssap;
411 }
412
413 /*
8f50020e
SO		 414 * Check if there already is a non WKS socket bound
415 * to this service name.
d646960f 416 */
8f50020e
SO		 417 if (nfc_llcp_sock_from_sn(local, sock->service_name,
418 sock->service_name_len) != NULL) {
d646960f
SO		 419 mutex_unlock(&local->sdp_lock);
420
421 return LLCP_SAP_MAX;
422 }
423
d646960f
SO		 424 mutex_unlock(&local->sdp_lock);
425
8f50020e 426 return LLCP_SDP_UNBOUND;
d646960f 427
ebbb16d9
SO		 428 } else if (sock->ssap != 0 && sock->ssap < LLCP_WKS_NUM_SAP) {
429 if (!test_bit(sock->ssap, &local->local_wks)) {
430 set_bit(sock->ssap, &local->local_wks);
431 mutex_unlock(&local->sdp_lock);
d646960f 432
ebbb16d9 433 return sock->ssap;
d646960f
SO		 434 }
435 }
436
437 mutex_unlock(&local->sdp_lock);
438
439 return LLCP_SAP_MAX;
440}
441
442u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local)
443{
444 u8 local_ssap;
445
446 mutex_lock(&local->sdp_lock);
447
448 local_ssap = find_first_zero_bit(&local->local_sap, LLCP_LOCAL_NUM_SAP);
449 if (local_ssap == LLCP_LOCAL_NUM_SAP) {
450 mutex_unlock(&local->sdp_lock);
451 return LLCP_SAP_MAX;
452 }
453
1762c17c 454 set_bit(local_ssap, &local->local_sap);
d646960f
SO		 455
456 mutex_unlock(&local->sdp_lock);
457
458 return local_ssap + LLCP_LOCAL_SAP_OFFSET;
459}
460
461void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap)
462{
463 u8 local_ssap;
464 unsigned long *sdp;
465
466 if (ssap < LLCP_WKS_NUM_SAP) {
467 local_ssap = ssap;
468 sdp = &local->local_wks;
469 } else if (ssap < LLCP_LOCAL_NUM_SAP) {
8f50020e
SO		 470 atomic_t *client_cnt;
471
d646960f
SO		 472 local_ssap = ssap - LLCP_WKS_NUM_SAP;
473 sdp = &local->local_sdp;
8f50020e
SO		 474 client_cnt = &local->local_sdp_cnt[local_ssap];
475
476 pr_debug("%d clients\n", atomic_read(client_cnt));
477
478 mutex_lock(&local->sdp_lock);
479
480 if (atomic_dec_and_test(client_cnt)) {
481 struct nfc_llcp_sock *l_sock;
482
483 pr_debug("No more clients for SAP %d\n", ssap);
484
485 clear_bit(local_ssap, sdp);
486
487 /* Find the listening sock and set it back to UNBOUND */
488 l_sock = nfc_llcp_sock_get(local, ssap, LLCP_SAP_SDP);
489 if (l_sock) {
490 l_sock->ssap = LLCP_SDP_UNBOUND;
491 nfc_llcp_sock_put(l_sock);
492 }
493 }
494
495 mutex_unlock(&local->sdp_lock);
496
497 return;
d646960f
SO		 498 } else if (ssap < LLCP_MAX_SAP) {
499 local_ssap = ssap - LLCP_LOCAL_NUM_SAP;
500 sdp = &local->local_sap;
501 } else {
502 return;
503 }
504
505 mutex_lock(&local->sdp_lock);
506
1762c17c 507 clear_bit(local_ssap, sdp);
d646960f
SO		 508
509 mutex_unlock(&local->sdp_lock);
510}
511
8f50020e
SO		 512static u8 nfc_llcp_reserve_sdp_ssap(struct nfc_llcp_local *local)
513{
514 u8 ssap;
515
516 mutex_lock(&local->sdp_lock);
517
518 ssap = find_first_zero_bit(&local->local_sdp, LLCP_SDP_NUM_SAP);
519 if (ssap == LLCP_SDP_NUM_SAP) {
520 mutex_unlock(&local->sdp_lock);
521
522 return LLCP_SAP_MAX;
523 }
524
525 pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap);
526
527 set_bit(ssap, &local->local_sdp);
528
529 mutex_unlock(&local->sdp_lock);
530
531 return LLCP_WKS_NUM_SAP + ssap;
532}
533
d646960f
SO		 534static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
535{
536 u8 *gb_cur, *version_tlv, version, version_length;
52feb444 537 u8 *lto_tlv, lto_length;
d646960f 538 u8 *wks_tlv, wks_length;
56d5876a 539 u8 *miux_tlv, miux_length;
d646960f 540 u8 gb_len = 0;
52da2449 541 int ret = 0;
d646960f
SO		 542
543 version = LLCP_VERSION_11;
544 version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version,
427a2eb1 545 1, &version_length);
d646960f
SO		 546 gb_len += version_length;
547
52feb444 548 lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_LTO, &local->lto, 1, &lto_length);
d646960f
SO		 549 gb_len += lto_length;
550
551 pr_debug("Local wks 0x%lx\n", local->local_wks);
552 wks_tlv = nfc_llcp_build_tlv(LLCP_TLV_WKS, (u8 *)&local->local_wks, 2,
427a2eb1 553 &wks_length);
d646960f
SO		 554 gb_len += wks_length;
555
52feb444 556 miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&local->miux, 0,
56d5876a
SO		 557 &miux_length);
558 gb_len += miux_length;
559
d646960f
SO		 560 gb_len += ARRAY_SIZE(llcp_magic);
561
562 if (gb_len > NFC_MAX_GT_LEN) {
52da2449
WY		 563 ret = -EINVAL;
564 goto out;
d646960f
SO		 565 }
566
567 gb_cur = local->gb;
568
569 memcpy(gb_cur, llcp_magic, ARRAY_SIZE(llcp_magic));
570 gb_cur += ARRAY_SIZE(llcp_magic);
571
572 memcpy(gb_cur, version_tlv, version_length);
573 gb_cur += version_length;
574
575 memcpy(gb_cur, lto_tlv, lto_length);
576 gb_cur += lto_length;
577
578 memcpy(gb_cur, wks_tlv, wks_length);
579 gb_cur += wks_length;
580
56d5876a
SO		 581 memcpy(gb_cur, miux_tlv, miux_length);
582 gb_cur += miux_length;
583
52da2449
WY		 584 local->gb_len = gb_len;
585
586out:
d646960f
SO		 587 kfree(version_tlv);
588 kfree(lto_tlv);
52da2449
WY		 589 kfree(wks_tlv);
590 kfree(miux_tlv);
d646960f 591
52da2449 592 return ret;
d646960f
SO		 593}
594
b8e7a06d
SO		 595u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len)
596{
597 struct nfc_llcp_local *local;
598
599 local = nfc_llcp_find_local(dev);
600 if (local == NULL) {
601 *general_bytes_len = 0;
602 return NULL;
603 }
604
605 nfc_llcp_build_gb(local);
606
607 *general_bytes_len = local->gb_len;
608
609 return local->gb;
610}
611
d646960f
SO		 612int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len)
613{
614 struct nfc_llcp_local *local = nfc_llcp_find_local(dev);
615
616 if (local == NULL) {
617 pr_err("No LLCP device\n");
618 return -ENODEV;
619 }
e9a4aa3b
DC		 620 if (gb_len < 3)
621 return -EINVAL;
d646960f
SO		 622
623 memset(local->remote_gb, 0, NFC_MAX_GT_LEN);
624 memcpy(local->remote_gb, gb, gb_len);
625 local->remote_gb_len = gb_len;
626
d646960f
SO		 627 if (memcmp(local->remote_gb, llcp_magic, 3)) {
628 pr_err("MAC does not support LLCP\n");
629 return -EINVAL;
630 }
631
7a06e586
SO		 632 return nfc_llcp_parse_gb_tlv(local,
633 &local->remote_gb[3],
634 local->remote_gb_len - 3);
d646960f
SO		 635}
636
d646960f
SO		 637static u8 nfc_llcp_dsap(struct sk_buff *pdu)
638{
639 return (pdu->data[0] & 0xfc) >> 2;
640}
641
642static u8 nfc_llcp_ptype(struct sk_buff *pdu)
643{
644 return ((pdu->data[0] & 0x03) << 2) | ((pdu->data[1] & 0xc0) >> 6);
645}
646
647static u8 nfc_llcp_ssap(struct sk_buff *pdu)
648{
649 return pdu->data[1] & 0x3f;
650}
651
652static u8 nfc_llcp_ns(struct sk_buff *pdu)
653{
654 return pdu->data[2] >> 4;
655}
656
657static u8 nfc_llcp_nr(struct sk_buff *pdu)
658{
659 return pdu->data[2] & 0xf;
660}
661
662static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu)
663{
279cf174 664 pdu->data[2] = (sock->send_n << 4) | (sock->recv_n);
d646960f
SO		 665 sock->send_n = (sock->send_n + 1) % 16;
666 sock->recv_ack_n = (sock->recv_n - 1) % 16;
667}
668
4463523b
TE		 669void nfc_llcp_send_to_raw_sock(struct nfc_llcp_local *local,
670 struct sk_buff *skb, u8 direction)
671{
4463523b
TE		 672 struct sk_buff *skb_copy = NULL, *nskb;
673 struct sock *sk;
674 u8 *data;
675
676 read_lock(&local->raw_sockets.lock);
677
b67bfe0d 678 sk_for_each(sk, &local->raw_sockets.head) {
4463523b
TE		 679 if (sk->sk_state != LLCP_BOUND)
680 continue;
681
682 if (skb_copy == NULL) {
683 skb_copy = __pskb_copy(skb, NFC_LLCP_RAW_HEADER_SIZE,
684 GFP_ATOMIC);
685
686 if (skb_copy == NULL)
687 continue;
688
689 data = skb_push(skb_copy, NFC_LLCP_RAW_HEADER_SIZE);
690
691 data[0] = local->dev ? local->dev->idx : 0xFF;
692 data[1] = direction;
693 }
694
695 nskb = skb_clone(skb_copy, GFP_ATOMIC);
696 if (!nskb)
697 continue;
698
699 if (sock_queue_rcv_skb(sk, nskb))
700 kfree_skb(nskb);
701 }
702
703 read_unlock(&local->raw_sockets.lock);
704
705 kfree_skb(skb_copy);
706}
707
84457960
SO		 708static void nfc_llcp_tx_work(struct work_struct *work)
709{
710 struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
711 tx_work);
712 struct sk_buff *skb;
713 struct sock *sk;
714 struct nfc_llcp_sock *llcp_sock;
715
716 skb = skb_dequeue(&local->tx_queue);
717 if (skb != NULL) {
718 sk = skb->sk;
719 llcp_sock = nfc_llcp_sock(sk);
a6a0915f
SO		 720
721 if (llcp_sock == NULL && nfc_llcp_ptype(skb) == LLCP_PDU_I) {
722 nfc_llcp_send_symm(local->dev);
723 } else {
be02b6b6
SO		 724 struct sk_buff *copy_skb = NULL;
725 u8 ptype = nfc_llcp_ptype(skb);
84457960
SO		 726 int ret;
727
728 pr_debug("Sending pending skb\n");
729 print_hex_dump(KERN_DEBUG, "LLCP Tx: ",
730 DUMP_PREFIX_OFFSET, 16, 1,
731 skb->data, skb->len, true);
732
be02b6b6
SO		 733 if (ptype == LLCP_PDU_I)
734 copy_skb = skb_copy(skb, GFP_ATOMIC);
735
2c2d45bd
TE		 736 __net_timestamp(skb);
737
4463523b
TE		 738 nfc_llcp_send_to_raw_sock(local, skb,
739 NFC_LLCP_DIRECTION_TX);
740
84457960
SO		 741 ret = nfc_data_exchange(local->dev, local->target_idx,
742 skb, nfc_llcp_recv, local);
743
be02b6b6
SO		 744 if (ret) {
745 kfree_skb(copy_skb);
746 goto out;
84457960 747 }
be02b6b6
SO		 748
749 if (ptype == LLCP_PDU_I && copy_skb)
750 skb_queue_tail(&llcp_sock->tx_pending_queue,
751 copy_skb);
84457960
SO		 752 }
753 } else {
754 nfc_llcp_send_symm(local->dev);
755 }
756
be02b6b6 757out:
84457960
SO		 758 mod_timer(&local->link_timer,
759 jiffies + msecs_to_jiffies(2 * local->remote_lto));
760}
761
a69f32af
SO		 762static struct nfc_llcp_sock *nfc_llcp_connecting_sock_get(struct nfc_llcp_local *local,
763 u8 ssap)
764{
765 struct sock *sk;
766 struct nfc_llcp_sock *llcp_sock;
a69f32af
SO		 767
768 read_lock(&local->connecting_sockets.lock);
769
b67bfe0d 770 sk_for_each(sk, &local->connecting_sockets.head) {
a69f32af
SO		 771 llcp_sock = nfc_llcp_sock(sk);
772
5a0f6f3b
SO		 773 if (llcp_sock->ssap == ssap) {
774 sock_hold(&llcp_sock->sk);
a69f32af 775 goto out;
5a0f6f3b 776 }
a69f32af
SO		 777 }
778
779 llcp_sock = NULL;
780
781out:
782 read_unlock(&local->connecting_sockets.lock);
783
a69f32af
SO		 784 return llcp_sock;
785}
786
a69f32af
SO		 787static struct nfc_llcp_sock *nfc_llcp_sock_get_sn(struct nfc_llcp_local *local,
788 u8 *sn, size_t sn_len)
789{
a69f32af
SO		 790 struct nfc_llcp_sock *llcp_sock;
791
8f50020e 792 llcp_sock = nfc_llcp_sock_from_sn(local, sn, sn_len);
d646960f 793
a69f32af
SO		 794 if (llcp_sock == NULL)
795 return NULL;
d646960f 796
a69f32af
SO		 797 sock_hold(&llcp_sock->sk);
798
799 return llcp_sock;
d646960f
SO		 800}
801
d646960f
SO		 802static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len)
803{
804 u8 *tlv = &skb->data[2], type, length;
805 size_t tlv_array_len = skb->len - LLCP_HEADER_SIZE, offset = 0;
806
807 while (offset < tlv_array_len) {
808 type = tlv[0];
809 length = tlv[1];
810
811 pr_debug("type 0x%x length %d\n", type, length);
812
813 if (type == LLCP_TLV_SN) {
814 *sn_len = length;
815 return &tlv[2];
816 }
817
818 offset += length + 2;
819 tlv += length + 2;
820 }
821
822 return NULL;
823}
824
968272bf
SO		 825static void nfc_llcp_recv_ui(struct nfc_llcp_local *local,
826 struct sk_buff *skb)
827{
828 struct nfc_llcp_sock *llcp_sock;
829 struct nfc_llcp_ui_cb *ui_cb;
830 u8 dsap, ssap;
831
832 dsap = nfc_llcp_dsap(skb);
833 ssap = nfc_llcp_ssap(skb);
834
835 ui_cb = nfc_llcp_ui_skb_cb(skb);
836 ui_cb->dsap = dsap;
837 ui_cb->ssap = ssap;
838
968272bf
SO		 839 pr_debug("%d %d\n", dsap, ssap);
840
841 /* We're looking for a bound socket, not a client one */
842 llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
843 if (llcp_sock == NULL || llcp_sock->sk.sk_type != SOCK_DGRAM)
844 return;
845
846 /* There is no sequence with UI frames */
847 skb_pull(skb, LLCP_HEADER_SIZE);
1727cf93
SO		 848 if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
849 /*
850 * UI frames will be freed from the socket layer, so we
851 * need to keep them alive until someone receives them.
852 */
853 skb_get(skb);
854 } else {
855 pr_err("Receive queue is full\n");
968272bf
SO		 856 }
857
858 nfc_llcp_sock_put(llcp_sock);
859}
860
d646960f 861static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
427a2eb1 862 struct sk_buff *skb)
d646960f
SO		 863{
864 struct sock *new_sk, *parent;
865 struct nfc_llcp_sock *sock, *new_sock;
a69f32af 866 u8 dsap, ssap, reason;
d646960f
SO		 867
868 dsap = nfc_llcp_dsap(skb);
869 ssap = nfc_llcp_ssap(skb);
870
871 pr_debug("%d %d\n", dsap, ssap);
872
d646960f 873 if (dsap != LLCP_SAP_SDP) {
a69f32af
SO		 874 sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
875 if (sock == NULL || sock->sk.sk_state != LLCP_LISTEN) {
d646960f
SO		 876 reason = LLCP_DM_NOBOUND;
877 goto fail;
878 }
d646960f
SO		 879 } else {
880 u8 *sn;
881 size_t sn_len;
882
883 sn = nfc_llcp_connect_sn(skb, &sn_len);
884 if (sn == NULL) {
885 reason = LLCP_DM_NOBOUND;
886 goto fail;
887 }
888
889 pr_debug("Service name length %zu\n", sn_len);
890
a69f32af
SO		 891 sock = nfc_llcp_sock_get_sn(local, sn, sn_len);
892 if (sock == NULL) {
893 reason = LLCP_DM_NOBOUND;
894 goto fail;
d646960f 895 }
d646960f
SO		 896 }
897
a69f32af 898 lock_sock(&sock->sk);
d646960f 899
d646960f
SO		 900 parent = &sock->sk;
901
902 if (sk_acceptq_is_full(parent)) {
903 reason = LLCP_DM_REJ;
904 release_sock(&sock->sk);
905 sock_put(&sock->sk);
906 goto fail;
907 }
908
8f50020e
SO		 909 if (sock->ssap == LLCP_SDP_UNBOUND) {
910 u8 ssap = nfc_llcp_reserve_sdp_ssap(local);
911
912 pr_debug("First client, reserving %d\n", ssap);
913
914 if (ssap == LLCP_SAP_MAX) {
915 reason = LLCP_DM_REJ;
916 release_sock(&sock->sk);
917 sock_put(&sock->sk);
918 goto fail;
919 }
920
921 sock->ssap = ssap;
922 }
923
427a2eb1 924 new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, GFP_ATOMIC);
d646960f
SO		 925 if (new_sk == NULL) {
926 reason = LLCP_DM_REJ;
927 release_sock(&sock->sk);
928 sock_put(&sock->sk);
929 goto fail;
930 }
931
932 new_sock = nfc_llcp_sock(new_sk);
933 new_sock->dev = local->dev;
c7aa1225 934 new_sock->local = nfc_llcp_local_get(local);
06d44f80
SO		 935 new_sock->rw = sock->rw;
936 new_sock->miux = sock->miux;
e4306bec 937 new_sock->remote_miu = local->remote_miu;
d646960f 938 new_sock->nfc_protocol = sock->nfc_protocol;
d646960f 939 new_sock->dsap = ssap;
025f1520 940 new_sock->target_idx = local->target_idx;
d646960f 941 new_sock->parent = parent;
8f50020e
SO		 942 new_sock->ssap = sock->ssap;
943 if (sock->ssap < LLCP_LOCAL_NUM_SAP && sock->ssap >= LLCP_WKS_NUM_SAP) {
944 atomic_t *client_count;
945
946 pr_debug("reserved_ssap %d for %p\n", sock->ssap, new_sock);
947
948 client_count =
949 &local->local_sdp_cnt[sock->ssap - LLCP_WKS_NUM_SAP];
950
951 atomic_inc(client_count);
952 new_sock->reserved_ssap = sock->ssap;
953 }
d646960f 954
7a06e586
SO		 955 nfc_llcp_parse_connection_tlv(new_sock, &skb->data[LLCP_HEADER_SIZE],
956 skb->len - LLCP_HEADER_SIZE);
957
d646960f
SO		 958 pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk);
959
a69f32af 960 nfc_llcp_sock_link(&local->sockets, new_sk);
d646960f
SO		 961
962 nfc_llcp_accept_enqueue(&sock->sk, new_sk);
963
964 nfc_get_device(local->dev->idx);
965
966 new_sk->sk_state = LLCP_CONNECTED;
967
968 /* Wake the listening processes */
969 parent->sk_data_ready(parent, 0);
970
971 /* Send CC */
972 nfc_llcp_send_cc(new_sock);
973
974 release_sock(&sock->sk);
975 sock_put(&sock->sk);
976
977 return;
978
979fail:
980 /* Send DM */
981 nfc_llcp_send_dm(local, dsap, ssap, reason);
d646960f
SO		 982}
983
d094afa1 984int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock)
4722d2b7 985{
d094afa1 986 int nr_frames = 0;
4722d2b7
SO		 987 struct nfc_llcp_local *local = sock->local;
988
989 pr_debug("Remote ready %d tx queue len %d remote rw %d",
427a2eb1 990 sock->remote_ready, skb_queue_len(&sock->tx_pending_queue),
e4306bec 991 sock->remote_rw);
4722d2b7
SO		 992
993 /* Try to queue some I frames for transmission */
994 while (sock->remote_ready &&
e4306bec 995 skb_queue_len(&sock->tx_pending_queue) < sock->remote_rw) {
84457960 996 struct sk_buff *pdu;
4722d2b7
SO		 997
998 pdu = skb_dequeue(&sock->tx_queue);
999 if (pdu == NULL)
1000 break;
1001
1002 /* Update N(S)/N(R) */
1003 nfc_llcp_set_nrns(sock, pdu);
1004
4722d2b7 1005 skb_queue_tail(&local->tx_queue, pdu);
d094afa1 1006 nr_frames++;
4722d2b7 1007 }
d094afa1
SO		 1008
1009 return nr_frames;
4722d2b7
SO		 1010}
1011
d646960f 1012static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
427a2eb1 1013 struct sk_buff *skb)
d646960f
SO		 1014{
1015 struct nfc_llcp_sock *llcp_sock;
1016 struct sock *sk;
1017 u8 dsap, ssap, ptype, ns, nr;
1018
1019 ptype = nfc_llcp_ptype(skb);
1020 dsap = nfc_llcp_dsap(skb);
1021 ssap = nfc_llcp_ssap(skb);
1022 ns = nfc_llcp_ns(skb);
1023 nr = nfc_llcp_nr(skb);
1024
1025 pr_debug("%d %d R %d S %d\n", dsap, ssap, nr, ns);
1026
1027 llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
1028 if (llcp_sock == NULL) {
1029 nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
1030 return;
1031 }
1032
1033 sk = &llcp_sock->sk;
1034 lock_sock(sk);
1035 if (sk->sk_state == LLCP_CLOSED) {
1036 release_sock(sk);
1037 nfc_llcp_sock_put(llcp_sock);
1038 }
1039
d646960f
SO		 1040 /* Pass the payload upstream */
1041 if (ptype == LLCP_PDU_I) {
1042 pr_debug("I frame, queueing on %p\n", &llcp_sock->sk);
1043
53aef920
SO		 1044 if (ns == llcp_sock->recv_n)
1045 llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16;
1046 else
1047 pr_err("Received out of sequence I PDU\n");
1048
d646960f 1049 skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE);
1727cf93
SO		 1050 if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
1051 /*
1052 * I frames will be freed from the socket layer, so we
1053 * need to keep them alive until someone receives them.
1054 */
1055 skb_get(skb);
1056 } else {
1057 pr_err("Receive queue is full\n");
d646960f
SO		 1058 }
1059 }
1060
1061 /* Remove skbs from the pending queue */
1062 if (llcp_sock->send_ack_n != nr) {
1063 struct sk_buff *s, *tmp;
28981491 1064 u8 n;
d646960f
SO		 1065
1066 llcp_sock->send_ack_n = nr;
1067
84457960
SO		 1068 /* Remove and free all skbs until ns == nr */
1069 skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp) {
28981491
WR		 1070 n = nfc_llcp_ns(s);
1071
84457960
SO		 1072 skb_unlink(s, &llcp_sock->tx_pending_queue);
1073 kfree_skb(s);
1074
28981491 1075 if (n == nr)
84457960
SO		 1076 break;
1077 }
1078
1079 /* Re-queue the remaining skbs for transmission */
1080 skb_queue_reverse_walk_safe(&llcp_sock->tx_pending_queue,
1081 s, tmp) {
1082 skb_unlink(s, &llcp_sock->tx_pending_queue);
1083 skb_queue_head(&local->tx_queue, s);
1084 }
d646960f
SO		 1085 }
1086
53aef920
SO		 1087 if (ptype == LLCP_PDU_RR)
1088 llcp_sock->remote_ready = true;
427a2eb1 1089 else if (ptype == LLCP_PDU_RNR)
53aef920
SO		 1090 llcp_sock->remote_ready = false;
1091
56af2568 1092 if (nfc_llcp_queue_i_frames(llcp_sock) == 0 && ptype == LLCP_PDU_I)
d094afa1 1093 nfc_llcp_send_rr(llcp_sock);
d646960f
SO		 1094
1095 release_sock(sk);
1096 nfc_llcp_sock_put(llcp_sock);
1097}
1098
1099static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
427a2eb1 1100 struct sk_buff *skb)
d646960f
SO		 1101{
1102 struct nfc_llcp_sock *llcp_sock;
1103 struct sock *sk;
1104 u8 dsap, ssap;
1105
1106 dsap = nfc_llcp_dsap(skb);
1107 ssap = nfc_llcp_ssap(skb);
1108
1109 llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
1110 if (llcp_sock == NULL) {
1111 nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
1112 return;
1113 }
1114
1115 sk = &llcp_sock->sk;
1116 lock_sock(sk);
f31652a5
SO		 1117
1118 nfc_llcp_socket_purge(llcp_sock);
1119
d646960f
SO		 1120 if (sk->sk_state == LLCP_CLOSED) {
1121 release_sock(sk);
1122 nfc_llcp_sock_put(llcp_sock);
1123 }
1124
d646960f
SO		 1125 if (sk->sk_state == LLCP_CONNECTED) {
1126 nfc_put_device(local->dev);
1127 sk->sk_state = LLCP_CLOSED;
1128 sk->sk_state_change(sk);
1129 }
1130
1131 nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_DISC);
1132
1133 release_sock(sk);
1134 nfc_llcp_sock_put(llcp_sock);
1135}
1136
427a2eb1 1137static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb)
d646960f
SO		 1138{
1139 struct nfc_llcp_sock *llcp_sock;
ff353d86 1140 struct sock *sk;
d646960f
SO		 1141 u8 dsap, ssap;
1142
d646960f
SO		 1143 dsap = nfc_llcp_dsap(skb);
1144 ssap = nfc_llcp_ssap(skb);
1145
a69f32af 1146 llcp_sock = nfc_llcp_connecting_sock_get(local, dsap);
d646960f
SO		 1147 if (llcp_sock == NULL) {
1148 pr_err("Invalid CC\n");
1149 nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
1150
1151 return;
1152 }
1153
ff353d86 1154 sk = &llcp_sock->sk;
d646960f 1155
a69f32af
SO		 1156 /* Unlink from connecting and link to the client array */
1157 nfc_llcp_sock_unlink(&local->connecting_sockets, sk);
1158 nfc_llcp_sock_link(&local->sockets, sk);
1159 llcp_sock->dsap = ssap;
1160
7a06e586
SO		 1161 nfc_llcp_parse_connection_tlv(llcp_sock, &skb->data[LLCP_HEADER_SIZE],
1162 skb->len - LLCP_HEADER_SIZE);
d646960f 1163
ff353d86
SO		 1164 sk->sk_state = LLCP_CONNECTED;
1165 sk->sk_state_change(sk);
1166
d646960f
SO		 1167 nfc_llcp_sock_put(llcp_sock);
1168}
1169
5c0560b7
SO		 1170static void nfc_llcp_recv_dm(struct nfc_llcp_local *local, struct sk_buff *skb)
1171{
1172 struct nfc_llcp_sock *llcp_sock;
1173 struct sock *sk;
1174 u8 dsap, ssap, reason;
1175
1176 dsap = nfc_llcp_dsap(skb);
1177 ssap = nfc_llcp_ssap(skb);
1178 reason = skb->data[2];
1179
1180 pr_debug("%d %d reason %d\n", ssap, dsap, reason);
1181
1182 switch (reason) {
1183 case LLCP_DM_NOBOUND:
1184 case LLCP_DM_REJ:
1185 llcp_sock = nfc_llcp_connecting_sock_get(local, dsap);
1186 break;
1187
1188 default:
1189 llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
1190 break;
1191 }
1192
1193 if (llcp_sock == NULL) {
a8df0f37 1194 pr_debug("Already closed\n");
5c0560b7
SO		 1195 return;
1196 }
1197
1198 sk = &llcp_sock->sk;
1199
1200 sk->sk_err = ENXIO;
1201 sk->sk_state = LLCP_CLOSED;
1202 sk->sk_state_change(sk);
1203
1204 nfc_llcp_sock_put(llcp_sock);
5c0560b7
SO		 1205}
1206
19cfe584
SO		 1207static void nfc_llcp_recv_snl(struct nfc_llcp_local *local,
1208 struct sk_buff *skb)
1209{
1210 struct nfc_llcp_sock *llcp_sock;
1211 u8 dsap, ssap, *tlv, type, length, tid, sap;
1212 u16 tlv_len, offset;
1213 char *service_name;
1214 size_t service_name_len;
e0ae7bac
TE		 1215 struct nfc_llcp_sdp_tlv *sdp;
1216 HLIST_HEAD(llc_sdres_list);
1217 size_t sdres_tlvs_len;
d9b8d8e1 1218 HLIST_HEAD(nl_sdres_list);
19cfe584
SO		 1219
1220 dsap = nfc_llcp_dsap(skb);
1221 ssap = nfc_llcp_ssap(skb);
1222
1223 pr_debug("%d %d\n", dsap, ssap);
1224
1225 if (dsap != LLCP_SAP_SDP || ssap != LLCP_SAP_SDP) {
1226 pr_err("Wrong SNL SAP\n");
1227 return;
1228 }
1229
1230 tlv = &skb->data[LLCP_HEADER_SIZE];
1231 tlv_len = skb->len - LLCP_HEADER_SIZE;
1232 offset = 0;
e0ae7bac 1233 sdres_tlvs_len = 0;
19cfe584 1234
0f450772 1235 while (offset < tlv_len) {
19cfe584
SO		 1236 type = tlv[0];
1237 length = tlv[1];
1238
1239 switch (type) {
1240 case LLCP_TLV_SDREQ:
1241 tid = tlv[2];
1242 service_name = (char *) &tlv[3];
1243 service_name_len = length - 1;
1244
e6904081 1245 pr_debug("Looking for %.16s\n", service_name);
19cfe584
SO		 1246
1247 if (service_name_len == strlen("urn:nfc:sn:sdp") &&
1248 !strncmp(service_name, "urn:nfc:sn:sdp",
1249 service_name_len)) {
1250 sap = 1;
e0ae7bac 1251 goto add_snl;
e6904081
SO		 1252 }
1253
1254 llcp_sock = nfc_llcp_sock_from_sn(local, service_name,
1255 service_name_len);
1256 if (!llcp_sock) {
1257 sap = 0;
e0ae7bac 1258 goto add_snl;
e6904081
SO		 1259 }
1260
1261 /*
1262 * We found a socket but its ssap has not been reserved
1263 * yet. We need to assign it for good and send a reply.
1264 * The ssap will be freed when the socket is closed.
1265 */
1266 if (llcp_sock->ssap == LLCP_SDP_UNBOUND) {
1267 atomic_t *client_count;
1268
1269 sap = nfc_llcp_reserve_sdp_ssap(local);
1270
1271 pr_debug("Reserving %d\n", sap);
1272
1273 if (sap == LLCP_SAP_MAX) {
1274 sap = 0;
e0ae7bac 1275 goto add_snl;
e6904081
SO		 1276 }
1277
1278 client_count =
1279 &local->local_sdp_cnt[sap -
1280 LLCP_WKS_NUM_SAP];
1281
1282 atomic_inc(client_count);
1283
1284 llcp_sock->ssap = sap;
1285 llcp_sock->reserved_ssap = sap;
19cfe584 1286 } else {
e6904081 1287 sap = llcp_sock->ssap;
19cfe584
SO		 1288 }
1289
e6904081
SO		 1290 pr_debug("%p %d\n", llcp_sock, sap);
1291
e0ae7bac
TE		 1292add_snl:
1293 sdp = nfc_llcp_build_sdres_tlv(tid, sap);
1294 if (sdp == NULL)
1295 goto exit;
1296
1297 sdres_tlvs_len += sdp->tlv_len;
1298 hlist_add_head(&sdp->node, &llc_sdres_list);
19cfe584
SO		 1299 break;
1300
d9b8d8e1
TE		 1301 case LLCP_TLV_SDRES:
1302 mutex_lock(&local->sdreq_lock);
1303
1304 pr_debug("LLCP_TLV_SDRES: searching tid %d\n", tlv[2]);
1305
1306 hlist_for_each_entry(sdp, &local->pending_sdreqs, node) {
1307 if (sdp->tid != tlv[2])
1308 continue;
1309
1310 sdp->sap = tlv[3];
1311
1312 pr_debug("Found: uri=%s, sap=%d\n",
1313 sdp->uri, sdp->sap);
1314
1315 hlist_del(&sdp->node);
1316
1317 hlist_add_head(&sdp->node, &nl_sdres_list);
1318
1319 break;
1320 }
1321
1322 mutex_unlock(&local->sdreq_lock);
1323 break;
1324
19cfe584
SO		 1325 default:
1326 pr_err("Invalid SNL tlv value 0x%x\n", type);
1327 break;
1328 }
1329
1330 offset += length + 2;
1331 tlv += length + 2;
1332 }
e0ae7bac
TE		 1333
1334exit:
d9b8d8e1
TE		 1335 if (!hlist_empty(&nl_sdres_list))
1336 nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list);
1337
e0ae7bac
TE		 1338 if (!hlist_empty(&llc_sdres_list))
1339 nfc_llcp_send_snl_sdres(local, &llc_sdres_list, sdres_tlvs_len);
19cfe584
SO		 1340}
1341
098dafcf 1342static void nfc_llcp_recv_agf(struct nfc_llcp_local *local, struct sk_buff *skb)
d646960f 1343{
098dafcf
TE		 1344 u8 ptype;
1345 u16 pdu_len;
1346 struct sk_buff *new_skb;
d646960f 1347
098dafcf
TE		 1348 if (skb->len <= LLCP_HEADER_SIZE) {
1349 pr_err("Malformed AGF PDU\n");
d646960f
SO		 1350 return;
1351 }
1352
098dafcf
TE		 1353 skb_pull(skb, LLCP_HEADER_SIZE);
1354
1355 while (skb->len > LLCP_AGF_PDU_HEADER_SIZE) {
1356 pdu_len = skb->data[0] << 8 | skb->data[1];
1357
1358 skb_pull(skb, LLCP_AGF_PDU_HEADER_SIZE);
1359
1360 if (pdu_len < LLCP_HEADER_SIZE || pdu_len > skb->len) {
1361 pr_err("Malformed AGF PDU\n");
1362 return;
1363 }
1364
1365 ptype = nfc_llcp_ptype(skb);
1366
1367 if (ptype == LLCP_PDU_SYMM || ptype == LLCP_PDU_AGF)
1368 goto next;
1369
1370 new_skb = nfc_alloc_recv_skb(pdu_len, GFP_KERNEL);
1371 if (new_skb == NULL) {
1372 pr_err("Could not allocate PDU\n");
1373 return;
1374 }
1375
1376 memcpy(skb_put(new_skb, pdu_len), skb->data, pdu_len);
1377
1378 nfc_llcp_rx_skb(local, new_skb);
1379
1380 kfree_skb(new_skb);
1381next:
1382 skb_pull(skb, pdu_len);
1383 }
1384}
1385
1386static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb)
1387{
1388 u8 dsap, ssap, ptype;
1389
d646960f
SO		 1390 ptype = nfc_llcp_ptype(skb);
1391 dsap = nfc_llcp_dsap(skb);
1392 ssap = nfc_llcp_ssap(skb);
1393
1394 pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap);
1395
4be646ec
SO		 1396 if (ptype != LLCP_PDU_SYMM)
1397 print_hex_dump(KERN_DEBUG, "LLCP Rx: ", DUMP_PREFIX_OFFSET,
1398 16, 1, skb->data, skb->len, true);
1399
d646960f
SO		 1400 switch (ptype) {
1401 case LLCP_PDU_SYMM:
1402 pr_debug("SYMM\n");
1403 break;
1404
968272bf
SO		 1405 case LLCP_PDU_UI:
1406 pr_debug("UI\n");
1407 nfc_llcp_recv_ui(local, skb);
1408 break;
1409
d646960f
SO		 1410 case LLCP_PDU_CONNECT:
1411 pr_debug("CONNECT\n");
1412 nfc_llcp_recv_connect(local, skb);
1413 break;
1414
1415 case LLCP_PDU_DISC:
1416 pr_debug("DISC\n");
1417 nfc_llcp_recv_disc(local, skb);
1418 break;
1419
1420 case LLCP_PDU_CC:
1421 pr_debug("CC\n");
1422 nfc_llcp_recv_cc(local, skb);
1423 break;
1424
5c0560b7
SO		 1425 case LLCP_PDU_DM:
1426 pr_debug("DM\n");
1427 nfc_llcp_recv_dm(local, skb);
1428 break;
1429
19cfe584
SO		 1430 case LLCP_PDU_SNL:
1431 pr_debug("SNL\n");
1432 nfc_llcp_recv_snl(local, skb);
1433 break;
1434
d646960f
SO		 1435 case LLCP_PDU_I:
1436 case LLCP_PDU_RR:
53aef920 1437 case LLCP_PDU_RNR:
d646960f
SO		 1438 pr_debug("I frame\n");
1439 nfc_llcp_recv_hdlc(local, skb);
1440 break;
1441
098dafcf
TE		 1442 case LLCP_PDU_AGF:
1443 pr_debug("AGF frame\n");
1444 nfc_llcp_recv_agf(local, skb);
1445 break;
d646960f 1446 }
098dafcf
TE		 1447}
1448
1449static void nfc_llcp_rx_work(struct work_struct *work)
1450{
1451 struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
1452 rx_work);
1453 struct sk_buff *skb;
1454
1455 skb = local->rx_pending;
1456 if (skb == NULL) {
1457 pr_debug("No pending SKB\n");
1458 return;
1459 }
1460
1461 __net_timestamp(skb);
1462
1463 nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_RX);
1464
1465 nfc_llcp_rx_skb(local, skb);
d646960f 1466
916082b0 1467 schedule_work(&local->tx_work);
d646960f
SO		 1468 kfree_skb(local->rx_pending);
1469 local->rx_pending = NULL;
d646960f
SO		 1470}
1471
1727cf93
SO		 1472static void __nfc_llcp_recv(struct nfc_llcp_local *local, struct sk_buff *skb)
1473{
1474 local->rx_pending = skb;
1475 del_timer(&local->link_timer);
1476 schedule_work(&local->rx_work);
1477}
1478
d646960f
SO		 1479void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
1480{
1481 struct nfc_llcp_local *local = (struct nfc_llcp_local *) data;
1482
1483 pr_debug("Received an LLCP PDU\n");
1484 if (err < 0) {
427a2eb1 1485 pr_err("err %d\n", err);
d646960f
SO		 1486 return;
1487 }
1488
1727cf93 1489 __nfc_llcp_recv(local, skb);
d646960f
SO		 1490}
1491
73167ced
SO		 1492int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb)
1493{
1494 struct nfc_llcp_local *local;
1495
1496 local = nfc_llcp_find_local(dev);
1497 if (local == NULL)
1498 return -ENODEV;
1499
1727cf93 1500 __nfc_llcp_recv(local, skb);
73167ced
SO		 1501
1502 return 0;
1503}
1504
d646960f
SO		 1505void nfc_llcp_mac_is_down(struct nfc_dev *dev)
1506{
1507 struct nfc_llcp_local *local;
1508
1509 local = nfc_llcp_find_local(dev);
1510 if (local == NULL)
1511 return;
1512
abd18d43
TE		 1513 local->remote_miu = LLCP_DEFAULT_MIU;
1514 local->remote_lto = LLCP_DEFAULT_LTO;
1515
d646960f 1516 /* Close and purge all existing sockets */
3bbc0ceb 1517 nfc_llcp_socket_release(local, true, 0);
d646960f
SO		 1518}
1519
1520void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
1521 u8 comm_mode, u8 rf_mode)
1522{
1523 struct nfc_llcp_local *local;
1524
1525 pr_debug("rf mode %d\n", rf_mode);
1526
1527 local = nfc_llcp_find_local(dev);
1528 if (local == NULL)
1529 return;
1530
1531 local->target_idx = target_idx;
1532 local->comm_mode = comm_mode;
1533 local->rf_mode = rf_mode;
1534
1535 if (rf_mode == NFC_RF_INITIATOR) {
1536 pr_debug("Queueing Tx work\n");
1537
916082b0 1538 schedule_work(&local->tx_work);
d646960f
SO		 1539 } else {
1540 mod_timer(&local->link_timer,
427a2eb1 1541 jiffies + msecs_to_jiffies(local->remote_lto));
d646960f
SO		 1542 }
1543}
1544
1545int nfc_llcp_register_device(struct nfc_dev *ndev)
1546{
d646960f 1547 struct nfc_llcp_local *local;
d646960f
SO		 1548
1549 local = kzalloc(sizeof(struct nfc_llcp_local), GFP_KERNEL);
1550 if (local == NULL)
1551 return -ENOMEM;
1552
1553 local->dev = ndev;
1554 INIT_LIST_HEAD(&local->list);
c7aa1225 1555 kref_init(&local->ref);
d646960f 1556 mutex_init(&local->sdp_lock);
d646960f
SO		 1557 init_timer(&local->link_timer);
1558 local->link_timer.data = (unsigned long) local;
1559 local->link_timer.function = nfc_llcp_symm_timer;
1560
1561 skb_queue_head_init(&local->tx_queue);
1562 INIT_WORK(&local->tx_work, nfc_llcp_tx_work);
d646960f
SO		 1563
1564 local->rx_pending = NULL;
1565 INIT_WORK(&local->rx_work, nfc_llcp_rx_work);
d646960f
SO		 1566
1567 INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work);
d646960f 1568
fe235b58
SJ		 1569 rwlock_init(&local->sockets.lock);
1570 rwlock_init(&local->connecting_sockets.lock);
4463523b 1571 rwlock_init(&local->raw_sockets.lock);
a69f32af 1572
52feb444
TE		 1573 local->lto = 150; /* 1500 ms */
1574 local->rw = LLCP_MAX_RW;
1575 local->miux = cpu_to_be16(LLCP_MAX_MIUX);
1576
d646960f
SO		 1577 nfc_llcp_build_gb(local);
1578
1579 local->remote_miu = LLCP_DEFAULT_MIU;
1580 local->remote_lto = LLCP_DEFAULT_LTO;
d646960f 1581
d9b8d8e1
TE		 1582 mutex_init(&local->sdreq_lock);
1583 INIT_HLIST_HEAD(&local->pending_sdreqs);
40213fa8
TE		 1584 init_timer(&local->sdreq_timer);
1585 local->sdreq_timer.data = (unsigned long) local;
1586 local->sdreq_timer.function = nfc_llcp_sdreq_timer;
1587 INIT_WORK(&local->sdreq_timeout_work, nfc_llcp_sdreq_timeout_work);
d9b8d8e1 1588
16a78e9f 1589 list_add(&local->list, &llcp_devices);
d646960f 1590
d646960f
SO		 1591 return 0;
1592}
1593
1594void nfc_llcp_unregister_device(struct nfc_dev *dev)
1595{
1596 struct nfc_llcp_local *local = nfc_llcp_find_local(dev);
1597
1598 if (local == NULL) {
1599 pr_debug("No such device\n");
1600 return;
1601 }
1602
3536da06
SO		 1603 local_cleanup(local, false);
1604
c7aa1225 1605 nfc_llcp_local_put(local);
d646960f
SO		 1606}
1607
1608int __init nfc_llcp_init(void)
1609{
1610 INIT_LIST_HEAD(&llcp_devices);
1611
1612 return nfc_llcp_sock_init();
1613}
1614
1615void nfc_llcp_exit(void)
1616{
1617 nfc_llcp_sock_exit();
1618}
Linux kernel - Deliverables
This page took 0.165395 seconds and 5 git commands to generate.