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