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