summary refs log tree commit diff
path: root/net/nfc
diff options
context:
space:
mode:
Diffstat (limited to 'net/nfc')
-rw-r--r--net/nfc/Kconfig4
-rw-r--r--net/nfc/core.c33
-rw-r--r--net/nfc/hci/command.c28
-rw-r--r--net/nfc/hci/core.c90
-rw-r--r--net/nfc/hci/llc.c2
-rw-r--r--net/nfc/hci/llc_shdlc.c7
-rw-r--r--net/nfc/llcp/Kconfig4
-rw-r--r--net/nfc/llcp/commands.c148
-rw-r--r--net/nfc/llcp/llcp.c248
-rw-r--r--net/nfc/llcp/llcp.h13
-rw-r--r--net/nfc/llcp/sock.c42
-rw-r--r--net/nfc/nci/Kconfig4
-rw-r--r--net/nfc/nci/core.c29
-rw-r--r--net/nfc/netlink.c157
-rw-r--r--net/nfc/nfc.h6
-rw-r--r--net/nfc/rawsock.c1
16 files changed, 692 insertions, 124 deletions
diff --git a/net/nfc/Kconfig b/net/nfc/Kconfig
index 8d8d9bc4b6ff..60c3bbb63e8e 100644
--- a/net/nfc/Kconfig
+++ b/net/nfc/Kconfig
@@ -3,8 +3,8 @@
 #
 
 menuconfig NFC
-	depends on NET && EXPERIMENTAL
-	tristate "NFC subsystem support (EXPERIMENTAL)"
+	depends on NET
+	tristate "NFC subsystem support"
 	default n
 	help
 	  Say Y here if you want to build support for NFC (Near field
diff --git a/net/nfc/core.c b/net/nfc/core.c
index 479bee36dc3e..aa64ea441676 100644
--- a/net/nfc/core.c
+++ b/net/nfc/core.c
@@ -40,6 +40,9 @@
 int nfc_devlist_generation;
 DEFINE_MUTEX(nfc_devlist_mutex);
 
+/* NFC device ID bitmap */
+static DEFINE_IDA(nfc_index_ida);
+
 /**
  * nfc_dev_up - turn on the NFC device
  *
@@ -181,6 +184,7 @@ int nfc_stop_poll(struct nfc_dev *dev)
 
 	dev->ops->stop_poll(dev);
 	dev->polling = false;
+	dev->rf_mode = NFC_RF_NONE;
 
 error:
 	device_unlock(&dev->dev);
@@ -194,7 +198,7 @@ static struct nfc_target *nfc_find_target(struct nfc_dev *dev, u32 target_idx)
 	if (dev->n_targets == 0)
 		return NULL;
 
-	for (i = 0; i < dev->n_targets ; i++) {
+	for (i = 0; i < dev->n_targets; i++) {
 		if (dev->targets[i].idx == target_idx)
 			return &dev->targets[i];
 	}
@@ -274,12 +278,14 @@ int nfc_dep_link_down(struct nfc_dev *dev)
 	if (!rc) {
 		dev->dep_link_up = false;
 		dev->active_target = NULL;
+		dev->rf_mode = NFC_RF_NONE;
 		nfc_llcp_mac_is_down(dev);
 		nfc_genl_dep_link_down_event(dev);
 	}
 
 error:
 	device_unlock(&dev->dev);
+
 	return rc;
 }
 
@@ -503,6 +509,7 @@ EXPORT_SYMBOL(nfc_tm_activated);
 int nfc_tm_deactivated(struct nfc_dev *dev)
 {
 	dev->dep_link_up = false;
+	dev->rf_mode = NFC_RF_NONE;
 
 	return nfc_genl_tm_deactivated(dev);
 }
@@ -697,6 +704,8 @@ static void nfc_check_pres_work(struct work_struct *work)
 
 	if (dev->active_target && timer_pending(&dev->check_pres_timer) == 0) {
 		rc = dev->ops->check_presence(dev, dev->active_target);
+		if (rc == -EOPNOTSUPP)
+			goto exit;
 		if (!rc) {
 			mod_timer(&dev->check_pres_timer, jiffies +
 				  msecs_to_jiffies(NFC_CHECK_PRES_FREQ_MS));
@@ -708,6 +717,7 @@ static void nfc_check_pres_work(struct work_struct *work)
 		}
 	}
 
+exit:
 	device_unlock(&dev->dev);
 }
 
@@ -753,7 +763,6 @@ struct nfc_dev *nfc_allocate_device(struct nfc_ops *ops,
 				    u32 supported_protocols,
 				    int tx_headroom, int tx_tailroom)
 {
-	static atomic_t dev_no = ATOMIC_INIT(0);
 	struct nfc_dev *dev;
 
 	if (!ops->start_poll || !ops->stop_poll || !ops->activate_target ||
@@ -767,11 +776,6 @@ struct nfc_dev *nfc_allocate_device(struct nfc_ops *ops,
 	if (!dev)
 		return NULL;
 
-	dev->dev.class = &nfc_class;
-	dev->idx = atomic_inc_return(&dev_no) - 1;
-	dev_set_name(&dev->dev, "nfc%d", dev->idx);
-	device_initialize(&dev->dev);
-
 	dev->ops = ops;
 	dev->supported_protocols = supported_protocols;
 	dev->tx_headroom = tx_headroom;
@@ -779,6 +783,7 @@ struct nfc_dev *nfc_allocate_device(struct nfc_ops *ops,
 
 	nfc_genl_data_init(&dev->genl_data);
 
+	dev->rf_mode = NFC_RF_NONE;
 
 	/* first generation must not be 0 */
 	dev->targets_generation = 1;
@@ -806,6 +811,14 @@ int nfc_register_device(struct nfc_dev *dev)
 
 	pr_debug("dev_name=%s\n", dev_name(&dev->dev));
 
+	dev->idx = ida_simple_get(&nfc_index_ida, 0, 0, GFP_KERNEL);
+	if (dev->idx < 0)
+		return dev->idx;
+
+	dev->dev.class = &nfc_class;
+	dev_set_name(&dev->dev, "nfc%d", dev->idx);
+	device_initialize(&dev->dev);
+
 	mutex_lock(&nfc_devlist_mutex);
 	nfc_devlist_generation++;
 	rc = device_add(&dev->dev);
@@ -834,10 +847,12 @@ EXPORT_SYMBOL(nfc_register_device);
  */
 void nfc_unregister_device(struct nfc_dev *dev)
 {
-	int rc;
+	int rc, id;
 
 	pr_debug("dev_name=%s\n", dev_name(&dev->dev));
 
+	id = dev->idx;
+
 	mutex_lock(&nfc_devlist_mutex);
 	nfc_devlist_generation++;
 
@@ -856,6 +871,8 @@ void nfc_unregister_device(struct nfc_dev *dev)
 		pr_debug("The userspace won't be notified that the device %s was removed\n",
 			 dev_name(&dev->dev));
 
+	ida_simple_remove(&nfc_index_ida, id);
+
 }
 EXPORT_SYMBOL(nfc_unregister_device);
 
diff --git a/net/nfc/hci/command.c b/net/nfc/hci/command.c
index 71c6a7086b8f..7d99410e6c1a 100644
--- a/net/nfc/hci/command.c
+++ b/net/nfc/hci/command.c
@@ -257,16 +257,16 @@ static u8 nfc_hci_create_pipe(struct nfc_hci_dev *hdev, u8 dest_host,
 	*result = nfc_hci_execute_cmd(hdev, NFC_HCI_ADMIN_PIPE,
 				      NFC_HCI_ADM_CREATE_PIPE,
 				      (u8 *) &params, sizeof(params), &skb);
-	if (*result == 0) {
-		resp = (struct hci_create_pipe_resp *)skb->data;
-		pipe = resp->pipe;
-		kfree_skb(skb);
+	if (*result < 0)
+		return NFC_HCI_INVALID_PIPE;
 
-		pr_debug("pipe created=%d\n", pipe);
+	resp = (struct hci_create_pipe_resp *)skb->data;
+	pipe = resp->pipe;
+	kfree_skb(skb);
 
-		return pipe;
-	} else
-		return NFC_HCI_INVALID_PIPE;
+	pr_debug("pipe created=%d\n", pipe);
+
+	return pipe;
 }
 
 static int nfc_hci_delete_pipe(struct nfc_hci_dev *hdev, u8 pipe)
@@ -279,8 +279,6 @@ static int nfc_hci_delete_pipe(struct nfc_hci_dev *hdev, u8 pipe)
 
 static int nfc_hci_clear_all_pipes(struct nfc_hci_dev *hdev)
 {
-	int r;
-
 	u8 param[2];
 
 	/* TODO: Find out what the identity reference data is
@@ -288,10 +286,8 @@ static int nfc_hci_clear_all_pipes(struct nfc_hci_dev *hdev)
 
 	pr_debug("\n");
 
-	r = nfc_hci_execute_cmd(hdev, NFC_HCI_ADMIN_PIPE,
-				NFC_HCI_ADM_CLEAR_ALL_PIPE, param, 2, NULL);
-
-	return 0;
+	return nfc_hci_execute_cmd(hdev, NFC_HCI_ADMIN_PIPE,
+				   NFC_HCI_ADM_CLEAR_ALL_PIPE, param, 2, NULL);
 }
 
 int nfc_hci_disconnect_gate(struct nfc_hci_dev *hdev, u8 gate)
@@ -348,7 +344,7 @@ int nfc_hci_connect_gate(struct nfc_hci_dev *hdev, u8 dest_host, u8 dest_gate,
 		return -EADDRINUSE;
 
 	if (pipe != NFC_HCI_INVALID_PIPE)
-		goto pipe_is_open;
+		goto open_pipe;
 
 	switch (dest_gate) {
 	case NFC_HCI_LINK_MGMT_GATE:
@@ -365,6 +361,7 @@ int nfc_hci_connect_gate(struct nfc_hci_dev *hdev, u8 dest_host, u8 dest_gate,
 		break;
 	}
 
+open_pipe:
 	r = nfc_hci_open_pipe(hdev, pipe);
 	if (r < 0) {
 		if (pipe_created)
@@ -375,7 +372,6 @@ int nfc_hci_connect_gate(struct nfc_hci_dev *hdev, u8 dest_host, u8 dest_gate,
 		return r;
 	}
 
-pipe_is_open:
 	hdev->gate2pipe[dest_gate] = pipe;
 
 	return 0;
diff --git a/net/nfc/hci/core.c b/net/nfc/hci/core.c
index 5fbb6e40793e..7bea574d5934 100644
--- a/net/nfc/hci/core.c
+++ b/net/nfc/hci/core.c
@@ -33,17 +33,20 @@
 /* Largest headroom needed for outgoing HCI commands */
 #define HCI_CMDS_HEADROOM 1
 
-static int nfc_hci_result_to_errno(u8 result)
+int nfc_hci_result_to_errno(u8 result)
 {
 	switch (result) {
 	case NFC_HCI_ANY_OK:
 		return 0;
+	case NFC_HCI_ANY_E_REG_PAR_UNKNOWN:
+		return -EOPNOTSUPP;
 	case NFC_HCI_ANY_E_TIMEOUT:
 		return -ETIME;
 	default:
 		return -1;
 	}
 }
+EXPORT_SYMBOL(nfc_hci_result_to_errno);
 
 static void nfc_hci_msg_tx_work(struct work_struct *work)
 {
@@ -65,8 +68,9 @@ static void nfc_hci_msg_tx_work(struct work_struct *work)
 							  -ETIME);
 			kfree(hdev->cmd_pending_msg);
 			hdev->cmd_pending_msg = NULL;
-		} else
+		} else {
 			goto exit;
+		}
 	}
 
 next_msg:
@@ -166,7 +170,7 @@ void nfc_hci_cmd_received(struct nfc_hci_dev *hdev, u8 pipe, u8 cmd,
 	kfree_skb(skb);
 }
 
-static u32 nfc_hci_sak_to_protocol(u8 sak)
+u32 nfc_hci_sak_to_protocol(u8 sak)
 {
 	switch (NFC_HCI_TYPE_A_SEL_PROT(sak)) {
 	case NFC_HCI_TYPE_A_SEL_PROT_MIFARE:
@@ -181,8 +185,9 @@ static u32 nfc_hci_sak_to_protocol(u8 sak)
 		return 0xffffffff;
 	}
 }
+EXPORT_SYMBOL(nfc_hci_sak_to_protocol);
 
-static int nfc_hci_target_discovered(struct nfc_hci_dev *hdev, u8 gate)
+int nfc_hci_target_discovered(struct nfc_hci_dev *hdev, u8 gate)
 {
 	struct nfc_target *targets;
 	struct sk_buff *atqa_skb = NULL;
@@ -263,7 +268,9 @@ static int nfc_hci_target_discovered(struct nfc_hci_dev *hdev, u8 gate)
 		break;
 	}
 
-	targets->hci_reader_gate = gate;
+	/* if driver set the new gate, we will skip the old one */
+	if (targets->hci_reader_gate == 0x00)
+		targets->hci_reader_gate = gate;
 
 	r = nfc_targets_found(hdev->ndev, targets, 1);
 
@@ -275,11 +282,18 @@ exit:
 
 	return r;
 }
+EXPORT_SYMBOL(nfc_hci_target_discovered);
 
 void nfc_hci_event_received(struct nfc_hci_dev *hdev, u8 pipe, u8 event,
 			    struct sk_buff *skb)
 {
 	int r = 0;
+	u8 gate = nfc_hci_pipe2gate(hdev, pipe);
+
+	if (gate == 0xff) {
+		pr_err("Discarded event %x to unopened pipe %x\n", event, pipe);
+		goto exit;
+	}
 
 	switch (event) {
 	case NFC_HCI_EVT_TARGET_DISCOVERED:
@@ -303,12 +317,14 @@ void nfc_hci_event_received(struct nfc_hci_dev *hdev, u8 pipe, u8 event,
 			goto exit;
 		}
 
-		r = nfc_hci_target_discovered(hdev,
-					      nfc_hci_pipe2gate(hdev, pipe));
+		r = nfc_hci_target_discovered(hdev, gate);
 		break;
 	default:
-		/* TODO: Unknown events are hardware specific
-		 * pass them to the driver (needs a new hci_ops) */
+		if (hdev->ops->event_received) {
+			hdev->ops->event_received(hdev, gate, event, skb);
+			return;
+		}
+
 		break;
 	}
 
@@ -410,6 +426,10 @@ static int hci_dev_version(struct nfc_hci_dev *hdev)
 
 	r = nfc_hci_get_param(hdev, NFC_HCI_ID_MGMT_GATE,
 			      NFC_HCI_ID_MGMT_VERSION_SW, &skb);
+	if (r == -EOPNOTSUPP) {
+		pr_info("Software/Hardware info not available\n");
+		return 0;
+	}
 	if (r < 0)
 		return r;
 
@@ -527,7 +547,8 @@ static int hci_start_poll(struct nfc_dev *nfc_dev,
 		return hdev->ops->start_poll(hdev, im_protocols, tm_protocols);
 	else
 		return nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
-				       NFC_HCI_EVT_READER_REQUESTED, NULL, 0);
+					  NFC_HCI_EVT_READER_REQUESTED,
+					  NULL, 0);
 }
 
 static void hci_stop_poll(struct nfc_dev *nfc_dev)
@@ -538,6 +559,28 @@ static void hci_stop_poll(struct nfc_dev *nfc_dev)
 			   NFC_HCI_EVT_END_OPERATION, NULL, 0);
 }
 
+static int hci_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
+				__u8 comm_mode, __u8 *gb, size_t gb_len)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+	if (hdev->ops->dep_link_up)
+		return hdev->ops->dep_link_up(hdev, target, comm_mode,
+						gb, gb_len);
+
+	return 0;
+}
+
+static int hci_dep_link_down(struct nfc_dev *nfc_dev)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+	if (hdev->ops->dep_link_down)
+		return hdev->ops->dep_link_down(hdev);
+
+	return 0;
+}
+
 static int hci_activate_target(struct nfc_dev *nfc_dev,
 			       struct nfc_target *target, u32 protocol)
 {
@@ -586,8 +629,8 @@ static int hci_transceive(struct nfc_dev *nfc_dev, struct nfc_target *target,
 	switch (target->hci_reader_gate) {
 	case NFC_HCI_RF_READER_A_GATE:
 	case NFC_HCI_RF_READER_B_GATE:
-		if (hdev->ops->data_exchange) {
-			r = hdev->ops->data_exchange(hdev, target, skb, cb,
+		if (hdev->ops->im_transceive) {
+			r = hdev->ops->im_transceive(hdev, target, skb, cb,
 						     cb_context);
 			if (r <= 0)	/* handled */
 				break;
@@ -604,14 +647,14 @@ static int hci_transceive(struct nfc_dev *nfc_dev, struct nfc_target *target,
 					   skb->len, hci_transceive_cb, hdev);
 		break;
 	default:
-		if (hdev->ops->data_exchange) {
-			r = hdev->ops->data_exchange(hdev, target, skb, cb,
+		if (hdev->ops->im_transceive) {
+			r = hdev->ops->im_transceive(hdev, target, skb, cb,
 						     cb_context);
 			if (r == 1)
 				r = -ENOTSUPP;
-		}
-		else
+		} else {
 			r = -ENOTSUPP;
+		}
 		break;
 	}
 
@@ -620,6 +663,16 @@ static int hci_transceive(struct nfc_dev *nfc_dev, struct nfc_target *target,
 	return r;
 }
 
+static int hci_tm_send(struct nfc_dev *nfc_dev, struct sk_buff *skb)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+	if (hdev->ops->tm_send)
+		return hdev->ops->tm_send(hdev, skb);
+	else
+		return -ENOTSUPP;
+}
+
 static int hci_check_presence(struct nfc_dev *nfc_dev,
 			      struct nfc_target *target)
 {
@@ -723,9 +776,12 @@ static struct nfc_ops hci_nfc_ops = {
 	.dev_down = hci_dev_down,
 	.start_poll = hci_start_poll,
 	.stop_poll = hci_stop_poll,
+	.dep_link_up = hci_dep_link_up,
+	.dep_link_down = hci_dep_link_down,
 	.activate_target = hci_activate_target,
 	.deactivate_target = hci_deactivate_target,
 	.im_transceive = hci_transceive,
+	.tm_send = hci_tm_send,
 	.check_presence = hci_check_presence,
 };
 
@@ -848,7 +904,7 @@ void nfc_hci_driver_failure(struct nfc_hci_dev *hdev, int err)
 }
 EXPORT_SYMBOL(nfc_hci_driver_failure);
 
-void inline nfc_hci_recv_frame(struct nfc_hci_dev *hdev, struct sk_buff *skb)
+void nfc_hci_recv_frame(struct nfc_hci_dev *hdev, struct sk_buff *skb)
 {
 	nfc_llc_rcv_from_drv(hdev->llc, skb);
 }
diff --git a/net/nfc/hci/llc.c b/net/nfc/hci/llc.c
index ae1205ded87f..fe5e966e5b88 100644
--- a/net/nfc/hci/llc.c
+++ b/net/nfc/hci/llc.c
@@ -72,7 +72,7 @@ int nfc_llc_register(const char *name, struct nfc_llc_ops *ops)
 	llc_engine->ops = ops;
 
 	INIT_LIST_HEAD(&llc_engine->entry);
-	list_add_tail (&llc_engine->entry, &llc_engines);
+	list_add_tail(&llc_engine->entry, &llc_engines);
 
 	return 0;
 }
diff --git a/net/nfc/hci/llc_shdlc.c b/net/nfc/hci/llc_shdlc.c
index 01cbc72943cd..27b313befc35 100644
--- a/net/nfc/hci/llc_shdlc.c
+++ b/net/nfc/hci/llc_shdlc.c
@@ -634,9 +634,9 @@ static void llc_shdlc_sm_work(struct work_struct *work)
 			r = llc_shdlc_connect_initiate(shdlc);
 		else
 			r = -ETIME;
-		if (r < 0)
+		if (r < 0) {
 			llc_shdlc_connect_complete(shdlc, r);
-		else {
+		} else {
 			mod_timer(&shdlc->connect_timer, jiffies +
 				  msecs_to_jiffies(SHDLC_CONNECT_VALUE_MS));
 
@@ -682,9 +682,8 @@ static void llc_shdlc_sm_work(struct work_struct *work)
 			llc_shdlc_handle_send_queue(shdlc);
 		}
 
-		if (shdlc->hard_fault) {
+		if (shdlc->hard_fault)
 			shdlc->llc_failure(shdlc->hdev, shdlc->hard_fault);
-		}
 		break;
 	default:
 		break;
diff --git a/net/nfc/llcp/Kconfig b/net/nfc/llcp/Kconfig
index fbf5e8150908..a1a41cd68255 100644
--- a/net/nfc/llcp/Kconfig
+++ b/net/nfc/llcp/Kconfig
@@ -1,6 +1,6 @@
 config NFC_LLCP
-       depends on NFC && EXPERIMENTAL
-       bool "NFC LLCP support (EXPERIMENTAL)"
+       depends on NFC
+       bool "NFC LLCP support"
        default n
        help
 	 Say Y here if you want to build support for a kernel NFC LLCP
diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c
index c45ccd6c094c..df24be48d4da 100644
--- a/net/nfc/llcp/commands.c
+++ b/net/nfc/llcp/commands.c
@@ -261,7 +261,6 @@ int nfc_llcp_disconnect(struct nfc_llcp_sock *sock)
 	struct sk_buff *skb;
 	struct nfc_dev *dev;
 	struct nfc_llcp_local *local;
-	u16 size = 0;
 
 	pr_debug("Sending DISC\n");
 
@@ -273,17 +272,10 @@ int nfc_llcp_disconnect(struct nfc_llcp_sock *sock)
 	if (dev == NULL)
 		return -ENODEV;
 
-	size += LLCP_HEADER_SIZE;
-	size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
-
-	skb = alloc_skb(size, GFP_ATOMIC);
+	skb = llcp_allocate_pdu(sock, LLCP_PDU_DISC, 0);
 	if (skb == NULL)
 		return -ENOMEM;
 
-	skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
-
-	skb = llcp_add_header(skb, sock->dsap, sock->ssap, LLCP_PDU_DISC);
-
 	skb_queue_tail(&local->tx_queue, skb);
 
 	return 0;
@@ -324,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;
 
@@ -343,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);
@@ -386,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;
 
@@ -397,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);
@@ -428,6 +414,52 @@ error_tlv:
 	return err;
 }
 
+int nfc_llcp_send_snl(struct nfc_llcp_local *local, u8 tid, u8 sap)
+{
+	struct sk_buff *skb;
+	struct nfc_dev *dev;
+	u8 *sdres_tlv = NULL, sdres_tlv_length, sdres[2];
+	u16 size = 0;
+
+	pr_debug("Sending SNL tid 0x%x sap 0x%x\n", tid, sap);
+
+	if (local == NULL)
+		return -ENODEV;
+
+	dev = local->dev;
+	if (dev == NULL)
+		return -ENODEV;
+
+	sdres[0] = tid;
+	sdres[1] = sap;
+	sdres_tlv = nfc_llcp_build_tlv(LLCP_TLV_SDRES, sdres, 0,
+				       &sdres_tlv_length);
+	if (sdres_tlv == NULL)
+		return -ENOMEM;
+
+	size += LLCP_HEADER_SIZE;
+	size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
+	size += sdres_tlv_length;
+
+	skb = alloc_skb(size, GFP_KERNEL);
+	if (skb == NULL) {
+		kfree(sdres_tlv);
+		return -ENOMEM;
+	}
+
+	skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
+
+	skb = llcp_add_header(skb, LLCP_SAP_SDP, LLCP_SAP_SDP, LLCP_PDU_SNL);
+
+	memcpy(skb_put(skb, sdres_tlv_length), sdres_tlv, sdres_tlv_length);
+
+	skb_queue_tail(&local->tx_queue, skb);
+
+	kfree(sdres_tlv);
+
+	return 0;
+}
+
 int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason)
 {
 	struct sk_buff *skb;
@@ -496,6 +528,23 @@ int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
 	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->rw &&
+	    skb_queue_len(&sock->tx_queue) >= 2 * sock->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->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;
@@ -541,6 +590,63 @@ int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
 	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;
+	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_fromiovec(msg_data, msg->msg_iov, len)) {
+		kfree(msg_data);
+		return -EFAULT;
+	}
+
+	remaining_len = len;
+	msg_ptr = msg_data;
+
+	while (remaining_len > 0) {
+
+		frag_len = min_t(size_t, sock->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);
+
+		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;
+	}
+
+	kfree(msg_data);
+
+	return len;
+}
+
 int nfc_llcp_send_rr(struct nfc_llcp_sock *sock)
 {
 	struct sk_buff *skb;
diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
index 9e8f4b2801f6..ec43914c92a9 100644
--- a/net/nfc/llcp/llcp.c
+++ b/net/nfc/llcp/llcp.c
@@ -45,12 +45,38 @@ void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *sk)
 	write_unlock(&l->lock);
 }
 
+static void nfc_llcp_socket_purge(struct nfc_llcp_sock *sock)
+{
+	struct nfc_llcp_local *local = sock->local;
+	struct sk_buff *s, *tmp;
+
+	pr_debug("%p\n", &sock->sk);
+
+	skb_queue_purge(&sock->tx_queue);
+	skb_queue_purge(&sock->tx_pending_queue);
+	skb_queue_purge(&sock->tx_backlog_queue);
+
+	if (local == NULL)
+		return;
+
+	/* Search for local pending SKBs that are related to this socket */
+	skb_queue_walk_safe(&local->tx_queue, s, tmp) {
+		if (s->sk != &sock->sk)
+			continue;
+
+		skb_unlink(s, &local->tx_queue);
+		kfree_skb(s);
+	}
+}
+
 static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen)
 {
 	struct sock *sk;
 	struct hlist_node *node, *tmp;
 	struct nfc_llcp_sock *llcp_sock;
 
+	skb_queue_purge(&local->tx_queue);
+
 	write_lock(&local->sockets.lock);
 
 	sk_for_each_safe(sk, node, tmp, &local->sockets.head) {
@@ -58,6 +84,8 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen)
 
 		bh_lock_sock(sk);
 
+		nfc_llcp_socket_purge(llcp_sock);
+
 		if (sk->sk_state == LLCP_CONNECTED)
 			nfc_put_device(llcp_sock->dev);
 
@@ -65,7 +93,8 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen)
 			struct nfc_llcp_sock *lsk, *n;
 			struct sock *accept_sk;
 
-			list_for_each_entry_safe(lsk, n, &llcp_sock->accept_queue,
+			list_for_each_entry_safe(lsk, n,
+						 &llcp_sock->accept_queue,
 						 accept_queue) {
 				accept_sk = &lsk->sk;
 				bh_lock_sock(accept_sk);
@@ -85,6 +114,16 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen)
 			}
 		}
 
+		/*
+		 * If we have a connection less socket bound, we keep it alive
+		 * if the device is still present.
+		 */
+		if (sk->sk_state == LLCP_BOUND && sk->sk_type == SOCK_DGRAM &&
+		    listen == true) {
+			bh_unlock_sock(sk);
+			continue;
+		}
+
 		sk->sk_state = LLCP_CLOSED;
 
 		bh_unlock_sock(sk);
@@ -134,7 +173,7 @@ static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
 {
 	struct sock *sk;
 	struct hlist_node *node;
-	struct nfc_llcp_sock *llcp_sock;
+	struct nfc_llcp_sock *llcp_sock, *tmp_sock;
 
 	pr_debug("ssap dsap %d %d\n", ssap, dsap);
 
@@ -146,10 +185,12 @@ static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
 	llcp_sock = NULL;
 
 	sk_for_each(sk, node, &local->sockets.head) {
-		llcp_sock = nfc_llcp_sock(sk);
+		tmp_sock = nfc_llcp_sock(sk);
 
-		if (llcp_sock->ssap == ssap && llcp_sock->dsap == dsap)
+		if (tmp_sock->ssap == ssap && tmp_sock->dsap == dsap) {
+			llcp_sock = tmp_sock;
 			break;
+		}
 	}
 
 	read_unlock(&local->sockets.lock);
@@ -249,7 +290,12 @@ struct nfc_llcp_sock *nfc_llcp_sock_from_sn(struct nfc_llcp_local *local,
 
 		pr_debug("llcp sock %p\n", tmp_sock);
 
-		if (tmp_sock->sk.sk_state != LLCP_LISTEN)
+		if (tmp_sock->sk.sk_type == SOCK_STREAM &&
+		    tmp_sock->sk.sk_state != LLCP_LISTEN)
+			continue;
+
+		if (tmp_sock->sk.sk_type == SOCK_DGRAM &&
+		    tmp_sock->sk.sk_state != LLCP_BOUND)
 			continue;
 
 		if (tmp_sock->service_name == NULL ||
@@ -421,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;
 
@@ -433,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);
@@ -443,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;
 
@@ -610,7 +652,12 @@ static void nfc_llcp_tx_work(struct work_struct *work)
 	if (skb != NULL) {
 		sk = skb->sk;
 		llcp_sock = nfc_llcp_sock(sk);
-		if (llcp_sock != NULL) {
+
+		if (llcp_sock == NULL && nfc_llcp_ptype(skb) == LLCP_PDU_I) {
+			nfc_llcp_send_symm(local->dev);
+		} else {
+			struct sk_buff *copy_skb = NULL;
+			u8 ptype = nfc_llcp_ptype(skb);
 			int ret;
 
 			pr_debug("Sending pending skb\n");
@@ -618,24 +665,29 @@ static void nfc_llcp_tx_work(struct work_struct *work)
 				       DUMP_PREFIX_OFFSET, 16, 1,
 				       skb->data, skb->len, true);
 
+			if (ptype == LLCP_PDU_I)
+				copy_skb = skb_copy(skb, GFP_ATOMIC);
+
 			nfc_llcp_send_to_raw_sock(local, skb,
 						  NFC_LLCP_DIRECTION_TX);
 
 			ret = nfc_data_exchange(local->dev, local->target_idx,
 						skb, nfc_llcp_recv, local);
 
-			if (!ret && nfc_llcp_ptype(skb) == LLCP_PDU_I) {
-				skb = skb_get(skb);
-				skb_queue_tail(&llcp_sock->tx_pending_queue,
-					       skb);
+			if (ret) {
+				kfree_skb(copy_skb);
+				goto out;
 			}
-		} else {
-			nfc_llcp_send_symm(local->dev);
+
+			if (ptype == LLCP_PDU_I && copy_skb)
+				skb_queue_tail(&llcp_sock->tx_pending_queue,
+					       copy_skb);
 		}
 	} else {
 		nfc_llcp_send_symm(local->dev);
 	}
 
+out:
 	mod_timer(&local->link_timer,
 		  jiffies + msecs_to_jiffies(2 * local->remote_lto));
 }
@@ -704,6 +756,39 @@ static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len)
 	return NULL;
 }
 
+static void nfc_llcp_recv_ui(struct nfc_llcp_local *local,
+			     struct sk_buff *skb)
+{
+	struct nfc_llcp_sock *llcp_sock;
+	struct nfc_llcp_ui_cb *ui_cb;
+	u8 dsap, ssap;
+
+	dsap = nfc_llcp_dsap(skb);
+	ssap = nfc_llcp_ssap(skb);
+
+	ui_cb = nfc_llcp_ui_skb_cb(skb);
+	ui_cb->dsap = dsap;
+	ui_cb->ssap = ssap;
+
+	printk("%s %d %d\n", __func__, dsap, ssap);
+
+	pr_debug("%d %d\n", dsap, ssap);
+
+	/* We're looking for a bound socket, not a client one */
+	llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
+	if (llcp_sock == NULL || llcp_sock->sk.sk_type != SOCK_DGRAM)
+		return;
+
+	/* There is no sequence with UI frames */
+	skb_pull(skb, LLCP_HEADER_SIZE);
+	if (sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
+		pr_err("receive queue is full\n");
+		skb_queue_head(&llcp_sock->tx_backlog_queue, skb);
+	}
+
+	nfc_llcp_sock_put(llcp_sock);
+}
+
 static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
 				  struct sk_buff *skb)
 {
@@ -823,9 +908,6 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
 fail:
 	/* Send DM */
 	nfc_llcp_send_dm(local, dsap, ssap, reason);
-
-	return;
-
 }
 
 int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock)
@@ -903,15 +985,18 @@ static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
 	/* Remove skbs from the pending queue */
 	if (llcp_sock->send_ack_n != nr) {
 		struct sk_buff *s, *tmp;
+		u8 n;
 
 		llcp_sock->send_ack_n = nr;
 
 		/* Remove and free all skbs until ns == nr */
 		skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp) {
+			n = nfc_llcp_ns(s);
+
 			skb_unlink(s, &llcp_sock->tx_pending_queue);
 			kfree_skb(s);
 
-			if (nfc_llcp_ns(s) == nr)
+			if (n == nr)
 				break;
 		}
 
@@ -953,6 +1038,9 @@ static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
 
 	sk = &llcp_sock->sk;
 	lock_sock(sk);
+
+	nfc_llcp_socket_purge(llcp_sock);
+
 	if (sk->sk_state == LLCP_CLOSED) {
 		release_sock(sk);
 		nfc_llcp_sock_put(llcp_sock);
@@ -1027,7 +1115,7 @@ static void nfc_llcp_recv_dm(struct nfc_llcp_local *local, struct sk_buff *skb)
 	}
 
 	if (llcp_sock == NULL) {
-		pr_err("Invalid DM\n");
+		pr_debug("Already closed\n");
 		return;
 	}
 
@@ -1038,8 +1126,100 @@ static void nfc_llcp_recv_dm(struct nfc_llcp_local *local, struct sk_buff *skb)
 	sk->sk_state_change(sk);
 
 	nfc_llcp_sock_put(llcp_sock);
+}
 
-	return;
+static void nfc_llcp_recv_snl(struct nfc_llcp_local *local,
+			      struct sk_buff *skb)
+{
+	struct nfc_llcp_sock *llcp_sock;
+	u8 dsap, ssap, *tlv, type, length, tid, sap;
+	u16 tlv_len, offset;
+	char *service_name;
+	size_t service_name_len;
+
+	dsap = nfc_llcp_dsap(skb);
+	ssap = nfc_llcp_ssap(skb);
+
+	pr_debug("%d %d\n", dsap, ssap);
+
+	if (dsap != LLCP_SAP_SDP || ssap != LLCP_SAP_SDP) {
+		pr_err("Wrong SNL SAP\n");
+		return;
+	}
+
+	tlv = &skb->data[LLCP_HEADER_SIZE];
+	tlv_len = skb->len - LLCP_HEADER_SIZE;
+	offset = 0;
+
+	while (offset < tlv_len) {
+		type = tlv[0];
+		length = tlv[1];
+
+		switch (type) {
+		case LLCP_TLV_SDREQ:
+			tid = tlv[2];
+			service_name = (char *) &tlv[3];
+			service_name_len = length - 1;
+
+			pr_debug("Looking for %.16s\n", service_name);
+
+			if (service_name_len == strlen("urn:nfc:sn:sdp") &&
+			    !strncmp(service_name, "urn:nfc:sn:sdp",
+				     service_name_len)) {
+				sap = 1;
+				goto send_snl;
+			}
+
+			llcp_sock = nfc_llcp_sock_from_sn(local, service_name,
+							  service_name_len);
+			if (!llcp_sock) {
+				sap = 0;
+				goto send_snl;
+			}
+
+			/*
+			 * We found a socket but its ssap has not been reserved
+			 * yet. We need to assign it for good and send a reply.
+			 * The ssap will be freed when the socket is closed.
+			 */
+			if (llcp_sock->ssap == LLCP_SDP_UNBOUND) {
+				atomic_t *client_count;
+
+				sap = nfc_llcp_reserve_sdp_ssap(local);
+
+				pr_debug("Reserving %d\n", sap);
+
+				if (sap == LLCP_SAP_MAX) {
+					sap = 0;
+					goto send_snl;
+				}
+
+				client_count =
+					&local->local_sdp_cnt[sap -
+							      LLCP_WKS_NUM_SAP];
+
+				atomic_inc(client_count);
+
+				llcp_sock->ssap = sap;
+				llcp_sock->reserved_ssap = sap;
+			} else {
+				sap = llcp_sock->ssap;
+			}
+
+			pr_debug("%p %d\n", llcp_sock, sap);
+
+send_snl:
+			nfc_llcp_send_snl(local, tid, sap);
+			break;
+
+		default:
+			pr_err("Invalid SNL tlv value 0x%x\n", type);
+			break;
+		}
+
+		offset += length + 2;
+		tlv += length + 2;
+	}
 }
 
 static void nfc_llcp_rx_work(struct work_struct *work)
@@ -1072,6 +1252,11 @@ static void nfc_llcp_rx_work(struct work_struct *work)
 		pr_debug("SYMM\n");
 		break;
 
+	case LLCP_PDU_UI:
+		pr_debug("UI\n");
+		nfc_llcp_recv_ui(local, skb);
+		break;
+
 	case LLCP_PDU_CONNECT:
 		pr_debug("CONNECT\n");
 		nfc_llcp_recv_connect(local, skb);
@@ -1092,6 +1277,11 @@ static void nfc_llcp_rx_work(struct work_struct *work)
 		nfc_llcp_recv_dm(local, skb);
 		break;
 
+	case LLCP_PDU_SNL:
+		pr_debug("SNL\n");
+		nfc_llcp_recv_snl(local, skb);
+		break;
+
 	case LLCP_PDU_I:
 	case LLCP_PDU_RR:
 	case LLCP_PDU_RNR:
@@ -1104,8 +1294,6 @@ static void nfc_llcp_rx_work(struct work_struct *work)
 	schedule_work(&local->tx_work);
 	kfree_skb(local->rx_pending);
 	local->rx_pending = NULL;
-
-	return;
 }
 
 void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
@@ -1121,8 +1309,6 @@ void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
 	local->rx_pending = skb_get(skb);
 	del_timer(&local->link_timer);
 	schedule_work(&local->rx_work);
-
-	return;
 }
 
 int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb)
@@ -1205,6 +1391,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 fdb2d24e60bd..0d62366f8cc3 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 */
@@ -124,6 +127,13 @@ struct nfc_llcp_sock {
 	struct sock *parent;
 };
 
+struct nfc_llcp_ui_cb {
+	__u8 dsap;
+	__u8 ssap;
+};
+
+#define nfc_llcp_ui_skb_cb(__skb) ((struct nfc_llcp_ui_cb *)&((__skb)->cb[0]))
+
 #define nfc_llcp_sock(sk) ((struct nfc_llcp_sock *) (sk))
 #define nfc_llcp_dev(sk)  (nfc_llcp_sock((sk))->dev)
 
@@ -209,10 +219,13 @@ int nfc_llcp_disconnect(struct nfc_llcp_sock *sock);
 int nfc_llcp_send_symm(struct nfc_dev *dev);
 int nfc_llcp_send_connect(struct nfc_llcp_sock *sock);
 int nfc_llcp_send_cc(struct nfc_llcp_sock *sock);
+int nfc_llcp_send_snl(struct nfc_llcp_local *local, u8 tid, u8 sap);
 int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason);
 int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock);
 int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
 			  struct msghdr *msg, size_t len);
+int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap,
+			   struct msghdr *msg, size_t len);
 int nfc_llcp_send_rr(struct nfc_llcp_sock *sock);
 
 /* Socket API */
diff --git a/net/nfc/llcp/sock.c b/net/nfc/llcp/sock.c
index 63e4cdc92376..0fa1e92ceac8 100644
--- a/net/nfc/llcp/sock.c
+++ b/net/nfc/llcp/sock.c
@@ -205,8 +205,8 @@ static int llcp_sock_listen(struct socket *sock, int backlog)
 
 	lock_sock(sk);
 
-	if ((sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM)
-	    || sk->sk_state != LLCP_BOUND) {
+	if ((sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM) ||
+	    sk->sk_state != LLCP_BOUND) {
 		ret = -EBADFD;
 		goto error;
 	}
@@ -608,6 +608,25 @@ static int llcp_sock_sendmsg(struct kiocb *iocb, struct socket *sock,
 
 	lock_sock(sk);
 
+	if (sk->sk_type == SOCK_DGRAM) {
+		struct sockaddr_nfc_llcp *addr =
+			(struct sockaddr_nfc_llcp *)msg->msg_name;
+
+		if (msg->msg_namelen < sizeof(*addr)) {
+			release_sock(sk);
+
+			pr_err("Invalid socket address length %d\n",
+			       msg->msg_namelen);
+
+			return -EINVAL;
+		}
+
+		release_sock(sk);
+
+		return nfc_llcp_send_ui_frame(llcp_sock, addr->dsap, addr->ssap,
+					      msg, len);
+	}
+
 	if (sk->sk_state != LLCP_CONNECTED) {
 		release_sock(sk);
 		return -ENOTCONN;
@@ -663,11 +682,28 @@ static int llcp_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
 		return -EFAULT;
 	}
 
+	if (sk->sk_type == SOCK_DGRAM && msg->msg_name) {
+		struct nfc_llcp_ui_cb *ui_cb = nfc_llcp_ui_skb_cb(skb);
+		struct sockaddr_nfc_llcp sockaddr;
+
+		pr_debug("Datagram socket %d %d\n", ui_cb->dsap, ui_cb->ssap);
+
+		sockaddr.sa_family = AF_NFC;
+		sockaddr.nfc_protocol = NFC_PROTO_NFC_DEP;
+		sockaddr.dsap = ui_cb->dsap;
+		sockaddr.ssap = ui_cb->ssap;
+
+		memcpy(msg->msg_name, &sockaddr, sizeof(sockaddr));
+		msg->msg_namelen = sizeof(sockaddr);
+	}
+
 	/* Mark read part of skb as used */
 	if (!(flags & MSG_PEEK)) {
 
 		/* SOCK_STREAM: re-queue skb if it contains unreceived data */
-		if (sk->sk_type == SOCK_STREAM || sk->sk_type == SOCK_RAW) {
+		if (sk->sk_type == SOCK_STREAM ||
+		    sk->sk_type == SOCK_DGRAM ||
+		    sk->sk_type == SOCK_RAW) {
 			skb_pull(skb, copied);
 			if (skb->len) {
 				skb_queue_head(&sk->sk_receive_queue, skb);
diff --git a/net/nfc/nci/Kconfig b/net/nfc/nci/Kconfig
index decdc49b26d8..6d69b5f0f19b 100644
--- a/net/nfc/nci/Kconfig
+++ b/net/nfc/nci/Kconfig
@@ -1,6 +1,6 @@
 config NFC_NCI
-	depends on NFC && EXPERIMENTAL
-	tristate "NCI protocol support (EXPERIMENTAL)"
+	depends on NFC
+	tristate "NCI protocol support"
 	default n
 	help
 	  NCI (NFC Controller Interface) is a communication protocol between
diff --git a/net/nfc/nci/core.c b/net/nfc/nci/core.c
index acf9abb7d99b..5f98dc1bf039 100644
--- a/net/nfc/nci/core.c
+++ b/net/nfc/nci/core.c
@@ -205,10 +205,10 @@ static void nci_rf_discover_req(struct nci_dev *ndev, unsigned long opt)
 	cmd.num_disc_configs = 0;
 
 	if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS) &&
-	    (protocols & NFC_PROTO_JEWEL_MASK
-	     || protocols & NFC_PROTO_MIFARE_MASK
-	     || protocols & NFC_PROTO_ISO14443_MASK
-	     || protocols & NFC_PROTO_NFC_DEP_MASK)) {
+	    (protocols & NFC_PROTO_JEWEL_MASK ||
+	     protocols & NFC_PROTO_MIFARE_MASK ||
+	     protocols & NFC_PROTO_ISO14443_MASK ||
+	     protocols & NFC_PROTO_NFC_DEP_MASK)) {
 		cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode =
 			NCI_NFC_A_PASSIVE_POLL_MODE;
 		cmd.disc_configs[cmd.num_disc_configs].frequency = 1;
@@ -224,8 +224,8 @@ static void nci_rf_discover_req(struct nci_dev *ndev, unsigned long opt)
 	}
 
 	if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS) &&
-	    (protocols & NFC_PROTO_FELICA_MASK
-	     || protocols & NFC_PROTO_NFC_DEP_MASK)) {
+	    (protocols & NFC_PROTO_FELICA_MASK ||
+	     protocols & NFC_PROTO_NFC_DEP_MASK)) {
 		cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode =
 			NCI_NFC_F_PASSIVE_POLL_MODE;
 		cmd.disc_configs[cmd.num_disc_configs].frequency = 1;
@@ -414,13 +414,13 @@ static int nci_set_local_general_bytes(struct nfc_dev *nfc_dev)
 	struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
 	struct nci_set_config_param param;
 	__u8 local_gb[NFC_MAX_GT_LEN];
-	int i, rc = 0;
+	int i;
 
 	param.val = nfc_get_local_general_bytes(nfc_dev, &param.len);
 	if ((param.val == NULL) || (param.len == 0))
-		return rc;
+		return 0;
 
-	if (param.len > NCI_MAX_PARAM_LEN)
+	if (param.len > NFC_MAX_GT_LEN)
 		return -EINVAL;
 
 	for (i = 0; i < param.len; i++)
@@ -429,10 +429,8 @@ static int nci_set_local_general_bytes(struct nfc_dev *nfc_dev)
 	param.id = NCI_PN_ATR_REQ_GEN_BYTES;
 	param.val = local_gb;
 
-	rc = nci_request(ndev, nci_set_config_req, (unsigned long)&param,
-			 msecs_to_jiffies(NCI_SET_CONFIG_TIMEOUT));
-
-	return rc;
+	return nci_request(ndev, nci_set_config_req, (unsigned long)&param,
+			   msecs_to_jiffies(NCI_SET_CONFIG_TIMEOUT));
 }
 
 static int nci_start_poll(struct nfc_dev *nfc_dev,
@@ -579,7 +577,6 @@ static void nci_deactivate_target(struct nfc_dev *nfc_dev,
 	}
 }
 
-
 static int nci_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
 			   __u8 comm_mode, __u8 *gb, size_t gb_len)
 {
@@ -806,8 +803,8 @@ int nci_recv_frame(struct sk_buff *skb)
 
 	pr_debug("len %d\n", skb->len);
 
-	if (!ndev || (!test_bit(NCI_UP, &ndev->flags)
-		      && !test_bit(NCI_INIT, &ndev->flags))) {
+	if (!ndev || (!test_bit(NCI_UP, &ndev->flags) &&
+	    !test_bit(NCI_INIT, &ndev->flags))) {
 		kfree_skb(skb);
 		return -ENXIO;
 	}
diff --git a/net/nfc/netlink.c b/net/nfc/netlink.c
index c1b5285cbde7..3568ae16786d 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,
 };
@@ -364,7 +366,8 @@ static int nfc_genl_send_device(struct sk_buff *msg, struct nfc_dev *dev,
 	if (nla_put_string(msg, NFC_ATTR_DEVICE_NAME, nfc_device_name(dev)) ||
 	    nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx) ||
 	    nla_put_u32(msg, NFC_ATTR_PROTOCOLS, dev->supported_protocols) ||
-	    nla_put_u8(msg, NFC_ATTR_DEVICE_POWERED, dev->dev_up))
+	    nla_put_u8(msg, NFC_ATTR_DEVICE_POWERED, dev->dev_up) ||
+	    nla_put_u8(msg, NFC_ATTR_RF_MODE, dev->rf_mode))
 		goto nla_put_failure;
 
 	return genlmsg_end(msg, hdr);
@@ -590,7 +593,7 @@ static int nfc_genl_start_poll(struct sk_buff *skb, struct genl_info *info)
 	if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
 	    ((!info->attrs[NFC_ATTR_IM_PROTOCOLS] &&
 	      !info->attrs[NFC_ATTR_PROTOCOLS]) &&
-	     !info->attrs[NFC_ATTR_TM_PROTOCOLS]))
+	      !info->attrs[NFC_ATTR_TM_PROTOCOLS]))
 		return -EINVAL;
 
 	idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
@@ -715,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,
@@ -759,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 c5e42b79a418..87d914d2876a 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;
diff --git a/net/nfc/rawsock.c b/net/nfc/rawsock.c
index 8b8a6a2b2bad..313bf1bc848a 100644
--- a/net/nfc/rawsock.c
+++ b/net/nfc/rawsock.c
@@ -256,7 +256,6 @@ static int rawsock_recvmsg(struct kiocb *iocb, struct socket *sock,
 	return rc ? : copied;
 }
 
-
 static const struct proto_ops rawsock_ops = {
 	.family         = PF_NFC,
 	.owner          = THIS_MODULE,
generated by cgit-pink 1.4.1 (git 2.36.1) at 2024-10-11 14:26:38 +0000