NFC: nci: Add dynamic logical connections support

The current NCI core only support the RF static connection.
For other NFC features such as Secure Element communication, we
may need to create logical connections to the NFCEE (Execution
Environment.

In order to track each logical connection ID dynamically, we add a
linked list of connection info pointers to the nci_dev structure.

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

4 files changed, 127 insertions, 33 deletions

diff --git a/net/nfc/nci/core.c b/net/nfc/nci/core.c
index 51feb5e63008..eb607970bd56 100644
--- a/net/nfc/nci/core.c
+++ b/net/nfc/nci/core.c
@@ -45,6 +45,19 @@ static void nci_cmd_work(struct work_struct *work);
 static void nci_rx_work(struct work_struct *work);
 static void nci_tx_work(struct work_struct *work);
 
+struct nci_conn_info *nci_get_conn_info_by_conn_id(struct nci_dev *ndev,
+                                                   int conn_id)
+{
+        struct nci_conn_info *conn_info;
+
+        list_for_each_entry(conn_info, &ndev->conn_info_list, list) {
+                if (conn_info->conn_id == conn_id)
+                        return conn_info;
+        }
+
+        return NULL;
+}
+
 /* ---- NCI requests ---- */
 
 void nci_req_complete(struct nci_dev *ndev, int result)
@@ -712,6 +725,11 @@ static int nci_transceive(struct nfc_dev *nfc_dev, struct nfc_target *target,
 {
         struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
         int rc;
+        struct nci_conn_info    *conn_info;
+
+        conn_info = nci_get_conn_info_by_conn_id(ndev, NCI_STATIC_RF_CONN_ID);
+        if (!conn_info)
+                return -EPROTO;
 
         pr_debug("target_idx %d, len %d\n", target->idx, skb->len);
 
@@ -724,8 +742,8 @@ static int nci_transceive(struct nfc_dev *nfc_dev, struct nfc_target *target,
                 return -EBUSY;
 
         /* store cb and context to be used on receiving data */
-        ndev->data_exchange_cb = cb;
-        ndev->data_exchange_cb_context = cb_context;
+        conn_info->data_exchange_cb = cb;
+        conn_info->data_exchange_cb_context = cb_context;
 
         rc = nci_send_data(ndev, NCI_STATIC_RF_CONN_ID, skb);
         if (rc)
@@ -913,6 +931,7 @@ int nci_register_device(struct nci_dev *ndev)
                     (unsigned long) ndev);
 
         mutex_init(&ndev->req_lock);
+        INIT_LIST_HEAD(&ndev->conn_info_list);
 
         rc = nfc_register_device(ndev->nfc_dev);
         if (rc)
@@ -938,12 +957,19 @@ EXPORT_SYMBOL(nci_register_device);
  */
 void nci_unregister_device(struct nci_dev *ndev)
 {
+        struct nci_conn_info    *conn_info, *n;
+
         nci_close_device(ndev);
 
         destroy_workqueue(ndev->cmd_wq);
         destroy_workqueue(ndev->rx_wq);
         destroy_workqueue(ndev->tx_wq);
 
+        list_for_each_entry_safe(conn_info, n, &ndev->conn_info_list, list) {
+                list_del(&conn_info->list);
+                /* conn_info is allocated with devm_kzalloc */
+        }
+
         nfc_unregister_device(ndev->nfc_dev);
 }
 EXPORT_SYMBOL(nci_unregister_device);
@@ -1027,20 +1053,25 @@ int nci_send_cmd(struct nci_dev *ndev, __u16 opcode, __u8 plen, void *payload)
 static void nci_tx_work(struct work_struct *work)
 {
         struct nci_dev *ndev = container_of(work, struct nci_dev, tx_work);
+        struct nci_conn_info    *conn_info;
         struct sk_buff *skb;
 
-        pr_debug("credits_cnt %d\n", atomic_read(&ndev->credits_cnt));
+        conn_info = nci_get_conn_info_by_conn_id(ndev, ndev->cur_conn_id);
+        if (!conn_info)
+                return;
+
+        pr_debug("credits_cnt %d\n", atomic_read(&conn_info->credits_cnt));
 
         /* Send queued tx data */
-        while (atomic_read(&ndev->credits_cnt)) {
+        while (atomic_read(&conn_info->credits_cnt)) {
                 skb = skb_dequeue(&ndev->tx_q);
                 if (!skb)
                         return;
 
                 /* Check if data flow control is used */
-                if (atomic_read(&ndev->credits_cnt) !=
+                if (atomic_read(&conn_info->credits_cnt) !=
                     NCI_DATA_FLOW_CONTROL_NOT_USED)
-                        atomic_dec(&ndev->credits_cnt);
+                        atomic_dec(&conn_info->credits_cnt);
 
                 pr_debug("NCI TX: MT=data, PBF=%d, conn_id=%d, plen=%d\n",
                          nci_pbf(skb->data),
@@ -1092,7 +1123,9 @@ static void nci_rx_work(struct work_struct *work)
         if (test_bit(NCI_DATA_EXCHANGE_TO, &ndev->flags)) {
                 /* complete the data exchange transaction, if exists */
                 if (test_bit(NCI_DATA_EXCHANGE, &ndev->flags))
-                        nci_data_exchange_complete(ndev, NULL, -ETIMEDOUT);
+                        nci_data_exchange_complete(ndev, NULL,
+                                                   ndev->cur_conn_id,
+                                                   -ETIMEDOUT);
 
                 clear_bit(NCI_DATA_EXCHANGE_TO, &ndev->flags);
         }
diff --git a/net/nfc/nci/data.c b/net/nfc/nci/data.c
index a2de2a8cb00e..566466d90048 100644
--- a/net/nfc/nci/data.c
+++ b/net/nfc/nci/data.c
@@ -36,10 +36,20 @@
 
 /* Complete data exchange transaction and forward skb to nfc core */
 void nci_data_exchange_complete(struct nci_dev *ndev, struct sk_buff *skb,
-                                int err)
+                                __u8 conn_id, int err)
 {
-        data_exchange_cb_t cb = ndev->data_exchange_cb;
-        void *cb_context = ndev->data_exchange_cb_context;
+        struct nci_conn_info    *conn_info;
+        data_exchange_cb_t cb;
+        void *cb_context;
+
+        conn_info = nci_get_conn_info_by_conn_id(ndev, conn_id);
+        if (!conn_info) {
+                kfree_skb(skb);
+                goto exit;
+        }
+
+        cb = conn_info->data_exchange_cb;
+        cb_context = conn_info->data_exchange_cb_context;
 
         pr_debug("len %d, err %d\n", skb ? skb->len : 0, err);
 
@@ -48,9 +58,6 @@ void nci_data_exchange_complete(struct nci_dev *ndev, struct sk_buff *skb,
         clear_bit(NCI_DATA_EXCHANGE_TO, &ndev->flags);
 
         if (cb) {
-                ndev->data_exchange_cb = NULL;
-                ndev->data_exchange_cb_context = NULL;
-
                 /* forward skb to nfc core */
                 cb(cb_context, skb, err);
         } else if (skb) {
@@ -60,6 +67,7 @@ void nci_data_exchange_complete(struct nci_dev *ndev, struct sk_buff *skb,
                 kfree_skb(skb);
         }
 
+exit:
         clear_bit(NCI_DATA_EXCHANGE, &ndev->flags);
 }
 
@@ -85,6 +93,7 @@ static inline void nci_push_data_hdr(struct nci_dev *ndev,
 static int nci_queue_tx_data_frags(struct nci_dev *ndev,
                                    __u8 conn_id,
                                    struct sk_buff *skb) {
+        struct nci_conn_info    *conn_info;
         int total_len = skb->len;
         unsigned char *data = skb->data;
         unsigned long flags;
@@ -95,11 +104,17 @@ static int nci_queue_tx_data_frags(struct nci_dev *ndev,
 
         pr_debug("conn_id 0x%x, total_len %d\n", conn_id, total_len);
 
+        conn_info = nci_get_conn_info_by_conn_id(ndev, conn_id);
+        if (!conn_info) {
+                rc = -EPROTO;
+                goto free_exit;
+        }
+
         __skb_queue_head_init(&frags_q);
 
         while (total_len) {
                 frag_len =
-                        min_t(int, total_len, ndev->max_data_pkt_payload_size);
+                        min_t(int, total_len, conn_info->max_pkt_payload_len);
 
                 skb_frag = nci_skb_alloc(ndev,
                                          (NCI_DATA_HDR_SIZE + frag_len),
@@ -151,12 +166,19 @@ exit:
 /* Send NCI data */
 int nci_send_data(struct nci_dev *ndev, __u8 conn_id, struct sk_buff *skb)
 {
+        struct nci_conn_info    *conn_info;
         int rc = 0;
 
         pr_debug("conn_id 0x%x, plen %d\n", conn_id, skb->len);
 
+        conn_info = nci_get_conn_info_by_conn_id(ndev, conn_id);
+        if (!conn_info) {
+                rc = -EPROTO;
+                goto free_exit;
+        }
+
         /* check if the packet need to be fragmented */
-        if (skb->len <= ndev->max_data_pkt_payload_size) {
+        if (skb->len <= conn_info->max_pkt_payload_len) {
                 /* no need to fragment packet */
                 nci_push_data_hdr(ndev, conn_id, skb, NCI_PBF_LAST);
 
@@ -170,6 +192,7 @@ int nci_send_data(struct nci_dev *ndev, __u8 conn_id, struct sk_buff *skb)
                 }
         }
 
+        ndev->cur_conn_id = conn_id;
         queue_work(ndev->tx_wq, &ndev->tx_work);
 
         goto exit;
@@ -185,7 +208,7 @@ exit:
 
 static void nci_add_rx_data_frag(struct nci_dev *ndev,
                                  struct sk_buff *skb,
-                                 __u8 pbf, __u8 status)
+                                 __u8 pbf, __u8 conn_id, __u8 status)
 {
         int reassembly_len;
         int err = 0;
@@ -229,16 +252,13 @@ static void nci_add_rx_data_frag(struct nci_dev *ndev,
         }
 
 exit:
-        if (ndev->nfc_dev->rf_mode == NFC_RF_INITIATOR) {
-                nci_data_exchange_complete(ndev, skb, err);
-        } else if (ndev->nfc_dev->rf_mode == NFC_RF_TARGET) {
+        if (ndev->nfc_dev->rf_mode == NFC_RF_TARGET) {
                 /* Data received in Target mode, forward to nfc core */
                 err = nfc_tm_data_received(ndev->nfc_dev, skb);
                 if (err)
                         pr_err("unable to handle received data\n");
         } else {
-                pr_err("rf mode unknown\n");
-                kfree_skb(skb);
+                nci_data_exchange_complete(ndev, skb, conn_id, err);
         }
 }
 
@@ -247,6 +267,8 @@ void nci_rx_data_packet(struct nci_dev *ndev, struct sk_buff *skb)
 {
         __u8 pbf = nci_pbf(skb->data);
         __u8 status = 0;
+        __u8 conn_id = nci_conn_id(skb->data);
+        struct nci_conn_info    *conn_info;
 
         pr_debug("len %d\n", skb->len);
 
@@ -255,6 +277,10 @@ void nci_rx_data_packet(struct nci_dev *ndev, struct sk_buff *skb)
                  nci_conn_id(skb->data),
                  nci_plen(skb->data));
 
+        conn_info = nci_get_conn_info_by_conn_id(ndev, nci_conn_id(skb->data));
+        if (!conn_info)
+                return;
+
         /* strip the nci data header */
         skb_pull(skb, NCI_DATA_HDR_SIZE);
 
@@ -268,5 +294,5 @@ void nci_rx_data_packet(struct nci_dev *ndev, struct sk_buff *skb)
                 skb_trim(skb, (skb->len - 1));
         }
 
-        nci_add_rx_data_frag(ndev, skb, pbf, nci_to_errno(status));
+        nci_add_rx_data_frag(ndev, skb, pbf, conn_id, nci_to_errno(status));
 }
diff --git a/net/nfc/nci/ntf.c b/net/nfc/nci/ntf.c
index 22e453cb787d..28fdbe234bd4 100644
--- a/net/nfc/nci/ntf.c
+++ b/net/nfc/nci/ntf.c
@@ -43,6 +43,7 @@ static void nci_core_conn_credits_ntf_packet(struct nci_dev *ndev,
                                              struct sk_buff *skb)
 {
         struct nci_core_conn_credit_ntf *ntf = (void *) skb->data;
+        struct nci_conn_info    *conn_info;
         int i;
 
         pr_debug("num_entries %d\n", ntf->num_entries);
@@ -59,11 +60,13 @@ static void nci_core_conn_credits_ntf_packet(struct nci_dev *ndev,
                          i, ntf->conn_entries[i].conn_id,
                          ntf->conn_entries[i].credits);
 
-                if (ntf->conn_entries[i].conn_id == NCI_STATIC_RF_CONN_ID) {
-                        /* found static rf connection */
-                        atomic_add(ntf->conn_entries[i].credits,
-                                   &ndev->credits_cnt);
-                }
+                conn_info = nci_get_conn_info_by_conn_id(ndev,
+                                                         ntf->conn_entries[i].conn_id);
+                if (!conn_info)
+                        return;
+
+                atomic_add(ntf->conn_entries[i].credits,
+                           &conn_info->credits_cnt);
         }
 
         /* trigger the next tx */
@@ -96,7 +99,7 @@ static void nci_core_conn_intf_error_ntf_packet(struct nci_dev *ndev,
 
         /* complete the data exchange transaction, if exists */
         if (test_bit(NCI_DATA_EXCHANGE, &ndev->flags))
-                nci_data_exchange_complete(ndev, NULL, -EIO);
+                nci_data_exchange_complete(ndev, NULL, ntf->conn_id, -EIO);
 }
 
 static __u8 *nci_extract_rf_params_nfca_passive_poll(struct nci_dev *ndev,
@@ -513,6 +516,7 @@ static int nci_store_general_bytes_nfc_dep(struct nci_dev *ndev,
 static void nci_rf_intf_activated_ntf_packet(struct nci_dev *ndev,
                                              struct sk_buff *skb)
 {
+        struct nci_conn_info    *conn_info;
         struct nci_rf_intf_activated_ntf ntf;
         __u8 *data = skb->data;
         int err = NCI_STATUS_OK;
@@ -614,11 +618,17 @@ static void nci_rf_intf_activated_ntf_packet(struct nci_dev *ndev,
 
 exit:
         if (err == NCI_STATUS_OK) {
-                ndev->max_data_pkt_payload_size = ntf.max_data_pkt_payload_size;
-                ndev->initial_num_credits = ntf.initial_num_credits;
+                conn_info = nci_get_conn_info_by_conn_id(ndev,
+                                                         NCI_STATIC_RF_CONN_ID);
+                if (!conn_info)
+                        return;
+
+                conn_info->max_pkt_payload_len = ntf.max_data_pkt_payload_size;
+                conn_info->initial_num_credits = ntf.initial_num_credits;
 
                 /* set the available credits to initial value */
-                atomic_set(&ndev->credits_cnt, ndev->initial_num_credits);
+                atomic_set(&conn_info->credits_cnt,
+                           conn_info->initial_num_credits);
 
                 /* store general bytes to be reported later in dep_link_up */
                 if (ntf.rf_interface == NCI_RF_INTERFACE_NFC_DEP) {
@@ -661,10 +671,16 @@ exit:
 static void nci_rf_deactivate_ntf_packet(struct nci_dev *ndev,
                                          struct sk_buff *skb)
 {
+        struct nci_conn_info    *conn_info;
         struct nci_rf_deactivate_ntf *ntf = (void *) skb->data;
 
         pr_debug("entry, type 0x%x, reason 0x%x\n", ntf->type, ntf->reason);
 
+        conn_info =
+                nci_get_conn_info_by_conn_id(ndev, NCI_STATIC_RF_CONN_ID);
+        if (!conn_info)
+                return;
+
         /* drop tx data queue */
         skb_queue_purge(&ndev->tx_q);
 
@@ -676,7 +692,8 @@ static void nci_rf_deactivate_ntf_packet(struct nci_dev *ndev,
 
         /* complete the data exchange transaction, if exists */
         if (test_bit(NCI_DATA_EXCHANGE, &ndev->flags))
-                nci_data_exchange_complete(ndev, NULL, -EIO);
+                nci_data_exchange_complete(ndev, NULL, NCI_STATIC_RF_CONN_ID,
+                                           -EIO);
 
         switch (ntf->type) {
         case NCI_DEACTIVATE_TYPE_IDLE_MODE:
diff --git a/net/nfc/nci/rsp.c b/net/nfc/nci/rsp.c
index 041de51ccdbe..93b914937263 100644
--- a/net/nfc/nci/rsp.c
+++ b/net/nfc/nci/rsp.c
@@ -140,13 +140,31 @@ static void nci_rf_disc_map_rsp_packet(struct nci_dev *ndev,
 
 static void nci_rf_disc_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)
+        if (status == NCI_STATUS_OK) {
                 atomic_set(&ndev->state, NCI_DISCOVERY);
 
+                conn_info = nci_get_conn_info_by_conn_id(ndev,
+                                                         NCI_STATIC_RF_CONN_ID);
+                if (!conn_info) {
+                        conn_info = devm_kzalloc(&ndev->nfc_dev->dev,
+                                                 sizeof(struct nci_conn_info),
+                                                 GFP_KERNEL);
+                        if (!conn_info) {
+                                status = NCI_STATUS_REJECTED;
+                                goto exit;
+                        }
+                        conn_info->conn_id = NCI_STATIC_RF_CONN_ID;
+                        INIT_LIST_HEAD(&conn_info->list);
+                        list_add(&conn_info->list, &ndev->conn_info_list);
+                }
+        }
+
+exit:
         nci_req_complete(ndev, status);
 }