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