projects / linux-2.6-block.git / blame
? search:
summary | shortlog | log | commit | commitdiff | tree
blob | blame (incremental) | history | HEAD
NFC: LLCP late binding
[linux-2.6-block.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
a69f32af 34void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *sk)
d646960f 35{
a69f32af
SO		 36 write_lock(&l->lock);
37 sk_add_node(sk, &l->head);
38 write_unlock(&l->lock);
39}
d646960f 40
a69f32af
SO		 41void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *sk)
42{
43 write_lock(&l->lock);
44 sk_del_node_init(sk);
45 write_unlock(&l->lock);
46}
d646960f 47
4d22ea15 48static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen)
a69f32af
SO		 49{
50 struct sock *sk;
51 struct hlist_node *node, *tmp;
52 struct nfc_llcp_sock *llcp_sock;
d646960f 53
a69f32af 54 write_lock(&local->sockets.lock);
40c75f81 55
a69f32af
SO		 56 sk_for_each_safe(sk, node, tmp, &local->sockets.head) {
57 llcp_sock = nfc_llcp_sock(sk);
d646960f 58
a69f32af 59 lock_sock(sk);
d646960f 60
a69f32af
SO		 61 if (sk->sk_state == LLCP_CONNECTED)
62 nfc_put_device(llcp_sock->dev);
d646960f 63
a69f32af 64 if (sk->sk_state == LLCP_LISTEN) {
d646960f
SO		 65 struct nfc_llcp_sock *lsk, *n;
66 struct sock *accept_sk;
67
a69f32af 68 list_for_each_entry_safe(lsk, n, &llcp_sock->accept_queue,
427a2eb1 69 accept_queue) {
d646960f
SO		 70 accept_sk = &lsk->sk;
71 lock_sock(accept_sk);
72
73 nfc_llcp_accept_unlink(accept_sk);
74
75 accept_sk->sk_state = LLCP_CLOSED;
d646960f
SO		 76
77 release_sock(accept_sk);
78
79 sock_orphan(accept_sk);
80 }
4d22ea15
SO		 81
82 if (listen == true) {
83 release_sock(sk);
84 continue;
85 }
d646960f
SO		 86 }
87
a69f32af 88 sk->sk_state = LLCP_CLOSED;
d646960f 89
a69f32af 90 release_sock(sk);
d646960f 91
a69f32af 92 sock_orphan(sk);
40c75f81 93
a69f32af 94 sk_del_node_init(sk);
d646960f
SO		 95 }
96
a69f32af 97 write_unlock(&local->sockets.lock);
d646960f
SO		 98}
99
c7aa1225
SO		 100struct nfc_llcp_local *nfc_llcp_local_get(struct nfc_llcp_local *local)
101{
102 kref_get(&local->ref);
103
104 return local;
105}
106
107static void local_release(struct kref *ref)
108{
109 struct nfc_llcp_local *local;
110
111 local = container_of(ref, struct nfc_llcp_local, ref);
112
113 list_del(&local->list);
4d22ea15 114 nfc_llcp_socket_release(local, false);
c7aa1225
SO		 115 del_timer_sync(&local->link_timer);
116 skb_queue_purge(&local->tx_queue);
117 destroy_workqueue(local->tx_wq);
118 destroy_workqueue(local->rx_wq);
07922bb1 119 destroy_workqueue(local->timeout_wq);
c7aa1225
SO		 120 kfree_skb(local->rx_pending);
121 kfree(local);
122}
123
124int nfc_llcp_local_put(struct nfc_llcp_local *local)
125{
a69f32af
SO		 126 WARN_ON(local == NULL);
127
128 if (local == NULL)
129 return 0;
130
c7aa1225
SO		 131 return kref_put(&local->ref, local_release);
132}
133
8f50020e
SO		 134static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
135 u8 ssap, u8 dsap)
136{
137 struct sock *sk;
138 struct hlist_node *node;
139 struct nfc_llcp_sock *llcp_sock;
140
141 pr_debug("ssap dsap %d %d\n", ssap, dsap);
142
143 if (ssap == 0 && dsap == 0)
144 return NULL;
145
146 read_lock(&local->sockets.lock);
147
148 llcp_sock = NULL;
149
150 sk_for_each(sk, node, &local->sockets.head) {
151 llcp_sock = nfc_llcp_sock(sk);
152
153 if (llcp_sock->ssap == ssap && llcp_sock->dsap == dsap)
154 break;
155 }
156
157 read_unlock(&local->sockets.lock);
158
159 if (llcp_sock == NULL)
160 return NULL;
161
162 sock_hold(&llcp_sock->sk);
163
164 return llcp_sock;
165}
166
167static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock)
168{
169 sock_put(&sock->sk);
170}
171
d646960f
SO		 172static void nfc_llcp_timeout_work(struct work_struct *work)
173{
174 struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
427a2eb1 175 timeout_work);
d646960f
SO		 176
177 nfc_dep_link_down(local->dev);
178}
179
180static void nfc_llcp_symm_timer(unsigned long data)
181{
182 struct nfc_llcp_local *local = (struct nfc_llcp_local *) data;
183
184 pr_err("SYMM timeout\n");
185
186 queue_work(local->timeout_wq, &local->timeout_work);
187}
188
189struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev)
190{
191 struct nfc_llcp_local *local, *n;
192
193 list_for_each_entry_safe(local, n, &llcp_devices, list)
194 if (local->dev == dev)
195 return local;
196
197 pr_debug("No device found\n");
198
199 return NULL;
200}
201
202static char *wks[] = {
203 NULL,
204 NULL, /* SDP */
205 "urn:nfc:sn:ip",
206 "urn:nfc:sn:obex",
207 "urn:nfc:sn:snep",
208};
209
210static int nfc_llcp_wks_sap(char *service_name, size_t service_name_len)
211{
212 int sap, num_wks;
213
214 pr_debug("%s\n", service_name);
215
216 if (service_name == NULL)
217 return -EINVAL;
218
219 num_wks = ARRAY_SIZE(wks);
220
427a2eb1 221 for (sap = 0; sap < num_wks; sap++) {
d646960f
SO		 222 if (wks[sap] == NULL)
223 continue;
224
225 if (strncmp(wks[sap], service_name, service_name_len) == 0)
226 return sap;
227 }
228
229 return -EINVAL;
230}
231
8f50020e
SO		 232static
233struct nfc_llcp_sock *nfc_llcp_sock_from_sn(struct nfc_llcp_local *local,
234 u8 *sn, size_t sn_len)
235{
236 struct sock *sk;
237 struct hlist_node *node;
238 struct nfc_llcp_sock *llcp_sock, *tmp_sock;
239
240 pr_debug("sn %zd %p\n", sn_len, sn);
241
242 if (sn == NULL || sn_len == 0)
243 return NULL;
244
245 read_lock(&local->sockets.lock);
246
247 llcp_sock = NULL;
248
249 sk_for_each(sk, node, &local->sockets.head) {
250 tmp_sock = nfc_llcp_sock(sk);
251
252 pr_debug("llcp sock %p\n", tmp_sock);
253
254 if (tmp_sock->sk.sk_state != LLCP_LISTEN)
255 continue;
256
257 if (tmp_sock->service_name == NULL ||
258 tmp_sock->service_name_len == 0)
259 continue;
260
261 if (tmp_sock->service_name_len != sn_len)
262 continue;
263
264 if (memcmp(sn, tmp_sock->service_name, sn_len) == 0) {
265 llcp_sock = tmp_sock;
266 break;
267 }
268 }
269
270 read_unlock(&local->sockets.lock);
271
272 pr_debug("Found llcp sock %p\n", llcp_sock);
273
274 return llcp_sock;
275}
276
d646960f 277u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
427a2eb1 278 struct nfc_llcp_sock *sock)
d646960f
SO		 279{
280 mutex_lock(&local->sdp_lock);
281
282 if (sock->service_name != NULL && sock->service_name_len > 0) {
283 int ssap = nfc_llcp_wks_sap(sock->service_name,
427a2eb1 284 sock->service_name_len);
d646960f
SO		 285
286 if (ssap > 0) {
287 pr_debug("WKS %d\n", ssap);
288
289 /* This is a WKS, let's check if it's free */
290 if (local->local_wks & BIT(ssap)) {
291 mutex_unlock(&local->sdp_lock);
292
293 return LLCP_SAP_MAX;
294 }
295
1762c17c 296 set_bit(ssap, &local->local_wks);
d646960f
SO		 297 mutex_unlock(&local->sdp_lock);
298
299 return ssap;
300 }
301
302 /*
8f50020e
SO		 303 * Check if there already is a non WKS socket bound
304 * to this service name.
d646960f 305 */
8f50020e
SO		 306 if (nfc_llcp_sock_from_sn(local, sock->service_name,
307 sock->service_name_len) != NULL) {
d646960f
SO		 308 mutex_unlock(&local->sdp_lock);
309
310 return LLCP_SAP_MAX;
311 }
312
d646960f
SO		 313 mutex_unlock(&local->sdp_lock);
314
8f50020e 315 return LLCP_SDP_UNBOUND;
d646960f 316
ebbb16d9
SO		 317 } else if (sock->ssap != 0 && sock->ssap < LLCP_WKS_NUM_SAP) {
318 if (!test_bit(sock->ssap, &local->local_wks)) {
319 set_bit(sock->ssap, &local->local_wks);
320 mutex_unlock(&local->sdp_lock);
d646960f 321
ebbb16d9 322 return sock->ssap;
d646960f
SO		 323 }
324 }
325
326 mutex_unlock(&local->sdp_lock);
327
328 return LLCP_SAP_MAX;
329}
330
331u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local)
332{
333 u8 local_ssap;
334
335 mutex_lock(&local->sdp_lock);
336
337 local_ssap = find_first_zero_bit(&local->local_sap, LLCP_LOCAL_NUM_SAP);
338 if (local_ssap == LLCP_LOCAL_NUM_SAP) {
339 mutex_unlock(&local->sdp_lock);
340 return LLCP_SAP_MAX;
341 }
342
1762c17c 343 set_bit(local_ssap, &local->local_sap);
d646960f
SO		 344
345 mutex_unlock(&local->sdp_lock);
346
347 return local_ssap + LLCP_LOCAL_SAP_OFFSET;
348}
349
350void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap)
351{
352 u8 local_ssap;
353 unsigned long *sdp;
354
355 if (ssap < LLCP_WKS_NUM_SAP) {
356 local_ssap = ssap;
357 sdp = &local->local_wks;
358 } else if (ssap < LLCP_LOCAL_NUM_SAP) {
8f50020e
SO		 359 atomic_t *client_cnt;
360
d646960f
SO		 361 local_ssap = ssap - LLCP_WKS_NUM_SAP;
362 sdp = &local->local_sdp;
8f50020e
SO		 363 client_cnt = &local->local_sdp_cnt[local_ssap];
364
365 pr_debug("%d clients\n", atomic_read(client_cnt));
366
367 mutex_lock(&local->sdp_lock);
368
369 if (atomic_dec_and_test(client_cnt)) {
370 struct nfc_llcp_sock *l_sock;
371
372 pr_debug("No more clients for SAP %d\n", ssap);
373
374 clear_bit(local_ssap, sdp);
375
376 /* Find the listening sock and set it back to UNBOUND */
377 l_sock = nfc_llcp_sock_get(local, ssap, LLCP_SAP_SDP);
378 if (l_sock) {
379 l_sock->ssap = LLCP_SDP_UNBOUND;
380 nfc_llcp_sock_put(l_sock);
381 }
382 }
383
384 mutex_unlock(&local->sdp_lock);
385
386 return;
d646960f
SO		 387 } else if (ssap < LLCP_MAX_SAP) {
388 local_ssap = ssap - LLCP_LOCAL_NUM_SAP;
389 sdp = &local->local_sap;
390 } else {
391 return;
392 }
393
394 mutex_lock(&local->sdp_lock);
395
1762c17c 396 clear_bit(local_ssap, sdp);
d646960f
SO		 397
398 mutex_unlock(&local->sdp_lock);
399}
400
8f50020e
SO		 401static u8 nfc_llcp_reserve_sdp_ssap(struct nfc_llcp_local *local)
402{
403 u8 ssap;
404
405 mutex_lock(&local->sdp_lock);
406
407 ssap = find_first_zero_bit(&local->local_sdp, LLCP_SDP_NUM_SAP);
408 if (ssap == LLCP_SDP_NUM_SAP) {
409 mutex_unlock(&local->sdp_lock);
410
411 return LLCP_SAP_MAX;
412 }
413
414 pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap);
415
416 set_bit(ssap, &local->local_sdp);
417
418 mutex_unlock(&local->sdp_lock);
419
420 return LLCP_WKS_NUM_SAP + ssap;
421}
422
d646960f
SO		 423static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
424{
425 u8 *gb_cur, *version_tlv, version, version_length;
426 u8 *lto_tlv, lto, lto_length;
427 u8 *wks_tlv, wks_length;
56d5876a
SO		 428 u8 *miux_tlv, miux_length;
429 __be16 miux;
d646960f
SO		 430 u8 gb_len = 0;
431
432 version = LLCP_VERSION_11;
433 version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version,
427a2eb1 434 1, &version_length);
d646960f
SO		 435 gb_len += version_length;
436
437 /* 1500 ms */
438 lto = 150;
91b0ade1 439 lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_LTO, &lto, 1, &lto_length);
d646960f
SO		 440 gb_len += lto_length;
441
442 pr_debug("Local wks 0x%lx\n", local->local_wks);
443 wks_tlv = nfc_llcp_build_tlv(LLCP_TLV_WKS, (u8 *)&local->local_wks, 2,
427a2eb1 444 &wks_length);
d646960f
SO		 445 gb_len += wks_length;
446
56d5876a
SO		 447 miux = cpu_to_be16(LLCP_MAX_MIUX);
448 miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0,
449 &miux_length);
450 gb_len += miux_length;
451
d646960f
SO		 452 gb_len += ARRAY_SIZE(llcp_magic);
453
454 if (gb_len > NFC_MAX_GT_LEN) {
455 kfree(version_tlv);
456 return -EINVAL;
457 }
458
459 gb_cur = local->gb;
460
461 memcpy(gb_cur, llcp_magic, ARRAY_SIZE(llcp_magic));
462 gb_cur += ARRAY_SIZE(llcp_magic);
463
464 memcpy(gb_cur, version_tlv, version_length);
465 gb_cur += version_length;
466
467 memcpy(gb_cur, lto_tlv, lto_length);
468 gb_cur += lto_length;
469
470 memcpy(gb_cur, wks_tlv, wks_length);
471 gb_cur += wks_length;
472
56d5876a
SO		 473 memcpy(gb_cur, miux_tlv, miux_length);
474 gb_cur += miux_length;
475
d646960f
SO		 476 kfree(version_tlv);
477 kfree(lto_tlv);
478
479 local->gb_len = gb_len;
480
481 return 0;
482}
483
b8e7a06d
SO		 484u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len)
485{
486 struct nfc_llcp_local *local;
487
488 local = nfc_llcp_find_local(dev);
489 if (local == NULL) {
490 *general_bytes_len = 0;
491 return NULL;
492 }
493
494 nfc_llcp_build_gb(local);
495
496 *general_bytes_len = local->gb_len;
497
498 return local->gb;
499}
500
d646960f
SO		 501int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len)
502{
503 struct nfc_llcp_local *local = nfc_llcp_find_local(dev);
504
505 if (local == NULL) {
506 pr_err("No LLCP device\n");
507 return -ENODEV;
508 }
509
510 memset(local->remote_gb, 0, NFC_MAX_GT_LEN);
511 memcpy(local->remote_gb, gb, gb_len);
512 local->remote_gb_len = gb_len;
513
427a2eb1 514 if (local->remote_gb == NULL || local->remote_gb_len == 0)
d646960f
SO		 515 return -ENODEV;
516
517 if (memcmp(local->remote_gb, llcp_magic, 3)) {
518 pr_err("MAC does not support LLCP\n");
519 return -EINVAL;
520 }
521
7a06e586
SO		 522 return nfc_llcp_parse_gb_tlv(local,
523 &local->remote_gb[3],
524 local->remote_gb_len - 3);
d646960f
SO		 525}
526
d646960f
SO		 527static u8 nfc_llcp_dsap(struct sk_buff *pdu)
528{
529 return (pdu->data[0] & 0xfc) >> 2;
530}
531
532static u8 nfc_llcp_ptype(struct sk_buff *pdu)
533{
534 return ((pdu->data[0] & 0x03) << 2) | ((pdu->data[1] & 0xc0) >> 6);
535}
536
537static u8 nfc_llcp_ssap(struct sk_buff *pdu)
538{
539 return pdu->data[1] & 0x3f;
540}
541
542static u8 nfc_llcp_ns(struct sk_buff *pdu)
543{
544 return pdu->data[2] >> 4;
545}
546
547static u8 nfc_llcp_nr(struct sk_buff *pdu)
548{
549 return pdu->data[2] & 0xf;
550}
551
552static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu)
553{
279cf174 554 pdu->data[2] = (sock->send_n << 4) | (sock->recv_n);
d646960f
SO		 555 sock->send_n = (sock->send_n + 1) % 16;
556 sock->recv_ack_n = (sock->recv_n - 1) % 16;
557}
558
84457960
SO		 559static void nfc_llcp_tx_work(struct work_struct *work)
560{
561 struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
562 tx_work);
563 struct sk_buff *skb;
564 struct sock *sk;
565 struct nfc_llcp_sock *llcp_sock;
566
567 skb = skb_dequeue(&local->tx_queue);
568 if (skb != NULL) {
569 sk = skb->sk;
570 llcp_sock = nfc_llcp_sock(sk);
571 if (llcp_sock != NULL) {
572 int ret;
573
574 pr_debug("Sending pending skb\n");
575 print_hex_dump(KERN_DEBUG, "LLCP Tx: ",
576 DUMP_PREFIX_OFFSET, 16, 1,
577 skb->data, skb->len, true);
578
579 ret = nfc_data_exchange(local->dev, local->target_idx,
580 skb, nfc_llcp_recv, local);
581
582 if (!ret && nfc_llcp_ptype(skb) == LLCP_PDU_I) {
583 skb = skb_get(skb);
584 skb_queue_tail(&llcp_sock->tx_pending_queue,
585 skb);
586 }
587 } else {
588 nfc_llcp_send_symm(local->dev);
589 }
590 } else {
591 nfc_llcp_send_symm(local->dev);
592 }
593
594 mod_timer(&local->link_timer,
595 jiffies + msecs_to_jiffies(2 * local->remote_lto));
596}
597
a69f32af
SO		 598static struct nfc_llcp_sock *nfc_llcp_connecting_sock_get(struct nfc_llcp_local *local,
599 u8 ssap)
600{
601 struct sock *sk;
602 struct nfc_llcp_sock *llcp_sock;
603 struct hlist_node *node;
604
605 read_lock(&local->connecting_sockets.lock);
606
607 sk_for_each(sk, node, &local->connecting_sockets.head) {
608 llcp_sock = nfc_llcp_sock(sk);
609
5a0f6f3b
SO		 610 if (llcp_sock->ssap == ssap) {
611 sock_hold(&llcp_sock->sk);
a69f32af 612 goto out;
5a0f6f3b 613 }
a69f32af
SO		 614 }
615
616 llcp_sock = NULL;
617
618out:
619 read_unlock(&local->connecting_sockets.lock);
620
a69f32af
SO		 621 return llcp_sock;
622}
623
a69f32af
SO		 624static struct nfc_llcp_sock *nfc_llcp_sock_get_sn(struct nfc_llcp_local *local,
625 u8 *sn, size_t sn_len)
626{
a69f32af
SO		 627 struct nfc_llcp_sock *llcp_sock;
628
8f50020e 629 llcp_sock = nfc_llcp_sock_from_sn(local, sn, sn_len);
d646960f 630
a69f32af
SO		 631 if (llcp_sock == NULL)
632 return NULL;
d646960f 633
a69f32af
SO		 634 sock_hold(&llcp_sock->sk);
635
636 return llcp_sock;
d646960f
SO		 637}
638
d646960f
SO		 639static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len)
640{
641 u8 *tlv = &skb->data[2], type, length;
642 size_t tlv_array_len = skb->len - LLCP_HEADER_SIZE, offset = 0;
643
644 while (offset < tlv_array_len) {
645 type = tlv[0];
646 length = tlv[1];
647
648 pr_debug("type 0x%x length %d\n", type, length);
649
650 if (type == LLCP_TLV_SN) {
651 *sn_len = length;
652 return &tlv[2];
653 }
654
655 offset += length + 2;
656 tlv += length + 2;
657 }
658
659 return NULL;
660}
661
662static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
427a2eb1 663 struct sk_buff *skb)
d646960f
SO		 664{
665 struct sock *new_sk, *parent;
666 struct nfc_llcp_sock *sock, *new_sock;
a69f32af 667 u8 dsap, ssap, reason;
d646960f
SO		 668
669 dsap = nfc_llcp_dsap(skb);
670 ssap = nfc_llcp_ssap(skb);
671
672 pr_debug("%d %d\n", dsap, ssap);
673
d646960f 674 if (dsap != LLCP_SAP_SDP) {
a69f32af
SO		 675 sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
676 if (sock == NULL || sock->sk.sk_state != LLCP_LISTEN) {
d646960f
SO		 677 reason = LLCP_DM_NOBOUND;
678 goto fail;
679 }
d646960f
SO		 680 } else {
681 u8 *sn;
682 size_t sn_len;
683
684 sn = nfc_llcp_connect_sn(skb, &sn_len);
685 if (sn == NULL) {
686 reason = LLCP_DM_NOBOUND;
687 goto fail;
688 }
689
690 pr_debug("Service name length %zu\n", sn_len);
691
a69f32af
SO		 692 sock = nfc_llcp_sock_get_sn(local, sn, sn_len);
693 if (sock == NULL) {
694 reason = LLCP_DM_NOBOUND;
695 goto fail;
d646960f 696 }
d646960f
SO		 697 }
698
a69f32af 699 lock_sock(&sock->sk);
d646960f 700
d646960f
SO		 701 parent = &sock->sk;
702
703 if (sk_acceptq_is_full(parent)) {
704 reason = LLCP_DM_REJ;
705 release_sock(&sock->sk);
706 sock_put(&sock->sk);
707 goto fail;
708 }
709
8f50020e
SO		 710 if (sock->ssap == LLCP_SDP_UNBOUND) {
711 u8 ssap = nfc_llcp_reserve_sdp_ssap(local);
712
713 pr_debug("First client, reserving %d\n", ssap);
714
715 if (ssap == LLCP_SAP_MAX) {
716 reason = LLCP_DM_REJ;
717 release_sock(&sock->sk);
718 sock_put(&sock->sk);
719 goto fail;
720 }
721
722 sock->ssap = ssap;
723 }
724
427a2eb1 725 new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, GFP_ATOMIC);
d646960f
SO		 726 if (new_sk == NULL) {
727 reason = LLCP_DM_REJ;
728 release_sock(&sock->sk);
729 sock_put(&sock->sk);
730 goto fail;
731 }
732
733 new_sock = nfc_llcp_sock(new_sk);
734 new_sock->dev = local->dev;
c7aa1225 735 new_sock->local = nfc_llcp_local_get(local);
93d7e490 736 new_sock->miu = local->remote_miu;
d646960f 737 new_sock->nfc_protocol = sock->nfc_protocol;
d646960f 738 new_sock->dsap = ssap;
025f1520 739 new_sock->target_idx = local->target_idx;
d646960f 740 new_sock->parent = parent;
8f50020e
SO		 741 new_sock->ssap = sock->ssap;
742 if (sock->ssap < LLCP_LOCAL_NUM_SAP && sock->ssap >= LLCP_WKS_NUM_SAP) {
743 atomic_t *client_count;
744
745 pr_debug("reserved_ssap %d for %p\n", sock->ssap, new_sock);
746
747 client_count =
748 &local->local_sdp_cnt[sock->ssap - LLCP_WKS_NUM_SAP];
749
750 atomic_inc(client_count);
751 new_sock->reserved_ssap = sock->ssap;
752 }
d646960f 753
7a06e586
SO		 754 nfc_llcp_parse_connection_tlv(new_sock, &skb->data[LLCP_HEADER_SIZE],
755 skb->len - LLCP_HEADER_SIZE);
756
d646960f
SO		 757 pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk);
758
a69f32af 759 nfc_llcp_sock_link(&local->sockets, new_sk);
d646960f
SO		 760
761 nfc_llcp_accept_enqueue(&sock->sk, new_sk);
762
763 nfc_get_device(local->dev->idx);
764
765 new_sk->sk_state = LLCP_CONNECTED;
766
767 /* Wake the listening processes */
768 parent->sk_data_ready(parent, 0);
769
770 /* Send CC */
771 nfc_llcp_send_cc(new_sock);
772
773 release_sock(&sock->sk);
774 sock_put(&sock->sk);
775
776 return;
777
778fail:
779 /* Send DM */
780 nfc_llcp_send_dm(local, dsap, ssap, reason);
781
782 return;
783
784}
785
d094afa1 786int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock)
4722d2b7 787{
d094afa1 788 int nr_frames = 0;
4722d2b7
SO		 789 struct nfc_llcp_local *local = sock->local;
790
791 pr_debug("Remote ready %d tx queue len %d remote rw %d",
427a2eb1 792 sock->remote_ready, skb_queue_len(&sock->tx_pending_queue),
7a06e586 793 sock->rw);
4722d2b7
SO		 794
795 /* Try to queue some I frames for transmission */
796 while (sock->remote_ready &&
7a06e586 797 skb_queue_len(&sock->tx_pending_queue) < sock->rw) {
84457960 798 struct sk_buff *pdu;
4722d2b7
SO		 799
800 pdu = skb_dequeue(&sock->tx_queue);
801 if (pdu == NULL)
802 break;
803
804 /* Update N(S)/N(R) */
805 nfc_llcp_set_nrns(sock, pdu);
806
4722d2b7 807 skb_queue_tail(&local->tx_queue, pdu);
d094afa1 808 nr_frames++;
4722d2b7 809 }
d094afa1
SO		 810
811 return nr_frames;
4722d2b7
SO		 812}
813
d646960f 814static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
427a2eb1 815 struct sk_buff *skb)
d646960f
SO		 816{
817 struct nfc_llcp_sock *llcp_sock;
818 struct sock *sk;
819 u8 dsap, ssap, ptype, ns, nr;
820
821 ptype = nfc_llcp_ptype(skb);
822 dsap = nfc_llcp_dsap(skb);
823 ssap = nfc_llcp_ssap(skb);
824 ns = nfc_llcp_ns(skb);
825 nr = nfc_llcp_nr(skb);
826
827 pr_debug("%d %d R %d S %d\n", dsap, ssap, nr, ns);
828
829 llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
830 if (llcp_sock == NULL) {
831 nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
832 return;
833 }
834
835 sk = &llcp_sock->sk;
836 lock_sock(sk);
837 if (sk->sk_state == LLCP_CLOSED) {
838 release_sock(sk);
839 nfc_llcp_sock_put(llcp_sock);
840 }
841
d646960f
SO		 842 /* Pass the payload upstream */
843 if (ptype == LLCP_PDU_I) {
844 pr_debug("I frame, queueing on %p\n", &llcp_sock->sk);
845
53aef920
SO		 846 if (ns == llcp_sock->recv_n)
847 llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16;
848 else
849 pr_err("Received out of sequence I PDU\n");
850
d646960f
SO		 851 skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE);
852 if (sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
853 pr_err("receive queue is full\n");
854 skb_queue_head(&llcp_sock->tx_backlog_queue, skb);
855 }
856 }
857
858 /* Remove skbs from the pending queue */
859 if (llcp_sock->send_ack_n != nr) {
860 struct sk_buff *s, *tmp;
861
862 llcp_sock->send_ack_n = nr;
863
84457960
SO		 864 /* Remove and free all skbs until ns == nr */
865 skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp) {
866 skb_unlink(s, &llcp_sock->tx_pending_queue);
867 kfree_skb(s);
868
869 if (nfc_llcp_ns(s) == nr)
870 break;
871 }
872
873 /* Re-queue the remaining skbs for transmission */
874 skb_queue_reverse_walk_safe(&llcp_sock->tx_pending_queue,
875 s, tmp) {
876 skb_unlink(s, &llcp_sock->tx_pending_queue);
877 skb_queue_head(&local->tx_queue, s);
878 }
d646960f
SO		 879 }
880
53aef920
SO		 881 if (ptype == LLCP_PDU_RR)
882 llcp_sock->remote_ready = true;
427a2eb1 883 else if (ptype == LLCP_PDU_RNR)
53aef920
SO		 884 llcp_sock->remote_ready = false;
885
56af2568 886 if (nfc_llcp_queue_i_frames(llcp_sock) == 0 && ptype == LLCP_PDU_I)
d094afa1 887 nfc_llcp_send_rr(llcp_sock);
d646960f
SO		 888
889 release_sock(sk);
890 nfc_llcp_sock_put(llcp_sock);
891}
892
893static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
427a2eb1 894 struct sk_buff *skb)
d646960f
SO		 895{
896 struct nfc_llcp_sock *llcp_sock;
897 struct sock *sk;
898 u8 dsap, ssap;
899
900 dsap = nfc_llcp_dsap(skb);
901 ssap = nfc_llcp_ssap(skb);
902
903 llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
904 if (llcp_sock == NULL) {
905 nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
906 return;
907 }
908
909 sk = &llcp_sock->sk;
910 lock_sock(sk);
911 if (sk->sk_state == LLCP_CLOSED) {
912 release_sock(sk);
913 nfc_llcp_sock_put(llcp_sock);
914 }
915
d646960f
SO		 916 if (sk->sk_state == LLCP_CONNECTED) {
917 nfc_put_device(local->dev);
918 sk->sk_state = LLCP_CLOSED;
919 sk->sk_state_change(sk);
920 }
921
922 nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_DISC);
923
924 release_sock(sk);
925 nfc_llcp_sock_put(llcp_sock);
926}
927
427a2eb1 928static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb)
d646960f
SO		 929{
930 struct nfc_llcp_sock *llcp_sock;
ff353d86 931 struct sock *sk;
d646960f
SO		 932 u8 dsap, ssap;
933
d646960f
SO		 934 dsap = nfc_llcp_dsap(skb);
935 ssap = nfc_llcp_ssap(skb);
936
a69f32af 937 llcp_sock = nfc_llcp_connecting_sock_get(local, dsap);
d646960f
SO		 938 if (llcp_sock == NULL) {
939 pr_err("Invalid CC\n");
940 nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
941
942 return;
943 }
944
ff353d86 945 sk = &llcp_sock->sk;
d646960f 946
a69f32af
SO		 947 /* Unlink from connecting and link to the client array */
948 nfc_llcp_sock_unlink(&local->connecting_sockets, sk);
949 nfc_llcp_sock_link(&local->sockets, sk);
950 llcp_sock->dsap = ssap;
951
7a06e586
SO		 952 nfc_llcp_parse_connection_tlv(llcp_sock, &skb->data[LLCP_HEADER_SIZE],
953 skb->len - LLCP_HEADER_SIZE);
d646960f 954
ff353d86
SO		 955 sk->sk_state = LLCP_CONNECTED;
956 sk->sk_state_change(sk);
957
d646960f
SO		 958 nfc_llcp_sock_put(llcp_sock);
959}
960
961static void nfc_llcp_rx_work(struct work_struct *work)
962{
963 struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
427a2eb1 964 rx_work);
d646960f
SO		 965 u8 dsap, ssap, ptype;
966 struct sk_buff *skb;
967
968 skb = local->rx_pending;
969 if (skb == NULL) {
970 pr_debug("No pending SKB\n");
971 return;
972 }
973
974 ptype = nfc_llcp_ptype(skb);
975 dsap = nfc_llcp_dsap(skb);
976 ssap = nfc_llcp_ssap(skb);
977
978 pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap);
979
4be646ec
SO		 980 if (ptype != LLCP_PDU_SYMM)
981 print_hex_dump(KERN_DEBUG, "LLCP Rx: ", DUMP_PREFIX_OFFSET,
982 16, 1, skb->data, skb->len, true);
983
d646960f
SO		 984 switch (ptype) {
985 case LLCP_PDU_SYMM:
986 pr_debug("SYMM\n");
987 break;
988
989 case LLCP_PDU_CONNECT:
990 pr_debug("CONNECT\n");
991 nfc_llcp_recv_connect(local, skb);
992 break;
993
994 case LLCP_PDU_DISC:
995 pr_debug("DISC\n");
996 nfc_llcp_recv_disc(local, skb);
997 break;
998
999 case LLCP_PDU_CC:
1000 pr_debug("CC\n");
1001 nfc_llcp_recv_cc(local, skb);
1002 break;
1003
1004 case LLCP_PDU_I:
1005 case LLCP_PDU_RR:
53aef920 1006 case LLCP_PDU_RNR:
d646960f
SO		 1007 pr_debug("I frame\n");
1008 nfc_llcp_recv_hdlc(local, skb);
1009 break;
1010
1011 }
1012
1013 queue_work(local->tx_wq, &local->tx_work);
1014 kfree_skb(local->rx_pending);
1015 local->rx_pending = NULL;
1016
1017 return;
1018}
1019
1020void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
1021{
1022 struct nfc_llcp_local *local = (struct nfc_llcp_local *) data;
1023
1024 pr_debug("Received an LLCP PDU\n");
1025 if (err < 0) {
427a2eb1 1026 pr_err("err %d\n", err);
d646960f
SO		 1027 return;
1028 }
1029
1030 local->rx_pending = skb_get(skb);
1031 del_timer(&local->link_timer);
1032 queue_work(local->rx_wq, &local->rx_work);
1033
1034 return;
1035}
1036
73167ced
SO		 1037int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb)
1038{
1039 struct nfc_llcp_local *local;
1040
1041 local = nfc_llcp_find_local(dev);
1042 if (local == NULL)
1043 return -ENODEV;
1044
1045 local->rx_pending = skb_get(skb);
1046 del_timer(&local->link_timer);
1047 queue_work(local->rx_wq, &local->rx_work);
1048
1049 return 0;
1050}
1051
d646960f
SO		 1052void nfc_llcp_mac_is_down(struct nfc_dev *dev)
1053{
1054 struct nfc_llcp_local *local;
1055
1056 local = nfc_llcp_find_local(dev);
1057 if (local == NULL)
1058 return;
1059
1060 /* Close and purge all existing sockets */
4d22ea15 1061 nfc_llcp_socket_release(local, true);
d646960f
SO		 1062}
1063
1064void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
1065 u8 comm_mode, u8 rf_mode)
1066{
1067 struct nfc_llcp_local *local;
1068
1069 pr_debug("rf mode %d\n", rf_mode);
1070
1071 local = nfc_llcp_find_local(dev);
1072 if (local == NULL)
1073 return;
1074
1075 local->target_idx = target_idx;
1076 local->comm_mode = comm_mode;
1077 local->rf_mode = rf_mode;
1078
1079 if (rf_mode == NFC_RF_INITIATOR) {
1080 pr_debug("Queueing Tx work\n");
1081
1082 queue_work(local->tx_wq, &local->tx_work);
1083 } else {
1084 mod_timer(&local->link_timer,
427a2eb1 1085 jiffies + msecs_to_jiffies(local->remote_lto));
d646960f
SO		 1086 }
1087}
1088
1089int nfc_llcp_register_device(struct nfc_dev *ndev)
1090{
1091 struct device *dev = &ndev->dev;
1092 struct nfc_llcp_local *local;
1093 char name[32];
1094 int err;
1095
1096 local = kzalloc(sizeof(struct nfc_llcp_local), GFP_KERNEL);
1097 if (local == NULL)
1098 return -ENOMEM;
1099
1100 local->dev = ndev;
1101 INIT_LIST_HEAD(&local->list);
c7aa1225 1102 kref_init(&local->ref);
d646960f 1103 mutex_init(&local->sdp_lock);
d646960f
SO		 1104 init_timer(&local->link_timer);
1105 local->link_timer.data = (unsigned long) local;
1106 local->link_timer.function = nfc_llcp_symm_timer;
1107
1108 skb_queue_head_init(&local->tx_queue);
1109 INIT_WORK(&local->tx_work, nfc_llcp_tx_work);
1110 snprintf(name, sizeof(name), "%s_llcp_tx_wq", dev_name(dev));
427a2eb1
SO		 1111 local->tx_wq =
1112 alloc_workqueue(name,
1113 WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
1114 1);
d646960f
SO		 1115 if (local->tx_wq == NULL) {
1116 err = -ENOMEM;
1117 goto err_local;
1118 }
1119
1120 local->rx_pending = NULL;
1121 INIT_WORK(&local->rx_work, nfc_llcp_rx_work);
1122 snprintf(name, sizeof(name), "%s_llcp_rx_wq", dev_name(dev));
427a2eb1
SO		 1123 local->rx_wq =
1124 alloc_workqueue(name,
1125 WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
1126 1);
d646960f
SO		 1127 if (local->rx_wq == NULL) {
1128 err = -ENOMEM;
1129 goto err_tx_wq;
1130 }
1131
1132 INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work);
1133 snprintf(name, sizeof(name), "%s_llcp_timeout_wq", dev_name(dev));
427a2eb1
SO		 1134 local->timeout_wq =
1135 alloc_workqueue(name,
1136 WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
1137 1);
d646960f
SO		 1138 if (local->timeout_wq == NULL) {
1139 err = -ENOMEM;
1140 goto err_rx_wq;
1141 }
1142
a69f32af
SO		 1143 local->sockets.lock = __RW_LOCK_UNLOCKED(local->sockets.lock);
1144 local->connecting_sockets.lock = __RW_LOCK_UNLOCKED(local->connecting_sockets.lock);
1145
d646960f
SO		 1146 nfc_llcp_build_gb(local);
1147
1148 local->remote_miu = LLCP_DEFAULT_MIU;
1149 local->remote_lto = LLCP_DEFAULT_LTO;
d646960f
SO		 1150
1151 list_add(&llcp_devices, &local->list);
1152
1153 return 0;
1154
1155err_rx_wq:
1156 destroy_workqueue(local->rx_wq);
1157
1158err_tx_wq:
1159 destroy_workqueue(local->tx_wq);
1160
1161err_local:
1162 kfree(local);
1163
1164 return 0;
1165}
1166
1167void nfc_llcp_unregister_device(struct nfc_dev *dev)
1168{
1169 struct nfc_llcp_local *local = nfc_llcp_find_local(dev);
1170
1171 if (local == NULL) {
1172 pr_debug("No such device\n");
1173 return;
1174 }
1175
c7aa1225 1176 nfc_llcp_local_put(local);
d646960f
SO		 1177}
1178
1179int __init nfc_llcp_init(void)
1180{
1181 INIT_LIST_HEAD(&llcp_devices);
1182
1183 return nfc_llcp_sock_init();
1184}
1185
1186void nfc_llcp_exit(void)
1187{
1188 nfc_llcp_sock_exit();
1189}
Linux 6.x block layer and io_uring tree(s)