NFC: Extend netlink interface for LTO, RW, and MIUX parameters support

NFC_CMD_LLC_GET_PARAMS: request LTO, RW, and MIUX parameters for a device

NFC_CMD_LLC_SET_PARAMS: set one or more of LTO, RW, and MIUX parameters for
a device. LTO must be set before the link is up otherwise -EINPROGRESS is
returned. RW and MIUX can be set at anytime and will be passed in subsequent
CONNECT and CC messages. If one of the passed parameters is wrong none is
set and -EINVAL is returned.

Signed-off-by: Thierry Escande <thierry.escande@linux.intel.com>
Signed-off-by: Samuel Ortiz <sameo@linux.intel.com>

5 files changed, 174 insertions, 19 deletions

diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c
index 7941535..ed2d173 100644
--- a/net/nfc/llcp/commands.c
+++ b/net/nfc/llcp/commands.c
@@ -316,8 +316,7 @@ int nfc_llcp_send_connect(struct nfc_llcp_sock *sock)
 	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;
-	__be16 miux;
+	u8 *rw_tlv = NULL, rw_tlv_length;
 	int err;
 	u16 size = 0;
 
@@ -335,13 +334,11 @@ int nfc_llcp_send_connect(struct nfc_llcp_sock *sock)
 		size += service_name_tlv_length;
 	}
 
-	miux = cpu_to_be16(LLCP_MAX_MIUX);
-	miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0,
+	miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&local->miux, 0,
 				      &miux_tlv_length);
 	size += miux_tlv_length;
 
-	rw = LLCP_MAX_RW;
-	rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length);
+	rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &local->rw, 0, &rw_tlv_length);
 	size += rw_tlv_length;
 
 	pr_debug("SKB size %d SN length %zu\n", size, sock->service_name_len);
@@ -378,8 +375,7 @@ 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;
-	__be16 miux;
+	u8 *rw_tlv = NULL, rw_tlv_length;
 	int err;
 	u16 size = 0;
 
@@ -389,13 +385,11 @@ int nfc_llcp_send_cc(struct nfc_llcp_sock *sock)
 	if (local == NULL)
 		return -ENODEV;
 
-	miux = cpu_to_be16(LLCP_MAX_MIUX);
-	miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0,
+	miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&local->miux, 0,
 				      &miux_tlv_length);
 	size += miux_tlv_length;
 
-	rw = LLCP_MAX_RW;
-	rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length);
+	rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &local->rw, 0, &rw_tlv_length);
 	size += rw_tlv_length;
 
 	skb = llcp_allocate_pdu(sock, LLCP_PDU_CC, size);
diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
index 2e23bd3..f680453 100644
--- a/net/nfc/llcp/llcp.c
+++ b/net/nfc/llcp/llcp.c
@@ -467,10 +467,9 @@ static u8 nfc_llcp_reserve_sdp_ssap(struct nfc_llcp_local *local)
 static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
 {
 	u8 *gb_cur, *version_tlv, version, version_length;
-	u8 *lto_tlv, lto, lto_length;
+	u8 *lto_tlv, lto_length;
 	u8 *wks_tlv, wks_length;
 	u8 *miux_tlv, miux_length;
-	__be16 miux;
 	u8 gb_len = 0;
 	int ret = 0;
 
@@ -479,9 +478,7 @@ static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
 					 1, &version_length);
 	gb_len += version_length;
 
-	/* 1500 ms */
-	lto = 150;
-	lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_LTO, &lto, 1, &lto_length);
+	lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_LTO, &local->lto, 1, &lto_length);
 	gb_len += lto_length;
 
 	pr_debug("Local wks 0x%lx\n", local->local_wks);
@@ -489,8 +486,7 @@ static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
 				     &wks_length);
 	gb_len += wks_length;
 
-	miux = cpu_to_be16(LLCP_MAX_MIUX);
-	miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0,
+	miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&local->miux, 0,
 				      &miux_length);
 	gb_len += miux_length;
 
@@ -1383,6 +1379,10 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
 	rwlock_init(&local->connecting_sockets.lock);
 	rwlock_init(&local->raw_sockets.lock);
 
+	local->lto = 150; /* 1500 ms */
+	local->rw = LLCP_MAX_RW;
+	local->miux = cpu_to_be16(LLCP_MAX_MIUX);
+
 	nfc_llcp_build_gb(local);
 
 	local->remote_miu = LLCP_DEFAULT_MIU;
diff --git a/net/nfc/llcp/llcp.h b/net/nfc/llcp/llcp.h
index 276da3a..0d62366 100644
--- a/net/nfc/llcp/llcp.h
+++ b/net/nfc/llcp/llcp.h
@@ -64,6 +64,9 @@ struct nfc_llcp_local {
 	u32 target_idx;
 	u8 rf_mode;
 	u8 comm_mode;
+	u8 lto;
+	u8 rw;
+	__be16 miux;
 	unsigned long local_wks;      /* Well known services */
 	unsigned long local_sdp;      /* Local services  */
 	unsigned long local_sap; /* Local SAPs, not available for discovery */
diff --git a/net/nfc/netlink.c b/net/nfc/netlink.c
index 614cfd0..3568ae1 100644
--- a/net/nfc/netlink.c
+++ b/net/nfc/netlink.c
@@ -29,6 +29,8 @@
 
 #include "nfc.h"
 
+#include "llcp/llcp.h"
+
 static struct genl_multicast_group nfc_genl_event_mcgrp = {
 	.name = NFC_GENL_MCAST_EVENT_NAME,
 };
@@ -716,6 +718,146 @@ static int nfc_genl_dep_link_down(struct sk_buff *skb, struct genl_info *info)
 	return rc;
 }
 
+static int nfc_genl_send_params(struct sk_buff *msg,
+				struct nfc_llcp_local *local,
+				u32 portid, u32 seq)
+{
+	void *hdr;
+
+	hdr = genlmsg_put(msg, portid, seq, &nfc_genl_family, 0,
+			  NFC_CMD_LLC_GET_PARAMS);
+	if (!hdr)
+		return -EMSGSIZE;
+
+	if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, local->dev->idx) ||
+	    nla_put_u8(msg, NFC_ATTR_LLC_PARAM_LTO, local->lto) ||
+	    nla_put_u8(msg, NFC_ATTR_LLC_PARAM_RW, local->rw) ||
+	    nla_put_u16(msg, NFC_ATTR_LLC_PARAM_MIUX, be16_to_cpu(local->miux)))
+		goto nla_put_failure;
+
+	return genlmsg_end(msg, hdr);
+
+nla_put_failure:
+
+	genlmsg_cancel(msg, hdr);
+	return -EMSGSIZE;
+}
+
+static int nfc_genl_llc_get_params(struct sk_buff *skb, struct genl_info *info)
+{
+	struct nfc_dev *dev;
+	struct nfc_llcp_local *local;
+	int rc = 0;
+	struct sk_buff *msg = NULL;
+	u32 idx;
+
+	if (!info->attrs[NFC_ATTR_DEVICE_INDEX])
+		return -EINVAL;
+
+	idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+	dev = nfc_get_device(idx);
+	if (!dev)
+		return -ENODEV;
+
+	device_lock(&dev->dev);
+
+	local = nfc_llcp_find_local(dev);
+	if (!local) {
+		rc = -ENODEV;
+		goto exit;
+	}
+
+	msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+	if (!msg) {
+		rc = -ENOMEM;
+		goto exit;
+	}
+
+	rc = nfc_genl_send_params(msg, local, info->snd_portid, info->snd_seq);
+
+exit:
+	device_unlock(&dev->dev);
+
+	nfc_put_device(dev);
+
+	if (rc < 0) {
+		if (msg)
+			nlmsg_free(msg);
+
+		return rc;
+	}
+
+	return genlmsg_reply(msg, info);
+}
+
+static int nfc_genl_llc_set_params(struct sk_buff *skb, struct genl_info *info)
+{
+	struct nfc_dev *dev;
+	struct nfc_llcp_local *local;
+	u8 rw = 0;
+	u16 miux = 0;
+	u32 idx;
+	int rc = 0;
+
+	if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
+	    (!info->attrs[NFC_ATTR_LLC_PARAM_LTO] &&
+	     !info->attrs[NFC_ATTR_LLC_PARAM_RW] &&
+	     !info->attrs[NFC_ATTR_LLC_PARAM_MIUX]))
+		return -EINVAL;
+
+	if (info->attrs[NFC_ATTR_LLC_PARAM_RW]) {
+		rw = nla_get_u8(info->attrs[NFC_ATTR_LLC_PARAM_RW]);
+
+		if (rw > LLCP_MAX_RW)
+			return -EINVAL;
+	}
+
+	if (info->attrs[NFC_ATTR_LLC_PARAM_MIUX]) {
+		miux = nla_get_u16(info->attrs[NFC_ATTR_LLC_PARAM_MIUX]);
+
+		if (miux > LLCP_MAX_MIUX)
+			return -EINVAL;
+	}
+
+	idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+	dev = nfc_get_device(idx);
+	if (!dev)
+		return -ENODEV;
+
+	device_lock(&dev->dev);
+
+	local = nfc_llcp_find_local(dev);
+	if (!local) {
+		nfc_put_device(dev);
+		rc = -ENODEV;
+		goto exit;
+	}
+
+	if (info->attrs[NFC_ATTR_LLC_PARAM_LTO]) {
+		if (dev->dep_link_up) {
+			rc = -EINPROGRESS;
+			goto exit;
+		}
+
+		local->lto = nla_get_u8(info->attrs[NFC_ATTR_LLC_PARAM_LTO]);
+	}
+
+	if (info->attrs[NFC_ATTR_LLC_PARAM_RW])
+		local->rw = rw;
+
+	if (info->attrs[NFC_ATTR_LLC_PARAM_MIUX])
+		local->miux = cpu_to_be16(miux);
+
+exit:
+	device_unlock(&dev->dev);
+
+	nfc_put_device(dev);
+
+	return rc;
+}
+
 static struct genl_ops nfc_genl_ops[] = {
 	{
 		.cmd = NFC_CMD_GET_DEVICE,
@@ -760,6 +902,16 @@ static struct genl_ops nfc_genl_ops[] = {
 		.done = nfc_genl_dump_targets_done,
 		.policy = nfc_genl_policy,
 	},
+	{
+		.cmd = NFC_CMD_LLC_GET_PARAMS,
+		.doit = nfc_genl_llc_get_params,
+		.policy = nfc_genl_policy,
+	},
+	{
+		.cmd = NFC_CMD_LLC_SET_PARAMS,
+		.doit = nfc_genl_llc_set_params,
+		.policy = nfc_genl_policy,
+	},
 };
 
 
diff --git a/net/nfc/nfc.h b/net/nfc/nfc.h
index c5e42b79..87d914d 100644
--- a/net/nfc/nfc.h
+++ b/net/nfc/nfc.h
@@ -56,6 +56,7 @@ void nfc_llcp_unregister_device(struct nfc_dev *dev);
 int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len);
 u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len);
 int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb);
+struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev);
 int __init nfc_llcp_init(void);
 void nfc_llcp_exit(void);
 
@@ -97,6 +98,11 @@ static inline int nfc_llcp_data_received(struct nfc_dev *dev,
 	return 0;
 }
 
+static inline struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev)
+{
+	return NULL;
+}
+
 static inline int nfc_llcp_init(void)
 {
 	return 0;