[PATCH v2 06/25] usb: gadget: f_tcm: Allocate matching number of commands to streams

[Thinh Nguyen <Thinh.Nguyen@xxxxxxxxxxxx>]

We can handle multiple commands concurently. Each command services a
stream id. At the moment, the driver will handle 32 outstanding streams,
which is equivalent to 32 commands. Make sure to allocate a matching
number of commands to the number of streams.

Signed-off-by: Thinh Nguyen <Thinh.Nguyen@xxxxxxxxxxxx>
---
 Changes in v2:
 - None

 drivers/usb/gadget/function/f_tcm.c | 110 ++++++++++++++--------------
 drivers/usb/gadget/function/tcm.h   |   8 +-
 2 files changed, 61 insertions(+), 57 deletions(-)

diff --git a/drivers/usb/gadget/function/f_tcm.c b/drivers/usb/gadget/function/f_tcm.c
index 7721216dc9bc..6e0b54985932 100644
--- a/drivers/usb/gadget/function/f_tcm.c
+++ b/drivers/usb/gadget/function/f_tcm.c
@@ -50,7 +50,7 @@ static int bot_enqueue_cmd_cbw(struct f_uas *fu)
 	if (fu->flags & USBG_BOT_CMD_PEND)
 		return 0;
 
-	ret = usb_ep_queue(fu->ep_out, fu->cmd.req, GFP_ATOMIC);
+	ret = usb_ep_queue(fu->ep_out, fu->cmd[0].req, GFP_ATOMIC);
 	if (!ret)
 		fu->flags |= USBG_BOT_CMD_PEND;
 	return ret;
@@ -136,7 +136,7 @@ static void bot_send_bad_status(struct usbg_cmd *cmd)
 		}
 		req->complete = bot_err_compl;
 		req->context = cmd;
-		req->buf = fu->cmd.buf;
+		req->buf = fu->cmd[0].buf;
 		usb_ep_queue(ep, req, GFP_KERNEL);
 	} else {
 		bot_enqueue_sense_code(fu, cmd);
@@ -314,8 +314,8 @@ static int bot_prepare_reqs(struct f_uas *fu)
 	if (!fu->bot_req_out)
 		goto err_out;
 
-	fu->cmd.req = usb_ep_alloc_request(fu->ep_out, GFP_KERNEL);
-	if (!fu->cmd.req)
+	fu->cmd[0].req = usb_ep_alloc_request(fu->ep_out, GFP_KERNEL);
+	if (!fu->cmd[0].req)
 		goto err_cmd;
 
 	fu->bot_status.req = usb_ep_alloc_request(fu->ep_in, GFP_KERNEL);
@@ -327,27 +327,27 @@ static int bot_prepare_reqs(struct f_uas *fu)
 	fu->bot_status.req->complete = bot_status_complete;
 	fu->bot_status.csw.Signature = cpu_to_le32(US_BULK_CS_SIGN);
 
-	fu->cmd.buf = kmalloc(fu->ep_out->maxpacket, GFP_KERNEL);
-	if (!fu->cmd.buf)
+	fu->cmd[0].buf = kmalloc(fu->ep_out->maxpacket, GFP_KERNEL);
+	if (!fu->cmd[0].buf)
 		goto err_buf;
 
-	fu->cmd.req->complete = bot_cmd_complete;
-	fu->cmd.req->buf = fu->cmd.buf;
-	fu->cmd.req->length = fu->ep_out->maxpacket;
-	fu->cmd.req->context = fu;
+	fu->cmd[0].req->complete = bot_cmd_complete;
+	fu->cmd[0].req->buf = fu->cmd[0].buf;
+	fu->cmd[0].req->length = fu->ep_out->maxpacket;
+	fu->cmd[0].req->context = fu;
 
 	ret = bot_enqueue_cmd_cbw(fu);
 	if (ret)
 		goto err_queue;
 	return 0;
 err_queue:
-	kfree(fu->cmd.buf);
-	fu->cmd.buf = NULL;
+	kfree(fu->cmd[0].buf);
+	fu->cmd[0].buf = NULL;
 err_buf:
 	usb_ep_free_request(fu->ep_in, fu->bot_status.req);
 err_sts:
-	usb_ep_free_request(fu->ep_out, fu->cmd.req);
-	fu->cmd.req = NULL;
+	usb_ep_free_request(fu->ep_out, fu->cmd[0].req);
+	fu->cmd[0].req = NULL;
 err_cmd:
 	usb_ep_free_request(fu->ep_out, fu->bot_req_out);
 	fu->bot_req_out = NULL;
@@ -372,16 +372,16 @@ static void bot_cleanup_old_alt(struct f_uas *fu)
 
 	usb_ep_free_request(fu->ep_in, fu->bot_req_in);
 	usb_ep_free_request(fu->ep_out, fu->bot_req_out);
-	usb_ep_free_request(fu->ep_out, fu->cmd.req);
+	usb_ep_free_request(fu->ep_out, fu->cmd[0].req);
 	usb_ep_free_request(fu->ep_in, fu->bot_status.req);
 
-	kfree(fu->cmd.buf);
+	kfree(fu->cmd[0].buf);
 
 	fu->bot_req_in = NULL;
 	fu->bot_req_out = NULL;
-	fu->cmd.req = NULL;
+	fu->cmd[0].req = NULL;
 	fu->bot_status.req = NULL;
-	fu->cmd.buf = NULL;
+	fu->cmd[0].buf = NULL;
 }
 
 static void bot_set_alt(struct f_uas *fu)
@@ -482,10 +482,14 @@ static void uasp_cleanup_one_stream(struct f_uas *fu, struct uas_stream *stream)
 
 static void uasp_free_cmdreq(struct f_uas *fu)
 {
-	usb_ep_free_request(fu->ep_cmd, fu->cmd.req);
-	kfree(fu->cmd.buf);
-	fu->cmd.req = NULL;
-	fu->cmd.buf = NULL;
+	int i;
+
+	for (i = 0; i < USBG_NUM_CMDS; i++) {
+		usb_ep_free_request(fu->ep_cmd, fu->cmd[i].req);
+		kfree(fu->cmd[i].buf);
+		fu->cmd[i].req = NULL;
+		fu->cmd[i].buf = NULL;
+	}
 }
 
 static void uasp_cleanup_old_alt(struct f_uas *fu)
@@ -500,7 +504,7 @@ static void uasp_cleanup_old_alt(struct f_uas *fu)
 	usb_ep_disable(fu->ep_status);
 	usb_ep_disable(fu->ep_cmd);
 
-	for (i = 0; i < UASP_SS_EP_COMP_NUM_STREAMS; i++)
+	for (i = 0; i < USBG_NUM_CMDS; i++)
 		uasp_cleanup_one_stream(fu, &fu->stream[i]);
 	uasp_free_cmdreq(fu);
 }
@@ -603,7 +607,8 @@ static void uasp_status_data_cmpl(struct usb_ep *ep, struct usb_request *req)
 
 	case UASP_QUEUE_COMMAND:
 		transport_generic_free_cmd(&cmd->se_cmd, 0);
-		usb_ep_queue(fu->ep_cmd, fu->cmd.req, GFP_ATOMIC);
+		usb_ep_queue(fu->ep_cmd, cmd->req, GFP_ATOMIC);
+
 		break;
 
 	default:
@@ -718,7 +723,7 @@ static int uasp_send_write_request(struct usbg_cmd *cmd)
 	return ret;
 }
 
-static int usbg_submit_command(struct f_uas *, void *, unsigned int);
+static int usbg_submit_command(struct f_uas *, struct usb_request *);
 
 static void uasp_cmd_complete(struct usb_ep *ep, struct usb_request *req)
 {
@@ -728,7 +733,7 @@ static void uasp_cmd_complete(struct usb_ep *ep, struct usb_request *req)
 	if (req->status < 0)
 		return;
 
-	ret = usbg_submit_command(fu, req->buf, req->actual);
+	ret = usbg_submit_command(fu, req);
 	/*
 	 * Once we tune for performance enqueue the command req here again so
 	 * we can receive a second command while we processing this one. Pay
@@ -737,7 +742,7 @@ static void uasp_cmd_complete(struct usb_ep *ep, struct usb_request *req)
 	 */
 	if (!ret)
 		return;
-	usb_ep_queue(fu->ep_cmd, fu->cmd.req, GFP_ATOMIC);
+	usb_ep_queue(fu->ep_cmd, req, GFP_ATOMIC);
 }
 
 static int uasp_alloc_stream_res(struct f_uas *fu, struct uas_stream *stream)
@@ -766,24 +771,24 @@ static int uasp_alloc_stream_res(struct f_uas *fu, struct uas_stream *stream)
 	return -ENOMEM;
 }
 
-static int uasp_alloc_cmd(struct f_uas *fu)
+static int uasp_alloc_cmd(struct f_uas *fu, int i)
 {
-	fu->cmd.req = usb_ep_alloc_request(fu->ep_cmd, GFP_KERNEL);
-	if (!fu->cmd.req)
+	fu->cmd[i].req = usb_ep_alloc_request(fu->ep_cmd, GFP_KERNEL);
+	if (!fu->cmd[i].req)
 		goto err;
 
-	fu->cmd.buf = kmalloc(fu->ep_cmd->maxpacket, GFP_KERNEL);
-	if (!fu->cmd.buf)
+	fu->cmd[i].buf = kmalloc(fu->ep_cmd->maxpacket, GFP_KERNEL);
+	if (!fu->cmd[i].buf)
 		goto err_buf;
 
-	fu->cmd.req->complete = uasp_cmd_complete;
-	fu->cmd.req->buf = fu->cmd.buf;
-	fu->cmd.req->length = fu->ep_cmd->maxpacket;
-	fu->cmd.req->context = fu;
+	fu->cmd[i].req->complete = uasp_cmd_complete;
+	fu->cmd[i].req->buf = fu->cmd[i].buf;
+	fu->cmd[i].req->length = fu->ep_cmd->maxpacket;
+	fu->cmd[i].req->context = fu;
 	return 0;
 
 err_buf:
-	usb_ep_free_request(fu->ep_cmd, fu->cmd.req);
+	usb_ep_free_request(fu->ep_cmd, fu->cmd[i].req);
 err:
 	return -ENOMEM;
 }
@@ -792,26 +797,22 @@ static int uasp_prepare_reqs(struct f_uas *fu)
 {
 	int ret;
 	int i;
-	int max_streams;
-
-	if (fu->flags & USBG_USE_STREAMS)
-		max_streams = UASP_SS_EP_COMP_NUM_STREAMS;
-	else
-		max_streams = 1;
 
-	for (i = 0; i < max_streams; i++) {
+	for (i = 0; i < USBG_NUM_CMDS; i++) {
 		ret = uasp_alloc_stream_res(fu, &fu->stream[i]);
 		if (ret)
 			goto err_cleanup;
 	}
 
-	ret = uasp_alloc_cmd(fu);
-	if (ret)
-		goto err_free_stream;
+	for (i = 0; i < USBG_NUM_CMDS; i++) {
+		ret = uasp_alloc_cmd(fu, i);
+		if (ret)
+			goto err_free_stream;
 
-	ret = usb_ep_queue(fu->ep_cmd, fu->cmd.req, GFP_ATOMIC);
-	if (ret)
-		goto err_free_stream;
+		ret = usb_ep_queue(fu->ep_cmd, fu->cmd[i].req, GFP_ATOMIC);
+		if (ret)
+			goto err_free_stream;
+	}
 
 	return 0;
 
@@ -1081,10 +1082,9 @@ static struct usbg_cmd *usbg_get_cmd(struct f_uas *fu,
 
 static void usbg_release_cmd(struct se_cmd *);
 
-static int usbg_submit_command(struct f_uas *fu,
-		void *cmdbuf, unsigned int len)
+static int usbg_submit_command(struct f_uas *fu, struct usb_request *req)
 {
-	struct command_iu *cmd_iu = cmdbuf;
+	struct command_iu *cmd_iu = req->buf;
 	struct usbg_cmd *cmd;
 	struct usbg_tpg *tpg = fu->tpg;
 	struct tcm_usbg_nexus *tv_nexus;
@@ -1114,8 +1114,7 @@ static int usbg_submit_command(struct f_uas *fu,
 	}
 	memcpy(cmd->cmd_buf, cmd_iu->cdb, cmd_len);
 
-	cmd->stream = &fu->stream[cmd->tag %
-		UASP_SS_EP_COMP_NUM_STREAMS];
+	cmd->stream = &fu->stream[cmd->tag % USBG_NUM_CMDS];
 
 	switch (cmd_iu->prio_attr & 0x7) {
 	case UAS_HEAD_TAG:
@@ -1137,6 +1136,7 @@ static int usbg_submit_command(struct f_uas *fu,
 	}
 
 	cmd->unpacked_lun = scsilun_to_int(&cmd_iu->lun);
+	cmd->req = req;
 
 	INIT_WORK(&cmd->work, usbg_cmd_work);
 	queue_work(tpg->workqueue, &cmd->work);
diff --git a/drivers/usb/gadget/function/tcm.h b/drivers/usb/gadget/function/tcm.h
index 6cb05dcd19ff..bcbe35bb5015 100644
--- a/drivers/usb/gadget/function/tcm.h
+++ b/drivers/usb/gadget/function/tcm.h
@@ -16,6 +16,8 @@
 #define UASP_SS_EP_COMP_LOG_STREAMS 5
 #define UASP_SS_EP_COMP_NUM_STREAMS (1 << UASP_SS_EP_COMP_LOG_STREAMS)
 
+#define USBG_NUM_CMDS		UASP_SS_EP_COMP_NUM_STREAMS
+
 enum {
 	USB_G_STR_INT_UAS = 0,
 	USB_G_STR_INT_BBB,
@@ -75,6 +77,8 @@ struct usbg_cmd {
 	struct completion write_complete;
 	struct kref ref;
 
+	struct usb_request *req;
+
 	/* UAS only */
 	u16 tag;
 	u16 prio_attr;
@@ -117,14 +121,14 @@ struct f_uas {
 #define USBG_IS_BOT		(1 << 3)
 #define USBG_BOT_CMD_PEND	(1 << 4)
 
-	struct usbg_cdb		cmd;
+	struct usbg_cdb		cmd[USBG_NUM_CMDS];
 	struct usb_ep		*ep_in;
 	struct usb_ep		*ep_out;
 
 	/* UAS */
 	struct usb_ep		*ep_status;
 	struct usb_ep		*ep_cmd;
-	struct uas_stream	stream[UASP_SS_EP_COMP_NUM_STREAMS];
+	struct uas_stream	stream[USBG_NUM_CMDS];
 
 	/* BOT */
 	struct bot_status	bot_status;
-- 
2.28.0