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 | |
4d22ea15 | 48 | static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen) |
a69f32af SO |
49 | { |
50 | struct sock *sk; | |
51 | struct hlist_node *node, *tmp; | |
52 | struct nfc_llcp_sock *llcp_sock; | |
d646960f | 53 | |
a69f32af | 54 | write_lock(&local->sockets.lock); |
40c75f81 | 55 | |
a69f32af SO |
56 | sk_for_each_safe(sk, node, tmp, &local->sockets.head) { |
57 | llcp_sock = nfc_llcp_sock(sk); | |
d646960f | 58 | |
a69f32af | 59 | lock_sock(sk); |
d646960f | 60 | |
a69f32af SO |
61 | if (sk->sk_state == LLCP_CONNECTED) |
62 | nfc_put_device(llcp_sock->dev); | |
d646960f | 63 | |
a69f32af | 64 | if (sk->sk_state == LLCP_LISTEN) { |
d646960f SO |
65 | struct nfc_llcp_sock *lsk, *n; |
66 | struct sock *accept_sk; | |
67 | ||
a69f32af | 68 | list_for_each_entry_safe(lsk, n, &llcp_sock->accept_queue, |
427a2eb1 | 69 | accept_queue) { |
d646960f SO |
70 | accept_sk = &lsk->sk; |
71 | lock_sock(accept_sk); | |
72 | ||
73 | nfc_llcp_accept_unlink(accept_sk); | |
74 | ||
75 | accept_sk->sk_state = LLCP_CLOSED; | |
d646960f SO |
76 | |
77 | release_sock(accept_sk); | |
78 | ||
79 | sock_orphan(accept_sk); | |
80 | } | |
4d22ea15 SO |
81 | |
82 | if (listen == true) { | |
83 | release_sock(sk); | |
84 | continue; | |
85 | } | |
d646960f SO |
86 | } |
87 | ||
a69f32af | 88 | sk->sk_state = LLCP_CLOSED; |
d646960f | 89 | |
a69f32af | 90 | release_sock(sk); |
d646960f | 91 | |
a69f32af | 92 | sock_orphan(sk); |
40c75f81 | 93 | |
a69f32af | 94 | sk_del_node_init(sk); |
d646960f SO |
95 | } |
96 | ||
a69f32af | 97 | write_unlock(&local->sockets.lock); |
d646960f SO |
98 | } |
99 | ||
c7aa1225 SO |
100 | struct nfc_llcp_local *nfc_llcp_local_get(struct nfc_llcp_local *local) |
101 | { | |
102 | kref_get(&local->ref); | |
103 | ||
104 | return local; | |
105 | } | |
106 | ||
107 | static void local_release(struct kref *ref) | |
108 | { | |
109 | struct nfc_llcp_local *local; | |
110 | ||
111 | local = container_of(ref, struct nfc_llcp_local, ref); | |
112 | ||
113 | list_del(&local->list); | |
4d22ea15 | 114 | nfc_llcp_socket_release(local, false); |
c7aa1225 SO |
115 | del_timer_sync(&local->link_timer); |
116 | skb_queue_purge(&local->tx_queue); | |
117 | destroy_workqueue(local->tx_wq); | |
118 | destroy_workqueue(local->rx_wq); | |
07922bb1 | 119 | destroy_workqueue(local->timeout_wq); |
c7aa1225 SO |
120 | kfree_skb(local->rx_pending); |
121 | kfree(local); | |
122 | } | |
123 | ||
124 | int nfc_llcp_local_put(struct nfc_llcp_local *local) | |
125 | { | |
a69f32af SO |
126 | WARN_ON(local == NULL); |
127 | ||
128 | if (local == NULL) | |
129 | return 0; | |
130 | ||
c7aa1225 SO |
131 | return kref_put(&local->ref, local_release); |
132 | } | |
133 | ||
8f50020e SO |
134 | static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local, |
135 | u8 ssap, u8 dsap) | |
136 | { | |
137 | struct sock *sk; | |
138 | struct hlist_node *node; | |
139 | struct nfc_llcp_sock *llcp_sock; | |
140 | ||
141 | pr_debug("ssap dsap %d %d\n", ssap, dsap); | |
142 | ||
143 | if (ssap == 0 && dsap == 0) | |
144 | return NULL; | |
145 | ||
146 | read_lock(&local->sockets.lock); | |
147 | ||
148 | llcp_sock = NULL; | |
149 | ||
150 | sk_for_each(sk, node, &local->sockets.head) { | |
151 | llcp_sock = nfc_llcp_sock(sk); | |
152 | ||
153 | if (llcp_sock->ssap == ssap && llcp_sock->dsap == dsap) | |
154 | break; | |
155 | } | |
156 | ||
157 | read_unlock(&local->sockets.lock); | |
158 | ||
159 | if (llcp_sock == NULL) | |
160 | return NULL; | |
161 | ||
162 | sock_hold(&llcp_sock->sk); | |
163 | ||
164 | return llcp_sock; | |
165 | } | |
166 | ||
167 | static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock) | |
168 | { | |
169 | sock_put(&sock->sk); | |
170 | } | |
171 | ||
d646960f SO |
172 | static void nfc_llcp_timeout_work(struct work_struct *work) |
173 | { | |
174 | struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, | |
427a2eb1 | 175 | timeout_work); |
d646960f SO |
176 | |
177 | nfc_dep_link_down(local->dev); | |
178 | } | |
179 | ||
180 | static void nfc_llcp_symm_timer(unsigned long data) | |
181 | { | |
182 | struct nfc_llcp_local *local = (struct nfc_llcp_local *) data; | |
183 | ||
184 | pr_err("SYMM timeout\n"); | |
185 | ||
186 | queue_work(local->timeout_wq, &local->timeout_work); | |
187 | } | |
188 | ||
189 | struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev) | |
190 | { | |
191 | struct nfc_llcp_local *local, *n; | |
192 | ||
193 | list_for_each_entry_safe(local, n, &llcp_devices, list) | |
194 | if (local->dev == dev) | |
195 | return local; | |
196 | ||
197 | pr_debug("No device found\n"); | |
198 | ||
199 | return NULL; | |
200 | } | |
201 | ||
202 | static char *wks[] = { | |
203 | NULL, | |
204 | NULL, /* SDP */ | |
205 | "urn:nfc:sn:ip", | |
206 | "urn:nfc:sn:obex", | |
207 | "urn:nfc:sn:snep", | |
208 | }; | |
209 | ||
210 | static int nfc_llcp_wks_sap(char *service_name, size_t service_name_len) | |
211 | { | |
212 | int sap, num_wks; | |
213 | ||
214 | pr_debug("%s\n", service_name); | |
215 | ||
216 | if (service_name == NULL) | |
217 | return -EINVAL; | |
218 | ||
219 | num_wks = ARRAY_SIZE(wks); | |
220 | ||
427a2eb1 | 221 | for (sap = 0; sap < num_wks; sap++) { |
d646960f SO |
222 | if (wks[sap] == NULL) |
223 | continue; | |
224 | ||
225 | if (strncmp(wks[sap], service_name, service_name_len) == 0) | |
226 | return sap; | |
227 | } | |
228 | ||
229 | return -EINVAL; | |
230 | } | |
231 | ||
8f50020e SO |
232 | static |
233 | struct nfc_llcp_sock *nfc_llcp_sock_from_sn(struct nfc_llcp_local *local, | |
234 | u8 *sn, size_t sn_len) | |
235 | { | |
236 | struct sock *sk; | |
237 | struct hlist_node *node; | |
238 | struct nfc_llcp_sock *llcp_sock, *tmp_sock; | |
239 | ||
240 | pr_debug("sn %zd %p\n", sn_len, sn); | |
241 | ||
242 | if (sn == NULL || sn_len == 0) | |
243 | return NULL; | |
244 | ||
245 | read_lock(&local->sockets.lock); | |
246 | ||
247 | llcp_sock = NULL; | |
248 | ||
249 | sk_for_each(sk, node, &local->sockets.head) { | |
250 | tmp_sock = nfc_llcp_sock(sk); | |
251 | ||
252 | pr_debug("llcp sock %p\n", tmp_sock); | |
253 | ||
254 | if (tmp_sock->sk.sk_state != LLCP_LISTEN) | |
255 | continue; | |
256 | ||
257 | if (tmp_sock->service_name == NULL || | |
258 | tmp_sock->service_name_len == 0) | |
259 | continue; | |
260 | ||
261 | if (tmp_sock->service_name_len != sn_len) | |
262 | continue; | |
263 | ||
264 | if (memcmp(sn, tmp_sock->service_name, sn_len) == 0) { | |
265 | llcp_sock = tmp_sock; | |
266 | break; | |
267 | } | |
268 | } | |
269 | ||
270 | read_unlock(&local->sockets.lock); | |
271 | ||
272 | pr_debug("Found llcp sock %p\n", llcp_sock); | |
273 | ||
274 | return llcp_sock; | |
275 | } | |
276 | ||
d646960f | 277 | u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local, |
427a2eb1 | 278 | struct nfc_llcp_sock *sock) |
d646960f SO |
279 | { |
280 | mutex_lock(&local->sdp_lock); | |
281 | ||
282 | if (sock->service_name != NULL && sock->service_name_len > 0) { | |
283 | int ssap = nfc_llcp_wks_sap(sock->service_name, | |
427a2eb1 | 284 | sock->service_name_len); |
d646960f SO |
285 | |
286 | if (ssap > 0) { | |
287 | pr_debug("WKS %d\n", ssap); | |
288 | ||
289 | /* This is a WKS, let's check if it's free */ | |
290 | if (local->local_wks & BIT(ssap)) { | |
291 | mutex_unlock(&local->sdp_lock); | |
292 | ||
293 | return LLCP_SAP_MAX; | |
294 | } | |
295 | ||
1762c17c | 296 | set_bit(ssap, &local->local_wks); |
d646960f SO |
297 | mutex_unlock(&local->sdp_lock); |
298 | ||
299 | return ssap; | |
300 | } | |
301 | ||
302 | /* | |
8f50020e SO |
303 | * Check if there already is a non WKS socket bound |
304 | * to this service name. | |
d646960f | 305 | */ |
8f50020e SO |
306 | if (nfc_llcp_sock_from_sn(local, sock->service_name, |
307 | sock->service_name_len) != NULL) { | |
d646960f SO |
308 | mutex_unlock(&local->sdp_lock); |
309 | ||
310 | return LLCP_SAP_MAX; | |
311 | } | |
312 | ||
d646960f SO |
313 | mutex_unlock(&local->sdp_lock); |
314 | ||
8f50020e | 315 | return LLCP_SDP_UNBOUND; |
d646960f | 316 | |
ebbb16d9 SO |
317 | } else if (sock->ssap != 0 && sock->ssap < LLCP_WKS_NUM_SAP) { |
318 | if (!test_bit(sock->ssap, &local->local_wks)) { | |
319 | set_bit(sock->ssap, &local->local_wks); | |
320 | mutex_unlock(&local->sdp_lock); | |
d646960f | 321 | |
ebbb16d9 | 322 | return sock->ssap; |
d646960f SO |
323 | } |
324 | } | |
325 | ||
326 | mutex_unlock(&local->sdp_lock); | |
327 | ||
328 | return LLCP_SAP_MAX; | |
329 | } | |
330 | ||
331 | u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local) | |
332 | { | |
333 | u8 local_ssap; | |
334 | ||
335 | mutex_lock(&local->sdp_lock); | |
336 | ||
337 | local_ssap = find_first_zero_bit(&local->local_sap, LLCP_LOCAL_NUM_SAP); | |
338 | if (local_ssap == LLCP_LOCAL_NUM_SAP) { | |
339 | mutex_unlock(&local->sdp_lock); | |
340 | return LLCP_SAP_MAX; | |
341 | } | |
342 | ||
1762c17c | 343 | set_bit(local_ssap, &local->local_sap); |
d646960f SO |
344 | |
345 | mutex_unlock(&local->sdp_lock); | |
346 | ||
347 | return local_ssap + LLCP_LOCAL_SAP_OFFSET; | |
348 | } | |
349 | ||
350 | void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap) | |
351 | { | |
352 | u8 local_ssap; | |
353 | unsigned long *sdp; | |
354 | ||
355 | if (ssap < LLCP_WKS_NUM_SAP) { | |
356 | local_ssap = ssap; | |
357 | sdp = &local->local_wks; | |
358 | } else if (ssap < LLCP_LOCAL_NUM_SAP) { | |
8f50020e SO |
359 | atomic_t *client_cnt; |
360 | ||
d646960f SO |
361 | local_ssap = ssap - LLCP_WKS_NUM_SAP; |
362 | sdp = &local->local_sdp; | |
8f50020e SO |
363 | client_cnt = &local->local_sdp_cnt[local_ssap]; |
364 | ||
365 | pr_debug("%d clients\n", atomic_read(client_cnt)); | |
366 | ||
367 | mutex_lock(&local->sdp_lock); | |
368 | ||
369 | if (atomic_dec_and_test(client_cnt)) { | |
370 | struct nfc_llcp_sock *l_sock; | |
371 | ||
372 | pr_debug("No more clients for SAP %d\n", ssap); | |
373 | ||
374 | clear_bit(local_ssap, sdp); | |
375 | ||
376 | /* Find the listening sock and set it back to UNBOUND */ | |
377 | l_sock = nfc_llcp_sock_get(local, ssap, LLCP_SAP_SDP); | |
378 | if (l_sock) { | |
379 | l_sock->ssap = LLCP_SDP_UNBOUND; | |
380 | nfc_llcp_sock_put(l_sock); | |
381 | } | |
382 | } | |
383 | ||
384 | mutex_unlock(&local->sdp_lock); | |
385 | ||
386 | return; | |
d646960f SO |
387 | } else if (ssap < LLCP_MAX_SAP) { |
388 | local_ssap = ssap - LLCP_LOCAL_NUM_SAP; | |
389 | sdp = &local->local_sap; | |
390 | } else { | |
391 | return; | |
392 | } | |
393 | ||
394 | mutex_lock(&local->sdp_lock); | |
395 | ||
1762c17c | 396 | clear_bit(local_ssap, sdp); |
d646960f SO |
397 | |
398 | mutex_unlock(&local->sdp_lock); | |
399 | } | |
400 | ||
8f50020e SO |
401 | static u8 nfc_llcp_reserve_sdp_ssap(struct nfc_llcp_local *local) |
402 | { | |
403 | u8 ssap; | |
404 | ||
405 | mutex_lock(&local->sdp_lock); | |
406 | ||
407 | ssap = find_first_zero_bit(&local->local_sdp, LLCP_SDP_NUM_SAP); | |
408 | if (ssap == LLCP_SDP_NUM_SAP) { | |
409 | mutex_unlock(&local->sdp_lock); | |
410 | ||
411 | return LLCP_SAP_MAX; | |
412 | } | |
413 | ||
414 | pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap); | |
415 | ||
416 | set_bit(ssap, &local->local_sdp); | |
417 | ||
418 | mutex_unlock(&local->sdp_lock); | |
419 | ||
420 | return LLCP_WKS_NUM_SAP + ssap; | |
421 | } | |
422 | ||
d646960f SO |
423 | static int nfc_llcp_build_gb(struct nfc_llcp_local *local) |
424 | { | |
425 | u8 *gb_cur, *version_tlv, version, version_length; | |
426 | u8 *lto_tlv, lto, lto_length; | |
427 | u8 *wks_tlv, wks_length; | |
56d5876a SO |
428 | u8 *miux_tlv, miux_length; |
429 | __be16 miux; | |
d646960f SO |
430 | u8 gb_len = 0; |
431 | ||
432 | version = LLCP_VERSION_11; | |
433 | version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version, | |
427a2eb1 | 434 | 1, &version_length); |
d646960f SO |
435 | gb_len += version_length; |
436 | ||
437 | /* 1500 ms */ | |
438 | lto = 150; | |
91b0ade1 | 439 | lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_LTO, <o, 1, <o_length); |
d646960f SO |
440 | gb_len += lto_length; |
441 | ||
442 | pr_debug("Local wks 0x%lx\n", local->local_wks); | |
443 | wks_tlv = nfc_llcp_build_tlv(LLCP_TLV_WKS, (u8 *)&local->local_wks, 2, | |
427a2eb1 | 444 | &wks_length); |
d646960f SO |
445 | gb_len += wks_length; |
446 | ||
56d5876a SO |
447 | miux = cpu_to_be16(LLCP_MAX_MIUX); |
448 | miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0, | |
449 | &miux_length); | |
450 | gb_len += miux_length; | |
451 | ||
d646960f SO |
452 | gb_len += ARRAY_SIZE(llcp_magic); |
453 | ||
454 | if (gb_len > NFC_MAX_GT_LEN) { | |
455 | kfree(version_tlv); | |
456 | return -EINVAL; | |
457 | } | |
458 | ||
459 | gb_cur = local->gb; | |
460 | ||
461 | memcpy(gb_cur, llcp_magic, ARRAY_SIZE(llcp_magic)); | |
462 | gb_cur += ARRAY_SIZE(llcp_magic); | |
463 | ||
464 | memcpy(gb_cur, version_tlv, version_length); | |
465 | gb_cur += version_length; | |
466 | ||
467 | memcpy(gb_cur, lto_tlv, lto_length); | |
468 | gb_cur += lto_length; | |
469 | ||
470 | memcpy(gb_cur, wks_tlv, wks_length); | |
471 | gb_cur += wks_length; | |
472 | ||
56d5876a SO |
473 | memcpy(gb_cur, miux_tlv, miux_length); |
474 | gb_cur += miux_length; | |
475 | ||
d646960f SO |
476 | kfree(version_tlv); |
477 | kfree(lto_tlv); | |
478 | ||
479 | local->gb_len = gb_len; | |
480 | ||
481 | return 0; | |
482 | } | |
483 | ||
b8e7a06d SO |
484 | u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len) |
485 | { | |
486 | struct nfc_llcp_local *local; | |
487 | ||
488 | local = nfc_llcp_find_local(dev); | |
489 | if (local == NULL) { | |
490 | *general_bytes_len = 0; | |
491 | return NULL; | |
492 | } | |
493 | ||
494 | nfc_llcp_build_gb(local); | |
495 | ||
496 | *general_bytes_len = local->gb_len; | |
497 | ||
498 | return local->gb; | |
499 | } | |
500 | ||
d646960f SO |
501 | int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len) |
502 | { | |
503 | struct nfc_llcp_local *local = nfc_llcp_find_local(dev); | |
504 | ||
505 | if (local == NULL) { | |
506 | pr_err("No LLCP device\n"); | |
507 | return -ENODEV; | |
508 | } | |
509 | ||
510 | memset(local->remote_gb, 0, NFC_MAX_GT_LEN); | |
511 | memcpy(local->remote_gb, gb, gb_len); | |
512 | local->remote_gb_len = gb_len; | |
513 | ||
427a2eb1 | 514 | if (local->remote_gb == NULL || local->remote_gb_len == 0) |
d646960f SO |
515 | return -ENODEV; |
516 | ||
517 | if (memcmp(local->remote_gb, llcp_magic, 3)) { | |
518 | pr_err("MAC does not support LLCP\n"); | |
519 | return -EINVAL; | |
520 | } | |
521 | ||
7a06e586 SO |
522 | return nfc_llcp_parse_gb_tlv(local, |
523 | &local->remote_gb[3], | |
524 | local->remote_gb_len - 3); | |
d646960f SO |
525 | } |
526 | ||
d646960f SO |
527 | static u8 nfc_llcp_dsap(struct sk_buff *pdu) |
528 | { | |
529 | return (pdu->data[0] & 0xfc) >> 2; | |
530 | } | |
531 | ||
532 | static u8 nfc_llcp_ptype(struct sk_buff *pdu) | |
533 | { | |
534 | return ((pdu->data[0] & 0x03) << 2) | ((pdu->data[1] & 0xc0) >> 6); | |
535 | } | |
536 | ||
537 | static u8 nfc_llcp_ssap(struct sk_buff *pdu) | |
538 | { | |
539 | return pdu->data[1] & 0x3f; | |
540 | } | |
541 | ||
542 | static u8 nfc_llcp_ns(struct sk_buff *pdu) | |
543 | { | |
544 | return pdu->data[2] >> 4; | |
545 | } | |
546 | ||
547 | static u8 nfc_llcp_nr(struct sk_buff *pdu) | |
548 | { | |
549 | return pdu->data[2] & 0xf; | |
550 | } | |
551 | ||
552 | static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu) | |
553 | { | |
279cf174 | 554 | pdu->data[2] = (sock->send_n << 4) | (sock->recv_n); |
d646960f SO |
555 | sock->send_n = (sock->send_n + 1) % 16; |
556 | sock->recv_ack_n = (sock->recv_n - 1) % 16; | |
557 | } | |
558 | ||
84457960 SO |
559 | static void nfc_llcp_tx_work(struct work_struct *work) |
560 | { | |
561 | struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, | |
562 | tx_work); | |
563 | struct sk_buff *skb; | |
564 | struct sock *sk; | |
565 | struct nfc_llcp_sock *llcp_sock; | |
566 | ||
567 | skb = skb_dequeue(&local->tx_queue); | |
568 | if (skb != NULL) { | |
569 | sk = skb->sk; | |
570 | llcp_sock = nfc_llcp_sock(sk); | |
571 | if (llcp_sock != NULL) { | |
572 | int ret; | |
573 | ||
574 | pr_debug("Sending pending skb\n"); | |
575 | print_hex_dump(KERN_DEBUG, "LLCP Tx: ", | |
576 | DUMP_PREFIX_OFFSET, 16, 1, | |
577 | skb->data, skb->len, true); | |
578 | ||
579 | ret = nfc_data_exchange(local->dev, local->target_idx, | |
580 | skb, nfc_llcp_recv, local); | |
581 | ||
582 | if (!ret && nfc_llcp_ptype(skb) == LLCP_PDU_I) { | |
583 | skb = skb_get(skb); | |
584 | skb_queue_tail(&llcp_sock->tx_pending_queue, | |
585 | skb); | |
586 | } | |
587 | } else { | |
588 | nfc_llcp_send_symm(local->dev); | |
589 | } | |
590 | } else { | |
591 | nfc_llcp_send_symm(local->dev); | |
592 | } | |
593 | ||
594 | mod_timer(&local->link_timer, | |
595 | jiffies + msecs_to_jiffies(2 * local->remote_lto)); | |
596 | } | |
597 | ||
a69f32af SO |
598 | static struct nfc_llcp_sock *nfc_llcp_connecting_sock_get(struct nfc_llcp_local *local, |
599 | u8 ssap) | |
600 | { | |
601 | struct sock *sk; | |
602 | struct nfc_llcp_sock *llcp_sock; | |
603 | struct hlist_node *node; | |
604 | ||
605 | read_lock(&local->connecting_sockets.lock); | |
606 | ||
607 | sk_for_each(sk, node, &local->connecting_sockets.head) { | |
608 | llcp_sock = nfc_llcp_sock(sk); | |
609 | ||
5a0f6f3b SO |
610 | if (llcp_sock->ssap == ssap) { |
611 | sock_hold(&llcp_sock->sk); | |
a69f32af | 612 | goto out; |
5a0f6f3b | 613 | } |
a69f32af SO |
614 | } |
615 | ||
616 | llcp_sock = NULL; | |
617 | ||
618 | out: | |
619 | read_unlock(&local->connecting_sockets.lock); | |
620 | ||
a69f32af SO |
621 | return llcp_sock; |
622 | } | |
623 | ||
a69f32af SO |
624 | static struct nfc_llcp_sock *nfc_llcp_sock_get_sn(struct nfc_llcp_local *local, |
625 | u8 *sn, size_t sn_len) | |
626 | { | |
a69f32af SO |
627 | struct nfc_llcp_sock *llcp_sock; |
628 | ||
8f50020e | 629 | llcp_sock = nfc_llcp_sock_from_sn(local, sn, sn_len); |
d646960f | 630 | |
a69f32af SO |
631 | if (llcp_sock == NULL) |
632 | return NULL; | |
d646960f | 633 | |
a69f32af SO |
634 | sock_hold(&llcp_sock->sk); |
635 | ||
636 | return llcp_sock; | |
d646960f SO |
637 | } |
638 | ||
d646960f SO |
639 | static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len) |
640 | { | |
641 | u8 *tlv = &skb->data[2], type, length; | |
642 | size_t tlv_array_len = skb->len - LLCP_HEADER_SIZE, offset = 0; | |
643 | ||
644 | while (offset < tlv_array_len) { | |
645 | type = tlv[0]; | |
646 | length = tlv[1]; | |
647 | ||
648 | pr_debug("type 0x%x length %d\n", type, length); | |
649 | ||
650 | if (type == LLCP_TLV_SN) { | |
651 | *sn_len = length; | |
652 | return &tlv[2]; | |
653 | } | |
654 | ||
655 | offset += length + 2; | |
656 | tlv += length + 2; | |
657 | } | |
658 | ||
659 | return NULL; | |
660 | } | |
661 | ||
662 | static void nfc_llcp_recv_connect(struct nfc_llcp_local *local, | |
427a2eb1 | 663 | struct sk_buff *skb) |
d646960f SO |
664 | { |
665 | struct sock *new_sk, *parent; | |
666 | struct nfc_llcp_sock *sock, *new_sock; | |
a69f32af | 667 | u8 dsap, ssap, reason; |
d646960f SO |
668 | |
669 | dsap = nfc_llcp_dsap(skb); | |
670 | ssap = nfc_llcp_ssap(skb); | |
671 | ||
672 | pr_debug("%d %d\n", dsap, ssap); | |
673 | ||
d646960f | 674 | if (dsap != LLCP_SAP_SDP) { |
a69f32af SO |
675 | sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP); |
676 | if (sock == NULL || sock->sk.sk_state != LLCP_LISTEN) { | |
d646960f SO |
677 | reason = LLCP_DM_NOBOUND; |
678 | goto fail; | |
679 | } | |
d646960f SO |
680 | } else { |
681 | u8 *sn; | |
682 | size_t sn_len; | |
683 | ||
684 | sn = nfc_llcp_connect_sn(skb, &sn_len); | |
685 | if (sn == NULL) { | |
686 | reason = LLCP_DM_NOBOUND; | |
687 | goto fail; | |
688 | } | |
689 | ||
690 | pr_debug("Service name length %zu\n", sn_len); | |
691 | ||
a69f32af SO |
692 | sock = nfc_llcp_sock_get_sn(local, sn, sn_len); |
693 | if (sock == NULL) { | |
694 | reason = LLCP_DM_NOBOUND; | |
695 | goto fail; | |
d646960f | 696 | } |
d646960f SO |
697 | } |
698 | ||
a69f32af | 699 | lock_sock(&sock->sk); |
d646960f | 700 | |
d646960f SO |
701 | parent = &sock->sk; |
702 | ||
703 | if (sk_acceptq_is_full(parent)) { | |
704 | reason = LLCP_DM_REJ; | |
705 | release_sock(&sock->sk); | |
706 | sock_put(&sock->sk); | |
707 | goto fail; | |
708 | } | |
709 | ||
8f50020e SO |
710 | if (sock->ssap == LLCP_SDP_UNBOUND) { |
711 | u8 ssap = nfc_llcp_reserve_sdp_ssap(local); | |
712 | ||
713 | pr_debug("First client, reserving %d\n", ssap); | |
714 | ||
715 | if (ssap == LLCP_SAP_MAX) { | |
716 | reason = LLCP_DM_REJ; | |
717 | release_sock(&sock->sk); | |
718 | sock_put(&sock->sk); | |
719 | goto fail; | |
720 | } | |
721 | ||
722 | sock->ssap = ssap; | |
723 | } | |
724 | ||
427a2eb1 | 725 | new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, GFP_ATOMIC); |
d646960f SO |
726 | if (new_sk == NULL) { |
727 | reason = LLCP_DM_REJ; | |
728 | release_sock(&sock->sk); | |
729 | sock_put(&sock->sk); | |
730 | goto fail; | |
731 | } | |
732 | ||
733 | new_sock = nfc_llcp_sock(new_sk); | |
734 | new_sock->dev = local->dev; | |
c7aa1225 | 735 | new_sock->local = nfc_llcp_local_get(local); |
93d7e490 | 736 | new_sock->miu = local->remote_miu; |
d646960f | 737 | new_sock->nfc_protocol = sock->nfc_protocol; |
d646960f | 738 | new_sock->dsap = ssap; |
025f1520 | 739 | new_sock->target_idx = local->target_idx; |
d646960f | 740 | new_sock->parent = parent; |
8f50020e SO |
741 | new_sock->ssap = sock->ssap; |
742 | if (sock->ssap < LLCP_LOCAL_NUM_SAP && sock->ssap >= LLCP_WKS_NUM_SAP) { | |
743 | atomic_t *client_count; | |
744 | ||
745 | pr_debug("reserved_ssap %d for %p\n", sock->ssap, new_sock); | |
746 | ||
747 | client_count = | |
748 | &local->local_sdp_cnt[sock->ssap - LLCP_WKS_NUM_SAP]; | |
749 | ||
750 | atomic_inc(client_count); | |
751 | new_sock->reserved_ssap = sock->ssap; | |
752 | } | |
d646960f | 753 | |
7a06e586 SO |
754 | nfc_llcp_parse_connection_tlv(new_sock, &skb->data[LLCP_HEADER_SIZE], |
755 | skb->len - LLCP_HEADER_SIZE); | |
756 | ||
d646960f SO |
757 | pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk); |
758 | ||
a69f32af | 759 | nfc_llcp_sock_link(&local->sockets, new_sk); |
d646960f SO |
760 | |
761 | nfc_llcp_accept_enqueue(&sock->sk, new_sk); | |
762 | ||
763 | nfc_get_device(local->dev->idx); | |
764 | ||
765 | new_sk->sk_state = LLCP_CONNECTED; | |
766 | ||
767 | /* Wake the listening processes */ | |
768 | parent->sk_data_ready(parent, 0); | |
769 | ||
770 | /* Send CC */ | |
771 | nfc_llcp_send_cc(new_sock); | |
772 | ||
773 | release_sock(&sock->sk); | |
774 | sock_put(&sock->sk); | |
775 | ||
776 | return; | |
777 | ||
778 | fail: | |
779 | /* Send DM */ | |
780 | nfc_llcp_send_dm(local, dsap, ssap, reason); | |
781 | ||
782 | return; | |
783 | ||
784 | } | |
785 | ||
d094afa1 | 786 | int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock) |
4722d2b7 | 787 | { |
d094afa1 | 788 | int nr_frames = 0; |
4722d2b7 SO |
789 | struct nfc_llcp_local *local = sock->local; |
790 | ||
791 | pr_debug("Remote ready %d tx queue len %d remote rw %d", | |
427a2eb1 | 792 | sock->remote_ready, skb_queue_len(&sock->tx_pending_queue), |
7a06e586 | 793 | sock->rw); |
4722d2b7 SO |
794 | |
795 | /* Try to queue some I frames for transmission */ | |
796 | while (sock->remote_ready && | |
7a06e586 | 797 | skb_queue_len(&sock->tx_pending_queue) < sock->rw) { |
84457960 | 798 | struct sk_buff *pdu; |
4722d2b7 SO |
799 | |
800 | pdu = skb_dequeue(&sock->tx_queue); | |
801 | if (pdu == NULL) | |
802 | break; | |
803 | ||
804 | /* Update N(S)/N(R) */ | |
805 | nfc_llcp_set_nrns(sock, pdu); | |
806 | ||
4722d2b7 | 807 | skb_queue_tail(&local->tx_queue, pdu); |
d094afa1 | 808 | nr_frames++; |
4722d2b7 | 809 | } |
d094afa1 SO |
810 | |
811 | return nr_frames; | |
4722d2b7 SO |
812 | } |
813 | ||
d646960f | 814 | static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local, |
427a2eb1 | 815 | struct sk_buff *skb) |
d646960f SO |
816 | { |
817 | struct nfc_llcp_sock *llcp_sock; | |
818 | struct sock *sk; | |
819 | u8 dsap, ssap, ptype, ns, nr; | |
820 | ||
821 | ptype = nfc_llcp_ptype(skb); | |
822 | dsap = nfc_llcp_dsap(skb); | |
823 | ssap = nfc_llcp_ssap(skb); | |
824 | ns = nfc_llcp_ns(skb); | |
825 | nr = nfc_llcp_nr(skb); | |
826 | ||
827 | pr_debug("%d %d R %d S %d\n", dsap, ssap, nr, ns); | |
828 | ||
829 | llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); | |
830 | if (llcp_sock == NULL) { | |
831 | nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); | |
832 | return; | |
833 | } | |
834 | ||
835 | sk = &llcp_sock->sk; | |
836 | lock_sock(sk); | |
837 | if (sk->sk_state == LLCP_CLOSED) { | |
838 | release_sock(sk); | |
839 | nfc_llcp_sock_put(llcp_sock); | |
840 | } | |
841 | ||
d646960f SO |
842 | /* Pass the payload upstream */ |
843 | if (ptype == LLCP_PDU_I) { | |
844 | pr_debug("I frame, queueing on %p\n", &llcp_sock->sk); | |
845 | ||
53aef920 SO |
846 | if (ns == llcp_sock->recv_n) |
847 | llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16; | |
848 | else | |
849 | pr_err("Received out of sequence I PDU\n"); | |
850 | ||
d646960f SO |
851 | skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE); |
852 | if (sock_queue_rcv_skb(&llcp_sock->sk, skb)) { | |
853 | pr_err("receive queue is full\n"); | |
854 | skb_queue_head(&llcp_sock->tx_backlog_queue, skb); | |
855 | } | |
856 | } | |
857 | ||
858 | /* Remove skbs from the pending queue */ | |
859 | if (llcp_sock->send_ack_n != nr) { | |
860 | struct sk_buff *s, *tmp; | |
861 | ||
862 | llcp_sock->send_ack_n = nr; | |
863 | ||
84457960 SO |
864 | /* Remove and free all skbs until ns == nr */ |
865 | skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp) { | |
866 | skb_unlink(s, &llcp_sock->tx_pending_queue); | |
867 | kfree_skb(s); | |
868 | ||
869 | if (nfc_llcp_ns(s) == nr) | |
870 | break; | |
871 | } | |
872 | ||
873 | /* Re-queue the remaining skbs for transmission */ | |
874 | skb_queue_reverse_walk_safe(&llcp_sock->tx_pending_queue, | |
875 | s, tmp) { | |
876 | skb_unlink(s, &llcp_sock->tx_pending_queue); | |
877 | skb_queue_head(&local->tx_queue, s); | |
878 | } | |
d646960f SO |
879 | } |
880 | ||
53aef920 SO |
881 | if (ptype == LLCP_PDU_RR) |
882 | llcp_sock->remote_ready = true; | |
427a2eb1 | 883 | else if (ptype == LLCP_PDU_RNR) |
53aef920 SO |
884 | llcp_sock->remote_ready = false; |
885 | ||
56af2568 | 886 | if (nfc_llcp_queue_i_frames(llcp_sock) == 0 && ptype == LLCP_PDU_I) |
d094afa1 | 887 | nfc_llcp_send_rr(llcp_sock); |
d646960f SO |
888 | |
889 | release_sock(sk); | |
890 | nfc_llcp_sock_put(llcp_sock); | |
891 | } | |
892 | ||
893 | static void nfc_llcp_recv_disc(struct nfc_llcp_local *local, | |
427a2eb1 | 894 | struct sk_buff *skb) |
d646960f SO |
895 | { |
896 | struct nfc_llcp_sock *llcp_sock; | |
897 | struct sock *sk; | |
898 | u8 dsap, ssap; | |
899 | ||
900 | dsap = nfc_llcp_dsap(skb); | |
901 | ssap = nfc_llcp_ssap(skb); | |
902 | ||
903 | llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); | |
904 | if (llcp_sock == NULL) { | |
905 | nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); | |
906 | return; | |
907 | } | |
908 | ||
909 | sk = &llcp_sock->sk; | |
910 | lock_sock(sk); | |
911 | if (sk->sk_state == LLCP_CLOSED) { | |
912 | release_sock(sk); | |
913 | nfc_llcp_sock_put(llcp_sock); | |
914 | } | |
915 | ||
d646960f SO |
916 | if (sk->sk_state == LLCP_CONNECTED) { |
917 | nfc_put_device(local->dev); | |
918 | sk->sk_state = LLCP_CLOSED; | |
919 | sk->sk_state_change(sk); | |
920 | } | |
921 | ||
922 | nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_DISC); | |
923 | ||
924 | release_sock(sk); | |
925 | nfc_llcp_sock_put(llcp_sock); | |
926 | } | |
927 | ||
427a2eb1 | 928 | static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb) |
d646960f SO |
929 | { |
930 | struct nfc_llcp_sock *llcp_sock; | |
ff353d86 | 931 | struct sock *sk; |
d646960f SO |
932 | u8 dsap, ssap; |
933 | ||
d646960f SO |
934 | dsap = nfc_llcp_dsap(skb); |
935 | ssap = nfc_llcp_ssap(skb); | |
936 | ||
a69f32af | 937 | llcp_sock = nfc_llcp_connecting_sock_get(local, dsap); |
d646960f SO |
938 | if (llcp_sock == NULL) { |
939 | pr_err("Invalid CC\n"); | |
940 | nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); | |
941 | ||
942 | return; | |
943 | } | |
944 | ||
ff353d86 | 945 | sk = &llcp_sock->sk; |
d646960f | 946 | |
a69f32af SO |
947 | /* Unlink from connecting and link to the client array */ |
948 | nfc_llcp_sock_unlink(&local->connecting_sockets, sk); | |
949 | nfc_llcp_sock_link(&local->sockets, sk); | |
950 | llcp_sock->dsap = ssap; | |
951 | ||
7a06e586 SO |
952 | nfc_llcp_parse_connection_tlv(llcp_sock, &skb->data[LLCP_HEADER_SIZE], |
953 | skb->len - LLCP_HEADER_SIZE); | |
d646960f | 954 | |
ff353d86 SO |
955 | sk->sk_state = LLCP_CONNECTED; |
956 | sk->sk_state_change(sk); | |
957 | ||
d646960f SO |
958 | nfc_llcp_sock_put(llcp_sock); |
959 | } | |
960 | ||
961 | static void nfc_llcp_rx_work(struct work_struct *work) | |
962 | { | |
963 | struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, | |
427a2eb1 | 964 | rx_work); |
d646960f SO |
965 | u8 dsap, ssap, ptype; |
966 | struct sk_buff *skb; | |
967 | ||
968 | skb = local->rx_pending; | |
969 | if (skb == NULL) { | |
970 | pr_debug("No pending SKB\n"); | |
971 | return; | |
972 | } | |
973 | ||
974 | ptype = nfc_llcp_ptype(skb); | |
975 | dsap = nfc_llcp_dsap(skb); | |
976 | ssap = nfc_llcp_ssap(skb); | |
977 | ||
978 | pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap); | |
979 | ||
4be646ec SO |
980 | if (ptype != LLCP_PDU_SYMM) |
981 | print_hex_dump(KERN_DEBUG, "LLCP Rx: ", DUMP_PREFIX_OFFSET, | |
982 | 16, 1, skb->data, skb->len, true); | |
983 | ||
d646960f SO |
984 | switch (ptype) { |
985 | case LLCP_PDU_SYMM: | |
986 | pr_debug("SYMM\n"); | |
987 | break; | |
988 | ||
989 | case LLCP_PDU_CONNECT: | |
990 | pr_debug("CONNECT\n"); | |
991 | nfc_llcp_recv_connect(local, skb); | |
992 | break; | |
993 | ||
994 | case LLCP_PDU_DISC: | |
995 | pr_debug("DISC\n"); | |
996 | nfc_llcp_recv_disc(local, skb); | |
997 | break; | |
998 | ||
999 | case LLCP_PDU_CC: | |
1000 | pr_debug("CC\n"); | |
1001 | nfc_llcp_recv_cc(local, skb); | |
1002 | break; | |
1003 | ||
1004 | case LLCP_PDU_I: | |
1005 | case LLCP_PDU_RR: | |
53aef920 | 1006 | case LLCP_PDU_RNR: |
d646960f SO |
1007 | pr_debug("I frame\n"); |
1008 | nfc_llcp_recv_hdlc(local, skb); | |
1009 | break; | |
1010 | ||
1011 | } | |
1012 | ||
1013 | queue_work(local->tx_wq, &local->tx_work); | |
1014 | kfree_skb(local->rx_pending); | |
1015 | local->rx_pending = NULL; | |
1016 | ||
1017 | return; | |
1018 | } | |
1019 | ||
1020 | void nfc_llcp_recv(void *data, struct sk_buff *skb, int err) | |
1021 | { | |
1022 | struct nfc_llcp_local *local = (struct nfc_llcp_local *) data; | |
1023 | ||
1024 | pr_debug("Received an LLCP PDU\n"); | |
1025 | if (err < 0) { | |
427a2eb1 | 1026 | pr_err("err %d\n", err); |
d646960f SO |
1027 | return; |
1028 | } | |
1029 | ||
1030 | local->rx_pending = skb_get(skb); | |
1031 | del_timer(&local->link_timer); | |
1032 | queue_work(local->rx_wq, &local->rx_work); | |
1033 | ||
1034 | return; | |
1035 | } | |
1036 | ||
73167ced SO |
1037 | int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb) |
1038 | { | |
1039 | struct nfc_llcp_local *local; | |
1040 | ||
1041 | local = nfc_llcp_find_local(dev); | |
1042 | if (local == NULL) | |
1043 | return -ENODEV; | |
1044 | ||
1045 | local->rx_pending = skb_get(skb); | |
1046 | del_timer(&local->link_timer); | |
1047 | queue_work(local->rx_wq, &local->rx_work); | |
1048 | ||
1049 | return 0; | |
1050 | } | |
1051 | ||
d646960f SO |
1052 | void nfc_llcp_mac_is_down(struct nfc_dev *dev) |
1053 | { | |
1054 | struct nfc_llcp_local *local; | |
1055 | ||
1056 | local = nfc_llcp_find_local(dev); | |
1057 | if (local == NULL) | |
1058 | return; | |
1059 | ||
1060 | /* Close and purge all existing sockets */ | |
4d22ea15 | 1061 | nfc_llcp_socket_release(local, true); |
d646960f SO |
1062 | } |
1063 | ||
1064 | void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx, | |
1065 | u8 comm_mode, u8 rf_mode) | |
1066 | { | |
1067 | struct nfc_llcp_local *local; | |
1068 | ||
1069 | pr_debug("rf mode %d\n", rf_mode); | |
1070 | ||
1071 | local = nfc_llcp_find_local(dev); | |
1072 | if (local == NULL) | |
1073 | return; | |
1074 | ||
1075 | local->target_idx = target_idx; | |
1076 | local->comm_mode = comm_mode; | |
1077 | local->rf_mode = rf_mode; | |
1078 | ||
1079 | if (rf_mode == NFC_RF_INITIATOR) { | |
1080 | pr_debug("Queueing Tx work\n"); | |
1081 | ||
1082 | queue_work(local->tx_wq, &local->tx_work); | |
1083 | } else { | |
1084 | mod_timer(&local->link_timer, | |
427a2eb1 | 1085 | jiffies + msecs_to_jiffies(local->remote_lto)); |
d646960f SO |
1086 | } |
1087 | } | |
1088 | ||
1089 | int nfc_llcp_register_device(struct nfc_dev *ndev) | |
1090 | { | |
1091 | struct device *dev = &ndev->dev; | |
1092 | struct nfc_llcp_local *local; | |
1093 | char name[32]; | |
1094 | int err; | |
1095 | ||
1096 | local = kzalloc(sizeof(struct nfc_llcp_local), GFP_KERNEL); | |
1097 | if (local == NULL) | |
1098 | return -ENOMEM; | |
1099 | ||
1100 | local->dev = ndev; | |
1101 | INIT_LIST_HEAD(&local->list); | |
c7aa1225 | 1102 | kref_init(&local->ref); |
d646960f | 1103 | mutex_init(&local->sdp_lock); |
d646960f SO |
1104 | init_timer(&local->link_timer); |
1105 | local->link_timer.data = (unsigned long) local; | |
1106 | local->link_timer.function = nfc_llcp_symm_timer; | |
1107 | ||
1108 | skb_queue_head_init(&local->tx_queue); | |
1109 | INIT_WORK(&local->tx_work, nfc_llcp_tx_work); | |
1110 | snprintf(name, sizeof(name), "%s_llcp_tx_wq", dev_name(dev)); | |
427a2eb1 SO |
1111 | local->tx_wq = |
1112 | alloc_workqueue(name, | |
1113 | WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, | |
1114 | 1); | |
d646960f SO |
1115 | if (local->tx_wq == NULL) { |
1116 | err = -ENOMEM; | |
1117 | goto err_local; | |
1118 | } | |
1119 | ||
1120 | local->rx_pending = NULL; | |
1121 | INIT_WORK(&local->rx_work, nfc_llcp_rx_work); | |
1122 | snprintf(name, sizeof(name), "%s_llcp_rx_wq", dev_name(dev)); | |
427a2eb1 SO |
1123 | local->rx_wq = |
1124 | alloc_workqueue(name, | |
1125 | WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, | |
1126 | 1); | |
d646960f SO |
1127 | if (local->rx_wq == NULL) { |
1128 | err = -ENOMEM; | |
1129 | goto err_tx_wq; | |
1130 | } | |
1131 | ||
1132 | INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work); | |
1133 | snprintf(name, sizeof(name), "%s_llcp_timeout_wq", dev_name(dev)); | |
427a2eb1 SO |
1134 | local->timeout_wq = |
1135 | alloc_workqueue(name, | |
1136 | WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, | |
1137 | 1); | |
d646960f SO |
1138 | if (local->timeout_wq == NULL) { |
1139 | err = -ENOMEM; | |
1140 | goto err_rx_wq; | |
1141 | } | |
1142 | ||
a69f32af SO |
1143 | local->sockets.lock = __RW_LOCK_UNLOCKED(local->sockets.lock); |
1144 | local->connecting_sockets.lock = __RW_LOCK_UNLOCKED(local->connecting_sockets.lock); | |
1145 | ||
d646960f SO |
1146 | nfc_llcp_build_gb(local); |
1147 | ||
1148 | local->remote_miu = LLCP_DEFAULT_MIU; | |
1149 | local->remote_lto = LLCP_DEFAULT_LTO; | |
d646960f SO |
1150 | |
1151 | list_add(&llcp_devices, &local->list); | |
1152 | ||
1153 | return 0; | |
1154 | ||
1155 | err_rx_wq: | |
1156 | destroy_workqueue(local->rx_wq); | |
1157 | ||
1158 | err_tx_wq: | |
1159 | destroy_workqueue(local->tx_wq); | |
1160 | ||
1161 | err_local: | |
1162 | kfree(local); | |
1163 | ||
1164 | return 0; | |
1165 | } | |
1166 | ||
1167 | void nfc_llcp_unregister_device(struct nfc_dev *dev) | |
1168 | { | |
1169 | struct nfc_llcp_local *local = nfc_llcp_find_local(dev); | |
1170 | ||
1171 | if (local == NULL) { | |
1172 | pr_debug("No such device\n"); | |
1173 | return; | |
1174 | } | |
1175 | ||
c7aa1225 | 1176 | nfc_llcp_local_put(local); |
d646960f SO |
1177 | } |
1178 | ||
1179 | int __init nfc_llcp_init(void) | |
1180 | { | |
1181 | INIT_LIST_HEAD(&llcp_devices); | |
1182 | ||
1183 | return nfc_llcp_sock_init(); | |
1184 | } | |
1185 | ||
1186 | void nfc_llcp_exit(void) | |
1187 | { | |
1188 | nfc_llcp_sock_exit(); | |
1189 | } |