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