1d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz/* 2d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz * Copyright (C) 2011 Intel Corporation. All rights reserved. 3d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz * 4d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz * This program is free software; you can redistribute it and/or modify 5d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz * it under the terms of the GNU General Public License as published by 6d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz * the Free Software Foundation; either version 2 of the License, or 7d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz * (at your option) any later version. 8d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz * 9d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz * This program is distributed in the hope that it will be useful, 10d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz * but WITHOUT ANY WARRANTY; without even the implied warranty of 11d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz * GNU General Public License for more details. 13d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz * 14d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz * You should have received a copy of the GNU General Public License 1598b32decc83ed3137e3ddbc918b102f8fc406b6dJeff Kirsher * along with this program; if not, see <http://www.gnu.org/licenses/>. 16d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz */ 17d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 18d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz#define pr_fmt(fmt) "llcp: %s: " fmt, __func__ 19d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 20d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz#include <linux/init.h> 21d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz#include <linux/kernel.h> 22d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz#include <linux/module.h> 23d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz#include <linux/nfc.h> 24d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 25d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz#include <net/nfc/nfc.h> 26d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 2730cc4587659e1c9b1bfade8b2a757d91e04758abSamuel Ortiz#include "nfc.h" 28d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz#include "llcp.h" 29d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 30d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortizstatic u8 llcp_tlv_length[LLCP_TLV_MAX] = { 31d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 0, 32d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 1, /* VERSION */ 33d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 2, /* MIUX */ 34d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 2, /* WKS */ 35d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 1, /* LTO */ 36d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 1, /* RW */ 37d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 0, /* SN */ 38d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 1, /* OPT */ 39d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 0, /* SDREQ */ 40d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 2, /* SDRES */ 41d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 42d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz}; 43d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 44d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortizstatic u8 llcp_tlv8(u8 *tlv, u8 type) 45d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz{ 46d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]]) 47d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return 0; 48d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 49d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return tlv[2]; 50d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz} 51d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 5276762b73693aa7621ae8d3ea5c7efbf74beda0b9Samuel Ortizstatic u16 llcp_tlv16(u8 *tlv, u8 type) 53d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz{ 54d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]]) 55d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return 0; 56d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 57d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return be16_to_cpu(*((__be16 *)(tlv + 2))); 58d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz} 59d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 60d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 61d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortizstatic u8 llcp_tlv_version(u8 *tlv) 62d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz{ 63d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return llcp_tlv8(tlv, LLCP_TLV_VERSION); 64d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz} 65d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 66d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortizstatic u16 llcp_tlv_miux(u8 *tlv) 67d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz{ 6876762b73693aa7621ae8d3ea5c7efbf74beda0b9Samuel Ortiz return llcp_tlv16(tlv, LLCP_TLV_MIUX) & 0x7ff; 69d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz} 70d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 71d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortizstatic u16 llcp_tlv_wks(u8 *tlv) 72d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz{ 73d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return llcp_tlv16(tlv, LLCP_TLV_WKS); 74d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz} 75d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 76d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortizstatic u16 llcp_tlv_lto(u8 *tlv) 77d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz{ 78d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return llcp_tlv8(tlv, LLCP_TLV_LTO); 79d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz} 80d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 81d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortizstatic u8 llcp_tlv_opt(u8 *tlv) 82d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz{ 83d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return llcp_tlv8(tlv, LLCP_TLV_OPT); 84d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz} 85d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 86d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortizstatic u8 llcp_tlv_rw(u8 *tlv) 87d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz{ 88d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return llcp_tlv8(tlv, LLCP_TLV_RW) & 0xf; 89d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz} 90d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 91d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortizu8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length) 92d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz{ 93d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz u8 *tlv, length; 94d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 95d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz pr_debug("type %d\n", type); 96d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 97d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz if (type >= LLCP_TLV_MAX) 98d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return NULL; 99d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 100d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz length = llcp_tlv_length[type]; 101d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz if (length == 0 && value_length == 0) 102d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return NULL; 103324b0af6f5a48dc38dac016eed14d019cac5903fSamuel Ortiz else if (length == 0) 104d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz length = value_length; 105d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 106d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz *tlv_length = 2 + length; 107d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz tlv = kzalloc(2 + length, GFP_KERNEL); 108d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz if (tlv == NULL) 109d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return tlv; 110d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 111d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz tlv[0] = type; 112d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz tlv[1] = length; 113d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz memcpy(tlv + 2, value, length); 114d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 115d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return tlv; 116d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz} 117d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 118e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escandestruct nfc_llcp_sdp_tlv *nfc_llcp_build_sdres_tlv(u8 tid, u8 sap) 119e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande{ 120e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande struct nfc_llcp_sdp_tlv *sdres; 121e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande u8 value[2]; 122e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande 123e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande sdres = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL); 124e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande if (sdres == NULL) 125e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande return NULL; 126e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande 127e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande value[0] = tid; 128e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande value[1] = sap; 129e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande 130e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande sdres->tlv = nfc_llcp_build_tlv(LLCP_TLV_SDRES, value, 2, 131e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande &sdres->tlv_len); 132e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande if (sdres->tlv == NULL) { 133e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande kfree(sdres); 134e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande return NULL; 135e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande } 136e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande 137e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande sdres->tid = tid; 138e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande sdres->sap = sap; 139e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande 140e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande INIT_HLIST_NODE(&sdres->node); 141e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande 142e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande return sdres; 143e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande} 144e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande 145d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escandestruct nfc_llcp_sdp_tlv *nfc_llcp_build_sdreq_tlv(u8 tid, char *uri, 146d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande size_t uri_len) 147d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande{ 148d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande struct nfc_llcp_sdp_tlv *sdreq; 149d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande 150d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande pr_debug("uri: %s, len: %zu\n", uri, uri_len); 151d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande 152d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande sdreq = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL); 153d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande if (sdreq == NULL) 154d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande return NULL; 155d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande 156d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande sdreq->tlv_len = uri_len + 3; 157d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande 158d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande if (uri[uri_len - 1] == 0) 159d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande sdreq->tlv_len--; 160d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande 161d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande sdreq->tlv = kzalloc(sdreq->tlv_len + 1, GFP_KERNEL); 162d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande if (sdreq->tlv == NULL) { 163d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande kfree(sdreq); 164d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande return NULL; 165d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande } 166d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande 167d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande sdreq->tlv[0] = LLCP_TLV_SDREQ; 168d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande sdreq->tlv[1] = sdreq->tlv_len - 2; 169d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande sdreq->tlv[2] = tid; 170d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande 171d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande sdreq->tid = tid; 172d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande sdreq->uri = sdreq->tlv + 3; 173d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande memcpy(sdreq->uri, uri, uri_len); 174d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande 17540213fa8513c2a92e7390f25571f7c17c7955e2bThierry Escande sdreq->time = jiffies; 17640213fa8513c2a92e7390f25571f7c17c7955e2bThierry Escande 177d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande INIT_HLIST_NODE(&sdreq->node); 178d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande 179d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande return sdreq; 180d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande} 181d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande 182e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escandevoid nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp) 183e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande{ 184e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande kfree(sdp->tlv); 185e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande kfree(sdp); 186e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande} 187e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande 188d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escandevoid nfc_llcp_free_sdp_tlv_list(struct hlist_head *head) 189d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande{ 190d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande struct nfc_llcp_sdp_tlv *sdp; 191d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande struct hlist_node *n; 192d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande 193d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande hlist_for_each_entry_safe(sdp, n, head, node) { 194d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande hlist_del(&sdp->node); 195d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande 196d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande nfc_llcp_free_sdp_tlv(sdp); 197d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande } 198d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande} 199d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande 2007a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortizint nfc_llcp_parse_gb_tlv(struct nfc_llcp_local *local, 2017a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz u8 *tlv_array, u16 tlv_array_len) 202d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz{ 203d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz u8 *tlv = tlv_array, type, length, offset = 0; 204d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 205d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz pr_debug("TLV array length %d\n", tlv_array_len); 206d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 207d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz if (local == NULL) 208d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return -ENODEV; 209d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 210d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz while (offset < tlv_array_len) { 211d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz type = tlv[0]; 212d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz length = tlv[1]; 213d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 214d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz pr_debug("type 0x%x length %d\n", type, length); 215d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 216d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz switch (type) { 217d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz case LLCP_TLV_VERSION: 218d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz local->remote_version = llcp_tlv_version(tlv); 219d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz break; 220d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz case LLCP_TLV_MIUX: 221d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz local->remote_miu = llcp_tlv_miux(tlv) + 128; 222d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz break; 223d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz case LLCP_TLV_WKS: 224d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz local->remote_wks = llcp_tlv_wks(tlv); 225d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz break; 226d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz case LLCP_TLV_LTO: 227d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz local->remote_lto = llcp_tlv_lto(tlv) * 10; 228d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz break; 229d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz case LLCP_TLV_OPT: 230d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz local->remote_opt = llcp_tlv_opt(tlv); 231d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz break; 2327a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz default: 2337a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz pr_err("Invalid gt tlv value 0x%x\n", type); 2347a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz break; 2357a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz } 2367a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz 2377a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz offset += length + 2; 2387a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz tlv += length + 2; 2397a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz } 2407a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz 2417a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz pr_debug("version 0x%x miu %d lto %d opt 0x%x wks 0x%x\n", 2427a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz local->remote_version, local->remote_miu, 2437a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz local->remote_lto, local->remote_opt, 2447a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz local->remote_wks); 2457a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz 2467a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz return 0; 2477a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz} 2487a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz 2497a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortizint nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock, 2507a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz u8 *tlv_array, u16 tlv_array_len) 2517a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz{ 2527a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz u8 *tlv = tlv_array, type, length, offset = 0; 2537a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz 2547a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz pr_debug("TLV array length %d\n", tlv_array_len); 2557a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz 2567a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz if (sock == NULL) 2577a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz return -ENOTCONN; 2587a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz 2597a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz while (offset < tlv_array_len) { 2607a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz type = tlv[0]; 2617a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz length = tlv[1]; 2627a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz 2637a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz pr_debug("type 0x%x length %d\n", type, length); 2647a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz 2657a06e586b9bfcaca310f40a857cf144d04abc8e6Samuel Ortiz switch (type) { 26693d7e490b7f4a72b6c7e1dfa475fa3c3e18eb9f1Samuel Ortiz case LLCP_TLV_MIUX: 267e4306bec47fc02178c612879c848d3a6544424ddSamuel Ortiz sock->remote_miu = llcp_tlv_miux(tlv) + 128; 26893d7e490b7f4a72b6c7e1dfa475fa3c3e18eb9f1Samuel Ortiz break; 269d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz case LLCP_TLV_RW: 270e4306bec47fc02178c612879c848d3a6544424ddSamuel Ortiz sock->remote_rw = llcp_tlv_rw(tlv); 271d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz break; 2729dda50f4c98f84e32a5f6dc4d9dd7af6085add43Samuel Ortiz case LLCP_TLV_SN: 2739dda50f4c98f84e32a5f6dc4d9dd7af6085add43Samuel Ortiz break; 274d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz default: 275d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz pr_err("Invalid gt tlv value 0x%x\n", type); 276d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz break; 277d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz } 278d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 279d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz offset += length + 2; 280d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz tlv += length + 2; 281d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz } 282d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 283e4306bec47fc02178c612879c848d3a6544424ddSamuel Ortiz pr_debug("sock %p rw %d miu %d\n", sock, 284e4306bec47fc02178c612879c848d3a6544424ddSamuel Ortiz sock->remote_rw, sock->remote_miu); 285d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 286d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return 0; 287d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz} 288d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 289d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortizstatic struct sk_buff *llcp_add_header(struct sk_buff *pdu, 290427a2eb1f568c9c5934a36105232c94553db9b69Samuel Ortiz u8 dsap, u8 ssap, u8 ptype) 291d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz{ 292d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz u8 header[2]; 293d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 294d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap); 295d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 296d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz header[0] = (u8)((dsap << 2) | (ptype >> 2)); 297d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz header[1] = (u8)((ptype << 6) | ssap); 298d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 299d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz pr_debug("header 0x%x 0x%x\n", header[0], header[1]); 300d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 301d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz memcpy(skb_put(pdu, LLCP_HEADER_SIZE), header, LLCP_HEADER_SIZE); 302d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 303d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return pdu; 304d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz} 305d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 306427a2eb1f568c9c5934a36105232c94553db9b69Samuel Ortizstatic struct sk_buff *llcp_add_tlv(struct sk_buff *pdu, u8 *tlv, 307427a2eb1f568c9c5934a36105232c94553db9b69Samuel Ortiz u8 tlv_length) 308d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz{ 309d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz /* XXX Add an skb length check */ 310d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 311d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz if (tlv == NULL) 312d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return NULL; 313d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 314d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz memcpy(skb_put(pdu, tlv_length), tlv, tlv_length); 315d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 316d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return pdu; 317d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz} 318d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 319d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortizstatic struct sk_buff *llcp_allocate_pdu(struct nfc_llcp_sock *sock, 320427a2eb1f568c9c5934a36105232c94553db9b69Samuel Ortiz u8 cmd, u16 size) 321d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz{ 322d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz struct sk_buff *skb; 323d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz int err; 324d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 325d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz if (sock->ssap == 0) 326d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return NULL; 327d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 328d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz skb = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT, 329427a2eb1f568c9c5934a36105232c94553db9b69Samuel Ortiz size + LLCP_HEADER_SIZE, &err); 330d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz if (skb == NULL) { 331d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz pr_err("Could not allocate PDU\n"); 332d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return NULL; 333d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz } 334d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 335d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz skb = llcp_add_header(skb, sock->dsap, sock->ssap, cmd); 336d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 337d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return skb; 338d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz} 339d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 34058e3dd1558f56e95e7077a63340bb33e7aa42946Thierry Escandeint nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock) 341d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz{ 342d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz struct sk_buff *skb; 343d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz struct nfc_dev *dev; 344d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz struct nfc_llcp_local *local; 345d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 346d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz pr_debug("Sending DISC\n"); 347d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 348d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz local = sock->local; 349d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz if (local == NULL) 350d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return -ENODEV; 351d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 352d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz dev = sock->dev; 353d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz if (dev == NULL) 354d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return -ENODEV; 355d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 356922239064bb39b4ed9329ffd9418c20f8d64cbbbSamuel Ortiz skb = llcp_allocate_pdu(sock, LLCP_PDU_DISC, 0); 357d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz if (skb == NULL) 358d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return -ENOMEM; 359d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 360d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz skb_queue_tail(&local->tx_queue, skb); 361d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 362d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return 0; 363d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz} 364d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 365d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortizint nfc_llcp_send_symm(struct nfc_dev *dev) 366d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz{ 367d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz struct sk_buff *skb; 368d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz struct nfc_llcp_local *local; 369d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz u16 size = 0; 370d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 371d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz pr_debug("Sending SYMM\n"); 372d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 373d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz local = nfc_llcp_find_local(dev); 374d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz if (local == NULL) 375d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return -ENODEV; 376d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 377d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz size += LLCP_HEADER_SIZE; 378d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE; 379d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 380d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz skb = alloc_skb(size, GFP_KERNEL); 381d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz if (skb == NULL) 382d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return -ENOMEM; 383d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 384d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE); 385d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 386d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz skb = llcp_add_header(skb, 0, 0, LLCP_PDU_SYMM); 387d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 3882c2d45bdcb7063ef58dc2c27d72026c394a8f584Thierry Escande __net_timestamp(skb); 3892c2d45bdcb7063ef58dc2c27d72026c394a8f584Thierry Escande 39057be1f3f3ec1ccab6432615ca161c4c9ece2a2aaHiren Tandel nfc_llcp_send_to_raw_sock(local, skb, NFC_DIRECTION_TX); 3914463523bef98ff827a89cf8219db7dfac4350241Thierry Escande 392d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return nfc_data_exchange(dev, local->target_idx, skb, 393427a2eb1f568c9c5934a36105232c94553db9b69Samuel Ortiz nfc_llcp_recv, local); 394d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz} 395d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 396d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortizint nfc_llcp_send_connect(struct nfc_llcp_sock *sock) 397d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz{ 398d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz struct nfc_llcp_local *local; 399d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz struct sk_buff *skb; 400d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz u8 *service_name_tlv = NULL, service_name_tlv_length; 401eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz u8 *miux_tlv = NULL, miux_tlv_length; 40206d44f806aafdafefec789583aba5f8bef301c0cSamuel Ortiz u8 *rw_tlv = NULL, rw_tlv_length, rw; 403d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz int err; 40406d44f806aafdafefec789583aba5f8bef301c0cSamuel Ortiz u16 size = 0, miux; 405d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 406d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz pr_debug("Sending CONNECT\n"); 407d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 408d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz local = sock->local; 409d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz if (local == NULL) 410d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return -ENODEV; 411d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 412d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz if (sock->service_name != NULL) { 413d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz service_name_tlv = nfc_llcp_build_tlv(LLCP_TLV_SN, 414427a2eb1f568c9c5934a36105232c94553db9b69Samuel Ortiz sock->service_name, 415427a2eb1f568c9c5934a36105232c94553db9b69Samuel Ortiz sock->service_name_len, 416427a2eb1f568c9c5934a36105232c94553db9b69Samuel Ortiz &service_name_tlv_length); 417d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz size += service_name_tlv_length; 418d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz } 419d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 42006d44f806aafdafefec789583aba5f8bef301c0cSamuel Ortiz /* If the socket parameters are not set, use the local ones */ 4215eef6669759f8e291ab0347894876b532c242324Samuel Ortiz miux = be16_to_cpu(sock->miux) > LLCP_MAX_MIUX ? 4225eef6669759f8e291ab0347894876b532c242324Samuel Ortiz local->miux : sock->miux; 42306d44f806aafdafefec789583aba5f8bef301c0cSamuel Ortiz rw = sock->rw > LLCP_MAX_RW ? local->rw : sock->rw; 42406d44f806aafdafefec789583aba5f8bef301c0cSamuel Ortiz 42506d44f806aafdafefec789583aba5f8bef301c0cSamuel Ortiz miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0, 426427a2eb1f568c9c5934a36105232c94553db9b69Samuel Ortiz &miux_tlv_length); 427eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz size += miux_tlv_length; 428eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz 42906d44f806aafdafefec789583aba5f8bef301c0cSamuel Ortiz rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length); 430eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz size += rw_tlv_length; 431eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz 432d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz pr_debug("SKB size %d SN length %zu\n", size, sock->service_name_len); 433d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 434d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz skb = llcp_allocate_pdu(sock, LLCP_PDU_CONNECT, size); 435d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz if (skb == NULL) { 436d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz err = -ENOMEM; 437d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz goto error_tlv; 438d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz } 439d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 440d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz if (service_name_tlv != NULL) 441d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz skb = llcp_add_tlv(skb, service_name_tlv, 442427a2eb1f568c9c5934a36105232c94553db9b69Samuel Ortiz service_name_tlv_length); 443d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 444eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz skb = llcp_add_tlv(skb, miux_tlv, miux_tlv_length); 445eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz skb = llcp_add_tlv(skb, rw_tlv, rw_tlv_length); 446eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz 447d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz skb_queue_tail(&local->tx_queue, skb); 448d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 449d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return 0; 450d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 451d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortizerror_tlv: 452d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz pr_err("error %d\n", err); 453d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 454d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz kfree(service_name_tlv); 455eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz kfree(miux_tlv); 456eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz kfree(rw_tlv); 457d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 458d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return err; 459d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz} 460d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 461d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortizint nfc_llcp_send_cc(struct nfc_llcp_sock *sock) 462d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz{ 463d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz struct nfc_llcp_local *local; 464d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz struct sk_buff *skb; 465eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz u8 *miux_tlv = NULL, miux_tlv_length; 46606d44f806aafdafefec789583aba5f8bef301c0cSamuel Ortiz u8 *rw_tlv = NULL, rw_tlv_length, rw; 467eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz int err; 46806d44f806aafdafefec789583aba5f8bef301c0cSamuel Ortiz u16 size = 0, miux; 469d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 470d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz pr_debug("Sending CC\n"); 471d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 472d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz local = sock->local; 473d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz if (local == NULL) 474d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return -ENODEV; 475d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 47606d44f806aafdafefec789583aba5f8bef301c0cSamuel Ortiz /* If the socket parameters are not set, use the local ones */ 4775eef6669759f8e291ab0347894876b532c242324Samuel Ortiz miux = be16_to_cpu(sock->miux) > LLCP_MAX_MIUX ? 4785eef6669759f8e291ab0347894876b532c242324Samuel Ortiz local->miux : sock->miux; 47906d44f806aafdafefec789583aba5f8bef301c0cSamuel Ortiz rw = sock->rw > LLCP_MAX_RW ? local->rw : sock->rw; 48006d44f806aafdafefec789583aba5f8bef301c0cSamuel Ortiz 48106d44f806aafdafefec789583aba5f8bef301c0cSamuel Ortiz miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0, 482427a2eb1f568c9c5934a36105232c94553db9b69Samuel Ortiz &miux_tlv_length); 483eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz size += miux_tlv_length; 484eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz 48506d44f806aafdafefec789583aba5f8bef301c0cSamuel Ortiz rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length); 486eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz size += rw_tlv_length; 487eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz 488eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz skb = llcp_allocate_pdu(sock, LLCP_PDU_CC, size); 489eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz if (skb == NULL) { 490eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz err = -ENOMEM; 491eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz goto error_tlv; 492eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz } 493eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz 494eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz skb = llcp_add_tlv(skb, miux_tlv, miux_tlv_length); 495eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz skb = llcp_add_tlv(skb, rw_tlv, rw_tlv_length); 496d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 497d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz skb_queue_tail(&local->tx_queue, skb); 498d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 499d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return 0; 500eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz 501eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortizerror_tlv: 502eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz pr_err("error %d\n", err); 503eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz 504eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz kfree(miux_tlv); 505eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz kfree(rw_tlv); 506eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz 507eda21f16a5ed2476c1740e83a7dfaae34d893d9bSamuel Ortiz return err; 508d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz} 509d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 510e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escandestatic struct sk_buff *nfc_llcp_allocate_snl(struct nfc_llcp_local *local, 511e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande size_t tlv_length) 512c43bb03d5a7df53684cfb2a1fed5ea20014c7056Samuel Ortiz{ 513c43bb03d5a7df53684cfb2a1fed5ea20014c7056Samuel Ortiz struct sk_buff *skb; 514c43bb03d5a7df53684cfb2a1fed5ea20014c7056Samuel Ortiz struct nfc_dev *dev; 515c43bb03d5a7df53684cfb2a1fed5ea20014c7056Samuel Ortiz u16 size = 0; 516c43bb03d5a7df53684cfb2a1fed5ea20014c7056Samuel Ortiz 517c43bb03d5a7df53684cfb2a1fed5ea20014c7056Samuel Ortiz if (local == NULL) 518e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande return ERR_PTR(-ENODEV); 519c43bb03d5a7df53684cfb2a1fed5ea20014c7056Samuel Ortiz 520c43bb03d5a7df53684cfb2a1fed5ea20014c7056Samuel Ortiz dev = local->dev; 521c43bb03d5a7df53684cfb2a1fed5ea20014c7056Samuel Ortiz if (dev == NULL) 522e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande return ERR_PTR(-ENODEV); 523c43bb03d5a7df53684cfb2a1fed5ea20014c7056Samuel Ortiz 524c43bb03d5a7df53684cfb2a1fed5ea20014c7056Samuel Ortiz size += LLCP_HEADER_SIZE; 525c43bb03d5a7df53684cfb2a1fed5ea20014c7056Samuel Ortiz size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE; 526e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande size += tlv_length; 527c43bb03d5a7df53684cfb2a1fed5ea20014c7056Samuel Ortiz 528c43bb03d5a7df53684cfb2a1fed5ea20014c7056Samuel Ortiz skb = alloc_skb(size, GFP_KERNEL); 529e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande if (skb == NULL) 530e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande return ERR_PTR(-ENOMEM); 531c43bb03d5a7df53684cfb2a1fed5ea20014c7056Samuel Ortiz 532c43bb03d5a7df53684cfb2a1fed5ea20014c7056Samuel Ortiz skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE); 533c43bb03d5a7df53684cfb2a1fed5ea20014c7056Samuel Ortiz 534c43bb03d5a7df53684cfb2a1fed5ea20014c7056Samuel Ortiz skb = llcp_add_header(skb, LLCP_SAP_SDP, LLCP_SAP_SDP, LLCP_PDU_SNL); 535c43bb03d5a7df53684cfb2a1fed5ea20014c7056Samuel Ortiz 536e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande return skb; 537e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande} 538c43bb03d5a7df53684cfb2a1fed5ea20014c7056Samuel Ortiz 539e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escandeint nfc_llcp_send_snl_sdres(struct nfc_llcp_local *local, 540e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande struct hlist_head *tlv_list, size_t tlvs_len) 541e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande{ 542e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande struct nfc_llcp_sdp_tlv *sdp; 543e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande struct hlist_node *n; 544e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande struct sk_buff *skb; 545e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande 546e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande skb = nfc_llcp_allocate_snl(local, tlvs_len); 547e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande if (IS_ERR(skb)) 548e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande return PTR_ERR(skb); 549e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande 550e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande hlist_for_each_entry_safe(sdp, n, tlv_list, node) { 551e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande memcpy(skb_put(skb, sdp->tlv_len), sdp->tlv, sdp->tlv_len); 552c43bb03d5a7df53684cfb2a1fed5ea20014c7056Samuel Ortiz 553e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande hlist_del(&sdp->node); 554e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande 555e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande nfc_llcp_free_sdp_tlv(sdp); 556e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande } 557e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande 558e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8Thierry Escande skb_queue_tail(&local->tx_queue, skb); 559c43bb03d5a7df53684cfb2a1fed5ea20014c7056Samuel Ortiz 560c43bb03d5a7df53684cfb2a1fed5ea20014c7056Samuel Ortiz return 0; 561c43bb03d5a7df53684cfb2a1fed5ea20014c7056Samuel Ortiz} 562c43bb03d5a7df53684cfb2a1fed5ea20014c7056Samuel Ortiz 563d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escandeint nfc_llcp_send_snl_sdreq(struct nfc_llcp_local *local, 564d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande struct hlist_head *tlv_list, size_t tlvs_len) 565d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande{ 566d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande struct nfc_llcp_sdp_tlv *sdreq; 567d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande struct hlist_node *n; 568d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande struct sk_buff *skb; 569d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande 570d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande skb = nfc_llcp_allocate_snl(local, tlvs_len); 571d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande if (IS_ERR(skb)) 572d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande return PTR_ERR(skb); 573d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande 574d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande mutex_lock(&local->sdreq_lock); 575d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande 57640213fa8513c2a92e7390f25571f7c17c7955e2bThierry Escande if (hlist_empty(&local->pending_sdreqs)) 57740213fa8513c2a92e7390f25571f7c17c7955e2bThierry Escande mod_timer(&local->sdreq_timer, 57840213fa8513c2a92e7390f25571f7c17c7955e2bThierry Escande jiffies + msecs_to_jiffies(3 * local->remote_lto)); 57940213fa8513c2a92e7390f25571f7c17c7955e2bThierry Escande 580d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande hlist_for_each_entry_safe(sdreq, n, tlv_list, node) { 581d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande pr_debug("tid %d for %s\n", sdreq->tid, sdreq->uri); 582d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande 583d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande memcpy(skb_put(skb, sdreq->tlv_len), sdreq->tlv, 584d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande sdreq->tlv_len); 585d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande 586d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande hlist_del(&sdreq->node); 587d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande 588d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande hlist_add_head(&sdreq->node, &local->pending_sdreqs); 589d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande } 590d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande 591d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande mutex_unlock(&local->sdreq_lock); 592d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande 593d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande skb_queue_tail(&local->tx_queue, skb); 594d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande 595d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande return 0; 596d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande} 597d9b8d8e19b073096d3609bbd60f82148d128b555Thierry Escande 598d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortizint nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason) 599d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz{ 600d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz struct sk_buff *skb; 601d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz struct nfc_dev *dev; 602d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz u16 size = 1; /* Reason code */ 603d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 604d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz pr_debug("Sending DM reason 0x%x\n", reason); 605d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 606d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz if (local == NULL) 607d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return -ENODEV; 608d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 609d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz dev = local->dev; 610d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz if (dev == NULL) 611d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return -ENODEV; 612d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 613d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz size += LLCP_HEADER_SIZE; 614d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE; 615d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 616d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz skb = alloc_skb(size, GFP_KERNEL); 617d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz if (skb == NULL) 618d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return -ENOMEM; 619d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 620d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE); 621d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 622ffc29315e5b665d3e7e17d6156ac82f85a6d0205Samuel Ortiz skb = llcp_add_header(skb, dsap, ssap, LLCP_PDU_DM); 623d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 624d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz memcpy(skb_put(skb, 1), &reason, 1); 625d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 626d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz skb_queue_head(&local->tx_queue, skb); 627d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 628d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz return 0; 629d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz} 630d646960f7986fefb460a2b062d5ccc8ccfeacc3aSamuel Ortiz 63153a0ac2ee810cf82ec374b686a1dc3c32399265aSamuel Ortizint nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock, 632427a2eb1f568c9c5934a36105232c94553db9b69Samuel Ortiz struct msghdr *msg, size_t len) 63353a0ac2ee810cf82ec374b686a1dc3c32399265aSamuel Ortiz{ 63453a0ac2ee810cf82ec374b686a1dc3c32399265aSamuel Ortiz struct sk_buff *pdu; 635e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz struct sock *sk = &sock->sk; 636e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz struct nfc_llcp_local *local; 637e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz size_t frag_len = 0, remaining_len; 638e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz u8 *msg_data, *msg_ptr; 63966cbfa10f3bdbc86222598ac700c352da90e588fThierry Escande u16 remote_miu; 64053a0ac2ee810cf82ec374b686a1dc3c32399265aSamuel Ortiz 641e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz pr_debug("Send I frame len %zd\n", len); 64253a0ac2ee810cf82ec374b686a1dc3c32399265aSamuel Ortiz 643e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz local = sock->local; 644e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz if (local == NULL) 645e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz return -ENODEV; 64653a0ac2ee810cf82ec374b686a1dc3c32399265aSamuel Ortiz 647dd2bf43ffcf0d1bba94d20abc6cc44ed294db66bSamuel Ortiz /* Remote is ready but has not acknowledged our frames */ 648dd2bf43ffcf0d1bba94d20abc6cc44ed294db66bSamuel Ortiz if((sock->remote_ready && 649e4306bec47fc02178c612879c848d3a6544424ddSamuel Ortiz skb_queue_len(&sock->tx_pending_queue) >= sock->remote_rw && 650e4306bec47fc02178c612879c848d3a6544424ddSamuel Ortiz skb_queue_len(&sock->tx_queue) >= 2 * sock->remote_rw)) { 651dd2bf43ffcf0d1bba94d20abc6cc44ed294db66bSamuel Ortiz pr_err("Pending queue is full %d frames\n", 652dd2bf43ffcf0d1bba94d20abc6cc44ed294db66bSamuel Ortiz skb_queue_len(&sock->tx_pending_queue)); 653dd2bf43ffcf0d1bba94d20abc6cc44ed294db66bSamuel Ortiz return -ENOBUFS; 654dd2bf43ffcf0d1bba94d20abc6cc44ed294db66bSamuel Ortiz } 655dd2bf43ffcf0d1bba94d20abc6cc44ed294db66bSamuel Ortiz 656dd2bf43ffcf0d1bba94d20abc6cc44ed294db66bSamuel Ortiz /* Remote is not ready and we've been queueing enough frames */ 657dd2bf43ffcf0d1bba94d20abc6cc44ed294db66bSamuel Ortiz if ((!sock->remote_ready && 658e4306bec47fc02178c612879c848d3a6544424ddSamuel Ortiz skb_queue_len(&sock->tx_queue) >= 2 * sock->remote_rw)) { 659dd2bf43ffcf0d1bba94d20abc6cc44ed294db66bSamuel Ortiz pr_err("Tx queue is full %d frames\n", 660dd2bf43ffcf0d1bba94d20abc6cc44ed294db66bSamuel Ortiz skb_queue_len(&sock->tx_queue)); 661dd2bf43ffcf0d1bba94d20abc6cc44ed294db66bSamuel Ortiz return -ENOBUFS; 662dd2bf43ffcf0d1bba94d20abc6cc44ed294db66bSamuel Ortiz } 663dd2bf43ffcf0d1bba94d20abc6cc44ed294db66bSamuel Ortiz 664e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz msg_data = kzalloc(len, GFP_KERNEL); 665e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz if (msg_data == NULL) 666e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz return -ENOMEM; 66753a0ac2ee810cf82ec374b686a1dc3c32399265aSamuel Ortiz 668e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz if (memcpy_fromiovec(msg_data, msg->msg_iov, len)) { 669427a2eb1f568c9c5934a36105232c94553db9b69Samuel Ortiz kfree(msg_data); 670427a2eb1f568c9c5934a36105232c94553db9b69Samuel Ortiz return -EFAULT; 67153a0ac2ee810cf82ec374b686a1dc3c32399265aSamuel Ortiz } 67253a0ac2ee810cf82ec374b686a1dc3c32399265aSamuel Ortiz 673e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz remaining_len = len; 674e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz msg_ptr = msg_data; 675e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz 6760b23d666a8857e521384d0eec75a7362b80a39b8Olivier Guiter do { 67766cbfa10f3bdbc86222598ac700c352da90e588fThierry Escande remote_miu = sock->remote_miu > LLCP_MAX_MIU ? 67811bfb1c4b94fe24a83ebeae5c3310280f9606e0eSzymon Janc LLCP_DEFAULT_MIU : sock->remote_miu; 67966cbfa10f3bdbc86222598ac700c352da90e588fThierry Escande 68066cbfa10f3bdbc86222598ac700c352da90e588fThierry Escande frag_len = min_t(size_t, remote_miu, remaining_len); 68153a0ac2ee810cf82ec374b686a1dc3c32399265aSamuel Ortiz 682e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz pr_debug("Fragment %zd bytes remaining %zd", 683e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz frag_len, remaining_len); 68453a0ac2ee810cf82ec374b686a1dc3c32399265aSamuel Ortiz 685e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz pdu = llcp_allocate_pdu(sock, LLCP_PDU_I, 686e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz frag_len + LLCP_SEQUENCE_SIZE); 68743d53c29dd8548404256c05573ff557c927d214bSzymon Janc if (pdu == NULL) { 68843d53c29dd8548404256c05573ff557c927d214bSzymon Janc kfree(msg_data); 689e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz return -ENOMEM; 69043d53c29dd8548404256c05573ff557c927d214bSzymon Janc } 691e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz 692e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz skb_put(pdu, LLCP_SEQUENCE_SIZE); 693e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz 6940b23d666a8857e521384d0eec75a7362b80a39b8Olivier Guiter if (likely(frag_len > 0)) 6950b23d666a8857e521384d0eec75a7362b80a39b8Olivier Guiter memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len); 696e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz 697bdbc59b35f2a66cdd9465f573f865cc2109ab33dSamuel Ortiz skb_queue_tail(&sock->tx_queue, pdu); 698e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz 699e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz lock_sock(sk); 700e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz 701e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz nfc_llcp_queue_i_frames(sock); 702e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz 703e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz release_sock(sk); 704e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz 705e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz remaining_len -= frag_len; 706b4838d12e1f3cb48c2489a0b08733b5dbf848297Samuel Ortiz msg_ptr += frag_len; 7070b23d666a8857e521384d0eec75a7362b80a39b8Olivier Guiter } while (remaining_len > 0); 70853a0ac2ee810cf82ec374b686a1dc3c32399265aSamuel Ortiz 709e65b0f46edfda746ba8c66ada28ccb97c682b7c0Samuel Ortiz kfree(msg_data); 71053a0ac2ee810cf82ec374b686a1dc3c32399265aSamuel Ortiz 71143472fffb4628773397297f7e365983accd4a0efSamuel Ortiz return len; 71253a0ac2ee810cf82ec374b686a1dc3c32399265aSamuel Ortiz} 713d094afa155273e03b82981ea818d39c7a2dfba86Samuel Ortiz 71494f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortizint nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap, 71594f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz struct msghdr *msg, size_t len) 71694f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz{ 71794f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz struct sk_buff *pdu; 71894f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz struct nfc_llcp_local *local; 71994f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz size_t frag_len = 0, remaining_len; 7206e950fd214645e71e94bce2429bea58b88e1b5d0Samuel Ortiz u8 *msg_ptr, *msg_data; 72166cbfa10f3bdbc86222598ac700c352da90e588fThierry Escande u16 remote_miu; 72294f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz int err; 72394f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz 72494f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz pr_debug("Send UI frame len %zd\n", len); 72594f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz 72694f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz local = sock->local; 72794f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz if (local == NULL) 72894f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz return -ENODEV; 72994f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz 7306e950fd214645e71e94bce2429bea58b88e1b5d0Samuel Ortiz msg_data = kzalloc(len, GFP_KERNEL); 7316e950fd214645e71e94bce2429bea58b88e1b5d0Samuel Ortiz if (msg_data == NULL) 7326e950fd214645e71e94bce2429bea58b88e1b5d0Samuel Ortiz return -ENOMEM; 7336e950fd214645e71e94bce2429bea58b88e1b5d0Samuel Ortiz 7346e950fd214645e71e94bce2429bea58b88e1b5d0Samuel Ortiz if (memcpy_fromiovec(msg_data, msg->msg_iov, len)) { 7356e950fd214645e71e94bce2429bea58b88e1b5d0Samuel Ortiz kfree(msg_data); 7366e950fd214645e71e94bce2429bea58b88e1b5d0Samuel Ortiz return -EFAULT; 7376e950fd214645e71e94bce2429bea58b88e1b5d0Samuel Ortiz } 7386e950fd214645e71e94bce2429bea58b88e1b5d0Samuel Ortiz 73994f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz remaining_len = len; 7406e950fd214645e71e94bce2429bea58b88e1b5d0Samuel Ortiz msg_ptr = msg_data; 74194f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz 7420b23d666a8857e521384d0eec75a7362b80a39b8Olivier Guiter do { 74366cbfa10f3bdbc86222598ac700c352da90e588fThierry Escande remote_miu = sock->remote_miu > LLCP_MAX_MIU ? 74466cbfa10f3bdbc86222598ac700c352da90e588fThierry Escande local->remote_miu : sock->remote_miu; 74566cbfa10f3bdbc86222598ac700c352da90e588fThierry Escande 74666cbfa10f3bdbc86222598ac700c352da90e588fThierry Escande frag_len = min_t(size_t, remote_miu, remaining_len); 74794f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz 74894f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz pr_debug("Fragment %zd bytes remaining %zd", 74994f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz frag_len, remaining_len); 75094f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz 75194f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz pdu = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT, 75294f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz frag_len + LLCP_HEADER_SIZE, &err); 75394f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz if (pdu == NULL) { 75494f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz pr_err("Could not allocate PDU\n"); 75594f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz continue; 75694f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz } 75794f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz 75894f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz pdu = llcp_add_header(pdu, dsap, ssap, LLCP_PDU_UI); 75994f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz 7600b23d666a8857e521384d0eec75a7362b80a39b8Olivier Guiter if (likely(frag_len > 0)) 7610b23d666a8857e521384d0eec75a7362b80a39b8Olivier Guiter memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len); 76294f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz 76394f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz /* No need to check for the peer RW for UI frames */ 76494f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz skb_queue_tail(&local->tx_queue, pdu); 76594f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz 76694f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz remaining_len -= frag_len; 76794f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz msg_ptr += frag_len; 7680b23d666a8857e521384d0eec75a7362b80a39b8Olivier Guiter } while (remaining_len > 0); 76994f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz 7706e950fd214645e71e94bce2429bea58b88e1b5d0Samuel Ortiz kfree(msg_data); 7716e950fd214645e71e94bce2429bea58b88e1b5d0Samuel Ortiz 77294f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz return len; 77394f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz} 77494f418a206648c9be6fd84d6681d6956b8f8b106Samuel Ortiz 775d094afa155273e03b82981ea818d39c7a2dfba86Samuel Ortizint nfc_llcp_send_rr(struct nfc_llcp_sock *sock) 776d094afa155273e03b82981ea818d39c7a2dfba86Samuel Ortiz{ 777d094afa155273e03b82981ea818d39c7a2dfba86Samuel Ortiz struct sk_buff *skb; 778d094afa155273e03b82981ea818d39c7a2dfba86Samuel Ortiz struct nfc_llcp_local *local; 779d094afa155273e03b82981ea818d39c7a2dfba86Samuel Ortiz 780d094afa155273e03b82981ea818d39c7a2dfba86Samuel Ortiz pr_debug("Send rr nr %d\n", sock->recv_n); 781d094afa155273e03b82981ea818d39c7a2dfba86Samuel Ortiz 782d094afa155273e03b82981ea818d39c7a2dfba86Samuel Ortiz local = sock->local; 783d094afa155273e03b82981ea818d39c7a2dfba86Samuel Ortiz if (local == NULL) 784d094afa155273e03b82981ea818d39c7a2dfba86Samuel Ortiz return -ENODEV; 785d094afa155273e03b82981ea818d39c7a2dfba86Samuel Ortiz 786d094afa155273e03b82981ea818d39c7a2dfba86Samuel Ortiz skb = llcp_allocate_pdu(sock, LLCP_PDU_RR, LLCP_SEQUENCE_SIZE); 787d094afa155273e03b82981ea818d39c7a2dfba86Samuel Ortiz if (skb == NULL) 788d094afa155273e03b82981ea818d39c7a2dfba86Samuel Ortiz return -ENOMEM; 789d094afa155273e03b82981ea818d39c7a2dfba86Samuel Ortiz 790d094afa155273e03b82981ea818d39c7a2dfba86Samuel Ortiz skb_put(skb, LLCP_SEQUENCE_SIZE); 791d094afa155273e03b82981ea818d39c7a2dfba86Samuel Ortiz 792279cf174aea84202c5fef4675ff3f1265f071c8eSamuel Ortiz skb->data[2] = sock->recv_n; 793d094afa155273e03b82981ea818d39c7a2dfba86Samuel Ortiz 794d094afa155273e03b82981ea818d39c7a2dfba86Samuel Ortiz skb_queue_head(&local->tx_queue, skb); 795d094afa155273e03b82981ea818d39c7a2dfba86Samuel Ortiz 796d094afa155273e03b82981ea818d39c7a2dfba86Samuel Ortiz return 0; 797d094afa155273e03b82981ea818d39c7a2dfba86Samuel Ortiz} 798