3 files changed, 34 insertions, 26 deletions

diff --git a/net/nfc/nci/hci.c b/net/nfc/nci/hci.c
index ecf253942606..ed54ec533836 100644
--- a/net/nfc/nci/hci.c
+++ b/net/nfc/nci/hci.c
@@ -615,12 +615,20 @@ static int nci_hci_dev_connect_gates(struct nci_dev *ndev,
 
 int nci_hci_dev_session_init(struct nci_dev *ndev)
 {
+	struct nci_conn_info    *conn_info;
 	struct sk_buff *skb;
 	int r;
 
 	ndev->hci_dev->count_pipes = 0;
 	ndev->hci_dev->expected_pipes = 0;
 
+	conn_info = ndev->hci_dev->conn_info;
+	if (!conn_info)
+		return -EPROTO;
+
+	conn_info->data_exchange_cb = nci_hci_data_received_cb;
+	conn_info->data_exchange_cb_context = ndev;
+
 	nci_hci_reset_pipes(ndev->hci_dev);
 
 	if (ndev->hci_dev->init_data.gates[0].gate != NCI_HCI_ADMIN_GATE)
diff --git a/net/nfc/nci/ntf.c b/net/nfc/nci/ntf.c
index 6bbbf6fdacc0..3218071072ac 100644
--- a/net/nfc/nci/ntf.c
+++ b/net/nfc/nci/ntf.c
@@ -723,7 +723,6 @@ static void nci_nfcee_discover_ntf_packet(struct nci_dev *ndev,
 					  struct sk_buff *skb)
 {
 	u8 status = NCI_STATUS_OK;
-	struct nci_conn_info    *conn_info;
 	struct nci_nfcee_discover_ntf   *nfcee_ntf =
 				(struct nci_nfcee_discover_ntf *)skb->data;
 
@@ -734,27 +733,9 @@ static void nci_nfcee_discover_ntf_packet(struct nci_dev *ndev,
 	 * and only one, NFCEE_DISCOVER_NTF with a Protocol type of
 	 * “HCI Access”, even if the HCI Network contains multiple NFCEEs.
 	 */
-	if (!ndev->hci_dev->conn_info) {
-		conn_info = devm_kzalloc(&ndev->nfc_dev->dev,
-					 sizeof(*conn_info), GFP_KERNEL);
-		if (!conn_info) {
-			status = NCI_STATUS_REJECTED;
-			goto exit;
-		}
-
-		conn_info->id = nfcee_ntf->nfcee_id;
-		conn_info->conn_id = NCI_INVALID_CONN_ID;
-
-		conn_info->data_exchange_cb = nci_hci_data_received_cb;
-		conn_info->data_exchange_cb_context = ndev;
+	ndev->hci_dev->nfcee_id = nfcee_ntf->nfcee_id;
+	ndev->cur_id = nfcee_ntf->nfcee_id;
 
-		INIT_LIST_HEAD(&conn_info->list);
-		list_add(&conn_info->list, &ndev->conn_info_list);
-
-		ndev->hci_dev->conn_info = conn_info;
-	}
-
-exit:
 	nci_req_complete(ndev, status);
 }
 
diff --git a/net/nfc/nci/rsp.c b/net/nfc/nci/rsp.c
index b419fed77ea3..02486bc2ceea 100644
--- a/net/nfc/nci/rsp.c
+++ b/net/nfc/nci/rsp.c
@@ -233,16 +233,27 @@ static void nci_core_conn_create_rsp_packet(struct nci_dev *ndev,
 
 	if (status == NCI_STATUS_OK) {
 		rsp = (struct nci_core_conn_create_rsp *)skb->data;
-		list_for_each_entry(conn_info, &ndev->conn_info_list, list) {
-			if (conn_info->id == ndev->cur_id)
-				break;
-		}
 
-		if (!conn_info || conn_info->id != ndev->cur_id) {
+		conn_info = devm_kzalloc(&ndev->nfc_dev->dev,
+					 sizeof(*conn_info), GFP_KERNEL);
+		if (!conn_info) {
 			status = NCI_STATUS_REJECTED;
 			goto exit;
 		}
 
+		conn_info->id = ndev->cur_id;
+		conn_info->conn_id = rsp->conn_id;
+
+		/* Note: data_exchange_cb and data_exchange_cb_context need to
+		 * be specify out of nci_core_conn_create_rsp_packet
+		 */
+
+		INIT_LIST_HEAD(&conn_info->list);
+		list_add(&conn_info->list, &ndev->conn_info_list);
+
+		if (ndev->cur_id == ndev->hci_dev->nfcee_id)
+			ndev->hci_dev->conn_info = conn_info;
+
 		conn_info->conn_id = rsp->conn_id;
 		conn_info->max_pkt_payload_len = rsp->max_ctrl_pkt_payload_len;
 		atomic_set(&conn_info->credits_cnt, rsp->credits_cnt);
@@ -255,9 +266,17 @@ exit:
 static void nci_core_conn_close_rsp_packet(struct nci_dev *ndev,
 					   struct sk_buff *skb)
 {
+	struct nci_conn_info *conn_info;
 	__u8 status = skb->data[0];
 
 	pr_debug("status 0x%x\n", status);
+	if (status == NCI_STATUS_OK) {
+		conn_info = nci_get_conn_info_by_conn_id(ndev, ndev->cur_id);
+		if (conn_info) {
+			list_del(&conn_info->list);
+			devm_kfree(&ndev->nfc_dev->dev, conn_info);
+		}
+	}
 	nci_req_complete(ndev, status);
 }