mirror of
https://github.com/torvalds/linux.git
synced 2024-11-10 14:11:52 +00:00
b5f185f33d
John W. Linville says: ==================== pull request: wireless-next 2014-12-08 Please pull this last batch of pending wireless updates for the 3.19 tree... For the wireless bits, Johannes says: "This time I have Felix's no-status rate control work, which will allow drivers to work better with rate control even if they don't have perfect status reporting. In addition to this, a small hwsim fix from Patrik, one of the regulatory patches from Arik, and a number of cleanups and fixes I did myself. Of note is a patch where I disable CFG80211_WEXT so that compatibility is no longer selectable - this is intended as a wake-up call for anyone who's still using it, and is still easily worked around (it's a one-line patch) before we fully remove the code as well in the future." For the Bluetooth bits, Johan says: "Here's one more bluetooth-next pull request for 3.19: - Minor cleanups for ieee802154 & mac802154 - Fix for the kernel warning with !TASK_RUNNING reported by Kirill A. Shutemov - Support for another ath3k device - Fix for tracking link key based security level - Device tree bindings for btmrvl + a state update fix - Fix for wrong ACL flags on LE links" And... "In addition to the previous one this contains two more cleanups to mac802154 as well as support for some new HCI features from the Bluetooth 4.2 specification. From the original request: 'Here's what should be the last bluetooth-next pull request for 3.19. It's rather large but the majority of it is the Low Energy Secure Connections feature that's part of the Bluetooth 4.2 specification. The specification went public only this week so we couldn't publish the corresponding code before that. The code itself can nevertheless be considered fairly mature as it's been in development for over 6 months and gone through several interoperability test events. Besides LE SC the pull request contains an important fix for command complete events for mgmt sockets which also fixes some leaks of hci_conn objects when powering off or unplugging Bluetooth adapters. A smaller feature that's part of the pull request is service discovery support. This is like normal device discovery except that devices not matching specific UUIDs or strong enough RSSI are filtered out. Other changes that the pull request contains are firmware dump support to the btmrvl driver, firmware download support for Broadcom BCM20702A0 variants, as well as some coding style cleanups in 6lowpan & ieee802154/mac802154 code.'" For the NFC bits, Samuel says: "With this one we get: - NFC digital improvements for DEP support: Chaining, NACK and ATN support added. - NCI improvements: Support for p2p target, SE IO operand addition, SE operands extensions to support proprietary implementations, and a few fixes. - NFC HCI improvements: OPEN_PIPE and NOTIFY_ALL_CLEARED support, and SE IO operand addition. - A bunch of minor improvements and fixes for STMicro st21nfcb and st21nfca" For the iwlwifi bits, Emmanuel says: "Major works are CSA and TDLS. On top of that I have a new firmware API for scan and a few rate control improvements. Johannes find a few tricks to improve our CPU utilization and adds support for a new spin of 7265 called 7265D. Along with this a few random things that don't stand out." And... "I deprecate here -8.ucode since -9 has been published long ago. Along with that I have a new activity, we have now better a infrastructure for firmware debugging. This will allow to have configurable probes insides the firmware. Luca continues his work on NetDetect, this feature is now complete. All the rest is minor fixes here and there." For the Atheros bits, Kalle says: "Only ath10k changes this time and no major changes. Most visible are: o new debugfs interface for runtime firmware debugging (Yanbo) o fix shared WEP (Sujith) o don't rebuild whenever kernel version changes (Johannes) o lots of refactoring to make it easier to add new hw support (Michal) There's also smaller fixes and improvements with no point of listing here." In addition, there are a few last minute updates to ath5k, ath9k, brcmfmac, brcmsmac, mwifiex, rt2x00, rtlwifi, and wil6210. Also included is a pull of the wireless tree to pick-up the fixes originally included in "pull request: wireless 2014-12-03"... Please let me know if there are problems! ==================== Signed-off-by: David S. Miller <davem@davemloft.net>
800 lines
16 KiB
C
800 lines
16 KiB
C
/*
|
|
* Copyright (C) 2011 Intel Corporation. All rights reserved.
|
|
*
|
|
* This program is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation; either version 2 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this program; if not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#define pr_fmt(fmt) "llcp: %s: " fmt, __func__
|
|
|
|
#include <linux/init.h>
|
|
#include <linux/kernel.h>
|
|
#include <linux/module.h>
|
|
#include <linux/nfc.h>
|
|
|
|
#include <net/nfc/nfc.h>
|
|
|
|
#include "nfc.h"
|
|
#include "llcp.h"
|
|
|
|
static u8 llcp_tlv_length[LLCP_TLV_MAX] = {
|
|
0,
|
|
1, /* VERSION */
|
|
2, /* MIUX */
|
|
2, /* WKS */
|
|
1, /* LTO */
|
|
1, /* RW */
|
|
0, /* SN */
|
|
1, /* OPT */
|
|
0, /* SDREQ */
|
|
2, /* SDRES */
|
|
|
|
};
|
|
|
|
static u8 llcp_tlv8(u8 *tlv, u8 type)
|
|
{
|
|
if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]])
|
|
return 0;
|
|
|
|
return tlv[2];
|
|
}
|
|
|
|
static u16 llcp_tlv16(u8 *tlv, u8 type)
|
|
{
|
|
if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]])
|
|
return 0;
|
|
|
|
return be16_to_cpu(*((__be16 *)(tlv + 2)));
|
|
}
|
|
|
|
|
|
static u8 llcp_tlv_version(u8 *tlv)
|
|
{
|
|
return llcp_tlv8(tlv, LLCP_TLV_VERSION);
|
|
}
|
|
|
|
static u16 llcp_tlv_miux(u8 *tlv)
|
|
{
|
|
return llcp_tlv16(tlv, LLCP_TLV_MIUX) & 0x7ff;
|
|
}
|
|
|
|
static u16 llcp_tlv_wks(u8 *tlv)
|
|
{
|
|
return llcp_tlv16(tlv, LLCP_TLV_WKS);
|
|
}
|
|
|
|
static u16 llcp_tlv_lto(u8 *tlv)
|
|
{
|
|
return llcp_tlv8(tlv, LLCP_TLV_LTO);
|
|
}
|
|
|
|
static u8 llcp_tlv_opt(u8 *tlv)
|
|
{
|
|
return llcp_tlv8(tlv, LLCP_TLV_OPT);
|
|
}
|
|
|
|
static u8 llcp_tlv_rw(u8 *tlv)
|
|
{
|
|
return llcp_tlv8(tlv, LLCP_TLV_RW) & 0xf;
|
|
}
|
|
|
|
u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length)
|
|
{
|
|
u8 *tlv, length;
|
|
|
|
pr_debug("type %d\n", type);
|
|
|
|
if (type >= LLCP_TLV_MAX)
|
|
return NULL;
|
|
|
|
length = llcp_tlv_length[type];
|
|
if (length == 0 && value_length == 0)
|
|
return NULL;
|
|
else if (length == 0)
|
|
length = value_length;
|
|
|
|
*tlv_length = 2 + length;
|
|
tlv = kzalloc(2 + length, GFP_KERNEL);
|
|
if (tlv == NULL)
|
|
return tlv;
|
|
|
|
tlv[0] = type;
|
|
tlv[1] = length;
|
|
memcpy(tlv + 2, value, length);
|
|
|
|
return tlv;
|
|
}
|
|
|
|
struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdres_tlv(u8 tid, u8 sap)
|
|
{
|
|
struct nfc_llcp_sdp_tlv *sdres;
|
|
u8 value[2];
|
|
|
|
sdres = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL);
|
|
if (sdres == NULL)
|
|
return NULL;
|
|
|
|
value[0] = tid;
|
|
value[1] = sap;
|
|
|
|
sdres->tlv = nfc_llcp_build_tlv(LLCP_TLV_SDRES, value, 2,
|
|
&sdres->tlv_len);
|
|
if (sdres->tlv == NULL) {
|
|
kfree(sdres);
|
|
return NULL;
|
|
}
|
|
|
|
sdres->tid = tid;
|
|
sdres->sap = sap;
|
|
|
|
INIT_HLIST_NODE(&sdres->node);
|
|
|
|
return sdres;
|
|
}
|
|
|
|
struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdreq_tlv(u8 tid, char *uri,
|
|
size_t uri_len)
|
|
{
|
|
struct nfc_llcp_sdp_tlv *sdreq;
|
|
|
|
pr_debug("uri: %s, len: %zu\n", uri, uri_len);
|
|
|
|
sdreq = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL);
|
|
if (sdreq == NULL)
|
|
return NULL;
|
|
|
|
sdreq->tlv_len = uri_len + 3;
|
|
|
|
if (uri[uri_len - 1] == 0)
|
|
sdreq->tlv_len--;
|
|
|
|
sdreq->tlv = kzalloc(sdreq->tlv_len + 1, GFP_KERNEL);
|
|
if (sdreq->tlv == NULL) {
|
|
kfree(sdreq);
|
|
return NULL;
|
|
}
|
|
|
|
sdreq->tlv[0] = LLCP_TLV_SDREQ;
|
|
sdreq->tlv[1] = sdreq->tlv_len - 2;
|
|
sdreq->tlv[2] = tid;
|
|
|
|
sdreq->tid = tid;
|
|
sdreq->uri = sdreq->tlv + 3;
|
|
memcpy(sdreq->uri, uri, uri_len);
|
|
|
|
sdreq->time = jiffies;
|
|
|
|
INIT_HLIST_NODE(&sdreq->node);
|
|
|
|
return sdreq;
|
|
}
|
|
|
|
void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp)
|
|
{
|
|
kfree(sdp->tlv);
|
|
kfree(sdp);
|
|
}
|
|
|
|
void nfc_llcp_free_sdp_tlv_list(struct hlist_head *head)
|
|
{
|
|
struct nfc_llcp_sdp_tlv *sdp;
|
|
struct hlist_node *n;
|
|
|
|
hlist_for_each_entry_safe(sdp, n, head, node) {
|
|
hlist_del(&sdp->node);
|
|
|
|
nfc_llcp_free_sdp_tlv(sdp);
|
|
}
|
|
}
|
|
|
|
int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local *local,
|
|
u8 *tlv_array, u16 tlv_array_len)
|
|
{
|
|
u8 *tlv = tlv_array, type, length, offset = 0;
|
|
|
|
pr_debug("TLV array length %d\n", tlv_array_len);
|
|
|
|
if (local == NULL)
|
|
return -ENODEV;
|
|
|
|
while (offset < tlv_array_len) {
|
|
type = tlv[0];
|
|
length = tlv[1];
|
|
|
|
pr_debug("type 0x%x length %d\n", type, length);
|
|
|
|
switch (type) {
|
|
case LLCP_TLV_VERSION:
|
|
local->remote_version = llcp_tlv_version(tlv);
|
|
break;
|
|
case LLCP_TLV_MIUX:
|
|
local->remote_miu = llcp_tlv_miux(tlv) + 128;
|
|
break;
|
|
case LLCP_TLV_WKS:
|
|
local->remote_wks = llcp_tlv_wks(tlv);
|
|
break;
|
|
case LLCP_TLV_LTO:
|
|
local->remote_lto = llcp_tlv_lto(tlv) * 10;
|
|
break;
|
|
case LLCP_TLV_OPT:
|
|
local->remote_opt = llcp_tlv_opt(tlv);
|
|
break;
|
|
default:
|
|
pr_err("Invalid gt tlv value 0x%x\n", type);
|
|
break;
|
|
}
|
|
|
|
offset += length + 2;
|
|
tlv += length + 2;
|
|
}
|
|
|
|
pr_debug("version 0x%x miu %d lto %d opt 0x%x wks 0x%x\n",
|
|
local->remote_version, local->remote_miu,
|
|
local->remote_lto, local->remote_opt,
|
|
local->remote_wks);
|
|
|
|
return 0;
|
|
}
|
|
|
|
int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock,
|
|
u8 *tlv_array, u16 tlv_array_len)
|
|
{
|
|
u8 *tlv = tlv_array, type, length, offset = 0;
|
|
|
|
pr_debug("TLV array length %d\n", tlv_array_len);
|
|
|
|
if (sock == NULL)
|
|
return -ENOTCONN;
|
|
|
|
while (offset < tlv_array_len) {
|
|
type = tlv[0];
|
|
length = tlv[1];
|
|
|
|
pr_debug("type 0x%x length %d\n", type, length);
|
|
|
|
switch (type) {
|
|
case LLCP_TLV_MIUX:
|
|
sock->remote_miu = llcp_tlv_miux(tlv) + 128;
|
|
break;
|
|
case LLCP_TLV_RW:
|
|
sock->remote_rw = llcp_tlv_rw(tlv);
|
|
break;
|
|
case LLCP_TLV_SN:
|
|
break;
|
|
default:
|
|
pr_err("Invalid gt tlv value 0x%x\n", type);
|
|
break;
|
|
}
|
|
|
|
offset += length + 2;
|
|
tlv += length + 2;
|
|
}
|
|
|
|
pr_debug("sock %p rw %d miu %d\n", sock,
|
|
sock->remote_rw, sock->remote_miu);
|
|
|
|
return 0;
|
|
}
|
|
|
|
static struct sk_buff *llcp_add_header(struct sk_buff *pdu,
|
|
u8 dsap, u8 ssap, u8 ptype)
|
|
{
|
|
u8 header[2];
|
|
|
|
pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap);
|
|
|
|
header[0] = (u8)((dsap << 2) | (ptype >> 2));
|
|
header[1] = (u8)((ptype << 6) | ssap);
|
|
|
|
pr_debug("header 0x%x 0x%x\n", header[0], header[1]);
|
|
|
|
memcpy(skb_put(pdu, LLCP_HEADER_SIZE), header, LLCP_HEADER_SIZE);
|
|
|
|
return pdu;
|
|
}
|
|
|
|
static struct sk_buff *llcp_add_tlv(struct sk_buff *pdu, u8 *tlv,
|
|
u8 tlv_length)
|
|
{
|
|
/* XXX Add an skb length check */
|
|
|
|
if (tlv == NULL)
|
|
return NULL;
|
|
|
|
memcpy(skb_put(pdu, tlv_length), tlv, tlv_length);
|
|
|
|
return pdu;
|
|
}
|
|
|
|
static struct sk_buff *llcp_allocate_pdu(struct nfc_llcp_sock *sock,
|
|
u8 cmd, u16 size)
|
|
{
|
|
struct sk_buff *skb;
|
|
int err;
|
|
|
|
if (sock->ssap == 0)
|
|
return NULL;
|
|
|
|
skb = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT,
|
|
size + LLCP_HEADER_SIZE, &err);
|
|
if (skb == NULL) {
|
|
pr_err("Could not allocate PDU\n");
|
|
return NULL;
|
|
}
|
|
|
|
skb = llcp_add_header(skb, sock->dsap, sock->ssap, cmd);
|
|
|
|
return skb;
|
|
}
|
|
|
|
int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock)
|
|
{
|
|
struct sk_buff *skb;
|
|
struct nfc_dev *dev;
|
|
struct nfc_llcp_local *local;
|
|
|
|
pr_debug("Sending DISC\n");
|
|
|
|
local = sock->local;
|
|
if (local == NULL)
|
|
return -ENODEV;
|
|
|
|
dev = sock->dev;
|
|
if (dev == NULL)
|
|
return -ENODEV;
|
|
|
|
skb = llcp_allocate_pdu(sock, LLCP_PDU_DISC, 0);
|
|
if (skb == NULL)
|
|
return -ENOMEM;
|
|
|
|
skb_queue_tail(&local->tx_queue, skb);
|
|
|
|
return 0;
|
|
}
|
|
|
|
int nfc_llcp_send_symm(struct nfc_dev *dev)
|
|
{
|
|
struct sk_buff *skb;
|
|
struct nfc_llcp_local *local;
|
|
u16 size = 0;
|
|
|
|
pr_debug("Sending SYMM\n");
|
|
|
|
local = nfc_llcp_find_local(dev);
|
|
if (local == NULL)
|
|
return -ENODEV;
|
|
|
|
size += LLCP_HEADER_SIZE;
|
|
size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
|
|
|
|
skb = alloc_skb(size, GFP_KERNEL);
|
|
if (skb == NULL)
|
|
return -ENOMEM;
|
|
|
|
skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
|
|
|
|
skb = llcp_add_header(skb, 0, 0, LLCP_PDU_SYMM);
|
|
|
|
__net_timestamp(skb);
|
|
|
|
nfc_llcp_send_to_raw_sock(local, skb, NFC_DIRECTION_TX);
|
|
|
|
return nfc_data_exchange(dev, local->target_idx, skb,
|
|
nfc_llcp_recv, local);
|
|
}
|
|
|
|
int nfc_llcp_send_connect(struct nfc_llcp_sock *sock)
|
|
{
|
|
struct nfc_llcp_local *local;
|
|
struct sk_buff *skb;
|
|
u8 *service_name_tlv = NULL, service_name_tlv_length;
|
|
u8 *miux_tlv = NULL, miux_tlv_length;
|
|
u8 *rw_tlv = NULL, rw_tlv_length, rw;
|
|
int err;
|
|
u16 size = 0;
|
|
__be16 miux;
|
|
|
|
pr_debug("Sending CONNECT\n");
|
|
|
|
local = sock->local;
|
|
if (local == NULL)
|
|
return -ENODEV;
|
|
|
|
if (sock->service_name != NULL) {
|
|
service_name_tlv = nfc_llcp_build_tlv(LLCP_TLV_SN,
|
|
sock->service_name,
|
|
sock->service_name_len,
|
|
&service_name_tlv_length);
|
|
size += service_name_tlv_length;
|
|
}
|
|
|
|
/* If the socket parameters are not set, use the local ones */
|
|
miux = be16_to_cpu(sock->miux) > LLCP_MAX_MIUX ?
|
|
local->miux : sock->miux;
|
|
rw = sock->rw > LLCP_MAX_RW ? local->rw : sock->rw;
|
|
|
|
miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0,
|
|
&miux_tlv_length);
|
|
size += miux_tlv_length;
|
|
|
|
rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length);
|
|
size += rw_tlv_length;
|
|
|
|
pr_debug("SKB size %d SN length %zu\n", size, sock->service_name_len);
|
|
|
|
skb = llcp_allocate_pdu(sock, LLCP_PDU_CONNECT, size);
|
|
if (skb == NULL) {
|
|
err = -ENOMEM;
|
|
goto error_tlv;
|
|
}
|
|
|
|
if (service_name_tlv != NULL)
|
|
skb = llcp_add_tlv(skb, service_name_tlv,
|
|
service_name_tlv_length);
|
|
|
|
skb = llcp_add_tlv(skb, miux_tlv, miux_tlv_length);
|
|
skb = llcp_add_tlv(skb, rw_tlv, rw_tlv_length);
|
|
|
|
skb_queue_tail(&local->tx_queue, skb);
|
|
|
|
return 0;
|
|
|
|
error_tlv:
|
|
pr_err("error %d\n", err);
|
|
|
|
kfree(service_name_tlv);
|
|
kfree(miux_tlv);
|
|
kfree(rw_tlv);
|
|
|
|
return err;
|
|
}
|
|
|
|
int nfc_llcp_send_cc(struct nfc_llcp_sock *sock)
|
|
{
|
|
struct nfc_llcp_local *local;
|
|
struct sk_buff *skb;
|
|
u8 *miux_tlv = NULL, miux_tlv_length;
|
|
u8 *rw_tlv = NULL, rw_tlv_length, rw;
|
|
int err;
|
|
u16 size = 0;
|
|
__be16 miux;
|
|
|
|
pr_debug("Sending CC\n");
|
|
|
|
local = sock->local;
|
|
if (local == NULL)
|
|
return -ENODEV;
|
|
|
|
/* If the socket parameters are not set, use the local ones */
|
|
miux = be16_to_cpu(sock->miux) > LLCP_MAX_MIUX ?
|
|
local->miux : sock->miux;
|
|
rw = sock->rw > LLCP_MAX_RW ? local->rw : sock->rw;
|
|
|
|
miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0,
|
|
&miux_tlv_length);
|
|
size += miux_tlv_length;
|
|
|
|
rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length);
|
|
size += rw_tlv_length;
|
|
|
|
skb = llcp_allocate_pdu(sock, LLCP_PDU_CC, size);
|
|
if (skb == NULL) {
|
|
err = -ENOMEM;
|
|
goto error_tlv;
|
|
}
|
|
|
|
skb = llcp_add_tlv(skb, miux_tlv, miux_tlv_length);
|
|
skb = llcp_add_tlv(skb, rw_tlv, rw_tlv_length);
|
|
|
|
skb_queue_tail(&local->tx_queue, skb);
|
|
|
|
return 0;
|
|
|
|
error_tlv:
|
|
pr_err("error %d\n", err);
|
|
|
|
kfree(miux_tlv);
|
|
kfree(rw_tlv);
|
|
|
|
return err;
|
|
}
|
|
|
|
static struct sk_buff *nfc_llcp_allocate_snl(struct nfc_llcp_local *local,
|
|
size_t tlv_length)
|
|
{
|
|
struct sk_buff *skb;
|
|
struct nfc_dev *dev;
|
|
u16 size = 0;
|
|
|
|
if (local == NULL)
|
|
return ERR_PTR(-ENODEV);
|
|
|
|
dev = local->dev;
|
|
if (dev == NULL)
|
|
return ERR_PTR(-ENODEV);
|
|
|
|
size += LLCP_HEADER_SIZE;
|
|
size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
|
|
size += tlv_length;
|
|
|
|
skb = alloc_skb(size, GFP_KERNEL);
|
|
if (skb == NULL)
|
|
return ERR_PTR(-ENOMEM);
|
|
|
|
skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
|
|
|
|
skb = llcp_add_header(skb, LLCP_SAP_SDP, LLCP_SAP_SDP, LLCP_PDU_SNL);
|
|
|
|
return skb;
|
|
}
|
|
|
|
int nfc_llcp_send_snl_sdres(struct nfc_llcp_local *local,
|
|
struct hlist_head *tlv_list, size_t tlvs_len)
|
|
{
|
|
struct nfc_llcp_sdp_tlv *sdp;
|
|
struct hlist_node *n;
|
|
struct sk_buff *skb;
|
|
|
|
skb = nfc_llcp_allocate_snl(local, tlvs_len);
|
|
if (IS_ERR(skb))
|
|
return PTR_ERR(skb);
|
|
|
|
hlist_for_each_entry_safe(sdp, n, tlv_list, node) {
|
|
memcpy(skb_put(skb, sdp->tlv_len), sdp->tlv, sdp->tlv_len);
|
|
|
|
hlist_del(&sdp->node);
|
|
|
|
nfc_llcp_free_sdp_tlv(sdp);
|
|
}
|
|
|
|
skb_queue_tail(&local->tx_queue, skb);
|
|
|
|
return 0;
|
|
}
|
|
|
|
int nfc_llcp_send_snl_sdreq(struct nfc_llcp_local *local,
|
|
struct hlist_head *tlv_list, size_t tlvs_len)
|
|
{
|
|
struct nfc_llcp_sdp_tlv *sdreq;
|
|
struct hlist_node *n;
|
|
struct sk_buff *skb;
|
|
|
|
skb = nfc_llcp_allocate_snl(local, tlvs_len);
|
|
if (IS_ERR(skb))
|
|
return PTR_ERR(skb);
|
|
|
|
mutex_lock(&local->sdreq_lock);
|
|
|
|
if (hlist_empty(&local->pending_sdreqs))
|
|
mod_timer(&local->sdreq_timer,
|
|
jiffies + msecs_to_jiffies(3 * local->remote_lto));
|
|
|
|
hlist_for_each_entry_safe(sdreq, n, tlv_list, node) {
|
|
pr_debug("tid %d for %s\n", sdreq->tid, sdreq->uri);
|
|
|
|
memcpy(skb_put(skb, sdreq->tlv_len), sdreq->tlv,
|
|
sdreq->tlv_len);
|
|
|
|
hlist_del(&sdreq->node);
|
|
|
|
hlist_add_head(&sdreq->node, &local->pending_sdreqs);
|
|
}
|
|
|
|
mutex_unlock(&local->sdreq_lock);
|
|
|
|
skb_queue_tail(&local->tx_queue, skb);
|
|
|
|
return 0;
|
|
}
|
|
|
|
int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason)
|
|
{
|
|
struct sk_buff *skb;
|
|
struct nfc_dev *dev;
|
|
u16 size = 1; /* Reason code */
|
|
|
|
pr_debug("Sending DM reason 0x%x\n", reason);
|
|
|
|
if (local == NULL)
|
|
return -ENODEV;
|
|
|
|
dev = local->dev;
|
|
if (dev == NULL)
|
|
return -ENODEV;
|
|
|
|
size += LLCP_HEADER_SIZE;
|
|
size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
|
|
|
|
skb = alloc_skb(size, GFP_KERNEL);
|
|
if (skb == NULL)
|
|
return -ENOMEM;
|
|
|
|
skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
|
|
|
|
skb = llcp_add_header(skb, dsap, ssap, LLCP_PDU_DM);
|
|
|
|
memcpy(skb_put(skb, 1), &reason, 1);
|
|
|
|
skb_queue_head(&local->tx_queue, skb);
|
|
|
|
return 0;
|
|
}
|
|
|
|
int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
|
|
struct msghdr *msg, size_t len)
|
|
{
|
|
struct sk_buff *pdu;
|
|
struct sock *sk = &sock->sk;
|
|
struct nfc_llcp_local *local;
|
|
size_t frag_len = 0, remaining_len;
|
|
u8 *msg_data, *msg_ptr;
|
|
u16 remote_miu;
|
|
|
|
pr_debug("Send I frame len %zd\n", len);
|
|
|
|
local = sock->local;
|
|
if (local == NULL)
|
|
return -ENODEV;
|
|
|
|
/* Remote is ready but has not acknowledged our frames */
|
|
if((sock->remote_ready &&
|
|
skb_queue_len(&sock->tx_pending_queue) >= sock->remote_rw &&
|
|
skb_queue_len(&sock->tx_queue) >= 2 * sock->remote_rw)) {
|
|
pr_err("Pending queue is full %d frames\n",
|
|
skb_queue_len(&sock->tx_pending_queue));
|
|
return -ENOBUFS;
|
|
}
|
|
|
|
/* Remote is not ready and we've been queueing enough frames */
|
|
if ((!sock->remote_ready &&
|
|
skb_queue_len(&sock->tx_queue) >= 2 * sock->remote_rw)) {
|
|
pr_err("Tx queue is full %d frames\n",
|
|
skb_queue_len(&sock->tx_queue));
|
|
return -ENOBUFS;
|
|
}
|
|
|
|
msg_data = kzalloc(len, GFP_KERNEL);
|
|
if (msg_data == NULL)
|
|
return -ENOMEM;
|
|
|
|
if (memcpy_from_msg(msg_data, msg, len)) {
|
|
kfree(msg_data);
|
|
return -EFAULT;
|
|
}
|
|
|
|
remaining_len = len;
|
|
msg_ptr = msg_data;
|
|
|
|
do {
|
|
remote_miu = sock->remote_miu > LLCP_MAX_MIU ?
|
|
LLCP_DEFAULT_MIU : sock->remote_miu;
|
|
|
|
frag_len = min_t(size_t, remote_miu, remaining_len);
|
|
|
|
pr_debug("Fragment %zd bytes remaining %zd",
|
|
frag_len, remaining_len);
|
|
|
|
pdu = llcp_allocate_pdu(sock, LLCP_PDU_I,
|
|
frag_len + LLCP_SEQUENCE_SIZE);
|
|
if (pdu == NULL) {
|
|
kfree(msg_data);
|
|
return -ENOMEM;
|
|
}
|
|
|
|
skb_put(pdu, LLCP_SEQUENCE_SIZE);
|
|
|
|
if (likely(frag_len > 0))
|
|
memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len);
|
|
|
|
skb_queue_tail(&sock->tx_queue, pdu);
|
|
|
|
lock_sock(sk);
|
|
|
|
nfc_llcp_queue_i_frames(sock);
|
|
|
|
release_sock(sk);
|
|
|
|
remaining_len -= frag_len;
|
|
msg_ptr += frag_len;
|
|
} while (remaining_len > 0);
|
|
|
|
kfree(msg_data);
|
|
|
|
return len;
|
|
}
|
|
|
|
int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap,
|
|
struct msghdr *msg, size_t len)
|
|
{
|
|
struct sk_buff *pdu;
|
|
struct nfc_llcp_local *local;
|
|
size_t frag_len = 0, remaining_len;
|
|
u8 *msg_ptr, *msg_data;
|
|
u16 remote_miu;
|
|
int err;
|
|
|
|
pr_debug("Send UI frame len %zd\n", len);
|
|
|
|
local = sock->local;
|
|
if (local == NULL)
|
|
return -ENODEV;
|
|
|
|
msg_data = kzalloc(len, GFP_KERNEL);
|
|
if (msg_data == NULL)
|
|
return -ENOMEM;
|
|
|
|
if (memcpy_from_msg(msg_data, msg, len)) {
|
|
kfree(msg_data);
|
|
return -EFAULT;
|
|
}
|
|
|
|
remaining_len = len;
|
|
msg_ptr = msg_data;
|
|
|
|
do {
|
|
remote_miu = sock->remote_miu > LLCP_MAX_MIU ?
|
|
local->remote_miu : sock->remote_miu;
|
|
|
|
frag_len = min_t(size_t, remote_miu, remaining_len);
|
|
|
|
pr_debug("Fragment %zd bytes remaining %zd",
|
|
frag_len, remaining_len);
|
|
|
|
pdu = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT,
|
|
frag_len + LLCP_HEADER_SIZE, &err);
|
|
if (pdu == NULL) {
|
|
pr_err("Could not allocate PDU\n");
|
|
continue;
|
|
}
|
|
|
|
pdu = llcp_add_header(pdu, dsap, ssap, LLCP_PDU_UI);
|
|
|
|
if (likely(frag_len > 0))
|
|
memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len);
|
|
|
|
/* No need to check for the peer RW for UI frames */
|
|
skb_queue_tail(&local->tx_queue, pdu);
|
|
|
|
remaining_len -= frag_len;
|
|
msg_ptr += frag_len;
|
|
} while (remaining_len > 0);
|
|
|
|
kfree(msg_data);
|
|
|
|
return len;
|
|
}
|
|
|
|
int nfc_llcp_send_rr(struct nfc_llcp_sock *sock)
|
|
{
|
|
struct sk_buff *skb;
|
|
struct nfc_llcp_local *local;
|
|
|
|
pr_debug("Send rr nr %d\n", sock->recv_n);
|
|
|
|
local = sock->local;
|
|
if (local == NULL)
|
|
return -ENODEV;
|
|
|
|
skb = llcp_allocate_pdu(sock, LLCP_PDU_RR, LLCP_SEQUENCE_SIZE);
|
|
if (skb == NULL)
|
|
return -ENOMEM;
|
|
|
|
skb_put(skb, LLCP_SEQUENCE_SIZE);
|
|
|
|
skb->data[2] = sock->recv_n;
|
|
|
|
skb_queue_head(&local->tx_queue, skb);
|
|
|
|
return 0;
|
|
}
|