1 files changed, 70 insertions, 61 deletions

diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
index 82f0f75..c12c5ef 100644
--- a/net/nfc/llcp/llcp.c
+++ b/net/nfc/llcp/llcp.c
@@ -56,7 +56,7 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen)
 	sk_for_each_safe(sk, node, tmp, &local->sockets.head) {
 		llcp_sock = nfc_llcp_sock(sk);
 
-		lock_sock(sk);
+		bh_lock_sock(sk);
 
 		if (sk->sk_state == LLCP_CONNECTED)
 			nfc_put_device(llcp_sock->dev);
@@ -68,26 +68,26 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen)
 			list_for_each_entry_safe(lsk, n, &llcp_sock->accept_queue,
 						 accept_queue) {
 				accept_sk = &lsk->sk;
-				lock_sock(accept_sk);
+				bh_lock_sock(accept_sk);
 
 				nfc_llcp_accept_unlink(accept_sk);
 
 				accept_sk->sk_state = LLCP_CLOSED;
 
-				release_sock(accept_sk);
+				bh_unlock_sock(accept_sk);
 
 				sock_orphan(accept_sk);
 			}
 
 			if (listen == true) {
-				release_sock(sk);
+				bh_unlock_sock(sk);
 				continue;
 			}
 		}
 
 		sk->sk_state = LLCP_CLOSED;
 
-		release_sock(sk);
+		bh_unlock_sock(sk);
 
 		sock_orphan(sk);
 
@@ -114,9 +114,9 @@ static void local_release(struct kref *ref)
 	nfc_llcp_socket_release(local, false);
 	del_timer_sync(&local->link_timer);
 	skb_queue_purge(&local->tx_queue);
-	destroy_workqueue(local->tx_wq);
-	destroy_workqueue(local->rx_wq);
-	destroy_workqueue(local->timeout_wq);
+	cancel_work_sync(&local->tx_work);
+	cancel_work_sync(&local->rx_work);
+	cancel_work_sync(&local->timeout_work);
 	kfree_skb(local->rx_pending);
 	kfree(local);
 }
@@ -181,7 +181,7 @@ static void nfc_llcp_symm_timer(unsigned long data)
 
 	pr_err("SYMM timeout\n");
 
-	queue_work(local->timeout_wq, &local->timeout_work);
+	queue_work(system_nrt_wq, &local->timeout_work);
 }
 
 struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev)
@@ -426,6 +426,7 @@ static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
 	u8 *miux_tlv, miux_length;
 	__be16 miux;
 	u8 gb_len = 0;
+	int ret = 0;
 
 	version = LLCP_VERSION_11;
 	version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version,
@@ -450,8 +451,8 @@ static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
 	gb_len += ARRAY_SIZE(llcp_magic);
 
 	if (gb_len > NFC_MAX_GT_LEN) {
-		kfree(version_tlv);
-		return -EINVAL;
+		ret = -EINVAL;
+		goto out;
 	}
 
 	gb_cur = local->gb;
@@ -471,12 +472,15 @@ static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
 	memcpy(gb_cur, miux_tlv, miux_length);
 	gb_cur += miux_length;
 
+	local->gb_len = gb_len;
+
+out:
 	kfree(version_tlv);
 	kfree(lto_tlv);
+	kfree(wks_tlv);
+	kfree(miux_tlv);
 
-	local->gb_len = gb_len;
-
-	return 0;
+	return ret;
 }
 
 u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len)
@@ -554,6 +558,46 @@ static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu)
 	sock->recv_ack_n = (sock->recv_n - 1) % 16;
 }
 
+void nfc_llcp_send_to_raw_sock(struct nfc_llcp_local *local,
+			       struct sk_buff *skb, u8 direction)
+{
+	struct hlist_node *node;
+	struct sk_buff *skb_copy = NULL, *nskb;
+	struct sock *sk;
+	u8 *data;
+
+	read_lock(&local->raw_sockets.lock);
+
+	sk_for_each(sk, node, &local->raw_sockets.head) {
+		if (sk->sk_state != LLCP_BOUND)
+			continue;
+
+		if (skb_copy == NULL) {
+			skb_copy = __pskb_copy(skb, NFC_LLCP_RAW_HEADER_SIZE,
+					       GFP_ATOMIC);
+
+			if (skb_copy == NULL)
+				continue;
+
+			data = skb_push(skb_copy, NFC_LLCP_RAW_HEADER_SIZE);
+
+			data[0] = local->dev ? local->dev->idx : 0xFF;
+			data[1] = direction;
+		}
+
+		nskb = skb_clone(skb_copy, GFP_ATOMIC);
+		if (!nskb)
+			continue;
+
+		if (sock_queue_rcv_skb(sk, nskb))
+			kfree_skb(nskb);
+	}
+
+	read_unlock(&local->raw_sockets.lock);
+
+	kfree_skb(skb_copy);
+}
+
 static void nfc_llcp_tx_work(struct work_struct *work)
 {
 	struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
@@ -574,6 +618,9 @@ static void nfc_llcp_tx_work(struct work_struct *work)
 				       DUMP_PREFIX_OFFSET, 16, 1,
 				       skb->data, skb->len, true);
 
+			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);
 
@@ -1018,6 +1065,8 @@ static void nfc_llcp_rx_work(struct work_struct *work)
 		print_hex_dump(KERN_DEBUG, "LLCP Rx: ", DUMP_PREFIX_OFFSET,
 			       16, 1, skb->data, skb->len, true);
 
+	nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_RX);
+
 	switch (ptype) {
 	case LLCP_PDU_SYMM:
 		pr_debug("SYMM\n");
@@ -1052,7 +1101,7 @@ static void nfc_llcp_rx_work(struct work_struct *work)
 
 	}
 
-	queue_work(local->tx_wq, &local->tx_work);
+	queue_work(system_nrt_wq, &local->tx_work);
 	kfree_skb(local->rx_pending);
 	local->rx_pending = NULL;
 
@@ -1071,7 +1120,7 @@ void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
 
 	local->rx_pending = skb_get(skb);
 	del_timer(&local->link_timer);
-	queue_work(local->rx_wq, &local->rx_work);
+	queue_work(system_nrt_wq, &local->rx_work);
 
 	return;
 }
@@ -1086,7 +1135,7 @@ int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb)
 
 	local->rx_pending = skb_get(skb);
 	del_timer(&local->link_timer);
-	queue_work(local->rx_wq, &local->rx_work);
+	queue_work(system_nrt_wq, &local->rx_work);
 
 	return 0;
 }
@@ -1121,7 +1170,7 @@ void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
 	if (rf_mode == NFC_RF_INITIATOR) {
 		pr_debug("Queueing Tx work\n");
 
-		queue_work(local->tx_wq, &local->tx_work);
+		queue_work(system_nrt_wq, &local->tx_work);
 	} else {
 		mod_timer(&local->link_timer,
 			  jiffies + msecs_to_jiffies(local->remote_lto));
@@ -1130,10 +1179,7 @@ void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
 
 int nfc_llcp_register_device(struct nfc_dev *ndev)
 {
-	struct device *dev = &ndev->dev;
 	struct nfc_llcp_local *local;
-	char name[32];
-	int err;
 
 	local = kzalloc(sizeof(struct nfc_llcp_local), GFP_KERNEL);
 	if (local == NULL)
@@ -1149,41 +1195,15 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
 
 	skb_queue_head_init(&local->tx_queue);
 	INIT_WORK(&local->tx_work, nfc_llcp_tx_work);
-	snprintf(name, sizeof(name), "%s_llcp_tx_wq", dev_name(dev));
-	local->tx_wq =
-		alloc_workqueue(name,
-				WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
-				1);
-	if (local->tx_wq == NULL) {
-		err = -ENOMEM;
-		goto err_local;
-	}
 
 	local->rx_pending = NULL;
 	INIT_WORK(&local->rx_work, nfc_llcp_rx_work);
-	snprintf(name, sizeof(name), "%s_llcp_rx_wq", dev_name(dev));
-	local->rx_wq =
-		alloc_workqueue(name,
-				WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
-				1);
-	if (local->rx_wq == NULL) {
-		err = -ENOMEM;
-		goto err_tx_wq;
-	}
 
 	INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work);
-	snprintf(name, sizeof(name), "%s_llcp_timeout_wq", dev_name(dev));
-	local->timeout_wq =
-		alloc_workqueue(name,
-				WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
-				1);
-	if (local->timeout_wq == NULL) {
-		err = -ENOMEM;
-		goto err_rx_wq;
-	}
 
-	local->sockets.lock = __RW_LOCK_UNLOCKED(local->sockets.lock);
-	local->connecting_sockets.lock = __RW_LOCK_UNLOCKED(local->connecting_sockets.lock);
+	rwlock_init(&local->sockets.lock);
+	rwlock_init(&local->connecting_sockets.lock);
+	rwlock_init(&local->raw_sockets.lock);
 
 	nfc_llcp_build_gb(local);
 
@@ -1193,17 +1213,6 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
 	list_add(&llcp_devices, &local->list);
 
 	return 0;
-
-err_rx_wq:
-	destroy_workqueue(local->rx_wq);
-
-err_tx_wq:
-	destroy_workqueue(local->tx_wq);
-
-err_local:
-	kfree(local);
-
-	return 0;
 }
 
 void nfc_llcp_unregister_device(struct nfc_dev *dev)