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 u8
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
) & 0x7f;
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
->rw
= llcp_tlv_rw(tlv
);
192 pr_err("Invalid gt tlv value 0x%x\n", type
);
196 offset
+= length
+ 2;
200 pr_debug("sock %p rw %d\n", sock
, sock
->rw
);
205 static struct sk_buff
*llcp_add_header(struct sk_buff
*pdu
,
206 u8 dsap
, u8 ssap
, u8 ptype
)
210 pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype
, dsap
, ssap
);
212 header
[0] = (u8
)((dsap
<< 2) | (ptype
>> 2));
213 header
[1] = (u8
)((ptype
<< 6) | ssap
);
215 pr_debug("header 0x%x 0x%x\n", header
[0], header
[1]);
217 memcpy(skb_put(pdu
, LLCP_HEADER_SIZE
), header
, LLCP_HEADER_SIZE
);
222 static struct sk_buff
*llcp_add_tlv(struct sk_buff
*pdu
, u8
*tlv
,
225 /* XXX Add an skb length check */
230 memcpy(skb_put(pdu
, tlv_length
), tlv
, tlv_length
);
235 static struct sk_buff
*llcp_allocate_pdu(struct nfc_llcp_sock
*sock
,
244 skb
= nfc_alloc_send_skb(sock
->dev
, &sock
->sk
, MSG_DONTWAIT
,
245 size
+ LLCP_HEADER_SIZE
, &err
);
247 pr_err("Could not allocate PDU\n");
251 skb
= llcp_add_header(skb
, sock
->dsap
, sock
->ssap
, cmd
);
256 int nfc_llcp_disconnect(struct nfc_llcp_sock
*sock
)
260 struct nfc_llcp_local
*local
;
263 pr_debug("Sending DISC\n");
273 size
+= LLCP_HEADER_SIZE
;
274 size
+= dev
->tx_headroom
+ dev
->tx_tailroom
+ NFC_HEADER_SIZE
;
276 skb
= alloc_skb(size
, GFP_ATOMIC
);
280 skb_reserve(skb
, dev
->tx_headroom
+ NFC_HEADER_SIZE
);
282 skb
= llcp_add_header(skb
, sock
->dsap
, sock
->ssap
, LLCP_PDU_DISC
);
284 skb_queue_tail(&local
->tx_queue
, skb
);
289 int nfc_llcp_send_symm(struct nfc_dev
*dev
)
292 struct nfc_llcp_local
*local
;
295 pr_debug("Sending SYMM\n");
297 local
= nfc_llcp_find_local(dev
);
301 size
+= LLCP_HEADER_SIZE
;
302 size
+= dev
->tx_headroom
+ dev
->tx_tailroom
+ NFC_HEADER_SIZE
;
304 skb
= alloc_skb(size
, GFP_KERNEL
);
308 skb_reserve(skb
, dev
->tx_headroom
+ NFC_HEADER_SIZE
);
310 skb
= llcp_add_header(skb
, 0, 0, LLCP_PDU_SYMM
);
312 return nfc_data_exchange(dev
, local
->target_idx
, skb
,
313 nfc_llcp_recv
, local
);
316 int nfc_llcp_send_connect(struct nfc_llcp_sock
*sock
)
318 struct nfc_llcp_local
*local
;
320 u8
*service_name_tlv
= NULL
, service_name_tlv_length
;
321 u8
*miux_tlv
= NULL
, miux_tlv_length
;
322 u8
*rw_tlv
= NULL
, rw_tlv_length
, rw
;
327 pr_debug("Sending CONNECT\n");
333 if (sock
->service_name
!= NULL
) {
334 service_name_tlv
= nfc_llcp_build_tlv(LLCP_TLV_SN
,
336 sock
->service_name_len
,
337 &service_name_tlv_length
);
338 size
+= service_name_tlv_length
;
341 miux
= cpu_to_be16(LLCP_MAX_MIUX
);
342 miux_tlv
= nfc_llcp_build_tlv(LLCP_TLV_MIUX
, (u8
*)&miux
, 0,
344 size
+= miux_tlv_length
;
347 rw_tlv
= nfc_llcp_build_tlv(LLCP_TLV_RW
, &rw
, 0, &rw_tlv_length
);
348 size
+= rw_tlv_length
;
350 pr_debug("SKB size %d SN length %zu\n", size
, sock
->service_name_len
);
352 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_CONNECT
, size
);
358 if (service_name_tlv
!= NULL
)
359 skb
= llcp_add_tlv(skb
, service_name_tlv
,
360 service_name_tlv_length
);
362 skb
= llcp_add_tlv(skb
, miux_tlv
, miux_tlv_length
);
363 skb
= llcp_add_tlv(skb
, rw_tlv
, rw_tlv_length
);
365 skb_queue_tail(&local
->tx_queue
, skb
);
370 pr_err("error %d\n", err
);
372 kfree(service_name_tlv
);
379 int nfc_llcp_send_cc(struct nfc_llcp_sock
*sock
)
381 struct nfc_llcp_local
*local
;
383 u8
*miux_tlv
= NULL
, miux_tlv_length
;
384 u8
*rw_tlv
= NULL
, rw_tlv_length
, rw
;
389 pr_debug("Sending CC\n");
395 miux
= cpu_to_be16(LLCP_MAX_MIUX
);
396 miux_tlv
= nfc_llcp_build_tlv(LLCP_TLV_MIUX
, (u8
*)&miux
, 0,
398 size
+= miux_tlv_length
;
401 rw_tlv
= nfc_llcp_build_tlv(LLCP_TLV_RW
, &rw
, 0, &rw_tlv_length
);
402 size
+= rw_tlv_length
;
404 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_CC
, size
);
410 skb
= llcp_add_tlv(skb
, miux_tlv
, miux_tlv_length
);
411 skb
= llcp_add_tlv(skb
, rw_tlv
, rw_tlv_length
);
413 skb_queue_tail(&local
->tx_queue
, skb
);
418 pr_err("error %d\n", err
);
426 int nfc_llcp_send_dm(struct nfc_llcp_local
*local
, u8 ssap
, u8 dsap
, u8 reason
)
430 u16 size
= 1; /* Reason code */
432 pr_debug("Sending DM reason 0x%x\n", reason
);
441 size
+= LLCP_HEADER_SIZE
;
442 size
+= dev
->tx_headroom
+ dev
->tx_tailroom
+ NFC_HEADER_SIZE
;
444 skb
= alloc_skb(size
, GFP_KERNEL
);
448 skb_reserve(skb
, dev
->tx_headroom
+ NFC_HEADER_SIZE
);
450 skb
= llcp_add_header(skb
, dsap
, ssap
, LLCP_PDU_DM
);
452 memcpy(skb_put(skb
, 1), &reason
, 1);
454 skb_queue_head(&local
->tx_queue
, skb
);
459 int nfc_llcp_send_disconnect(struct nfc_llcp_sock
*sock
)
462 struct nfc_llcp_local
*local
;
464 pr_debug("Send DISC\n");
470 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_DISC
, 0);
474 skb_queue_head(&local
->tx_queue
, skb
);
479 int nfc_llcp_send_i_frame(struct nfc_llcp_sock
*sock
,
480 struct msghdr
*msg
, size_t len
)
483 struct sock
*sk
= &sock
->sk
;
484 struct nfc_llcp_local
*local
;
485 size_t frag_len
= 0, remaining_len
;
486 u8
*msg_data
, *msg_ptr
;
488 pr_debug("Send I frame len %zd\n", len
);
494 msg_data
= kzalloc(len
, GFP_KERNEL
);
495 if (msg_data
== NULL
)
498 if (memcpy_fromiovec(msg_data
, msg
->msg_iov
, len
)) {
506 while (remaining_len
> 0) {
508 frag_len
= min_t(size_t, local
->remote_miu
, remaining_len
);
510 pr_debug("Fragment %zd bytes remaining %zd",
511 frag_len
, remaining_len
);
513 pdu
= llcp_allocate_pdu(sock
, LLCP_PDU_I
,
514 frag_len
+ LLCP_SEQUENCE_SIZE
);
518 skb_put(pdu
, LLCP_SEQUENCE_SIZE
);
520 memcpy(skb_put(pdu
, frag_len
), msg_ptr
, frag_len
);
522 skb_queue_tail(&sock
->tx_queue
, pdu
);
526 nfc_llcp_queue_i_frames(sock
);
530 remaining_len
-= frag_len
;
539 int nfc_llcp_send_rr(struct nfc_llcp_sock
*sock
)
542 struct nfc_llcp_local
*local
;
544 pr_debug("Send rr nr %d\n", sock
->recv_n
);
550 skb
= llcp_allocate_pdu(sock
, LLCP_PDU_RR
, LLCP_SEQUENCE_SIZE
);
554 skb_put(skb
, LLCP_SEQUENCE_SIZE
);
556 skb
->data
[2] = sock
->recv_n
;
558 skb_queue_head(&local
->tx_queue
, skb
);
This page took 0.044414 seconds and 6 git commands to generate.