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
;
266 pr_debug("Sending DISC\n");
276 size
+= LLCP_HEADER_SIZE
;
277 size
+= dev
->tx_headroom
+ dev
->tx_tailroom
+ NFC_HEADER_SIZE
;
279 skb
= alloc_skb(size
, GFP_ATOMIC
);
283 skb_reserve(skb
, dev
->tx_headroom
+ NFC_HEADER_SIZE
);
285 skb
= llcp_add_header(skb
, sock
->dsap
, sock
->ssap
, LLCP_PDU_DISC
);
287 skb_queue_tail(&local
->tx_queue
, skb
);
292 int nfc_llcp_send_symm(struct nfc_dev
*dev
)
295 struct nfc_llcp_local
*local
;
298 pr_debug("Sending SYMM\n");
300 local
= nfc_llcp_find_local(dev
);
304 size
+= LLCP_HEADER_SIZE
;
305 size
+= dev
->tx_headroom
+ dev
->tx_tailroom
+ NFC_HEADER_SIZE
;
307 skb
= alloc_skb(size
, GFP_KERNEL
);
311 skb_reserve(skb
, dev
->tx_headroom
+ NFC_HEADER_SIZE
);
313 skb
= llcp_add_header(skb
, 0, 0, LLCP_PDU_SYMM
);
315 nfc_llcp_send_to_raw_sock(local
, skb
, NFC_LLCP_DIRECTION_TX
);
317 return nfc_data_exchange(dev
, local
->target_idx
, skb
,
318 nfc_llcp_recv
, local
);
321 int nfc_llcp_send_connect(struct nfc_llcp_sock
*sock
)
323 struct nfc_llcp_local
*local
;
325 u8
*service_name_tlv
= NULL
, service_name_tlv_length
;
326 u8
*miux_tlv
= NULL
, miux_tlv_length
;
327 u8
*rw_tlv
= NULL
, rw_tlv_length
, rw
;
332 pr_debug("Sending CONNECT\n");
338 if (sock
->service_name
!= NULL
) {
339 service_name_tlv
= nfc_llcp_build_tlv(LLCP_TLV_SN
,
341 sock
->service_name_len
,
342 &service_name_tlv_length
);
343 size
+= service_name_tlv_length
;
346 miux
= cpu_to_be16(LLCP_MAX_MIUX
);
347 miux_tlv
= nfc_llcp_build_tlv(LLCP_TLV_MIUX
, (u8
*)&miux
, 0,
349 size
+= miux_tlv_length
;
352 rw_tlv
= nfc_llcp_build_tlv(LLCP_TLV_RW
, &rw
, 0, &rw_tlv_length
);
353 size
+= rw_tlv_length
;
355 pr_debug("SKB size %d SN length %zu\n", size
, sock
->service_name_len
);
357 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_CONNECT
, size
);
363 if (service_name_tlv
!= NULL
)
364 skb
= llcp_add_tlv(skb
, service_name_tlv
,
365 service_name_tlv_length
);
367 skb
= llcp_add_tlv(skb
, miux_tlv
, miux_tlv_length
);
368 skb
= llcp_add_tlv(skb
, rw_tlv
, rw_tlv_length
);
370 skb_queue_tail(&local
->tx_queue
, skb
);
375 pr_err("error %d\n", err
);
377 kfree(service_name_tlv
);
384 int nfc_llcp_send_cc(struct nfc_llcp_sock
*sock
)
386 struct nfc_llcp_local
*local
;
388 u8
*miux_tlv
= NULL
, miux_tlv_length
;
389 u8
*rw_tlv
= NULL
, rw_tlv_length
, rw
;
394 pr_debug("Sending CC\n");
400 miux
= cpu_to_be16(LLCP_MAX_MIUX
);
401 miux_tlv
= nfc_llcp_build_tlv(LLCP_TLV_MIUX
, (u8
*)&miux
, 0,
403 size
+= miux_tlv_length
;
406 rw_tlv
= nfc_llcp_build_tlv(LLCP_TLV_RW
, &rw
, 0, &rw_tlv_length
);
407 size
+= rw_tlv_length
;
409 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_CC
, size
);
415 skb
= llcp_add_tlv(skb
, miux_tlv
, miux_tlv_length
);
416 skb
= llcp_add_tlv(skb
, rw_tlv
, rw_tlv_length
);
418 skb_queue_tail(&local
->tx_queue
, skb
);
423 pr_err("error %d\n", err
);
431 int nfc_llcp_send_dm(struct nfc_llcp_local
*local
, u8 ssap
, u8 dsap
, u8 reason
)
435 u16 size
= 1; /* Reason code */
437 pr_debug("Sending DM reason 0x%x\n", reason
);
446 size
+= LLCP_HEADER_SIZE
;
447 size
+= dev
->tx_headroom
+ dev
->tx_tailroom
+ NFC_HEADER_SIZE
;
449 skb
= alloc_skb(size
, GFP_KERNEL
);
453 skb_reserve(skb
, dev
->tx_headroom
+ NFC_HEADER_SIZE
);
455 skb
= llcp_add_header(skb
, dsap
, ssap
, LLCP_PDU_DM
);
457 memcpy(skb_put(skb
, 1), &reason
, 1);
459 skb_queue_head(&local
->tx_queue
, skb
);
464 int nfc_llcp_send_disconnect(struct nfc_llcp_sock
*sock
)
467 struct nfc_llcp_local
*local
;
469 pr_debug("Send DISC\n");
475 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_DISC
, 0);
479 skb_queue_head(&local
->tx_queue
, skb
);
484 int nfc_llcp_send_i_frame(struct nfc_llcp_sock
*sock
,
485 struct msghdr
*msg
, size_t len
)
488 struct sock
*sk
= &sock
->sk
;
489 struct nfc_llcp_local
*local
;
490 size_t frag_len
= 0, remaining_len
;
491 u8
*msg_data
, *msg_ptr
;
493 pr_debug("Send I frame len %zd\n", len
);
499 msg_data
= kzalloc(len
, GFP_KERNEL
);
500 if (msg_data
== NULL
)
503 if (memcpy_fromiovec(msg_data
, msg
->msg_iov
, len
)) {
511 while (remaining_len
> 0) {
513 frag_len
= min_t(size_t, sock
->miu
, remaining_len
);
515 pr_debug("Fragment %zd bytes remaining %zd",
516 frag_len
, remaining_len
);
518 pdu
= llcp_allocate_pdu(sock
, LLCP_PDU_I
,
519 frag_len
+ LLCP_SEQUENCE_SIZE
);
523 skb_put(pdu
, LLCP_SEQUENCE_SIZE
);
525 memcpy(skb_put(pdu
, frag_len
), msg_ptr
, frag_len
);
527 skb_queue_tail(&sock
->tx_queue
, pdu
);
531 nfc_llcp_queue_i_frames(sock
);
535 remaining_len
-= frag_len
;
544 int nfc_llcp_send_rr(struct nfc_llcp_sock
*sock
)
547 struct nfc_llcp_local
*local
;
549 pr_debug("Send rr nr %d\n", sock
->recv_n
);
555 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_RR
, LLCP_SEQUENCE_SIZE
);
559 skb_put(skb
, LLCP_SEQUENCE_SIZE
);
561 skb
->data
[2] = sock
->recv_n
;
563 skb_queue_head(&local
->tx_queue
, skb
);
This page took 0.062423 seconds and 6 git commands to generate.