Root/
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, see <http://www.gnu.org/licenses/>. |
16 | */ |
17 | |
18 | #define pr_fmt(fmt) "llcp: %s: " fmt, __func__ |
19 | |
20 | #include <linux/init.h> |
21 | #include <linux/kernel.h> |
22 | #include <linux/module.h> |
23 | #include <linux/nfc.h> |
24 | |
25 | #include <net/nfc/nfc.h> |
26 | |
27 | #include "nfc.h" |
28 | #include "llcp.h" |
29 | |
30 | static u8 llcp_tlv_length[LLCP_TLV_MAX] = { |
31 | 0, |
32 | 1, /* VERSION */ |
33 | 2, /* MIUX */ |
34 | 2, /* WKS */ |
35 | 1, /* LTO */ |
36 | 1, /* RW */ |
37 | 0, /* SN */ |
38 | 1, /* OPT */ |
39 | 0, /* SDREQ */ |
40 | 2, /* SDRES */ |
41 | |
42 | }; |
43 | |
44 | static u8 llcp_tlv8(u8 *tlv, u8 type) |
45 | { |
46 | if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]]) |
47 | return 0; |
48 | |
49 | return tlv[2]; |
50 | } |
51 | |
52 | static u16 llcp_tlv16(u8 *tlv, u8 type) |
53 | { |
54 | if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]]) |
55 | return 0; |
56 | |
57 | return be16_to_cpu(*((__be16 *)(tlv + 2))); |
58 | } |
59 | |
60 | |
61 | static u8 llcp_tlv_version(u8 *tlv) |
62 | { |
63 | return llcp_tlv8(tlv, LLCP_TLV_VERSION); |
64 | } |
65 | |
66 | static u16 llcp_tlv_miux(u8 *tlv) |
67 | { |
68 | return llcp_tlv16(tlv, LLCP_TLV_MIUX) & 0x7ff; |
69 | } |
70 | |
71 | static u16 llcp_tlv_wks(u8 *tlv) |
72 | { |
73 | return llcp_tlv16(tlv, LLCP_TLV_WKS); |
74 | } |
75 | |
76 | static u16 llcp_tlv_lto(u8 *tlv) |
77 | { |
78 | return llcp_tlv8(tlv, LLCP_TLV_LTO); |
79 | } |
80 | |
81 | static u8 llcp_tlv_opt(u8 *tlv) |
82 | { |
83 | return llcp_tlv8(tlv, LLCP_TLV_OPT); |
84 | } |
85 | |
86 | static u8 llcp_tlv_rw(u8 *tlv) |
87 | { |
88 | return llcp_tlv8(tlv, LLCP_TLV_RW) & 0xf; |
89 | } |
90 | |
91 | u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length) |
92 | { |
93 | u8 *tlv, length; |
94 | |
95 | pr_debug("type %d\n", type); |
96 | |
97 | if (type >= LLCP_TLV_MAX) |
98 | return NULL; |
99 | |
100 | length = llcp_tlv_length[type]; |
101 | if (length == 0 && value_length == 0) |
102 | return NULL; |
103 | else if (length == 0) |
104 | length = value_length; |
105 | |
106 | *tlv_length = 2 + length; |
107 | tlv = kzalloc(2 + length, GFP_KERNEL); |
108 | if (tlv == NULL) |
109 | return tlv; |
110 | |
111 | tlv[0] = type; |
112 | tlv[1] = length; |
113 | memcpy(tlv + 2, value, length); |
114 | |
115 | return tlv; |
116 | } |
117 | |
118 | struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdres_tlv(u8 tid, u8 sap) |
119 | { |
120 | struct nfc_llcp_sdp_tlv *sdres; |
121 | u8 value[2]; |
122 | |
123 | sdres = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL); |
124 | if (sdres == NULL) |
125 | return NULL; |
126 | |
127 | value[0] = tid; |
128 | value[1] = sap; |
129 | |
130 | sdres->tlv = nfc_llcp_build_tlv(LLCP_TLV_SDRES, value, 2, |
131 | &sdres->tlv_len); |
132 | if (sdres->tlv == NULL) { |
133 | kfree(sdres); |
134 | return NULL; |
135 | } |
136 | |
137 | sdres->tid = tid; |
138 | sdres->sap = sap; |
139 | |
140 | INIT_HLIST_NODE(&sdres->node); |
141 | |
142 | return sdres; |
143 | } |
144 | |
145 | struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdreq_tlv(u8 tid, char *uri, |
146 | size_t uri_len) |
147 | { |
148 | struct nfc_llcp_sdp_tlv *sdreq; |
149 | |
150 | pr_debug("uri: %s, len: %zu\n", uri, uri_len); |
151 | |
152 | sdreq = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL); |
153 | if (sdreq == NULL) |
154 | return NULL; |
155 | |
156 | sdreq->tlv_len = uri_len + 3; |
157 | |
158 | if (uri[uri_len - 1] == 0) |
159 | sdreq->tlv_len--; |
160 | |
161 | sdreq->tlv = kzalloc(sdreq->tlv_len + 1, GFP_KERNEL); |
162 | if (sdreq->tlv == NULL) { |
163 | kfree(sdreq); |
164 | return NULL; |
165 | } |
166 | |
167 | sdreq->tlv[0] = LLCP_TLV_SDREQ; |
168 | sdreq->tlv[1] = sdreq->tlv_len - 2; |
169 | sdreq->tlv[2] = tid; |
170 | |
171 | sdreq->tid = tid; |
172 | sdreq->uri = sdreq->tlv + 3; |
173 | memcpy(sdreq->uri, uri, uri_len); |
174 | |
175 | sdreq->time = jiffies; |
176 | |
177 | INIT_HLIST_NODE(&sdreq->node); |
178 | |
179 | return sdreq; |
180 | } |
181 | |
182 | void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp) |
183 | { |
184 | kfree(sdp->tlv); |
185 | kfree(sdp); |
186 | } |
187 | |
188 | void nfc_llcp_free_sdp_tlv_list(struct hlist_head *head) |
189 | { |
190 | struct nfc_llcp_sdp_tlv *sdp; |
191 | struct hlist_node *n; |
192 | |
193 | hlist_for_each_entry_safe(sdp, n, head, node) { |
194 | hlist_del(&sdp->node); |
195 | |
196 | nfc_llcp_free_sdp_tlv(sdp); |
197 | } |
198 | } |
199 | |
200 | int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local *local, |
201 | u8 *tlv_array, u16 tlv_array_len) |
202 | { |
203 | u8 *tlv = tlv_array, type, length, offset = 0; |
204 | |
205 | pr_debug("TLV array length %d\n", tlv_array_len); |
206 | |
207 | if (local == NULL) |
208 | return -ENODEV; |
209 | |
210 | while (offset < tlv_array_len) { |
211 | type = tlv[0]; |
212 | length = tlv[1]; |
213 | |
214 | pr_debug("type 0x%x length %d\n", type, length); |
215 | |
216 | switch (type) { |
217 | case LLCP_TLV_VERSION: |
218 | local->remote_version = llcp_tlv_version(tlv); |
219 | break; |
220 | case LLCP_TLV_MIUX: |
221 | local->remote_miu = llcp_tlv_miux(tlv) + 128; |
222 | break; |
223 | case LLCP_TLV_WKS: |
224 | local->remote_wks = llcp_tlv_wks(tlv); |
225 | break; |
226 | case LLCP_TLV_LTO: |
227 | local->remote_lto = llcp_tlv_lto(tlv) * 10; |
228 | break; |
229 | case LLCP_TLV_OPT: |
230 | local->remote_opt = llcp_tlv_opt(tlv); |
231 | break; |
232 | default: |
233 | pr_err("Invalid gt tlv value 0x%x\n", type); |
234 | break; |
235 | } |
236 | |
237 | offset += length + 2; |
238 | tlv += length + 2; |
239 | } |
240 | |
241 | pr_debug("version 0x%x miu %d lto %d opt 0x%x wks 0x%x\n", |
242 | local->remote_version, local->remote_miu, |
243 | local->remote_lto, local->remote_opt, |
244 | local->remote_wks); |
245 | |
246 | return 0; |
247 | } |
248 | |
249 | int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock, |
250 | u8 *tlv_array, u16 tlv_array_len) |
251 | { |
252 | u8 *tlv = tlv_array, type, length, offset = 0; |
253 | |
254 | pr_debug("TLV array length %d\n", tlv_array_len); |
255 | |
256 | if (sock == NULL) |
257 | return -ENOTCONN; |
258 | |
259 | while (offset < tlv_array_len) { |
260 | type = tlv[0]; |
261 | length = tlv[1]; |
262 | |
263 | pr_debug("type 0x%x length %d\n", type, length); |
264 | |
265 | switch (type) { |
266 | case LLCP_TLV_MIUX: |
267 | sock->remote_miu = llcp_tlv_miux(tlv) + 128; |
268 | break; |
269 | case LLCP_TLV_RW: |
270 | sock->remote_rw = llcp_tlv_rw(tlv); |
271 | break; |
272 | case LLCP_TLV_SN: |
273 | break; |
274 | default: |
275 | pr_err("Invalid gt tlv value 0x%x\n", type); |
276 | break; |
277 | } |
278 | |
279 | offset += length + 2; |
280 | tlv += length + 2; |
281 | } |
282 | |
283 | pr_debug("sock %p rw %d miu %d\n", sock, |
284 | sock->remote_rw, sock->remote_miu); |
285 | |
286 | return 0; |
287 | } |
288 | |
289 | static struct sk_buff *llcp_add_header(struct sk_buff *pdu, |
290 | u8 dsap, u8 ssap, u8 ptype) |
291 | { |
292 | u8 header[2]; |
293 | |
294 | pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap); |
295 | |
296 | header[0] = (u8)((dsap << 2) | (ptype >> 2)); |
297 | header[1] = (u8)((ptype << 6) | ssap); |
298 | |
299 | pr_debug("header 0x%x 0x%x\n", header[0], header[1]); |
300 | |
301 | memcpy(skb_put(pdu, LLCP_HEADER_SIZE), header, LLCP_HEADER_SIZE); |
302 | |
303 | return pdu; |
304 | } |
305 | |
306 | static struct sk_buff *llcp_add_tlv(struct sk_buff *pdu, u8 *tlv, |
307 | u8 tlv_length) |
308 | { |
309 | /* XXX Add an skb length check */ |
310 | |
311 | if (tlv == NULL) |
312 | return NULL; |
313 | |
314 | memcpy(skb_put(pdu, tlv_length), tlv, tlv_length); |
315 | |
316 | return pdu; |
317 | } |
318 | |
319 | static struct sk_buff *llcp_allocate_pdu(struct nfc_llcp_sock *sock, |
320 | u8 cmd, u16 size) |
321 | { |
322 | struct sk_buff *skb; |
323 | int err; |
324 | |
325 | if (sock->ssap == 0) |
326 | return NULL; |
327 | |
328 | skb = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT, |
329 | size + LLCP_HEADER_SIZE, &err); |
330 | if (skb == NULL) { |
331 | pr_err("Could not allocate PDU\n"); |
332 | return NULL; |
333 | } |
334 | |
335 | skb = llcp_add_header(skb, sock->dsap, sock->ssap, cmd); |
336 | |
337 | return skb; |
338 | } |
339 | |
340 | int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock) |
341 | { |
342 | struct sk_buff *skb; |
343 | struct nfc_dev *dev; |
344 | struct nfc_llcp_local *local; |
345 | |
346 | pr_debug("Sending DISC\n"); |
347 | |
348 | local = sock->local; |
349 | if (local == NULL) |
350 | return -ENODEV; |
351 | |
352 | dev = sock->dev; |
353 | if (dev == NULL) |
354 | return -ENODEV; |
355 | |
356 | skb = llcp_allocate_pdu(sock, LLCP_PDU_DISC, 0); |
357 | if (skb == NULL) |
358 | return -ENOMEM; |
359 | |
360 | skb_queue_tail(&local->tx_queue, skb); |
361 | |
362 | return 0; |
363 | } |
364 | |
365 | int nfc_llcp_send_symm(struct nfc_dev *dev) |
366 | { |
367 | struct sk_buff *skb; |
368 | struct nfc_llcp_local *local; |
369 | u16 size = 0; |
370 | |
371 | pr_debug("Sending SYMM\n"); |
372 | |
373 | local = nfc_llcp_find_local(dev); |
374 | if (local == NULL) |
375 | return -ENODEV; |
376 | |
377 | size += LLCP_HEADER_SIZE; |
378 | size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE; |
379 | |
380 | skb = alloc_skb(size, GFP_KERNEL); |
381 | if (skb == NULL) |
382 | return -ENOMEM; |
383 | |
384 | skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE); |
385 | |
386 | skb = llcp_add_header(skb, 0, 0, LLCP_PDU_SYMM); |
387 | |
388 | __net_timestamp(skb); |
389 | |
390 | nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_TX); |
391 | |
392 | return nfc_data_exchange(dev, local->target_idx, skb, |
393 | nfc_llcp_recv, local); |
394 | } |
395 | |
396 | int nfc_llcp_send_connect(struct nfc_llcp_sock *sock) |
397 | { |
398 | struct nfc_llcp_local *local; |
399 | struct sk_buff *skb; |
400 | u8 *service_name_tlv = NULL, service_name_tlv_length; |
401 | u8 *miux_tlv = NULL, miux_tlv_length; |
402 | u8 *rw_tlv = NULL, rw_tlv_length, rw; |
403 | int err; |
404 | u16 size = 0, miux; |
405 | |
406 | pr_debug("Sending CONNECT\n"); |
407 | |
408 | local = sock->local; |
409 | if (local == NULL) |
410 | return -ENODEV; |
411 | |
412 | if (sock->service_name != NULL) { |
413 | service_name_tlv = nfc_llcp_build_tlv(LLCP_TLV_SN, |
414 | sock->service_name, |
415 | sock->service_name_len, |
416 | &service_name_tlv_length); |
417 | size += service_name_tlv_length; |
418 | } |
419 | |
420 | /* If the socket parameters are not set, use the local ones */ |
421 | miux = be16_to_cpu(sock->miux) > LLCP_MAX_MIUX ? |
422 | local->miux : sock->miux; |
423 | rw = sock->rw > LLCP_MAX_RW ? local->rw : sock->rw; |
424 | |
425 | miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0, |
426 | &miux_tlv_length); |
427 | size += miux_tlv_length; |
428 | |
429 | rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length); |
430 | size += rw_tlv_length; |
431 | |
432 | pr_debug("SKB size %d SN length %zu\n", size, sock->service_name_len); |
433 | |
434 | skb = llcp_allocate_pdu(sock, LLCP_PDU_CONNECT, size); |
435 | if (skb == NULL) { |
436 | err = -ENOMEM; |
437 | goto error_tlv; |
438 | } |
439 | |
440 | if (service_name_tlv != NULL) |
441 | skb = llcp_add_tlv(skb, service_name_tlv, |
442 | service_name_tlv_length); |
443 | |
444 | skb = llcp_add_tlv(skb, miux_tlv, miux_tlv_length); |
445 | skb = llcp_add_tlv(skb, rw_tlv, rw_tlv_length); |
446 | |
447 | skb_queue_tail(&local->tx_queue, skb); |
448 | |
449 | return 0; |
450 | |
451 | error_tlv: |
452 | pr_err("error %d\n", err); |
453 | |
454 | kfree(service_name_tlv); |
455 | kfree(miux_tlv); |
456 | kfree(rw_tlv); |
457 | |
458 | return err; |
459 | } |
460 | |
461 | int nfc_llcp_send_cc(struct nfc_llcp_sock *sock) |
462 | { |
463 | struct nfc_llcp_local *local; |
464 | struct sk_buff *skb; |
465 | u8 *miux_tlv = NULL, miux_tlv_length; |
466 | u8 *rw_tlv = NULL, rw_tlv_length, rw; |
467 | int err; |
468 | u16 size = 0, miux; |
469 | |
470 | pr_debug("Sending CC\n"); |
471 | |
472 | local = sock->local; |
473 | if (local == NULL) |
474 | return -ENODEV; |
475 | |
476 | /* If the socket parameters are not set, use the local ones */ |
477 | miux = be16_to_cpu(sock->miux) > LLCP_MAX_MIUX ? |
478 | local->miux : sock->miux; |
479 | rw = sock->rw > LLCP_MAX_RW ? local->rw : sock->rw; |
480 | |
481 | miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0, |
482 | &miux_tlv_length); |
483 | size += miux_tlv_length; |
484 | |
485 | rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length); |
486 | size += rw_tlv_length; |
487 | |
488 | skb = llcp_allocate_pdu(sock, LLCP_PDU_CC, size); |
489 | if (skb == NULL) { |
490 | err = -ENOMEM; |
491 | goto error_tlv; |
492 | } |
493 | |
494 | skb = llcp_add_tlv(skb, miux_tlv, miux_tlv_length); |
495 | skb = llcp_add_tlv(skb, rw_tlv, rw_tlv_length); |
496 | |
497 | skb_queue_tail(&local->tx_queue, skb); |
498 | |
499 | return 0; |
500 | |
501 | error_tlv: |
502 | pr_err("error %d\n", err); |
503 | |
504 | kfree(miux_tlv); |
505 | kfree(rw_tlv); |
506 | |
507 | return err; |
508 | } |
509 | |
510 | static struct sk_buff *nfc_llcp_allocate_snl(struct nfc_llcp_local *local, |
511 | size_t tlv_length) |
512 | { |
513 | struct sk_buff *skb; |
514 | struct nfc_dev *dev; |
515 | u16 size = 0; |
516 | |
517 | if (local == NULL) |
518 | return ERR_PTR(-ENODEV); |
519 | |
520 | dev = local->dev; |
521 | if (dev == NULL) |
522 | return ERR_PTR(-ENODEV); |
523 | |
524 | size += LLCP_HEADER_SIZE; |
525 | size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE; |
526 | size += tlv_length; |
527 | |
528 | skb = alloc_skb(size, GFP_KERNEL); |
529 | if (skb == NULL) |
530 | return ERR_PTR(-ENOMEM); |
531 | |
532 | skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE); |
533 | |
534 | skb = llcp_add_header(skb, LLCP_SAP_SDP, LLCP_SAP_SDP, LLCP_PDU_SNL); |
535 | |
536 | return skb; |
537 | } |
538 | |
539 | int nfc_llcp_send_snl_sdres(struct nfc_llcp_local *local, |
540 | struct hlist_head *tlv_list, size_t tlvs_len) |
541 | { |
542 | struct nfc_llcp_sdp_tlv *sdp; |
543 | struct hlist_node *n; |
544 | struct sk_buff *skb; |
545 | |
546 | skb = nfc_llcp_allocate_snl(local, tlvs_len); |
547 | if (IS_ERR(skb)) |
548 | return PTR_ERR(skb); |
549 | |
550 | hlist_for_each_entry_safe(sdp, n, tlv_list, node) { |
551 | memcpy(skb_put(skb, sdp->tlv_len), sdp->tlv, sdp->tlv_len); |
552 | |
553 | hlist_del(&sdp->node); |
554 | |
555 | nfc_llcp_free_sdp_tlv(sdp); |
556 | } |
557 | |
558 | skb_queue_tail(&local->tx_queue, skb); |
559 | |
560 | return 0; |
561 | } |
562 | |
563 | int nfc_llcp_send_snl_sdreq(struct nfc_llcp_local *local, |
564 | struct hlist_head *tlv_list, size_t tlvs_len) |
565 | { |
566 | struct nfc_llcp_sdp_tlv *sdreq; |
567 | struct hlist_node *n; |
568 | struct sk_buff *skb; |
569 | |
570 | skb = nfc_llcp_allocate_snl(local, tlvs_len); |
571 | if (IS_ERR(skb)) |
572 | return PTR_ERR(skb); |
573 | |
574 | mutex_lock(&local->sdreq_lock); |
575 | |
576 | if (hlist_empty(&local->pending_sdreqs)) |
577 | mod_timer(&local->sdreq_timer, |
578 | jiffies + msecs_to_jiffies(3 * local->remote_lto)); |
579 | |
580 | hlist_for_each_entry_safe(sdreq, n, tlv_list, node) { |
581 | pr_debug("tid %d for %s\n", sdreq->tid, sdreq->uri); |
582 | |
583 | memcpy(skb_put(skb, sdreq->tlv_len), sdreq->tlv, |
584 | sdreq->tlv_len); |
585 | |
586 | hlist_del(&sdreq->node); |
587 | |
588 | hlist_add_head(&sdreq->node, &local->pending_sdreqs); |
589 | } |
590 | |
591 | mutex_unlock(&local->sdreq_lock); |
592 | |
593 | skb_queue_tail(&local->tx_queue, skb); |
594 | |
595 | return 0; |
596 | } |
597 | |
598 | int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason) |
599 | { |
600 | struct sk_buff *skb; |
601 | struct nfc_dev *dev; |
602 | u16 size = 1; /* Reason code */ |
603 | |
604 | pr_debug("Sending DM reason 0x%x\n", reason); |
605 | |
606 | if (local == NULL) |
607 | return -ENODEV; |
608 | |
609 | dev = local->dev; |
610 | if (dev == NULL) |
611 | return -ENODEV; |
612 | |
613 | size += LLCP_HEADER_SIZE; |
614 | size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE; |
615 | |
616 | skb = alloc_skb(size, GFP_KERNEL); |
617 | if (skb == NULL) |
618 | return -ENOMEM; |
619 | |
620 | skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE); |
621 | |
622 | skb = llcp_add_header(skb, dsap, ssap, LLCP_PDU_DM); |
623 | |
624 | memcpy(skb_put(skb, 1), &reason, 1); |
625 | |
626 | skb_queue_head(&local->tx_queue, skb); |
627 | |
628 | return 0; |
629 | } |
630 | |
631 | int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock, |
632 | struct msghdr *msg, size_t len) |
633 | { |
634 | struct sk_buff *pdu; |
635 | struct sock *sk = &sock->sk; |
636 | struct nfc_llcp_local *local; |
637 | size_t frag_len = 0, remaining_len; |
638 | u8 *msg_data, *msg_ptr; |
639 | u16 remote_miu; |
640 | |
641 | pr_debug("Send I frame len %zd\n", len); |
642 | |
643 | local = sock->local; |
644 | if (local == NULL) |
645 | return -ENODEV; |
646 | |
647 | /* Remote is ready but has not acknowledged our frames */ |
648 | if((sock->remote_ready && |
649 | skb_queue_len(&sock->tx_pending_queue) >= sock->remote_rw && |
650 | skb_queue_len(&sock->tx_queue) >= 2 * sock->remote_rw)) { |
651 | pr_err("Pending queue is full %d frames\n", |
652 | skb_queue_len(&sock->tx_pending_queue)); |
653 | return -ENOBUFS; |
654 | } |
655 | |
656 | /* Remote is not ready and we've been queueing enough frames */ |
657 | if ((!sock->remote_ready && |
658 | skb_queue_len(&sock->tx_queue) >= 2 * sock->remote_rw)) { |
659 | pr_err("Tx queue is full %d frames\n", |
660 | skb_queue_len(&sock->tx_queue)); |
661 | return -ENOBUFS; |
662 | } |
663 | |
664 | msg_data = kzalloc(len, GFP_KERNEL); |
665 | if (msg_data == NULL) |
666 | return -ENOMEM; |
667 | |
668 | if (memcpy_fromiovec(msg_data, msg->msg_iov, len)) { |
669 | kfree(msg_data); |
670 | return -EFAULT; |
671 | } |
672 | |
673 | remaining_len = len; |
674 | msg_ptr = msg_data; |
675 | |
676 | do { |
677 | remote_miu = sock->remote_miu > LLCP_MAX_MIU ? |
678 | LLCP_DEFAULT_MIU : sock->remote_miu; |
679 | |
680 | frag_len = min_t(size_t, remote_miu, remaining_len); |
681 | |
682 | pr_debug("Fragment %zd bytes remaining %zd", |
683 | frag_len, remaining_len); |
684 | |
685 | pdu = llcp_allocate_pdu(sock, LLCP_PDU_I, |
686 | frag_len + LLCP_SEQUENCE_SIZE); |
687 | if (pdu == NULL) { |
688 | kfree(msg_data); |
689 | return -ENOMEM; |
690 | } |
691 | |
692 | skb_put(pdu, LLCP_SEQUENCE_SIZE); |
693 | |
694 | if (likely(frag_len > 0)) |
695 | memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len); |
696 | |
697 | skb_queue_tail(&sock->tx_queue, pdu); |
698 | |
699 | lock_sock(sk); |
700 | |
701 | nfc_llcp_queue_i_frames(sock); |
702 | |
703 | release_sock(sk); |
704 | |
705 | remaining_len -= frag_len; |
706 | msg_ptr += frag_len; |
707 | } while (remaining_len > 0); |
708 | |
709 | kfree(msg_data); |
710 | |
711 | return len; |
712 | } |
713 | |
714 | int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap, |
715 | struct msghdr *msg, size_t len) |
716 | { |
717 | struct sk_buff *pdu; |
718 | struct nfc_llcp_local *local; |
719 | size_t frag_len = 0, remaining_len; |
720 | u8 *msg_ptr, *msg_data; |
721 | u16 remote_miu; |
722 | int err; |
723 | |
724 | pr_debug("Send UI frame len %zd\n", len); |
725 | |
726 | local = sock->local; |
727 | if (local == NULL) |
728 | return -ENODEV; |
729 | |
730 | msg_data = kzalloc(len, GFP_KERNEL); |
731 | if (msg_data == NULL) |
732 | return -ENOMEM; |
733 | |
734 | if (memcpy_fromiovec(msg_data, msg->msg_iov, len)) { |
735 | kfree(msg_data); |
736 | return -EFAULT; |
737 | } |
738 | |
739 | remaining_len = len; |
740 | msg_ptr = msg_data; |
741 | |
742 | do { |
743 | remote_miu = sock->remote_miu > LLCP_MAX_MIU ? |
744 | local->remote_miu : sock->remote_miu; |
745 | |
746 | frag_len = min_t(size_t, remote_miu, remaining_len); |
747 | |
748 | pr_debug("Fragment %zd bytes remaining %zd", |
749 | frag_len, remaining_len); |
750 | |
751 | pdu = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT, |
752 | frag_len + LLCP_HEADER_SIZE, &err); |
753 | if (pdu == NULL) { |
754 | pr_err("Could not allocate PDU\n"); |
755 | continue; |
756 | } |
757 | |
758 | pdu = llcp_add_header(pdu, dsap, ssap, LLCP_PDU_UI); |
759 | |
760 | if (likely(frag_len > 0)) |
761 | memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len); |
762 | |
763 | /* No need to check for the peer RW for UI frames */ |
764 | skb_queue_tail(&local->tx_queue, pdu); |
765 | |
766 | remaining_len -= frag_len; |
767 | msg_ptr += frag_len; |
768 | } while (remaining_len > 0); |
769 | |
770 | kfree(msg_data); |
771 | |
772 | return len; |
773 | } |
774 | |
775 | int nfc_llcp_send_rr(struct nfc_llcp_sock *sock) |
776 | { |
777 | struct sk_buff *skb; |
778 | struct nfc_llcp_local *local; |
779 | |
780 | pr_debug("Send rr nr %d\n", sock->recv_n); |
781 | |
782 | local = sock->local; |
783 | if (local == NULL) |
784 | return -ENODEV; |
785 | |
786 | skb = llcp_allocate_pdu(sock, LLCP_PDU_RR, LLCP_SEQUENCE_SIZE); |
787 | if (skb == NULL) |
788 | return -ENOMEM; |
789 | |
790 | skb_put(skb, LLCP_SEQUENCE_SIZE); |
791 | |
792 | skb->data[2] = sock->recv_n; |
793 | |
794 | skb_queue_head(&local->tx_queue, skb); |
795 | |
796 | return 0; |
797 | } |
798 |
Branches:
ben-wpan
ben-wpan-stefan
javiroman/ks7010
jz-2.6.34
jz-2.6.34-rc5
jz-2.6.34-rc6
jz-2.6.34-rc7
jz-2.6.35
jz-2.6.36
jz-2.6.37
jz-2.6.38
jz-2.6.39
jz-3.0
jz-3.1
jz-3.11
jz-3.12
jz-3.13
jz-3.15
jz-3.16
jz-3.18-dt
jz-3.2
jz-3.3
jz-3.4
jz-3.5
jz-3.6
jz-3.6-rc2-pwm
jz-3.9
jz-3.9-clk
jz-3.9-rc8
jz47xx
jz47xx-2.6.38
master
Tags:
od-2011-09-04
od-2011-09-18
v2.6.34-rc5
v2.6.34-rc6
v2.6.34-rc7
v3.9