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

source code of linux/net/nfc/llcp_core.c