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