NFC: nci: Support all destinations type when creating a connection

The current implementation limits nci_core_conn_create_req()
to only manage NCI_DESTINATION_NFCEE.
Add new parameters to nci_core_conn_create() to support all
destination types described in the NCI specification.
Because there are some parameters with variable size dynamic
buffer allocation is needed.

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

drivers/nfc/st21nfcb/st21nfcb_se.c

@@ -494,7 +494,8 @@ EXPORT_SYMBOL_GPL(st21nfcb_nci_enable_se);
 
 static int st21nfcb_hci_network_init(struct nci_dev *ndev)
 {
-	struct core_conn_create_dest_spec_params dest_params;
+	struct core_conn_create_dest_spec_params *dest_params;
+	struct dest_spec_params spec_params;
 	struct nci_conn_info    *conn_info;
 	int r, dev_num;
 
@@ -502,17 +503,29 @@ static int st21nfcb_hci_network_init(struct nci_dev *ndev)
 	if (r != NCI_STATUS_OK)
 		goto exit;
 
-	dest_params.type = NCI_DESTINATION_SPECIFIC_PARAM_NFCEE_TYPE;
-	dest_params.length = sizeof(struct dest_spec_params);
-	dest_params.value.id = ndev->hci_dev->conn_info->id;
-	dest_params.value.protocol = NCI_NFCEE_INTERFACE_HCI_ACCESS;
-	r = nci_core_conn_create(ndev, &dest_params);
-	if (r != NCI_STATUS_OK)
+	dest_params =
+		kzalloc(sizeof(struct core_conn_create_dest_spec_params) +
+			sizeof(struct dest_spec_params), GFP_KERNEL);
+	if (dest_params == NULL) {
+		r = -ENOMEM;
 		goto exit;
+	}
+
+	dest_params->type = NCI_DESTINATION_SPECIFIC_PARAM_NFCEE_TYPE;
+	dest_params->length = sizeof(struct dest_spec_params);
+	spec_params.id = ndev->hci_dev->conn_info->id;
+	spec_params.protocol = NCI_NFCEE_INTERFACE_HCI_ACCESS;
+	memcpy(dest_params->value, &spec_params, sizeof(struct dest_spec_params));
+	r = nci_core_conn_create(ndev, NCI_DESTINATION_NFCEE, 1,
+				 sizeof(struct core_conn_create_dest_spec_params) +
+				 sizeof(struct dest_spec_params),
+				 dest_params);
+	if (r != NCI_STATUS_OK)
+		goto free_dest_params;
 
 	conn_info = ndev->hci_dev->conn_info;
 	if (!conn_info)
-		goto exit;
+		goto free_dest_params;
 
 	memcpy(ndev->hci_dev->init_data.gates, st21nfcb_gates,
 	       sizeof(st21nfcb_gates));
@@ -522,8 +535,10 @@ static int st21nfcb_hci_network_init(struct nci_dev *ndev)
 	 * persistent info to discriminate 2 identical chips
 	 */
 	dev_num = find_first_zero_bit(dev_mask, ST21NFCB_NUM_DEVICES);
-	if (dev_num >= ST21NFCB_NUM_DEVICES)
-		return -ENODEV;
+	if (dev_num >= ST21NFCB_NUM_DEVICES) {
+		r = -ENODEV;
+		goto free_dest_params;
+	}
 
 	scnprintf(ndev->hci_dev->init_data.session_id,
 		  sizeof(ndev->hci_dev->init_data.session_id),
@@ -540,6 +555,9 @@ static int st21nfcb_hci_network_init(struct nci_dev *ndev)
 
 	return 0;
 
+free_dest_params:
+	kfree(dest_params);
+
 exit:
 	return r;
 }

include/net/nfc/nci.h

@@ -244,21 +244,23 @@ struct nci_core_set_config_cmd {
 } __packed;
 
 #define NCI_OP_CORE_CONN_CREATE_CMD	nci_opcode_pack(NCI_GID_CORE, 0x04)
+#define DEST_SPEC_PARAMS_ID_INDEX	0
+#define DEST_SPEC_PARAMS_PROTOCOL_INDEX	1
 struct dest_spec_params {
-	__u8	id;
-	__u8	protocol;
+	__u8    id;
+	__u8    protocol;
 } __packed;
 
 struct core_conn_create_dest_spec_params {
-	__u8	type;
-	__u8	length;
-	struct dest_spec_params value;
+	__u8    type;
+	__u8    length;
+	__u8    value[0];
 } __packed;
 
 struct nci_core_conn_create_cmd {
-	__u8	destination_type;
-	__u8	number_destination_params;
-	struct core_conn_create_dest_spec_params params;
+	__u8    destination_type;
+	__u8    number_destination_params;
+	struct core_conn_create_dest_spec_params params[0];
 } __packed;
 
 #define NCI_OP_CORE_CONN_CLOSE_CMD	nci_opcode_pack(NCI_GID_CORE, 0x05)

include/net/nfc/nci_core.h

@@ -263,7 +263,9 @@ int nci_set_config(struct nci_dev *ndev, __u8 id, size_t len, __u8 *val);
 
 int nci_nfcee_discover(struct nci_dev *ndev, u8 action);
 int nci_nfcee_mode_set(struct nci_dev *ndev, u8 nfcee_id, u8 nfcee_mode);
-int nci_core_conn_create(struct nci_dev *ndev,
+int nci_core_conn_create(struct nci_dev *ndev, u8 destination_type,
+			 u8 number_destination_params,
+			 size_t params_len,
 			 struct core_conn_create_dest_spec_params *params);
 int nci_core_conn_close(struct nci_dev *ndev, u8 conn_id);
 

net/nfc/nci/core.c

@@ -41,6 +41,11 @@
 #include <net/nfc/nci_core.h>
 #include <linux/nfc.h>
 
+struct core_conn_create_data {
+	int length;
+	struct nci_core_conn_create_cmd *cmd;
+};
+
 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);
@@ -509,25 +514,38 @@ EXPORT_SYMBOL(nci_nfcee_mode_set);
 
 static void nci_core_conn_create_req(struct nci_dev *ndev, unsigned long opt)
 {
-	struct nci_core_conn_create_cmd cmd;
-	struct core_conn_create_dest_spec_params *params =
-				(struct core_conn_create_dest_spec_params *)opt;
-
-	cmd.destination_type = NCI_DESTINATION_NFCEE;
-	cmd.number_destination_params = 1;
-	memcpy(&cmd.params.type, params,
-	       sizeof(struct core_conn_create_dest_spec_params));
-	nci_send_cmd(ndev, NCI_OP_CORE_CONN_CREATE_CMD,
-		     sizeof(struct nci_core_conn_create_cmd), &cmd);
+	struct core_conn_create_data *data =
+					(struct core_conn_create_data *)opt;
+
+	nci_send_cmd(ndev, NCI_OP_CORE_CONN_CREATE_CMD, data->length, data->cmd);
 }
 
-int nci_core_conn_create(struct nci_dev *ndev,
+int nci_core_conn_create(struct nci_dev *ndev, u8 destination_type,
+			 u8 number_destination_params,
+			 size_t params_len,
 			 struct core_conn_create_dest_spec_params *params)
 {
-	ndev->cur_id = params->value.id;
-	return nci_request(ndev, nci_core_conn_create_req,
-			(unsigned long)params,
-			msecs_to_jiffies(NCI_CMD_TIMEOUT));
+	int r;
+	struct nci_core_conn_create_cmd *cmd;
+	struct core_conn_create_data data;
+
+	data.length = params_len + sizeof(struct nci_core_conn_create_cmd);
+	cmd = kzalloc(data.length, GFP_KERNEL);
+	if (!cmd)
+		return -ENOMEM;
+
+	cmd->destination_type = destination_type;
+	cmd->number_destination_params = number_destination_params;
+	memcpy(cmd->params, params, params_len);
+
+	data.cmd = cmd;
+	ndev->cur_id = params->value[DEST_SPEC_PARAMS_ID_INDEX];
+
+	r = __nci_request(ndev, nci_core_conn_create_req,
+			  (unsigned long)&data,
+			  msecs_to_jiffies(NCI_CMD_TIMEOUT));
+	kfree(cmd);
+	return r;
 }
 EXPORT_SYMBOL(nci_core_conn_create);