net/nfc/llcp_commands.c

32 static u8 llcp_tlv8(const u8 *tlv, u8 type)
34 	if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]])
37 	return tlv[2];
40 static u16 llcp_tlv16(const u8 *tlv, u8 type)
42 	if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]])
45 	return be16_to_cpu(*((__be16 *)(tlv + 2)));
49 static u8 llcp_tlv_version(const u8 *tlv)
51 	return llcp_tlv8(tlv, LLCP_TLV_VERSION);
54 static u16 llcp_tlv_miux(const u8 *tlv)
56 	return llcp_tlv16(tlv, LLCP_TLV_MIUX) & 0x7ff;
59 static u16 llcp_tlv_wks(const u8 *tlv)
61 	return llcp_tlv16(tlv, LLCP_TLV_WKS);
64 static u16 llcp_tlv_lto(const u8 *tlv)
66 	return llcp_tlv8(tlv, LLCP_TLV_LTO);
69 static u8 llcp_tlv_opt(const u8 *tlv)
71 	return llcp_tlv8(tlv, LLCP_TLV_OPT);
74 static u8 llcp_tlv_rw(const u8 *tlv)
76 	return llcp_tlv8(tlv, LLCP_TLV_RW) & 0xf;
81 	u8 *tlv, length;
95 	tlv = kzalloc(2 + length, GFP_KERNEL);
96 	if (tlv == NULL)
97 		return tlv;
99 	tlv[0] = type;
100 	tlv[1] = length;
101 	memcpy(tlv + 2, value, length);
103 	return tlv;
118 	sdres->tlv = nfc_llcp_build_tlv(LLCP_TLV_SDRES, value, 2,
120 	if (sdres->tlv == NULL) {
153 	sdreq->tlv = kzalloc(sdreq->tlv_len + 1, GFP_KERNEL);
154 	if (sdreq->tlv == NULL) {
159 	sdreq->tlv[0] = LLCP_TLV_SDREQ;
160 	sdreq->tlv[1] = sdreq->tlv_len - 2;
161 	sdreq->tlv[2] = tid;
164 	sdreq->uri = sdreq->tlv + 3;
176 	kfree(sdp->tlv);
195 	const u8 *tlv = tlv_array;
204 		type = tlv[0];
205 		length = tlv[1];
211 			local->remote_version = llcp_tlv_version(tlv);
214 			local->remote_miu = llcp_tlv_miux(tlv) + 128;
217 			local->remote_wks = llcp_tlv_wks(tlv);
220 			local->remote_lto = llcp_tlv_lto(tlv) * 10;
223 			local->remote_opt = llcp_tlv_opt(tlv);
226 			pr_err("Invalid gt tlv value 0x%x\n", type);
231 		tlv += length + 2;
245 	const u8 *tlv = tlv_array;
254 		type = tlv[0];
255 		length = tlv[1];
261 			sock->remote_miu = llcp_tlv_miux(tlv) + 128;
264 			sock->remote_rw = llcp_tlv_rw(tlv);
269 			pr_err("Invalid gt tlv value 0x%x\n", type);
274 		tlv += length + 2;
300 static struct sk_buff *llcp_add_tlv(struct sk_buff *pdu, const u8 *tlv,
305 	if (tlv == NULL)
308 	skb_put_data(pdu, tlv, tlv_length);
567 		skb_put_data(skb, sdp->tlv, sdp->tlv_len);
599 		skb_put_data(skb, sdreq->tlv, sdreq->tlv_len);