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