2 * Copyright (C) 2011 Intel Corporation. All rights reserved.
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.
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.
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.
20 #define pr_fmt(fmt) "llcp: %s: " fmt, __func__
22 #include <linux/init.h>
23 #include <linux/kernel.h>
24 #include <linux/module.h>
25 #include <linux/nfc.h>
27 #include <net/nfc/nfc.h>
32 static u8 llcp_tlv_length
[LLCP_TLV_MAX
] = {
46 static u8
llcp_tlv8(u8
*tlv
, u8 type
)
48 if (tlv
[0] != type
|| tlv
[1] != llcp_tlv_length
[tlv
[0]])
54 static u16
llcp_tlv16(u8
*tlv
, u8 type
)
56 if (tlv
[0] != type
|| tlv
[1] != llcp_tlv_length
[tlv
[0]])
59 return be16_to_cpu(*((__be16
*)(tlv
+ 2)));
63 static u8
llcp_tlv_version(u8
*tlv
)
65 return llcp_tlv8(tlv
, LLCP_TLV_VERSION
);
68 static u16
llcp_tlv_miux(u8
*tlv
)
70 return llcp_tlv16(tlv
, LLCP_TLV_MIUX
) & 0x7ff;
73 static u16
llcp_tlv_wks(u8
*tlv
)
75 return llcp_tlv16(tlv
, LLCP_TLV_WKS
);
78 static u16
llcp_tlv_lto(u8
*tlv
)
80 return llcp_tlv8(tlv
, LLCP_TLV_LTO
);
83 static u8
llcp_tlv_opt(u8
*tlv
)
85 return llcp_tlv8(tlv
, LLCP_TLV_OPT
);
88 static u8
llcp_tlv_rw(u8
*tlv
)
90 return llcp_tlv8(tlv
, LLCP_TLV_RW
) & 0xf;
93 u8
*nfc_llcp_build_tlv(u8 type
, u8
*value
, u8 value_length
, u8
*tlv_length
)
97 pr_debug("type %d\n", type
);
99 if (type
>= LLCP_TLV_MAX
)
102 length
= llcp_tlv_length
[type
];
103 if (length
== 0 && value_length
== 0)
105 else if (length
== 0)
106 length
= value_length
;
108 *tlv_length
= 2 + length
;
109 tlv
= kzalloc(2 + length
, GFP_KERNEL
);
115 memcpy(tlv
+ 2, value
, length
);
120 int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local
*local
,
121 u8
*tlv_array
, u16 tlv_array_len
)
123 u8
*tlv
= tlv_array
, type
, length
, offset
= 0;
125 pr_debug("TLV array length %d\n", tlv_array_len
);
130 while (offset
< tlv_array_len
) {
134 pr_debug("type 0x%x length %d\n", type
, length
);
137 case LLCP_TLV_VERSION
:
138 local
->remote_version
= llcp_tlv_version(tlv
);
141 local
->remote_miu
= llcp_tlv_miux(tlv
) + 128;
144 local
->remote_wks
= llcp_tlv_wks(tlv
);
147 local
->remote_lto
= llcp_tlv_lto(tlv
) * 10;
150 local
->remote_opt
= llcp_tlv_opt(tlv
);
153 pr_err("Invalid gt tlv value 0x%x\n", type
);
157 offset
+= length
+ 2;
161 pr_debug("version 0x%x miu %d lto %d opt 0x%x wks 0x%x\n",
162 local
->remote_version
, local
->remote_miu
,
163 local
->remote_lto
, local
->remote_opt
,
169 int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock
*sock
,
170 u8
*tlv_array
, u16 tlv_array_len
)
172 u8
*tlv
= tlv_array
, type
, length
, offset
= 0;
174 pr_debug("TLV array length %d\n", tlv_array_len
);
179 while (offset
< tlv_array_len
) {
183 pr_debug("type 0x%x length %d\n", type
, length
);
187 sock
->miu
= llcp_tlv_miux(tlv
) + 128;
190 sock
->rw
= llcp_tlv_rw(tlv
);
195 pr_err("Invalid gt tlv value 0x%x\n", type
);
199 offset
+= length
+ 2;
203 pr_debug("sock %p rw %d miu %d\n", sock
, sock
->rw
, sock
->miu
);
208 static struct sk_buff
*llcp_add_header(struct sk_buff
*pdu
,
209 u8 dsap
, u8 ssap
, u8 ptype
)
213 pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype
, dsap
, ssap
);
215 header
[0] = (u8
)((dsap
<< 2) | (ptype
>> 2));
216 header
[1] = (u8
)((ptype
<< 6) | ssap
);
218 pr_debug("header 0x%x 0x%x\n", header
[0], header
[1]);
220 memcpy(skb_put(pdu
, LLCP_HEADER_SIZE
), header
, LLCP_HEADER_SIZE
);
225 static struct sk_buff
*llcp_add_tlv(struct sk_buff
*pdu
, u8
*tlv
,
228 /* XXX Add an skb length check */
233 memcpy(skb_put(pdu
, tlv_length
), tlv
, tlv_length
);
238 static struct sk_buff
*llcp_allocate_pdu(struct nfc_llcp_sock
*sock
,
247 skb
= nfc_alloc_send_skb(sock
->dev
, &sock
->sk
, MSG_DONTWAIT
,
248 size
+ LLCP_HEADER_SIZE
, &err
);
250 pr_err("Could not allocate PDU\n");
254 skb
= llcp_add_header(skb
, sock
->dsap
, sock
->ssap
, cmd
);
259 int nfc_llcp_disconnect(struct nfc_llcp_sock
*sock
)
263 struct nfc_llcp_local
*local
;
265 pr_debug("Sending DISC\n");
275 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_DISC
, 0);
279 skb_queue_tail(&local
->tx_queue
, skb
);
284 int nfc_llcp_send_symm(struct nfc_dev
*dev
)
287 struct nfc_llcp_local
*local
;
290 pr_debug("Sending SYMM\n");
292 local
= nfc_llcp_find_local(dev
);
296 size
+= LLCP_HEADER_SIZE
;
297 size
+= dev
->tx_headroom
+ dev
->tx_tailroom
+ NFC_HEADER_SIZE
;
299 skb
= alloc_skb(size
, GFP_KERNEL
);
303 skb_reserve(skb
, dev
->tx_headroom
+ NFC_HEADER_SIZE
);
305 skb
= llcp_add_header(skb
, 0, 0, LLCP_PDU_SYMM
);
307 __net_timestamp(skb
);
309 nfc_llcp_send_to_raw_sock(local
, skb
, NFC_LLCP_DIRECTION_TX
);
311 return nfc_data_exchange(dev
, local
->target_idx
, skb
,
312 nfc_llcp_recv
, local
);
315 int nfc_llcp_send_connect(struct nfc_llcp_sock
*sock
)
317 struct nfc_llcp_local
*local
;
319 u8
*service_name_tlv
= NULL
, service_name_tlv_length
;
320 u8
*miux_tlv
= NULL
, miux_tlv_length
;
321 u8
*rw_tlv
= NULL
, rw_tlv_length
;
325 pr_debug("Sending CONNECT\n");
331 if (sock
->service_name
!= NULL
) {
332 service_name_tlv
= nfc_llcp_build_tlv(LLCP_TLV_SN
,
334 sock
->service_name_len
,
335 &service_name_tlv_length
);
336 size
+= service_name_tlv_length
;
339 miux_tlv
= nfc_llcp_build_tlv(LLCP_TLV_MIUX
, (u8
*)&local
->miux
, 0,
341 size
+= miux_tlv_length
;
343 rw_tlv
= nfc_llcp_build_tlv(LLCP_TLV_RW
, &local
->rw
, 0, &rw_tlv_length
);
344 size
+= rw_tlv_length
;
346 pr_debug("SKB size %d SN length %zu\n", size
, sock
->service_name_len
);
348 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_CONNECT
, size
);
354 if (service_name_tlv
!= NULL
)
355 skb
= llcp_add_tlv(skb
, service_name_tlv
,
356 service_name_tlv_length
);
358 skb
= llcp_add_tlv(skb
, miux_tlv
, miux_tlv_length
);
359 skb
= llcp_add_tlv(skb
, rw_tlv
, rw_tlv_length
);
361 skb_queue_tail(&local
->tx_queue
, skb
);
366 pr_err("error %d\n", err
);
368 kfree(service_name_tlv
);
375 int nfc_llcp_send_cc(struct nfc_llcp_sock
*sock
)
377 struct nfc_llcp_local
*local
;
379 u8
*miux_tlv
= NULL
, miux_tlv_length
;
380 u8
*rw_tlv
= NULL
, rw_tlv_length
;
384 pr_debug("Sending CC\n");
390 miux_tlv
= nfc_llcp_build_tlv(LLCP_TLV_MIUX
, (u8
*)&local
->miux
, 0,
392 size
+= miux_tlv_length
;
394 rw_tlv
= nfc_llcp_build_tlv(LLCP_TLV_RW
, &local
->rw
, 0, &rw_tlv_length
);
395 size
+= rw_tlv_length
;
397 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_CC
, size
);
403 skb
= llcp_add_tlv(skb
, miux_tlv
, miux_tlv_length
);
404 skb
= llcp_add_tlv(skb
, rw_tlv
, rw_tlv_length
);
406 skb_queue_tail(&local
->tx_queue
, skb
);
411 pr_err("error %d\n", err
);
419 int nfc_llcp_send_snl(struct nfc_llcp_local
*local
, u8 tid
, u8 sap
)
423 u8
*sdres_tlv
= NULL
, sdres_tlv_length
, sdres
[2];
426 pr_debug("Sending SNL tid 0x%x sap 0x%x\n", tid
, sap
);
437 sdres_tlv
= nfc_llcp_build_tlv(LLCP_TLV_SDRES
, sdres
, 0,
439 if (sdres_tlv
== NULL
)
442 size
+= LLCP_HEADER_SIZE
;
443 size
+= dev
->tx_headroom
+ dev
->tx_tailroom
+ NFC_HEADER_SIZE
;
444 size
+= sdres_tlv_length
;
446 skb
= alloc_skb(size
, GFP_KERNEL
);
452 skb_reserve(skb
, dev
->tx_headroom
+ NFC_HEADER_SIZE
);
454 skb
= llcp_add_header(skb
, LLCP_SAP_SDP
, LLCP_SAP_SDP
, LLCP_PDU_SNL
);
456 memcpy(skb_put(skb
, sdres_tlv_length
), sdres_tlv
, sdres_tlv_length
);
458 skb_queue_tail(&local
->tx_queue
, skb
);
465 int nfc_llcp_send_dm(struct nfc_llcp_local
*local
, u8 ssap
, u8 dsap
, u8 reason
)
469 u16 size
= 1; /* Reason code */
471 pr_debug("Sending DM reason 0x%x\n", reason
);
480 size
+= LLCP_HEADER_SIZE
;
481 size
+= dev
->tx_headroom
+ dev
->tx_tailroom
+ NFC_HEADER_SIZE
;
483 skb
= alloc_skb(size
, GFP_KERNEL
);
487 skb_reserve(skb
, dev
->tx_headroom
+ NFC_HEADER_SIZE
);
489 skb
= llcp_add_header(skb
, dsap
, ssap
, LLCP_PDU_DM
);
491 memcpy(skb_put(skb
, 1), &reason
, 1);
493 skb_queue_head(&local
->tx_queue
, skb
);
498 int nfc_llcp_send_disconnect(struct nfc_llcp_sock
*sock
)
501 struct nfc_llcp_local
*local
;
503 pr_debug("Send DISC\n");
509 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_DISC
, 0);
513 skb_queue_head(&local
->tx_queue
, skb
);
518 int nfc_llcp_send_i_frame(struct nfc_llcp_sock
*sock
,
519 struct msghdr
*msg
, size_t len
)
522 struct sock
*sk
= &sock
->sk
;
523 struct nfc_llcp_local
*local
;
524 size_t frag_len
= 0, remaining_len
;
525 u8
*msg_data
, *msg_ptr
;
527 pr_debug("Send I frame len %zd\n", len
);
533 /* Remote is ready but has not acknowledged our frames */
534 if((sock
->remote_ready
&&
535 skb_queue_len(&sock
->tx_pending_queue
) >= sock
->rw
&&
536 skb_queue_len(&sock
->tx_queue
) >= 2 * sock
->rw
)) {
537 pr_err("Pending queue is full %d frames\n",
538 skb_queue_len(&sock
->tx_pending_queue
));
542 /* Remote is not ready and we've been queueing enough frames */
543 if ((!sock
->remote_ready
&&
544 skb_queue_len(&sock
->tx_queue
) >= 2 * sock
->rw
)) {
545 pr_err("Tx queue is full %d frames\n",
546 skb_queue_len(&sock
->tx_queue
));
550 msg_data
= kzalloc(len
, GFP_KERNEL
);
551 if (msg_data
== NULL
)
554 if (memcpy_fromiovec(msg_data
, msg
->msg_iov
, len
)) {
562 while (remaining_len
> 0) {
564 frag_len
= min_t(size_t, sock
->miu
, remaining_len
);
566 pr_debug("Fragment %zd bytes remaining %zd",
567 frag_len
, remaining_len
);
569 pdu
= llcp_allocate_pdu(sock
, LLCP_PDU_I
,
570 frag_len
+ LLCP_SEQUENCE_SIZE
);
574 skb_put(pdu
, LLCP_SEQUENCE_SIZE
);
576 memcpy(skb_put(pdu
, frag_len
), msg_ptr
, frag_len
);
578 skb_queue_tail(&sock
->tx_queue
, pdu
);
582 nfc_llcp_queue_i_frames(sock
);
586 remaining_len
-= frag_len
;
595 int nfc_llcp_send_ui_frame(struct nfc_llcp_sock
*sock
, u8 ssap
, u8 dsap
,
596 struct msghdr
*msg
, size_t len
)
599 struct nfc_llcp_local
*local
;
600 size_t frag_len
= 0, remaining_len
;
601 u8
*msg_ptr
, *msg_data
;
604 pr_debug("Send UI frame len %zd\n", len
);
610 msg_data
= kzalloc(len
, GFP_KERNEL
);
611 if (msg_data
== NULL
)
614 if (memcpy_fromiovec(msg_data
, msg
->msg_iov
, len
)) {
622 while (remaining_len
> 0) {
624 frag_len
= min_t(size_t, sock
->miu
, remaining_len
);
626 pr_debug("Fragment %zd bytes remaining %zd",
627 frag_len
, remaining_len
);
629 pdu
= nfc_alloc_send_skb(sock
->dev
, &sock
->sk
, MSG_DONTWAIT
,
630 frag_len
+ LLCP_HEADER_SIZE
, &err
);
632 pr_err("Could not allocate PDU\n");
636 pdu
= llcp_add_header(pdu
, dsap
, ssap
, LLCP_PDU_UI
);
638 memcpy(skb_put(pdu
, frag_len
), msg_ptr
, frag_len
);
640 /* No need to check for the peer RW for UI frames */
641 skb_queue_tail(&local
->tx_queue
, pdu
);
643 remaining_len
-= frag_len
;
652 int nfc_llcp_send_rr(struct nfc_llcp_sock
*sock
)
655 struct nfc_llcp_local
*local
;
657 pr_debug("Send rr nr %d\n", sock
->recv_n
);
663 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_RR
, LLCP_SEQUENCE_SIZE
);
667 skb_put(skb
, LLCP_SEQUENCE_SIZE
);
669 skb
->data
[2] = sock
->recv_n
;
671 skb_queue_head(&local
->tx_queue
, skb
);
This page took 0.045918 seconds and 5 git commands to generate.