Linux Kernel  3.7.1
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Groups Pages
llcp.c
Go to the documentation of this file.
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 
30 static u8 llcp_magic[3] = {0x46, 0x66, 0x6d};
31 
32 static struct list_head llcp_devices;
33 
34 void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *sk)
35 {
36  write_lock(&l->lock);
37  sk_add_node(sk, &l->head);
38  write_unlock(&l->lock);
39 }
40 
42 {
43  write_lock(&l->lock);
44  sk_del_node_init(sk);
45  write_unlock(&l->lock);
46 }
47 
48 static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen)
49 {
50  struct sock *sk;
51  struct hlist_node *node, *tmp;
52  struct nfc_llcp_sock *llcp_sock;
53 
54  write_lock(&local->sockets.lock);
55 
56  sk_for_each_safe(sk, node, tmp, &local->sockets.head) {
57  llcp_sock = nfc_llcp_sock(sk);
58 
59  bh_lock_sock(sk);
60 
61  if (sk->sk_state == LLCP_CONNECTED)
62  nfc_put_device(llcp_sock->dev);
63 
64  if (sk->sk_state == LLCP_LISTEN) {
65  struct nfc_llcp_sock *lsk, *n;
66  struct sock *accept_sk;
67 
68  list_for_each_entry_safe(lsk, n, &llcp_sock->accept_queue,
69  accept_queue) {
70  accept_sk = &lsk->sk;
71  bh_lock_sock(accept_sk);
72 
73  nfc_llcp_accept_unlink(accept_sk);
74 
75  accept_sk->sk_state = LLCP_CLOSED;
76 
77  bh_unlock_sock(accept_sk);
78 
79  sock_orphan(accept_sk);
80  }
81 
82  if (listen == true) {
83  bh_unlock_sock(sk);
84  continue;
85  }
86  }
87 
88  sk->sk_state = LLCP_CLOSED;
89 
90  bh_unlock_sock(sk);
91 
92  sock_orphan(sk);
93 
94  sk_del_node_init(sk);
95  }
96 
97  write_unlock(&local->sockets.lock);
98 }
99 
101 {
102  kref_get(&local->ref);
103 
104  return local;
105 }
106 
107 static 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);
114  nfc_llcp_socket_release(local, false);
115  del_timer_sync(&local->link_timer);
116  skb_queue_purge(&local->tx_queue);
117  cancel_work_sync(&local->tx_work);
118  cancel_work_sync(&local->rx_work);
120  kfree_skb(local->rx_pending);
121  kfree(local);
122 }
123 
125 {
126  if (local == NULL)
127  return 0;
128 
129  return kref_put(&local->ref, local_release);
130 }
131 
132 static 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 
165 static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock)
166 {
167  sock_put(&sock->sk);
168 }
169 
170 static void nfc_llcp_timeout_work(struct work_struct *work)
171 {
172  struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
173  timeout_work);
174 
175  nfc_dep_link_down(local->dev);
176 }
177 
178 static 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 
184  schedule_work(&local->timeout_work);
185 }
186 
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 
200 static char *wks[] = {
201  NULL,
202  NULL, /* SDP */
203  "urn:nfc:sn:ip",
204  "urn:nfc:sn:obex",
205  "urn:nfc:sn:snep",
206 };
207 
208 static 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 
219  for (sap = 0; sap < num_wks; sap++) {
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 
230 static
231 struct 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 
276  struct nfc_llcp_sock *sock)
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,
282  sock->service_name_len);
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 
294  set_bit(ssap, &local->local_wks);
295  mutex_unlock(&local->sdp_lock);
296 
297  return ssap;
298  }
299 
300  /*
301  * Check if there already is a non WKS socket bound
302  * to this service name.
303  */
304  if (nfc_llcp_sock_from_sn(local, sock->service_name,
305  sock->service_name_len) != NULL) {
306  mutex_unlock(&local->sdp_lock);
307 
308  return LLCP_SAP_MAX;
309  }
310 
311  mutex_unlock(&local->sdp_lock);
312 
313  return LLCP_SDP_UNBOUND;
314 
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);
319 
320  return sock->ssap;
321  }
322  }
323 
324  mutex_unlock(&local->sdp_lock);
325 
326  return LLCP_SAP_MAX;
327 }
328 
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 
341  set_bit(local_ssap, &local->local_sap);
342 
343  mutex_unlock(&local->sdp_lock);
344 
345  return local_ssap + LLCP_LOCAL_SAP_OFFSET;
346 }
347 
348 void 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) {
357  atomic_t *client_cnt;
358 
359  local_ssap = ssap - LLCP_WKS_NUM_SAP;
360  sdp = &local->local_sdp;
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;
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 
394  clear_bit(local_ssap, sdp);
395 
396  mutex_unlock(&local->sdp_lock);
397 }
398 
399 static u8 nfc_llcp_reserve_sdp_ssap(struct nfc_llcp_local *local)
400 {
401  u8 ssap;
402 
403  mutex_lock(&local->sdp_lock);
404 
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 
421 static 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;
426  u8 *miux_tlv, miux_length;
427  __be16 miux;
428  u8 gb_len = 0;
429  int ret = 0;
430 
431  version = LLCP_VERSION_11;
432  version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version,
433  1, &version_length);
434  gb_len += version_length;
435 
436  /* 1500 ms */
437  lto = 150;
438  lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_LTO, &lto, 1, &lto_length);
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,
443  &wks_length);
444  gb_len += wks_length;
445 
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 
451  gb_len += ARRAY_SIZE(llcp_magic);
452 
453  if (gb_len > NFC_MAX_GT_LEN) {
454  ret = -EINVAL;
455  goto out;
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 
472  memcpy(gb_cur, miux_tlv, miux_length);
473  gb_cur += miux_length;
474 
475  local->gb_len = gb_len;
476 
477 out:
478  kfree(version_tlv);
479  kfree(lto_tlv);
480  kfree(wks_tlv);
481  kfree(miux_tlv);
482 
483  return ret;
484 }
485 
486 u8 *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 
503 int 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 
516  if (local->remote_gb == NULL || local->remote_gb_len == 0)
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 
524  return nfc_llcp_parse_gb_tlv(local,
525  &local->remote_gb[3],
526  local->remote_gb_len - 3);
527 }
528 
529 static u8 nfc_llcp_dsap(struct sk_buff *pdu)
530 {
531  return (pdu->data[0] & 0xfc) >> 2;
532 }
533 
534 static u8 nfc_llcp_ptype(struct sk_buff *pdu)
535 {
536  return ((pdu->data[0] & 0x03) << 2) | ((pdu->data[1] & 0xc0) >> 6);
537 }
538 
539 static u8 nfc_llcp_ssap(struct sk_buff *pdu)
540 {
541  return pdu->data[1] & 0x3f;
542 }
543 
544 static u8 nfc_llcp_ns(struct sk_buff *pdu)
545 {
546  return pdu->data[2] >> 4;
547 }
548 
549 static u8 nfc_llcp_nr(struct sk_buff *pdu)
550 {
551  return pdu->data[2] & 0xf;
552 }
553 
554 static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu)
555 {
556  pdu->data[2] = (sock->send_n << 4) | (sock->recv_n);
557  sock->send_n = (sock->send_n + 1) % 16;
558  sock->recv_ack_n = (sock->recv_n - 1) % 16;
559 }
560 
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 
601 static 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 
621  nfc_llcp_send_to_raw_sock(local, skb,
623 
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 
643 static 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 
655  if (llcp_sock->ssap == ssap) {
656  sock_hold(&llcp_sock->sk);
657  goto out;
658  }
659  }
660 
661  llcp_sock = NULL;
662 
663 out:
664  read_unlock(&local->connecting_sockets.lock);
665 
666  return llcp_sock;
667 }
668 
669 static struct nfc_llcp_sock *nfc_llcp_sock_get_sn(struct nfc_llcp_local *local,
670  u8 *sn, size_t sn_len)
671 {
672  struct nfc_llcp_sock *llcp_sock;
673 
674  llcp_sock = nfc_llcp_sock_from_sn(local, sn, sn_len);
675 
676  if (llcp_sock == NULL)
677  return NULL;
678 
679  sock_hold(&llcp_sock->sk);
680 
681  return llcp_sock;
682 }
683 
684 static 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 
707 static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
708  struct sk_buff *skb)
709 {
710  struct sock *new_sk, *parent;
711  struct nfc_llcp_sock *sock, *new_sock;
712  u8 dsap, ssap, reason;
713 
714  dsap = nfc_llcp_dsap(skb);
715  ssap = nfc_llcp_ssap(skb);
716 
717  pr_debug("%d %d\n", dsap, ssap);
718 
719  if (dsap != LLCP_SAP_SDP) {
720  sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
721  if (sock == NULL || sock->sk.sk_state != LLCP_LISTEN) {
722  reason = LLCP_DM_NOBOUND;
723  goto fail;
724  }
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 
737  sock = nfc_llcp_sock_get_sn(local, sn, sn_len);
738  if (sock == NULL) {
739  reason = LLCP_DM_NOBOUND;
740  goto fail;
741  }
742  }
743 
744  lock_sock(&sock->sk);
745 
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 
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 
770  new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, GFP_ATOMIC);
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;
780  new_sock->local = nfc_llcp_local_get(local);
781  new_sock->miu = local->remote_miu;
782  new_sock->nfc_protocol = sock->nfc_protocol;
783  new_sock->dsap = ssap;
784  new_sock->target_idx = local->target_idx;
785  new_sock->parent = parent;
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  }
798 
800  skb->len - LLCP_HEADER_SIZE);
801 
802  pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk);
803 
804  nfc_llcp_sock_link(&local->sockets, new_sk);
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 
823 fail:
824  /* Send DM */
825  nfc_llcp_send_dm(local, dsap, ssap, reason);
826 
827  return;
828 
829 }
830 
832 {
833  int nr_frames = 0;
834  struct nfc_llcp_local *local = sock->local;
835 
836  pr_debug("Remote ready %d tx queue len %d remote rw %d",
837  sock->remote_ready, skb_queue_len(&sock->tx_pending_queue),
838  sock->rw);
839 
840  /* Try to queue some I frames for transmission */
841  while (sock->remote_ready &&
842  skb_queue_len(&sock->tx_pending_queue) < sock->rw) {
843  struct sk_buff *pdu;
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 
852  skb_queue_tail(&local->tx_queue, pdu);
853  nr_frames++;
854  }
855 
856  return nr_frames;
857 }
858 
859 static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
860  struct sk_buff *skb)
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 
887  /* Pass the payload upstream */
888  if (ptype == LLCP_PDU_I) {
889  pr_debug("I frame, queueing on %p\n", &llcp_sock->sk);
890 
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 
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 
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  }
924  }
925 
926  if (ptype == LLCP_PDU_RR)
927  llcp_sock->remote_ready = true;
928  else if (ptype == LLCP_PDU_RNR)
929  llcp_sock->remote_ready = false;
930 
931  if (nfc_llcp_queue_i_frames(llcp_sock) == 0 && ptype == LLCP_PDU_I)
932  nfc_llcp_send_rr(llcp_sock);
933 
934  release_sock(sk);
935  nfc_llcp_sock_put(llcp_sock);
936 }
937 
938 static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
939  struct sk_buff *skb)
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 
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 
973 static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb)
974 {
975  struct nfc_llcp_sock *llcp_sock;
976  struct sock *sk;
977  u8 dsap, ssap;
978 
979  dsap = nfc_llcp_dsap(skb);
980  ssap = nfc_llcp_ssap(skb);
981 
982  llcp_sock = nfc_llcp_connecting_sock_get(local, dsap);
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 
990  sk = &llcp_sock->sk;
991 
992  /* Unlink from connecting and link to the client array */
994  nfc_llcp_sock_link(&local->sockets, sk);
995  llcp_sock->dsap = ssap;
996 
998  skb->len - LLCP_HEADER_SIZE);
999 
1000  sk->sk_state = LLCP_CONNECTED;
1001  sk->sk_state_change(sk);
1002 
1003  nfc_llcp_sock_put(llcp_sock);
1004 }
1005 
1006 static 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 
1045 static void nfc_llcp_rx_work(struct work_struct *work)
1046 {
1047  struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
1048  rx_work);
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 
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 
1069 
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 
1090  case LLCP_PDU_DM:
1091  pr_debug("DM\n");
1092  nfc_llcp_recv_dm(local, skb);
1093  break;
1094 
1095  case LLCP_PDU_I:
1096  case LLCP_PDU_RR:
1097  case LLCP_PDU_RNR:
1098  pr_debug("I frame\n");
1099  nfc_llcp_recv_hdlc(local, skb);
1100  break;
1101 
1102  }
1103 
1104  schedule_work(&local->tx_work);
1105  kfree_skb(local->rx_pending);
1106  local->rx_pending = NULL;
1107 
1108  return;
1109 }
1110 
1111 void 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) {
1117  pr_err("err %d\n", err);
1118  return;
1119  }
1120 
1121  local->rx_pending = skb_get(skb);
1122  del_timer(&local->link_timer);
1123  schedule_work(&local->rx_work);
1124 
1125  return;
1126 }
1127 
1128 int 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);
1138  schedule_work(&local->rx_work);
1139 
1140  return 0;
1141 }
1142 
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 */
1152  nfc_llcp_socket_release(local, true);
1153 }
1154 
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 
1173  schedule_work(&local->tx_work);
1174  } else {
1175  mod_timer(&local->link_timer,
1176  jiffies + msecs_to_jiffies(local->remote_lto));
1177  }
1178 }
1179 
1181 {
1182  struct nfc_llcp_local *local;
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);
1190  kref_init(&local->ref);
1191  mutex_init(&local->sdp_lock);
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);
1198 
1199  local->rx_pending = NULL;
1200  INIT_WORK(&local->rx_work, nfc_llcp_rx_work);
1201 
1202  INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work);
1203 
1204  rwlock_init(&local->sockets.lock);
1205  rwlock_init(&local->connecting_sockets.lock);
1206  rwlock_init(&local->raw_sockets.lock);
1207 
1208  nfc_llcp_build_gb(local);
1209 
1210  local->remote_miu = LLCP_DEFAULT_MIU;
1211  local->remote_lto = LLCP_DEFAULT_LTO;
1212 
1213  list_add(&local->list, &llcp_devices);
1214 
1215  return 0;
1216 }
1217 
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 
1227  nfc_llcp_local_put(local);
1228 }
1229 
1231 {
1232  INIT_LIST_HEAD(&llcp_devices);
1233 
1234  return nfc_llcp_sock_init();
1235 }
1236 
1237 void nfc_llcp_exit(void)
1238 {
1240 }