nfc: nci: Add an additional parameter to identify a connection id

According to NCI specification, destination type and destination
specific parameters shall uniquely identify a single destination
for the Logical Connection.

Signed-off-by: Christophe Ricard <christophe-h.ricard@st.com>
Signed-off-by: Samuel Ortiz <sameo@linux.intel.com>

3 files changed, 36 insertions, 11 deletions

diff --git a/net/nfc/nci/core.c b/net/nfc/nci/core.c
index 0884f1444817..74f2d54df4fc 100644
--- a/net/nfc/nci/core.c
+++ b/net/nfc/nci/core.c
@@ -64,18 +64,26 @@ struct nci_conn_info *nci_get_conn_info_by_conn_id(struct nci_dev *ndev,
         return NULL;
 }
 
-int nci_get_conn_info_by_id(struct nci_dev *ndev, u8 id)
+int nci_get_conn_info_by_dest_type_params(struct nci_dev *ndev, u8 dest_type,
+                                          struct dest_spec_params *params)
 {
         struct nci_conn_info *conn_info;
 
         list_for_each_entry(conn_info, &ndev->conn_info_list, list) {
-                if (conn_info->id == id)
-                        return conn_info->conn_id;
+                if (conn_info->dest_type == dest_type) {
+                        if (!params)
+                                return conn_info->conn_id;
+                        if (conn_info) {
+                                if (params->id == conn_info->dest_params->id &&
+                                    params->protocol == conn_info->dest_params->protocol)
+                                        return conn_info->conn_id;
+                        }
+                }
         }
 
         return -EINVAL;
 }
-EXPORT_SYMBOL(nci_get_conn_info_by_id);
+EXPORT_SYMBOL(nci_get_conn_info_by_dest_type_params);
 
 /* ---- NCI requests ---- */
 
@@ -623,12 +631,15 @@ int nci_core_conn_create(struct nci_dev *ndev, u8 destination_type,
         if (params) {
                 memcpy(cmd->params, params, params_len);
                 if (params->length > 0)
-                        ndev->cur_id = params->value[DEST_SPEC_PARAMS_ID_INDEX];
+                        memcpy(&ndev->cur_params,
+                               &params->value[DEST_SPEC_PARAMS_ID_INDEX],
+                               sizeof(struct dest_spec_params));
                 else
-                        ndev->cur_id = 0;
+                        ndev->cur_params.id = 0;
         } else {
-                ndev->cur_id = 0;
+                ndev->cur_params.id = 0;
         }
+        ndev->cur_dest_type = destination_type;
 
         r = __nci_request(ndev, nci_core_conn_create_req, (unsigned long)&data,
                           msecs_to_jiffies(NCI_CMD_TIMEOUT));
diff --git a/net/nfc/nci/ntf.c b/net/nfc/nci/ntf.c
index 2ada2b39e355..1e8c1a12aaec 100644
--- a/net/nfc/nci/ntf.c
+++ b/net/nfc/nci/ntf.c
@@ -734,7 +734,7 @@ static void nci_nfcee_discover_ntf_packet(struct nci_dev *ndev,
          * “HCI Access”, even if the HCI Network contains multiple NFCEEs.
          */
         ndev->hci_dev->nfcee_id = nfcee_ntf->nfcee_id;
-        ndev->cur_id = nfcee_ntf->nfcee_id;
+        ndev->cur_params.id = nfcee_ntf->nfcee_id;
 
         nci_req_complete(ndev, status);
 }
diff --git a/net/nfc/nci/rsp.c b/net/nfc/nci/rsp.c
index 69fe163b7350..e3bbf1937d0e 100644
--- a/net/nfc/nci/rsp.c
+++ b/net/nfc/nci/rsp.c
@@ -226,7 +226,7 @@ static void nci_core_conn_create_rsp_packet(struct nci_dev *ndev,
                                             struct sk_buff *skb)
 {
         __u8 status = skb->data[0];
-        struct nci_conn_info *conn_info;
+        struct nci_conn_info *conn_info = NULL;
         struct nci_core_conn_create_rsp *rsp;
 
         pr_debug("status 0x%x\n", status);
@@ -241,7 +241,17 @@ static void nci_core_conn_create_rsp_packet(struct nci_dev *ndev,
                         goto exit;
                 }
 
-                conn_info->id = ndev->cur_id;
+                conn_info->dest_params = devm_kzalloc(&ndev->nfc_dev->dev,
+                                                sizeof(struct dest_spec_params),
+                                                GFP_KERNEL);
+                if (!conn_info->dest_params) {
+                        status = NCI_STATUS_REJECTED;
+                        goto free_conn_info;
+                }
+
+                conn_info->dest_type = ndev->cur_dest_type;
+                conn_info->dest_params->id = ndev->cur_params.id;
+                conn_info->dest_params->protocol = ndev->cur_params.protocol;
                 conn_info->conn_id = rsp->conn_id;
 
                 /* Note: data_exchange_cb and data_exchange_cb_context need to
@@ -251,7 +261,7 @@ static void nci_core_conn_create_rsp_packet(struct nci_dev *ndev,
                 INIT_LIST_HEAD(&conn_info->list);
                 list_add(&conn_info->list, &ndev->conn_info_list);
 
-                if (ndev->cur_id == ndev->hci_dev->nfcee_id)
+                if (ndev->cur_params.id == ndev->hci_dev->nfcee_id)
                         ndev->hci_dev->conn_info = conn_info;
 
                 conn_info->conn_id = rsp->conn_id;
@@ -259,7 +269,11 @@ static void nci_core_conn_create_rsp_packet(struct nci_dev *ndev,
                 atomic_set(&conn_info->credits_cnt, rsp->credits_cnt);
         }
 
+free_conn_info:
+        if (status == NCI_STATUS_REJECTED)
+                devm_kfree(&ndev->nfc_dev->dev, conn_info);
 exit:
+
         nci_req_complete(ndev, status);
 }