target: Allow control CDBs with data > 1 page

We need to handle >1 page control cdbs, so extend the code to do a vmap
if bigger than 1 page. It seems like kmap() is still preferable if just
a page, fewer TLB shootdowns(?), so keep using that when possible.

Rename function pair for their new scope.

Signed-off-by: Andy Grover <agrover@redhat.com>
Cc: <stable@vger.kernel.org>
Signed-off-by: Nicholas Bellinger <nab@linux-iscsi.org>
diff --git a/drivers/target/target_core_alua.c b/drivers/target/target_core_alua.c
index 1b1edd1..01a2691 100644
--- a/drivers/target/target_core_alua.c
+++ b/drivers/target/target_core_alua.c
@@ -78,7 +78,7 @@
 		return -EINVAL;
 	}
 
-	buf = transport_kmap_first_data_page(cmd);
+	buf = transport_kmap_data_sg(cmd);
 
 	spin_lock(&su_dev->t10_alua.tg_pt_gps_lock);
 	list_for_each_entry(tg_pt_gp, &su_dev->t10_alua.tg_pt_gps_list,
@@ -163,7 +163,7 @@
 	buf[2] = ((rd_len >> 8) & 0xff);
 	buf[3] = (rd_len & 0xff);
 
-	transport_kunmap_first_data_page(cmd);
+	transport_kunmap_data_sg(cmd);
 
 	task->task_scsi_status = GOOD;
 	transport_complete_task(task, 1);
@@ -194,7 +194,7 @@
 		cmd->scsi_sense_reason = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
 		return -EINVAL;
 	}
-	buf = transport_kmap_first_data_page(cmd);
+	buf = transport_kmap_data_sg(cmd);
 
 	/*
 	 * Determine if explict ALUA via SET_TARGET_PORT_GROUPS is allowed
@@ -351,7 +351,7 @@
 	}
 
 out:
-	transport_kunmap_first_data_page(cmd);
+	transport_kunmap_data_sg(cmd);
 	task->task_scsi_status = GOOD;
 	transport_complete_task(task, 1);
 	return 0;
diff --git a/drivers/target/target_core_cdb.c b/drivers/target/target_core_cdb.c
index 2f2235e..07a3025 100644
--- a/drivers/target/target_core_cdb.c
+++ b/drivers/target/target_core_cdb.c
@@ -83,7 +83,7 @@
 		return -EINVAL;
 	}
 
-	buf = transport_kmap_first_data_page(cmd);
+	buf = transport_kmap_data_sg(cmd);
 
 	if (dev == tpg->tpg_virt_lun0.lun_se_dev) {
 		buf[0] = 0x3f; /* Not connected */
@@ -134,7 +134,7 @@
 	buf[4] = 31; /* Set additional length to 31 */
 
 out:
-	transport_kunmap_first_data_page(cmd);
+	transport_kunmap_data_sg(cmd);
 	return 0;
 }
 
@@ -716,7 +716,7 @@
 		return -EINVAL;
 	}
 
-	buf = transport_kmap_first_data_page(cmd);
+	buf = transport_kmap_data_sg(cmd);
 
 	buf[0] = dev->transport->get_device_type(dev);
 
@@ -733,7 +733,7 @@
 	ret = -EINVAL;
 
 out_unmap:
-	transport_kunmap_first_data_page(cmd);
+	transport_kunmap_data_sg(cmd);
 out:
 	if (!ret) {
 		task->task_scsi_status = GOOD;
@@ -755,7 +755,7 @@
 	else
 		blocks = (u32)blocks_long;
 
-	buf = transport_kmap_first_data_page(cmd);
+	buf = transport_kmap_data_sg(cmd);
 
 	buf[0] = (blocks >> 24) & 0xff;
 	buf[1] = (blocks >> 16) & 0xff;
@@ -771,7 +771,7 @@
 	if (dev->se_sub_dev->se_dev_attrib.emulate_tpu || dev->se_sub_dev->se_dev_attrib.emulate_tpws)
 		put_unaligned_be32(0xFFFFFFFF, &buf[0]);
 
-	transport_kunmap_first_data_page(cmd);
+	transport_kunmap_data_sg(cmd);
 
 	task->task_scsi_status = GOOD;
 	transport_complete_task(task, 1);
@@ -785,7 +785,7 @@
 	unsigned char *buf;
 	unsigned long long blocks = dev->transport->get_blocks(dev);
 
-	buf = transport_kmap_first_data_page(cmd);
+	buf = transport_kmap_data_sg(cmd);
 
 	buf[0] = (blocks >> 56) & 0xff;
 	buf[1] = (blocks >> 48) & 0xff;
@@ -806,7 +806,7 @@
 	if (dev->se_sub_dev->se_dev_attrib.emulate_tpu || dev->se_sub_dev->se_dev_attrib.emulate_tpws)
 		buf[14] = 0x80;
 
-	transport_kunmap_first_data_page(cmd);
+	transport_kunmap_data_sg(cmd);
 
 	task->task_scsi_status = GOOD;
 	transport_complete_task(task, 1);
@@ -1019,9 +1019,9 @@
 			offset = cmd->data_length;
 	}
 
-	rbuf = transport_kmap_first_data_page(cmd);
+	rbuf = transport_kmap_data_sg(cmd);
 	memcpy(rbuf, buf, offset);
-	transport_kunmap_first_data_page(cmd);
+	transport_kunmap_data_sg(cmd);
 
 	task->task_scsi_status = GOOD;
 	transport_complete_task(task, 1);
@@ -1043,7 +1043,7 @@
 		return -ENOSYS;
 	}
 
-	buf = transport_kmap_first_data_page(cmd);
+	buf = transport_kmap_data_sg(cmd);
 
 	if (!core_scsi3_ua_clear_for_request_sense(cmd, &ua_asc, &ua_ascq)) {
 		/*
@@ -1089,7 +1089,7 @@
 	}
 
 end:
-	transport_kunmap_first_data_page(cmd);
+	transport_kunmap_data_sg(cmd);
 	task->task_scsi_status = GOOD;
 	transport_complete_task(task, 1);
 	return 0;
@@ -1123,7 +1123,7 @@
 	dl = get_unaligned_be16(&cdb[0]);
 	bd_dl = get_unaligned_be16(&cdb[2]);
 
-	buf = transport_kmap_first_data_page(cmd);
+	buf = transport_kmap_data_sg(cmd);
 
 	ptr = &buf[offset];
 	pr_debug("UNMAP: Sub: %s Using dl: %hu bd_dl: %hu size: %hu"
@@ -1147,7 +1147,7 @@
 	}
 
 err:
-	transport_kunmap_first_data_page(cmd);
+	transport_kunmap_data_sg(cmd);
 	if (!ret) {
 		task->task_scsi_status = GOOD;
 		transport_complete_task(task, 1);
diff --git a/drivers/target/target_core_device.c b/drivers/target/target_core_device.c
index de5f4fe..edbcabb 100644
--- a/drivers/target/target_core_device.c
+++ b/drivers/target/target_core_device.c
@@ -657,7 +657,7 @@
 	unsigned char *buf;
 	u32 cdb_offset = 0, lun_count = 0, offset = 8, i;
 
-	buf = transport_kmap_first_data_page(se_cmd);
+	buf = (unsigned char *) transport_kmap_data_sg(se_cmd);
 
 	/*
 	 * If no struct se_session pointer is present, this struct se_cmd is
@@ -695,7 +695,7 @@
 	 * See SPC3 r07, page 159.
 	 */
 done:
-	transport_kunmap_first_data_page(se_cmd);
+	transport_kunmap_data_sg(se_cmd);
 	lun_count *= 8;
 	buf[0] = ((lun_count >> 24) & 0xff);
 	buf[1] = ((lun_count >> 16) & 0xff);
diff --git a/drivers/target/target_core_pr.c b/drivers/target/target_core_pr.c
index 68c71cd..b7c7793 100644
--- a/drivers/target/target_core_pr.c
+++ b/drivers/target/target_core_pr.c
@@ -1535,7 +1535,7 @@
 	tidh_new->dest_local_nexus = 1;
 	list_add_tail(&tidh_new->dest_list, &tid_dest_list);
 
-	buf = transport_kmap_first_data_page(cmd);
+	buf = transport_kmap_data_sg(cmd);
 	/*
 	 * For a PERSISTENT RESERVE OUT specify initiator ports payload,
 	 * first extract TransportID Parameter Data Length, and make sure
@@ -1786,7 +1786,7 @@
 
 	}
 
-	transport_kunmap_first_data_page(cmd);
+	transport_kunmap_data_sg(cmd);
 
 	/*
 	 * Go ahead and create a registrations from tid_dest_list for the
@@ -1834,7 +1834,7 @@
 
 	return 0;
 out:
-	transport_kunmap_first_data_page(cmd);
+	transport_kunmap_data_sg(cmd);
 	/*
 	 * For the failure case, release everything from tid_dest_list
 	 * including *dest_pr_reg and the configfs dependances..
@@ -3411,14 +3411,14 @@
 	 * will be moved to for the TransportID containing SCSI initiator WWN
 	 * information.
 	 */
-	buf = transport_kmap_first_data_page(cmd);
+	buf = transport_kmap_data_sg(cmd);
 	rtpi = (buf[18] & 0xff) << 8;
 	rtpi |= buf[19] & 0xff;
 	tid_len = (buf[20] & 0xff) << 24;
 	tid_len |= (buf[21] & 0xff) << 16;
 	tid_len |= (buf[22] & 0xff) << 8;
 	tid_len |= buf[23] & 0xff;
-	transport_kunmap_first_data_page(cmd);
+	transport_kunmap_data_sg(cmd);
 	buf = NULL;
 
 	if ((tid_len + 24) != cmd->data_length) {
@@ -3470,7 +3470,7 @@
 		return -EINVAL;
 	}
 
-	buf = transport_kmap_first_data_page(cmd);
+	buf = transport_kmap_data_sg(cmd);
 	proto_ident = (buf[24] & 0x0f);
 #if 0
 	pr_debug("SPC-3 PR REGISTER_AND_MOVE: Extracted Protocol Identifier:"
@@ -3504,7 +3504,7 @@
 		goto out;
 	}
 
-	transport_kunmap_first_data_page(cmd);
+	transport_kunmap_data_sg(cmd);
 	buf = NULL;
 
 	pr_debug("SPC-3 PR [%s] Extracted initiator %s identifier: %s"
@@ -3769,13 +3769,13 @@
 					" REGISTER_AND_MOVE\n");
 	}
 
-	transport_kunmap_first_data_page(cmd);
+	transport_kunmap_data_sg(cmd);
 
 	core_scsi3_put_pr_reg(dest_pr_reg);
 	return 0;
 out:
 	if (buf)
-		transport_kunmap_first_data_page(cmd);
+		transport_kunmap_data_sg(cmd);
 	if (dest_se_deve)
 		core_scsi3_lunacl_undepend_item(dest_se_deve);
 	if (dest_node_acl)
@@ -3849,7 +3849,7 @@
 	scope = (cdb[2] & 0xf0);
 	type = (cdb[2] & 0x0f);
 
-	buf = transport_kmap_first_data_page(cmd);
+	buf = transport_kmap_data_sg(cmd);
 	/*
 	 * From PERSISTENT_RESERVE_OUT parameter list (payload)
 	 */
@@ -3867,7 +3867,7 @@
 		aptpl = (buf[17] & 0x01);
 		unreg = (buf[17] & 0x02);
 	}
-	transport_kunmap_first_data_page(cmd);
+	transport_kunmap_data_sg(cmd);
 	buf = NULL;
 
 	/*
@@ -3967,7 +3967,7 @@
 		return -EINVAL;
 	}
 
-	buf = transport_kmap_first_data_page(cmd);
+	buf = transport_kmap_data_sg(cmd);
 	buf[0] = ((su_dev->t10_pr.pr_generation >> 24) & 0xff);
 	buf[1] = ((su_dev->t10_pr.pr_generation >> 16) & 0xff);
 	buf[2] = ((su_dev->t10_pr.pr_generation >> 8) & 0xff);
@@ -4001,7 +4001,7 @@
 	buf[6] = ((add_len >> 8) & 0xff);
 	buf[7] = (add_len & 0xff);
 
-	transport_kunmap_first_data_page(cmd);
+	transport_kunmap_data_sg(cmd);
 
 	return 0;
 }
@@ -4027,7 +4027,7 @@
 		return -EINVAL;
 	}
 
-	buf = transport_kmap_first_data_page(cmd);
+	buf = transport_kmap_data_sg(cmd);
 	buf[0] = ((su_dev->t10_pr.pr_generation >> 24) & 0xff);
 	buf[1] = ((su_dev->t10_pr.pr_generation >> 16) & 0xff);
 	buf[2] = ((su_dev->t10_pr.pr_generation >> 8) & 0xff);
@@ -4086,7 +4086,7 @@
 
 err:
 	spin_unlock(&se_dev->dev_reservation_lock);
-	transport_kunmap_first_data_page(cmd);
+	transport_kunmap_data_sg(cmd);
 
 	return 0;
 }
@@ -4110,7 +4110,7 @@
 		return -EINVAL;
 	}
 
-	buf = transport_kmap_first_data_page(cmd);
+	buf = transport_kmap_data_sg(cmd);
 
 	buf[0] = ((add_len << 8) & 0xff);
 	buf[1] = (add_len & 0xff);
@@ -4142,7 +4142,7 @@
 	buf[4] |= 0x02; /* PR_TYPE_WRITE_EXCLUSIVE */
 	buf[5] |= 0x01; /* PR_TYPE_EXCLUSIVE_ACCESS_ALLREG */
 
-	transport_kunmap_first_data_page(cmd);
+	transport_kunmap_data_sg(cmd);
 
 	return 0;
 }
@@ -4172,7 +4172,7 @@
 		return -EINVAL;
 	}
 
-	buf = transport_kmap_first_data_page(cmd);
+	buf = transport_kmap_data_sg(cmd);
 
 	buf[0] = ((su_dev->t10_pr.pr_generation >> 24) & 0xff);
 	buf[1] = ((su_dev->t10_pr.pr_generation >> 16) & 0xff);
@@ -4293,7 +4293,7 @@
 	buf[6] = ((add_len >> 8) & 0xff);
 	buf[7] = (add_len & 0xff);
 
-	transport_kunmap_first_data_page(cmd);
+	transport_kunmap_data_sg(cmd);
 
 	return 0;
 }
diff --git a/drivers/target/target_core_pscsi.c b/drivers/target/target_core_pscsi.c
index d35467d..8d4def3 100644
--- a/drivers/target/target_core_pscsi.c
+++ b/drivers/target/target_core_pscsi.c
@@ -693,7 +693,7 @@
 
 		if (task->task_se_cmd->se_deve->lun_flags &
 				TRANSPORT_LUNFLAGS_READ_ONLY) {
-			unsigned char *buf = transport_kmap_first_data_page(task->task_se_cmd);
+			unsigned char *buf = transport_kmap_data_sg(task->task_se_cmd);
 
 			if (cdb[0] == MODE_SENSE_10) {
 				if (!(buf[3] & 0x80))
@@ -703,7 +703,7 @@
 					buf[2] |= 0x80;
 			}
 
-			transport_kunmap_first_data_page(task->task_se_cmd);
+			transport_kunmap_data_sg(task->task_se_cmd);
 		}
 	}
 after_mode_sense:
diff --git a/drivers/target/target_core_transport.c b/drivers/target/target_core_transport.c
index e186f7d..cf996d8 100644
--- a/drivers/target/target_core_transport.c
+++ b/drivers/target/target_core_transport.c
@@ -3084,11 +3084,6 @@
 	     (cmd->se_cmd_flags & SCF_SCSI_DATA_SG_IO_CDB)))
 		goto out_unsupported_cdb;
 
-	/* Let's limit control cdbs to a page, for simplicity's sake. */
-	if ((cmd->se_cmd_flags & SCF_SCSI_CONTROL_SG_IO_CDB) &&
-	    size > PAGE_SIZE)
-		goto out_invalid_cdb_field;
-
 	transport_set_supported_SAM_opcode(cmd);
 	return ret;
 
@@ -3492,9 +3487,11 @@
 }
 EXPORT_SYMBOL(transport_generic_map_mem_to_cmd);
 
-void *transport_kmap_first_data_page(struct se_cmd *cmd)
+void *transport_kmap_data_sg(struct se_cmd *cmd)
 {
 	struct scatterlist *sg = cmd->t_data_sg;
+	struct page **pages;
+	int i;
 
 	BUG_ON(!sg);
 	/*
@@ -3502,15 +3499,41 @@
 	 * tcm_loop who may be using a contig buffer from the SCSI midlayer for
 	 * control CDBs passed as SGLs via transport_generic_map_mem_to_cmd()
 	 */
-	return kmap(sg_page(sg)) + sg->offset;
-}
-EXPORT_SYMBOL(transport_kmap_first_data_page);
+	if (!cmd->t_data_nents)
+		return NULL;
+	else if (cmd->t_data_nents == 1)
+		return kmap(sg_page(sg)) + sg->offset;
 
-void transport_kunmap_first_data_page(struct se_cmd *cmd)
-{
-	kunmap(sg_page(cmd->t_data_sg));
+	/* >1 page. use vmap */
+	pages = kmalloc(sizeof(*pages) * cmd->t_data_nents, GFP_KERNEL);
+	if (!pages)
+		return NULL;
+
+	/* convert sg[] to pages[] */
+	for_each_sg(cmd->t_data_sg, sg, cmd->t_data_nents, i) {
+		pages[i] = sg_page(sg);
+	}
+
+	cmd->t_data_vmap = vmap(pages, cmd->t_data_nents,  VM_MAP, PAGE_KERNEL);
+	kfree(pages);
+	if (!cmd->t_data_vmap)
+		return NULL;
+
+	return cmd->t_data_vmap + cmd->t_data_sg[0].offset;
 }
-EXPORT_SYMBOL(transport_kunmap_first_data_page);
+EXPORT_SYMBOL(transport_kmap_data_sg);
+
+void transport_kunmap_data_sg(struct se_cmd *cmd)
+{
+	if (!cmd->t_data_nents)
+		return;
+	else if (cmd->t_data_nents == 1)
+		kunmap(sg_page(cmd->t_data_sg));
+
+	vunmap(cmd->t_data_vmap);
+	cmd->t_data_vmap = NULL;
+}
+EXPORT_SYMBOL(transport_kunmap_data_sg);
 
 static int
 transport_generic_get_mem(struct se_cmd *cmd)
diff --git a/include/target/target_core_backend.h b/include/target/target_core_backend.h
index 4866499..e5e6ff9 100644
--- a/include/target/target_core_backend.h
+++ b/include/target/target_core_backend.h
@@ -59,7 +59,7 @@
 int	transport_set_vpd_ident(struct t10_vpd *, unsigned char *);
 
 /* core helpers also used by command snooping in pscsi */
-void	*transport_kmap_first_data_page(struct se_cmd *);
-void	transport_kunmap_first_data_page(struct se_cmd *);
+void	*transport_kmap_data_sg(struct se_cmd *);
+void	transport_kunmap_data_sg(struct se_cmd *);
 
 #endif /* TARGET_CORE_BACKEND_H */
diff --git a/include/target/target_core_base.h b/include/target/target_core_base.h
index daf532b..dc4e345 100644
--- a/include/target/target_core_base.h
+++ b/include/target/target_core_base.h
@@ -582,6 +582,7 @@
 
 	struct scatterlist	*t_data_sg;
 	unsigned int		t_data_nents;
+	void			*t_data_vmap;
 	struct scatterlist	*t_bidi_data_sg;
 	unsigned int		t_bidi_data_nents;