Commit | Line | Data |
---|---|---|
d646960f SO |
1 | /* |
2 | * Copyright (C) 2011 Intel Corporation. All rights reserved. | |
3 | * | |
4 | * This program is free software; you can redistribute it and/or modify | |
5 | * it under the terms of the GNU General Public License as published by | |
6 | * the Free Software Foundation; either version 2 of the License, or | |
7 | * (at your option) any later version. | |
8 | * | |
9 | * This program is distributed in the hope that it will be useful, | |
10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of | |
11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
12 | * GNU General Public License for more details. | |
13 | * | |
14 | * You should have received a copy of the GNU General Public License | |
15 | * along with this program; if not, write to the | |
16 | * Free Software Foundation, Inc., | |
17 | * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. | |
18 | */ | |
19 | ||
20 | #define pr_fmt(fmt) "llcp: %s: " fmt, __func__ | |
21 | ||
22 | #include <linux/init.h> | |
23 | #include <linux/kernel.h> | |
24 | #include <linux/list.h> | |
25 | #include <linux/nfc.h> | |
26 | ||
27 | #include "../nfc.h" | |
28 | #include "llcp.h" | |
29 | ||
30 | static u8 llcp_magic[3] = {0x46, 0x66, 0x6d}; | |
31 | ||
32 | static struct list_head llcp_devices; | |
33 | ||
34 | static void nfc_llcp_socket_release(struct nfc_llcp_local *local) | |
35 | { | |
36 | struct nfc_llcp_sock *parent, *s, *n; | |
37 | struct sock *sk, *parent_sk; | |
38 | int i; | |
39 | ||
40 | ||
41 | mutex_lock(&local->socket_lock); | |
42 | ||
43 | for (i = 0; i < LLCP_MAX_SAP; i++) { | |
44 | parent = local->sockets[i]; | |
45 | if (parent == NULL) | |
46 | continue; | |
47 | ||
48 | /* Release all child sockets */ | |
49 | list_for_each_entry_safe(s, n, &parent->list, list) { | |
50 | list_del(&s->list); | |
51 | sk = &s->sk; | |
52 | ||
53 | lock_sock(sk); | |
54 | ||
55 | if (sk->sk_state == LLCP_CONNECTED) | |
56 | nfc_put_device(s->dev); | |
57 | ||
58 | sk->sk_state = LLCP_CLOSED; | |
59 | sock_set_flag(sk, SOCK_DEAD); | |
60 | ||
61 | release_sock(sk); | |
62 | } | |
63 | ||
64 | parent_sk = &parent->sk; | |
65 | ||
66 | lock_sock(parent_sk); | |
67 | ||
68 | if (parent_sk->sk_state == LLCP_LISTEN) { | |
69 | struct nfc_llcp_sock *lsk, *n; | |
70 | struct sock *accept_sk; | |
71 | ||
72 | list_for_each_entry_safe(lsk, n, &parent->accept_queue, | |
73 | accept_queue) { | |
74 | accept_sk = &lsk->sk; | |
75 | lock_sock(accept_sk); | |
76 | ||
77 | nfc_llcp_accept_unlink(accept_sk); | |
78 | ||
79 | accept_sk->sk_state = LLCP_CLOSED; | |
80 | sock_set_flag(accept_sk, SOCK_DEAD); | |
81 | ||
82 | release_sock(accept_sk); | |
83 | ||
84 | sock_orphan(accept_sk); | |
85 | } | |
86 | } | |
87 | ||
88 | if (parent_sk->sk_state == LLCP_CONNECTED) | |
89 | nfc_put_device(parent->dev); | |
90 | ||
91 | parent_sk->sk_state = LLCP_CLOSED; | |
92 | sock_set_flag(parent_sk, SOCK_DEAD); | |
93 | ||
94 | release_sock(parent_sk); | |
95 | } | |
96 | ||
97 | mutex_unlock(&local->socket_lock); | |
98 | } | |
99 | ||
100 | static void nfc_llcp_timeout_work(struct work_struct *work) | |
101 | { | |
102 | struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, | |
103 | timeout_work); | |
104 | ||
105 | nfc_dep_link_down(local->dev); | |
106 | } | |
107 | ||
108 | static void nfc_llcp_symm_timer(unsigned long data) | |
109 | { | |
110 | struct nfc_llcp_local *local = (struct nfc_llcp_local *) data; | |
111 | ||
112 | pr_err("SYMM timeout\n"); | |
113 | ||
114 | queue_work(local->timeout_wq, &local->timeout_work); | |
115 | } | |
116 | ||
117 | struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev) | |
118 | { | |
119 | struct nfc_llcp_local *local, *n; | |
120 | ||
121 | list_for_each_entry_safe(local, n, &llcp_devices, list) | |
122 | if (local->dev == dev) | |
123 | return local; | |
124 | ||
125 | pr_debug("No device found\n"); | |
126 | ||
127 | return NULL; | |
128 | } | |
129 | ||
130 | static char *wks[] = { | |
131 | NULL, | |
132 | NULL, /* SDP */ | |
133 | "urn:nfc:sn:ip", | |
134 | "urn:nfc:sn:obex", | |
135 | "urn:nfc:sn:snep", | |
136 | }; | |
137 | ||
138 | static int nfc_llcp_wks_sap(char *service_name, size_t service_name_len) | |
139 | { | |
140 | int sap, num_wks; | |
141 | ||
142 | pr_debug("%s\n", service_name); | |
143 | ||
144 | if (service_name == NULL) | |
145 | return -EINVAL; | |
146 | ||
147 | num_wks = ARRAY_SIZE(wks); | |
148 | ||
149 | for (sap = 0 ; sap < num_wks; sap++) { | |
150 | if (wks[sap] == NULL) | |
151 | continue; | |
152 | ||
153 | if (strncmp(wks[sap], service_name, service_name_len) == 0) | |
154 | return sap; | |
155 | } | |
156 | ||
157 | return -EINVAL; | |
158 | } | |
159 | ||
160 | u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local, | |
161 | struct nfc_llcp_sock *sock) | |
162 | { | |
163 | mutex_lock(&local->sdp_lock); | |
164 | ||
165 | if (sock->service_name != NULL && sock->service_name_len > 0) { | |
166 | int ssap = nfc_llcp_wks_sap(sock->service_name, | |
167 | sock->service_name_len); | |
168 | ||
169 | if (ssap > 0) { | |
170 | pr_debug("WKS %d\n", ssap); | |
171 | ||
172 | /* This is a WKS, let's check if it's free */ | |
173 | if (local->local_wks & BIT(ssap)) { | |
174 | mutex_unlock(&local->sdp_lock); | |
175 | ||
176 | return LLCP_SAP_MAX; | |
177 | } | |
178 | ||
179 | set_bit(BIT(ssap), &local->local_wks); | |
180 | mutex_unlock(&local->sdp_lock); | |
181 | ||
182 | return ssap; | |
183 | } | |
184 | ||
185 | /* | |
186 | * This is not a well known service, | |
187 | * we should try to find a local SDP free spot | |
188 | */ | |
189 | ssap = find_first_zero_bit(&local->local_sdp, LLCP_SDP_NUM_SAP); | |
190 | if (ssap == LLCP_SDP_NUM_SAP) { | |
191 | mutex_unlock(&local->sdp_lock); | |
192 | ||
193 | return LLCP_SAP_MAX; | |
194 | } | |
195 | ||
196 | pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap); | |
197 | ||
198 | set_bit(BIT(ssap), &local->local_sdp); | |
199 | mutex_unlock(&local->sdp_lock); | |
200 | ||
201 | return LLCP_WKS_NUM_SAP + ssap; | |
202 | ||
203 | } else if (sock->ssap != 0) { | |
204 | if (sock->ssap < LLCP_WKS_NUM_SAP) { | |
205 | if (!(local->local_wks & BIT(sock->ssap))) { | |
206 | set_bit(BIT(sock->ssap), &local->local_wks); | |
207 | mutex_unlock(&local->sdp_lock); | |
208 | ||
209 | return sock->ssap; | |
210 | } | |
211 | ||
212 | } else if (sock->ssap < LLCP_SDP_NUM_SAP) { | |
213 | if (!(local->local_sdp & | |
214 | BIT(sock->ssap - LLCP_WKS_NUM_SAP))) { | |
215 | set_bit(BIT(sock->ssap - LLCP_WKS_NUM_SAP), | |
216 | &local->local_sdp); | |
217 | mutex_unlock(&local->sdp_lock); | |
218 | ||
219 | return sock->ssap; | |
220 | } | |
221 | } | |
222 | } | |
223 | ||
224 | mutex_unlock(&local->sdp_lock); | |
225 | ||
226 | return LLCP_SAP_MAX; | |
227 | } | |
228 | ||
229 | u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local) | |
230 | { | |
231 | u8 local_ssap; | |
232 | ||
233 | mutex_lock(&local->sdp_lock); | |
234 | ||
235 | local_ssap = find_first_zero_bit(&local->local_sap, LLCP_LOCAL_NUM_SAP); | |
236 | if (local_ssap == LLCP_LOCAL_NUM_SAP) { | |
237 | mutex_unlock(&local->sdp_lock); | |
238 | return LLCP_SAP_MAX; | |
239 | } | |
240 | ||
241 | set_bit(BIT(local_ssap), &local->local_sap); | |
242 | ||
243 | mutex_unlock(&local->sdp_lock); | |
244 | ||
245 | return local_ssap + LLCP_LOCAL_SAP_OFFSET; | |
246 | } | |
247 | ||
248 | void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap) | |
249 | { | |
250 | u8 local_ssap; | |
251 | unsigned long *sdp; | |
252 | ||
253 | if (ssap < LLCP_WKS_NUM_SAP) { | |
254 | local_ssap = ssap; | |
255 | sdp = &local->local_wks; | |
256 | } else if (ssap < LLCP_LOCAL_NUM_SAP) { | |
257 | local_ssap = ssap - LLCP_WKS_NUM_SAP; | |
258 | sdp = &local->local_sdp; | |
259 | } else if (ssap < LLCP_MAX_SAP) { | |
260 | local_ssap = ssap - LLCP_LOCAL_NUM_SAP; | |
261 | sdp = &local->local_sap; | |
262 | } else { | |
263 | return; | |
264 | } | |
265 | ||
266 | mutex_lock(&local->sdp_lock); | |
267 | ||
268 | clear_bit(1 << local_ssap, sdp); | |
269 | ||
270 | mutex_unlock(&local->sdp_lock); | |
271 | } | |
272 | ||
273 | u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, u8 *general_bytes_len) | |
274 | { | |
275 | struct nfc_llcp_local *local; | |
276 | ||
277 | local = nfc_llcp_find_local(dev); | |
278 | if (local == NULL) { | |
279 | *general_bytes_len = 0; | |
280 | return NULL; | |
281 | } | |
282 | ||
283 | *general_bytes_len = local->gb_len; | |
284 | ||
285 | return local->gb; | |
286 | } | |
287 | ||
288 | static int nfc_llcp_build_gb(struct nfc_llcp_local *local) | |
289 | { | |
290 | u8 *gb_cur, *version_tlv, version, version_length; | |
291 | u8 *lto_tlv, lto, lto_length; | |
292 | u8 *wks_tlv, wks_length; | |
293 | u8 gb_len = 0; | |
294 | ||
295 | version = LLCP_VERSION_11; | |
296 | version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version, | |
297 | 1, &version_length); | |
298 | gb_len += version_length; | |
299 | ||
300 | /* 1500 ms */ | |
301 | lto = 150; | |
302 | lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, <o, 1, <o_length); | |
303 | gb_len += lto_length; | |
304 | ||
305 | pr_debug("Local wks 0x%lx\n", local->local_wks); | |
306 | wks_tlv = nfc_llcp_build_tlv(LLCP_TLV_WKS, (u8 *)&local->local_wks, 2, | |
307 | &wks_length); | |
308 | gb_len += wks_length; | |
309 | ||
310 | gb_len += ARRAY_SIZE(llcp_magic); | |
311 | ||
312 | if (gb_len > NFC_MAX_GT_LEN) { | |
313 | kfree(version_tlv); | |
314 | return -EINVAL; | |
315 | } | |
316 | ||
317 | gb_cur = local->gb; | |
318 | ||
319 | memcpy(gb_cur, llcp_magic, ARRAY_SIZE(llcp_magic)); | |
320 | gb_cur += ARRAY_SIZE(llcp_magic); | |
321 | ||
322 | memcpy(gb_cur, version_tlv, version_length); | |
323 | gb_cur += version_length; | |
324 | ||
325 | memcpy(gb_cur, lto_tlv, lto_length); | |
326 | gb_cur += lto_length; | |
327 | ||
328 | memcpy(gb_cur, wks_tlv, wks_length); | |
329 | gb_cur += wks_length; | |
330 | ||
331 | kfree(version_tlv); | |
332 | kfree(lto_tlv); | |
333 | ||
334 | local->gb_len = gb_len; | |
335 | ||
336 | return 0; | |
337 | } | |
338 | ||
339 | int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len) | |
340 | { | |
341 | struct nfc_llcp_local *local = nfc_llcp_find_local(dev); | |
342 | ||
343 | if (local == NULL) { | |
344 | pr_err("No LLCP device\n"); | |
345 | return -ENODEV; | |
346 | } | |
347 | ||
348 | memset(local->remote_gb, 0, NFC_MAX_GT_LEN); | |
349 | memcpy(local->remote_gb, gb, gb_len); | |
350 | local->remote_gb_len = gb_len; | |
351 | ||
352 | if (local->remote_gb == NULL || | |
353 | local->remote_gb_len == 0) | |
354 | return -ENODEV; | |
355 | ||
356 | if (memcmp(local->remote_gb, llcp_magic, 3)) { | |
357 | pr_err("MAC does not support LLCP\n"); | |
358 | return -EINVAL; | |
359 | } | |
360 | ||
361 | return nfc_llcp_parse_tlv(local, | |
362 | &local->remote_gb[3], local->remote_gb_len - 3); | |
363 | } | |
364 | ||
365 | static void nfc_llcp_tx_work(struct work_struct *work) | |
366 | { | |
367 | struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, | |
368 | tx_work); | |
369 | struct sk_buff *skb; | |
370 | ||
371 | skb = skb_dequeue(&local->tx_queue); | |
372 | if (skb != NULL) { | |
373 | pr_debug("Sending pending skb\n"); | |
374 | nfc_data_exchange(local->dev, local->target_idx, | |
375 | skb, nfc_llcp_recv, local); | |
376 | } else { | |
377 | nfc_llcp_send_symm(local->dev); | |
378 | } | |
379 | ||
380 | mod_timer(&local->link_timer, | |
381 | jiffies + msecs_to_jiffies(local->remote_lto)); | |
382 | } | |
383 | ||
384 | static u8 nfc_llcp_dsap(struct sk_buff *pdu) | |
385 | { | |
386 | return (pdu->data[0] & 0xfc) >> 2; | |
387 | } | |
388 | ||
389 | static u8 nfc_llcp_ptype(struct sk_buff *pdu) | |
390 | { | |
391 | return ((pdu->data[0] & 0x03) << 2) | ((pdu->data[1] & 0xc0) >> 6); | |
392 | } | |
393 | ||
394 | static u8 nfc_llcp_ssap(struct sk_buff *pdu) | |
395 | { | |
396 | return pdu->data[1] & 0x3f; | |
397 | } | |
398 | ||
399 | static u8 nfc_llcp_ns(struct sk_buff *pdu) | |
400 | { | |
401 | return pdu->data[2] >> 4; | |
402 | } | |
403 | ||
404 | static u8 nfc_llcp_nr(struct sk_buff *pdu) | |
405 | { | |
406 | return pdu->data[2] & 0xf; | |
407 | } | |
408 | ||
409 | static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu) | |
410 | { | |
411 | pdu->data[2] = (sock->send_n << 4) | ((sock->recv_n - 1) % 16); | |
412 | sock->send_n = (sock->send_n + 1) % 16; | |
413 | sock->recv_ack_n = (sock->recv_n - 1) % 16; | |
414 | } | |
415 | ||
416 | static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local, | |
417 | u8 ssap, u8 dsap) | |
418 | { | |
419 | struct nfc_llcp_sock *sock, *llcp_sock, *n; | |
420 | ||
421 | if (ssap == 0 && dsap == 0) | |
422 | return NULL; | |
423 | ||
424 | mutex_lock(&local->socket_lock); | |
425 | sock = local->sockets[ssap]; | |
426 | if (sock == NULL) { | |
427 | mutex_unlock(&local->socket_lock); | |
428 | return NULL; | |
429 | } | |
430 | ||
431 | pr_debug("root dsap %d (%d)\n", sock->dsap, dsap); | |
432 | ||
433 | if (sock->dsap == dsap) { | |
434 | sock_hold(&sock->sk); | |
435 | mutex_unlock(&local->socket_lock); | |
436 | return sock; | |
437 | } | |
438 | ||
439 | list_for_each_entry_safe(llcp_sock, n, &sock->list, list) { | |
440 | pr_debug("llcp_sock %p sk %p dsap %d\n", llcp_sock, | |
441 | &llcp_sock->sk, llcp_sock->dsap); | |
442 | if (llcp_sock->dsap == dsap) { | |
443 | sock_hold(&llcp_sock->sk); | |
444 | mutex_unlock(&local->socket_lock); | |
445 | return llcp_sock; | |
446 | } | |
447 | } | |
448 | ||
449 | pr_err("Could not find socket for %d %d\n", ssap, dsap); | |
450 | ||
451 | mutex_unlock(&local->socket_lock); | |
452 | ||
453 | return NULL; | |
454 | } | |
455 | ||
456 | static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock) | |
457 | { | |
458 | sock_put(&sock->sk); | |
459 | } | |
460 | ||
461 | static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len) | |
462 | { | |
463 | u8 *tlv = &skb->data[2], type, length; | |
464 | size_t tlv_array_len = skb->len - LLCP_HEADER_SIZE, offset = 0; | |
465 | ||
466 | while (offset < tlv_array_len) { | |
467 | type = tlv[0]; | |
468 | length = tlv[1]; | |
469 | ||
470 | pr_debug("type 0x%x length %d\n", type, length); | |
471 | ||
472 | if (type == LLCP_TLV_SN) { | |
473 | *sn_len = length; | |
474 | return &tlv[2]; | |
475 | } | |
476 | ||
477 | offset += length + 2; | |
478 | tlv += length + 2; | |
479 | } | |
480 | ||
481 | return NULL; | |
482 | } | |
483 | ||
484 | static void nfc_llcp_recv_connect(struct nfc_llcp_local *local, | |
485 | struct sk_buff *skb) | |
486 | { | |
487 | struct sock *new_sk, *parent; | |
488 | struct nfc_llcp_sock *sock, *new_sock; | |
489 | u8 dsap, ssap, bound_sap, reason; | |
490 | ||
491 | dsap = nfc_llcp_dsap(skb); | |
492 | ssap = nfc_llcp_ssap(skb); | |
493 | ||
494 | pr_debug("%d %d\n", dsap, ssap); | |
495 | ||
496 | nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE], | |
497 | skb->len - LLCP_HEADER_SIZE); | |
498 | ||
499 | if (dsap != LLCP_SAP_SDP) { | |
500 | bound_sap = dsap; | |
501 | ||
502 | mutex_lock(&local->socket_lock); | |
503 | sock = local->sockets[dsap]; | |
504 | if (sock == NULL) { | |
505 | mutex_unlock(&local->socket_lock); | |
506 | reason = LLCP_DM_NOBOUND; | |
507 | goto fail; | |
508 | } | |
509 | ||
510 | sock_hold(&sock->sk); | |
511 | mutex_unlock(&local->socket_lock); | |
512 | ||
513 | lock_sock(&sock->sk); | |
514 | ||
515 | if (sock->dsap == LLCP_SAP_SDP && | |
516 | sock->sk.sk_state == LLCP_LISTEN) | |
517 | goto enqueue; | |
518 | } else { | |
519 | u8 *sn; | |
520 | size_t sn_len; | |
521 | ||
522 | sn = nfc_llcp_connect_sn(skb, &sn_len); | |
523 | if (sn == NULL) { | |
524 | reason = LLCP_DM_NOBOUND; | |
525 | goto fail; | |
526 | } | |
527 | ||
528 | pr_debug("Service name length %zu\n", sn_len); | |
529 | ||
530 | mutex_lock(&local->socket_lock); | |
531 | for (bound_sap = 0; bound_sap < LLCP_LOCAL_SAP_OFFSET; | |
532 | bound_sap++) { | |
533 | sock = local->sockets[bound_sap]; | |
534 | if (sock == NULL) | |
535 | continue; | |
536 | ||
537 | if (sock->service_name == NULL || | |
538 | sock->service_name_len == 0) | |
539 | continue; | |
540 | ||
541 | if (sock->service_name_len != sn_len) | |
542 | continue; | |
543 | ||
544 | if (sock->dsap == LLCP_SAP_SDP && | |
545 | sock->sk.sk_state == LLCP_LISTEN && | |
546 | !memcmp(sn, sock->service_name, sn_len)) { | |
547 | pr_debug("Found service name at SAP %d\n", | |
548 | bound_sap); | |
549 | sock_hold(&sock->sk); | |
550 | mutex_unlock(&local->socket_lock); | |
551 | ||
552 | lock_sock(&sock->sk); | |
553 | ||
554 | goto enqueue; | |
555 | } | |
556 | } | |
557 | ||
558 | } | |
559 | ||
560 | mutex_unlock(&local->socket_lock); | |
561 | ||
562 | reason = LLCP_DM_NOBOUND; | |
563 | goto fail; | |
564 | ||
565 | enqueue: | |
566 | parent = &sock->sk; | |
567 | ||
568 | if (sk_acceptq_is_full(parent)) { | |
569 | reason = LLCP_DM_REJ; | |
570 | release_sock(&sock->sk); | |
571 | sock_put(&sock->sk); | |
572 | goto fail; | |
573 | } | |
574 | ||
575 | new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, | |
576 | GFP_ATOMIC); | |
577 | if (new_sk == NULL) { | |
578 | reason = LLCP_DM_REJ; | |
579 | release_sock(&sock->sk); | |
580 | sock_put(&sock->sk); | |
581 | goto fail; | |
582 | } | |
583 | ||
584 | new_sock = nfc_llcp_sock(new_sk); | |
585 | new_sock->dev = local->dev; | |
586 | new_sock->local = local; | |
587 | new_sock->nfc_protocol = sock->nfc_protocol; | |
588 | new_sock->ssap = bound_sap; | |
589 | new_sock->dsap = ssap; | |
590 | new_sock->parent = parent; | |
591 | ||
592 | pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk); | |
593 | ||
594 | list_add_tail(&new_sock->list, &sock->list); | |
595 | ||
596 | nfc_llcp_accept_enqueue(&sock->sk, new_sk); | |
597 | ||
598 | nfc_get_device(local->dev->idx); | |
599 | ||
600 | new_sk->sk_state = LLCP_CONNECTED; | |
601 | ||
602 | /* Wake the listening processes */ | |
603 | parent->sk_data_ready(parent, 0); | |
604 | ||
605 | /* Send CC */ | |
606 | nfc_llcp_send_cc(new_sock); | |
607 | ||
608 | release_sock(&sock->sk); | |
609 | sock_put(&sock->sk); | |
610 | ||
611 | return; | |
612 | ||
613 | fail: | |
614 | /* Send DM */ | |
615 | nfc_llcp_send_dm(local, dsap, ssap, reason); | |
616 | ||
617 | return; | |
618 | ||
619 | } | |
620 | ||
621 | static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local, | |
622 | struct sk_buff *skb) | |
623 | { | |
624 | struct nfc_llcp_sock *llcp_sock; | |
625 | struct sock *sk; | |
626 | u8 dsap, ssap, ptype, ns, nr; | |
627 | ||
628 | ptype = nfc_llcp_ptype(skb); | |
629 | dsap = nfc_llcp_dsap(skb); | |
630 | ssap = nfc_llcp_ssap(skb); | |
631 | ns = nfc_llcp_ns(skb); | |
632 | nr = nfc_llcp_nr(skb); | |
633 | ||
634 | pr_debug("%d %d R %d S %d\n", dsap, ssap, nr, ns); | |
635 | ||
636 | llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); | |
637 | if (llcp_sock == NULL) { | |
638 | nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); | |
639 | return; | |
640 | } | |
641 | ||
642 | sk = &llcp_sock->sk; | |
643 | lock_sock(sk); | |
644 | if (sk->sk_state == LLCP_CLOSED) { | |
645 | release_sock(sk); | |
646 | nfc_llcp_sock_put(llcp_sock); | |
647 | } | |
648 | ||
649 | if (ns == llcp_sock->recv_n) | |
650 | llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16; | |
651 | else | |
652 | pr_err("Received out of sequence I PDU\n"); | |
653 | ||
654 | /* Pass the payload upstream */ | |
655 | if (ptype == LLCP_PDU_I) { | |
656 | pr_debug("I frame, queueing on %p\n", &llcp_sock->sk); | |
657 | ||
658 | skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE); | |
659 | if (sock_queue_rcv_skb(&llcp_sock->sk, skb)) { | |
660 | pr_err("receive queue is full\n"); | |
661 | skb_queue_head(&llcp_sock->tx_backlog_queue, skb); | |
662 | } | |
663 | } | |
664 | ||
665 | /* Remove skbs from the pending queue */ | |
666 | if (llcp_sock->send_ack_n != nr) { | |
667 | struct sk_buff *s, *tmp; | |
668 | ||
669 | llcp_sock->send_ack_n = nr; | |
670 | ||
671 | skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp) | |
672 | if (nfc_llcp_ns(s) <= nr) { | |
673 | skb_unlink(s, &llcp_sock->tx_pending_queue); | |
674 | kfree_skb(s); | |
675 | } | |
676 | } | |
677 | ||
678 | /* Queue some I frames for transmission */ | |
679 | while (llcp_sock->remote_ready && | |
680 | skb_queue_len(&llcp_sock->tx_pending_queue) <= local->remote_rw) { | |
681 | struct sk_buff *pdu, *pending_pdu; | |
682 | ||
683 | pdu = skb_dequeue(&llcp_sock->tx_queue); | |
684 | if (pdu == NULL) | |
685 | break; | |
686 | ||
687 | /* Update N(S)/N(R) */ | |
688 | nfc_llcp_set_nrns(llcp_sock, pdu); | |
689 | ||
690 | pending_pdu = skb_clone(pdu, GFP_KERNEL); | |
691 | ||
692 | skb_queue_tail(&local->tx_queue, pdu); | |
693 | skb_queue_tail(&llcp_sock->tx_pending_queue, pending_pdu); | |
694 | } | |
695 | ||
696 | release_sock(sk); | |
697 | nfc_llcp_sock_put(llcp_sock); | |
698 | } | |
699 | ||
700 | static void nfc_llcp_recv_disc(struct nfc_llcp_local *local, | |
701 | struct sk_buff *skb) | |
702 | { | |
703 | struct nfc_llcp_sock *llcp_sock; | |
704 | struct sock *sk; | |
705 | u8 dsap, ssap; | |
706 | ||
707 | dsap = nfc_llcp_dsap(skb); | |
708 | ssap = nfc_llcp_ssap(skb); | |
709 | ||
710 | llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); | |
711 | if (llcp_sock == NULL) { | |
712 | nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); | |
713 | return; | |
714 | } | |
715 | ||
716 | sk = &llcp_sock->sk; | |
717 | lock_sock(sk); | |
718 | if (sk->sk_state == LLCP_CLOSED) { | |
719 | release_sock(sk); | |
720 | nfc_llcp_sock_put(llcp_sock); | |
721 | } | |
722 | ||
723 | ||
724 | if (sk->sk_state == LLCP_CONNECTED) { | |
725 | nfc_put_device(local->dev); | |
726 | sk->sk_state = LLCP_CLOSED; | |
727 | sk->sk_state_change(sk); | |
728 | } | |
729 | ||
730 | nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_DISC); | |
731 | ||
732 | release_sock(sk); | |
733 | nfc_llcp_sock_put(llcp_sock); | |
734 | } | |
735 | ||
736 | static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, | |
737 | struct sk_buff *skb) | |
738 | { | |
739 | struct nfc_llcp_sock *llcp_sock; | |
740 | u8 dsap, ssap; | |
741 | ||
742 | ||
743 | dsap = nfc_llcp_dsap(skb); | |
744 | ssap = nfc_llcp_ssap(skb); | |
745 | ||
746 | llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); | |
747 | ||
748 | if (llcp_sock == NULL) | |
749 | llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP); | |
750 | ||
751 | if (llcp_sock == NULL) { | |
752 | pr_err("Invalid CC\n"); | |
753 | nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); | |
754 | ||
755 | return; | |
756 | } | |
757 | ||
758 | llcp_sock->dsap = ssap; | |
759 | ||
760 | nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE], | |
761 | skb->len - LLCP_HEADER_SIZE); | |
762 | ||
763 | nfc_llcp_sock_put(llcp_sock); | |
764 | } | |
765 | ||
766 | static void nfc_llcp_rx_work(struct work_struct *work) | |
767 | { | |
768 | struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, | |
769 | rx_work); | |
770 | u8 dsap, ssap, ptype; | |
771 | struct sk_buff *skb; | |
772 | ||
773 | skb = local->rx_pending; | |
774 | if (skb == NULL) { | |
775 | pr_debug("No pending SKB\n"); | |
776 | return; | |
777 | } | |
778 | ||
779 | ptype = nfc_llcp_ptype(skb); | |
780 | dsap = nfc_llcp_dsap(skb); | |
781 | ssap = nfc_llcp_ssap(skb); | |
782 | ||
783 | pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap); | |
784 | ||
785 | switch (ptype) { | |
786 | case LLCP_PDU_SYMM: | |
787 | pr_debug("SYMM\n"); | |
788 | break; | |
789 | ||
790 | case LLCP_PDU_CONNECT: | |
791 | pr_debug("CONNECT\n"); | |
792 | nfc_llcp_recv_connect(local, skb); | |
793 | break; | |
794 | ||
795 | case LLCP_PDU_DISC: | |
796 | pr_debug("DISC\n"); | |
797 | nfc_llcp_recv_disc(local, skb); | |
798 | break; | |
799 | ||
800 | case LLCP_PDU_CC: | |
801 | pr_debug("CC\n"); | |
802 | nfc_llcp_recv_cc(local, skb); | |
803 | break; | |
804 | ||
805 | case LLCP_PDU_I: | |
806 | case LLCP_PDU_RR: | |
807 | pr_debug("I frame\n"); | |
808 | nfc_llcp_recv_hdlc(local, skb); | |
809 | break; | |
810 | ||
811 | } | |
812 | ||
813 | queue_work(local->tx_wq, &local->tx_work); | |
814 | kfree_skb(local->rx_pending); | |
815 | local->rx_pending = NULL; | |
816 | ||
817 | return; | |
818 | } | |
819 | ||
820 | void nfc_llcp_recv(void *data, struct sk_buff *skb, int err) | |
821 | { | |
822 | struct nfc_llcp_local *local = (struct nfc_llcp_local *) data; | |
823 | ||
824 | pr_debug("Received an LLCP PDU\n"); | |
825 | if (err < 0) { | |
826 | pr_err("err %d", err); | |
827 | return; | |
828 | } | |
829 | ||
830 | local->rx_pending = skb_get(skb); | |
831 | del_timer(&local->link_timer); | |
832 | queue_work(local->rx_wq, &local->rx_work); | |
833 | ||
834 | return; | |
835 | } | |
836 | ||
837 | void nfc_llcp_mac_is_down(struct nfc_dev *dev) | |
838 | { | |
839 | struct nfc_llcp_local *local; | |
840 | ||
841 | local = nfc_llcp_find_local(dev); | |
842 | if (local == NULL) | |
843 | return; | |
844 | ||
845 | /* Close and purge all existing sockets */ | |
846 | nfc_llcp_socket_release(local); | |
847 | } | |
848 | ||
849 | void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx, | |
850 | u8 comm_mode, u8 rf_mode) | |
851 | { | |
852 | struct nfc_llcp_local *local; | |
853 | ||
854 | pr_debug("rf mode %d\n", rf_mode); | |
855 | ||
856 | local = nfc_llcp_find_local(dev); | |
857 | if (local == NULL) | |
858 | return; | |
859 | ||
860 | local->target_idx = target_idx; | |
861 | local->comm_mode = comm_mode; | |
862 | local->rf_mode = rf_mode; | |
863 | ||
864 | if (rf_mode == NFC_RF_INITIATOR) { | |
865 | pr_debug("Queueing Tx work\n"); | |
866 | ||
867 | queue_work(local->tx_wq, &local->tx_work); | |
868 | } else { | |
869 | mod_timer(&local->link_timer, | |
870 | jiffies + msecs_to_jiffies(local->remote_lto)); | |
871 | } | |
872 | } | |
873 | ||
874 | int nfc_llcp_register_device(struct nfc_dev *ndev) | |
875 | { | |
876 | struct device *dev = &ndev->dev; | |
877 | struct nfc_llcp_local *local; | |
878 | char name[32]; | |
879 | int err; | |
880 | ||
881 | local = kzalloc(sizeof(struct nfc_llcp_local), GFP_KERNEL); | |
882 | if (local == NULL) | |
883 | return -ENOMEM; | |
884 | ||
885 | local->dev = ndev; | |
886 | INIT_LIST_HEAD(&local->list); | |
887 | mutex_init(&local->sdp_lock); | |
888 | mutex_init(&local->socket_lock); | |
889 | init_timer(&local->link_timer); | |
890 | local->link_timer.data = (unsigned long) local; | |
891 | local->link_timer.function = nfc_llcp_symm_timer; | |
892 | ||
893 | skb_queue_head_init(&local->tx_queue); | |
894 | INIT_WORK(&local->tx_work, nfc_llcp_tx_work); | |
895 | snprintf(name, sizeof(name), "%s_llcp_tx_wq", dev_name(dev)); | |
896 | local->tx_wq = alloc_workqueue(name, | |
897 | WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, 1); | |
898 | if (local->tx_wq == NULL) { | |
899 | err = -ENOMEM; | |
900 | goto err_local; | |
901 | } | |
902 | ||
903 | local->rx_pending = NULL; | |
904 | INIT_WORK(&local->rx_work, nfc_llcp_rx_work); | |
905 | snprintf(name, sizeof(name), "%s_llcp_rx_wq", dev_name(dev)); | |
906 | local->rx_wq = alloc_workqueue(name, | |
907 | WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, 1); | |
908 | if (local->rx_wq == NULL) { | |
909 | err = -ENOMEM; | |
910 | goto err_tx_wq; | |
911 | } | |
912 | ||
913 | INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work); | |
914 | snprintf(name, sizeof(name), "%s_llcp_timeout_wq", dev_name(dev)); | |
915 | local->timeout_wq = alloc_workqueue(name, | |
916 | WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, 1); | |
917 | if (local->timeout_wq == NULL) { | |
918 | err = -ENOMEM; | |
919 | goto err_rx_wq; | |
920 | } | |
921 | ||
922 | nfc_llcp_build_gb(local); | |
923 | ||
924 | local->remote_miu = LLCP_DEFAULT_MIU; | |
925 | local->remote_lto = LLCP_DEFAULT_LTO; | |
926 | local->remote_rw = LLCP_DEFAULT_RW; | |
927 | ||
928 | list_add(&llcp_devices, &local->list); | |
929 | ||
930 | return 0; | |
931 | ||
932 | err_rx_wq: | |
933 | destroy_workqueue(local->rx_wq); | |
934 | ||
935 | err_tx_wq: | |
936 | destroy_workqueue(local->tx_wq); | |
937 | ||
938 | err_local: | |
939 | kfree(local); | |
940 | ||
941 | return 0; | |
942 | } | |
943 | ||
944 | void nfc_llcp_unregister_device(struct nfc_dev *dev) | |
945 | { | |
946 | struct nfc_llcp_local *local = nfc_llcp_find_local(dev); | |
947 | ||
948 | if (local == NULL) { | |
949 | pr_debug("No such device\n"); | |
950 | return; | |
951 | } | |
952 | ||
953 | list_del(&local->list); | |
954 | nfc_llcp_socket_release(local); | |
955 | del_timer_sync(&local->link_timer); | |
956 | skb_queue_purge(&local->tx_queue); | |
957 | destroy_workqueue(local->tx_wq); | |
958 | destroy_workqueue(local->rx_wq); | |
959 | kfree(local->rx_pending); | |
960 | kfree(local); | |
961 | } | |
962 | ||
963 | int __init nfc_llcp_init(void) | |
964 | { | |
965 | INIT_LIST_HEAD(&llcp_devices); | |
966 | ||
967 | return nfc_llcp_sock_init(); | |
968 | } | |
969 | ||
970 | void nfc_llcp_exit(void) | |
971 | { | |
972 | nfc_llcp_sock_exit(); | |
973 | } |