[PATCH v2 23/25] usb: gadget: f_tcm: Handle TASK_MANAGEMENT commands

[Thinh Nguyen <Thinh.Nguyen@synopsys.com>]

Handle target_core_fabric_ops TASK MANAGEMENT functions and their
response. If a TASK MANAGEMENT command is received, the driver will
interpret the function TMF_*, translate to TMR_*, and fire off a command
work executing target_submit_tmr(). On completion, it will handle the
TASK MANAGEMENT response through uasp_send_tm_response().

Signed-off-by: Thinh Nguyen <Thinh.Nguyen@synopsys.com>
---
 Changes in v2:
 - Splitted into smaller change. Remove handling of overlapped tag.
 - Directly respond to host if f_tcm detected failure with response IU rather
   than reporting to target core.

 drivers/usb/gadget/function/f_tcm.c | 206 +++++++++++++++++++++++++---
 drivers/usb/gadget/function/tcm.h   |   7 +-
 2 files changed, 193 insertions(+), 20 deletions(-)

diff --git a/drivers/usb/gadget/function/f_tcm.c b/drivers/usb/gadget/function/f_tcm.c
index 116be1c47ac9..8b99ee07df87 100644
--- a/drivers/usb/gadget/function/f_tcm.c
+++ b/drivers/usb/gadget/function/f_tcm.c
@@ -12,6 +12,7 @@
 #include <linux/string.h>
 #include <linux/configfs.h>
 #include <linux/ctype.h>
+#include <linux/delay.h>
 #include <linux/usb/ch9.h>
 #include <linux/usb/composite.h>
 #include <linux/usb/gadget.h>
@@ -462,6 +463,51 @@ static int usbg_bot_setup(struct usb_function *f,
 
 /* Start uas.c code */
 
+static int tcm_to_uasp_response(enum tcm_tmrsp_table code)
+{
+	switch (code) {
+	case TMR_FUNCTION_FAILED:
+		return RC_TMF_FAILED;
+	case TMR_FUNCTION_COMPLETE:
+	case TMR_TASK_DOES_NOT_EXIST:
+		return RC_TMF_COMPLETE;
+	case TMR_LUN_DOES_NOT_EXIST:
+		return RC_INCORRECT_LUN;
+	case TMR_OVERLAPPED_TAG_ATTEMPTED:
+		return RC_OVERLAPPED_TAG;
+	case TMR_FUNCTION_REJECTED:
+	case TMR_TASK_MGMT_FUNCTION_NOT_SUPPORTED:
+	default:
+		return RC_TMF_NOT_SUPPORTED;
+	}
+}
+
+static unsigned char uasp_to_tcm_func(int code)
+{
+	switch (code) {
+	case TMF_ABORT_TASK:
+		return TMR_ABORT_TASK;
+	case TMF_ABORT_TASK_SET:
+		return TMR_ABORT_TASK_SET;
+	case TMF_CLEAR_TASK_SET:
+		return TMR_CLEAR_TASK_SET;
+	case TMF_LOGICAL_UNIT_RESET:
+		return TMR_LUN_RESET;
+	case TMF_I_T_NEXUS_RESET:
+		return TMR_I_T_NEXUS_RESET;
+	case TMF_CLEAR_ACA:
+		return TMR_CLEAR_ACA;
+	case TMF_QUERY_TASK:
+		return TMR_QUERY_TASK;
+	case TMF_QUERY_TASK_SET:
+		return TMR_QUERY_TASK_SET;
+	case TMF_QUERY_ASYNC_EVENT:
+		return TMR_QUERY_ASYNC_EVENT;
+	default:
+		return TMR_UNKNOWN;
+	}
+}
+
 static void uasp_cleanup_one_stream(struct f_uas *fu, struct uas_stream *stream)
 {
 	/* We have either all three allocated or none */
@@ -581,6 +627,33 @@ static void uasp_prepare_status(struct usbg_cmd *cmd)
 	stream->req_status->complete = uasp_status_data_cmpl;
 }
 
+static void uasp_prepare_response(struct usbg_cmd *cmd)
+{
+	struct se_cmd *se_cmd = &cmd->se_cmd;
+	struct response_iu *rsp_iu = &cmd->response_iu;
+	struct uas_stream *stream = uasp_get_stream_by_tag(cmd->fu, cmd->tag);
+
+	cmd->state = UASP_QUEUE_COMMAND;
+	rsp_iu->iu_id = IU_ID_RESPONSE;
+	rsp_iu->tag = cpu_to_be16(cmd->tag);
+
+	if (cmd->tmr_rsp != RC_RESPONSE_UNKNOWN)
+		rsp_iu->response_code = cmd->tmr_rsp;
+	else
+		rsp_iu->response_code =
+			tcm_to_uasp_response(se_cmd->se_tmr_req->response);
+
+	stream->req_status->is_last = 1;
+	stream->req_status->stream_id = cmd->tag;
+	stream->req_status->context = cmd;
+	stream->req_status->length = sizeof(struct response_iu);
+	stream->req_status->buf = rsp_iu;
+	stream->req_status->complete = uasp_status_data_cmpl;
+}
+
+static void usbg_release_cmd(struct se_cmd *se_cmd);
+static int uasp_send_tm_response(struct usbg_cmd *cmd);
+
 static void uasp_status_data_cmpl(struct usb_ep *ep, struct usb_request *req)
 {
 	struct usbg_cmd *cmd = req->context;
@@ -619,9 +692,26 @@ static void uasp_status_data_cmpl(struct usb_ep *ep, struct usb_request *req)
 		break;
 
 	case UASP_QUEUE_COMMAND:
-		transport_generic_free_cmd(&cmd->se_cmd, 0);
-		usb_ep_queue(fu->ep_cmd, cmd->req, GFP_ATOMIC);
 
+		stream->cmd = NULL;
+
+		/*
+		 * If no command submitted to target core here, just free the
+		 * bitmap index. This is for the cases where f_tcm handles
+		 * status response instead of the target core.
+		 */
+		if (cmd->tmr_rsp != RC_RESPONSE_UNKNOWN) {
+			struct se_session *se_sess;
+
+			se_sess = fu->tpg->tpg_nexus->tvn_se_sess;
+			sbitmap_queue_clear(&se_sess->sess_tag_pool,
+					    cmd->se_cmd.map_tag,
+					    cmd->se_cmd.map_cpu);
+		} else {
+			transport_generic_free_cmd(&cmd->se_cmd, 0);
+		}
+
+		usb_ep_queue(fu->ep_cmd, cmd->req, GFP_ATOMIC);
 		break;
 
 	default:
@@ -630,6 +720,7 @@ static void uasp_status_data_cmpl(struct usb_ep *ep, struct usb_request *req)
 	return;
 
 cleanup:
+	stream->cmd = NULL;
 	transport_generic_free_cmd(&cmd->se_cmd, 0);
 }
 
@@ -645,6 +736,18 @@ static int uasp_send_status_response(struct usbg_cmd *cmd)
 	return usb_ep_queue(fu->ep_status, stream->req_status, GFP_ATOMIC);
 }
 
+static int uasp_send_tm_response(struct usbg_cmd *cmd)
+{
+	struct f_uas *fu = cmd->fu;
+	struct uas_stream *stream = uasp_get_stream_by_tag(fu, cmd->tag);
+	struct response_iu *iu = &cmd->response_iu;
+
+	iu->tag = cpu_to_be16(cmd->tag);
+	cmd->fu = fu;
+	uasp_prepare_response(cmd);
+	return usb_ep_queue(fu->ep_status, stream->req_status, GFP_ATOMIC);
+}
+
 static int uasp_send_read_response(struct usbg_cmd *cmd)
 {
 	struct f_uas *fu = cmd->fu;
@@ -1045,9 +1148,24 @@ static int usbg_send_read_response(struct se_cmd *se_cmd)
 		return uasp_send_read_response(cmd);
 }
 
-static void usbg_cmd_work(struct work_struct *work)
+static void usbg_submit_tmr(struct usbg_cmd *cmd)
 {
 	struct usbg_cmd *cmd = container_of(work, struct usbg_cmd, work);
+	struct se_session *se_sess;
+	struct se_cmd *se_cmd;
+	int flags = TARGET_SCF_ACK_KREF;
+
+	se_cmd = &cmd->se_cmd;
+	se_sess = cmd->fu->tpg->tpg_nexus->tvn_se_sess;
+
+	target_submit_tmr(se_cmd, se_sess,
+			  cmd->response_iu.add_response_info,
+			  cmd->unpacked_lun, NULL, cmd->tmr_func,
+			  GFP_ATOMIC, cmd->tag, flags);
+}
+
+static void usbg_submit_cmd(struct usbg_cmd *cmd)
+{
 	struct se_cmd *se_cmd;
 	struct tcm_usbg_nexus *tv_nexus;
 	struct usbg_tpg *tpg;
@@ -1076,6 +1194,29 @@ static void usbg_cmd_work(struct work_struct *work)
 			TCM_UNSUPPORTED_SCSI_OPCODE, 0);
 }
 
+static void usbg_cmd_work(struct work_struct *work)
+{
+	struct usbg_cmd *cmd = container_of(work, struct usbg_cmd, work);
+
+	/*
+	 * Failure is detected by f_tcm here. Skip submitting the command to the
+	 * target core if we already know the failing response and send the usb
+	 * response to the host directly.
+	 */
+	if (cmd->tmr_rsp != RC_RESPONSE_UNKNOWN)
+		goto skip;
+
+	if (cmd->tmr_func)
+		usbg_submit_tmr(cmd);
+	else
+		usbg_submit_cmd(cmd);
+
+	return;
+
+skip:
+	uasp_send_tm_response(cmd);
+}
+
 static struct usbg_cmd *usbg_get_cmd(struct f_uas *fu,
 		struct tcm_usbg_nexus *tv_nexus, u32 scsi_tag)
 {
@@ -1102,37 +1243,63 @@ static void usbg_release_cmd(struct se_cmd *);
 
 static int usbg_submit_command(struct f_uas *fu, struct usb_request *req)
 {
-	struct command_iu *cmd_iu = req->buf;
+	struct iu *iu = req->buf;
 	struct usbg_cmd *cmd;
 	struct usbg_tpg *tpg = fu->tpg;
 	struct tcm_usbg_nexus *tv_nexus;
+	struct uas_stream *stream;
+	struct command_iu *cmd_iu;
 	u32 cmd_len;
 	u16 scsi_tag;
 
-	if (cmd_iu->iu_id != IU_ID_COMMAND) {
-		pr_err("Unsupported type %d\n", cmd_iu->iu_id);
-		return -EINVAL;
-	}
-
 	tv_nexus = tpg->tpg_nexus;
 	if (!tv_nexus) {
 		pr_err("Missing nexus, ignoring command\n");
 		return -EINVAL;
 	}
 
-	cmd_len = (cmd_iu->len & ~0x3) + 16;
-	if (cmd_len > USBG_MAX_CMD)
-		return -EINVAL;
-
-	scsi_tag = be16_to_cpup(&cmd_iu->tag);
+	scsi_tag = be16_to_cpup(&iu->tag);
 	cmd = usbg_get_cmd(fu, tv_nexus, scsi_tag);
 	if (IS_ERR(cmd)) {
 		pr_err("usbg_get_cmd failed\n");
 		return -ENOMEM;
 	}
-	memcpy(cmd->cmd_buf, cmd_iu->cdb, cmd_len);
 
-	cmd->stream = &fu->stream[cmd->tag % USBG_NUM_CMDS];
+	cmd->req = req;
+	cmd->fu = fu;
+	cmd->tag = scsi_tag;
+	cmd->se_cmd.tag = scsi_tag;
+	cmd->tmr_func = 0;
+	cmd->tmr_rsp = RC_RESPONSE_UNKNOWN;
+
+	cmd_iu = (struct command_iu *)iu;
+
+	/* Command and Task Management IUs share the same LUN offset */
+	cmd->unpacked_lun = scsilun_to_int(&cmd_iu->lun);
+
+	if (iu->iu_id != IU_ID_COMMAND && iu->iu_id != IU_ID_TASK_MGMT) {
+		cmd->tmr_rsp = RC_INVALID_INFO_UNIT;
+		goto skip;
+	}
+
+	stream->cmd = cmd;
+
+	if (iu->iu_id == IU_ID_TASK_MGMT) {
+		struct task_mgmt_iu *tm_iu;
+
+		tm_iu = (struct task_mgmt_iu *)iu;
+		cmd->tmr_func = uasp_to_tcm_func(tm_iu->function);
+		goto skip;
+	}
+
+	cmd_len = (cmd_iu->len & ~0x3) + 16;
+	if (cmd_len > USBG_MAX_CMD) {
+		pr_err("invalid len %d\n", cmd_len);
+		target_free_tag(tv_nexus->tvn_se_sess, &cmd->se_cmd);
+		stream->cmd = NULL;
+		return -EINVAL;
+	}
+	memcpy(cmd->cmd_buf, cmd_iu->cdb, cmd_len);
 
 	switch (cmd_iu->prio_attr & 0x7) {
 	case UAS_HEAD_TAG:
@@ -1153,9 +1320,7 @@ static int usbg_submit_command(struct f_uas *fu, struct usb_request *req)
 		break;
 	}
 
-	cmd->unpacked_lun = scsilun_to_int(&cmd_iu->lun);
-	cmd->req = req;
-
+skip:
 	INIT_WORK(&cmd->work, usbg_cmd_work);
 	queue_work(tpg->workqueue, &cmd->work);
 
@@ -1301,6 +1466,9 @@ static int usbg_get_cmd_state(struct se_cmd *se_cmd)
 
 static void usbg_queue_tm_rsp(struct se_cmd *se_cmd)
 {
+	struct usbg_cmd *cmd = container_of(se_cmd, struct usbg_cmd, se_cmd);
+
+	uasp_send_tm_response(cmd);
 }
 
 static void usbg_aborted_task(struct se_cmd *se_cmd)
diff --git a/drivers/usb/gadget/function/tcm.h b/drivers/usb/gadget/function/tcm.h
index 5157af1b166b..7fafc4336984 100644
--- a/drivers/usb/gadget/function/tcm.h
+++ b/drivers/usb/gadget/function/tcm.h
@@ -82,8 +82,11 @@ struct usbg_cmd {
 	u16 tag;
 	u16 prio_attr;
 	struct sense_iu sense_iu;
+	struct response_iu response_iu;
 	enum uas_state state;
-	struct uas_stream *stream;
+	int tmr_func;
+	int tmr_rsp;
+#define	RC_RESPONSE_UNKNOWN 0xff
 
 	/* BOT only */
 	__le32 bot_tag;
@@ -96,6 +99,8 @@ struct uas_stream {
 	struct usb_request	*req_in;
 	struct usb_request	*req_out;
 	struct usb_request	*req_status;
+
+	struct usbg_cmd		*cmd;
 };
 
 struct usbg_cdb {
-- 
2.28.0